Allow specifying mimic joint properties per DoF (dartsim#1684) (darts…

…im#1752)

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

dart/constraint/ConstraintSolver.cpp

@@ -631,10 +631,7 @@ void ConstraintSolver::updateConstraints()
           && joint->getMimicJoint())
       {
         mMimicMotorConstraints.push_back(std::make_shared<MimicMotorConstraint>(
-            joint,
-            joint->getMimicJoint(),
-            joint->getMimicMultiplier(),
-            joint->getMimicOffset()));
+            joint, joint->getMimicDofProperties()));
       }
     }
   }

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();

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<int>(mJoint->getNumDofs());

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<dynamics::MimicDofProperties>& 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),

dart/constraint/MimicMotorConstraint.hpp

@@ -33,7 +33,10 @@
 #ifndef DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_
 #define DART_CONSTRAINT_MIMICMOTORCONSTRAINT_HPP_
 
+#include <vector>
+
 #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<dynamics::MimicDofProperties>& 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<dynamics::MimicDofProperties> 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

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();

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<MimicDofProperties>& mimicProps)
+{
+  mAspectProperties.mMimicDofProps = mimicProps;
+}
+
+//==============================================================================
+void Joint::setMimicJointDofs(
+    const std::map<std::size_t, MimicDofProperties>& 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<MimicDofProperties>& Joint::getMimicDofProperties() const
 {
-  return mAspectProperties.mMimicOffset;
+  return mAspectProperties.mMimicDofProps;
 }
 
 //==============================================================================

dart/dynamics/Joint.hpp

@@ -42,13 +42,15 @@
 #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"
 
 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<MimicDofProperties>& 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<std::size_t, MimicDofProperties>& 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<MimicDofProperties>& getMimicDofProperties() const;
 
   /// Return true if this joint is kinematic joint.
   ///