How to Implement Balance Constraints for Humanoid Robots in DART

DART provides the BalanceConstraint class that computes the distance between a robot's center of mass and its support polygon, driving the hierarchical inverse kinematics solver to maintain stability by either shifting supporting feet or adjusting the CoM position.

The Dynamic Animation and Robotics Toolkit (DART) offers a specialized kinematic balance constraint for humanoid robots that integrates directly with its hierarchical inverse kinematics (IK) solver. This constraint ensures the robot's center of mass (CoM) remains within the support polygon defined by foot contacts, enabling stable standing and walking behaviors. In this guide, you'll learn how to implement balance constraints using the actual source code from the dartsim/dart repository.

How Balance Constraints Work in DART

The dart::constraint::BalanceConstraint class evaluates balance by calculating the distance between the robot's CoM projection and the support polygon formed by active end-effectors. According to the implementation in dart/constraint/balance_constraint.cpp, the constraint supports three error calculation methods defined in the ErrorMethod_t enum:

• FROM_CENTROID – Measures the distance from the CoM projection to the geometric centroid of the support polygon (default behavior).
• FROM_EDGE – Computes the distance to the nearest polygon edge, useful for edge-contact balancing scenarios.
• OPTIMIZE_BALANCE – Treats balance as a continuous optimization objective rather than a hard constraint.

The constraint also provides two balance methods via BalanceMethod_t:

• SHIFT_SUPPORT – Moves the supporting end-effectors (feet) to recenter the CoM.
• SHIFT_COM – Adjusts the robot's body configuration to move the CoM while keeping feet fixed.

Source Code Architecture

The balance constraint implementation spans two primary files in the DART codebase:

In dart/constraint/balance_constraint.hpp, the class declares the public API including eval(), evalGradient(), clone(), and accessor methods for the error and balance methods. The header defines the enums ErrorMethod_t and BalanceMethod_t that control constraint behavior.

The dart/constraint/balance_constraint.cpp file contains the computational logic:

• eval() computes the CoM, projects it onto the support-polygon plane, and calculates the error vector to either the centroid or nearest edge.
• evalGradient() constructs the optimization gradient using a damped pseudoinverse of the relevant Jacobians. When using SHIFT_SUPPORT, it moves the supporting end-effectors; with SHIFT_COM, it adjusts the CoM directly while utilizing a cached null-space to preserve non-target DOFs.

Helper functions addDampedPseudoInverseToGradient() and convertJacobianMethodOutputToGradient() handle the linear algebra conversion from Cartesian space back to joint-space updates.

Creating Support End-Effectors

Before applying balance constraints, you must define support geometries for the robot's feet. The SupportGeometry defines the contact polygon that constrains the CoM.

#include <dart/dart.hpp>
using namespace dart;
using namespace dart::dynamics;
using namespace dart::constraint;

