Added coupler constraint

Signed-off-by: Aditya Pande <[email protected]>

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

dart/constraint/CouplerConstraint.hpp

@@ -0,0 +1,32 @@
+#ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_
+#define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_
+
+#include <dart/dynamics/ConstraintBase.hpp>
+#include <dart/dynamics/BodyNode.hpp>
+
+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_
+