How DART's Constraint Solver Handles Joint Limits and Contacts Simultaneously

DART's constraint solver treats joint limits and contact constraints as unified ConstraintBase objects, assembling them into a single Linear Complementarity Problem (LCP) that is solved together each simulation step to ensure physically consistent interactions.

The dartsim/dart physics engine resolves kinematic and dynamic restrictions through a centralized constraint solver. Rather than processing joint limits and contacts in separate passes, the solver aggregates all active constraints into one sparse LCP matrix, solving them simultaneously to guarantee that joint limits are respected even when contacts exert external forces on the articulated body.

Constraint Collection in ConstraintSolver::updateConstraints()

During each simulation step, the ConstraintSolver::solve() method initiates a full pipeline that begins with updateConstraints(). This function, implemented in dart/constraint/constraint_solver.cpp, scans the simulation state to identify which restrictions are currently violated or active.

The solver maintains three primary categories: manual constraints defined by users, contact constraints derived from collision detection, and joint constraints including limits. Both contact and joint-limit objects inherit from ConstraintBase, allowing uniform treatment in the solver pipeline.

Detecting Contact Constraints

When bodies collide, the solver queries the collision group to generate contacts. The code iterates through detected contacts and instantiates ContactConstraint objects through the contact surface handler:

mCollisionResult.clear();
mCollisionGroup->collide(mCollisionOption, &mCollisionResult);
// creates a ContactConstraint for each contact
for (auto* contact : mContactPtrs) {
  auto contactConstraint = mContactSurfaceHandler->createConstraint(
      *contact, numContacts, mTimeStep);
  mContactConstraints.push_back(contactConstraint);
  if (contactConstraint->isActive())
    mActiveConstraints.push_back(contactConstraint);
}

Each ContactConstraint implements update() to compute penetration depth and activates itself when bodies are intersecting.

Detecting Joint Limit Constraints

For articulated skeletons, the solver examines each joint to determine if limits enforcement is enabled via Joint::areLimitsEnforced(). When true, the solver creates a JointLimitConstraint that monitors the joint's position relative to its upper and lower bounds:

for (const auto& skel : mSkeletons) {
  for (std::size_t i = 0; i < skel->getNumJoints(); ++i) {
    dynamics::Joint* joint = skel->getJoint(i);
    if (joint->areLimitsEnforced() ||
        joint->hasActuatorType(dynamics::Joint::SERVO)) {
      // JointLimitConstraint is created here
      mJointConstraints.push_back(
          std::allocate_shared<JointConstraint>(..., joint));
    }
  }
}

// activate joint-limit constraints
for (auto& jointLimitConstraint : mJointConstraints) {
  jointLimitConstraint->update();
  if (jointLimitConstraint->isActive())
    mActiveConstraints.push_back(jointLimitConstraint);
}

These constraints are stored alongside contacts in the mActiveConstraints vector, preparing them for unified processing.

Assembling the Unified LCP Matrix

Once all active constraints are collected, the solver constructs the LCP system in solveConstrainedGroupInternal(). The matrix A and vectors b, lo, and hi are populated by iterating through every constraint and extracting its Jacobian information via getInformation() and applyUnitImpulse().

Jacobian Assembly for Joint Limits

For joint limits, the constraint computes spatial Jacobians using Joint::getRelativeJacobianStatic(). The applyUnitImpulse() method excites the system to measure how a unit constraint impulse affects the velocities of the connected bodies:

for (std::size_t i = 0; i < numConstraints; ++i) {
  // Fill lo/hi/b/w vectors for each constraint (bounds, bias, etc.)
  constraints[i]->getInformation(&constInfo);

  // Excite the constraint → compute Jacobian contributions
  constraints[i]->excite();
  for (std::size_t j = 0; j < constraints[i]->getDimension(); ++j) {
    constraints[i]->applyUnitImpulse(j);
    // Fill upper-triangle of A
    constraints[i]->getVelocityChange(mA.data() + index, true);
    // Fill cross-terms with later constraints
    for (std::size_t k = i + 1; k < numConstraints; ++k)
      constraints[k]->getVelocityChange(mA.data() + crossIdx, false);
  }
  constraints[i]->unexcite();
}

Jacobian Assembly for Contacts

Similarly, contact constraints invoke applyUnitImpulse() to fill the matrix using spatial normal Jacobians (mSpatialNormalA and mSpatialNormalB from dart/constraint/contact_constraint.hpp) that map contact normals to body velocities. Because both constraint types write into the same matrix A, the solver naturally accounts for coupling between joint compliance and contact forces.

Cross-Coupling Between Constraint Types

The nested loop structure explicitly fills cross-terms between different constraint types. When a joint limit constraint and a contact constraint both affect the same body, their interaction terms appear in the off-diagonal blocks of A, ensuring that solving for contact impulses considers the joint's resistance to motion.

