How to Implement Custom Constraints in DART for Closed-Loop Mechanisms

To implement custom constraints in DART for closed-loop mechanisms, derive from dart::constraint::DynamicJointConstraint, override the pure virtual interface defined in ConstraintBase, and register the instance with the world's ConstraintSolver.

DART (Dynamic Animation and Robotics Toolkit) provides a modular constraint system that enables closed-loop kinematic chains through runtime constraint management. This article explains how to implement custom constraints in DART for closed-loop mechanisms by extending the appropriate base classes, implementing the required virtual methods, and integrating with the ConstraintSolver.

Understanding DART's Constraint Hierarchy

DART’s constraint architecture separates low-level mathematical constraints from high-level joint management. Choosing the correct base class determines how your constraint interacts with the simulation engine and the LCP (Linear Complementarity Problem) solver.

ConstraintBase vs DynamicJointConstraint

Base Class	Use Case	Key Header
dart::constraint::ConstraintBase	Static constraints that never change bodies or require minimal solver integration.	[constraint_base.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constraint_base.hpp)
dart::constraint::DynamicJointConstraint	Constraints binding two BodyNodes that may be added or removed at runtime, such as loop closures.	[dynamic_joint_constraint.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/dynamic_joint_constraint.hpp)

For closed-loop mechanisms, always inherit from DynamicJointConstraint. This base class provides bookkeeping for the two participating bodies, static error-reduction parameters (ERP), and constraint force mixing (CFM), while requiring you to implement the pure virtual interface from ConstraintBase.

Implementing a Custom Loop-Closure Constraint

Creating a functional constraint requires implementing nine pure virtual methods that define the constraint's Jacobian, violation, and impulse application logic. The following implementation demonstrates a custom weld-like constraint for closing kinematic loops.

Class Definition and Constructor

Define your constraint in a header file, inheriting from DynamicJointConstraint and storing the relative transform and Jacobian matrices:

// my_custom_loop_constraint.hpp
#ifndef MY_CUSTOM_LOOP_CONSTRAINT_HPP_
#define MY_CUSTOM_LOOP_CONSTRAINT_HPP_

#include <dart/constraint/dynamic_joint_constraint.hpp>
#include <dart/dynamics/BodyNode.hpp>
#include <Eigen/Dense>

namespace myproject {

class CustomLoopConstraint final : public dart::constraint::DynamicJointConstraint
{
public:
  CustomLoopConstraint(dart::dynamics::BodyNode* body1,
                       dart::dynamics::BodyNode* body2)
    : DynamicJointConstraint(body1, body2),
      mRelativeTransform(Eigen::Isometry3d::Identity()),
      mJacobian1(Eigen::Matrix6d::Zero()),
      mJacobian2(Eigen::Matrix6d::Zero()),
      mAppliedImpulseIndex(0)
  {}

  void setRelativeTransform(const Eigen::Isometry3d& tf) { 
    mRelativeTransform = tf; 
  }
  
  const Eigen::Isometry3d& getRelativeTransform() const { 
    return mRelativeTransform; 
  }

  // ConstraintBase interface
  void update() override;
  void getInformation(ConstraintInfo* info) override;
  void applyUnitImpulse(std::size_t index) override;
  void getVelocityChange(double* vel, bool withCfm) override;
  void excite() override;
  void unexcite() override;
  void applyImpulse(double* lambda) override;
  bool isActive() const override { return true; }
  dart::dynamics::SkeletonPtr getRootSkeleton() const override;

private:
  Eigen::Isometry3d mRelativeTransform;
  Eigen::Vector6d   mViolation;
  Eigen::Matrix6d   mJacobian1;  // ∂C/∂v₁
  Eigen::Matrix6d   mJacobian2;  // ∂C/∂v₂
  std::size_t       mAppliedImpulseIndex;
};

} // namespace myproject
#endif

Required Virtual Methods

Implement the following methods in your .cpp file to integrate with DART's LCP solver:

