Add coupler constraint support and example

dart/constraint/ConstraintSolver.cpp

@@ -44,6 +44,7 @@
 #include "dart/constraint/ConstrainedGroup.hpp"
 #include "dart/constraint/ContactConstraint.hpp"
 #include "dart/constraint/ContactSurface.hpp"
+#include "dart/constraint/CouplerConstraint.hpp"
 #include "dart/constraint/JointConstraint.hpp"
 #include "dart/constraint/JointCoulombFrictionConstraint.hpp"
 #include "dart/constraint/LCPSolver.hpp"
@@ -493,6 +494,7 @@ void ConstraintSolver::updateConstraints()
   // Destroy previous joint constraints
   mJointConstraints.clear();
   mMimicMotorConstraints.clear();
+  mCouplerConstraints.clear();
   mJointCoulombFrictionConstraints.clear();
 
   // Create new joint constraints
@@ -520,8 +522,14 @@ void ConstraintSolver::updateConstraints()
 
       if (joint->getActuatorType() == dynamics::Joint::MIMIC
           && joint->getMimicJoint()) {
-        mMimicMotorConstraints.push_back(std::make_shared<MimicMotorConstraint>(
-            joint, joint->getMimicDofProperties()));
+        if (joint->isUsingCouplerConstraint()) {
+          mCouplerConstraints.push_back(std::make_shared<CouplerConstraint>(
+              joint, joint->getMimicDofProperties()));
+        } else {
+          mMimicMotorConstraints.push_back(
+              std::make_shared<MimicMotorConstraint>(
+                  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();
 

dart/constraint/ConstraintSolver.hpp

@@ -263,6 +263,9 @@ class DART_API ConstraintSolver
   /// Mimic motor constraints those are automatically created
   std::vector<MimicMotorConstraintPtr> mMimicMotorConstraints;
 
+  /// Coupler constraints that are automatically created
+  std::vector<CouplerConstraintPtr> mCouplerConstraints;
+
   /// Joint Coulomb friction constraints those are automatically created
   std::vector<JointCoulombFrictionConstraintPtr>
       mJointCoulombFrictionConstraints;