diff --git a/CHANGELOG.md b/CHANGELOG.md index 688eff6f5c855..f83c3ac04a5f4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -2,15 +2,28 @@ ### [DART 6.10.0 (20XX-XX-XX)](https://github.com/dartsim/dart/milestone/58?closed=1) +* Kinematics + + * Added IkFast parameter accessors to IkFast class: [#1396](https://github.com/dartsim/dart/pull/1396) + * Dynamics - * Fix friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369) + * Fixed friction and restitution of individual shapes in a body: [#1369](https://github.com/dartsim/dart/pull/1369) + * Fixed soft body simulation when command input is not resetted: [#1372](https://github.com/dartsim/dart/pull/1372) + * Added joint velocity limit constraint support: [#1407](https://github.com/dartsim/dart/pull/1407) * GUI * Fixed memory leaks from dart::gui::osg::Viewer: [#1349](https://github.com/dartsim/dart/pull/1349) * Added point rendering mode to PointCloudShape: [#1351](https://github.com/dartsim/dart/pull/1351), [#1355](https://github.com/dartsim/dart/pull/1355) * Updated ImGui to 1.71: [#1362](https://github.com/dartsim/dart/pull/1362) + * Fixed refresh of LineSegmentShapeNode: [#1381](https://github.com/dartsim/dart/pull/1381) + +* Parser + + * Allowed parsing SDF up to version 1.6: [#1385](https://github.com/dartsim/dart/pull/1385) + * Fixed SDF parser not creating dynamics aspect for collision shape: [#1386](https://github.com/dartsim/dart/pull/1386) + * Added root joint parsing option in URDF parser: [#1399](https://github.com/dartsim/dart/pull/1399), [#1406](https://github.com/dartsim/dart/pull/1406) * dartpy @@ -19,11 +32,25 @@ * Added shadow technique: [#1348](https://github.com/dartsim/dart/pull/1348) * Added findSolution and solveAndApply to InverseKinematics: [#1358](https://github.com/dartsim/dart/pull/1358) * Added InteractiveFrame and ImGui APIs: [#1359](https://github.com/dartsim/dart/pull/1359) + * Added bindings for Joint::getTransformFrom{Parent|Child}BodyNode(): [#1377](https://github.com/dartsim/dart/pull/1377) + * Added bindings for BodyNode::getChild{BodyNode|Joint}(): [#1387](https://github.com/dartsim/dart/pull/1387) + * Added bindings for Inertia: [#1388](https://github.com/dartsim/dart/pull/1388) + * Added bindings for getting all BodyNodes from a Skeleton: [#1397](https://github.com/dartsim/dart/pull/1397) + * Added bindings for background color support in osg viewer: [#1398](https://github.com/dartsim/dart/pull/1398) + * Added bindings for BallJoint::convertToPositions(): [#1408](https://github.com/dartsim/dart/pull/1408) + * Fixed typos in Skeleton: [#1392](https://github.com/dartsim/dart/pull/1392) * Build * Fixed compiler warnings from GCC 9.1: [#1366](https://github.com/dartsim/dart/pull/1366) - * Replace M_PI with dart::math::constantsd::pi(): [#1367](https://github.com/dartsim/dart/pull/1367) + * Replaced M_PI with dart::math::constantsd::pi(): [#1367](https://github.com/dartsim/dart/pull/1367) + * Enabled octomap support on macOS: [#1078](https://github.com/dartsim/dart/pull/1078) + +### [DART 6.9.2 (2019-08-16)](https://github.com/dartsim/dart/milestone/60?closed=1) + +* Dynamics + + * Allowed constraint force mixing > 1: [#1371](https://github.com/dartsim/dart/pull/1371) ### [DART 6.9.1 (2019-06-06)](https://github.com/dartsim/dart/milestone/59?closed=1) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index b56c7bf73e22f..bac4c39d6655c 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -609,7 +609,7 @@ void ConstraintSolver::updateConstraints() } } - if (joint->isPositionLimitEnforced()) + if (joint->areLimitsEnforced()) { mJointLimitConstraints.push_back( std::make_shared(joint)); diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index d766b152c4c23..364d6153bb98a 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -55,48 +55,34 @@ double JointLimitConstraint::mMaxErrorReductionVelocity = DART_MAX_ERV; double JointLimitConstraint::mConstraintForceMixing = DART_CFM; //============================================================================== -JointLimitConstraint::JointLimitConstraint(dynamics::Joint* _joint) +JointLimitConstraint::JointLimitConstraint(dynamics::Joint* joint) : ConstraintBase(), - mJoint(_joint), - mBodyNode(_joint->getChildBodyNode()), + mJoint(joint), + mBodyNode(joint->getChildBodyNode()), mAppliedImpulseIndex(0) { - assert(_joint); + assert(joint); assert(mBodyNode); - mLifeTime[0] = 0; - mLifeTime[1] = 0; - mLifeTime[2] = 0; - mLifeTime[3] = 0; - mLifeTime[4] = 0; - mLifeTime[5] = 0; - - mActive[0] = false; - mActive[1] = false; - mActive[2] = false; - mActive[3] = false; - mActive[4] = false; - mActive[5] = false; -} + mLifeTime.setZero(); -//============================================================================== -JointLimitConstraint::~JointLimitConstraint() -{ + mIsPositionLimitViolated.setConstant(false); + mIsVelocityLimitViolated.setConstant(false); } //============================================================================== -void JointLimitConstraint::setErrorAllowance(double _allowance) +void JointLimitConstraint::setErrorAllowance(double allowance) { // Clamp error reduction parameter if it is out of the range - if (_allowance < 0.0) + if (allowance < 0.0) { - dtwarn << "Error reduction parameter[" << _allowance + dtwarn << "Error reduction parameter[" << allowance << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mErrorAllowance = 0.0; } - mErrorAllowance = _allowance; + mErrorAllowance = allowance; } //============================================================================== @@ -106,23 +92,23 @@ double JointLimitConstraint::getErrorAllowance() } //============================================================================== -void JointLimitConstraint::setErrorReductionParameter(double _erp) +void JointLimitConstraint::setErrorReductionParameter(double erp) { // Clamp error reduction parameter if it is out of the range [0, 1] - if (_erp < 0.0) + if (erp < 0.0) { - dtwarn << "Error reduction parameter[" << _erp << "] is lower than 0.0. " + dtwarn << "Error reduction parameter[" << erp << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mErrorReductionParameter = 0.0; } - if (_erp > 1.0) + if (erp > 1.0) { - dtwarn << "Error reduction parameter[" << _erp << "] is greater than 1.0. " + dtwarn << "Error reduction parameter[" << erp << "] is greater than 1.0. " << "It is set to 1.0." << std::endl; mErrorReductionParameter = 1.0; } - mErrorReductionParameter = _erp; + mErrorReductionParameter = erp; } //============================================================================== @@ -132,18 +118,18 @@ double JointLimitConstraint::getErrorReductionParameter() } //============================================================================== -void JointLimitConstraint::setMaxErrorReductionVelocity(double _erv) +void JointLimitConstraint::setMaxErrorReductionVelocity(double erv) { // Clamp maximum error reduction velocity if it is out of the range - if (_erv < 0.0) + if (erv < 0.0) { - dtwarn << "Maximum error reduction velocity[" << _erv + dtwarn << "Maximum error reduction velocity[" << erv << "] is lower than 0.0. " << "It is set to 0.0." << std::endl; mMaxErrorReductionVelocity = 0.0; } - mMaxErrorReductionVelocity = _erv; + mMaxErrorReductionVelocity = erv; } //============================================================================== @@ -153,12 +139,12 @@ double JointLimitConstraint::getMaxErrorReductionVelocity() } //============================================================================== -void JointLimitConstraint::setConstraintForceMixing(double _cfm) +void JointLimitConstraint::setConstraintForceMixing(double cfm) { // Clamp constraint force mixing parameter if it is out of the range - if (_cfm < 1e-9) + if (cfm < 1e-9) { - dtwarn << "Constraint force mixing parameter[" << _cfm + dtwarn << "Constraint force mixing parameter[" << cfm << "] is lower than 1e-9. " << "It is set to 1e-9." << std::endl; mConstraintForceMixing = 1e-9; @@ -171,7 +157,7 @@ void JointLimitConstraint::setConstraintForceMixing(double _cfm) mConstraintForceMixing = 1.0; } - mConstraintForceMixing = _cfm; + mConstraintForceMixing = cfm; } //============================================================================== @@ -186,25 +172,56 @@ void JointLimitConstraint::update() // Reset dimention mDim = 0; - std::size_t dof = mJoint->getNumDofs(); - for (std::size_t i = 0; i < dof; ++i) + const int dof = static_cast(mJoint->getNumDofs()); + + const Eigen::VectorXd positions = mJoint->getPositions(); + const Eigen::VectorXd velocities = mJoint->getVelocities(); + + const Eigen::VectorXd positionLowerLimits = mJoint->getPositionLowerLimits(); + const Eigen::VectorXd positionUpperLimits = mJoint->getPositionUpperLimits(); + + const Eigen::VectorXd velocityLowerLimits = mJoint->getVelocityLowerLimits(); + const Eigen::VectorXd velocityUpperLimits = mJoint->getVelocityUpperLimits(); + + for (int i = 0; i < dof; ++i) { - // Lower bound check - mViolation[i] = mJoint->getPosition(i) - mJoint->getPositionLowerLimit(i); - if (mViolation[i] <= 0.0) + // Check lower position bound + mViolation[i] = positions[i] - positionLowerLimits[i]; + if (mViolation[i] < 0.0) { - mNegativeVel[i] = -mJoint->getVelocity(i); - + mNegativeVel[i] = -velocities[i]; mLowerBound[i] = 0.0; - mUpperBound[i] = dInfinity; + mUpperBound[i] = static_cast(dInfinity); + + if (mIsPositionLimitViolated[i]) + { + ++(mLifeTime[i]); + } + else + { + mIsPositionLimitViolated[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + continue; + } + + // Check upper position bound + mViolation[i] = positions[i] - positionUpperLimits[i]; + if (mViolation[i] > 0.0) + { + mNegativeVel[i] = -velocities[i]; + mLowerBound[i] = -static_cast(dInfinity); + mUpperBound[i] = 0.0; - if (mActive[i]) + if (mIsPositionLimitViolated[i]) { ++(mLifeTime[i]); } else { - mActive[i] = true; + mIsPositionLimitViolated[i] = true; mLifeTime[i] = 0; } @@ -212,22 +229,45 @@ void JointLimitConstraint::update() continue; } - // Upper bound check - mViolation[i] = mJoint->getPosition(i) - mJoint->getPositionUpperLimit(i); - if (mViolation[i] >= 0.0) + mIsPositionLimitViolated[i] = false; + + // Check lower velocity bound + mViolation[i] = velocities[i] - velocityLowerLimits[i]; + if (mViolation[i] < 0.0) { - mNegativeVel[i] = -mJoint->getVelocity(i); + mNegativeVel[i] = -mViolation[i]; + mLowerBound[i] = 0.0; + mUpperBound[i] = static_cast(dInfinity); + + if (mIsVelocityLimitViolated[i]) + { + ++(mLifeTime[i]); + } + else + { + mIsVelocityLimitViolated[i] = true; + mLifeTime[i] = 0; + } - mLowerBound[i] = -dInfinity; + ++mDim; + continue; + } + + // Check upper velocity bound + mViolation[i] = velocities[i] - velocityUpperLimits[i]; + if (mViolation[i] > 0.0) + { + mNegativeVel[i] = -mViolation[i]; + mLowerBound[i] = -static_cast(dInfinity); mUpperBound[i] = 0.0; - if (mActive[i]) + if (mIsVelocityLimitViolated[i]) { ++(mLifeTime[i]); } else { - mActive[i] = true; + mIsVelocityLimitViolated[i] = true; mLifeTime[i] = 0; } @@ -235,63 +275,83 @@ void JointLimitConstraint::update() continue; } - mActive[i] = false; + mIsVelocityLimitViolated[i] = false; } } //============================================================================== -void JointLimitConstraint::getInformation(ConstraintInfo* _lcp) +void JointLimitConstraint::getInformation(ConstraintInfo* lcp) { std::size_t index = 0; - std::size_t dof = mJoint->getNumDofs(); - for (std::size_t i = 0; i < dof; ++i) + const int dof = static_cast(mJoint->getNumDofs()); + for (int i = 0; i < dof; ++i) { - if (mActive[i] == false) - continue; + if (mIsPositionLimitViolated[i]) + { + assert(lcp->w[index] == 0.0); - assert(_lcp->w[index] == 0.0); + double bouncingVel = -mViolation[i]; - double bouncingVel = -mViolation[i]; + if (bouncingVel > 0.0) + bouncingVel = -mErrorAllowance; + else + bouncingVel = +mErrorAllowance; - if (bouncingVel > 0.0) - bouncingVel = -mErrorAllowance; - else - bouncingVel = +mErrorAllowance; + bouncingVel *= lcp->invTimeStep * mErrorReductionParameter; - bouncingVel *= _lcp->invTimeStep * mErrorReductionParameter; + if (bouncingVel > mMaxErrorReductionVelocity) + bouncingVel = mMaxErrorReductionVelocity; - if (bouncingVel > mMaxErrorReductionVelocity) - bouncingVel = mMaxErrorReductionVelocity; + lcp->b[index] = mNegativeVel[i] + bouncingVel; - _lcp->b[index] = mNegativeVel[i] + bouncingVel; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; - _lcp->lo[index] = mLowerBound[i]; - _lcp->hi[index] = mUpperBound[i]; +#ifndef NDEBUG // Debug mode + if (lcp->lo[index] > lcp->hi[index]) + { + std::cout << "dim: " << mDim << std::endl; + std::cout << "lb: " << mLowerBound[i] << std::endl; + std::cout << "ub: " << mUpperBound[i] << std::endl; + std::cout << "lb: " << lcp->lo[index] << std::endl; + std::cout << "ub: " << lcp->hi[index] << std::endl; + } +#endif - if (_lcp->lo[index] > _lcp->hi[index]) - { - std::cout << "dim: " << mDim << std::endl; - std::cout << "lb: " << mLowerBound[i] << std::endl; - std::cout << "ub: " << mUpperBound[i] << std::endl; - std::cout << "lb: " << _lcp->lo[index] << std::endl; - std::cout << "ub: " << _lcp->hi[index] << std::endl; + assert(lcp->findex[index] == -1); + + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; } - assert(_lcp->findex[index] == -1); + if (mIsVelocityLimitViolated[i]) + { + assert(lcp->w[index] == 0.0); - if (mLifeTime[i]) - _lcp->x[index] = mOldX[i]; - else - _lcp->x[index] = 0.0; + lcp->b[index] = mNegativeVel[i]; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; + + assert(lcp->findex[index] == -1); - index++; + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; + } } } //============================================================================== -void JointLimitConstraint::applyUnitImpulse(std::size_t _index) +void JointLimitConstraint::applyUnitImpulse(std::size_t index) { - assert(_index < mDim && "Invalid Index."); + assert(index < mDim && "Invalid Index."); std::size_t localIndex = 0; const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); @@ -299,10 +359,13 @@ void JointLimitConstraint::applyUnitImpulse(std::size_t _index) std::size_t dof = mJoint->getNumDofs(); for (std::size_t i = 0; i < dof; ++i) { - if (mActive[i] == false) + if (not mIsPositionLimitViolated[static_cast(i)] + && not mIsVelocityLimitViolated[static_cast(i)]) + { continue; + } - if (localIndex == _index) + if (localIndex == index) { skeleton->clearConstraintImpulses(); mJoint->setConstraintImpulse(i, 1.0); @@ -314,35 +377,38 @@ void JointLimitConstraint::applyUnitImpulse(std::size_t _index) ++localIndex; } - mAppliedImpulseIndex = _index; + mAppliedImpulseIndex = index; } //============================================================================== -void JointLimitConstraint::getVelocityChange(double* _delVel, bool _withCfm) +void JointLimitConstraint::getVelocityChange(double* delVel, bool withCfm) { - assert(_delVel != nullptr && "Null pointer is not allowed."); + assert(delVel != nullptr && "Null pointer is not allowed."); std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); for (std::size_t i = 0; i < dof; ++i) { - if (mActive[i] == false) + if (not mIsPositionLimitViolated[static_cast(i)] + && not mIsVelocityLimitViolated[static_cast(i)]) + { continue; + } if (mJoint->getSkeleton()->isImpulseApplied()) - _delVel[localIndex] = mJoint->getVelocityChange(i); + delVel[localIndex] = mJoint->getVelocityChange(i); else - _delVel[localIndex] = 0.0; + delVel[localIndex] = 0.0; ++localIndex; } // Add small values to diagnal to keep it away from singular, similar to cfm // varaible in ODE - if (_withCfm) + if (withCfm) { - _delVel[mAppliedImpulseIndex] - += _delVel[mAppliedImpulseIndex] * mConstraintForceMixing; + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; } assert(localIndex == mDim); @@ -361,19 +427,22 @@ void JointLimitConstraint::unexcite() } //============================================================================== -void JointLimitConstraint::applyImpulse(double* _lambda) +void JointLimitConstraint::applyImpulse(double* lambda) { std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); for (std::size_t i = 0; i < dof; ++i) { - if (mActive[i] == false) + if (not mIsPositionLimitViolated[static_cast(i)] + && not mIsVelocityLimitViolated[static_cast(i)]) + { continue; + } mJoint->setConstraintImpulse( - i, mJoint->getConstraintImpulse(i) + _lambda[localIndex]); + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); - mOldX[i] = _lambda[localIndex]; + mOldX[static_cast(i)] = lambda[localIndex]; ++localIndex; } @@ -388,13 +457,8 @@ dynamics::SkeletonPtr JointLimitConstraint::getRootSkeleton() const //============================================================================== bool JointLimitConstraint::isActive() const { - for (std::size_t i = 0; i < 6; ++i) - { - if (mActive[i]) - return true; - } - - return false; + return mIsPositionLimitViolated.array().any() + || mIsVelocityLimitViolated.array().any(); } } // namespace constraint diff --git a/dart/constraint/JointLimitConstraint.hpp b/dart/constraint/JointLimitConstraint.hpp index 7960a2cbeb4d4..05830b0d4d671 100644 --- a/dart/constraint/JointLimitConstraint.hpp +++ b/dart/constraint/JointLimitConstraint.hpp @@ -33,6 +33,8 @@ #ifndef DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_ #define DART_CONSTRAINT_JOINTLIMITCONSTRAINT_HPP_ +#include + #include "dart/constraint/ConstraintBase.hpp" namespace dart { @@ -44,41 +46,40 @@ class Joint; namespace constraint { -/// JointLimitConstraint handles joint position or velocity limits -// TOOD: better naming +/// JointLimitConstraint handles joint position and velocity limits class JointLimitConstraint : public ConstraintBase { public: /// Constructor - explicit JointLimitConstraint(dynamics::Joint* _joint); + explicit JointLimitConstraint(dynamics::Joint* joint); /// Destructor - virtual ~JointLimitConstraint(); + ~JointLimitConstraint() override = default; //---------------------------------------------------------------------------- // Property settings //---------------------------------------------------------------------------- /// Set global error reduction parameter - static void setErrorAllowance(double _allowance); + static void setErrorAllowance(double allowance); /// Get global error reduction parameter static double getErrorAllowance(); /// Set global error reduction parameter - static void setErrorReductionParameter(double _erp); + static void setErrorReductionParameter(double erp); /// Get global error reduction parameter static double getErrorReductionParameter(); /// Set global error reduction parameter - static void setMaxErrorReductionVelocity(double _erv); + static void setMaxErrorReductionVelocity(double erv); /// Get global error reduction parameter static double getMaxErrorReductionVelocity(); /// Set global constraint force mixing parameter - static void setConstraintForceMixing(double _cfm); + static void setConstraintForceMixing(double cfm); /// Get global constraint force mixing parameter static double getConstraintForceMixing(); @@ -99,13 +100,13 @@ class JointLimitConstraint : public ConstraintBase void update() override; // Documentation inherited - void getInformation(ConstraintInfo* _lcp) override; + void getInformation(ConstraintInfo* lcp) override; // Documentation inherited - void applyUnitImpulse(std::size_t _index) override; + void applyUnitImpulse(std::size_t index) override; // Documentation inherited - void getVelocityChange(double* _delVel, bool _withCfm) override; + void getVelocityChange(double* delVel, bool withCfm) override; // Documentation inherited void excite() override; @@ -114,7 +115,7 @@ class JointLimitConstraint : public ConstraintBase void unexcite() override; // Documentation inherited - void applyImpulse(double* _lambda) override; + void applyImpulse(double* lambda) override; // Documentation inherited dynamics::SkeletonPtr getRootSkeleton() const override; @@ -123,35 +124,38 @@ class JointLimitConstraint : public ConstraintBase bool isActive() const override; private: - /// + /// The Joint that this constraint is associated with. dynamics::Joint* mJoint; - /// + /// The child BodyNode of the Joint that this constraint is associated with. dynamics::BodyNode* mBodyNode; /// Index of applied impulse std::size_t mAppliedImpulseIndex; - /// - std::size_t mLifeTime[6]; + /// Life time of constraint of each DOF. + Eigen::Matrix mLifeTime; + + /// Whether joint value exceeds the position limit. + Eigen::Matrix mIsPositionLimitViolated; - /// - bool mActive[6]; + /// Whether joint value exceeds the velocity limit. + Eigen::Matrix mIsVelocityLimitViolated; - /// - double mViolation[6]; + /// How much current joint values are exceeded from the limits. + Eigen::Matrix mViolation; - /// - double mNegativeVel[6]; + /// The desired delta velocity to satisfy the joint limit constraint. + Eigen::Matrix mNegativeVel; - /// - double mOldX[6]; + /// Constraint impulse of the previous step. + Eigen::Matrix mOldX; - /// - double mUpperBound[6]; + /// Upper limit of the constraint impulse. + Eigen::Matrix mUpperBound; - /// - double mLowerBound[6]; + /// Lower limit of the constraint impulse. + Eigen::Matrix mLowerBound; /// Global constraint error allowance static double mErrorAllowance; diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index e29b55b027637..f9c2af0eb52b7 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -117,7 +117,7 @@ void Joint::setAspectProperties(const AspectProperties& properties) setName(properties.mName); setTransformFromParentBodyNode(properties.mT_ParentBodyToJoint); setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); - setPositionLimitEnforced(properties.mIsPositionLimitEnforced); + setLimitEnforcement(properties.mIsPositionLimitEnforced); setActuatorType(properties.mActuatorType); setMimicJoint( properties.mMimicJoint, @@ -389,13 +389,25 @@ const Eigen::Vector6d& Joint::getRelativePrimaryAcceleration() const } //============================================================================== -void Joint::setPositionLimitEnforced(bool _isPositionLimitEnforced) +void Joint::setPositionLimitEnforced(bool enforced) { - mAspectProperties.mIsPositionLimitEnforced = _isPositionLimitEnforced; + setLimitEnforcement(enforced); +} + +//============================================================================== +void Joint::setLimitEnforcement(bool enforced) +{ + mAspectProperties.mIsPositionLimitEnforced = enforced; } //============================================================================== bool Joint::isPositionLimitEnforced() const +{ + return areLimitsEnforced(); +} + +//============================================================================== +bool Joint::areLimitsEnforced() const { return mAspectProperties.mIsPositionLimitEnforced; } diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index 1d19f805d247d..12761884bbed2 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -191,22 +191,46 @@ class Joint : public virtual common::Subject, /// Get transformation from child body node to this joint const Eigen::Isometry3d& getTransformFromChildBodyNode() const; - /// Set to enforce joint position limit + /// Sets whether enforcing joint position and velocity limits. /// - /// The joint position limit is valid when the actutor type is one of + /// The joint position limit is valid when the actuator type is one of /// PASSIVE/FORCE. /// /// \sa ActuatorType - void setPositionLimitEnforced(bool _isPositionLimitEnforced); + /// + /// \deprecated Deprecated since DART 6.10. Please use + /// setLimitEnforcement() instead + DART_DEPRECATED(6.10) + void setPositionLimitEnforced(bool enforced); - /// Get whether enforcing joint position limit + /// Sets whether enforcing joint position and velocity limits. /// - /// The joint position limit is valid when the actutor type is one of - /// PASSIVE/FORCE. + /// This enforcement is only enabled when the actuator type is PASSIVE or + /// FORCE. + /// + /// \sa ActuatorType + void setLimitEnforcement(bool enforced); + + /// Returns whether enforcing joint position limit + /// + /// This enforcement is only enabled when the actuator type is PASSIVE or + /// FORCE. /// /// \sa ActuatorType + /// + /// \deprecated Deprecated since DART 6.10. Please use + /// areLimitsEnforced() instead + DART_DEPRECATED(6.10) bool isPositionLimitEnforced() const; + /// Returns whether enforcing joint position and velocity limits + /// + /// This enforcement is only enabled when the actuator type is PASSIVE or + /// FORCE. + /// + /// \sa ActuatorType + bool areLimitsEnforced() const; + /// Get a unique index in skeleton of a generalized coordinate in this Joint virtual std::size_t getIndexInSkeleton(std::size_t _index) const = 0; diff --git a/dart/dynamics/detail/JointAspect.hpp b/dart/dynamics/detail/JointAspect.hpp index 993bc12aca5a7..31755c31bce80 100644 --- a/dart/dynamics/detail/JointAspect.hpp +++ b/dart/dynamics/detail/JointAspect.hpp @@ -119,8 +119,10 @@ struct JointProperties /// Transformation from child BodyNode to this Joint Eigen::Isometry3d mT_ChildBodyToJoint; - /// True if the joint limits should be enforced in dynamic simulation + /// True if the joint position or velocity limits should be enforced in + /// dynamic simulation bool mIsPositionLimitEnforced; + // TODO(JS): Rename this to mAreLimitsEnforced /// Actuator type ActuatorType mActuatorType; diff --git a/data/skel/test/joint_limit_test.skel b/data/skel/test/joint_limit_test.skel index ff3192e82dbc8..ae133e0e07655 100644 --- a/data/skel/test/joint_limit_test.skel +++ b/data/skel/test/joint_limit_test.skel @@ -29,7 +29,7 @@ 0.1 1.0 0.1 - + 1 @@ -53,7 +53,7 @@ 0.1 1.0 0.1 - + world @@ -75,6 +75,6 @@ - + diff --git a/examples/deprecated_examples/glut_human_joint_limits/humanJointLimits.cpp b/examples/deprecated_examples/glut_human_joint_limits/humanJointLimits.cpp index 5125d8e24f986..9c7a14160620a 100644 --- a/examples/deprecated_examples/glut_human_joint_limits/humanJointLimits.cpp +++ b/examples/deprecated_examples/glut_human_joint_limits/humanJointLimits.cpp @@ -126,7 +126,7 @@ int main(int argc, char* argv[]) auto skel = world->getSkeleton("human"); for (auto joint : skel->getJoints()) { - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); } // world->getSkeleton("arm")->enableSelfCollisionCheck(); diff --git a/examples/deprecated_examples/glut_operational_space_control/Controller.cpp b/examples/deprecated_examples/glut_operational_space_control/Controller.cpp index af1f7498a961e..6798af66814c1 100644 --- a/examples/deprecated_examples/glut_operational_space_control/Controller.cpp +++ b/examples/deprecated_examples/glut_operational_space_control/Controller.cpp @@ -55,7 +55,7 @@ Controller::Controller( // Remove position limits for (int i = 0; i < dof; ++i) - _robot->getJoint(i)->setPositionLimitEnforced(false); + _robot->getJoint(i)->setLimitEnforcement(false); // Set joint damping for (int i = 0; i < dof; ++i) diff --git a/examples/operational_space_control/main.cpp b/examples/operational_space_control/main.cpp index a1b1d12c7b7dc..c95edb52da749 100644 --- a/examples/operational_space_control/main.cpp +++ b/examples/operational_space_control/main.cpp @@ -62,7 +62,7 @@ class OperationalSpaceControlWorld : public dart::gui::osg::WorldNode // Set joint properties for (std::size_t i = 0; i < mRobot->getNumJoints(); ++i) { - mRobot->getJoint(i)->setPositionLimitEnforced(false); + mRobot->getJoint(i)->setLimitEnforcement(false); mRobot->getJoint(i)->setDampingCoefficient(0, 0.5); } diff --git a/python/dartpy/dynamics/Joint.cpp b/python/dartpy/dynamics/Joint.cpp index fcbb401ed9604..ea927d57cacce 100644 --- a/python/dartpy/dynamics/Joint.cpp +++ b/python/dartpy/dynamics/Joint.cpp @@ -256,16 +256,25 @@ void Joint(py::module& m) }, ::py::arg("T")) .def( - "setPositionLimitEnforced", - +[](dart::dynamics::Joint* self, - bool isPositionLimitEnforced) -> void { - return self->setPositionLimitEnforced(isPositionLimitEnforced); + "getTransformFromParentBodyNode", + +[](const dart::dynamics::Joint* self) -> const Eigen::Isometry3d& { + return self->getTransformFromParentBodyNode(); + }) + .def( + "getTransformFromChildBodyNode", + +[](const dart::dynamics::Joint* self) -> const Eigen::Isometry3d& { + return self->getTransformFromChildBodyNode(); + }) + .def( + "setLimitEnforcement", + +[](dart::dynamics::Joint* self, bool enforce) -> void { + return self->setLimitEnforcement(enforce); }, - ::py::arg("isPositionLimitEnforced")) + ::py::arg("enforced")) .def( - "isPositionLimitEnforced", + "areLimitsEnforced", +[](const dart::dynamics::Joint* self) -> bool { - return self->isPositionLimitEnforced(); + return self->areLimitsEnforced(); }) .def( "getIndexInSkeleton", diff --git a/tutorials/tutorial_biped_finished/main.cpp b/tutorials/tutorial_biped_finished/main.cpp index b9cb36094bffb..c30ef6042eefa 100644 --- a/tutorials/tutorial_biped_finished/main.cpp +++ b/tutorials/tutorial_biped_finished/main.cpp @@ -309,7 +309,7 @@ SkeletonPtr loadBiped() // Set joint limits for (std::size_t i = 0; i < biped->getNumJoints(); ++i) - biped->getJoint(i)->setPositionLimitEnforced(true); + biped->getJoint(i)->setLimitEnforcement(true); // Enable self collision check but ignore adjacent bodies biped->enableSelfCollisionCheck(); diff --git a/unittests/comprehensive/test_Joints.cpp b/unittests/comprehensive/test_Joints.cpp index 1663a91e0012c..f9578824333ba 100644 --- a/unittests/comprehensive/test_Joints.cpp +++ b/unittests/comprehensive/test_Joints.cpp @@ -456,11 +456,11 @@ TEST_F(JOINTS, POSITION_LIMIT) double limit0 = constantsd::pi() / 6.0; double limit1 = constantsd::pi() / 6.0; - joint0->setPositionLimitEnforced(true); + joint0->setLimitEnforcement(true); joint0->setPositionLowerLimit(0, -limit0); joint0->setPositionUpperLimit(0, limit0); - joint1->setPositionLimitEnforced(true); + joint1->setLimitEnforcement(true); joint1->setPositionLowerLimit(0, -limit1); joint1->setPositionUpperLimit(0, limit1); @@ -507,6 +507,104 @@ TEST_F(JOINTS, POSITION_LIMIT) } } +//============================================================================== +TEST_F(JOINTS, POSITION_AND_VELOCITY_LIMIT) +{ + using namespace dart::math::suffixes; + + const double tol = 1e-3; + + simulation::WorldPtr myWorld = utils::SkelParser::readWorld( + "dart://sample/skel/test/joint_limit_test.skel"); + EXPECT_TRUE(myWorld != nullptr); + + myWorld->setGravity(Eigen::Vector3d(0.0, 0.0, 0.0)); + + dynamics::SkeletonPtr pendulum = myWorld->getSkeleton("double_pendulum"); + EXPECT_TRUE(pendulum != nullptr); + + dynamics::Joint* joint0 = pendulum->getJoint("joint0"); + dynamics::Joint* joint1 = pendulum->getJoint("joint1"); + + EXPECT_TRUE(joint0 != nullptr); + EXPECT_TRUE(joint1 != nullptr); + + const double posLimit0 = math::toRadian(360.0); + const double posLimit1 = math::toRadian(360.0); + const double velLimit0 = math::toRadian(5.0); // 5 degree per second + const double velLimit1 = math::toRadian(5.0); // 5 degree per second + + joint0->setLimitEnforcement(true); + joint0->setPositionLowerLimit(0, -posLimit0); + joint0->setPositionUpperLimit(0, posLimit0); + joint0->setVelocityLowerLimit(0, -velLimit0); + joint0->setVelocityUpperLimit(0, velLimit0); + + joint1->setLimitEnforcement(true); + joint1->setPositionLowerLimit(0, -posLimit1); + joint1->setPositionUpperLimit(0, posLimit1); + joint1->setVelocityLowerLimit(0, -velLimit1); + joint1->setVelocityUpperLimit(0, velLimit1); + +#ifndef NDEBUG // Debug mode + double simTime = 0.2; +#else + double simTime = 2.0; +#endif // ------- Debug mode + double timeStep = myWorld->getTimeStep(); + int nSteps = simTime / timeStep; + + // Two seconds with positive control forces + for (int i = 0; i < nSteps; i++) + { + // Apply sufficient force to hit the limits + joint0->setForce(0, 1.0); + joint1->setForce(0, 1.0); + myWorld->step(); + + // Check position limits + const double jointPos0 = joint0->getPosition(0); + const double jointPos1 = joint1->getPosition(0); + EXPECT_GE(jointPos0, -posLimit0 - tol); + EXPECT_GE(jointPos1, -posLimit1 - tol); + EXPECT_LE(jointPos0, posLimit0 + tol); + EXPECT_LE(jointPos1, posLimit1 + tol); + + // Check velocity limits + const double jointVel0 = joint0->getVelocity(0); + const double jointVel1 = joint1->getVelocity(0); + EXPECT_GE(jointVel0, -velLimit0 - tol); + EXPECT_GE(jointVel1, -velLimit1 - tol); + EXPECT_LE(jointVel0, velLimit0 + tol); + EXPECT_LE(jointVel1, velLimit1 + tol); + } + + // Two more seconds with negative control forces + for (int i = 0; i < nSteps; i++) + { + // Apply sufficient force to hit the limits + joint0->setForce(0, -1.0); + joint1->setForce(0, -1.0); + myWorld->step(); + + // Check position limits + const double jointPos0 = joint0->getPosition(0); + const double jointPos1 = joint1->getPosition(0); + EXPECT_GE(jointPos0, -posLimit0 - tol); + EXPECT_GE(jointPos1, -posLimit1 - tol); + EXPECT_LE(jointPos0, posLimit0 + tol); + EXPECT_LE(jointPos1, posLimit1 + tol); + + // Check velocity limits + const double jointVel0 = joint0->getVelocity(0); + const double jointVel1 = joint1->getVelocity(0); + EXPECT_GE(jointVel0, -velLimit0 - tol); + EXPECT_GE(jointVel1, -velLimit1 - tol); + EXPECT_LE(jointVel0, velLimit0 + tol); + EXPECT_LE(jointVel1, velLimit1 + tol); + } +} + //============================================================================== TEST_F(JOINTS, JOINT_LIMITS) { @@ -600,8 +698,8 @@ void testJointCoulombFrictionForce(double _timeStep) double frictionForce = 5.0; - joint0->setPositionLimitEnforced(false); - joint1->setPositionLimitEnforced(false); + joint0->setLimitEnforcement(false); + joint1->setLimitEnforcement(false); joint0->setCoulombFriction(0, frictionForce); joint1->setCoulombFriction(0, frictionForce); @@ -725,7 +823,7 @@ SkeletonPtr createPendulum(Joint::ActuatorType actType) joint->setPosition(0, 90.0_deg); joint->setDampingCoefficient(0, 0.0); joint->setSpringStiffness(0, 0.0); - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); joint->setCoulombFriction(0, 0.0); return pendulum; @@ -933,7 +1031,7 @@ void testMimicJoint() joint->setPosition(0, 90.0_deg); joint->setDampingCoefficient(0, 0.0); joint->setSpringStiffness(0, 0.0); - joint->setPositionLimitEnforced(true); + joint->setLimitEnforcement(true); joint->setCoulombFriction(0, 0.0); joints[i] = joint; @@ -1010,8 +1108,8 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) double frictionForce = 5.0; - joint0->setPositionLimitEnforced(true); - joint1->setPositionLimitEnforced(true); + joint0->setLimitEnforcement(true); + joint1->setLimitEnforcement(true); const double ll = -constantsd::pi() / 12.0; // -15 degree const double ul = +constantsd::pi() / 12.0; // +15 degree