diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 979d144997d8a..23200875515ac 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -44,6 +44,7 @@ #include "dart/constraint/ConstrainedGroup.hpp" #include "dart/constraint/ContactConstraint.hpp" #include "dart/constraint/ContactSurface.hpp" +#include "dart/constraint/CouplerConstraint.hpp" #include "dart/constraint/JointConstraint.hpp" #include "dart/constraint/JointCoulombFrictionConstraint.hpp" #include "dart/constraint/LCPSolver.hpp" @@ -493,6 +494,7 @@ void ConstraintSolver::updateConstraints() // Destroy previous joint constraints mJointConstraints.clear(); mMimicMotorConstraints.clear(); + mCouplerConstraints.clear(); mJointCoulombFrictionConstraints.clear(); // Create new joint constraints @@ -520,8 +522,14 @@ void ConstraintSolver::updateConstraints() if (joint->getActuatorType() == dynamics::Joint::MIMIC && joint->getMimicJoint()) { - mMimicMotorConstraints.push_back(std::make_shared( - joint, joint->getMimicDofProperties())); + if (joint->isUsingCouplerConstraint()) { + mCouplerConstraints.push_back(std::make_shared( + joint, joint->getMimicDofProperties())); + } else { + mMimicMotorConstraints.push_back( + std::make_shared( + joint, joint->getMimicDofProperties())); + } } } } @@ -541,6 +549,13 @@ void ConstraintSolver::updateConstraints() mActiveConstraints.push_back(mimicMotorConstraint); } + for (auto& couplerConstraint : mCouplerConstraints) { + couplerConstraint->update(); + + if (couplerConstraint->isActive()) + mActiveConstraints.push_back(couplerConstraint); + } + for (auto& jointFrictionConstraint : mJointCoulombFrictionConstraints) { jointFrictionConstraint->update(); diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp index 3403ea42079c7..db4c18666d122 100644 --- a/dart/constraint/ConstraintSolver.hpp +++ b/dart/constraint/ConstraintSolver.hpp @@ -263,6 +263,9 @@ class DART_API ConstraintSolver /// Mimic motor constraints those are automatically created std::vector mMimicMotorConstraints; + /// Coupler constraints that are automatically created + std::vector mCouplerConstraints; + /// Joint Coulomb friction constraints those are automatically created std::vector mJointCoulombFrictionConstraints; diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp new file mode 100644 index 0000000000000..1b87cfc670f90 --- /dev/null +++ b/dart/constraint/CouplerConstraint.cpp @@ -0,0 +1,369 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "dart/constraint/CouplerConstraint.hpp" + +#include "dart/common/Logging.hpp" +#include "dart/common/Macros.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Joint.hpp" +#include "dart/dynamics/Skeleton.hpp" + +#define DART_CFM 1e-9 + +namespace dart { +namespace constraint { + +double CouplerConstraint::mConstraintForceMixing = DART_CFM; + +//============================================================================== +CouplerConstraint::CouplerConstraint( + dynamics::Joint* joint, + const std::vector& mimicDofProperties) + : ConstraintBase(), + mJoint(joint), + mMimicProps(mimicDofProperties), + mBodyNode(joint->getChildBodyNode()), + mAppliedImpulseIndex(0) +{ + DART_ASSERT(joint); + DART_ASSERT(joint->getNumDofs() <= mMimicProps.size()); + DART_ASSERT(mBodyNode); + + std::fill(mLifeTime, mLifeTime + 6, 0); + std::fill(mActive, mActive + 6, false); +} + +//============================================================================== +CouplerConstraint::~CouplerConstraint() +{ + // Do nothing +} + +//============================================================================== +const std::string& CouplerConstraint::getType() const +{ + return getStaticType(); +} + +//============================================================================== +const std::string& CouplerConstraint::getStaticType() +{ + static const std::string name = "CouplerConstraint"; + return name; +} + +//============================================================================== +void CouplerConstraint::setConstraintForceMixing(double cfm) +{ + double clamped = cfm; + if (clamped < 1e-9) { + DART_WARN( + "[CouplerConstraint::setConstraintForceMixing] Constraint force " + "mixing parameter[{}] is lower than 1e-9. It is set to 1e-9.", + cfm); + clamped = 1e-9; + } + + mConstraintForceMixing = clamped; +} + +//============================================================================== +double CouplerConstraint::getConstraintForceMixing() +{ + return mConstraintForceMixing; +} + +//============================================================================== +void CouplerConstraint::update() +{ + // 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; + + DART_ASSERT(lcp->w[index] == 0.0); + + lcp->b[index] = mNegativeVelocityError[i]; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; + + DART_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) +{ + DART_ASSERT(index < mDim && "Invalid Index."); + + std::size_t localIndex = 0; + const dynamics::SkeletonPtr& dependentSkeleton = 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]; + dynamics::Joint* referenceJoint + = const_cast(mimicProp.mReferenceJoint); + dynamics::Skeleton* referenceSkeleton = const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()); + dynamics::BodyNode* referenceBodyNode = const_cast( + mimicProp.mReferenceJoint->getChildBodyNode()); + + DART_ASSERT(referenceJoint != nullptr); + DART_ASSERT(referenceSkeleton != nullptr); + DART_ASSERT(referenceBodyNode != nullptr); + + dependentSkeleton->clearConstraintImpulses(); + if (referenceSkeleton != dependentSkeleton.get()) + referenceSkeleton->clearConstraintImpulses(); + + double impulse = 1.0; + mJoint->setConstraintImpulse(i, impulse); + + referenceJoint->setConstraintImpulse( + mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); + + dependentSkeleton->updateBiasImpulse(mBodyNode); + referenceSkeleton->updateBiasImpulse(referenceBodyNode); + + dependentSkeleton->updateVelocityChange(); + referenceSkeleton->updateVelocityChange(); + + mJoint->setConstraintImpulse(i, 0.0); + referenceJoint->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); + } + + ++localIndex; + } + + mAppliedImpulseIndex = index; +} + +//============================================================================== +void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) +{ + DART_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; + } + + DART_ASSERT(localIndex == mDim); +} + +//============================================================================== +void CouplerConstraint::excite() +{ + mJoint->getSkeleton()->setImpulseApplied(true); + for (const auto& mimicProp : mMimicProps) { + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(true); + } +} + +//============================================================================== +void CouplerConstraint::unexcite() +{ + mJoint->getSkeleton()->setImpulseApplied(false); + for (const auto& mimicProp : mMimicProps) { + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(false); + } +} + +//============================================================================== +void CouplerConstraint::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) + continue; + + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + + auto& mimicProp = mMimicProps[i]; + dynamics::Joint* referenceJoint + = const_cast(mimicProp.mReferenceJoint); + DART_ASSERT(referenceJoint != nullptr); + referenceJoint->setConstraintImpulse( + mimicProp.mReferenceDofIndex, + referenceJoint->getConstraintImpulse(mimicProp.mReferenceDofIndex) + - lambda[localIndex] * mimicProp.mMultiplier); + + mOldX[i] = lambda[localIndex]; + + ++localIndex; + } +} + +//============================================================================== +dynamics::SkeletonPtr CouplerConstraint::getRootSkeleton() const +{ + return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton()); +} + +//============================================================================== +void CouplerConstraint::uniteSkeletons() +{ + if (mJoint == nullptr || mBodyNode == nullptr) + return; + + if (!mBodyNode->isReactive()) + return; + + auto dependentSkeleton = mJoint->getSkeleton(); + if (!dependentSkeleton) + return; + + auto dependentRoot = ConstraintBase::compressPath(dependentSkeleton); + + for (const auto& mimicProp : mMimicProps) { + if (mimicProp.mReferenceJoint == nullptr) + continue; + + auto referenceBody = mimicProp.mReferenceJoint->getChildBodyNode(); + if (referenceBody == nullptr || !referenceBody->isReactive()) + continue; + + auto referenceSkeletonConst = mimicProp.mReferenceJoint->getSkeleton(); + if (!referenceSkeletonConst) + continue; + + auto referenceSkeleton + = std::const_pointer_cast(referenceSkeletonConst); + if (!referenceSkeleton) + continue; + + if (referenceSkeleton == dependentSkeleton) + continue; + + auto referenceRoot = ConstraintBase::compressPath(referenceSkeleton); + dependentRoot = ConstraintBase::compressPath(dependentSkeleton); + + if (dependentRoot == referenceRoot) + continue; + + if (dependentRoot->mUnionSize < referenceRoot->mUnionSize) { + dependentRoot->mUnionRootSkeleton = referenceRoot; + referenceRoot->mUnionSize += dependentRoot->mUnionSize; + dependentRoot = referenceRoot; + dependentSkeleton = referenceRoot; + } else { + referenceRoot->mUnionRootSkeleton = dependentRoot; + dependentRoot->mUnionSize += referenceRoot->mUnionSize; + } + } +} + +//============================================================================== +bool CouplerConstraint::isActive() const +{ + if (mJoint->getActuatorType() == dynamics::Joint::MIMIC) + return true; + + return false; +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp new file mode 100644 index 0000000000000..be1d26003be59 --- /dev/null +++ b/dart/constraint/CouplerConstraint.hpp @@ -0,0 +1,155 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ +#define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ + +#include + +#include + +#include + +#include + +namespace dart { +namespace constraint { + +/// CouplerConstraint enforces the mimic relationship between two joints by +/// applying equal-and-opposite impulses based on MimicDofProperties. Unlike +/// MimicMotorConstraint, which behaves like a servo that only drives the +/// dependent joint, CouplerConstraint is bilateral: both the reference and +/// dependent joints participate in the constraint solve, so any impulse on one +/// joint is reflected on the other joint. +class DART_API CouplerConstraint : public ConstraintBase +{ +public: + /// 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); + + /// 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; + + // 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; + + // Documentation inherited + void uniteSkeletons() override; + +private: + /// 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_ diff --git a/dart/constraint/Fwd.hpp b/dart/constraint/Fwd.hpp index bce535a223f19..7452074f447c3 100644 --- a/dart/constraint/Fwd.hpp +++ b/dart/constraint/Fwd.hpp @@ -45,6 +45,7 @@ class BoxedLcpSolver; class ConstrainedGroup; class ConstraintBase; class ConstraintSolver; +class CouplerConstraint; class ContactConstraint; struct ContactSurfaceParams; @@ -82,6 +83,7 @@ DART_COMMON_DECLARE_SHARED_WEAK(ContactSurfaceHandler) DART_COMMON_DECLARE_SHARED_WEAK(SoftContactConstraint) DART_COMMON_DECLARE_SHARED_WEAK(JointConstraint) DART_COMMON_DECLARE_SHARED_WEAK(MimicMotorConstraint) +DART_COMMON_DECLARE_SHARED_WEAK(CouplerConstraint) DART_COMMON_DECLARE_SHARED_WEAK(JointCoulombFrictionConstraint) DART_COMMON_DECLARE_SHARED_WEAK(LCPSolver) diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp index 2dc00dcef61fe..931f54af11a03 100644 --- a/dart/constraint/MimicMotorConstraint.cpp +++ b/dart/constraint/MimicMotorConstraint.cpp @@ -99,16 +99,16 @@ const std::string& MimicMotorConstraint::getStaticType() //============================================================================== void MimicMotorConstraint::setConstraintForceMixing(double cfm) { - // Clamp constraint force mixing parameter if it is out of the range - if (cfm < 1e-9) { + double clamped = cfm; + if (clamped < 1e-9) { DART_WARN( "Constraint force mixing parameter[{}] is lower than 1e-9. It is set " "to 1e-9.", cfm); - mConstraintForceMixing = 1e-9; + clamped = 1e-9; } - mConstraintForceMixing = cfm; + mConstraintForceMixing = clamped; } //============================================================================== diff --git a/dart/constraint/MimicMotorConstraint.hpp b/dart/constraint/MimicMotorConstraint.hpp index ddab7c8a9c9ae..4f7c32f6d0859 100644 --- a/dart/constraint/MimicMotorConstraint.hpp +++ b/dart/constraint/MimicMotorConstraint.hpp @@ -48,7 +48,11 @@ class Joint; namespace constraint { -/// Servo motor constraint +/// MimicMotorConstraint behaves like a servo motor: it drives only the +/// dependent joint toward the reference joint's trajectory by applying +/// impulses locally. The reference joint does not receive an equal and +/// opposite impulse. Use CouplerConstraint instead when you need a bilateral +/// constraint that reacts on both joints. class MimicMotorConstraint : public ConstraintBase { public: diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 2dd4097298bdd..9f30a924edab8 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -84,6 +84,7 @@ JointProperties::JointProperties( prop.mReferenceDofIndex = i; prop.mMultiplier = _mimicMultiplier; prop.mOffset = _mimicOffset; + prop.mConstraintType = MimicConstraintType::Motor; } } @@ -126,8 +127,8 @@ void Joint::setAspectProperties(const AspectProperties& properties) setTransformFromParentBodyNode(properties.mT_ParentBodyToJoint); setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); setLimitEnforcement(properties.mIsPositionLimitEnforced); - setActuatorType(properties.mActuatorType); setMimicJointDofs(properties.mMimicDofProps); + setActuatorType(properties.mActuatorType); } //============================================================================== @@ -213,6 +214,7 @@ Joint::ActuatorType Joint::getActuatorType() const void Joint::setMimicJoint( const Joint* referenceJoint, double mimicMultiplier, double mimicOffset) { + const auto constraintType = getMimicConstraintType(); std::size_t numDofs = getNumDofs(); mAspectProperties.mMimicDofProps.resize(numDofs); @@ -222,6 +224,7 @@ void Joint::setMimicJoint( prop.mReferenceDofIndex = i; prop.mMultiplier = mimicMultiplier; prop.mOffset = mimicOffset; + prop.mConstraintType = constraintType; setMimicJointDof(i, prop); } } @@ -274,6 +277,34 @@ const std::vector& Joint::getMimicDofProperties() const return mAspectProperties.mMimicDofProps; } +//============================================================================== +void Joint::setMimicConstraintType(MimicConstraintType type) +{ + for (auto& prop : mAspectProperties.mMimicDofProps) + prop.mConstraintType = type; +} + +//============================================================================== +MimicConstraintType Joint::getMimicConstraintType() const +{ + if (mAspectProperties.mMimicDofProps.empty()) + return MimicConstraintType::Motor; + return mAspectProperties.mMimicDofProps.front().mConstraintType; +} + +//============================================================================== +void Joint::setUseCouplerConstraint(bool enable) +{ + setMimicConstraintType( + enable ? MimicConstraintType::Coupler : MimicConstraintType::Motor); +} + +//============================================================================== +bool Joint::isUsingCouplerConstraint() const +{ + return getMimicConstraintType() == MimicConstraintType::Coupler; +} + //============================================================================== bool Joint::isKinematic() const { diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index f3996eec47591..6b6d45bf5b53d 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -178,6 +178,18 @@ class DART_API Joint /// joint. const std::vector& getMimicDofProperties() const; + /// Sets the constraint type used to enforce the mimic relationship. + void setMimicConstraintType(MimicConstraintType type); + + /// Returns the constraint type used to enforce the mimic relationship. + MimicConstraintType getMimicConstraintType() const; + + /// Enable or disable the CouplerConstraint enforcement for this joint. + void setUseCouplerConstraint(bool enable); + + /// Returns true if the CouplerConstraint enforcement is enabled. + bool isUsingCouplerConstraint() const; + /// Return true if this joint is kinematic joint. /// /// Kinematic joint means the motion is prescribed by position or velocity or diff --git a/dart/dynamics/MimicDofProperties.hpp b/dart/dynamics/MimicDofProperties.hpp index 8fd9ca799c8c6..742e7c90be7b5 100644 --- a/dart/dynamics/MimicDofProperties.hpp +++ b/dart/dynamics/MimicDofProperties.hpp @@ -40,6 +40,13 @@ namespace dynamics { class Joint; +/// Type of coupling constraint used for a mimic joint. +enum class MimicConstraintType +{ + Motor, + Coupler +}; + /// Properties of a mimicked Degree of Freedom (DoF) in a joint. struct MimicDofProperties { @@ -54,6 +61,9 @@ struct MimicDofProperties /// Offset added to the reference joint's DoF value. double mOffset = 0.0; + + /// Coupling behavior used to enforce the mimic relationship. + MimicConstraintType mConstraintType = MimicConstraintType::Motor; }; } // namespace dynamics diff --git a/docs/onboarding/README.md b/docs/onboarding/README.md index 65cb036d2b404..290c8436f3ffe 100644 --- a/docs/onboarding/README.md +++ b/docs/onboarding/README.md @@ -499,7 +499,8 @@ graph TB **Key Elements**: - [`ConstraintSolver::solve()`](dart/constraint/ConstraintSolver.cpp#L159) - Main constraint solving loop - [`ConstraintSolver::addConstraint()`](dart/constraint/ConstraintSolver.cpp#L86) - Register constraint -- Constraint types: Contact, JointLimit, Motor, Servo, Mimic, custom +- Constraint types: Contact, JointLimit, Motor, Servo, Mimic (MimicMotor or + Coupler), custom - LCP solvers: Dantzig (primary), PGS (fallback) - Skeleton grouping for independent constraint solving @@ -973,6 +974,7 @@ sequenceDiagram - `JointLimitConstraint` - Joint position/velocity limits - `ServoMotorConstraint` - Joint servo control - `MimicMotorConstraint` - Mimic joint behavior +- `CouplerConstraint` - Bilateral mimic coupling (equal-and-opposite impulses) - Custom constraints **Key Fields**: diff --git a/docs/onboarding/architecture.md b/docs/onboarding/architecture.md index 3804bd587ac7c..4f12d226b7e64 100644 --- a/docs/onboarding/architecture.md +++ b/docs/onboarding/architecture.md @@ -466,7 +466,8 @@ ConstraintSolver **Joint Constraints:** - **JointLimitConstraint** - Position/velocity limit enforcement - **ServoMotorConstraint** - PD control as constraint -- **MimicMotorConstraint** - Joint coupling +- **MimicMotorConstraint** - Joint coupling (unilateral servo) +- **CouplerConstraint** - Bilateral mimic coupling (equal/opposite impulses) **Specialized Constraints:** - **WeldJointConstraint** - Rigid connection between bodies diff --git a/docs/onboarding/constraints.md b/docs/onboarding/constraints.md index 31a72ce0511d3..332c9f99915c8 100644 --- a/docs/onboarding/constraints.md +++ b/docs/onboarding/constraints.md @@ -24,6 +24,7 @@ This document provides a comprehensive analysis of the constraint subsystem in t - JointCoulombFrictionConstraint.hpp/cpp - Joint friction - ServoMotorConstraint.hpp/cpp - Servo motor control - MimicMotorConstraint.hpp/cpp - Mimic motor behavior +- CouplerConstraint.hpp/cpp - Bilateral mimic coupling - BalanceConstraint.hpp/cpp - Balance constraints - BallJointConstraint.hpp/cpp - Ball joint constraints - WeldJointConstraint.hpp/cpp - Weld joint constraints @@ -109,7 +110,7 @@ struct ConstraintInfo { - Soft contact constraints - Joint limit constraints - Joint Coulomb friction constraints - - Mimic motor constraints + - Mimic joint constraints (CouplerConstraint or MimicMotorConstraint) 2. **Manual Constraints** (user-added): - Custom constraints added via `addConstraint()` @@ -250,6 +251,27 @@ where each x[i], w[i] satisfies: **Use Case:** Enables PD-style control through constraint framework. +#### CouplerConstraint + +**File:** `dart/constraint/CouplerConstraint.hpp` + +**Role:** Bilaterally enforces a mimic relationship between two joints by +applying equal-and-opposite impulses. Unlike `MimicMotorConstraint`, both the +reference joint and the dependent joint participate in the constraint solve, so +reaction torques propagate through the articulated system. + +**Key Features:** +- Shares the same `MimicDofProperties` multipliers/offsets as mimic motors +- Activated by `Joint::setUseCouplerConstraint(true)` (per mimic joint) +- Couples multiple skeletons when reference and dependent joints belong to + different rigs +- Uses the same LCP infrastructure (lifetime, impulse bounds, CFM/Coulomb + handling) as the other joint constraints + +**Recommended Use:** Gearbox-style mimic joints that require reaction forces +on both shafts (e.g., Gazebo gearbox joints). Leave the flag disabled to keep +the legacy servo-style behavior when bilateral coupling is not required. + ### 6. LCP Solvers #### BoxedLcpSolver Interface @@ -324,7 +346,7 @@ ConstraintSolver::solve() | ├─> Detect collisions → ContactConstraints | ├─> Check joint limits → JointLimitConstraints | ├─> Check joint friction → JointCoulombFrictionConstraints - | ├─> Check mimic motors → MimicMotorConstraints + | ├─> Check mimic joints → CouplerConstraints or MimicMotorConstraints | └─> Update manual constraints | ├─> [2] buildConstrainedGroups() diff --git a/docs/onboarding/dynamics.md b/docs/onboarding/dynamics.md index 1adc308a2216f..b1257f4ff6aa5 100644 --- a/docs/onboarding/dynamics.md +++ b/docs/onboarding/dynamics.md @@ -206,6 +206,10 @@ This document provides an exploration of the core dynamics classes in DART (Dyna - Allows a joint to mimic the motion of another joint - Supports different multipliers and offsets per DOF - Useful for coupled mechanisms +- `Joint::setUseCouplerConstraint(true)` swaps the legacy servo-style mimic + motor for a bilateral `CouplerConstraint`, so both the reference and + dependent joints feel equal-and-opposite impulses. Leave it disabled to keep + the original MimicMotorConstraint behavior. **Internal Update Methods:** - `updateRelativeTransform()`, `updateRelativeSpatialVelocity()` diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 430bdad056539..faaecab3814c5 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -13,6 +13,7 @@ add_subdirectory(atlas_simbicon) add_subdirectory(biped_stand) add_subdirectory(box_stacking) add_subdirectory(boxes) +add_subdirectory(coupler_constraint) add_subdirectory(drag_and_drop) add_subdirectory(empty) add_subdirectory(hardcoded_design) diff --git a/examples/coupler_constraint/CMakeLists.txt b/examples/coupler_constraint/CMakeLists.txt new file mode 100644 index 0000000000000..d043fe1dd3438 --- /dev/null +++ b/examples/coupler_constraint/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.22.1) + +get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) + +project(${example_name}) + +# Aggregate dependencies +set(required_components gui) +set(required_libraries dart dart-gui) + +if(DART_IN_SOURCE_BUILD) + dart_build_example_in_source(${example_name} LINK_LIBRARIES ${required_libraries}) + return() +endif() + +find_package(DART 6.15.0 REQUIRED COMPONENTS ${required_components} CONFIG) + +file(GLOB srcs "*.cpp" "*.hpp") +add_executable(${example_name} ${srcs}) +target_link_libraries(${example_name} PUBLIC ${required_libraries}) +target_include_directories(${example_name} PUBLIC ${DART_INCLUDE_DIRS}) diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp new file mode 100644 index 0000000000000..7abaa86f8a99e --- /dev/null +++ b/examples/coupler_constraint/main.cpp @@ -0,0 +1,755 @@ +/* + * Copyright (c) 2011-2025, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/main/LICENSE + * + * This file is provided under the following "BSD-style" License: + * Redistribution and use in source and binary forms, with or + * without modification, are permitted provided that the following + * conditions are met: + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND + * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR + * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF + * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +using dart::dynamics::BodyNode; +using dart::dynamics::BodyNodePtr; +using dart::dynamics::Joint; +using dart::dynamics::RevoluteJoint; +using dart::dynamics::Skeleton; +using dart::dynamics::SkeletonPtr; +using dart::simulation::World; +using dart::simulation::WorldPtr; + +namespace { + +struct MimicAssembly +{ + dart::dynamics::SkeletonPtr skeleton; + dart::dynamics::Joint* referenceJoint; + dart::dynamics::Joint* followerJoint; + dart::dynamics::BodyNode* referenceBody; + dart::dynamics::BodyNode* followerBody; + dart::dynamics::SimpleFramePtr lowerLimitGuide; + dart::dynamics::SimpleFramePtr upperLimitGuide; +}; + +dart::dynamics::SimpleFramePtr createLimitGuide( + const std::string& name, + dart::dynamics::Joint* followerJoint, + double angle, + const Eigen::Vector3d& color, + const dart::simulation::WorldPtr& world) +{ + using namespace dart::dynamics; + if (!world || !followerJoint) + return nullptr; + + const auto* revolute + = dynamic_cast(followerJoint); + if (revolute == nullptr) + return nullptr; + + auto frame = SimpleFrame::createShared(Frame::World(), name); + auto line = std::make_shared(0.05f); + line->addVertex(Eigen::Vector3d::Zero()); + line->addVertex(Eigen::Vector3d::UnitX() * 0.65); + line->addConnection(0, 1); + frame->setShape(line); + auto visual = frame->createVisualAspect(); + visual->setColor(color); + + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + if (const auto* parent = followerJoint->getParentBodyNode()) + tf = parent->getWorldTransform(); + tf = tf * followerJoint->getTransformFromParentBodyNode(); + tf.rotate(Eigen::AngleAxisd(angle, revolute->getAxis())); + tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); + frame->setRelativeTransform(tf); + + world->addSimpleFrame(frame); + return frame; +} + +MimicAssembly createMimicAssembly( + const std::string& label, + const Eigen::Vector3d& baseOffset, + bool useCouplerConstraint, + const Eigen::Vector3d& referenceColor, + const Eigen::Vector3d& followerColor, + const dart::simulation::WorldPtr& world) +{ + SkeletonPtr skeleton = Skeleton::create(label + "_rig"); + + const std::string baseJointName = label + "_base_joint"; + const std::string baseBodyName = label + "_base"; + const std::string referenceJointName = label + "_reference_joint"; + const std::string referenceBodyName = label + "_reference_body"; + const std::string followerJointName = label + "_follower_joint"; + const std::string followerBodyName = label + "_follower_body"; + + dart::dynamics::WeldJoint::Properties baseJointProps; + baseJointProps.mName = baseJointName; + baseJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(baseOffset); + + BodyNode::Properties baseBodyProps; + baseBodyProps.mName = baseBodyName; + dart::dynamics::Inertia baseInertia; + baseInertia.setMass(0.1); + baseInertia.setMoment(1e-4, 1e-4, 1e-4, 0.0, 0.0, 0.0); + baseBodyProps.mInertia = baseInertia; + + auto basePair + = skeleton->createJointAndBodyNodePair( + nullptr, baseJointProps, baseBodyProps); + auto* baseBody = basePair.second; + + RevoluteJoint::Properties referenceJointProps; + referenceJointProps.mName = referenceJointName; + referenceJointProps.mAxis = Eigen::Vector3d::UnitZ(); + referenceJointProps.mT_ParentBodyToJoint + = Eigen::Translation3d(0.0, 0.15, 0.0); + referenceJointProps.mT_ChildBodyToJoint + = Eigen::Translation3d(-0.25, 0.0, 0.0); + + BodyNode::Properties referenceBodyProps; + referenceBodyProps.mName = referenceBodyName; + dart::dynamics::Inertia referenceInertia; + referenceInertia.setMass(1.0); + referenceInertia.setMoment(0.02, 0.02, 0.03, 0.0, 0.0, 0.0); + referenceBodyProps.mInertia = referenceInertia; + + auto referencePair = skeleton->createJointAndBodyNodePair( + baseBody, referenceJointProps, referenceBodyProps); + auto* referenceJoint = referencePair.first; + auto* referenceBody = referencePair.second; + + referenceJoint->setActuatorType(Joint::FORCE); + referenceJoint->setForceLowerLimit(0, -120.0); + referenceJoint->setForceUpperLimit(0, 120.0); + referenceJoint->setDampingCoefficient(0, 0.02); + + auto referenceShape = std::make_shared( + Eigen::Vector3d(0.5, 0.05, 0.05)); + auto referenceShapeNode = referenceBody->createShapeNodeWith< + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(referenceShape); + referenceShapeNode->setRelativeTranslation(Eigen::Vector3d(0.25, 0.0, 0.0)); + referenceShapeNode->getVisualAspect()->setColor(referenceColor); + + RevoluteJoint::Properties followerJointProps; + followerJointProps.mName = followerJointName; + followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); + followerJointProps.mT_ParentBodyToJoint + = Eigen::Translation3d(0.0, -0.15, 0.0); + followerJointProps.mT_ChildBodyToJoint + = Eigen::Translation3d(-0.25, 0.0, 0.0); + + BodyNode::Properties followerBodyProps; + followerBodyProps.mName = followerBodyName; + dart::dynamics::Inertia followerInertia; + followerInertia.setMass(1.0); + followerInertia.setMoment(0.02, 0.02, 0.03, 0.0, 0.0, 0.0); + followerBodyProps.mInertia = followerInertia; + + auto followerPair = skeleton->createJointAndBodyNodePair( + baseBody, followerJointProps, followerBodyProps); + auto* followerJoint = followerPair.first; + auto* followerBody = followerPair.second; + + followerJoint->setActuatorType(Joint::MIMIC); + followerJoint->setMimicJoint(referenceJoint, -1.0, 0.0); + followerJoint->setUseCouplerConstraint(useCouplerConstraint); + followerJoint->setForceLowerLimit(0, -120.0); + followerJoint->setForceUpperLimit(0, 120.0); + followerJoint->setDampingCoefficient(0, 0.02); + followerJoint->setPositionLowerLimit(0, -0.35); + followerJoint->setPositionUpperLimit(0, 0.35); + followerJoint->setLimitEnforcement(true); + + auto followerShape = std::make_shared( + Eigen::Vector3d(0.5, 0.05, 0.05)); + auto followerShapeNode = followerBody->createShapeNodeWith< + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(followerShape); + followerShapeNode->setRelativeTranslation(Eigen::Vector3d(0.25, 0.0, 0.0)); + followerShapeNode->getVisualAspect()->setColor(followerColor); + + referenceJoint->setPosition(0, 0.0); + followerJoint->setPosition(0, 0.0); + + MimicAssembly assembly{ + skeleton, + referenceJoint, + followerJoint, + referenceBody, + followerBody, + nullptr, + nullptr, + }; + + if (world) { + const double lower = followerJoint->getPositionLowerLimit(0); + const double upper = followerJoint->getPositionUpperLimit(0); + assembly.lowerLimitGuide = createLimitGuide( + label + "_lower_limit", + followerJoint, + lower, + Eigen::Vector3d(0.92, 0.35, 0.35), + world); + assembly.upperLimitGuide = createLimitGuide( + label + "_upper_limit", + followerJoint, + upper, + Eigen::Vector3d(0.35, 0.92, 0.35), + world); + } + + return assembly; +} + +class CouplerController +{ +public: + struct PairRegistration + { + std::string label; + bool usesCoupler; + dart::dynamics::SkeletonPtr skeleton; + dart::dynamics::Joint* referenceJoint; + dart::dynamics::Joint* followerJoint; + dart::dynamics::BodyNode* referenceBody; + dart::dynamics::BodyNode* followerBody; + dart::dynamics::SimpleFramePtr linkFrame; + dart::dynamics::LineSegmentShapePtr linkShape; + dart::dynamics::VisualAspect* linkVisual; + double targetAngle{0.0}; + double torqueLimit{80.0}; + double proportionalGain{300.0}; + double dampingGain{20.0}; + }; + + struct PairStatus + { + std::string label; + bool usesCoupler; + bool couplerEnabled; + double targetAngle; + double referencePosition; + double followerPosition; + double desiredFollowerPosition; + double positionError; + double followerLowerLimit; + double followerUpperLimit; + bool followerAtLowerLimit; + bool followerAtUpperLimit; + }; + + void addPair(PairRegistration registration) + { + PairData pair; + pair.label = registration.label; + pair.usesCoupler = registration.usesCoupler; + pair.couplerEnabled + = registration.usesCoupler + ? registration.followerJoint->isUsingCouplerConstraint() + : false; + pair.skeleton = std::move(registration.skeleton); + pair.referenceJoint = registration.referenceJoint; + pair.followerJoint = registration.followerJoint; + pair.referenceBody = registration.referenceBody; + pair.followerBody = registration.followerBody; + pair.linkFrame = std::move(registration.linkFrame); + pair.linkShape = std::move(registration.linkShape); + pair.linkVisual = registration.linkVisual; + pair.initialPositions = pair.skeleton->getPositions(); + pair.targetAngle = registration.targetAngle; + pair.torqueLimit = registration.torqueLimit; + pair.kp = registration.proportionalGain; + pair.kd = registration.dampingGain; + + mPairs.push_back(std::move(pair)); + refreshPairVisual(mPairs.back()); + } + + void update() + { + for (auto& pair : mPairs) { + driveReferenceJoint(pair); + refreshPairVisual(pair); + } + } + + void reset() + { + for (auto& pair : mPairs) { + if (!pair.skeleton) + continue; + pair.skeleton->setPositions(pair.initialPositions); + pair.skeleton->setVelocities( + Eigen::VectorXd::Zero(pair.skeleton->getNumDofs())); + pair.skeleton->clearExternalForces(); + } + } + + std::vector getStatuses() const + { + std::vector result; + result.reserve(mPairs.size()); + + for (const auto& pair : mPairs) { + PairStatus status; + status.label = pair.label; + status.usesCoupler = pair.usesCoupler; + status.couplerEnabled = pair.couplerEnabled; + status.targetAngle = pair.targetAngle; + status.referencePosition + = pair.referenceJoint ? pair.referenceJoint->getPosition(0) : 0.0; + status.followerPosition + = pair.followerJoint ? pair.followerJoint->getPosition(0) : 0.0; + status.desiredFollowerPosition + = pair.followerJoint ? computeDesiredFollowerPosition(pair) : 0.0; + status.positionError + = status.followerPosition - status.desiredFollowerPosition; + status.followerLowerLimit + = pair.followerJoint ? pair.followerJoint->getPositionLowerLimit(0) + : 0.0; + status.followerUpperLimit + = pair.followerJoint ? pair.followerJoint->getPositionUpperLimit(0) + : 0.0; + const double lowerSlack + = status.followerPosition - status.followerLowerLimit; + const double upperSlack + = status.followerUpperLimit - status.followerPosition; + status.followerAtLowerLimit = lowerSlack < 1e-3; + status.followerAtUpperLimit = upperSlack < 1e-3; + + result.push_back(status); + } + + return result; + } + +private: + struct PairData + { + std::string label; + bool usesCoupler{false}; + bool couplerEnabled{false}; + dart::dynamics::SkeletonPtr skeleton; + dart::dynamics::Joint* referenceJoint{nullptr}; + dart::dynamics::Joint* followerJoint{nullptr}; + dart::dynamics::BodyNode* referenceBody{nullptr}; + dart::dynamics::BodyNode* followerBody{nullptr}; + dart::dynamics::SimpleFramePtr linkFrame; + dart::dynamics::LineSegmentShapePtr linkShape; + dart::dynamics::VisualAspect* linkVisual{nullptr}; + Eigen::VectorXd initialPositions; + double targetAngle{0.0}; + double torqueLimit{80.0}; + double kp{300.0}; + double kd{20.0}; + }; + + double computeDesiredFollowerPosition(const PairData& pair) const + { + if (pair.followerJoint == nullptr) + return 0.0; + + const auto& props = pair.followerJoint->getMimicDofProperties()[0]; + const auto* refJoint = props.mReferenceJoint; + if (refJoint == nullptr) + return 0.0; + + return refJoint->getPosition(props.mReferenceDofIndex) * props.mMultiplier + + props.mOffset; + } + + void refreshPairVisual(PairData& pair) + { + if (!pair.linkShape || !pair.referenceBody || !pair.followerBody) + return; + + pair.linkShape->setVertex( + 0, pair.referenceBody->getTransform().translation()); + pair.linkShape->setVertex( + 1, pair.followerBody->getTransform().translation()); + + if (!pair.linkVisual) + return; + + const double desired = computeDesiredFollowerPosition(pair); + const double actual + = pair.followerJoint ? pair.followerJoint->getPosition(0) : 0.0; + const double error = actual - desired; + const double severity = std::min(1.0, std::abs(error) * 8.0); + + Eigen::Vector3d color; + if (pair.usesCoupler) { + const Eigen::Vector3d good(0.6, 0.95, 0.4); + const Eigen::Vector3d bad(1.0, 0.6, 0.2); + color = good + severity * (bad - good); + } else { + const Eigen::Vector3d base(0.7, 0.7, 0.85); + const Eigen::Vector3d alert(1.0, 0.4, 0.3); + color = base + severity * (alert - base); + } + pair.linkVisual->setColor(color); + } + + void driveReferenceJoint(PairData& pair) + { + if (pair.referenceJoint == nullptr) + return; + + const double position = pair.referenceJoint->getPosition(0); + const double velocity = pair.referenceJoint->getVelocity(0); + double torque + = pair.kp * (pair.targetAngle - position) - pair.kd * velocity; + torque = dart::math::clip(torque, -pair.torqueLimit, pair.torqueLimit); + pair.referenceJoint->setForce(0, torque); + } + + std::vector mPairs; +}; + +class CouplerWorldNode : public dart::gui::osg::RealTimeWorldNode +{ +public: + CouplerWorldNode(const WorldPtr& world, CouplerController* controller) + : RealTimeWorldNode(world), mController(controller) + { + } + + void customPreStep() override + { + mController->update(); + } + +private: + CouplerController* mController; +}; + +class CouplerEventHandler : public ::osgGA::GUIEventHandler +{ +public: + CouplerEventHandler(CouplerController* controller) : mController(controller) + { + } + + bool handle( + const ::osgGA::GUIEventAdapter& ea, ::osgGA::GUIActionAdapter&) override + { + if (ea.getEventType() != ::osgGA::GUIEventAdapter::KEYDOWN) + return false; + + switch (ea.getKey()) { + case 'r': + mController->reset(); + std::cout << "Reset both rigs to their initial configuration" + << std::endl; + return true; + default: + return false; + } + } + +private: + CouplerController* mController; +}; + +class CouplerOverlay : public dart::gui::osg::ImGuiWidget +{ +public: + CouplerOverlay( + dart::gui::osg::ImGuiViewer* viewer, CouplerController* controller) + : mViewer(viewer), mController(controller) + { + } + + void render() override + { + if (mViewer == nullptr || mController == nullptr) + return; + + ImGui::SetNextWindowPos(ImVec2(12, 12)); + ImGui::SetNextWindowSize(ImVec2(360, 280), ImGuiCond_Once); + ImGui::SetNextWindowBgAlpha(0.55f); + + if (!ImGui::Begin( + "Coupler Constraint", + nullptr, + ImGuiWindowFlags_NoCollapse | ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::End(); + return; + } + + const double radToDeg = 180.0 / dart::math::constantsd::pi(); + const auto statuses = mController->getStatuses(); + + ImGui::TextWrapped( + "Left rig: CouplerConstraint (bilateral). Right rig: Mimic motor " + "(servo). Both reference joints chase the same command while the " + "followers are limited to ±20°."); + ImGui::Separator(); + + for (const auto& status : statuses) { + ImGui::Separator(); + ImGui::TextColored( + status.usesCoupler ? ImVec4(0.8f, 1.0f, 0.6f, 1.0f) + : ImVec4(0.9f, 0.7f, 1.0f, 1.0f), + "%s", + status.label.c_str()); + ImGui::Text( + "Constraint: %s", + status.usesCoupler ? "Coupler (bilateral)" : "Mimic motor (servo)"); + ImGui::Text("Reference target: %6.2f deg", status.targetAngle * radToDeg); + ImGui::Text( + "Reference: %6.2f deg", status.referencePosition * radToDeg); + ImGui::Text( + "Follower: %6.2f deg", status.followerPosition * radToDeg); + ImGui::Text( + "Follower limits: [%5.2f, %5.2f] deg", + status.followerLowerLimit * radToDeg, + status.followerUpperLimit * radToDeg); + ImGui::Text( + "Desired mimic: %6.2f deg", + status.desiredFollowerPosition * radToDeg); + ImGui::TextColored( + std::abs(status.positionError) < 1e-3 + ? ImVec4(0.8f, 1.0f, 0.6f, 1.0f) + : ImVec4(1.0f, 0.6f, 0.3f, 1.0f), + "Error: %+6.3f deg", + status.positionError * radToDeg); + if (status.followerAtLowerLimit) { + ImGui::TextColored( + ImVec4(1.0f, 0.7f, 0.3f, 1.0f), "Lower limit engaged"); + } else if (status.followerAtUpperLimit) { + ImGui::TextColored( + ImVec4(1.0f, 0.7f, 0.3f, 1.0f), "Upper limit engaged"); + } + } + + if (ImGui::CollapsingHeader("Controls", ImGuiTreeNodeFlags_DefaultOpen)) { + ImGui::TextWrapped("%s", mViewer->getInstructions().c_str()); + } + + ImGui::End(); + } + +private: + dart::gui::osg::ImGuiViewer* mViewer; + CouplerController* mController; +}; + +} // namespace + +int main(int /*argc*/, char* /*argv*/[]) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3d::Zero()); + world->setTimeStep(1e-3); + + auto couplerAssembly = createMimicAssembly( + "coupler", + Eigen::Vector3d(-0.45, 0.0, 0.0), + true, + Eigen::Vector3d(0.85, 0.35, 0.25), + Eigen::Vector3d(0.25, 0.58, 0.92), + world); + auto motorAssembly = createMimicAssembly( + "motor", + Eigen::Vector3d(0.45, 0.0, 0.0), + false, + Eigen::Vector3d(0.68, 0.32, 0.70), + Eigen::Vector3d(0.25, 0.75, 0.70), + world); + + world->addSkeleton(couplerAssembly.skeleton); + world->addSkeleton(motorAssembly.skeleton); + const double targetAngle + = 45.0 * dart::math::constantsd::pi() / 180.0; // command for references + + std::cout + << "Coupler constraint demo:\n" + << " • Left rig uses the bilateral CouplerConstraint (red/blue).\n" + << " • Right rig uses the legacy MimicMotorConstraint (purple/teal).\n" + << "Both reference joints chase the same command while follower joints\n" + << "are clamped to ±20°. When the follower saturates, only the Coupler\n" + << "propagates the reaction torque back to the reference joint.\n" + << "Press 'r' to reset both rigs to their initial pose.\n" + << std::endl; + + auto controller = std::make_unique(); + + auto couplerFrame = dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World(), "coupler_visual"); + auto couplerLine = std::make_shared(0.06f); + couplerLine->addVertex(Eigen::Vector3d::Zero()); + couplerLine->addVertex(Eigen::Vector3d::Zero()); + couplerLine->addConnection(0, 1); + couplerFrame->setShape(couplerLine); + auto couplerVisual = couplerFrame->createVisualAspect(); + couplerVisual->setColor(Eigen::Vector3d(0.95, 0.95, 0.2)); + world->addSimpleFrame(couplerFrame); + + auto motorFrame = dart::dynamics::SimpleFrame::createShared( + dart::dynamics::Frame::World(), "motor_visual"); + auto motorLine = std::make_shared(0.04f); + motorLine->addVertex(Eigen::Vector3d::Zero()); + motorLine->addVertex(Eigen::Vector3d::Zero()); + motorLine->addConnection(0, 1); + motorFrame->setShape(motorLine); + auto motorVisual = motorFrame->createVisualAspect(); + motorVisual->setColor(Eigen::Vector3d(0.75, 0.75, 0.9)); + world->addSimpleFrame(motorFrame); + + controller->addPair({ + .label = "Coupler pair (left)", + .usesCoupler = true, + .skeleton = couplerAssembly.skeleton, + .referenceJoint = couplerAssembly.referenceJoint, + .followerJoint = couplerAssembly.followerJoint, + .referenceBody = couplerAssembly.referenceBody, + .followerBody = couplerAssembly.followerBody, + .linkFrame = couplerFrame, + .linkShape = couplerLine, + .linkVisual = couplerVisual, + .targetAngle = targetAngle, + .torqueLimit = 90.0, + .proportionalGain = 320.0, + .dampingGain = 25.0, + }); + + controller->addPair({ + .label = "Mimic motor pair (right)", + .usesCoupler = false, + .skeleton = motorAssembly.skeleton, + .referenceJoint = motorAssembly.referenceJoint, + .followerJoint = motorAssembly.followerJoint, + .referenceBody = motorAssembly.referenceBody, + .followerBody = motorAssembly.followerBody, + .linkFrame = motorFrame, + .linkShape = motorLine, + .linkVisual = motorVisual, + .targetAngle = targetAngle, + .torqueLimit = 90.0, + .proportionalGain = 320.0, + .dampingGain = 25.0, + }); + + ::osg::ref_ptr worldNode + = new CouplerWorldNode(world, controller.get()); + ::osg::ref_ptr handler + = new CouplerEventHandler(controller.get()); + + osg::ref_ptr viewer + = new dart::gui::osg::ImGuiViewer(); + if (osg::GraphicsContext::getWindowingSystemInterface() == nullptr) { + std::cerr << "No OSG windowing system detected. Running the GUI example " + "requires an active display server.\n"; + return EXIT_FAILURE; + } + viewer->addWorldNode(worldNode); + viewer->addEventHandler(handler); + + viewer->addInstructionText("space: toggle simulation (auto-starts)\n"); + viewer->addInstructionText("'r': reset both rigs\n"); + std::cout << viewer->getInstructions() << std::endl; + + auto grid = ::osg::ref_ptr( + new dart::gui::osg::GridVisual()); + grid->setPlaneType(dart::gui::osg::GridVisual::PlaneType::XY); + grid->setNumCells(25); + grid->setMinorLineStepSize(0.05); + viewer->addAttachment(grid); + + viewer->setUpViewInWindow(0, 0, 960, 720); + viewer->getCameraManipulator()->setHomePosition( + ::osg::Vec3(1.5f, 1.5f, 1.2f), + ::osg::Vec3(0.4f, 0.0f, 0.2f), + ::osg::Vec3(0.0f, 0.0f, 1.0f)); + viewer->setCameraManipulator(viewer->getCameraManipulator()); + + viewer->simulate(true); + + viewer->getImGuiHandler()->addWidget( + std::make_shared(viewer.get(), controller.get())); + + if (!viewer->isRealized()) + viewer->realize(); + + osg::ref_ptr gc + = viewer->getCamera() ? viewer->getCamera()->getGraphicsContext() + : nullptr; + if (!viewer->isRealized() || !gc || !gc->valid()) { + std::cerr << "Failed to create an OSG window. " + << "Ensure DISPLAY is set or use a virtual framebuffer.\n"; + return EXIT_FAILURE; + } + + const int runResult = viewer->run(); + if (runResult != 0) { + std::cerr << "OSG viewer exited early (status " << runResult + << "). Ensure a valid OpenGL context is available.\n"; + return runResult; + } + + return 0; +} diff --git a/pixi.toml b/pixi.toml index bd6b6e607f762..1283994bbe849 100644 --- a/pixi.toml +++ b/pixi.toml @@ -704,6 +704,36 @@ bm = { depends-on = [ { arg = "build_type", default = "Release" }, ] } +test-joints = { cmd = [ + "bash", + "-lc", + """ +set -euo pipefail +cmake \ + --build build/$PIXI_ENVIRONMENT_NAME/cpp/Release \ + --parallel \ + --target INTEGRATION_dynamics_Joints +./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/INTEGRATION_dynamics_Joints +""", +], depends-on = [ + "config", +] } + +test-joints-coupler = { cmd = [ + "bash", + "-lc", + """ +set -euo pipefail +cmake \ + --build build/$PIXI_ENVIRONMENT_NAME/cpp/Release \ + --parallel \ + --target INTEGRATION_dynamics_Joints +./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/INTEGRATION_dynamics_Joints --gtest_filter=JOINTS.MIMIC_JOINT:JOINTS.MIMIC_JOINT_COUPLER +""", +], depends-on = [ + "config", +] } + bm-boxes = { depends-on = [ { task = "run-cpp-target", args = [ "BM_INTEGRATION_boxes", diff --git a/python/dartpy/dynamics/Joint.cpp b/python/dartpy/dynamics/Joint.cpp index ca2cc40e0037c..c21e74b5d7c14 100644 --- a/python/dartpy/dynamics/Joint.cpp +++ b/python/dartpy/dynamics/Joint.cpp @@ -55,6 +55,11 @@ void Joint(py::module& m) m.attr("DefaultActuatorType") = dart::dynamics::detail::DefaultActuatorType; + ::py::enum_(m, "MimicConstraintType") + .value("Motor", dart::dynamics::MimicConstraintType::Motor) + .value("Coupler", dart::dynamics::MimicConstraintType::Coupler) + .export_values(); + ::py::class_(m, "MimicDofProperties") .def(::py::init<>()) .def_readwrite( @@ -65,7 +70,10 @@ void Joint(py::module& m) &dart::dynamics::MimicDofProperties::mReferenceDofIndex) .def_readwrite( "mMultiplier", &dart::dynamics::MimicDofProperties::mMultiplier) - .def_readwrite("mOffset", &dart::dynamics::MimicDofProperties::mOffset); + .def_readwrite("mOffset", &dart::dynamics::MimicDofProperties::mOffset) + .def_readwrite( + "mConstraintType", + &dart::dynamics::MimicDofProperties::mConstraintType); ::py::class_(m, "JointProperties") .def( @@ -247,6 +255,17 @@ void Joint(py::module& m) -> dart::dynamics::Joint::ActuatorType { return self->getActuatorType(); }) + .def( + "setUseCouplerConstraint", + +[](dart::dynamics::Joint* self, bool enable) -> void { + self->setUseCouplerConstraint(enable); + }, + ::py::arg("enable")) + .def( + "isUsingCouplerConstraint", + +[](const dart::dynamics::Joint* self) -> bool { + return self->isUsingCouplerConstraint(); + }) .def( "isKinematic", +[](const dart::dynamics::Joint* self) -> bool { diff --git a/python/dartpy_docs/dynamics.py b/python/dartpy_docs/dynamics.py index f96619a47163d..16de2a9b210d4 100644 --- a/python/dartpy_docs/dynamics.py +++ b/python/dartpy_docs/dynamics.py @@ -4,7 +4,7 @@ import dartpy.optimizer import numpy import typing -__all__: list[str] = ['ACCELERATION', 'ActuatorType', 'ArrowShape', 'ArrowShapeProperties', 'BallJoint', 'BallJointProperties', 'BodyNode', 'BodyNodeAspectProperties', 'BodyNodeProperties', 'BoxShape', 'CapsuleShape', 'Chain', 'ChainCriteria', 'CollisionAspect', 'CompositeJoiner_EmbedProperties_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedProperties_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R1GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R2GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SE3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SO3GenericJointStateGenericJointUniqueProperties_Joint', 'ConeShape', 'CylinderShape', 'DefaultActuatorType', 'DegreeOfFreedom', 'Detachable', 'DynamicsAspect', 'EllipsoidShape', 'EmbedPropertiesOnTopOf_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'EmbedPropertiesOnTopOf_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'EmbedProperties_EulerJoint_EulerJointUniqueProperties', 'EmbedProperties_Joint_JointProperties', 'EmbedProperties_PlanarJoint_PlanarJointUniqueProperties', 'EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties', 'EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties', 'EmbedProperties_ScrewJoint_ScrewJointUniqueProperties', 'EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'EmbedProperties_UniversalJoint_UniversalJointUniqueProperties', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndProperties_GenericJoint_R1GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R2GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SE3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SO3GenericJointState_GenericJointUniqueProperties', 'Entity', 'EulerJoint', 'EulerJointProperties', 'EulerJointUniqueProperties', 'FORCE', 'Frame', 'FreeJoint', 'FreeJointProperties', 'GenericJointProperties_R1', 'GenericJointProperties_R2', 'GenericJointProperties_R3', 'GenericJointProperties_SE3', 'GenericJointProperties_SO3', 'GenericJointUniqueProperties_R1', 'GenericJointUniqueProperties_R2', 'GenericJointUniqueProperties_R3', 'GenericJointUniqueProperties_SE3', 'GenericJointUniqueProperties_SO3', 'GenericJoint_R1', 'GenericJoint_R2', 'GenericJoint_R3', 'GenericJoint_SE3', 'GenericJoint_SO3', 'Inertia', 'InverseKinematics', 'InverseKinematicsErrorMethod', 'InverseKinematicsErrorMethodProperties', 'InverseKinematicsTaskSpaceRegion', 'InverseKinematicsTaskSpaceRegionProperties', 'InverseKinematicsTaskSpaceRegionUniqueProperties', 'JacobianNode', 'Joint', 'JointProperties', 'LOCKED', 'LineSegmentShape', 'Linkage', 'LinkageCriteria', 'MIMIC', 'MeshShape', 'MetaSkeleton', 'MimicDofProperties', 'MultiSphereConvexHullShape', 'Node', 'PASSIVE', 'PlanarJoint', 'PlanarJointProperties', 'PlanarJointUniqueProperties', 'PlaneShape', 'PointCloudShape', 'PrismaticJoint', 'PrismaticJointProperties', 'PrismaticJointUniqueProperties', 'ReferentialSkeleton', 'RequiresAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'RevoluteJoint', 'RevoluteJointProperties', 'RevoluteJointUniqueProperties', 'SERVO', 'ScrewJoint', 'ScrewJointProperties', 'ScrewJointUniqueProperties', 'Shape', 'ShapeFrame', 'ShapeNode', 'SimpleFrame', 'Skeleton', 'SoftMeshShape', 'SpecializedForAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'SphereShape', 'TemplatedJacobianBodyNode', 'TranslationalJoint', 'TranslationalJoint2D', 'TranslationalJoint2DProperties', 'TranslationalJoint2DUniqueProperties', 'TranslationalJointProperties', 'UniversalJoint', 'UniversalJointProperties', 'UniversalJointUniqueProperties', 'VELOCITY', 'VisualAspect', 'WeldJoint', 'ZeroDofJoint', 'ZeroDofJointProperties'] +__all__: list[str] = ['ACCELERATION', 'ActuatorType', 'ArrowShape', 'ArrowShapeProperties', 'BallJoint', 'BallJointProperties', 'BodyNode', 'BodyNodeAspectProperties', 'BodyNodeProperties', 'BoxShape', 'CapsuleShape', 'Chain', 'ChainCriteria', 'CollisionAspect', 'CompositeJoiner_EmbedProperties_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedProperties_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R1GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R2GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SE3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SO3GenericJointStateGenericJointUniqueProperties_Joint', 'ConeShape', 'CylinderShape', 'DefaultActuatorType', 'DegreeOfFreedom', 'Detachable', 'DynamicsAspect', 'EllipsoidShape', 'EmbedPropertiesOnTopOf_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'EmbedPropertiesOnTopOf_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'EmbedProperties_EulerJoint_EulerJointUniqueProperties', 'EmbedProperties_Joint_JointProperties', 'EmbedProperties_PlanarJoint_PlanarJointUniqueProperties', 'EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties', 'EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties', 'EmbedProperties_ScrewJoint_ScrewJointUniqueProperties', 'EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'EmbedProperties_UniversalJoint_UniversalJointUniqueProperties', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndProperties_GenericJoint_R1GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R2GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SE3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SO3GenericJointState_GenericJointUniqueProperties', 'Entity', 'EndEffector', 'EulerJoint', 'EulerJointProperties', 'EulerJointUniqueProperties', 'FORCE', 'Frame', 'FreeJoint', 'FreeJointProperties', 'GenericJointProperties_R1', 'GenericJointProperties_R2', 'GenericJointProperties_R3', 'GenericJointProperties_SE3', 'GenericJointProperties_SO3', 'GenericJointUniqueProperties_R1', 'GenericJointUniqueProperties_R2', 'GenericJointUniqueProperties_R3', 'GenericJointUniqueProperties_SE3', 'GenericJointUniqueProperties_SO3', 'GenericJoint_R1', 'GenericJoint_R2', 'GenericJoint_R3', 'GenericJoint_SE3', 'GenericJoint_SO3', 'Inertia', 'InverseKinematics', 'InverseKinematicsErrorMethod', 'InverseKinematicsErrorMethodProperties', 'InverseKinematicsTaskSpaceRegion', 'InverseKinematicsTaskSpaceRegionProperties', 'InverseKinematicsTaskSpaceRegionUniqueProperties', 'JacobianNode', 'Joint', 'JointProperties', 'LOCKED', 'LineSegmentShape', 'Linkage', 'LinkageCriteria', 'MIMIC', 'MeshShape', 'MetaSkeleton', 'MimicConstraintType', 'MimicDofProperties', 'MultiSphereConvexHullShape', 'Node', 'PASSIVE', 'PlanarJoint', 'PlanarJointProperties', 'PlanarJointUniqueProperties', 'PlaneShape', 'PointCloudShape', 'PrismaticJoint', 'PrismaticJointProperties', 'PrismaticJointUniqueProperties', 'ReferentialSkeleton', 'RequiresAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'RevoluteJoint', 'RevoluteJointProperties', 'RevoluteJointUniqueProperties', 'SERVO', 'ScrewJoint', 'ScrewJointProperties', 'ScrewJointUniqueProperties', 'Shape', 'ShapeFrame', 'ShapeNode', 'SimpleFrame', 'Skeleton', 'SoftMeshShape', 'SpecializedForAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'SphereShape', 'Support', 'TemplatedJacobianBodyNode', 'TranslationalJoint', 'TranslationalJoint2D', 'TranslationalJoint2DProperties', 'TranslationalJoint2DUniqueProperties', 'TranslationalJointProperties', 'UniversalJoint', 'UniversalJointProperties', 'UniversalJointUniqueProperties', 'VELOCITY', 'VisualAspect', 'WeldJoint', 'ZeroDofJoint', 'ZeroDofJointProperties'] M = typing.TypeVar("M", bound=int) N = typing.TypeVar("N", bound=int) class ActuatorType: @@ -424,6 +424,10 @@ def getNumDependentGenCoords(self) -> int: ... def getNumEndEffectors(self) -> int: ... + def createEndEffector(self, name: str = 'EndEffector') -> EndEffector: + ... + def getEndEffector(self, index: int) -> EndEffector: + ... def getNumMarkers(self) -> int: ... def getNumShapeNodes(self) -> int: @@ -532,6 +536,52 @@ def setProperties(self, properties: BodyNodeAspectProperties) -> None: ... def split(self, skeletonName: str) -> ...: ... +class Support: + def setGeometry( + self, + geometry: typing.Iterable[ + numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], + numpy.dtype[numpy.float64], + ] + ], + ) -> None: + ... + + def getGeometry( + self, + ) -> list[ + numpy.ndarray[ + tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64] + ] + ]: + ... + + def setActive(self, supporting: bool = True) -> None: + ... + + def isActive(self) -> bool: + ... + + +class EndEffector(JacobianNode): + def setDefaultRelativeTransform(self, transform: dartpy.math.Isometry3, useNow: bool = False) -> None: + ... + def resetRelativeTransform(self) -> None: + ... + def createSupport(self) -> Support: + ... + @typing.overload + def getSupport(self) -> typing.Optional[Support]: + ... + @typing.overload + def getSupport(self, createIfNull: bool) -> Support: + ... + def hasSupport(self) -> bool: + ... + def removeSupport(self) -> None: + ... + class BodyNodeAspectProperties: mGravityMode: bool mInertia: ... @@ -3291,6 +3341,8 @@ def getAccelerations(self) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.d ... def getActuatorType(self) -> ActuatorType: ... + def isUsingCouplerConstraint(self) -> bool: + ... def getBodyConstraintWrench(self) -> numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]: ... def getChildBodyNode(self) -> BodyNode: @@ -3467,6 +3519,8 @@ def setAccelerations(self, accelerations: numpy.ndarray[tuple[M, typing.Literal[ ... def setActuatorType(self, actuatorType: ActuatorType) -> None: ... + def setUseCouplerConstraint(self, enable: bool) -> None: + ... def setAspectProperties(self, properties: ..., dart: ...) -> None: ... def setCommand(self, index: int, command: float) -> None: @@ -4332,6 +4386,7 @@ def setVelocityUpperLimits(self, velocities: numpy.ndarray[tuple[M, typing.Liter def setVelocityUpperLimits(self, indices: list[int], velocities: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None: ... class MimicDofProperties: + mConstraintType: MimicConstraintType mMultiplier: float mOffset: float mReferenceDofIndex: int diff --git a/python/stubs/dartpy/dynamics.pyi b/python/stubs/dartpy/dynamics.pyi index bf0baef4a45f5..16de2a9b210d4 100644 --- a/python/stubs/dartpy/dynamics.pyi +++ b/python/stubs/dartpy/dynamics.pyi @@ -4,7 +4,7 @@ import dartpy.math import dartpy.optimizer import numpy import typing -__all__: list[str] = ['ACCELERATION', 'ActuatorType', 'ArrowShape', 'ArrowShapeProperties', 'BallJoint', 'BallJointProperties', 'BodyNode', 'BodyNodeAspectProperties', 'BodyNodeProperties', 'BoxShape', 'CapsuleShape', 'Chain', 'ChainCriteria', 'CollisionAspect', 'CompositeJoiner_EmbedProperties_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedProperties_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R1GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R2GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SE3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SO3GenericJointStateGenericJointUniqueProperties_Joint', 'ConeShape', 'CylinderShape', 'DefaultActuatorType', 'DegreeOfFreedom', 'Detachable', 'DynamicsAspect', 'EllipsoidShape', 'EmbedPropertiesOnTopOf_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'EmbedPropertiesOnTopOf_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'EmbedProperties_EulerJoint_EulerJointUniqueProperties', 'EmbedProperties_Joint_JointProperties', 'EmbedProperties_PlanarJoint_PlanarJointUniqueProperties', 'EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties', 'EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties', 'EmbedProperties_ScrewJoint_ScrewJointUniqueProperties', 'EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'EmbedProperties_UniversalJoint_UniversalJointUniqueProperties', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndProperties_GenericJoint_R1GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R2GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SE3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SO3GenericJointState_GenericJointUniqueProperties', 'Entity', 'EndEffector', 'EulerJoint', 'EulerJointProperties', 'EulerJointUniqueProperties', 'FORCE', 'Frame', 'FreeJoint', 'GenericJointProperties_R1', 'GenericJointProperties_R2', 'GenericJointProperties_R3', 'GenericJointProperties_SE3', 'GenericJointProperties_SO3', 'GenericJointUniqueProperties_R1', 'GenericJointUniqueProperties_R2', 'GenericJointUniqueProperties_R3', 'GenericJointUniqueProperties_SE3', 'GenericJointUniqueProperties_SO3', 'GenericJoint_R1', 'GenericJoint_R2', 'GenericJoint_R3', 'GenericJoint_SE3', 'GenericJoint_SO3', 'Inertia', 'InverseKinematics', 'InverseKinematicsGradientMethod', 'InverseKinematicsJacobianDLS', 'HierarchicalIK', 'InverseKinematicsErrorMethod', 'InverseKinematicsErrorMethodProperties', 'InverseKinematicsTaskSpaceRegion', 'InverseKinematicsTaskSpaceRegionProperties', 'InverseKinematicsTaskSpaceRegionUniqueProperties', 'JacobianNode', 'Joint', 'JointProperties', 'LOCKED', 'LineSegmentShape', 'Linkage', 'LinkageCriteria', 'MIMIC', 'MeshShape', 'MetaSkeleton', 'MimicDofProperties', 'MultiSphereConvexHullShape', 'Node', 'PASSIVE', 'PlanarJoint', 'PlanarJointProperties', 'PlanarJointUniqueProperties', 'PlaneShape', 'PointCloudShape', 'PrismaticJoint', 'PrismaticJointProperties', 'PrismaticJointUniqueProperties', 'ReferentialSkeleton', 'RequiresAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'RevoluteJoint', 'RevoluteJointProperties', 'RevoluteJointUniqueProperties', 'SERVO', 'ScrewJoint', 'ScrewJointProperties', 'ScrewJointUniqueProperties', 'Shape', 'ShapeFrame', 'ShapeNode', 'WholeBodyIK', 'SimpleFrame', 'Skeleton', 'SoftMeshShape', 'SpecializedForAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'SphereShape', 'Support', 'TemplatedJacobianBodyNode', 'TranslationalJoint', 'TranslationalJoint2D', 'TranslationalJoint2DProperties', 'TranslationalJoint2DUniqueProperties', 'TranslationalJointProperties', 'UniversalJoint', 'UniversalJointProperties', 'UniversalJointUniqueProperties', 'VELOCITY', 'VisualAspect', 'WeldJoint', 'ZeroDofJoint', 'ZeroDofJointProperties'] +__all__: list[str] = ['ACCELERATION', 'ActuatorType', 'ArrowShape', 'ArrowShapeProperties', 'BallJoint', 'BallJointProperties', 'BodyNode', 'BodyNodeAspectProperties', 'BodyNodeProperties', 'BoxShape', 'CapsuleShape', 'Chain', 'ChainCriteria', 'CollisionAspect', 'CompositeJoiner_EmbedProperties_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'CompositeJoiner_EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'CompositeJoiner_EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedProperties_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R1GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R2GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_R3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SE3GenericJointStateGenericJointUniqueProperties_Joint', 'CompositeJoiner_EmbedStateAndProperties_GenericJoint_SO3GenericJointStateGenericJointUniqueProperties_Joint', 'ConeShape', 'CylinderShape', 'DefaultActuatorType', 'DegreeOfFreedom', 'Detachable', 'DynamicsAspect', 'EllipsoidShape', 'EmbedPropertiesOnTopOf_EulerJoint_EulerJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PlanarJoint_PlanarJointUniqueProperties_GenericJoint_R3Space', 'EmbedPropertiesOnTopOf_PrismaticJoint_PrismaticJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_RevoluteJoint_RevoluteJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_ScrewJoint_ScrewJointUniqueProperties_GenericJoint_R1Space', 'EmbedPropertiesOnTopOf_TranslationalJoint2D_TranslationalJoint2DUniqueProperties_GenericJoint_R2Space', 'EmbedPropertiesOnTopOf_UniversalJoint_UniversalJointUniqueProperties_GenericJoint_R2Space', 'EmbedProperties_EulerJoint_EulerJointUniqueProperties', 'EmbedProperties_Joint_JointProperties', 'EmbedProperties_PlanarJoint_PlanarJointUniqueProperties', 'EmbedProperties_PrismaticJoint_PrismaticJointUniqueProperties', 'EmbedProperties_RevoluteJoint_RevoluteJointUniqueProperties', 'EmbedProperties_ScrewJoint_ScrewJointUniqueProperties', 'EmbedProperties_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'EmbedProperties_UniversalJoint_UniversalJointUniqueProperties', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndPropertiesOnTopOf_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties_Joint', 'EmbedStateAndProperties_GenericJoint_R1GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R2GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_R3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SE3GenericJointState_GenericJointUniqueProperties', 'EmbedStateAndProperties_GenericJoint_SO3GenericJointState_GenericJointUniqueProperties', 'Entity', 'EndEffector', 'EulerJoint', 'EulerJointProperties', 'EulerJointUniqueProperties', 'FORCE', 'Frame', 'FreeJoint', 'FreeJointProperties', 'GenericJointProperties_R1', 'GenericJointProperties_R2', 'GenericJointProperties_R3', 'GenericJointProperties_SE3', 'GenericJointProperties_SO3', 'GenericJointUniqueProperties_R1', 'GenericJointUniqueProperties_R2', 'GenericJointUniqueProperties_R3', 'GenericJointUniqueProperties_SE3', 'GenericJointUniqueProperties_SO3', 'GenericJoint_R1', 'GenericJoint_R2', 'GenericJoint_R3', 'GenericJoint_SE3', 'GenericJoint_SO3', 'Inertia', 'InverseKinematics', 'InverseKinematicsErrorMethod', 'InverseKinematicsErrorMethodProperties', 'InverseKinematicsTaskSpaceRegion', 'InverseKinematicsTaskSpaceRegionProperties', 'InverseKinematicsTaskSpaceRegionUniqueProperties', 'JacobianNode', 'Joint', 'JointProperties', 'LOCKED', 'LineSegmentShape', 'Linkage', 'LinkageCriteria', 'MIMIC', 'MeshShape', 'MetaSkeleton', 'MimicConstraintType', 'MimicDofProperties', 'MultiSphereConvexHullShape', 'Node', 'PASSIVE', 'PlanarJoint', 'PlanarJointProperties', 'PlanarJointUniqueProperties', 'PlaneShape', 'PointCloudShape', 'PrismaticJoint', 'PrismaticJointProperties', 'PrismaticJointUniqueProperties', 'ReferentialSkeleton', 'RequiresAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'RequiresAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'RequiresAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'RevoluteJoint', 'RevoluteJointProperties', 'RevoluteJointUniqueProperties', 'SERVO', 'ScrewJoint', 'ScrewJointProperties', 'ScrewJointUniqueProperties', 'Shape', 'ShapeFrame', 'ShapeNode', 'SimpleFrame', 'Skeleton', 'SoftMeshShape', 'SpecializedForAspect_EmbeddedPropertiesAspect_EulerJoint_EulerJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_Joint_JointProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PlanarJoint_PlanarJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_PrismaticJoint_PrismaticJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_RevoluteJoint_RevoluteJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_ScrewJoint_ScrewJointUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_TranslationalJoint2D_TranslationalJoint2DUniqueProperties', 'SpecializedForAspect_EmbeddedPropertiesAspect_UniversalJoint_UniversalJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R1_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R2_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_R3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SE3_GenericJointState_GenericJointUniqueProperties', 'SpecializedForAspect_EmbeddedStateAndPropertiesAspect_GenericJoint_SO3_GenericJointState_GenericJointUniqueProperties', 'SphereShape', 'Support', 'TemplatedJacobianBodyNode', 'TranslationalJoint', 'TranslationalJoint2D', 'TranslationalJoint2DProperties', 'TranslationalJoint2DUniqueProperties', 'TranslationalJointProperties', 'UniversalJoint', 'UniversalJointProperties', 'UniversalJointUniqueProperties', 'VELOCITY', 'VisualAspect', 'WeldJoint', 'ZeroDofJoint', 'ZeroDofJointProperties'] M = typing.TypeVar("M", bound=int) N = typing.TypeVar("N", bound=int) class ActuatorType: @@ -280,8 +280,6 @@ class BodyNode(TemplatedJacobianBodyNode, Frame): @typing.overload def createShapeNode(self, shape: Shape, name: str) -> ShapeNode: ... - def createEndEffector(self, name: str = 'EndEffector') -> EndEffector: - ... @typing.overload def createTranslationalJoint2DAndBodyNodePair(self) -> tuple[..., BodyNode]: ... @@ -426,6 +424,8 @@ class BodyNode(TemplatedJacobianBodyNode, Frame): ... def getNumEndEffectors(self) -> int: ... + def createEndEffector(self, name: str = 'EndEffector') -> EndEffector: + ... def getEndEffector(self, index: int) -> EndEffector: ... def getNumMarkers(self) -> int: @@ -582,7 +582,6 @@ class EndEffector(JacobianNode): def removeSupport(self) -> None: ... - class BodyNodeAspectProperties: mGravityMode: bool mInertia: ... @@ -3030,8 +3029,6 @@ class InverseKinematics(dartpy.common.Subject): ... def getErrorMethod(self) -> InverseKinematicsErrorMethod: ... - def getGradientMethod(self) -> InverseKinematicsGradientMethod: - ... def getHierarchyLevel(self) -> int: ... def getNullSpaceObjective(self) -> dartpy.optimizer.Function: @@ -3105,22 +3102,6 @@ class InverseKinematics(dartpy.common.Subject): ... def useWholeBody(self) -> None: ... -class InverseKinematicsGradientMethod: - def clearCache(self) -> None: - ... - def getComponentWeights(self) -> numpy.ndarray[typing.Any, numpy.dtype[numpy.float64]]: - ... - def getComponentWiseClamp(self) -> bool: - ... - def setComponentWeights(self, weights: numpy.ndarray[typing.Any, numpy.dtype[numpy.float64]]) -> None: - ... - def setComponentWiseClamp(self, clamp: bool) -> None: - ... -class InverseKinematicsJacobianDLS(InverseKinematicsGradientMethod): - def getDampingCoefficient(self) -> float: - ... - def setDampingCoefficient(self, damping: float = ...) -> None: - ... class InverseKinematicsErrorMethod(dartpy.common.Subject): def clearCache(self) -> None: ... @@ -3239,18 +3220,6 @@ class InverseKinematicsTaskSpaceRegionUniqueProperties: mReferenceFrame: SimpleFrame def __init__(self, computeErrorFromCenter: bool = True, referenceFrame: SimpleFrame = None) -> None: ... - - -class HierarchicalIK(dartpy.common.Subject): - def solveAndApply(self, allowIncompleteResult: bool = True) -> bool: - ... - - -class WholeBodyIK(HierarchicalIK): - def refreshIKHierarchy(self) -> None: - ... - - class JacobianNode(Frame, Node): def clearIK(self) -> None: ... @@ -3372,6 +3341,8 @@ class Joint(dartpy.common.Subject, EmbedProperties_Joint_JointProperties): ... def getActuatorType(self) -> ActuatorType: ... + def isUsingCouplerConstraint(self) -> bool: + ... def getBodyConstraintWrench(self) -> numpy.ndarray[tuple[typing.Literal[6], typing.Literal[1]], numpy.dtype[numpy.float64]]: ... def getChildBodyNode(self) -> BodyNode: @@ -3548,6 +3519,8 @@ class Joint(dartpy.common.Subject, EmbedProperties_Joint_JointProperties): ... def setActuatorType(self, actuatorType: ActuatorType) -> None: ... + def setUseCouplerConstraint(self, enable: bool) -> None: + ... def setAspectProperties(self, properties: ..., dart: ...) -> None: ... def setCommand(self, index: int, command: float) -> None: @@ -4413,6 +4386,7 @@ class MetaSkeleton: def setVelocityUpperLimits(self, indices: list[int], velocities: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None: ... class MimicDofProperties: + mConstraintType: MimicConstraintType mMultiplier: float mOffset: float mReferenceDofIndex: int @@ -5840,9 +5814,6 @@ class Skeleton(MetaSkeleton): def getIK(self) -> ...: ... @typing.overload - def getIK(self, createIfNull: bool) -> ...: - ... - @typing.overload def getIndexOf(self, bn: BodyNode) -> int: ... @typing.overload @@ -5960,15 +5931,6 @@ class Skeleton(MetaSkeleton): @typing.overload def getNumEndEffectors(self, treeIndex: int) -> int: ... - @typing.overload - def getEndEffector(self, index: int) -> EndEffector: - ... - @typing.overload - def getEndEffector(self, treeIndex: int, nodeIndex: int) -> EndEffector: - ... - @typing.overload - def getEndEffector(self, name: str) -> EndEffector: - ... def getNumJoints(self) -> int: ... @typing.overload diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index 89bce6ec8569a..dd4ffc792cef5 100644 --- a/tests/integration/dynamics/test_Joints.cpp +++ b/tests/integration/dynamics/test_Joints.cpp @@ -34,6 +34,7 @@ #include "helpers/dynamics_helpers.hpp" +#include "dart/constraint/CouplerConstraint.hpp" #include "dart/dynamics/BallJoint.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/EulerJoint.hpp" @@ -65,6 +66,14 @@ using namespace dart::test; #define JOINT_TOL 0.01 +class CouplerConstraintTestHelper : public dart::constraint::CouplerConstraint +{ +public: + using CouplerConstraint::applyImpulse; + using CouplerConstraint::CouplerConstraint; + using CouplerConstraint::update; +}; + //============================================================================== class Joints : public testing::Test { @@ -1193,13 +1202,131 @@ void testMimicJoint() } //============================================================================== -TEST_F(Joints, MimicJoint) +void testMimicCouplerJoint() +{ + using namespace dart::math::suffixes; + + double timestep = 1e-3; + double tolPos = 1e-3; + double sufficient_force = 1e+5; + + simulation::WorldPtr world = simulation::World::create(); + ASSERT_TRUE(world != nullptr); + + world->setGravity(Eigen::Vector3d::Zero()); + world->setTimeStep(timestep); + + Vector3d dim(1, 1, 1); + Vector3d offset(0, 0, 2); + + SkeletonPtr pendulum = createNLinkPendulum(2, dim, DOF_ROLL, offset); + ASSERT_NE(pendulum, nullptr); + + pendulum->disableSelfCollisionCheck(); + + for (std::size_t i = 0; i < pendulum->getNumBodyNodes(); ++i) { + auto bodyNode = pendulum->getBodyNode(i); + bodyNode->removeAllShapeNodesWith(); + } + + std::vector joints(2); + + for (std::size_t i = 0; i < pendulum->getNumBodyNodes(); ++i) { + dynamics::Joint* joint = pendulum->getJoint(i); + ASSERT_NE(joint, nullptr); + + joint->setDampingCoefficient(0, 0.0); + joint->setSpringStiffness(0, 0.0); + joint->setLimitEnforcement(true); + joint->setCoulombFriction(0, 0.0); + + joints[i] = joint; + } + + joints[0]->setActuatorType(Joint::PASSIVE); + joints[0]->setForceUpperLimit(0, sufficient_force); + joints[0]->setForceLowerLimit(0, -sufficient_force); + + joints[1]->setActuatorType(Joint::MIMIC); + joints[1]->setMimicJoint(joints[0], 1., 0.); + joints[1]->setUseCouplerConstraint(true); + joints[1]->setForceUpperLimit(0, sufficient_force); + joints[1]->setForceLowerLimit(0, -sufficient_force); + + joints[0]->setPosition(0, 0.0_deg); + joints[1]->setPosition(0, 30.0_deg); + + world->addSkeleton(pendulum); + + double initialReferencePosition = joints[0]->getPosition(0); + double initialError = joints[1]->getPosition(0) - joints[0]->getPosition(0); + + for (int i = 0; i < 400; ++i) + world->step(); + + double finalError = joints[1]->getPosition(0) - joints[0]->getPosition(0); + + EXPECT_LT(std::abs(finalError), std::abs(initialError)); + EXPECT_NEAR(joints[0]->getPosition(0), joints[1]->getPosition(0), tolPos); + EXPECT_GT( + std::abs(joints[0]->getPosition(0) - initialReferencePosition), 1e-4); +} + +//============================================================================== +TEST_F(Joints, MIMIC_JOINT) { testMimicJoint(); } //============================================================================== -TEST_F(Joints, JointCoulombFrictionAndPositionLimit) +TEST_F(Joints, MIMIC_JOINT_COUPLER) +{ + testMimicCouplerJoint(); +} + +//============================================================================== +TEST_F(Joints, COUPLER_CONSTRAINT_APPLY_IMPULSE) +{ + Vector3d dim(1, 1, 1); + Vector3d offset(0, 0, 0); + + SkeletonPtr pendulum = createNLinkPendulum(2, dim, DOF_ROLL, offset); + ASSERT_NE(pendulum, nullptr); + pendulum->setTimeStep(1e-3); + + Joint* referenceJoint = pendulum->getJoint(0); + Joint* followerJoint = pendulum->getJoint(1); + ASSERT_NE(referenceJoint, nullptr); + ASSERT_NE(followerJoint, nullptr); + + followerJoint->setActuatorType(Joint::MIMIC); + followerJoint->setMimicJoint(referenceJoint, 1.0, 0.0); + followerJoint->setUseCouplerConstraint(true); + followerJoint->setVelocityLowerLimit(0, -100.0); + followerJoint->setVelocityUpperLimit(0, 100.0); + followerJoint->setForceLowerLimit(0, -100.0); + followerJoint->setForceUpperLimit(0, 100.0); + + referenceJoint->setPosition(0, 0.1); + followerJoint->setPosition(0, 0.0); + + CouplerConstraintTestHelper constraint( + followerJoint, followerJoint->getMimicDofProperties()); + constraint.update(); + ASSERT_GT(constraint.getDimension(), 0u); + + double lambda[1] = {2.3}; + constraint.applyImpulse(lambda); + + EXPECT_NEAR(followerJoint->getConstraintImpulse(0), lambda[0], 1e-12); + EXPECT_NEAR( + referenceJoint->getConstraintImpulse(0), + -lambda[0] * followerJoint->getMimicMultiplier(0), + 1e-12); +} + +//============================================================================== +TEST_F(Joints, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) { const double timeStep = 1e-3; const double tol = 1e-2;