Solving the LCP with Dantzig and PGS Solvers

After assembly, the populated math::LcpProblem is passed to the primary solver. The solution vector x will contain impulse magnitudes for all constraints—contacts first, followed by joint limits—maintaining the order established during collection.

Primary and Secondary Solution Stages

The default configuration uses math::DantzigSolver as the primary LCP solver. If the Dantzig solver fails to converge, the solver falls back to math::PgsSolver (Projected Gauss-Seidel) to obtain a feasible solution:

math::LcpProblem problem(Ablock, mB, mLo, mHi, mFIndex);
math::LcpResult primaryResult = mLcpSolver->solve(problem, mX, options);

Applying Constraint Impulses

With the solution computed, the solver iterates through the active constraint list and distributes the results via applyImpulse():

for (std::size_t i = 0; i < numConstraints; ++i) {
  const ConstraintBasePtr& constraint = constraints[i];
  constraint->applyImpulse(mX.data() + mOffset[i]); // joint limit OR contact
  constraint->excite(); // keep bodies up-to-date for next step
}

For joint limits, applyImpulse() modifies the joint velocity to keep the configuration within bounds. For contacts, it applies normal and friction impulses to prevent penetration. Because both receive impulses from the same solution vector, the corrections are globally consistent.

Split-Impulse Position Correction

When setSplitImpulseEnabled(true) is activated, the solver runs a secondary position-correction phase via solvePositionConstrainedGroups(). This phase solves only contact constraints to resolve penetration visually while leaving joint limit impulses untouched from the velocity phase, preventing jitter in articulated systems.

Practical Implementation Example

The following C++ snippet demonstrates a simulation where a robotic arm with joint limits interacts with ground contacts:

#include <dart/dart.hpp>

int main()
{
  // Create world and a simple two-link arm
  dart::simulation::WorldPtr world = dart::simulation::World::create();

  // Load a skeleton (URDF example) that has joint limits defined
  dart::dynamics::SkeletonPtr arm = dart::utils::SkelParser::readSkeleton(
      "dart://sample/arm.urdf");

  world->addSkeleton(arm);

  // Enable gravity so the end-effector will hit the ground
  world->setGravity(Eigen::Vector3d(0, -9.81, 0));

  // Optionally enable split-impulse for better penetration handling
  world->getConstraintSolver()->setSplitImpulseEnabled(true);

  // Simulate
  for (int i = 0; i < 200; ++i) {
    world->step();
  }
}

In this example, world->step() triggers the complete pipeline: collision detection creates ContactConstraint objects, joint limits generate JointLimitConstraint objects, and the solver assembles them into a single LCP before computing the time step.

Summary

• DART aggregates joint limits and contact constraints into a single ConstraintBase hierarchy managed by ConstraintSolver in dart/constraint/constraint_solver.hpp.
• The updateConstraints() method collects active violations from both sources into the mActiveConstraints vector.
• All constraints contribute Jacobians to the same sparse LCP matrix A in solveConstrainedGroupInternal(), using applyUnitImpulse() and getVelocityChange().
• The system solves simultaneously using math::DantzigSolver with math::PgsSolver as fallback, producing a unified impulse vector x.
• applyImpulse() distributes solution vectors to correct velocities for both joint limits and contacts.
• Optional split-impulse mode handles position-level contact correction separately via solvePositionConstrainedGroups() while preserving joint limit velocities.

Frequently Asked Questions

Does DART process joint limits before or after contact constraints?

Neither. According to the dartsim/dart source code, both constraint types are assembled into the same LCP matrix simultaneously. The solveConstrainedGroupInternal() function fills the matrix with Jacobians from all active constraints—joint limits and contacts included—then solves them together in a single linear complementarity problem.

What happens if a contact force pushes a joint past its limit in DART?

The unified LCP formulation naturally handles this scenario. Because the matrix A contains cross-coupling terms between contact Jacobians and joint limit Jacobians, the solver computes impulses that respect both constraints simultaneously. If a contact would violate a joint limit, the solution vector adjusts the contact impulse magnitude to accommodate the joint's resistance, as implemented in the nested loop of solveConstrainedGroupInternal().

Which LCP solver does DART use for constraint resolution?

DART primarily uses the math::DantzigSolver (a Lemke-style algorithm) as implemented in dart/math/dantzig_solver.hpp. If the primary solver fails to converge, the constraint solver automatically falls back to math::PgsSolver, a Projected Gauss-Seidel iterative method, ensuring robustness across different contact configurations.

Can I disable joint limits while keeping contacts active in DART?

Yes. Joint limits are only enforced when Joint::areLimitsEnforced() returns true, which is checked during ConstraintSolver::updateConstraints(). Disabling limits at the joint level prevents the creation of JointLimitConstraint objects, causing the solver to assemble an LCP containing only contact constraints and other active restrictions.

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