How DART's Articulated Body Algorithm Computes Joint Accelerations

DART computes joint accelerations using the Articulated Body Algorithm (ABA) through a two-pass recursion over the kinematic tree: a backward pass to compute bias forces and articulated inertia, followed by a forward pass to calculate accelerations, transmitted forces, and joint torques.

The Dynamic Animation and Robotics Toolkit (DART) implements the Articulated Body Algorithm (ABA) inside Skeleton::computeForwardDynamics() to efficiently determine joint accelerations without solving large linear systems. This method propagates articulated inertias and bias forces through the skeleton's kinematic tree, making it suitable for real-time robotics simulation. Understanding how DART's ABA computes joint accelerations requires examining the backward and forward recursions defined in the source code.

The Two-Pass Articulated Body Algorithm

DART's ABA operates in linear time relative to the number of bodies by traversing the kinematic tree twice. The first pass walks from leaf nodes to the root, computing bias forces that encapsulate Coriolis, centrifugal, and gravity effects. The second pass walks from the root to the leaves, computing joint accelerations using the articulated inertia accumulated during the first pass.

Backward Recursion: Bias Force and Articulated Inertia

Computing Bias Forces from Leaves to Root

The backward pass begins in dart/dynamics/skeleton.cpp within Skeleton::computeForwardDynamics(). DART iterates through mSkelCache.mBodyNodes in reverse order, calling updateBiasForce() for each BodyNode:

// skeleton.cpp: computeForwardDynamics()
for (auto it = mSkelCache.mBodyNodes.rbegin();
     it != mSkelCache.mBodyNodes.rend(); ++it) {
    (*it)->updateBiasForce(mAspectProperties.mGravity,
                           mAspectProperties.mTimeStep);
}

This loop accumulates the bias force vector b by combining gravitational acceleration with velocity-dependent Coriolis and centrifugal terms. Each body node incorporates the articulated inertia of its children to propagate forces upward through the tree.

Articulated Inertia Assembly

During the backward pass, DART computes the articulated inertia for each body using BodyNode::getArticulatedInertiaImplicit(). This inertia matrix represents the effective mass of the subtree rooted at that body, accounting for all child bodies and their joints. The articulated inertia is stored implicitly and accessed on demand during both the bias force computation and the subsequent forward acceleration calculation.

Forward Recursion: Acceleration and Force Propagation

After completing the backward pass, DART executes a forward recursion from the root to the leaves. This phase computes joint accelerations, transmitted forces, and joint torques using the bias forces and articulated inertias prepared earlier.

Joint Acceleration Calculation

The forward pass in skeleton.cpp invokes three critical methods for each BodyNode:

// skeleton.cpp: computeForwardDynamics() – forward pass
for (auto& bodyNode : mSkelCache.mBodyNodes) {
    bodyNode->updateAccelerationFD();          // (1) joint acceleration α
    bodyNode->updateTransmittedForceFD();      // (2) transmitted force f⁺
    bodyNode->updateJointForceFD(mAspectProperties.mTimeStep,
                                 true, true); // (3) joint torque τ
}

The acceleration computation occurs in BodyNode::updateAccelerationFD() (lines 1991‑2002 of dart/dynamics/body_node.cpp). This method retrieves the articulated inertia via getArticulatedInertiaImplicit() and calls the parent joint's updateAcceleration() method:

// body_node.cpp
if (mParentBodyNode) {
    mParentJoint->updateAcceleration(
        getArticulatedInertiaImplicit(),
        mParentBodyNode->getSpatialAcceleration());
} else {
    mParentJoint->updateAcceleration(
        getArticulatedInertiaImplicit(),
        Eigen::Vector6d::Zero());
}

Inside Joint::updateAcceleration() (defined in dart/dynamics/joint.cpp), DART evaluates the ABA formula:

[ \alpha = I^{-1} \bigl( \tau - J^{T} b \bigr) ]

Where I is the articulated inertia, τ is the joint force vector set via setForces(), and b is the bias force accumulated during the backward pass. The resulting spatial acceleration is stored in the joint's generalized acceleration vector, accessible via Joint::getAccelerations().

Transmitted Force and Joint Torque

Following acceleration calculation, updateTransmittedForceFD() computes the transmitted force f⁺ = b + I·α, propagating the dynamic effect of the child's motion to the parent. Finally, updateJointForceFD() calculates the joint torques required to maintain dynamic equilibrium, using the time step and constraint parameters provided.

