Added coupler constraint

dart/constraint/CouplerConstraint.cpp

@@ -0,0 +1,273 @@
+#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 {
+
+double CouplerConstraint::mConstraintForceMixing = DART_CFM;
+
+//==============================================================================
+CouplerConstraint::CouplerConstraint(
+    dynamics::Joint* joint,
+    const std::vector<dynamics::MimicDofProperties>& 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);
+}
+
+//==============================================================================
+CouplerConstraint::~CouplerConstraint()
+{
+  // Do nothing
+}
+
+//==============================================================================
+const std::string& CouplerConstraint::getType() const
+{
+  return getStaticType();
+}
+
+//==============================================================================
+const std::string& CouplerConstraint::getStaticType()
+{
+  static const std::string name = "CouplerConstraint";
+  return name;
+}
+
+//==============================================================================
+void CouplerConstraint::setConstraintForceMixing(double cfm)
+{
+  // 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()
+{
+  // 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<dynamics::Joint*>(mimicProp.mReferenceJoint)->setConstraintImpulse(
+          mimicProp.mReferenceDofIndex, -impulse * mimicProp.mMultiplier);
+
+      skeleton->updateBiasImpulse(mBodyNode);
+      const_cast<dynamics::Skeleton*>(mimicProp.mReferenceJoint->getSkeleton().get())
+          ->updateBiasImpulse(const_cast<dynamics::BodyNode*>(mimicProp.mReferenceJoint->getChildBodyNode()));
+
+      skeleton->updateVelocityChange();
+      const_cast<dynamics::Skeleton*>(mimicProp.mReferenceJoint->getSkeleton().get())
+          ->updateVelocityChange();
+
+      mJoint->setConstraintImpulse(i, 0.0);
+      const_cast<dynamics::Joint*>(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<dynamics::Skeleton*>(mimicProp.mReferenceJoint->getSkeleton().get())
+        ->setImpulseApplied(true);
+  }
+}
+
+//==============================================================================
+void CouplerConstraint::unexcite()
+{
+  mJoint->getSkeleton()->setImpulseApplied(false);
+  for (const auto& mimicProp : mMimicProps)
+  {
+    const_cast<dynamics::Skeleton*>(mimicProp.mReferenceJoint->getSkeleton().get())
+        ->setImpulseApplied(false);
+  }
+}
+
+//==============================================================================
+void CouplerConstraint::applyImpulse(double* lambda)
+{
+  std::size_t localIndex = 0;
+  std::size_t dof = mJoint->getNumDofs();
+  for (std::size_t i = 0; i < dof; ++i) {
+    if (mActive[i] == false)
+      continue;
+
+    mJoint->setConstraintImpulse(
+        i, mJoint->getConstraintImpulse(i) + lambda[localIndex]);
+
+    auto& mimicProp = mMimicProps[i];
+
+    // Using const_cast to remove constness for methods that modify state
+    const_cast<dynamics::Joint*>(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
+} // namespace dart

dart/constraint/CouplerConstraint.hpp

@@ -0,0 +1,112 @@
+#ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_
+#define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_
+
+#include <dart/constraint/ConstraintBase.hpp>
+#include <dart/dynamics/MimicDofProperties.hpp>
+#include <vector>
+
+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:
+  /// 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<dynamics::MimicDofProperties>& mimicDofProperties);
+
+  /// Destructor
+  ~CouplerConstraint() override;
+
+  // Documentation inherited
+  const std::string& getType() const override;
+
+  /// Returns constraint type for this class.
+  static const std::string& getStaticType();
+
+  /// Set global constraint force mixing parameter
+  static void setConstraintForceMixing(double cfm);
+
+  /// Get global constraint force mixing parameter
+  static double getConstraintForceMixing();
+
+  // Friendship
+  friend class ConstraintSolver;
+  friend class ConstrainedGroup;
+
+protected:
+  // Documentation inherited
+  void update() override;
+
+  // Documentation inherited
+  void getInformation(ConstraintInfo* lcp) override;
+
+  // Documentation inherited
+  void applyUnitImpulse(std::size_t index) override;
+
+  // Documentation inherited
+  void getVelocityChange(double* delVel, bool withCfm) override;
+
+  // Documentation inherited
+  void excite() override;
+
+  // Documentation inherited
+  void unexcite() override;
+
+  // Documentation inherited
+  void applyImpulse(double* lambda) override;
+
+  // Documentation inherited
+  dynamics::SkeletonPtr getRootSkeleton() const override;
+
+  // Documentation inherited
+  bool isActive() const override;
+
+private:
+  /// Dependent joint whose motion is influenced by the reference joint.
+  dynamics::Joint* mJoint;
+
+  /// Vector of MimicDofProperties for the dependent joint.
+  std::vector<dynamics::MimicDofProperties> 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_