How DART Computes Dynamics Jacobians for Control: A Technical Deep Dive

DART computes dynamics Jacobians for control by assembling relative joint Jacobians up the kinematic chain at each BodyNode, transforming them into world coordinates via adjoint operations, and aggregating them into full Skeleton-level matrices for operational-space control.

The Dynamic Animation and Robotics Toolkit (DART) provides a robust framework to compute dynamics Jacobians for control, inverse dynamics, and operational-space applications. Understanding how DART builds these Jacobians—from local joint transformations to full skeleton aggregation—is critical for implementing high-performance robotic controllers. This article examines the source code implementation in dart/dynamics/body_node.cpp and dart/dynamics/skeleton.cpp to reveal the exact mechanisms DART uses to compute dynamics Jacobians for control.

Local Joint Jacobians and Kinematic Chains

Every Joint in DART supplies the foundation for Jacobian computation through its relative Jacobian. The Joint::getRelativeJacobian() method returns the Jacobian of the joint's motion expressed in the child BodyNode frame. This local Jacobian represents how the joint's degrees of freedom (DoFs) affect the motion of the attached body relative to its parent.

When DART computes dynamics Jacobians for control, it propagates these local joint Jacobians up the kinematic tree. Each BodyNode inherits the Jacobian contributions from its parent and appends its own joint's relative Jacobian, creating a cumulative map from configuration space velocities to body spatial velocities.

BodyNode-Level Jacobian Assembly

The BodyNode class serves as the primary computational unit for assembling Jacobians. DART stores the body-node Jacobian in BodyNode::mBodyJacobian and updates it via BodyNode::updateBodyJacobian().

Assembling the Body Jacobian

The update process first copies the parent node's Jacobian (if one exists) and then appends the current joint's local Jacobian:

// body_node.cpp  L1258-L1262
mBodyJacobian.leftCols(numParentDOFs) = mParentBodyNode->getJacobian();
mBodyJacobian.rightCols(localDof)     = mParentJoint->getRelativeJacobian();

This concatenation ensures that the body Jacobian mBodyJacobian maps all ancestor DoFs to the spatial velocity of the current body node.

Transforming to World Coordinates

To compute dynamics Jacobians for control in world coordinates, DART transforms the body Jacobian using the adjoint of the world transformation. The updateWorldJacobian() method applies the math::AdRJac operation:

// body_node.cpp  L1259-L1261
mWorldJacobian = math::AdRJac(getWorldTransform(), getJacobian());

This adjoint transformation converts the Jacobian from the body frame to the world frame, yielding the world Jacobian essential for operational-space control.

Computing Jacobian Derivatives for Control

Advanced control algorithms require not only the Jacobian but also its time derivative. DART computes both spatial and classical (rotational and linear) derivatives to support inverse dynamics and acceleration-based controllers.

Spatial Derivatives

The spatial time derivative is computed in BodyNode::updateBodyJacobianSpatialDeriv() (lines 2630-2646 in body_node.cpp). This method implements the standard kinematic formula using the spatial cross-product operator ad and the adjoint of the relative transform:

The derivative combines the parent's spatial derivative with the current joint's contribution, accounting for velocity propagation through the kinematic chain using spatial algebra.

Classic Derivatives

For controllers requiring separate rotational and linear components, DART provides BodyNode::updateWorldJacobianClassicDeriv() (lines 2515-2575 in body_node.cpp). This method computes the classical time derivative of the world Jacobian, separating the angular and linear velocity components using cross-product operations:

These derivatives enable precise computation of Coriolis and centrifugal effects in operational space, critical for high-performance model-based control.

Skeleton-Level Aggregation

While individual BodyNode objects compute local Jacobians, control applications typically require the full skeleton Jacobian mapping all degrees of freedom to specific operational points. The Skeleton class aggregates these contributions through variadicGetJacobian().

The templated helper creates a zero matrix of size 6 × Skeleton::getNumDofs() and populates it by calling assignJacobian():

// skeleton.cpp  L1781-L1795
const math::Jacobian JBodyNode = _node->getJacobian(args...);
assignJacobian<math::Jacobian>(J, _node, JBodyNode);

The assignJacobian function maps each BodyNode's local Jacobian columns to the corresponding global DoF indices, ensuring the final matrix correctly represents the kinematic chain. This aggregation produces the spatial Jacobian J ∈ ℝ⁶ˣⁿ (where n is the total DoFs) required for operational-space control and inverse dynamics.

Key Source Files

File	Role
dart/dynamics/body_node.cpp	Implements body-node Jacobian assembly (updateBodyJacobian), world transformation (updateWorldJacobian), and derivative calculations (updateBodyJacobianSpatialDeriv, updateWorldJacobianClassicDeriv).
dart/dynamics/skeleton.cpp	Provides the public API (Skeleton::getJacobian, getWorldJacobian) and assembles full-skeleton Jacobians via variadicGetJacobian and assignJacobian.
dart/dynamics/jacobian_node.hpp	Defines the JacobianNode base class inherited by BodyNode for Jacobian interface standardization.
dart/math/Helpers.hpp	Contains transformation helpers including math::AdRJac, math::AdInvTJac, and math::adJac used in Jacobian coordinate transforms.
dart/dynamics/joint.cpp	Supplies Joint::getRelativeJacobian(), providing the local joint Jacobians that propagate up the kinematic tree.

