# How to Implement Operational Space Control in DART: A Complete C++ Guide > Implement operational space control in DART using C++ Calculate joint torques from Cartesian errors by leveraging the mass matrix Jacobian and damped pseudo-inverse. Apply torques via customPreStep. - Repository: [DART: Dynamic Animation and Robotics Toolkit/dart](https://github.com/dartsim/dart) - Tags: how-to-guide - Published: 2026-02-28 --- **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`](https://github.com/dartsim/dart/blob/main/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) ```cpp 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\)). ```cpp 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`](https://github.com/dartsim/dart/blob/main/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 \] ```cpp // 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. ```cpp #include #include 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::Frame::World(), "target", tf); mTarget->setShape(std::make_shared(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`](https://github.com/dartsim/dart/blob/main/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`](https://github.com/dartsim/dart/blob/main/examples/operational_space_control/main.cpp) – Full interactive demo with GUI - [`docs/readthedocs/topics/control-theory.md`](https://github.com/dartsim/dart/blob/main/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.