math::SupportGeometry makeFootGeometry()
{
  math::SupportGeometry geom;
  geom.emplace_back(Eigen::Vector3d(0.10,  0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(0.10, -0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(-0.10, -0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(-0.10,  0.05, 0.0));
  return geom;
}

EndEffector* addFoot(SkeletonPtr skel,
                     BodyNode* parent,
                     const std::string& name,
                     const Eigen::Vector3d& offset,
                     const math::SupportGeometry& geom)
{
  EndEffector* ee = parent->createEndEffector(name);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
  tf.translation() = offset;
  ee->setDefaultRelativeTransform(tf);
  ee->resetRelativeTransform();

  auto* support = ee->getSupport(true);
  support->setGeometry(geom);
  support->setActive(true);
  return ee;
}

Each EndEffector must have its support geometry activated via setActive(true) to be included in the support polygon calculation during IK solving.

Configuring BalanceConstraint Parameters

Error Method Selection

The error method determines how the constraint calculates the "balance error" that drives the optimization:

• FROM_CENTROID (default): Drives the CoM toward the center of the support polygon. Best for stable standing poses.
• FROM_EDGE: Pushes the CoM away from the nearest polygon edge. Use this when balancing on edges or narrow supports.
• OPTIMIZE_BALANCE: Treats the distance as an optimization objective, continuously driving the CoM toward the centroid without hard constraints.

Balance Method Selection

The balance method determines which degrees of freedom the solver adjusts:

• SHIFT_SUPPORT (default): The solver moves the supporting end-effectors (feet) to achieve balance. Use this when foot placement is flexible.
• SHIFT_COM: The solver adjusts the robot's joint configuration to move the CoM while keeping feet fixed. Use this for in-place balancing where foot positions are constrained by terrain.

Integrating with HierarchicalIK

The BalanceConstraint is designed to work with dart::dynamics::HierarchicalIK or its concrete subclass WholeBodyIK. The typical integration workflow involves four steps:

1. Create a WholeBodyIK instance from your skeleton.
2. Instantiate a BalanceConstraint with the IK pointer and selected methods.
3. Add the constraint to the IK problem.
4. Solve the IK problem.

// Create skeleton with floating base
SkeletonPtr robot = Skeleton::create("humanoid");
robot->setGravity(Eigen::Vector3d(0, 0, -9.81));
auto [rootJoint, rootBody] = robot->createJointAndBodyNodePair<FreeJoint>();
rootBody->setMass(10.0);

// Add feet with support geometry
auto footGeom = makeFootGeometry();
addFoot(robot, rootBody, "left_foot",
        Eigen::Vector3d(0.8,  0.15, 0.0), footGeom);
addFoot(robot, rootBody, "right_foot",
        Eigen::Vector3d(1.0, -0.15, 0.0), footGeom);

// Create IK solver
auto ik = WholeBodyIK::create(robot);

// Configure balance constraint
BalanceConstraint balance(
    ik,
    BalanceConstraint::SHIFT_SUPPORT,
    BalanceConstraint::FROM_CENTROID);

// Add to solver
ik->addConstraint(std::make_shared<BalanceConstraint>(balance));

When ik->solve() is called, the hierarchical solver evaluates BalanceConstraint::evalGradient() to compute joint updates that reduce the balance error to zero.

Complete Implementation Example

The following complete example demonstrates constructing a biped, creating an intentional imbalance, and using the constraint to restore stability:

#include <dart/dart.hpp>
#include <iostream>

using namespace dart;
using namespace dart::dynamics;
using namespace dart::constraint;

math::SupportGeometry makeFootGeometry()
{
  math::SupportGeometry geom;
  geom.emplace_back(Eigen::Vector3d(0.10,  0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(0.10, -0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(-0.10, -0.05, 0.0));
  geom.emplace_back(Eigen::Vector3d(-0.10,  0.05, 0.0));
  return geom;
}

EndEffector* addFoot(SkeletonPtr skel,
                     BodyNode* parent,
                     const std::string& name,
                     const Eigen::Vector3d& offset,
                     const math::SupportGeometry& geom)
{
  EndEffector* ee = parent->createEndEffector(name);
  Eigen::Isometry3d tf = Eigen::Isometry3d::Identity();
  tf.translation() = offset;
  ee->setDefaultRelativeTransform(tf);
  ee->resetRelativeTransform();

  auto* support = ee->getSupport(true);
  support->setGeometry(geom);
  support->setActive(true);
  return ee;
}

int main()
{
  // Build skeleton
  SkeletonPtr robot = Skeleton::create("humanoid");
  robot->setGravity(Eigen::Vector3d(0, 0, -9.81));
  
  auto [rootJoint, rootBody] = robot->createJointAndBodyNodePair<FreeJoint>();
  rootBody->setMass(10.0);
  
  auto footGeom = makeFootGeometry();
  addFoot(robot, rootBody, "left_foot",
          Eigen::Vector3d(0.8,  0.15, 0.0), footGeom);
  addFoot(robot, rootBody, "right_foot",
          Eigen::Vector3d(1.0, -0.15, 0.0), footGeom);
  
  // Create IK and balance constraint
  auto ik = WholeBodyIK::create(robot);
  
  BalanceConstraint balance(
      ik,
      BalanceConstraint::SHIFT_SUPPORT,
      BalanceConstraint::FROM_CENTROID);
  
  ik->addConstraint(std::make_shared<BalanceConstraint>(balance));
  
  // Initialize neutral pose
  Eigen::VectorXd q = robot->getPositions();
  q.setZero();
  robot->setPositions(q);
  
  // Artificially shift CoM to create imbalance
  if (robot->getNumDofs() > 0) {
    q[0] = 0.5;  // Shift root X position
    robot->setPositions(q);
  }
  
  std::cout << "Initial CoM: " << robot->getCOM().transpose() << std::endl;
  
  // Solve to restore balance
  ik->solve();
  
  std::cout << "Final CoM: " << robot->getCOM().transpose() << std::endl;
  
  return 0;
}

The solve() method internally calls eval() to compute the balance error and evalGradient() to generate joint updates. When using SHIFT_SUPPORT, the solver adjusts foot positions until the CoM projection lies within the support polygon centroid.

Tuning and Advanced Options

The BalanceConstraint class provides several parameters to stabilize the optimization:

• setOptimizationTolerance(double) – When using OPTIMIZE_BALANCE, this sets the threshold below which the error is treated as zero.
• setPseudoInverseDamping(double) – Controls the damping factor in the damped pseudoinverse calculation within evalGradient(), preventing numerical instability when Jacobians are near-singular.

For uneven terrain scenarios, combine FROM_EDGE with SHIFT_COM to maintain edge-distance margins while keeping feet planted on irregular surfaces. The unit tests in tests/unit/constraint/test_balance_constraint.cpp demonstrate additional edge cases including single-foot balance and moving support polygons.

Summary

• The BalanceConstraint class in dart/constraint/balance_constraint.hpp provides kinematic balance control for humanoid robots by measuring CoM-to-polygon distance.
• Three error methods (FROM_CENTROID, FROM_EDGE, OPTIMIZE_BALANCE) and two balance methods (SHIFT_SUPPORT, SHIFT_COM) adapt the constraint to different stability scenarios.
• Support end-effectors require SupportGeometry definitions and must be activated via setActive(true) to contribute to the balance polygon.
• Integration requires wrapping the constraint in a shared_ptr and adding it to a WholeBodyIK or HierarchicalIK instance before calling solve().
• The evalGradient() method uses damped pseudoinverse calculations to convert Cartesian balance errors into feasible joint-space updates.

Frequently Asked Questions

What is the difference between FROM_CENTROID and FROM_EDGE error methods?

FROM_CENTROID measures the distance from the projected CoM to the geometric center of the support polygon, driving the robot toward the most stable central position. FROM_EDGE calculates the distance to the nearest polygon edge, which is useful when balancing on narrow supports where proximity to any edge represents instability rather than distance from the center.

When should I use SHIFT_COM instead of SHIFT_SUPPORT?

Use SHIFT_COM when the robot's foot positions are constrained by terrain or contact requirements, requiring the solver to adjust the body configuration (torso, arms, legs) to move the CoM. Use SHIFT_SUPPORT (the default) when foot placement is flexible, allowing the solver to reposition the feet to achieve balance.

How do I handle balance constraints during walking motions?

For dynamic walking, combine the balance constraint with a trajectory-following objective in the hierarchical IK setup. Use SHIFT_SUPPORT with FROM_EDGE during single-support phases to maintain minimum distance from the support polygon edge, and switch to FROM_CENTROID during double-support phases for maximum stability. The constraint integrates naturally with DART's hierarchical solver, allowing you to prioritize foot placement objectives over balance when needed.

What files contain the BalanceConstraint implementation?

The implementation is split between the header dart/constraint/balance_constraint.hpp (declaring the class, enums, and API) and the source dart/constraint/balance_constraint.cpp (containing eval(), evalGradient(), and the damped pseudoinverse logic). Reference implementations and test cases are available in tests/unit/constraint/test_balance_constraint.cpp.

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