• update() – Compute the 6-DOF violation vector (position/orientation error) and fill mJacobian1 and mJacobian2 using the world transforms of the two bodies. Follow the Jacobian computation pattern found in [weld_joint_constraint.cpp](https://github.com/dartsim/dart/blob/main/dart/constraint/weld_joint_constraint.cpp).

• getInformation(ConstraintInfo* info) – Populate the ConstraintInfo struct with pointers to the constraint's LCP variables: solution vector x, bounds lo/hi, right-hand side b, and constraint force mixing diagonal w.

• applyUnitImpulse(std::size_t index) – Apply a unit impulse in constraint space to the bodies using BodyNode::addConstraintImpulse. The index parameter identifies which row of the constraint Jacobian is being processed.

• getVelocityChange(double* vel, bool withCfm) – Return the velocity change caused by the applied unit impulse, optionally incorporating CFM scaling.

• excite() / unexcite() – Control error reduction parameter (ERP) application for warm-starting or stabilizing the constraint.

• applyImpulse(double* lambda) – Apply the final impulse vector lambda computed by the LCP solver to the bodies.

• getRootSkeleton() – Return the common ancestor skeleton of the two bodies, calling uniteSkeletons() if necessary to ensure they belong to the same ConstrainedGroup.

Registering and Managing Constraints

After instantiation, you must register the constraint with the world's constraint solver to include it in the LCP solve step.

// Retrieve the two bodies participating in the closed loop
auto* bodyA = skeleton->getBodyNode("link1");
auto* bodyB = skeleton->getBodyNode("link2");

// Instantiate the custom constraint
auto loopConstraint = std::make_shared<myproject::CustomLoopConstraint>(bodyA, bodyB);
loopConstraint->setRelativeTransform(Eigen::Isometry3d::Identity());

// Register with the constraint solver
world->getConstraintSolver()->addConstraint(loopConstraint);

The ConstraintSolver (defined in [constraint_solver.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constraint_solver.hpp)) manages ConstrainedGroup instances (see [constrained_group.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constrained_group.hpp)), which handle union-find operations to batch connected skeletons into single LCP solves.

Factory Helper Pattern

For cleaner client code, expose a factory function:

std::shared_ptr<myproject::CustomLoopConstraint>
makeLoopConstraint(dart::dynamics::BodyNode* a,
                   dart::dynamics::BodyNode* b,
                   const Eigen::Isometry3d& tf = Eigen::Isometry3d::Identity())
{
  auto c = std::make_shared<myproject::CustomLoopConstraint>(a, b);
  c->setRelativeTransform(tf);
  return c;
}

Usage becomes:

world->getConstraintSolver()->addConstraint(
    makeLoopConstraint(bodyA, bodyB, desiredTransform));

Reference Implementation and Key Source Files

Study the built-in WeldJointConstraint to understand the exact Jacobian math and LCP filling pattern:

Component	Header	Source
Base interface	[constraint_base.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constraint_base.hpp)	—
Dynamic joint base	[dynamic_joint_constraint.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/dynamic_joint_constraint.hpp)	—
Reference implementation (Weld)	[weld_joint_constraint.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/weld_joint_constraint.hpp)	[weld_joint_constraint.cpp](https://github.com/dartsim/dart/blob/main/dart/constraint/weld_joint_constraint.cpp)
Solver & groups	[constraint_solver.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constraint_solver.hpp)	[constrained_group.hpp](https://github.com/dartsim/dart/blob/main/dart/constraint/constrained_group.hpp)
World access	[world.hpp](https://github.com/dartsim/dart/blob/main/dart/simulation/world.hpp)	—
Example application	—	[examples/rigid_loop/main.cpp](https://github.com/dartsim/dart/blob/main/examples/rigid_loop/main.cpp)

Summary

To implement custom constraints in DART for closed-loop mechanisms, follow these essential steps:

• Inherit from DynamicJointConstraint rather than the lower-level ConstraintBase to leverage built-in body management and error reduction parameters.
• Override all pure virtual methods from ConstraintBase, particularly update() for Jacobian computation, getInformation() for LCP setup, and impulse application methods.
• Compute Jacobians using world transforms of the participating BodyNodes, following the mathematical pattern in weld_joint_constraint.cpp.
• Register constraints with the simulation world via world->getConstraintSolver()->addConstraint() to include them in the LCP solve step.
• Manage skeleton connectivity by implementing getRootSkeleton() to ensure the constraint solver correctly groups connected bodies using union-find logic.

Frequently Asked Questions

What is the difference between ConstraintBase and DynamicJointConstraint in DART?

ConstraintBase is the abstract root class defining the pure virtual interface that all constraints must implement, including methods for updating Jacobians and applying impulses. DynamicJointConstraint extends this interface specifically for constraints that bind two BodyNode instances and may be created or destroyed during simulation runtime. For closed-loop mechanisms, you must use DynamicJointConstraint because it provides the bookkeeping necessary to manage body pairs and error reduction parameters.

How do I compute Jacobians for a custom constraint in DART?

In your update() override, compute the 6-DOF violation vector representing position and orientation error between the two bodies. Then fill mJacobian1 and mJacobian2 (typically 6×6 matrices) with the partial derivatives of the constraint error with respect to each body's velocity, using their world transforms and spatial algebra. The reference implementation in weld_joint_constraint.cpp demonstrates the exact math for constructing these Jacobians from relative transforms.

Can I add and remove constraints dynamically during simulation?

Yes. Because you inherit from DynamicJointConstraint, your custom constraint supports dynamic addition and removal. Use world->getConstraintSolver()->addConstraint() to register the constraint at any time, and call removeConstraint() to detach it. The ConstrainedGroup class automatically handles union-find operations to batch connected skeletons into constraint groups, ensuring that adding or removing a constraint correctly updates the LCP solve structure without requiring manual skeleton management.

Where can I find a complete working example of a closed-loop mechanism in DART?

The DART repository includes a working example in examples/rigid_loop/main.cpp, which demonstrates how to create a rigid closed-loop mechanism using the built-in WeldJointConstraint. Study this file alongside weld_joint_constraint.hpp and weld_joint_constraint.cpp to see the complete implementation pattern, including how to compute Jacobians, fill the ConstraintInfo struct for the LCP solver, and apply constraint impulses to body nodes.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}