Practical Implementation Examples

The following examples demonstrate how to extract and use dynamics Jacobians for control in DART applications.

Example 1: Computing the End-Effector Jacobian

Retrieve the spatial Jacobian for an end-effector in world coordinates:

#include <dart/dart.hpp>

int main()
{
  // Load a simple robot (e.g. a 2‑link arm)
  dart::dynamics::SkeletonPtr robot = dart::utils::SdfParser::readSkeleton(
      "path/to/robot.sdf");

  // Assume the end‑effector node is named "ee"
  auto* ee = robot->getBodyNode("ee");

  // Spatial Jacobian (6×n) expressed in world coordinates
  Eigen::MatrixXd J_ee = robot->getJacobian(ee);               // world Jacobian
  // Or explicitly request world Jacobian:
  // Eigen::MatrixXd J_ee = robot->getWorldJacobian(ee);

  // Linear part (3×n) of the Jacobian (velocity of the tip)
  Eigen::MatrixXd Jv_ee = robot->getLinearJacobian(ee);
}

Example 2: Computing Jacobian Derivatives

Access the classical time derivative for acceleration-based control:

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

int main()
{
  auto robot = dart::utils::SdfParser::readSkeleton("robot.sdf");
  auto* wrist = robot->getBodyNode("wrist");

  // Classic Jacobian derivative (rotational + linear) in world frame
  Eigen::MatrixXd dJ_classic = robot->getJacobianClassicDeriv(wrist);

  std::cout << "dJ_classic (6×n):\n" << dJ_classic << std::endl;
}

Example 3: Operational-Space Control

Implement a damped least-squares controller using the Jacobian:

#include <dart/dart.hpp>
#include <Eigen/Dense>

int main()
{
  auto robot = dart::utils::SdfParser::readSkeleton("robot.sdf");
  auto* tip = robot->getBodyNode("tip");

  // Desired Cartesian velocity for the tip
  Eigen::Vector6d v_des;
  v_des << 0.1, 0, 0, 0, 0, 0;   // 0.1 m/s along x

  // Compute spatial Jacobian (world)
  Eigen::MatrixXd J = robot->getJacobian(tip);

  // Damped least‑squares joint velocity command
  double lambda = 1e-4;
  Eigen::VectorXd qdot = J.transpose() *
                         (J * J.transpose() + lambda * Eigen::MatrixXd::Identity(6,6))
                         .inverse() *
                         v_des;

  robot->setVelocities(qdot);
}

Summary

• DART computes dynamics Jacobians for control by assembling relative joint Jacobians (Joint::getRelativeJacobian) up the kinematic chain at each BodyNode.
• The BodyNode::updateBodyJacobian() method constructs local Jacobians by concatenating parent Jacobians with current joint Jacobians, while updateWorldJacobian() applies adjoint transformations (math::AdRJac) to convert to world coordinates.
• Jacobian derivatives for acceleration-based control are computed via updateBodyJacobianSpatialDeriv() (spatial) and updateWorldJacobianClassicDeriv() (classical), utilizing spatial cross-product operators and adjoint math.
• The Skeleton class aggregates individual body Jacobians into full 6×n matrices (where n is total DoFs) using variadicGetJacobian() and assignJacobian(), producing the complete spatial Jacobian required for whole-body control algorithms.

Frequently Asked Questions

What is the difference between a BodyNode Jacobian and a World Jacobian in DART?

The BodyNode Jacobian (mBodyJacobian) expresses the spatial velocity of the body relative to the body frame, while the World Jacobian (mWorldJacobian) transforms this quantity into world coordinates using the adjoint of the body's world transformation (math::AdRJac). Control applications typically use the World Jacobian to map joint velocities to Cartesian velocities in a global reference frame.

How does DART handle Jacobian derivatives for inverse dynamics?

DART computes Jacobian derivatives through two specialized methods in BodyNode: updateBodyJacobianSpatialDeriv() for spatial derivatives and updateWorldJacobianClassicDeriv() for classical (rotational and linear) derivatives. These methods implement standard kinematic formulas using the spatial cross-product operator ad and adjoint transformations, allowing controllers to account for Coriolis and centrifugal effects when computing operational space accelerations.

Where does DART assemble the full Skeleton Jacobian from individual BodyNodes?

The assembly occurs in Skeleton::variadicGetJacobian() (lines 1781-1795 in skeleton.cpp), which creates a zero matrix of size 6 × Skeleton::getNumDofs() and populates it by calling assignJacobian(). This function maps each BodyNode's local Jacobian columns to the corresponding global DoF indices, producing the complete spatial Jacobian matrix required for whole-body control algorithms.

What is the role of the adjoint transformation in DART's Jacobian calculations?

The adjoint transformation (implemented in math::AdRJac and related helpers in dart/math/Helpers.hpp) converts Jacobians between reference frames without re-computing geometric relationships from scratch. Specifically, updateWorldJacobian() uses math::AdRJac(getWorldTransform(), getJacobian()) to transform the body-frame Jacobian into world coordinates, leveraging the property that spatial velocities transform via the adjoint of the homogeneous transformation matrix.

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