diff --git a/dart/constraint/CouplerConstraint.cpp b/dart/constraint/CouplerConstraint.cpp new file mode 100644 index 0000000000000..7d61027918b56 --- /dev/null +++ b/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 diff --git a/dart/constraint/CouplerConstraint.hpp b/dart/constraint/CouplerConstraint.hpp new file mode 100644 index 0000000000000..237e21250b3c6 --- /dev/null +++ b/dart/constraint/CouplerConstraint.hpp @@ -0,0 +1,32 @@ +#ifndef DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ +#define DART_CONSTRAINT_COUPLERCONSTRAINT_HPP_ + +#include +#include + +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_ +