Practical Implementation Example

The following C++ example demonstrates how to configure a skeleton, apply external torques, and retrieve joint accelerations using DART's ABA implementation:

#include <dart/dart.hpp>

int main()
{
  // 1. Build a simple 2-link pendulum
  dart::dynamics::SkeletonPtr skel = dart::dynamics::Skeleton::create("pendulum");

  auto joint1 = skel->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>(
      nullptr, dart::dynamics::RevoluteJoint::Properties()).first;
  auto joint2 = skel->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>(
      joint1->getChildBodyNode(), dart::dynamics::RevoluteJoint::Properties()
      ).first;

  // 2. Set a gravity vector
  skel->setGravity(Eigen::Vector3d::UnitY() * -9.81);

  // 3. Give the skeleton a configuration (positions and velocities)
  skel->setPositions(Eigen::Vector2d(0.5, -0.3));
  skel->setVelocities(Eigen::Vector2d(1.0, -0.5));

  // 4. Apply external torques (e.g., a torque on joint 2)
  skel->setForces(Eigen::Vector2d(0.0, 2.0));

  // 5. Run the Articulated-Body forward dynamics
  skel->computeForwardDynamics();

  // 6. Retrieve the joint accelerations
  Eigen::Vector2d acc = skel->getAccelerations();   // → [α₁, α₂]
  std::cout << "Joint accelerations: " << acc.transpose() << std::endl;
}

Key implementation details from this example include calling Skeleton::computeForwardDynamics() to trigger the ABA, and accessing results via Skeleton::getAccelerations() or individual Joint::getAccelerations() calls.

Summary

• DART's ABA implementation in Skeleton::computeForwardDynamics() computes joint accelerations in O(n) time by traversing the kinematic tree twice.
• Backward recursion (leaves to root) calculates bias forces containing Coriolis, centrifugal, and gravity terms using BodyNode::updateBiasForce() and articulated inertias from getArticulatedInertiaImplicit().
• Forward recursion (root to leaves) computes joint accelerations via BodyNode::updateAccelerationFD() and Joint::updateAcceleration(), applying the formula α = I⁻¹(τ - Jᵀb).
• Result access after computeForwardDynamics() is available through Skeleton::getAccelerations() or Joint::getAccelerations().

Frequently Asked Questions

What is the computational complexity of DART's ABA implementation?

DART's Articulated Body Algorithm runs in O(n) time complexity, where n is the number of bodies in the skeleton. This linear scaling is achieved by performing exactly two passes over the kinematic tree: one backward pass to compute bias forces and articulated inertias, and one forward pass to compute accelerations. This is asymptotically faster than constructing and solving the full joint-space inertia matrix, which would be O(n³) using dense linear algebra.

How does DART handle articulated inertias for joints with zero degrees of freedom?

DART handles zero-degree-of-freedom (0-DoF) joints through the ZeroDofJoint class, which still participates in the ABA pipeline. In ZeroDofJoint::updateAcceleration(), the joint acceleration is trivially zero since no motion is permitted, but the articulated inertia is still propagated correctly to parent bodies. This ensures that fixed joints and weld joints contribute their rigid-body inertia to the parent body's articulated inertia without adding degrees of freedom to the state vector.

Can DART's ABA compute accelerations for soft body nodes?

Yes, DART extends the ABA to soft body dynamics through SoftBodyNode, which overrides updateAccelerationFD() in soft_body_node.cpp. Soft body nodes treat the underlying point masses as additional degrees of freedom that are coupled to the rigid parent body. The articulated inertia computation accounts for the deformation modes, and the forward dynamics pass computes accelerations for both the rigid joint degrees of freedom and the soft body's modal coordinates.

Where does DART store the computed joint accelerations after running computeForwardDynamics()?

After Skeleton::computeForwardDynamics() completes, joint accelerations are stored in the generalized acceleration vector of each Joint object. You can access these values at the skeleton level via Skeleton::getAccelerations(), which returns a concatenated vector of all joint accelerations in the order defined by the skeleton's degrees of freedom. For individual joints, call Joint::getAccelerations() to retrieve the specific acceleration values for that joint's degrees of freedom. These values represent the spatial accelerations computed by the ABA formula α = I⁻¹(τ - Jᵀb).

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