How to Implement Operational Space Control in DART: A Complete C++ Guide

You implement operational space control in DART by computing joint torques from Cartesian tracking errors using the robot's mass matrix, end-effector Jacobian, and a damped pseudo-inverse, then applying these torques via Skeleton::setForces() inside the customPreStep callback.

Operational space control (OSC) enables direct command of a robot's end-effector in Cartesian coordinates while dynamically compensating for the robot's natural dynamics and coupling. If you are working with the dartsim/dart physics engine, you can implement operational space control in DART by leveraging the Skeleton and BodyNode APIs to extract real-time dynamics and kinematics, then computing the control law in the simulation loop before physics integration.

The Three Steps of Operational Space Control in DART

The implementation follows a standard three-step pipeline performed every simulation tick inside your WorldNode::customPreStep override.

1. Retrieve Robot Dynamics and Jacobians

First, extract the current dynamic properties and kinematic mappings from the skeleton. According to the reference implementation in examples/operational_space_control/main.cpp, you need:

• Mass matrix M via Skeleton::getMassMatrix() (line 91)
• Linear Jacobian J via BodyNode::getLinearJacobian() (lines 93-94)
• Jacobian time-derivative Ĵ via BodyNode::getLinearJacobianDeriv() (lines 99-100)
• Coriolis and gravity forces Cg via Skeleton::getCoriolisAndGravityForces() (line 110)

Eigen::MatrixXd M = mRobot->getMassMatrix();
Eigen::MatrixXd Cg = mRobot->getCoriolisAndGravityForces();

Eigen::Vector3d offset(0.05, 0.0, 0.0); // End-effector offset
Eigen::MatrixXd J = mEE->getLinearJacobian(offset);
Eigen::MatrixXd dJ = mEE->getLinearJacobianDeriv(offset);

2. Compute the Damped Pseudo-Inverse

To avoid numerical instabilities when the Jacobian approaches singularity, compute a damped pseudo-inverse (regularized inverse). The formula used in the DART example (lines 94-98 and 101-103) follows the standard regularization:

