From 206a838fa8e6f154d52858dade0d050f98948b7a Mon Sep 17 00:00:00 2001 From: Jeongseok Lee Date: Sat, 13 May 2023 08:37:01 -0700 Subject: [PATCH] Set mimic joint properties per dof --- dart/common/Memory.hpp | 10 +-- dart/constraint/ConstraintSolver.cpp | 5 +- .../JointCoulombFrictionConstraint.cpp | 2 +- dart/constraint/JointLimitConstraint.cpp | 2 +- dart/constraint/MimicMotorConstraint.cpp | 21 +++-- dart/constraint/MimicMotorConstraint.hpp | 38 +++++---- dart/constraint/ServoMotorConstraint.cpp | 2 +- dart/dynamics/Joint.cpp | 83 ++++++++++++++----- dart/dynamics/Joint.hpp | 41 ++++++--- dart/dynamics/MimicDofProperties.hpp | 62 ++++++++++++++ dart/dynamics/detail/JointAspect.hpp | 11 +-- dart/utils/urdf/DartLoader.cpp | 11 +-- python/dartpy/dynamics/Joint.cpp | 52 ++++++++---- unittests/integration/test_Joints.cpp | 4 +- 14 files changed, 245 insertions(+), 99 deletions(-) create mode 100644 dart/dynamics/MimicDofProperties.hpp diff --git a/dart/common/Memory.hpp b/dart/common/Memory.hpp index 458e26f46e135..2c61d3701f562 100644 --- a/dart/common/Memory.hpp +++ b/dart/common/Memory.hpp @@ -173,11 +173,11 @@ public: \ // Define static creator function that returns std::unique_ptr to the object #define DART_DEFINE_UNIQUE_OBJECT_CREATOR(class_name) \ duke /*! Create unique instance of this class */ \ - _DART_DEFINE_OBJECT_CREATOR( \ - class_name, \ - DART_UNIQUE_PTR_CREATOR_NAME, \ - std::unique_ptr, \ - ::std::make_unique) + _DART_DEFINE_OBJECT_CREATOR( \ + class_name, \ + DART_UNIQUE_PTR_CREATOR_NAME, \ + std::unique_ptr, \ + ::std::make_unique) // Define static creator function that returns std::unique_ptr to the object // where the constructor is protected diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index c30b25b2e788f..0a4c4b45bf652 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -631,10 +631,7 @@ void ConstraintSolver::updateConstraints() && joint->getMimicJoint()) { mMimicMotorConstraints.push_back(std::make_shared( - joint, - joint->getMimicJoint(), - joint->getMimicMultiplier(), - joint->getMimicOffset())); + joint, joint->getMimicDofProperties())); } } } diff --git a/dart/constraint/JointCoulombFrictionConstraint.cpp b/dart/constraint/JointCoulombFrictionConstraint.cpp index 1c5c4b7f0fd9f..b6f0a271d6010 100644 --- a/dart/constraint/JointCoulombFrictionConstraint.cpp +++ b/dart/constraint/JointCoulombFrictionConstraint.cpp @@ -112,7 +112,7 @@ double JointCoulombFrictionConstraint::getConstraintForceMixing() //============================================================================== void JointCoulombFrictionConstraint::update() { - // Reset dimention + // Reset dimension mDim = 0; std::size_t dof = mJoint->getNumDofs(); diff --git a/dart/constraint/JointLimitConstraint.cpp b/dart/constraint/JointLimitConstraint.cpp index 1a1d33a294fff..24fd90ffb6219 100644 --- a/dart/constraint/JointLimitConstraint.cpp +++ b/dart/constraint/JointLimitConstraint.cpp @@ -174,7 +174,7 @@ double JointLimitConstraint::getConstraintForceMixing() //============================================================================== void JointLimitConstraint::update() { - // Reset dimention + // Reset dimension mDim = 0; const int dof = static_cast(mJoint->getNumDofs()); diff --git a/dart/constraint/MimicMotorConstraint.cpp b/dart/constraint/MimicMotorConstraint.cpp index 2c439f96dd394..393c6e590b555 100644 --- a/dart/constraint/MimicMotorConstraint.cpp +++ b/dart/constraint/MimicMotorConstraint.cpp @@ -50,21 +50,16 @@ double MimicMotorConstraint::mConstraintForceMixing = DART_CFM; //============================================================================== MimicMotorConstraint::MimicMotorConstraint( dynamics::Joint* joint, - const dynamics::Joint* mimicJoint, - double multiplier, - double offset) + const std::vector& mimicDofProperties) : ConstraintBase(), mJoint(joint), - mMimicJoint(mimicJoint), - mMultiplier(multiplier), - mOffset(offset), + mMimicProps(mimicDofProperties), mBodyNode(joint->getChildBodyNode()), mAppliedImpulseIndex(0) { assert(joint); - assert(mimicJoint); + assert(joint->getNumDofs() <= mMimicProps.size()); assert(mBodyNode); - assert(joint->getNumDofs() == mimicJoint->getNumDofs()); mLifeTime[0] = 0; mLifeTime[1] = 0; @@ -125,15 +120,19 @@ double MimicMotorConstraint::getConstraintForceMixing() //============================================================================== void MimicMotorConstraint::update() { - // Reset dimention + // 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 = mMimicJoint->getPosition(i) * mMultiplier + mOffset - - mJoint->getPosition(i); + double qError + = mimicProp.mReferenceJoint->getPosition(mimicProp.mReferenceDofIndex) + * mimicProp.mMultiplier + + mimicProp.mOffset - mJoint->getPosition(i); double desiredVelocity = math::clip( qError / timeStep, mJoint->getVelocityLowerLimit(i), diff --git a/dart/constraint/MimicMotorConstraint.hpp b/dart/constraint/MimicMotorConstraint.hpp index 6aaf58e48e5d8..d5ec17e6b7a73 100644 --- a/dart/constraint/MimicMotorConstraint.hpp +++ b/dart/constraint/MimicMotorConstraint.hpp @@ -33,7 +33,10 @@ #ifndef DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_ #define DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_ +#include + #include "dart/constraint/ConstraintBase.hpp" +#include "dart/dynamics/MimicDofProperties.hpp" namespace dart { @@ -48,12 +51,14 @@ namespace constraint { class MimicMotorConstraint : public ConstraintBase { public: - /// Constructor + /// Constructor that creates a MimicMotorConstraint 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 MimicMotorConstraint( dynamics::Joint* joint, - const dynamics::Joint* mimicJoint, - double multiplier = 1.0, - double offset = 0.0); + const std::vector& mimicDofProperties); /// Destructor ~MimicMotorConstraint() override; @@ -114,39 +119,36 @@ class MimicMotorConstraint : public ConstraintBase bool isActive() const override; private: - /// + /// Dependent joint whose motion is influenced by the reference joint. dynamics::Joint* mJoint; - /// - const dynamics::Joint* mMimicJoint; - - /// - double mMultiplier, mOffset; + /// Vector of MimicDofProperties for the dependent joint. + std::vector mMimicProps; - /// + /// BodyNode associated with the dependent joint. dynamics::BodyNode* mBodyNode; - /// Index of applied impulse + /// 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]; // TODO(JS): Lifetime should be considered only when we use iterative lcp // solver - /// + /// 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 diff --git a/dart/constraint/ServoMotorConstraint.cpp b/dart/constraint/ServoMotorConstraint.cpp index e188358a99140..c9483b0684d84 100644 --- a/dart/constraint/ServoMotorConstraint.cpp +++ b/dart/constraint/ServoMotorConstraint.cpp @@ -113,7 +113,7 @@ double ServoMotorConstraint::getConstraintForceMixing() //============================================================================== void ServoMotorConstraint::update() { - // Reset dimention + // Reset dimension mDim = 0; std::size_t dof = mJoint->getNumDofs(); diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 93815224251cd..635909a771be8 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -71,12 +71,20 @@ JointProperties::JointProperties( mT_ParentBodyToJoint(_T_ParentBodyToJoint), mT_ChildBodyToJoint(_T_ChildBodyToJoint), mIsPositionLimitEnforced(_isPositionLimitEnforced), - mActuatorType(_actuatorType), - mMimicJoint(_mimicJoint), - mMimicMultiplier(_mimicMultiplier), - mMimicOffset(_mimicOffset) + mActuatorType(_actuatorType) { - // Do nothing + mMimicDofProps.resize(6); + // TODO: Dof 6, which is the max value at the moment, is used because + // JointProperties doesn't have the Dof information + + for (auto i = 0u; i < mMimicDofProps.size(); ++i) + { + auto& prop = mMimicDofProps[i]; + prop.mReferenceJoint = _mimicJoint; + prop.mReferenceDofIndex = i; + prop.mMultiplier = _mimicMultiplier; + prop.mOffset = _mimicOffset; + } } } // namespace detail @@ -119,10 +127,7 @@ void Joint::setAspectProperties(const AspectProperties& properties) setTransformFromChildBodyNode(properties.mT_ChildBodyToJoint); setLimitEnforcement(properties.mIsPositionLimitEnforced); setActuatorType(properties.mActuatorType); - setMimicJoint( - properties.mMimicJoint, - properties.mMimicMultiplier, - properties.mMimicOffset); + setMimicJointDofs(properties.mMimicDofProps); } //============================================================================== @@ -206,29 +211,69 @@ Joint::ActuatorType Joint::getActuatorType() const //============================================================================== void Joint::setMimicJoint( - const Joint* _mimicJoint, double _mimicMultiplier, double _mimicOffset) + const Joint* referenceJoint, double mimicMultiplier, double mimicOffset) +{ + std::size_t numDofs = getNumDofs(); + mAspectProperties.mMimicDofProps.resize(numDofs); + + for (std::size_t i = 0; i < numDofs; ++i) + { + MimicDofProperties prop; + prop.mReferenceJoint = referenceJoint; + prop.mReferenceDofIndex = i; + prop.mMultiplier = mimicMultiplier; + prop.mOffset = mimicOffset; + setMimicJointDof(i, prop); + } +} + +//============================================================================== +void Joint::setMimicJointDof( + std::size_t index, const MimicDofProperties& mimicProp) +{ + mAspectProperties.mMimicDofProps[index] = mimicProp; +} + +//============================================================================== +void Joint::setMimicJointDofs(const std::vector& mimicProps) +{ + mAspectProperties.mMimicDofProps = mimicProps; +} + +//============================================================================== +void Joint::setMimicJointDofs( + const std::map& mimicPropMap) +{ + for (const auto& pair : mimicPropMap) + { + const auto& index = pair.first; + const auto& prop = pair.second; + setMimicJointDof(index, prop); + } +} + +//============================================================================== +const Joint* Joint::getMimicJoint(std::size_t index) const { - mAspectProperties.mMimicJoint = _mimicJoint; - mAspectProperties.mMimicMultiplier = _mimicMultiplier; - mAspectProperties.mMimicOffset = _mimicOffset; + return mAspectProperties.mMimicDofProps[index].mReferenceJoint; } //============================================================================== -const Joint* Joint::getMimicJoint() const +double Joint::getMimicMultiplier(std::size_t index) const { - return mAspectProperties.mMimicJoint; + return mAspectProperties.mMimicDofProps[index].mMultiplier; } //============================================================================== -double Joint::getMimicMultiplier() const +double Joint::getMimicOffset(std::size_t index) const { - return mAspectProperties.mMimicMultiplier; + return mAspectProperties.mMimicDofProps[index].mOffset; } //============================================================================== -double Joint::getMimicOffset() const +const std::vector& Joint::getMimicDofProperties() const { - return mAspectProperties.mMimicOffset; + return mAspectProperties.mMimicDofProps; } //============================================================================== diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index 438d1ccb74979..37ddd84f7ce1e 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -42,6 +42,7 @@ #include "dart/common/Subject.hpp" #include "dart/common/VersionCounter.hpp" #include "dart/dynamics/Frame.hpp" +#include "dart/dynamics/MimicDofProperties.hpp" #include "dart/dynamics/SmartPointer.hpp" #include "dart/dynamics/detail/JointAspect.hpp" #include "dart/math/MathTypes.hpp" @@ -49,6 +50,7 @@ namespace dart { namespace dynamics { +class Joint; class BodyNode; class Skeleton; class DegreeOfFreedom; @@ -137,20 +139,39 @@ class Joint : public virtual common::Subject, /// Get actuator type ActuatorType getActuatorType() const; - /// Set mimic joint + /// Set the mimic joint with a single reference joint and the same multiplier + /// and offset for all dependent joint's DoFs. void setMimicJoint( - const Joint* _mimicJoint, - double _mimicMultiplier = 1.0, - double _mimicOffset = 0.0); + const Joint* referenceJoint, + double mimicMultiplier = 1.0, + double mimicOffset = 0.0); - /// Get mimic joint - const Joint* getMimicJoint() const; + /// Sets the mimic joint properties for a specific DoF of the dependent joint. + void setMimicJointDof(std::size_t index, const MimicDofProperties& mimicProp); - /// Get mimic joint multiplier - double getMimicMultiplier() const; + /// Sets the mimic joint properties for all DoFs of the dependent joint using + /// a vector of MimicDofProperties. + void setMimicJointDofs(const std::vector& mimicProps); - /// Get mimic joint offset - double getMimicOffset() const; + /// Sets the mimic joint properties for specific DoFs of the dependent joint + /// using a map of DoF index and MimicDofProperties. + void setMimicJointDofs( + const std::map& mimicPropMap); + + /// Returns the reference joint for the specified DoF of the dependent joint. + const Joint* getMimicJoint(std::size_t index = 0) const; + + /// Returns the mimic joint multiplier for the specified DoF of the dependent + /// joint. + double getMimicMultiplier(std::size_t index = 0) const; + + /// Returns the mimic joint offset for the specified DoF of the dependent + /// joint. + double getMimicOffset(std::size_t index = 0) const; + + /// Returns the vector of MimicDofProperties for all DoFs of the dependent + /// joint. + const std::vector& getMimicDofProperties() const; /// Return true if this joint is kinematic joint. /// diff --git a/dart/dynamics/MimicDofProperties.hpp b/dart/dynamics/MimicDofProperties.hpp new file mode 100644 index 0000000000000..4287ba04806d5 --- /dev/null +++ b/dart/dynamics/MimicDofProperties.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (c) 2011-2022, The DART development contributors + * All rights reserved. + * + * The list of contributors can be found at: + * https://github.com/dartsim/dart/blob/master/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_DYNAMICS_MIMICDOFPROPERTIES_HPP_ +#define DART_DYNAMICS_MIMICDOFPROPERTIES_HPP_ + +#include + +namespace dart { +namespace dynamics { + +class Joint; + +/// Properties of a mimicked Degree of Freedom (DoF) in a joint. +struct MimicDofProperties +{ + /// Pointer to the reference joint used for mimicking behavior. + const Joint* mReferenceJoint; + + /// Index of the reference DoF in the reference joint. + std::size_t mReferenceDofIndex = 0; + + /// Multiplier applied to the reference joint's DoF value. + double mMultiplier = 1.0; + + /// Offset added to the reference joint's DoF value. + double mOffset = 0.0; +}; + +} // namespace dynamics +} // namespace dart + +#endif // DART_DYNAMICS_MIMICDOFPROPERTIES_HPP_ diff --git a/dart/dynamics/detail/JointAspect.hpp b/dart/dynamics/detail/JointAspect.hpp index aa5a4adb4f21a..2403cdcffd850 100644 --- a/dart/dynamics/detail/JointAspect.hpp +++ b/dart/dynamics/detail/JointAspect.hpp @@ -33,9 +33,13 @@ #ifndef DART_DYNAMICS_DETAIL_JOINTASPECT_HPP_ #define DART_DYNAMICS_DETAIL_JOINTASPECT_HPP_ +#include + #include #include +#include "dart/dynamics/MimicDofProperties.hpp" + namespace dart { namespace dynamics { namespace detail { @@ -127,11 +131,8 @@ struct JointProperties /// Actuator type ActuatorType mActuatorType; - /// Mimic joint - const Joint* mMimicJoint; - - /// Mimic joint properties - double mMimicMultiplier, mMimicOffset; + /// Vector of MimicDofProperties for each dependent DoF in the joint. + std::vector mMimicDofProps; /// Constructor JointProperties( diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp index 2a171e18db871..fc50255f6d63d 100644 --- a/dart/utils/urdf/DartLoader.cpp +++ b/dart/utils/urdf/DartLoader.cpp @@ -428,8 +428,6 @@ bool DartLoader::addMimicJointsRecursive( const std::string& mimicJointName = jt->mimic->joint_name; dynamics::Joint* joint = _skel->getJoint(jointName); - const dynamics::Joint* mimicJoint = _skel->getJoint(mimicJointName); - if (!joint) { dterr << "Failed to configure a mimic joint [" << jointName @@ -438,6 +436,7 @@ bool DartLoader::addMimicJointsRecursive( return false; } + const dynamics::Joint* mimicJoint = _skel->getJoint(mimicJointName); if (!mimicJoint) { dterr << "Failed to configure a mimic joint [" << jointName @@ -446,12 +445,8 @@ bool DartLoader::addMimicJointsRecursive( return false; } - dynamics::Joint::Properties properties = joint->getJointProperties(); - properties.mActuatorType = dynamics::Joint::MIMIC; - properties.mMimicJoint = mimicJoint; - properties.mMimicMultiplier = multiplier; - properties.mMimicOffset = offset; - joint->setProperties(properties); + joint->setActuatorType(dynamics::Joint::MIMIC); + joint->setMimicJoint(mimicJoint, multiplier, offset); } for (std::size_t i = 0; i < _lk->child_links.size(); ++i) diff --git a/python/dartpy/dynamics/Joint.cpp b/python/dartpy/dynamics/Joint.cpp index a66cfd3ab64b5..0f6b13789a59c 100644 --- a/python/dartpy/dynamics/Joint.cpp +++ b/python/dartpy/dynamics/Joint.cpp @@ -42,30 +42,54 @@ namespace python { void Joint(py::module& m) { - ::py::class_(m, "JointProperties") + ::py::class_(m, "MimicDofProperties") .def(::py::init<>()) - .def(::py::init(), ::py::arg("name")) - .def_readwrite("mName", &dart::dynamics::detail::JointProperties::mName) .def_readwrite( - "mT_ParentBodyToJoint", + "mReferenceJoint", + &dart::dynamics::MimicDofProperties::mReferenceJoint) + .def_readwrite( + "mReferenceDofIndex", + &dart::dynamics::MimicDofProperties::mReferenceDofIndex) + .def_readwrite( + "multiplier", &dart::dynamics::MimicDofProperties::mMultiplier) + .def_readwrite("offset", &dart::dynamics::MimicDofProperties::mOffset); + + ::py::class_(m, "JointProperties") + .def( + ::py::init< + const std::string&, + const Eigen::Isometry3d&, + const Eigen::Isometry3d&, + bool, + dart::dynamics::detail::ActuatorType, + const dart::dynamics::Joint*, + double, + double>(), + ::py::arg("name") = "Joint", + ::py::arg("T_ParentBodyToJoint") = Eigen::Isometry3d::Identity(), + ::py::arg("T_ChildBodyToJoint") = Eigen::Isometry3d::Identity(), + ::py::arg("isPositionLimitEnforced") = false, + ::py::arg("actuatorType") + = dart::dynamics::detail::DefaultActuatorType, + ::py::arg("mimicJoint") = nullptr, + ::py::arg("mimicMultiplier") = 1.0, + ::py::arg("mimicOffset") = 0.0) + .def_readwrite("name", &dart::dynamics::detail::JointProperties::mName) + .def_readwrite( + "T_ParentBodyToJoint", &dart::dynamics::detail::JointProperties::mT_ParentBodyToJoint) .def_readwrite( - "mT_ChildBodyToJoint", + "T_ChildBodyToJoint", &dart::dynamics::detail::JointProperties::mT_ChildBodyToJoint) .def_readwrite( - "mIsPositionLimitEnforced", + "isPositionLimitEnforced", &dart::dynamics::detail::JointProperties::mIsPositionLimitEnforced) .def_readwrite( - "mActuatorType", + "actuatorType", &dart::dynamics::detail::JointProperties::mActuatorType) .def_readwrite( - "mMimicJoint", &dart::dynamics::detail::JointProperties::mMimicJoint) - .def_readwrite( - "mMimicMultiplier", - &dart::dynamics::detail::JointProperties::mMimicMultiplier) - .def_readwrite( - "mMimicOffset", - &dart::dynamics::detail::JointProperties::mMimicOffset); + "mimicDofProps", + &dart::dynamics::detail::JointProperties::mMimicDofProps); ::py::class_< dart::common::SpecializedForAspectgetVelocity(0), expected_vel, tol); - // Check if the mimic joint follows the "master" joint + // Check if the mimic joint follows the reference joint EXPECT_NEAR(joints[0]->getPosition(0), joints[1]->getPosition(0), tolPos); } - // In the end, check once more if the mimic joint followed the "master" joint + // In the end, check once more if the mimic joint followed the reference joint EXPECT_NEAR(joints[0]->getPosition(0), joints[1]->getPosition(0), tolPos); }