From a54fa4dafe0890c572ebc0fcfe6b3a21265ba936 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Fri, 9 Aug 2024 16:06:45 +0530 Subject: [PATCH 01/20] Added coupler constraint Signed-off-by: Aditya Pande --- dart/constraint/CouplerConstraint.cpp | 37 +++++++++++++++++++++++++++ dart/constraint/CouplerConstraint.hpp | 32 +++++++++++++++++++++++ 2 files changed, 69 insertions(+) create mode 100644 dart/constraint/CouplerConstraint.cpp create mode 100644 dart/constraint/CouplerConstraint.hpp diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp new file mode 100644 index 0000000000000..7d61027918b56 --- /dev/null +++ b/dart/constraint/CouplerConstraint.cpp @@ -0,0 +1,37 @@ +#include "dart/constraint/CouplerConstraint.hpp" + +namespace dart { +namespace constraint { + +CouplerConstraint::CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio) + : mBodyNode1(body1), mBodyNode2(body2), mRatio(ratio), mImpulse(Eigen::Vector6d::Zero()) {} + +void CouplerConstraint::setRatio(double ratio) +{ + mRatio = ratio; +} + +double CouplerConstraint::getRatio() const +{ + return mRatio; +} + +void CouplerConstraint::update() +{ + // Implement the logic to update the constraint based on the ratio + // For simplicity, we'll assume a direct proportional relationship + Eigen::Vector6d velocity1 = mBodyNode1->getSpatialVelocity(); + Eigen::Vector6d velocity2 = mBodyNode2->getSpatialVelocity(); + + mImpulse = mRatio * (velocity2 - velocity1); +} + +void CouplerConstraint::applyImpulse() +{ + // Apply equal and opposite impulses to the connected bodies + mBodyNode1->addConstraintImpulse(mImpulse); + mBodyNode2->addConstraintImpulse(-mImpulse); +} + +} // namespace constraint +} // namespace dart diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp new file mode 100644 index 0000000000000..237e21250b3c6 --- /dev/null +++ b/dart/constraint/CouplerConstraint.hpp @@ -0,0 +1,32 @@ +#ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ +#define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ + +#include +#include + +namespace dart { +namespace constraint { + +class CouplerConstraint : public ConstraintBase +{ +public: + CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio); + + void setRatio(double ratio); + double getRatio() const; + + void update() override; + void applyImpulse() override; + +private: + dynamics::BodyNode* mBodyNode1; + dynamics::BodyNode* mBodyNode2; + double mRatio; + Eigen::Vector6d mImpulse; +}; + +} // namespace constraint +} // namespace dart + +#endif // DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ + From 1321a7fa4b4c9390dd5ff91a177d0b82207c6950 Mon Sep 17 00:00:00 2001 From: Aditya Pande Date: Sat, 10 Aug 2024 09:58:40 +0000 Subject: [PATCH 02/20] Fixed build, use mimic motor constriant as a base Signed-off-by: Aditya Pande --- dart/constraint/CouplerConstraint.cpp | 266 ++++++++++++++++++++++++-- dart/constraint/CouplerConstraint.hpp | 102 ++++++++-- 2 files changed, 342 insertions(+), 26 deletions(-) diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index 7d61027918b56..b4c7b0de9246c 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -1,36 +1,272 @@ #include "dart/constraint/CouplerConstraint.hpp" +#include "dart/common/Console.hpp" +#include "dart/dynamics/BodyNode.hpp" +#include "dart/dynamics/Joint.hpp" +#include "dart/dynamics/Skeleton.hpp" +#include "dart/external/odelcpsolver/lcp.h" + +#define DART_CFM 1e-9 namespace dart { namespace constraint { -CouplerConstraint::CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio) - : mBodyNode1(body1), mBodyNode2(body2), mRatio(ratio), mImpulse(Eigen::Vector6d::Zero()) {} +double CouplerConstraint::mConstraintForceMixing = DART_CFM; + +//============================================================================== +CouplerConstraint::CouplerConstraint( + dynamics::Joint* joint, + const std::vector& mimicDofProperties) + : ConstraintBase(), + mJoint(joint), + mMimicProps(mimicDofProperties), + mBodyNode(joint->getChildBodyNode()), + mAppliedImpulseIndex(0) +{ + assert(joint); + assert(joint->getNumDofs() <= mMimicProps.size()); + assert(mBodyNode); + + std::fill(mLifeTime, mLifeTime + 6, 0); + std::fill(mActive, mActive + 6, false); +} -void CouplerConstraint::setRatio(double ratio) +//============================================================================== +CouplerConstraint::~CouplerConstraint() { - mRatio = ratio; + // Do nothing } -double CouplerConstraint::getRatio() const +//============================================================================== +const std::string& CouplerConstraint::getType() const { - return mRatio; + return getStaticType(); } +//============================================================================== +const std::string& CouplerConstraint::getStaticType() +{ + static const std::string name = "CouplerConstraint"; + return name; +} + +//============================================================================== +void CouplerConstraint::setConstraintForceMixing(double cfm) +{ + // Clamp constraint force mixing parameter if it is out of the range + if (cfm < 1e-9) { + dtwarn << "[CouplerConstraint::setConstraintForceMixing] " + << "Constraint force mixing parameter[" << cfm + << "] is lower than 1e-9. " + << "It is set to 1e-9.\n"; + mConstraintForceMixing = 1e-9; + } + + mConstraintForceMixing = cfm; +} + +//============================================================================== +double CouplerConstraint::getConstraintForceMixing() +{ + return mConstraintForceMixing; +} + +//============================================================================== void CouplerConstraint::update() { - // Implement the logic to update the constraint based on the ratio - // For simplicity, we'll assume a direct proportional relationship - Eigen::Vector6d velocity1 = mBodyNode1->getSpatialVelocity(); - Eigen::Vector6d velocity2 = mBodyNode2->getSpatialVelocity(); + // Reset dimension + mDim = 0; + + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + const auto& mimicProp = mMimicProps[i]; + + double timeStep = mJoint->getSkeleton()->getTimeStep(); + double qError + = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex) + * mimicProp.mMultiplier + + mimicProp.mOffset - mJoint->getPosition(i); + double desiredVelocity = math::clip( + qError / timeStep, + mJoint->getVelocityLowerLimit(i), + mJoint->getVelocityUpperLimit(i)); + + mNegativeVelocityError[i] = desiredVelocity - mJoint->getVelocity(i); + + if (mNegativeVelocityError[i] != 0.0) { + mUpperBound[i] = mJoint->getForceUpperLimit(i) * timeStep; + mLowerBound[i] = mJoint->getForceLowerLimit(i) * timeStep; + + if (mActive[i]) { + ++(mLifeTime[i]); + } else { + mActive[i] = true; + mLifeTime[i] = 0; + } + + ++mDim; + } else { + mActive[i] = false; + } + } +} + +//============================================================================== +void CouplerConstraint::getInformation(ConstraintInfo* lcp) +{ + std::size_t index = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + assert(lcp->w[index] == 0.0); + + lcp->b[index] = mNegativeVelocityError[i]; + lcp->lo[index] = mLowerBound[i]; + lcp->hi[index] = mUpperBound[i]; + + assert(lcp->findex[index] == -1); + + if (mLifeTime[i]) + lcp->x[index] = mOldX[i]; + else + lcp->x[index] = 0.0; + + index++; + } +} + +//============================================================================== +void CouplerConstraint::applyUnitImpulse(std::size_t index) +{ + assert(index < mDim && "Invalid Index."); + + std::size_t localIndex = 0; + const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); + + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + if (localIndex == index) { + const auto& mimicProp = mMimicProps[i]; + + skeleton->clearConstraintImpulses(); + + double impulse = 1.0; + mJoint->setConstraintImpulse(i, impulse); + + // Using const_cast to remove constness for methods that modify state + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( + mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); + + skeleton->updateBiasImpulse(mBodyNode); + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->updateBiasImpulse(const_cast(mimicProp.mReferenceJoint->getChildBodyNode())); + + skeleton->updateVelocityChange(); + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->updateVelocityChange(); + + mJoint->setConstraintImpulse(i, 0.0); + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); + } + + ++localIndex; + } + + mAppliedImpulseIndex = index; +} + +//============================================================================== +void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) +{ + assert(delVel != nullptr && "Null pointer is not allowed."); + + std::size_t localIndex = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + if (mJoint->getSkeleton()->isImpulseApplied()) + delVel[localIndex] = mJoint->getVelocityChange(i); + else + delVel[localIndex] = 0.0; + + ++localIndex; + } + + if (withCfm) { + delVel[mAppliedImpulseIndex] + += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; + } + + assert(localIndex == mDim); +} + +//============================================================================== +void CouplerConstraint::excite() +{ + mJoint->getSkeleton()->setImpulseApplied(true); + for (const auto& mimicProp : mMimicProps) + { + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(true); + } +} - mImpulse = mRatio * (velocity2 - velocity1); +//============================================================================== +void CouplerConstraint::unexcite() +{ + mJoint->getSkeleton()->setImpulseApplied(false); + for (const auto& mimicProp : mMimicProps) + { + const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + ->setImpulseApplied(false); + } } -void CouplerConstraint::applyImpulse() +//============================================================================== +void CouplerConstraint::applyImpulse(double* lambda) { - // Apply equal and opposite impulses to the connected bodies - mBodyNode1->addConstraintImpulse(mImpulse); - mBodyNode2->addConstraintImpulse(-mImpulse); + std::size_t localIndex = 0; + std::size_t dof = mJoint->getNumDofs(); + for (std::size_t i = 0; i < dof; ++i) { + if (mActive[i] == false) + continue; + + mJoint->setConstraintImpulse( + i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); + + auto& mimicProp = mMimicProps[i]; + + // Using const_cast to remove constness for methods that modify state + const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( + mimicProp.mReferenceDofIndex, + mimicProp.mReferenceJoint->getConstraintImpulse(mimicProp.mReferenceDofIndex) + - lambda[localIndex] * mimicProp.mMultiplier); + + mOldX[i] = lambda[localIndex]; + + ++localIndex; + } +} + +//============================================================================== +dynamics::SkeletonPtr CouplerConstraint::getRootSkeleton() const +{ + return ConstraintBase::getRootSkeleton(mJoint->getSkeleton()->getSkeleton()); +} + +//============================================================================== +bool CouplerConstraint::isActive() const +{ + if (mJoint->getActuatorType() == dynamics::Joint::MIMIC) + return true; + + return false; } } // namespace constraint diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index 237e21250b3c6..95b43062ef342 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -1,32 +1,112 @@ #ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ #define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ -#include -#include +#include +#include +#include namespace dart { namespace constraint { +/// Coupler constraint that couples the motions of two joints by applying +/// equal and opposite impulses based on MimicDofProperties. class CouplerConstraint : public ConstraintBase { public: - CouplerConstraint(dynamics::BodyNode* body1, dynamics::BodyNode* body2, double ratio); + /// Constructor that creates a CouplerConstraint using the given + /// MimicDofProperties for each dependent joint's DoF. + /// \param[in] joint The dependent joint. + /// \param[in] mimicDofProperties A vector of MimicDofProperties for each DoF + /// of the dependent joint. + explicit CouplerConstraint( + dynamics::Joint* joint, + const std::vector& mimicDofProperties); - void setRatio(double ratio); - double getRatio() const; + /// Destructor + ~CouplerConstraint() override; + // Documentation inherited + const std::string& getType() const override; + + /// Returns constraint type for this class. + static const std::string& getStaticType(); + + /// Set global constraint force mixing parameter + static void setConstraintForceMixing(double cfm); + + /// Get global constraint force mixing parameter + static double getConstraintForceMixing(); + + // Friendship + friend class ConstraintSolver; + friend class ConstrainedGroup; + +protected: + // Documentation inherited void update() override; - void applyImpulse() override; + + // Documentation inherited + void getInformation(ConstraintInfo* lcp) override; + + // Documentation inherited + void applyUnitImpulse(std::size_t index) override; + + // Documentation inherited + void getVelocityChange(double* delVel, bool withCfm) override; + + // Documentation inherited + void excite() override; + + // Documentation inherited + void unexcite() override; + + // Documentation inherited + void applyImpulse(double* lambda) override; + + // Documentation inherited + dynamics::SkeletonPtr getRootSkeleton() const override; + + // Documentation inherited + bool isActive() const override; private: - dynamics::BodyNode* mBodyNode1; - dynamics::BodyNode* mBodyNode2; - double mRatio; - Eigen::Vector6d mImpulse; + /// Dependent joint whose motion is influenced by the reference joint. + dynamics::Joint* mJoint; + + /// Vector of MimicDofProperties for the dependent joint. + std::vector mMimicProps; + + /// BodyNode associated with the dependent joint. + dynamics::BodyNode* mBodyNode; + + /// Index of the applied impulse for the dependent joint. + std::size_t mAppliedImpulseIndex; + + /// Array storing the lifetime of each constraint (in iterations). + std::size_t mLifeTime[6]; + + /// Array indicating whether each constraint is active or not. + bool mActive[6]; + + /// Array storing the negative velocity errors for each constraint. + double mNegativeVelocityError[6]; + + /// Array storing the previous values of the constraint forces. + double mOldX[6]; + + /// Array storing the upper bounds for the constraint forces. + double mUpperBound[6]; + + /// Array storing the lower bounds for the constraint forces. + double mLowerBound[6]; + + /// Global constraint force mixing parameter in the range of [1e-9, 1]. The + /// default is 1e-5 + /// \sa http://www.ode.org/ode-latest-userguide.html#sec_3_8_0 + static double mConstraintForceMixing; }; } // namespace constraint } // namespace dart #endif // DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ - From db13a9c1b5af9eff024d789236005eaf027aea8a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 4 Nov 2025 11:10:19 -0800 Subject: [PATCH 03/20] Add coupler constraint support and example --- dart/constraint/ConstraintSolver.cpp | 19 +- dart/constraint/ConstraintSolver.hpp | 3 + dart/constraint/CouplerConstraint.cpp | 96 +++++- dart/constraint/CouplerConstraint.hpp | 35 +++ dart/constraint/Fwd.hpp | 2 + dart/dynamics/Joint.cpp | 16 +- dart/dynamics/Joint.hpp | 6 + dart/dynamics/detail/JointAspect.hpp | 3 + examples/CMakeLists.txt | 1 + examples/coupler_constraint/CMakeLists.txt | 29 ++ examples/coupler_constraint/main.cpp | 335 +++++++++++++++++++++ pixi.toml | 18 ++ python/dartpy/dynamics/Joint.cpp | 14 + python/dartpy_docs/dynamics.py | 5 + python/stubs/dartpy/dynamics.pyi | 5 + tests/integration/dynamics/test_Joints.cpp | 81 ++++- 16 files changed, 657 insertions(+), 11 deletions(-) create mode 100644 examples/coupler_constraint/CMakeLists.txt create mode 100644 examples/coupler_constraint/main.cpp diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 979d144997d8a..b88edba0a68fa 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -42,6 +42,7 @@ #include "dart/common/Macros.hpp" #include "dart/common/Profile.hpp" #include "dart/constraint/ConstrainedGroup.hpp" +#include "dart/constraint/CouplerConstraint.hpp" #include "dart/constraint/ContactConstraint.hpp" #include "dart/constraint/ContactSurface.hpp" #include "dart/constraint/JointConstraint.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 index b4c7b0de9246c..3cb522ac470aa 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -1,9 +1,40 @@ +/* + * 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/Console.hpp" +#include "dart/common/Logging.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Joint.hpp" #include "dart/dynamics/Skeleton.hpp" -#include "dart/external/odelcpsolver/lcp.h" #define DART_CFM 1e-9 @@ -54,10 +85,10 @@ void CouplerConstraint::setConstraintForceMixing(double cfm) { // Clamp constraint force mixing parameter if it is out of the range if (cfm < 1e-9) { - dtwarn << "[CouplerConstraint::setConstraintForceMixing] " - << "Constraint force mixing parameter[" << cfm - << "] is lower than 1e-9. " - << "It is set to 1e-9.\n"; + DART_WARN( + "[CouplerConstraint::setConstraintForceMixing] Constraint force " + "mixing parameter[{}] is lower than 1e-9. It is set to 1e-9.", + cfm); mConstraintForceMixing = 1e-9; } @@ -260,6 +291,59 @@ 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 { diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index 95b43062ef342..1b71825b106f0 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -1,3 +1,35 @@ +/* + * 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_ @@ -69,6 +101,9 @@ class CouplerConstraint : public ConstraintBase // 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; 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/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 2dd4097298bdd..aa99279bdcb60 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -72,7 +72,8 @@ JointProperties::JointProperties( mT_ParentBodyToJoint(_T_ParentBodyToJoint), mT_ChildBodyToJoint(_T_ChildBodyToJoint), mIsPositionLimitEnforced(_isPositionLimitEnforced), - mActuatorType(_actuatorType) + mActuatorType(_actuatorType), + mUseCouplerConstraint(false) { mMimicDofProps.resize(6); // TODO: Dof 6, which is the max value at the moment, is used because @@ -127,6 +128,7 @@ void Joint::setAspectProperties(const AspectProperties& properties) setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); setLimitEnforcement(properties.mIsPositionLimitEnforced); setActuatorType(properties.mActuatorType); + setUseCouplerConstraint(properties.mUseCouplerConstraint); setMimicJointDofs(properties.mMimicDofProps); } @@ -274,6 +276,18 @@ const std::vector& Joint::getMimicDofProperties() const return mAspectProperties.mMimicDofProps; } +//============================================================================== +void Joint::setUseCouplerConstraint(bool enable) +{ + mAspectProperties.mUseCouplerConstraint = enable; +} + +//============================================================================== +bool Joint::isUsingCouplerConstraint() const +{ + return mAspectProperties.mUseCouplerConstraint; +} + //============================================================================== bool Joint::isKinematic() const { diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index f3996eec47591..e371014af2384 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -178,6 +178,12 @@ class DART_API Joint /// joint. const std::vector& getMimicDofProperties() 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/detail/JointAspect.hpp b/dart/dynamics/detail/JointAspect.hpp index 530e872159a7b..5147911d89e84 100644 --- a/dart/dynamics/detail/JointAspect.hpp +++ b/dart/dynamics/detail/JointAspect.hpp @@ -133,6 +133,9 @@ struct DART_API JointProperties /// Actuator type ActuatorType mActuatorType; + /// True if the joint should enforce mimic coupling via constraint. + bool mUseCouplerConstraint; + /// Vector of MimicDofProperties for each dependent DoF in the joint. std::vector mMimicDofProps; 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..749a3b2e0b5dc --- /dev/null +++ b/examples/coupler_constraint/CMakeLists.txt @@ -0,0 +1,29 @@ +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-osg) +set(required_libraries dart dart-gui-osg) + +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}) + +option(DART_COUPLER_CONSTRAINT_ENABLE_GUI "Launch the coupler constraint GUI example" OFF) +if(DART_COUPLER_CONSTRAINT_ENABLE_GUI) + message(STATUS "${example_name}: GUI support enabled") +else() + message(STATUS "${example_name}: building in headless mode (set DART_COUPLER_CONSTRAINT_ENABLE_GUI=ON to enable viewer)") + target_compile_definitions(${example_name} PRIVATE DART_COUPLER_HEADLESS_DEFAULT) +endif() diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp new file mode 100644 index 0000000000000..4f4bacaf0d723 --- /dev/null +++ b/examples/coupler_constraint/main.cpp @@ -0,0 +1,335 @@ +/* + * 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. + */ + +#ifdef DART_COUPLER_HEADLESS_DEFAULT + +#include +#include + +int main(int /*argc*/, char* /*argv*/[]) +{ + std::cout << "Coupler constraint GUI example built in headless mode; " + "skipping viewer startup." << std::endl; + return EXIT_SUCCESS; +} + +#else + +#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 +{ + +SkeletonPtr createCoupledPendulum() +{ + SkeletonPtr skeleton = Skeleton::create("coupled_pendulum"); + + // Reference link (root) + RevoluteJoint::Properties rootJointProps; + rootJointProps.mName = "reference_joint"; + rootJointProps.mAxis = Eigen::Vector3d::UnitZ(); + rootJointProps.mT_ParentBodyToJoint = Eigen::Isometry3d::Identity(); + rootJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); + + BodyNode::Properties rootBodyProps; + rootBodyProps.mName = "reference_body"; + dart::dynamics::Inertia rootInertia; + rootInertia.setMass(1.0); + rootInertia.setMoment(0.01, 0.01, 0.02, 0.0, 0.0, 0.0); + rootBodyProps.mInertia = rootInertia; + + auto rootPair = skeleton->createJointAndBodyNodePair( + nullptr, rootJointProps, rootBodyProps); + auto* referenceJoint = rootPair.first; + auto* referenceBody = rootPair.second; + + referenceJoint->setActuatorType(Joint::FORCE); + referenceJoint->setForceLowerLimit(0, -50.0); + referenceJoint->setForceUpperLimit(0, 50.0); + referenceJoint->setDampingCoefficient(0, 0.01); + + auto referenceShape = std::make_shared( + Eigen::Vector3d(0.3, 0.04, 0.04)); + auto referenceShapeNode = referenceBody->createShapeNodeWith< + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(referenceShape); + referenceShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); + referenceShapeNode->getVisualAspect()->setColor( + Eigen::Vector3d(0.3, 0.6, 0.9)); + + // Coupled link (follower) + RevoluteJoint::Properties followerJointProps; + followerJointProps.mName = "coupled_joint"; + followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); + followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.3, 0.0, 0.0); + followerJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); + + BodyNode::Properties followerBodyProps; + followerBodyProps.mName = "coupled_body"; + dart::dynamics::Inertia followerInertia; + followerInertia.setMass(1.0); + followerInertia.setMoment(0.01, 0.01, 0.02, 0.0, 0.0, 0.0); + followerBodyProps.mInertia = followerInertia; + + auto followerPair = skeleton->createJointAndBodyNodePair( + referenceBody, followerJointProps, followerBodyProps); + auto* coupledJoint = followerPair.first; + auto* coupledBody = followerPair.second; + + coupledJoint->setActuatorType(Joint::MIMIC); + coupledJoint->setMimicJoint(referenceJoint, -1.0, 0.0); + coupledJoint->setUseCouplerConstraint(true); + coupledJoint->setForceLowerLimit(0, -50.0); + coupledJoint->setForceUpperLimit(0, 50.0); + coupledJoint->setDampingCoefficient(0, 0.01); + + auto coupledShape = std::make_shared( + Eigen::Vector3d(0.3, 0.04, 0.04)); + auto coupledShapeNode = coupledBody->createShapeNodeWith< + dart::dynamics::VisualAspect, + dart::dynamics::CollisionAspect, + dart::dynamics::DynamicsAspect>(coupledShape); + coupledShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); + coupledShapeNode->getVisualAspect()->setColor( + Eigen::Vector3d(0.9, 0.4, 0.3)); + + // Provide a small initial offset so the constraint is active immediately + referenceJoint->setPosition(0, 15.0 * dart::math::constantsd::pi() / 180.0); + coupledJoint->setPosition(0, -15.0 * dart::math::constantsd::pi() / 180.0); + + return skeleton; +} + +class CouplerController +{ +public: + explicit CouplerController(SkeletonPtr skeleton) + : mSkeleton(std::move(skeleton)), + mInitialPositions(mSkeleton->getPositions()), + mTorqueSteps(0), + mCouplerEnabled(true) + { + assert(mSkeleton); + } + + void update() + { + // Clear any commands on the reference joint so it responds purely to + // incoming constraint impulses. + auto* referenceJoint = mSkeleton->getJoint("reference_joint"); + referenceJoint->setForce(0, 0.0); + + if (mTorqueSteps > 0) { + auto* coupledBody = mSkeleton->getBodyNode("coupled_body"); + coupledBody->addExtTorque(Eigen::Vector3d(0.0, 0.0, mTorqueMagnitude)); + --mTorqueSteps; + } + } + + void triggerFollowerImpulse() + { + mTorqueSteps = 240; + } + + void toggleCoupler() + { + mCouplerEnabled = !mCouplerEnabled; + auto* coupledJoint = mSkeleton->getJoint("coupled_joint"); + coupledJoint->setUseCouplerConstraint(mCouplerEnabled); + } + + bool isCouplerEnabled() const + { + return mCouplerEnabled; + } + + void reset() + { + mSkeleton->setPositions(mInitialPositions); + mSkeleton->setVelocities(Eigen::VectorXd::Zero(mSkeleton->getNumDofs())); + mSkeleton->clearExternalForces(); + mTorqueSteps = 0; + } + +private: + SkeletonPtr mSkeleton; + Eigen::VectorXd mInitialPositions; + int mTorqueSteps; + double mTorqueMagnitude{4.0}; + bool mCouplerEnabled; +}; + +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 'c': + mController->toggleCoupler(); + std::cout << (mController->isCouplerEnabled() ? "Coupler enabled" + : "Coupler disabled") + << std::endl; + return true; + case 'p': + mController->triggerFollowerImpulse(); + std::cout << "Applied torque pulse to the follower link" << std::endl; + return true; + case 'r': + mController->reset(); + std::cout << "Reset configuration" << std::endl; + return true; + default: + return false; + } + } + +private: + CouplerController* mController; +}; + +} // namespace + +int main(int /*argc*/, char* /*argv*/[]) +{ + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3d::Zero()); + world->setTimeStep(1e-3); + + SkeletonPtr skeleton = createCoupledPendulum(); + world->addSkeleton(skeleton); + + auto controller = std::make_unique(skeleton); + + auto* worldNode = new CouplerWorldNode(world, controller.get()); + auto* handler = new CouplerEventHandler(controller.get()); + + dart::gui::osg::Viewer viewer; + 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"); + viewer.addInstructionText("'p': apply torque pulse to the follower link"); + viewer.addInstructionText("'c': toggle the coupler constraint"); + viewer.addInstructionText("'r': reset configuration"); + std::cout << viewer.getInstructions() << std::endl; + + 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()); + + 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; +} + +#endif // DART_COUPLER_HEADLESS_DEFAULT diff --git a/pixi.toml b/pixi.toml index bd6b6e607f762..bd7ded0c89781 100644 --- a/pixi.toml +++ b/pixi.toml @@ -704,6 +704,24 @@ 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 test_Joints +./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/test_Joints +"""], depends-on = ["config"] } + +test-joints-coupler = { cmd = ["bash", "-lc", """ +set -euo pipefail +cmake \ + --build build/$PIXI_ENVIRONMENT_NAME/cpp/Release \ + --parallel \ + --target test_Joints +./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/test_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..a6e28857f87e3 100644 --- a/python/dartpy/dynamics/Joint.cpp +++ b/python/dartpy/dynamics/Joint.cpp @@ -100,6 +100,9 @@ void Joint(py::module& m) .def_readwrite( "mActuatorType", &dart::dynamics::detail::JointProperties::mActuatorType) + .def_readwrite( + "mUseCouplerConstraint", + &dart::dynamics::detail::JointProperties::mUseCouplerConstraint) .def_readwrite( "mMimicDofProps", &dart::dynamics::detail::JointProperties::mMimicDofProps); @@ -247,6 +250,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..d1eaa632079fb 100644 --- a/python/dartpy_docs/dynamics.py +++ b/python/dartpy_docs/dynamics.py @@ -3291,6 +3291,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 +3469,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: @@ -3553,6 +3557,7 @@ def setVelocityUpperLimits(self, upperLimits: numpy.ndarray[tuple[M, typing.Lite ... class JointProperties: mActuatorType: ActuatorType + mUseCouplerConstraint: bool mIsPositionLimitEnforced: bool mName: str mT_ChildBodyToJoint: dartpy.math.Isometry3 diff --git a/python/stubs/dartpy/dynamics.pyi b/python/stubs/dartpy/dynamics.pyi index bf0baef4a45f5..01118ca261f45 100644 --- a/python/stubs/dartpy/dynamics.pyi +++ b/python/stubs/dartpy/dynamics.pyi @@ -3372,6 +3372,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 +3550,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: @@ -3634,6 +3638,7 @@ class Joint(dartpy.common.Subject, EmbedProperties_Joint_JointProperties): ... class JointProperties: mActuatorType: ActuatorType + mUseCouplerConstraint: bool mIsPositionLimitEnforced: bool mName: str mT_ChildBodyToJoint: dartpy.math.Isometry3 diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index 89bce6ec8569a..afe09b5c2158d 100644 --- a/tests/integration/dynamics/test_Joints.cpp +++ b/tests/integration/dynamics/test_Joints.cpp @@ -1193,13 +1193,90 @@ 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, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) { const double timeStep = 1e-3; const double tol = 1e-2; From 5b71ede5fad2885ac4f576b97356742b68136092 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 4 Nov 2025 16:49:57 -0800 Subject: [PATCH 04/20] Fix formatting issues --- dart/constraint/ConstraintSolver.cpp | 2 +- dart/constraint/CouplerConstraint.cpp | 44 +++++++++------- dart/constraint/CouplerConstraint.hpp | 2 + examples/coupler_constraint/main.cpp | 73 ++++++++++++++------------- 4 files changed, 68 insertions(+), 53 deletions(-) diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index b88edba0a68fa..23200875515ac 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -42,9 +42,9 @@ #include "dart/common/Macros.hpp" #include "dart/common/Profile.hpp" #include "dart/constraint/ConstrainedGroup.hpp" -#include "dart/constraint/CouplerConstraint.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" diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index 3cb522ac470aa..f9de85a3dad9f 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -31,6 +31,7 @@ */ #include "dart/constraint/CouplerConstraint.hpp" + #include "dart/common/Logging.hpp" #include "dart/dynamics/BodyNode.hpp" #include "dart/dynamics/Joint.hpp" @@ -189,19 +190,24 @@ void CouplerConstraint::applyUnitImpulse(std::size_t index) mJoint->setConstraintImpulse(i, impulse); // Using const_cast to remove constness for methods that modify state - const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( - mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); + const_cast(mimicProp.mReferenceJoint) + ->setConstraintImpulse( + mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); skeleton->updateBiasImpulse(mBodyNode); - const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) - ->updateBiasImpulse(const_cast(mimicProp.mReferenceJoint->getChildBodyNode())); + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) + ->updateBiasImpulse(const_cast( + mimicProp.mReferenceJoint->getChildBodyNode())); skeleton->updateVelocityChange(); - const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) ->updateVelocityChange(); mJoint->setConstraintImpulse(i, 0.0); - const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); + const_cast(mimicProp.mReferenceJoint) + ->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); } ++localIndex; @@ -241,9 +247,9 @@ void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) void CouplerConstraint::excite() { mJoint->getSkeleton()->setImpulseApplied(true); - for (const auto& mimicProp : mMimicProps) - { - const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + for (const auto& mimicProp : mMimicProps) { + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) ->setImpulseApplied(true); } } @@ -252,9 +258,9 @@ void CouplerConstraint::excite() void CouplerConstraint::unexcite() { mJoint->getSkeleton()->setImpulseApplied(false); - for (const auto& mimicProp : mMimicProps) - { - const_cast(mimicProp.mReferenceJoint->getSkeleton().get()) + for (const auto& mimicProp : mMimicProps) { + const_cast( + mimicProp.mReferenceJoint->getSkeleton().get()) ->setImpulseApplied(false); } } @@ -274,10 +280,12 @@ void CouplerConstraint::applyImpulse(double* lambda) auto& mimicProp = mMimicProps[i]; // Using const_cast to remove constness for methods that modify state - const_cast(mimicProp.mReferenceJoint)->setConstraintImpulse( - mimicProp.mReferenceDofIndex, - mimicProp.mReferenceJoint->getConstraintImpulse(mimicProp.mReferenceDofIndex) - - lambda[localIndex] * mimicProp.mMultiplier); + const_cast(mimicProp.mReferenceJoint) + ->setConstraintImpulse( + mimicProp.mReferenceDofIndex, + mimicProp.mReferenceJoint->getConstraintImpulse( + mimicProp.mReferenceDofIndex) + - lambda[localIndex] * mimicProp.mMultiplier); mOldX[i] = lambda[localIndex]; @@ -318,8 +326,8 @@ void CouplerConstraint::uniteSkeletons() if (!referenceSkeletonConst) continue; - auto referenceSkeleton = std::const_pointer_cast( - referenceSkeletonConst); + auto referenceSkeleton + = std::const_pointer_cast(referenceSkeletonConst); if (!referenceSkeleton) continue; diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index 1b71825b106f0..01e12eb18b1c5 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -34,7 +34,9 @@ #define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ #include + #include + #include namespace dart { diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 4f4bacaf0d723..4d09b91da3f45 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -32,40 +32,45 @@ #ifdef DART_COUPLER_HEADLESS_DEFAULT -#include -#include + #include + + #include int main(int /*argc*/, char* /*argv*/[]) { std::cout << "Coupler constraint GUI example built in headless mode; " - "skipping viewer startup." << std::endl; + "skipping viewer startup." + << std::endl; return EXIT_SUCCESS; } #else -#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 + + #include + #include + #include + #include + + #include + #include + #include + + #include + #include using dart::dynamics::BodyNode; using dart::dynamics::BodyNodePtr; @@ -76,8 +81,7 @@ using dart::dynamics::SkeletonPtr; using dart::simulation::World; using dart::simulation::WorldPtr; -namespace -{ +namespace { SkeletonPtr createCoupledPendulum() { @@ -122,7 +126,8 @@ SkeletonPtr createCoupledPendulum() followerJointProps.mName = "coupled_joint"; followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.3, 0.0, 0.0); - followerJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); + followerJointProps.mT_ChildBodyToJoint + = Eigen::Translation3d(-0.15, 0.0, 0.0); BodyNode::Properties followerBodyProps; followerBodyProps.mName = "coupled_body"; @@ -150,8 +155,7 @@ SkeletonPtr createCoupledPendulum() dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(coupledShape); coupledShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); - coupledShapeNode->getVisualAspect()->setColor( - Eigen::Vector3d(0.9, 0.4, 0.3)); + coupledShapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.9, 0.4, 0.3)); // Provide a small initial offset so the constraint is active immediately referenceJoint->setPosition(0, 15.0 * dart::math::constantsd::pi() / 180.0); @@ -252,9 +256,10 @@ class CouplerEventHandler : public ::osgGA::GUIEventHandler switch (ea.getKey()) { case 'c': mController->toggleCoupler(); - std::cout << (mController->isCouplerEnabled() ? "Coupler enabled" - : "Coupler disabled") - << std::endl; + std::cout + << (mController->isCouplerEnabled() ? "Coupler enabled" + : "Coupler disabled") + << std::endl; return true; case 'p': mController->triggerFollowerImpulse(); @@ -329,7 +334,7 @@ int main(int /*argc*/, char* /*argv*/[]) return runResult; } -return 0; + return 0; } #endif // DART_COUPLER_HEADLESS_DEFAULT From 177176eda539a5e161cb65dac02e97c6b00e8f86 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 5 Nov 2025 06:05:25 -0800 Subject: [PATCH 05/20] Always run coupler constraint GUI --- examples/coupler_constraint/CMakeLists.txt | 8 ---- examples/coupler_constraint/main.cpp | 56 ++++++++-------------- 2 files changed, 19 insertions(+), 45 deletions(-) diff --git a/examples/coupler_constraint/CMakeLists.txt b/examples/coupler_constraint/CMakeLists.txt index 749a3b2e0b5dc..08a9d73ee7e74 100644 --- a/examples/coupler_constraint/CMakeLists.txt +++ b/examples/coupler_constraint/CMakeLists.txt @@ -19,11 +19,3 @@ 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}) - -option(DART_COUPLER_CONSTRAINT_ENABLE_GUI "Launch the coupler constraint GUI example" OFF) -if(DART_COUPLER_CONSTRAINT_ENABLE_GUI) - message(STATUS "${example_name}: GUI support enabled") -else() - message(STATUS "${example_name}: building in headless mode (set DART_COUPLER_CONSTRAINT_ENABLE_GUI=ON to enable viewer)") - target_compile_definitions(${example_name} PRIVATE DART_COUPLER_HEADLESS_DEFAULT) -endif() diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 4d09b91da3f45..4804edcbc7219 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -30,47 +30,31 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifdef DART_COUPLER_HEADLESS_DEFAULT +#include +#include - #include +#include - #include +#include +#include +#include +#include +#include +#include -int main(int /*argc*/, char* /*argv*/[]) -{ - std::cout << "Coupler constraint GUI example built in headless mode; " - "skipping viewer startup." - << std::endl; - return EXIT_SUCCESS; -} - -#else +#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; @@ -336,5 +320,3 @@ int main(int /*argc*/, char* /*argv*/[]) return 0; } - -#endif // DART_COUPLER_HEADLESS_DEFAULT From db42b0d9596fae845c67320b149f438f2a9d435d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sun, 9 Nov 2025 17:48:24 -0800 Subject: [PATCH 06/20] Improve coupler docs and example comparison --- dart/constraint/CouplerConstraint.hpp | 8 +- dart/constraint/MimicMotorConstraint.hpp | 6 +- examples/coupler_constraint/main.cpp | 567 +++++++++++++++++++---- 3 files changed, 488 insertions(+), 93 deletions(-) diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index 01e12eb18b1c5..dd243e3ec883a 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -42,8 +42,12 @@ namespace dart { namespace constraint { -/// Coupler constraint that couples the motions of two joints by applying -/// equal and opposite impulses based on MimicDofProperties. +/// 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 CouplerConstraint : public ConstraintBase { public: 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/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 4804edcbc7219..8dcaff3bc4dea 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -30,6 +30,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include +#include +#include #include #include @@ -38,20 +42,25 @@ #include #include #include +#include #include #include +#include #include #include #include #include +#include #include #include +#include #include #include #include +#include #include #include @@ -67,19 +76,38 @@ using dart::simulation::WorldPtr; namespace { -SkeletonPtr createCoupledPendulum() +struct MimicAssembly { - SkeletonPtr skeleton = Skeleton::create("coupled_pendulum"); + dart::dynamics::SkeletonPtr skeleton; + dart::dynamics::Joint* referenceJoint; + dart::dynamics::Joint* followerJoint; + dart::dynamics::BodyNode* referenceBody; + dart::dynamics::BodyNode* followerBody; +}; + +MimicAssembly createMimicAssembly( + const std::string& label, + const Eigen::Vector3d& baseOffset, + bool useCouplerConstraint, + const Eigen::Vector3d& referenceColor, + const Eigen::Vector3d& followerColor) +{ + SkeletonPtr skeleton = Skeleton::create(label + "_rig"); + + 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"; // Reference link (root) RevoluteJoint::Properties rootJointProps; - rootJointProps.mName = "reference_joint"; + rootJointProps.mName = referenceJointName; rootJointProps.mAxis = Eigen::Vector3d::UnitZ(); - rootJointProps.mT_ParentBodyToJoint = Eigen::Isometry3d::Identity(); + rootJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(baseOffset); rootJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); BodyNode::Properties rootBodyProps; - rootBodyProps.mName = "reference_body"; + rootBodyProps.mName = referenceBodyName; dart::dynamics::Inertia rootInertia; rootInertia.setMass(1.0); rootInertia.setMoment(0.01, 0.01, 0.02, 0.0, 0.0, 0.0); @@ -90,7 +118,7 @@ SkeletonPtr createCoupledPendulum() auto* referenceJoint = rootPair.first; auto* referenceBody = rootPair.second; - referenceJoint->setActuatorType(Joint::FORCE); + referenceJoint->setActuatorType(Joint::PASSIVE); referenceJoint->setForceLowerLimit(0, -50.0); referenceJoint->setForceUpperLimit(0, 50.0); referenceJoint->setDampingCoefficient(0, 0.01); @@ -102,19 +130,18 @@ SkeletonPtr createCoupledPendulum() dart::dynamics::CollisionAspect, dart::dynamics::DynamicsAspect>(referenceShape); referenceShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); - referenceShapeNode->getVisualAspect()->setColor( - Eigen::Vector3d(0.3, 0.6, 0.9)); + referenceShapeNode->getVisualAspect()->setColor(referenceColor); - // Coupled link (follower) + // Dependent link (follower) RevoluteJoint::Properties followerJointProps; - followerJointProps.mName = "coupled_joint"; + followerJointProps.mName = followerJointName; followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.3, 0.0, 0.0); followerJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); BodyNode::Properties followerBodyProps; - followerBodyProps.mName = "coupled_body"; + followerBodyProps.mName = followerBodyName; dart::dynamics::Inertia followerInertia; followerInertia.setMass(1.0); followerInertia.setMoment(0.01, 0.01, 0.02, 0.0, 0.0, 0.0); @@ -122,89 +149,276 @@ SkeletonPtr createCoupledPendulum() auto followerPair = skeleton->createJointAndBodyNodePair( referenceBody, followerJointProps, followerBodyProps); - auto* coupledJoint = followerPair.first; - auto* coupledBody = followerPair.second; + auto* followerJoint = followerPair.first; + auto* followerBody = followerPair.second; - coupledJoint->setActuatorType(Joint::MIMIC); - coupledJoint->setMimicJoint(referenceJoint, -1.0, 0.0); - coupledJoint->setUseCouplerConstraint(true); - coupledJoint->setForceLowerLimit(0, -50.0); - coupledJoint->setForceUpperLimit(0, 50.0); - coupledJoint->setDampingCoefficient(0, 0.01); + followerJoint->setActuatorType(Joint::MIMIC); + followerJoint->setMimicJoint(referenceJoint, -1.0, 0.0); + followerJoint->setUseCouplerConstraint(useCouplerConstraint); + followerJoint->setForceLowerLimit(0, -50.0); + followerJoint->setForceUpperLimit(0, 50.0); + followerJoint->setDampingCoefficient(0, 0.01); - auto coupledShape = std::make_shared( + auto followerShape = std::make_shared( Eigen::Vector3d(0.3, 0.04, 0.04)); - auto coupledShapeNode = coupledBody->createShapeNodeWith< + auto followerShapeNode = followerBody->createShapeNodeWith< dart::dynamics::VisualAspect, dart::dynamics::CollisionAspect, - dart::dynamics::DynamicsAspect>(coupledShape); - coupledShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); - coupledShapeNode->getVisualAspect()->setColor(Eigen::Vector3d(0.9, 0.4, 0.3)); + dart::dynamics::DynamicsAspect>(followerShape); + followerShapeNode->setRelativeTranslation(Eigen::Vector3d(0.15, 0.0, 0.0)); + followerShapeNode->getVisualAspect()->setColor(followerColor); - // Provide a small initial offset so the constraint is active immediately referenceJoint->setPosition(0, 15.0 * dart::math::constantsd::pi() / 180.0); - coupledJoint->setPosition(0, -15.0 * dart::math::constantsd::pi() / 180.0); + followerJoint->setPosition(0, -15.0 * dart::math::constantsd::pi() / 180.0); - return skeleton; + return {skeleton, referenceJoint, followerJoint, referenceBody, followerBody}; } class CouplerController { public: - explicit CouplerController(SkeletonPtr skeleton) - : mSkeleton(std::move(skeleton)), - mInitialPositions(mSkeleton->getPositions()), - mTorqueSteps(0), - mCouplerEnabled(true) + 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; + }; + + struct PairStatus { - assert(mSkeleton); + std::string label; + bool usesCoupler; + bool couplerEnabled; + double referencePosition; + double followerPosition; + double desiredFollowerPosition; + double positionError; + bool torquePulseActive; + int remainingTorqueSteps; + int stepsUntilNextPulse; + }; + + 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(); + + mPairs.push_back(std::move(pair)); + refreshPairVisual(mPairs.back()); } void update() { - // Clear any commands on the reference joint so it responds purely to - // incoming constraint impulses. - auto* referenceJoint = mSkeleton->getJoint("reference_joint"); - referenceJoint->setForce(0, 0.0); - - if (mTorqueSteps > 0) { - auto* coupledBody = mSkeleton->getBodyNode("coupled_body"); - coupledBody->addExtTorque(Eigen::Vector3d(0.0, 0.0, mTorqueMagnitude)); - --mTorqueSteps; + for (auto& pair : mPairs) { + if (pair.referenceJoint) + pair.referenceJoint->setForce(0, 0.0); + applyTorqueToFollower(pair); + refreshPairVisual(pair); + } + + if (mAutoPulseEnabled) { + ++mAutoPulseCounter; + if (mAutoPulseCounter >= mAutoPulseIntervalSteps) { + triggerFollowerImpulse(false); + mAutoPulseCounter = 0; + } } } - void triggerFollowerImpulse() + void triggerFollowerImpulse(bool announce = true) { - mTorqueSteps = 240; + for (auto& pair : mPairs) + pair.torqueStepsRemaining = mPulseDurationSteps; + + if (announce) { + std::cout << "Applied torque pulse to both follower links" << std::endl; + } } void toggleCoupler() { - mCouplerEnabled = !mCouplerEnabled; - auto* coupledJoint = mSkeleton->getJoint("coupled_joint"); - coupledJoint->setUseCouplerConstraint(mCouplerEnabled); + for (auto& pair : mPairs) { + if (!pair.usesCoupler) + continue; + pair.couplerEnabled = !pair.couplerEnabled; + pair.followerJoint->setUseCouplerConstraint(pair.couplerEnabled); + std::cout << pair.label + << (pair.couplerEnabled ? ": coupler enabled" + : ": coupler disabled") + << std::endl; + } + } + + void toggleAutoPulse() + { + enableAutoPulse(!mAutoPulseEnabled); + } + + void enableAutoPulse(bool enable) + { + mAutoPulseEnabled = enable; + mAutoPulseCounter = 0; + std::cout + << (mAutoPulseEnabled ? "Auto pulse enabled" : "Auto pulse disabled") + << std::endl; } - bool isCouplerEnabled() const + bool isAutoPulseEnabled() const { - return mCouplerEnabled; + return mAutoPulseEnabled; + } + + int getStepsUntilNextPulse() const + { + if (!mAutoPulseEnabled) + return 0; + return std::max(0, mAutoPulseIntervalSteps - mAutoPulseCounter); } void reset() { - mSkeleton->setPositions(mInitialPositions); - mSkeleton->setVelocities(Eigen::VectorXd::Zero(mSkeleton->getNumDofs())); - mSkeleton->clearExternalForces(); - mTorqueSteps = 0; + 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(); + pair.torqueStepsRemaining = 0; + refreshPairVisual(pair); + } + mAutoPulseCounter = 0; + } + + 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.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.torquePulseActive = pair.torqueStepsRemaining > 0; + status.remainingTorqueSteps = pair.torqueStepsRemaining; + status.stepsUntilNextPulse = getStepsUntilNextPulse(); + + result.push_back(status); + } + + return result; } private: - SkeletonPtr mSkeleton; - Eigen::VectorXd mInitialPositions; - int mTorqueSteps; - double mTorqueMagnitude{4.0}; - bool mCouplerEnabled; + 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; + int torqueStepsRemaining{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 applyTorqueToFollower(PairData& pair) + { + if (pair.torqueStepsRemaining <= 0 || pair.followerBody == nullptr) + return; + + pair.followerBody->addExtTorque( + Eigen::Vector3d(0.0, 0.0, mTorqueMagnitude)); + --pair.torqueStepsRemaining; + } + + std::vector mPairs; + bool mAutoPulseEnabled{false}; + int mAutoPulseIntervalSteps{1200}; + int mAutoPulseCounter{0}; + int mPulseDurationSteps{240}; + double mTorqueMagnitude{50.0}; }; class CouplerWorldNode : public dart::gui::osg::RealTimeWorldNode @@ -240,18 +454,17 @@ class CouplerEventHandler : public ::osgGA::GUIEventHandler switch (ea.getKey()) { case 'c': mController->toggleCoupler(); - std::cout - << (mController->isCouplerEnabled() ? "Coupler enabled" - : "Coupler disabled") - << std::endl; return true; case 'p': mController->triggerFollowerImpulse(); - std::cout << "Applied torque pulse to the follower link" << std::endl; return true; case 'r': mController->reset(); - std::cout << "Reset configuration" << std::endl; + std::cout << "Reset both mimic pairs to their initial configuration" + << std::endl; + return true; + case 'a': + mController->toggleAutoPulse(); return true; default: return false; @@ -262,6 +475,88 @@ class CouplerEventHandler : public ::osgGA::GUIEventHandler 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: bilateral CouplerConstraint (red/blue). Right: mimic motor " + "(purple/teal). Both receive the same follower torque input."); + ImGui::Separator(); + + ImGui::Text( + "Automatic pulses: %s", + mController->isAutoPulseEnabled() ? "enabled" : "disabled"); + if (mController->isAutoPulseEnabled()) { + ImGui::Text( + "Next pulse in %d steps", mController->getStepsUntilNextPulse()); + } else { + ImGui::TextUnformatted("Press 'p' to inject a pulse."); + } + + 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: %6.2f deg", status.referencePosition * radToDeg); + ImGui::Text("Follower: %6.2f deg", status.followerPosition * radToDeg); + ImGui::Text( + "Desired: %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.torquePulseActive) { + ImGui::Text( + "Pulse active (%d steps remain)", status.remainingTorqueSteps); + } + } + + 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*/[]) @@ -270,48 +565,139 @@ int main(int /*argc*/, char* /*argv*/[]) world->setGravity(Eigen::Vector3d::Zero()); world->setTimeStep(1e-3); - SkeletonPtr skeleton = createCoupledPendulum(); - world->addSkeleton(skeleton); - - auto controller = std::make_unique(skeleton); - - auto* worldNode = new CouplerWorldNode(world, controller.get()); - auto* handler = new CouplerEventHandler(controller.get()); - - dart::gui::osg::Viewer viewer; + 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)); + 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->addSkeleton(couplerAssembly.skeleton); + world->addSkeleton(motorAssembly.skeleton); + + std::cout + << "Coupler constraint demo:\n" + << " • Left pair uses the bilateral CouplerConstraint (red/blue).\n" + << " • Right pair uses the legacy MimicMotorConstraint (purple/teal).\n" + << "Both followers receive the same external torque so you can observe " + << "how the bilateral constraint reacts on the reference joint.\n" + << "Press 'p' to inject a torque pulse, 'c' to disable/enable the " + "coupler on the left pair, and 'a' to toggle periodic pulses.\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, + }); + + 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, + }); + + controller->enableAutoPulse(true); + controller->triggerFollowerImpulse(false); + + ::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"); - viewer.addInstructionText("'p': apply torque pulse to the follower link"); - viewer.addInstructionText("'c': toggle the coupler constraint"); - viewer.addInstructionText("'r': reset configuration"); - std::cout << viewer.getInstructions() << std::endl; - - viewer.setUpViewInWindow(0, 0, 960, 720); - viewer.getCameraManipulator()->setHomePosition( + viewer->addWorldNode(worldNode); + viewer->addEventHandler(handler); + + viewer->addInstructionText("space: toggle simulation (auto-starts)\n"); + viewer->addInstructionText("'p': apply torque pulse to both followers\n"); + viewer->addInstructionText("'c': toggle the coupler on the left pair\n"); + viewer->addInstructionText("'r': reset both pairs\n"); + viewer->addInstructionText("'a': toggle automatic torque pulses\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->setCameraManipulator(viewer->getCameraManipulator()); + + viewer->simulate(true); + + viewer->getImGuiHandler()->addWidget( + std::make_shared(viewer.get(), controller.get())); - if (!viewer.isRealized()) - viewer.realize(); + if (!viewer->isRealized()) + viewer->realize(); osg::ref_ptr gc - = viewer.getCamera() ? viewer.getCamera()->getGraphicsContext() : nullptr; - if (!viewer.isRealized() || !gc || !gc->valid()) { + = 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(); + const int runResult = viewer->run(); if (runResult != 0) { std::cerr << "OSG viewer exited early (status " << runResult << "). Ensure a valid OpenGL context is available.\n"; @@ -320,3 +706,4 @@ int main(int /*argc*/, char* /*argv*/[]) return 0; } +#include From bb713e26e3c1e407cc8654d1cba1578286eaff8c Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Mon, 17 Nov 2025 23:51:05 -0800 Subject: [PATCH 07/20] Improve coupler example and docs --- docs/onboarding/README.md | 4 +- docs/onboarding/architecture.md | 3 +- docs/onboarding/constraints.md | 26 ++- docs/onboarding/dynamics.md | 4 + examples/coupler_constraint/main.cpp | 297 ++++++++++++--------------- pixi.toml | 8 +- 6 files changed, 173 insertions(+), 169 deletions(-) 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/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 8dcaff3bc4dea..fcc527a865648 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -47,6 +47,7 @@ #include #include #include +#include #include @@ -94,82 +95,100 @@ MimicAssembly createMimicAssembly( { 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"; - // Reference link (root) - RevoluteJoint::Properties rootJointProps; - rootJointProps.mName = referenceJointName; - rootJointProps.mAxis = Eigen::Vector3d::UnitZ(); - rootJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(baseOffset); - rootJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.15, 0.0, 0.0); - - BodyNode::Properties rootBodyProps; - rootBodyProps.mName = referenceBodyName; - dart::dynamics::Inertia rootInertia; - rootInertia.setMass(1.0); - rootInertia.setMoment(0.01, 0.01, 0.02, 0.0, 0.0, 0.0); - rootBodyProps.mInertia = rootInertia; - - auto rootPair = skeleton->createJointAndBodyNodePair( - nullptr, rootJointProps, rootBodyProps); - auto* referenceJoint = rootPair.first; - auto* referenceBody = rootPair.second; - - referenceJoint->setActuatorType(Joint::PASSIVE); - referenceJoint->setForceLowerLimit(0, -50.0); - referenceJoint->setForceUpperLimit(0, 50.0); - referenceJoint->setDampingCoefficient(0, 0.01); + 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.3, 0.04, 0.04)); + 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.15, 0.0, 0.0)); + referenceShapeNode->setRelativeTranslation(Eigen::Vector3d(0.25, 0.0, 0.0)); referenceShapeNode->getVisualAspect()->setColor(referenceColor); - // Dependent link (follower) RevoluteJoint::Properties followerJointProps; followerJointProps.mName = followerJointName; followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); - followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.3, 0.0, 0.0); + followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.0, -0.15, 0.0); followerJointProps.mT_ChildBodyToJoint - = Eigen::Translation3d(-0.15, 0.0, 0.0); + = 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.01, 0.01, 0.02, 0.0, 0.0, 0.0); + followerInertia.setMoment(0.02, 0.02, 0.03, 0.0, 0.0, 0.0); followerBodyProps.mInertia = followerInertia; auto followerPair = skeleton->createJointAndBodyNodePair( - referenceBody, followerJointProps, followerBodyProps); + 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, -50.0); - followerJoint->setForceUpperLimit(0, 50.0); - followerJoint->setDampingCoefficient(0, 0.01); + 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.3, 0.04, 0.04)); + 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.15, 0.0, 0.0)); + followerShapeNode->setRelativeTranslation(Eigen::Vector3d(0.25, 0.0, 0.0)); followerShapeNode->getVisualAspect()->setColor(followerColor); - referenceJoint->setPosition(0, 15.0 * dart::math::constantsd::pi() / 180.0); - followerJoint->setPosition(0, -15.0 * dart::math::constantsd::pi() / 180.0); + referenceJoint->setPosition(0, 0.0); + followerJoint->setPosition(0, 0.0); return {skeleton, referenceJoint, followerJoint, referenceBody, followerBody}; } @@ -189,6 +208,10 @@ class CouplerController 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 @@ -196,13 +219,15 @@ class CouplerController std::string label; bool usesCoupler; bool couplerEnabled; + double targetAngle; double referencePosition; double followerPosition; double desiredFollowerPosition; double positionError; - bool torquePulseActive; - int remainingTorqueSteps; - int stepsUntilNextPulse; + double followerLowerLimit; + double followerUpperLimit; + bool followerAtLowerLimit; + bool followerAtUpperLimit; }; void addPair(PairRegistration registration) @@ -223,6 +248,10 @@ class CouplerController 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()); @@ -231,69 +260,9 @@ class CouplerController void update() { for (auto& pair : mPairs) { - if (pair.referenceJoint) - pair.referenceJoint->setForce(0, 0.0); - applyTorqueToFollower(pair); + driveReferenceJoint(pair); refreshPairVisual(pair); } - - if (mAutoPulseEnabled) { - ++mAutoPulseCounter; - if (mAutoPulseCounter >= mAutoPulseIntervalSteps) { - triggerFollowerImpulse(false); - mAutoPulseCounter = 0; - } - } - } - - void triggerFollowerImpulse(bool announce = true) - { - for (auto& pair : mPairs) - pair.torqueStepsRemaining = mPulseDurationSteps; - - if (announce) { - std::cout << "Applied torque pulse to both follower links" << std::endl; - } - } - - void toggleCoupler() - { - for (auto& pair : mPairs) { - if (!pair.usesCoupler) - continue; - pair.couplerEnabled = !pair.couplerEnabled; - pair.followerJoint->setUseCouplerConstraint(pair.couplerEnabled); - std::cout << pair.label - << (pair.couplerEnabled ? ": coupler enabled" - : ": coupler disabled") - << std::endl; - } - } - - void toggleAutoPulse() - { - enableAutoPulse(!mAutoPulseEnabled); - } - - void enableAutoPulse(bool enable) - { - mAutoPulseEnabled = enable; - mAutoPulseCounter = 0; - std::cout - << (mAutoPulseEnabled ? "Auto pulse enabled" : "Auto pulse disabled") - << std::endl; - } - - bool isAutoPulseEnabled() const - { - return mAutoPulseEnabled; - } - - int getStepsUntilNextPulse() const - { - if (!mAutoPulseEnabled) - return 0; - return std::max(0, mAutoPulseIntervalSteps - mAutoPulseCounter); } void reset() @@ -305,10 +274,7 @@ class CouplerController pair.skeleton->setVelocities( Eigen::VectorXd::Zero(pair.skeleton->getNumDofs())); pair.skeleton->clearExternalForces(); - pair.torqueStepsRemaining = 0; - refreshPairVisual(pair); } - mAutoPulseCounter = 0; } std::vector getStatuses() const @@ -321,6 +287,7 @@ class CouplerController 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 @@ -329,9 +296,20 @@ class CouplerController = pair.followerJoint ? computeDesiredFollowerPosition(pair) : 0.0; status.positionError = status.followerPosition - status.desiredFollowerPosition; - status.torquePulseActive = pair.torqueStepsRemaining > 0; - status.remainingTorqueSteps = pair.torqueStepsRemaining; - status.stepsUntilNextPulse = getStepsUntilNextPulse(); + 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); } @@ -354,7 +332,10 @@ class CouplerController dart::dynamics::LineSegmentShapePtr linkShape; dart::dynamics::VisualAspect* linkVisual{nullptr}; Eigen::VectorXd initialPositions; - int torqueStepsRemaining{0}; + double targetAngle{0.0}; + double torqueLimit{80.0}; + double kp{300.0}; + double kd{20.0}; }; double computeDesiredFollowerPosition(const PairData& pair) const @@ -403,22 +384,19 @@ class CouplerController pair.linkVisual->setColor(color); } - void applyTorqueToFollower(PairData& pair) + void driveReferenceJoint(PairData& pair) { - if (pair.torqueStepsRemaining <= 0 || pair.followerBody == nullptr) + if (pair.referenceJoint == nullptr) return; - pair.followerBody->addExtTorque( - Eigen::Vector3d(0.0, 0.0, mTorqueMagnitude)); - --pair.torqueStepsRemaining; + 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; - bool mAutoPulseEnabled{false}; - int mAutoPulseIntervalSteps{1200}; - int mAutoPulseCounter{0}; - int mPulseDurationSteps{240}; - double mTorqueMagnitude{50.0}; }; class CouplerWorldNode : public dart::gui::osg::RealTimeWorldNode @@ -452,20 +430,11 @@ class CouplerEventHandler : public ::osgGA::GUIEventHandler return false; switch (ea.getKey()) { - case 'c': - mController->toggleCoupler(); - return true; - case 'p': - mController->triggerFollowerImpulse(); - return true; case 'r': mController->reset(); - std::cout << "Reset both mimic pairs to their initial configuration" + std::cout << "Reset both rigs to their initial configuration" << std::endl; return true; - case 'a': - mController->toggleAutoPulse(); - return true; default: return false; } @@ -505,20 +474,11 @@ class CouplerOverlay : public dart::gui::osg::ImGuiWidget const auto statuses = mController->getStatuses(); ImGui::TextWrapped( - "Left: bilateral CouplerConstraint (red/blue). Right: mimic motor " - "(purple/teal). Both receive the same follower torque input."); + "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(); - ImGui::Text( - "Automatic pulses: %s", - mController->isAutoPulseEnabled() ? "enabled" : "disabled"); - if (mController->isAutoPulseEnabled()) { - ImGui::Text( - "Next pulse in %d steps", mController->getStepsUntilNextPulse()); - } else { - ImGui::TextUnformatted("Press 'p' to inject a pulse."); - } - for (const auto& status : statuses) { ImGui::Separator(); ImGui::TextColored( @@ -529,19 +489,31 @@ class CouplerOverlay : public dart::gui::osg::ImGuiWidget ImGui::Text( "Constraint: %s", status.usesCoupler ? "Coupler (bilateral)" : "Mimic motor (servo)"); - ImGui::Text("Reference: %6.2f deg", status.referencePosition * radToDeg); - ImGui::Text("Follower: %6.2f deg", status.followerPosition * radToDeg); ImGui::Text( - "Desired: %6.2f deg", status.desiredFollowerPosition * radToDeg); + "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", + "Error: %+6.3f deg", status.positionError * radToDeg); - if (status.torquePulseActive) { - ImGui::Text( - "Pulse active (%d steps remain)", status.remainingTorqueSteps); + 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"); } } @@ -580,15 +552,17 @@ int main(int /*argc*/, char* /*argv*/[]) 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 pair uses the bilateral CouplerConstraint (red/blue).\n" - << " • Right pair uses the legacy MimicMotorConstraint (purple/teal).\n" - << "Both followers receive the same external torque so you can observe " - << "how the bilateral constraint reacts on the reference joint.\n" - << "Press 'p' to inject a torque pulse, 'c' to disable/enable the " - "coupler on the left pair, and 'a' to toggle periodic pulses.\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(); @@ -626,6 +600,10 @@ int main(int /*argc*/, char* /*argv*/[]) .linkFrame = couplerFrame, .linkShape = couplerLine, .linkVisual = couplerVisual, + .targetAngle = targetAngle, + .torqueLimit = 90.0, + .proportionalGain = 320.0, + .dampingGain = 25.0, }); controller->addPair({ @@ -639,11 +617,12 @@ int main(int /*argc*/, char* /*argv*/[]) .linkFrame = motorFrame, .linkShape = motorLine, .linkVisual = motorVisual, + .targetAngle = targetAngle, + .torqueLimit = 90.0, + .proportionalGain = 320.0, + .dampingGain = 25.0, }); - controller->enableAutoPulse(true); - controller->triggerFollowerImpulse(false); - ::osg::ref_ptr worldNode = new CouplerWorldNode(world, controller.get()); ::osg::ref_ptr handler @@ -660,10 +639,7 @@ int main(int /*argc*/, char* /*argv*/[]) viewer->addEventHandler(handler); viewer->addInstructionText("space: toggle simulation (auto-starts)\n"); - viewer->addInstructionText("'p': apply torque pulse to both followers\n"); - viewer->addInstructionText("'c': toggle the coupler on the left pair\n"); - viewer->addInstructionText("'r': reset both pairs\n"); - viewer->addInstructionText("'a': toggle automatic torque pulses\n"); + viewer->addInstructionText("'r': reset both rigs\n"); std::cout << viewer->getInstructions() << std::endl; auto grid = ::osg::ref_ptr( @@ -706,4 +682,3 @@ int main(int /*argc*/, char* /*argv*/[]) return 0; } -#include diff --git a/pixi.toml b/pixi.toml index bd7ded0c89781..c6f49e656caf3 100644 --- a/pixi.toml +++ b/pixi.toml @@ -709,8 +709,8 @@ set -euo pipefail cmake \ --build build/$PIXI_ENVIRONMENT_NAME/cpp/Release \ --parallel \ - --target test_Joints -./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/test_Joints + --target INTEGRATION_dynamics_Joints +./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/INTEGRATION_dynamics_Joints """], depends-on = ["config"] } test-joints-coupler = { cmd = ["bash", "-lc", """ @@ -718,8 +718,8 @@ set -euo pipefail cmake \ --build build/$PIXI_ENVIRONMENT_NAME/cpp/Release \ --parallel \ - --target test_Joints -./build/$PIXI_ENVIRONMENT_NAME/cpp/Release/tests/integration/test_Joints --gtest_filter=JOINTS.MIMIC_JOINT:JOINTS.MIMIC_JOINT_COUPLER + --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 = [ From 2a45e3855f01e6ef1cfe78caff899902d92bdd15 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 18 Nov 2025 05:48:31 -0800 Subject: [PATCH 08/20] Address coupler constraint review --- dart/constraint/CouplerConstraint.cpp | 74 ++++++++++++---------- tests/integration/dynamics/test_Joints.cpp | 42 ++++++++++++ 2 files changed, 82 insertions(+), 34 deletions(-) diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index f9de85a3dad9f..cee5dc1b71e42 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -33,6 +33,7 @@ #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" @@ -54,9 +55,9 @@ CouplerConstraint::CouplerConstraint( mBodyNode(joint->getChildBodyNode()), mAppliedImpulseIndex(0) { - assert(joint); - assert(joint->getNumDofs() <= mMimicProps.size()); - assert(mBodyNode); + DART_ASSERT(joint); + DART_ASSERT(joint->getNumDofs() <= mMimicProps.size()); + DART_ASSERT(mBodyNode); std::fill(mLifeTime, mLifeTime + 6, 0); std::fill(mActive, mActive + 6, false); @@ -151,13 +152,13 @@ void CouplerConstraint::getInformation(ConstraintInfo* lcp) if (mActive[i] == false) continue; - assert(lcp->w[index] == 0.0); + DART_ASSERT(lcp->w[index] == 0.0); lcp->b[index] = mNegativeVelocityError[i]; lcp->lo[index] = mLowerBound[i]; lcp->hi[index] = mUpperBound[i]; - assert(lcp->findex[index] == -1); + DART_ASSERT(lcp->findex[index] == -1); if (mLifeTime[i]) lcp->x[index] = mOldX[i]; @@ -171,10 +172,10 @@ void CouplerConstraint::getInformation(ConstraintInfo* lcp) //============================================================================== void CouplerConstraint::applyUnitImpulse(std::size_t index) { - assert(index < mDim && "Invalid Index."); + DART_ASSERT(index < mDim && "Invalid Index."); std::size_t localIndex = 0; - const dynamics::SkeletonPtr& skeleton = mJoint->getSkeleton(); + const dynamics::SkeletonPtr& dependentSkeleton = mJoint->getSkeleton(); std::size_t dof = mJoint->getNumDofs(); for (std::size_t i = 0; i < dof; ++i) { @@ -183,31 +184,37 @@ void CouplerConstraint::applyUnitImpulse(std::size_t index) if (localIndex == index) { const auto& mimicProp = mMimicProps[i]; - - skeleton->clearConstraintImpulses(); + 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); - // Using const_cast to remove constness for methods that modify state - const_cast(mimicProp.mReferenceJoint) - ->setConstraintImpulse( - mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); + referenceJoint->setConstraintImpulse( + mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier); - skeleton->updateBiasImpulse(mBodyNode); - const_cast( - mimicProp.mReferenceJoint->getSkeleton().get()) - ->updateBiasImpulse(const_cast( - mimicProp.mReferenceJoint->getChildBodyNode())); + dependentSkeleton->updateBiasImpulse(mBodyNode); + referenceSkeleton->updateBiasImpulse(referenceBodyNode); - skeleton->updateVelocityChange(); - const_cast( - mimicProp.mReferenceJoint->getSkeleton().get()) - ->updateVelocityChange(); + dependentSkeleton->updateVelocityChange(); + referenceSkeleton->updateVelocityChange(); mJoint->setConstraintImpulse(i, 0.0); - const_cast(mimicProp.mReferenceJoint) - ->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); + referenceJoint->setConstraintImpulse(mimicProp.mReferenceDofIndex, 0.0); } ++localIndex; @@ -219,7 +226,7 @@ void CouplerConstraint::applyUnitImpulse(std::size_t index) //============================================================================== void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) { - assert(delVel != nullptr && "Null pointer is not allowed."); + DART_ASSERT(delVel != nullptr && "Null pointer is not allowed."); std::size_t localIndex = 0; std::size_t dof = mJoint->getNumDofs(); @@ -240,7 +247,7 @@ void CouplerConstraint::getVelocityChange(double* delVel, bool withCfm) += delVel[mAppliedImpulseIndex] * mConstraintForceMixing; } - assert(localIndex == mDim); + DART_ASSERT(localIndex == mDim); } //============================================================================== @@ -278,14 +285,13 @@ void CouplerConstraint::applyImpulse(double* lambda) i, mJoint->getConstraintImpulse(i) + lambda[localIndex]); auto& mimicProp = mMimicProps[i]; - - // Using const_cast to remove constness for methods that modify state - const_cast(mimicProp.mReferenceJoint) - ->setConstraintImpulse( - mimicProp.mReferenceDofIndex, - mimicProp.mReferenceJoint->getConstraintImpulse( - mimicProp.mReferenceDofIndex) - - lambda[localIndex] * mimicProp.mMultiplier); + 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]; diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index afe09b5c2158d..eb463a616c382 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" @@ -1275,6 +1276,47 @@ 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); + + dart::constraint::CouplerConstraint 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) { From c6354f4a4b988db5fd4f753e50d5492d70dd8dbd Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 18 Nov 2025 05:50:16 -0800 Subject: [PATCH 09/20] Format code --- dart/constraint/CouplerConstraint.cpp | 10 ++++----- examples/coupler_constraint/main.cpp | 30 ++++++++++++++------------- pixi.toml | 20 ++++++++++++++---- 3 files changed, 36 insertions(+), 24 deletions(-) diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index cee5dc1b71e42..fbb02a13ce8f8 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -186,12 +186,10 @@ void CouplerConstraint::applyUnitImpulse(std::size_t 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()); + 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); diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index fcc527a865648..f41f60f3eb5c6 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -113,15 +113,18 @@ MimicAssembly createMimicAssembly( 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 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); + 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; @@ -152,7 +155,8 @@ MimicAssembly createMimicAssembly( RevoluteJoint::Properties followerJointProps; followerJointProps.mName = followerJointName; followerJointProps.mAxis = Eigen::Vector3d::UnitZ(); - followerJointProps.mT_ParentBodyToJoint = Eigen::Translation3d(0.0, -0.15, 0.0); + followerJointProps.mT_ParentBodyToJoint + = Eigen::Translation3d(0.0, -0.15, 0.0); followerJointProps.mT_ChildBodyToJoint = Eigen::Translation3d(-0.25, 0.0, 0.0); @@ -297,13 +301,11 @@ class CouplerController status.positionError = status.followerPosition - status.desiredFollowerPosition; status.followerLowerLimit - = pair.followerJoint - ? pair.followerJoint->getPositionLowerLimit(0) - : 0.0; + = pair.followerJoint ? pair.followerJoint->getPositionLowerLimit(0) + : 0.0; status.followerUpperLimit - = pair.followerJoint - ? pair.followerJoint->getPositionUpperLimit(0) - : 0.0; + = pair.followerJoint ? pair.followerJoint->getPositionUpperLimit(0) + : 0.0; const double lowerSlack = status.followerPosition - status.followerLowerLimit; const double upperSlack @@ -391,7 +393,8 @@ class CouplerController const double position = pair.referenceJoint->getPosition(0); const double velocity = pair.referenceJoint->getVelocity(0); - double torque = pair.kp * (pair.targetAngle - position) - pair.kd * velocity; + double torque + = pair.kp * (pair.targetAngle - position) - pair.kd * velocity; torque = dart::math::clip(torque, -pair.torqueLimit, pair.torqueLimit); pair.referenceJoint->setForce(0, torque); } @@ -489,8 +492,7 @@ class CouplerOverlay : public dart::gui::osg::ImGuiWidget ImGui::Text( "Constraint: %s", status.usesCoupler ? "Coupler (bilateral)" : "Mimic motor (servo)"); - ImGui::Text( - "Reference target: %6.2f deg", status.targetAngle * radToDeg); + ImGui::Text("Reference target: %6.2f deg", status.targetAngle * radToDeg); ImGui::Text( "Reference: %6.2f deg", status.referencePosition * radToDeg); ImGui::Text( diff --git a/pixi.toml b/pixi.toml index c6f49e656caf3..1283994bbe849 100644 --- a/pixi.toml +++ b/pixi.toml @@ -704,23 +704,35 @@ bm = { depends-on = [ { arg = "build_type", default = "Release" }, ] } -test-joints = { cmd = ["bash", "-lc", """ +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"] } +""", +], depends-on = [ + "config", +] } -test-joints-coupler = { cmd = ["bash", "-lc", """ +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"] } +""", +], depends-on = [ + "config", +] } bm-boxes = { depends-on = [ { task = "run-cpp-target", args = [ From de51d72a17e6cacc2f0be3904af0f6c43fe44d24 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 18 Nov 2025 05:56:18 -0800 Subject: [PATCH 10/20] Fix gui target name --- examples/coupler_constraint/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/examples/coupler_constraint/CMakeLists.txt b/examples/coupler_constraint/CMakeLists.txt index 08a9d73ee7e74..d043fe1dd3438 100644 --- a/examples/coupler_constraint/CMakeLists.txt +++ b/examples/coupler_constraint/CMakeLists.txt @@ -5,8 +5,8 @@ get_filename_component(example_name ${CMAKE_CURRENT_LIST_DIR} NAME) project(${example_name}) # Aggregate dependencies -set(required_components gui-osg) -set(required_libraries dart dart-gui-osg) +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}) From 3768ece954ba569df67abeea34cb55c919cbf88d Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 18 Nov 2025 15:20:30 -0800 Subject: [PATCH 11/20] Make mimic constraint type configurable --- dart/dynamics/Joint.cpp | 29 +++++++-- dart/dynamics/Joint.hpp | 6 ++ dart/dynamics/MimicDofProperties.hpp | 10 +++ dart/dynamics/detail/JointAspect.hpp | 3 - python/dartpy/dynamics/Joint.cpp | 13 ++-- python/dartpy_docs/dynamics.py | 4 +- python/stubs/dartpy/dynamics.pyi | 97 +--------------------------- 7 files changed, 52 insertions(+), 110 deletions(-) diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index aa99279bdcb60..9f30a924edab8 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -72,8 +72,7 @@ JointProperties::JointProperties( mT_ParentBodyToJoint(_T_ParentBodyToJoint), mT_ChildBodyToJoint(_T_ChildBodyToJoint), mIsPositionLimitEnforced(_isPositionLimitEnforced), - mActuatorType(_actuatorType), - mUseCouplerConstraint(false) + mActuatorType(_actuatorType) { mMimicDofProps.resize(6); // TODO: Dof 6, which is the max value at the moment, is used because @@ -85,6 +84,7 @@ JointProperties::JointProperties( prop.mReferenceDofIndex = i; prop.mMultiplier = _mimicMultiplier; prop.mOffset = _mimicOffset; + prop.mConstraintType = MimicConstraintType::Motor; } } @@ -127,9 +127,8 @@ void Joint::setAspectProperties(const AspectProperties& properties) setTransformFromParentBodyNode(properties.mT_ParentBodyToJoint); setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); setLimitEnforcement(properties.mIsPositionLimitEnforced); - setActuatorType(properties.mActuatorType); - setUseCouplerConstraint(properties.mUseCouplerConstraint); setMimicJointDofs(properties.mMimicDofProps); + setActuatorType(properties.mActuatorType); } //============================================================================== @@ -215,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); @@ -224,6 +224,7 @@ void Joint::setMimicJoint( prop.mReferenceDofIndex = i; prop.mMultiplier = mimicMultiplier; prop.mOffset = mimicOffset; + prop.mConstraintType = constraintType; setMimicJointDof(i, prop); } } @@ -276,16 +277,32 @@ 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) { - mAspectProperties.mUseCouplerConstraint = enable; + setMimicConstraintType( + enable ? MimicConstraintType::Coupler : MimicConstraintType::Motor); } //============================================================================== bool Joint::isUsingCouplerConstraint() const { - return mAspectProperties.mUseCouplerConstraint; + return getMimicConstraintType() == MimicConstraintType::Coupler; } //============================================================================== diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index e371014af2384..6b6d45bf5b53d 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -178,6 +178,12 @@ 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); 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/dart/dynamics/detail/JointAspect.hpp b/dart/dynamics/detail/JointAspect.hpp index 5147911d89e84..530e872159a7b 100644 --- a/dart/dynamics/detail/JointAspect.hpp +++ b/dart/dynamics/detail/JointAspect.hpp @@ -133,9 +133,6 @@ struct DART_API JointProperties /// Actuator type ActuatorType mActuatorType; - /// True if the joint should enforce mimic coupling via constraint. - bool mUseCouplerConstraint; - /// Vector of MimicDofProperties for each dependent DoF in the joint. std::vector mMimicDofProps; diff --git a/python/dartpy/dynamics/Joint.cpp b/python/dartpy/dynamics/Joint.cpp index a6e28857f87e3..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( @@ -100,9 +108,6 @@ void Joint(py::module& m) .def_readwrite( "mActuatorType", &dart::dynamics::detail::JointProperties::mActuatorType) - .def_readwrite( - "mUseCouplerConstraint", - &dart::dynamics::detail::JointProperties::mUseCouplerConstraint) .def_readwrite( "mMimicDofProps", &dart::dynamics::detail::JointProperties::mMimicDofProps); diff --git a/python/dartpy_docs/dynamics.py b/python/dartpy_docs/dynamics.py index d1eaa632079fb..df7db42cbfb5c 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', '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', '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: @@ -3557,7 +3557,6 @@ def setVelocityUpperLimits(self, upperLimits: numpy.ndarray[tuple[M, typing.Lite ... class JointProperties: mActuatorType: ActuatorType - mUseCouplerConstraint: bool mIsPositionLimitEnforced: bool mName: str mT_ChildBodyToJoint: dartpy.math.Isometry3 @@ -4337,6 +4336,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 01118ca261f45..df7db42cbfb5c 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', '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', '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,8 +424,6 @@ class BodyNode(TemplatedJacobianBodyNode, Frame): ... def getNumEndEffectors(self) -> int: ... - def getEndEffector(self, index: int) -> EndEffector: - ... def getNumMarkers(self) -> int: ... def getNumShapeNodes(self) -> int: @@ -536,53 +532,6 @@ class BodyNode(TemplatedJacobianBodyNode, Frame): ... 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: ... @@ -3030,8 +2979,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 +3052,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 +3170,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: ... @@ -3638,7 +3557,6 @@ class Joint(dartpy.common.Subject, EmbedProperties_Joint_JointProperties): ... class JointProperties: mActuatorType: ActuatorType - mUseCouplerConstraint: bool mIsPositionLimitEnforced: bool mName: str mT_ChildBodyToJoint: dartpy.math.Isometry3 @@ -4418,6 +4336,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 @@ -5845,9 +5764,6 @@ class Skeleton(MetaSkeleton): def getIK(self) -> ...: ... @typing.overload - def getIK(self, createIfNull: bool) -> ...: - ... - @typing.overload def getIndexOf(self, bn: BodyNode) -> int: ... @typing.overload @@ -5965,15 +5881,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 From d85053747bc2520478676177cd15e70b51749a4b Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Tue, 18 Nov 2025 15:51:11 -0800 Subject: [PATCH 12/20] Fix coupler test helper access --- tests/integration/dynamics/test_Joints.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index eb463a616c382..63f7801085417 100644 --- a/tests/integration/dynamics/test_Joints.cpp +++ b/tests/integration/dynamics/test_Joints.cpp @@ -66,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 { @@ -1302,7 +1310,7 @@ TEST_F(JOINTS, COUPLER_CONSTRAINT_APPLY_IMPULSE) referenceJoint->setPosition(0, 0.1); followerJoint->setPosition(0, 0.0); - dart::constraint::CouplerConstraint constraint( + CouplerConstraintTestHelper constraint( followerJoint, followerJoint->getMimicDofProperties()); constraint.update(); ASSERT_GT(constraint.getDimension(), 0u); From 86ada16af9cfa6cd8816a005d5e43adf280e7332 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 02:25:10 -0800 Subject: [PATCH 13/20] Fix test fixture name case --- tests/integration/dynamics/test_Joints.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/tests/integration/dynamics/test_Joints.cpp b/tests/integration/dynamics/test_Joints.cpp index 63f7801085417..dd4ffc792cef5 100644 --- a/tests/integration/dynamics/test_Joints.cpp +++ b/tests/integration/dynamics/test_Joints.cpp @@ -1273,19 +1273,19 @@ void testMimicCouplerJoint() } //============================================================================== -TEST_F(JOINTS, MIMIC_JOINT) +TEST_F(Joints, MIMIC_JOINT) { testMimicJoint(); } //============================================================================== -TEST_F(JOINTS, MIMIC_JOINT_COUPLER) +TEST_F(Joints, MIMIC_JOINT_COUPLER) { testMimicCouplerJoint(); } //============================================================================== -TEST_F(JOINTS, COUPLER_CONSTRAINT_APPLY_IMPULSE) +TEST_F(Joints, COUPLER_CONSTRAINT_APPLY_IMPULSE) { Vector3d dim(1, 1, 1); Vector3d offset(0, 0, 0); @@ -1326,7 +1326,7 @@ TEST_F(JOINTS, COUPLER_CONSTRAINT_APPLY_IMPULSE) } //============================================================================== -TEST_F(JOINTS, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) +TEST_F(Joints, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) { const double timeStep = 1e-3; const double tol = 1e-2; From 29cd6b558a180a4604af8613214e0002fbb9141f Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 07:47:10 -0800 Subject: [PATCH 14/20] Restore end-effector stubs and clamp coupler CFM --- dart/constraint/CouplerConstraint.cpp | 8 ++-- dart/constraint/MimicMotorConstraint.cpp | 8 ++-- python/dartpy_docs/dynamics.py | 52 +++++++++++++++++++++++- python/stubs/dartpy/dynamics.pyi | 52 +++++++++++++++++++++++- 4 files changed, 110 insertions(+), 10 deletions(-) diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp index fbb02a13ce8f8..1b87cfc670f90 100644 --- a/dart/constraint/CouplerConstraint.cpp +++ b/dart/constraint/CouplerConstraint.cpp @@ -85,16 +85,16 @@ const std::string& CouplerConstraint::getStaticType() //============================================================================== void CouplerConstraint::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( "[CouplerConstraint::setConstraintForceMixing] 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.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/python/dartpy_docs/dynamics.py b/python/dartpy_docs/dynamics.py index df7db42cbfb5c..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', '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', '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: ... diff --git a/python/stubs/dartpy/dynamics.pyi b/python/stubs/dartpy/dynamics.pyi index df7db42cbfb5c..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', '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', '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 @@ 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: ... def getNumShapeNodes(self) -> int: @@ -532,6 +536,52 @@ class BodyNode(TemplatedJacobianBodyNode, Frame): ... 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: ... From 3ae932a5a479d78aa749b4265dc1519352856082 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 10:02:09 -0800 Subject: [PATCH 15/20] visualize follower limits in coupler example --- examples/coupler_constraint/main.cpp | 69 ++++++++++++++++++++++++++-- 1 file changed, 65 insertions(+), 4 deletions(-) diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index f41f60f3eb5c6..167e5a0ebfb33 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -84,14 +84,46 @@ struct MimicAssembly 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, + const Eigen::Vector3d& baseOffset, + double angle, + const Eigen::Vector3d& color, + const dart::simulation::WorldPtr& world) +{ + using namespace dart::dynamics; + if (!world) + 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(); + tf.translate(baseOffset + Eigen::Vector3d(0.0, 0.0, 0.02)); + tf.rotate(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ())); + 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 Eigen::Vector3d& followerColor, + const dart::simulation::WorldPtr& world) { SkeletonPtr skeleton = Skeleton::create(label + "_rig"); @@ -194,7 +226,34 @@ MimicAssembly createMimicAssembly( referenceJoint->setPosition(0, 0.0); followerJoint->setPosition(0, 0.0); - return {skeleton, referenceJoint, followerJoint, referenceBody, followerBody}; + 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", + baseOffset, + lower, + Eigen::Vector3d(0.92, 0.35, 0.35), + world); + assembly.upperLimitGuide = createLimitGuide( + label + "_upper_limit", + baseOffset, + upper, + Eigen::Vector3d(0.35, 0.92, 0.35), + world); + } + + return assembly; } class CouplerController @@ -544,13 +603,15 @@ int main(int /*argc*/, char* /*argv*/[]) 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)); + 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)); + Eigen::Vector3d(0.25, 0.75, 0.70), + world); world->addSkeleton(couplerAssembly.skeleton); world->addSkeleton(motorAssembly.skeleton); From e241fafec9e71cdf8128d92a90d800570624d912 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 14:28:51 -0800 Subject: [PATCH 16/20] export coupler constraint symbols --- dart/constraint/CouplerConstraint.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index dd243e3ec883a..a8f95145eafdc 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -35,6 +35,7 @@ #include +#include #include #include @@ -48,7 +49,7 @@ namespace constraint { /// 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 CouplerConstraint : public ConstraintBase +class DART_API CouplerConstraint : public ConstraintBase { public: /// Constructor that creates a CouplerConstraint using the given From b6e2e6a565f6bb7e51c385d16b5b33361f6b850e Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 14:54:20 -0800 Subject: [PATCH 17/20] anchor limit guides at follower joints --- examples/coupler_constraint/main.cpp | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 167e5a0ebfb33..a4e5b4f06b12b 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -90,13 +90,13 @@ struct MimicAssembly dart::dynamics::SimpleFramePtr createLimitGuide( const std::string& name, - const Eigen::Vector3d& baseOffset, + const dart::dynamics::Joint* followerJoint, double angle, const Eigen::Vector3d& color, const dart::simulation::WorldPtr& world) { using namespace dart::dynamics; - if (!world) + if (!world || !followerJoint) return nullptr; auto frame = SimpleFrame::createShared(Frame::World(), name); @@ -108,8 +108,13 @@ dart::dynamics::SimpleFramePtr createLimitGuide( auto visual = frame->createVisualAspect(); visual->setColor(color); - Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); - tf.translate(baseOffset + Eigen::Vector3d(0.0, 0.0, 0.02)); + const auto* parent = followerJoint->getParentBodyNode(); + if (parent == nullptr) + return nullptr; + + Eigen::Isometry3d tf + = parent->getWorldTransform() * followerJoint->getTransformFromParentBodyNode(); + tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); tf.rotate(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ())); frame->setRelativeTransform(tf); @@ -241,13 +246,13 @@ MimicAssembly createMimicAssembly( const double upper = followerJoint->getPositionUpperLimit(0); assembly.lowerLimitGuide = createLimitGuide( label + "_lower_limit", - baseOffset, + followerJoint, lower, Eigen::Vector3d(0.92, 0.35, 0.35), world); assembly.upperLimitGuide = createLimitGuide( label + "_upper_limit", - baseOffset, + followerJoint, upper, Eigen::Vector3d(0.35, 0.92, 0.35), world); From 6adb4a1b63137e2cfef590c5c6fb3ea0c6d0d734 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 15:08:01 -0800 Subject: [PATCH 18/20] attach limit guides to joint parents --- examples/coupler_constraint/main.cpp | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index a4e5b4f06b12b..b599a5ca4d05b 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -90,7 +90,7 @@ struct MimicAssembly dart::dynamics::SimpleFramePtr createLimitGuide( const std::string& name, - const dart::dynamics::Joint* followerJoint, + dart::dynamics::Joint* followerJoint, double angle, const Eigen::Vector3d& color, const dart::simulation::WorldPtr& world) @@ -99,7 +99,11 @@ dart::dynamics::SimpleFramePtr createLimitGuide( if (!world || !followerJoint) return nullptr; - auto frame = SimpleFrame::createShared(Frame::World(), name); + auto* parentBody = followerJoint->getParentBodyNode(); + if (parentBody == nullptr) + return nullptr; + + auto frame = SimpleFrame::createShared(parentBody, name); auto line = std::make_shared(0.05f); line->addVertex(Eigen::Vector3d::Zero()); line->addVertex(Eigen::Vector3d::UnitX() * 0.65); @@ -108,14 +112,9 @@ dart::dynamics::SimpleFramePtr createLimitGuide( auto visual = frame->createVisualAspect(); visual->setColor(color); - const auto* parent = followerJoint->getParentBodyNode(); - if (parent == nullptr) - return nullptr; - - Eigen::Isometry3d tf - = parent->getWorldTransform() * followerJoint->getTransformFromParentBodyNode(); - tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); + Eigen::Isometry3d tf = followerJoint->getTransformFromParentBodyNode(); tf.rotate(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ())); + tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); frame->setRelativeTransform(tf); world->addSimpleFrame(frame); From 502f8be261a0795ec3d3b73a918635d3a7fdf369 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 15:52:21 -0800 Subject: [PATCH 19/20] recompute limit guide world transforms --- examples/coupler_constraint/main.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index b599a5ca4d05b..897ae4e5e2ddd 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -99,11 +99,7 @@ dart::dynamics::SimpleFramePtr createLimitGuide( if (!world || !followerJoint) return nullptr; - auto* parentBody = followerJoint->getParentBodyNode(); - if (parentBody == nullptr) - return nullptr; - - auto frame = SimpleFrame::createShared(parentBody, name); + 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); @@ -112,8 +108,11 @@ dart::dynamics::SimpleFramePtr createLimitGuide( auto visual = frame->createVisualAspect(); visual->setColor(color); - Eigen::Isometry3d tf = followerJoint->getTransformFromParentBodyNode(); - tf.rotate(Eigen::AngleAxisd(angle, Eigen::Vector3d::UnitZ())); + Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); + if (const auto* parent = followerJoint->getParentBodyNode()) + tf = parent->getWorldTransform(); + tf *= followerJoint->getTransformFromParentBodyNode(); + tf.rotate(Eigen::AngleAxisd(angle, followerJoint->getAxis(0))); tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); frame->setRelativeTransform(tf); From ef990a8ef6b9a5c0189f30acb684f4b7d86e9d98 Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Wed, 19 Nov 2025 20:32:30 -0800 Subject: [PATCH 20/20] Fix coupler guide transform and formatting --- dart/constraint/CouplerConstraint.hpp | 3 ++- examples/coupler_constraint/main.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp index a8f95145eafdc..be1d26003be59 100644 --- a/dart/constraint/CouplerConstraint.hpp +++ b/dart/constraint/CouplerConstraint.hpp @@ -35,9 +35,10 @@ #include -#include #include +#include + #include namespace dart { diff --git a/examples/coupler_constraint/main.cpp b/examples/coupler_constraint/main.cpp index 897ae4e5e2ddd..7abaa86f8a99e 100644 --- a/examples/coupler_constraint/main.cpp +++ b/examples/coupler_constraint/main.cpp @@ -99,6 +99,11 @@ dart::dynamics::SimpleFramePtr createLimitGuide( 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()); @@ -111,8 +116,8 @@ dart::dynamics::SimpleFramePtr createLimitGuide( Eigen::Isometry3d tf = Eigen::Isometry3d::Identity(); if (const auto* parent = followerJoint->getParentBodyNode()) tf = parent->getWorldTransform(); - tf *= followerJoint->getTransformFromParentBodyNode(); - tf.rotate(Eigen::AngleAxisd(angle, followerJoint->getAxis(0))); + tf = tf * followerJoint->getTransformFromParentBodyNode(); + tf.rotate(Eigen::AngleAxisd(angle, revolute->getAxis())); tf.translate(Eigen::Vector3d(0.0, 0.0, 0.02)); frame->setRelativeTransform(tf);