[ J^{#} = J^{\mathsf{T}}(JJ^{\mathsf{T}} + \varepsilon I)^{-1} ]

Where (\varepsilon) is a small damping constant (typically (0.05^2)).

const double eps = 0.05 * 0.05;
Eigen::Matrix3d I3 = Eigen::Matrix3d::Identity();

Eigen::MatrixXd Jpinv = J.transpose() * (J * J.transpose() + eps * I3).inverse();
Eigen::MatrixXd dJpinv = dJ.transpose() * (dJ * dJ.transpose() + eps * I3).inverse();

3. Apply the OSC Control Law

Finally, compute the joint-space torques (\tau) that realize the desired Cartesian behavior. The control law implemented in main.cpp (lines 112-115) combines:

• Position error (e = p_{\text{target}} - p_{\text{ee}})
• Velocity error (e_d = -\dot{p}_{\text{ee}}) (since desired velocity is zero for regulation)
• Proportional gain (K_p) and derivative gain (K_d)

[ \tau = M\bigl(J^{#}K_{p}e_{d} + \dot{J}^{#}K_{p}e\bigr) + C_{g} + K_{d}J^{#}K_{p}e ]

// Errors
Eigen::Vector3d e = mTarget->getWorldTransform().translation() 
                  - mEE->getWorldTransform() * offset;
Eigen::Vector3d de = -mEE->getLinearVelocity(offset);

// Control law
Eigen::VectorXd tau = M * (Jpinv * mKp * de + dJpinv * mKp * e)
                    + Cg + mKd * Jpinv * mKp * e;

mRobot->setForces(tau);

The torques are applied via Skeleton::setForces() before the physics integration step, ensuring the controller commands affect the next simulation state.

Complete C++ Implementation Example

Below is a minimal, runnable structure based on the official operational_space_control example. It inherits from dart::gui::WorldNode and overrides customPreStep to inject the controller.

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

class MyOSCWorld : public dart::gui::WorldNode
{
public:
  MyOSCWorld(dart::simulation::WorldPtr world)
    : dart::gui::WorldNode(world)
  {
    mRobot = mWorld->getSkeleton(0);
    mEE    = mRobot->getBodyNode(mRobot->getNumBodyNodes() - 1);

    // Gains
    mKp.setIdentity();  mKp.diagonal() << 50, 50, 50;
    mKd.setIdentity();  mKd *= 5.0;

    // Target frame (red sphere)
    Eigen::Isometry3d tf = mEE->getWorldTransform();
    tf.pretranslate(Eigen::Vector3d(0.05, 0, 0));
    mTarget = std::make_shared<dart::dynamics::SimpleFrame>(
        dart::dynamics::Frame::World(), "target", tf);
    mTarget->setShape(std::make_shared<dart::dynamics::SphereShape>(0.025));
    mWorld->addSimpleFrame(mTarget);
  }

  void customPreStep() override
  {
    // Dynamics
    Eigen::MatrixXd M  = mRobot->getMassMatrix();
    Eigen::MatrixXd Cg = mRobot->getCoriolisAndGravityForces();

    // Jacobians
    Eigen::Vector3d offset(0.05, 0, 0);
    Eigen::MatrixXd J  = mEE->getLinearJacobian(offset);
    Eigen::MatrixXd dJ = mEE->getLinearJacobianDeriv(offset);

    // Damped pseudo-inverse
    const double eps = 0.05 * 0.05;
    Eigen::MatrixXd Jpinv  = J.transpose() * (J * J.transpose() + eps * Eigen::Matrix3d::Identity()).inverse();
    Eigen::MatrixXd dJpinv = dJ.transpose() * (dJ * dJ.transpose() + eps * Eigen::Matrix3d::Identity()).inverse();

    // Errors
    Eigen::Vector3d e  = mTarget->getWorldTransform().translation() - mEE->getWorldTransform() * offset;
    Eigen::Vector3d de = -mEE->getLinearVelocity(offset);

    // OSC torque
    Eigen::VectorXd tau = M * (Jpinv * mKp * de + dJpinv * mKp * e) + Cg + mKd * Jpinv * mKp * e;

    mRobot->setForces(tau);
  }

private:
  dart::dynamics::SkeletonPtr mRobot;
  dart::dynamics::BodyNode*   mEE;
  dart::dynamics::SimpleFramePtr mTarget;
  Eigen::Matrix3d mKp;
  Eigen::Matrix3d mKd;
};

To run this, instantiate the class and add it to a dart::gui::Viewer as shown in the official example at examples/operational_space_control/main.cpp.

Key DART APIs for Operational Space Control

The following classes and methods form the foundation of any OSC implementation in DART:

Component	DART API	Purpose
Mass Matrix	Skeleton::getMassMatrix()	Retrieves the joint-space inertia matrix (M) for dynamic compensation.
Bias Forces	Skeleton::getCoriolisAndGravityForces()	Returns the Coriolis and gravity vector (C_g).
Linear Jacobian	BodyNode::getLinearJacobian()	Computes the Jacobian (J) mapping joint velocities to end-effector linear velocity.
Jacobian Derivative	BodyNode::getLinearJacobianDeriv()	Computes (\dot{J}) required for acceleration-level control.
Force Application	Skeleton::setForces()	Applies the computed joint torques (\tau) to the skeleton.
Control Hook	WorldNode::customPreStep()	Virtual override called before each physics step to inject controller logic.

Reference implementations and theoretical background are available in:

• examples/operational_space_control/main.cpp – Full interactive demo with GUI
• docs/readthedocs/topics/control-theory.md – Mapping of control theory symbols to DART APIs

Summary

• Operational space control in DART requires computing joint torques from Cartesian errors using the robot's dynamic model.
• Three core steps occur every tick: retrieve M, J, Ĵ, and Cg via Skeleton and BodyNode APIs; compute the damped pseudo-inverse of the Jacobian; and apply the OSC control law.
• Implementation happens inside customPreStep() of a WorldNode subclass, ensuring torques are applied via Skeleton::setForces() before the physics integration.
• Regularization (damping) is essential to handle singularities when inverting the Jacobian.

Frequently Asked Questions

What is the difference between joint space and operational space control?

Joint space control computes torques based on desired joint angles and velocities, which requires inverse kinematics to reach Cartesian goals. Operational space control computes torques directly from Cartesian errors (position and velocity of the end-effector), automatically handling the robot's dynamics and redundancy without separate inverse kinematics steps.

How do I handle singularities when implementing operational space control in DART?

Use a damped pseudo-inverse instead of a direct matrix inverse. Add a small regularization term (\varepsilon I) to the product (JJ^{\mathsf{T}}) before inversion, as shown in the example: J.transpose() * (J * J.transpose() + eps * I).inverse(). This prevents numerical instability when the Jacobian loses rank near singular configurations.

Can I use operational space control for orientation tracking as well as position?

Yes. Instead of getLinearJacobian(), use getAngularJacobian() or getJacobian() to obtain the full 6×n spatial Jacobian. Extend the error vector to include orientation error (using axis-angle or quaternion differences) and adjust the gain matrices (K_p) and (K_d) to 6×6 dimensions to control both position and orientation simultaneously.

What is the purpose of the customPreStep() method in DART controllers?

customPreStep() is a virtual method in dart::gui::WorldNode that executes immediately before the physics integration step. It provides the correct timing to read the current state, compute control torques, and apply them via setForces(), ensuring the controller sees the most recent state and the computed torques affect the next simulation step.

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