diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index 7d61027918b56..b4c7b0de9246c 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -1,36 +1,272 @@ #include "dart/constraint/CouplerConstraint.hpp" +#include "dart/common/Console.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Joint.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" + +#define DART_CFM 1e-9 namespace dart { namespace constraint { -CouplerConstraint::CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio) - : mBodyNode1(body1), mBodyNode2(body2), mRatio(ratio), mImpulse(Eigen::Vector6d::Zero()) {} +double CouplerConstraint::mConstraintForceMixing = DART_CFM; + +//============================================================================== +CouplerConstraint::CouplerConstraint( + dynamics::Joint* joint, + const std::vector& mimicDofProperties) + : ConstraintBase(), + mJoint(joint), + mMimicProps(mimicDofProperties), + mBodyNode(joint->getChildBodyNode()), + mAppliedImpulseIndex(0) +{ + assert(joint); + assert(joint->getNumDofs() <= mMimicProps.size()); + assert(mBodyNode); + + std::fill(mLifeTime, mLifeTime + 6, 0); + std::fill(mActive, mActive + 6, false); +} -void CouplerConstraint::setRatio(double ratio) +//============================================================================== +CouplerConstraint::~CouplerConstraint() { - mRatio = ratio; + // Do nothing } -double CouplerConstraint::getRatio() const +//============================================================================== +const std::string& CouplerConstraint::getType() const { - return mRatio; + return getStaticType(); } +//============================================================================== +const std::string& CouplerConstraint::getStaticType() +{ + static const std::string name = "CouplerConstraint"; + return name; +} + +//============================================================================== +void CouplerConstraint::setConstraintForceMixing(double cfm) +{ + // Clamp constraint force mixing parameter if it is out of the range + if (cfm < 1e-9) { + dtwarn << "[CouplerConstraint::setConstraintForceMixing] " + << "Constraint force mixing parameter[" << cfm + << "] is lower than 1e-9. " + << "It is set to 1e-9.\n"; + mConstraintForceMixing = 1e-9; + } + + mConstraintForceMixing = cfm; +} + +//============================================================================== +double CouplerConstraint::getConstraintForceMixing() +{ + return mConstraintForceMixing; +} + +//============================================================================== void CouplerConstraint::update() { - // Implement the logic to update the constraint based on the ratio - // For simplicity, we'll assume a direct proportional relationship - Eigen::Vector6d velocity1 = mBodyNode1->getSpatialVelocity(); - Eigen::Vector6d velocity2 = mBodyNode2->getSpatialVelocity(); + // Reset dimension + mDim = 0; + + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + const auto& mimicProp = mMimicProps[i]; + + double timeStep = mJoint->getSkeleton()->getTimeStep(); + double qError + = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex) + * mimicProp.mMultiplier + + mimicProp.mOffset - mJoint->getPosition(i); + double desiredVelocity = math::clip( + qError / timeStep, + mJoint->getVelocityLowerLimit(i), + mJoint->getVelocityUpperLimit(i)); + + mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i); + + if (mNegativeVelocityError[i] != 0.0) { + mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep; + mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep; + + if (mActive[i]) { + ++(mLifeTime[i]); + } else { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + } else { + mActive[i] = false; + } + } +} + +//============================================================================== +void CouplerConstraint::getInformation(ConstraintInfo* lcp) +{ + std::size_t index = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + assert(lcp->w[index] == 0.0); + + lcp->b[index] = mNegativeVelocityError[i]; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; + + assert(lcp->findex[index] == -1); + + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; + } +} + +//============================================================================== +void CouplerConstraint::applyUnitImpulse(std::size_t index) +{ + assert(index < mDim && "Invalid Index."); + + std::size_t localIndex = 0; + const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); + + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + if (localIndex == index) { + const auto& mimicProp = mMimicProps[i]; + + skeleton->clearConstraintImpulses(); + + double impulse = 1.0; + mJoint->setConstraintImpulse(i, impulse); + + // Using const_cast to remove constness for methods that modify state + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( + mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); + + skeleton->updateBiasImpulse(mBodyNode); + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->updateBiasImpulse(const_cast(mimicProp.mReferenceJoint->getChildBodyNode())); + + skeleton->updateVelocityChange(); + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->updateVelocityChange(); + + mJoint->setConstraintImpulse(i, 0.0); + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); + } + + ++localIndex; + } + + mAppliedImpulseIndex = index; +} + +//============================================================================== +void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) +{ + 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) + continue; + + if (mJoint->getSkeleton()->isImpulseApplied()) + delVel[localIndex] = mJoint->getVelocityChange(i); + else + delVel[localIndex] = 0.0; + + ++localIndex; + } + + if (withCfm) { + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; + } + + assert(localIndex == mDim); +} + +//============================================================================== +void CouplerConstraint::excite() +{ + mJoint->getSkeleton()->setImpulseApplied(true); + for (const auto& mimicProp : mMimicProps) + { + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(true); + } +} - mImpulse = mRatio * (velocity2 - velocity1); +//============================================================================== +void CouplerConstraint::unexcite() +{ + mJoint->getSkeleton()->setImpulseApplied(false); + for (const auto& mimicProp : mMimicProps) + { + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(false); + } } -void CouplerConstraint::applyImpulse() +//============================================================================== +void CouplerConstraint::applyImpulse(double* lambda) { - // Apply equal and opposite impulses to the connected bodies - mBodyNode1->addConstraintImpulse(mImpulse); - mBodyNode2->addConstraintImpulse(-mImpulse); + std::size_t localIndex = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + + auto& mimicProp = mMimicProps[i]; + + // Using const_cast to remove constness for methods that modify state + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( + mimicProp.mReferenceDofIndex, + mimicProp.mReferenceJoint->getConstraintImpulse(mimicProp.mReferenceDofIndex) + - lambda[localIndex] * mimicProp.mMultiplier); + + mOldX[i] = lambda[localIndex]; + + ++localIndex; + } +} + +//============================================================================== +dynamics::SkeletonPtr CouplerConstraint::getRootSkeleton() const +{ + return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton()); +} + +//============================================================================== +bool CouplerConstraint::isActive() const +{ + if (mJoint->getActuatorType() == dynamics::Joint::MIMIC) + return true; + + return false; } } // namespace constraint diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index 237e21250b3c6..95b43062ef342 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -1,32 +1,112 @@ #ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ #define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ -#include -#include +#include +#include +#include namespace dart { namespace constraint { +/// Coupler constraint that couples the motions of two joints by applying +/// equal and opposite impulses based on MimicDofProperties. class CouplerConstraint : public ConstraintBase { public: - CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio); + /// Constructor that creates a CouplerConstraint using the given + /// MimicDofProperties for each dependent joint's DoF. + /// \param[in] joint The dependent joint. + /// \param[in] mimicDofProperties A vector of MimicDofProperties for each DoF + /// of the dependent joint. + explicit CouplerConstraint( + dynamics::Joint* joint, + const std::vector& mimicDofProperties); - void setRatio(double ratio); - double getRatio() const; + /// Destructor + ~CouplerConstraint() override; + // Documentation inherited + const std::string& getType() const override; + + /// Returns constraint type for this class. + static const std::string& getStaticType(); + + /// Set global constraint force mixing parameter + static void setConstraintForceMixing(double cfm); + + /// Get global constraint force mixing parameter + static double getConstraintForceMixing(); + + // Friendship + friend class ConstraintSolver; + friend class ConstrainedGroup; + +protected: + // Documentation inherited void update() override; - void applyImpulse() override; + + // Documentation inherited + void getInformation(ConstraintInfo* lcp) override; + + // Documentation inherited + void applyUnitImpulse(std::size_t index) override; + + // Documentation inherited + void getVelocityChange(double* delVel, bool withCfm) override; + + // Documentation inherited + void excite() override; + + // Documentation inherited + void unexcite() override; + + // Documentation inherited + void applyImpulse(double* lambda) override; + + // Documentation inherited + dynamics::SkeletonPtr getRootSkeleton() const override; + + // Documentation inherited + bool isActive() const override; private: - dynamics::BodyNode* mBodyNode1; - dynamics::BodyNode* mBodyNode2; - double mRatio; - Eigen::Vector6d mImpulse; + /// Dependent joint whose motion is influenced by the reference joint. + dynamics::Joint* mJoint; + + /// Vector of MimicDofProperties for the dependent joint. + std::vector mMimicProps; + + /// BodyNode associated with the dependent joint. + dynamics::BodyNode* mBodyNode; + + /// Index of the applied impulse for the dependent joint. + std::size_t mAppliedImpulseIndex; + + /// Array storing the lifetime of each constraint (in iterations). + std::size_t mLifeTime[6]; + + /// Array indicating whether each constraint is active or not. + bool mActive[6]; + + /// Array storing the negative velocity errors for each constraint. + double mNegativeVelocityError[6]; + + /// Array storing the previous values of the constraint forces. + double mOldX[6]; + + /// Array storing the upper bounds for the constraint forces. + double mUpperBound[6]; + + /// Array storing the lower bounds for the constraint forces. + double mLowerBound[6]; + + /// Global constraint force mixing parameter in the range of [1e-9, 1]. The + /// default is 1e-5 + /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0 + static double mConstraintForceMixing; }; } // namespace constraint } // namespace dart #endif // DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ -