You have already seen the standard joint-space dynamics of an \( n \)-DOF rigid manipulator:

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \boldsymbol{\tau} , \]

where \( \mathbf{q} \in \mathbb{R}^n \) are joint coordinates, \( \mathbf{M}(\mathbf{q}) \) is the symmetric positive-definite inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \) collects Coriolis/centrifugal terms, and \( \mathbf{g}(\mathbf{q}) \) is gravity. It is often convenient to group nonlinear terms as

\[ \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) \triangleq \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}), \quad \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \boldsymbol{\tau}. \]

A task-space (operational-space) output is a differentiable function of the joints, for instance the end-effector position and orientation:

\[ \mathbf{x} = \mathbf{f}(\mathbf{q}) \in \mathbb{R}^m, \quad m \le n. \]

Using the manipulator Jacobian \( \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{f}}{\partial \mathbf{q}} \), we have the task-space velocity and acceleration relations:

\[ \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}}, \qquad \ddot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\ddot{\mathbf{q}} + \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}. \]

Operational-space control aims to directly shape \( \ddot{\mathbf{x}} \) (and thus the behavior of \( \mathbf{x} \)) by constructing a control law in task coordinates and mapping it back to joint torques \( \boldsymbol{\tau} \).

The link between joint torques and task-space wrenches is given by the principle of virtual work. For a task-space wrench \( \mathbf{F} \in \mathbb{R}^m \) applied at the end-effector, the joint torques are

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^{\top} \mathbf{F}. \]

Substitute this into the joint-space dynamics (ignore external joint torques other than those induced by \( \mathbf{F} \)):

\[ \mathbf{M}(\mathbf{q})\ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) = \mathbf{J}(\mathbf{q})^{\top}\mathbf{F}. \]

Solve for \( \ddot{\mathbf{q}} \):

\[ \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1}\mathbf{J}(\mathbf{q})^{\top}\mathbf{F} - \mathbf{M}(\mathbf{q})^{-1}\mathbf{h}(\mathbf{q},\dot{\mathbf{q}}). \]

Substitute into the task-space acceleration relation:

\[ \begin{aligned} \ddot{\mathbf{x}} &= \mathbf{J}\ddot{\mathbf{q}} + \dot{\mathbf{J}}\dot{\mathbf{q}} \\ &= \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top}\mathbf{F} - \mathbf{J}\mathbf{M}^{-1}\mathbf{h} + \dot{\mathbf{J}}\dot{\mathbf{q}}. \end{aligned} \]

Rearrange to isolate \( \mathbf{F} \):

\[ \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1}\mathbf{J}(\mathbf{q})^{\top}\mathbf{F} = \ddot{\mathbf{x}} + \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1}\mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) - \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}. \]

Assume \( \mathbf{J}(\mathbf{q}) \) has full row rank (\( m \) independent task coordinates). Then \( \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top} \) is symmetric positive definite and invertible. Define the operational-space inertia matrix:

\[ \boldsymbol{\Lambda}(\mathbf{q}) \triangleq \left( \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1}\mathbf{J}(\mathbf{q})^{\top} \right)^{-1}. \]

Then we obtain the compact operational-space dynamics:

\[ \begin{aligned} \mathbf{F} &= \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\Lambda}(\mathbf{q}) \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1} \mathbf{h}(\mathbf{q},\dot{\mathbf{q}}) - \boldsymbol{\Lambda}(\mathbf{q}) \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \\ &\triangleq \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}), \end{aligned} \]

where the task-space Coriolis/centrifugal term and gravity term are defined as

\[ \begin{aligned} \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) &\triangleq \boldsymbol{\Lambda}(\mathbf{q}) \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1} \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \boldsymbol{\Lambda}(\mathbf{q}) \dot{\mathbf{J}}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}}, \\ \mathbf{p}(\mathbf{q}) &\triangleq \boldsymbol{\Lambda}(\mathbf{q}) \mathbf{J}(\mathbf{q})\mathbf{M}(\mathbf{q})^{-1}\mathbf{g}(\mathbf{q}). \end{aligned} \]

In summary, the operational-space dynamics take the form

\[ \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}) = \mathbf{F}. \]

Positive definiteness of \( \boldsymbol{\Lambda}(\mathbf{q}) \). Since \( \mathbf{M}(\mathbf{q}) \) is positive definite and \( \mathbf{J}(\mathbf{q}) \) has full row rank, for any nonzero \( \mathbf{y} \in \mathbb{R}^m \) we have

\[ \mathbf{y}^{\top} \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top}\mathbf{y} = (\mathbf{J}^{\top}\mathbf{y})^{\top} \mathbf{M}^{-1} (\mathbf{J}^{\top}\mathbf{y}) > 0, \]

so \( \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top} \) is positive definite and its inverse \( \boldsymbol{\Lambda}(\mathbf{q}) \) is positive definite. This guarantees that each task coordinate behaves like a point mass with configuration-dependent inertia.

Suppose we are given a desired smooth task trajectory \( \mathbf{x}_d(t) \) with \( \dot{\mathbf{x}}_d(t), \ddot{\mathbf{x}}_d(t) \). Define the tracking error

\[ \mathbf{e} \triangleq \mathbf{x} - \mathbf{x}_d, \qquad \dot{\mathbf{e}} \triangleq \dot{\mathbf{x}} - \dot{\mathbf{x}}_d. \]

Let \( \mathbf{K}_P, \mathbf{K}_D \in \mathbb{R}^{m \times m} \) be symmetric positive-definite gain matrices. We choose a desired task-space acceleration shaping law

\[ \ddot{\mathbf{x}}_r \triangleq \ddot{\mathbf{x}}_d - \mathbf{K}_D \dot{\mathbf{e}} - \mathbf{K}_P \mathbf{e}. \]

The operational-space control law is then

\[ \begin{aligned} \mathbf{F} &= \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}}_r + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}), \\ \boldsymbol{\tau} &= \mathbf{J}(\mathbf{q})^{\top}\mathbf{F}. \end{aligned} \]

Closed-loop task-space dynamics. Assuming perfect modeling and neglecting unmodeled disturbances, the true task-space dynamics satisfy

\[ \boldsymbol{\Lambda}(\mathbf{q})\ddot{\mathbf{x}} + \boldsymbol{\mu}(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}(\mathbf{q}) = \mathbf{F}. \]

Substituting the control law:

\[ \begin{aligned} \boldsymbol{\Lambda}\ddot{\mathbf{x}} + \boldsymbol{\mu} + \mathbf{p} &= \boldsymbol{\Lambda}\ddot{\mathbf{x}}_r + \boldsymbol{\mu} + \mathbf{p} \\ \Rightarrow\quad \boldsymbol{\Lambda}(\ddot{\mathbf{x}} - \ddot{\mathbf{x}}_r) &= \mathbf{0}. \end{aligned} \]

Using the definition of \( \ddot{\mathbf{x}}_r \) and \( \mathbf{e} \),

\[ \begin{aligned} \ddot{\mathbf{x}} - \ddot{\mathbf{x}}_r &= \ddot{\mathbf{x}} - \ddot{\mathbf{x}}_d + \mathbf{K}_D \dot{\mathbf{e}} + \mathbf{K}_P \mathbf{e} \\ &= \ddot{\mathbf{e}} + \mathbf{K}_D \dot{\mathbf{e}} + \mathbf{K}_P \mathbf{e}. \end{aligned} \]

Hence

\[ \boldsymbol{\Lambda}(\mathbf{q}) \left( \ddot{\mathbf{e}} + \mathbf{K}_D \dot{\mathbf{e}} + \mathbf{K}_P \mathbf{e} \right) = \mathbf{0}. \]

Since \( \boldsymbol{\Lambda}(\mathbf{q}) \) is positive definite, the only solution is

\[ \ddot{\mathbf{e}} + \mathbf{K}_D \dot{\mathbf{e}} + \mathbf{K}_P \mathbf{e} = \mathbf{0}, \]

i.e., each component of the task error behaves as a second-order linear system with characteristic polynomial \( s^2 + K_{D,i} s + K_{P,i} \) (for diagonal gains), which is exponentially stable for positive gains. This is the key structural property: operational-space control shapes task-space behavior directly.

The controller can be seen as an outer loop in task coordinates and an inner map that converts the desired task-space wrench into joint torques.

flowchart TD
  Xd["Desired task trajectory: x_d, x_d_dot, x_d_ddot"] --> ERR["Compute task error: e, e_dot"]
  ERR --> XREF["Compute desired task acceleration x_ddot_ref"]
  XREF --> DYN["Evaluate robot model: M(q), h(q,q_dot), J(q), J_dot(q,q_dot)"]
  DYN --> OPS["Compute Lambda(q), mu(q,q_dot), p(q)"]
  OPS --> FCTRL["Compute task wrench F"]
  FCTRL --> TAU["Map to joint torques: tau = J(q)^T F"]
  TAU --> ACT["Send tau to low-level torque controllers"]
        

In redundant manipulators, one usually adds a null-space torque that does not affect the task; this will be treated systematically in the next lesson on redundancy handling.

At each control cycle, the operational-space controller performs:

1. Read joint states \( \mathbf{q}, \dot{\mathbf{q}} \).
2. Compute task state \( \mathbf{x} = \mathbf{f}(\mathbf{q}) \) and \( \dot{\mathbf{x}} = \mathbf{J}(\mathbf{q})\dot{\mathbf{q}} \).
3. Compute tracking error \( \mathbf{e}, \dot{\mathbf{e}} \).
4. Compute desired task acceleration \( \ddot{\mathbf{x}}_r = \ddot{\mathbf{x}}_d - \mathbf{K}_D \dot{\mathbf{e}} - \mathbf{K}_P \mathbf{e} \).
5. Evaluate joint-space dynamics \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) and Jacobian \( \mathbf{J}, \dot{\mathbf{J}} \).
6. Compute \( \boldsymbol{\Lambda}, \boldsymbol{\mu}, \mathbf{p} \) and then task wrench \( \mathbf{F} = \boldsymbol{\Lambda}\ddot{\mathbf{x}}_r + \boldsymbol{\mu} + \mathbf{p} \).
7. Compute joint torques \( \boldsymbol{\tau} = \mathbf{J}^{\top}\mathbf{F} \) and send them to the low-level actuator controller.

Numerically, particular care is needed when \( \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top} \) is close to singular (e.g., near kinematic singularities), where one may use damping or task re-definition.

In Python, we can implement operational-space control using numpy for linear algebra and connect to a dynamics library such as pinocchio or roboticstoolbox for \( \mathbf{M}, \mathbf{h}, \mathbf{J} \).

import numpy as np

def op_space_control_step(q, qd, x_d, xd_d, xdd_d,
                          Kp, Kd,
                          M, h, J, Jdot):
    """
    One step of operational-space control.

    Parameters
    ----------
    q, qd : (n,) arrays
        Joint positions and velocities.
    x_d, xd_d, xdd_d : (m,) arrays
        Desired task position, velocity, acceleration.
    Kp, Kd : (m, m) arrays
        Task-space gain matrices (typically diagonal and positive).
    M : (n, n) array
        Joint-space inertia at q.
    h : (n,) array
        Joint-space nonlinear term C(q,qd)@qd + g(q).
    J : (m, n) array
        Task Jacobian at q.
    Jdot : (m, n) array
        Time derivative of J(q) evaluated at (q, qd).

    Returns
    -------
    tau : (n,) array
        Joint torques for this step.
    """
    # Task-space state
    # Here x, xd should be computed from the robot model; we pass them in or compute outside.
    # Assume we are given current x and xd:
    # x = f(q); xd = J @ qd
    # For modularity, let caller provide x, xd if needed.
    # For this core routine, we derive them from J:
    # WARNING: this assumes x and xd are known; here we only need xd for error derivative.

    # Caller should compute x and xd; for demo:
    # x = x_current
    # xd = J @ qd
    # For this snippet, assume x, xd known outside and precomputed.
    raise_not_implemented = False
    if raise_not_implemented:
        raise NotImplementedError("Provide current x and xd from the kinematics model.")

    # Example: assume x and xd are globals or closure variables (pseudo-code)
    # x, xd = current_task_state(q, qd)

    # Placeholders to indicate interface:
    x = np.zeros_like(x_d)
    xd = np.zeros_like(x_d)

    # Task-space error
    e = x - x_d
    ed = xd - xd_d

    # Desired task acceleration
    xdd_ref = xdd_d - Kd @ ed - Kp @ e

    # Lambda = (J M^-1 J^T)^-1
    Minv = np.linalg.inv(M)
    JMJT = J @ Minv @ J.T
    Lambda = np.linalg.inv(JMJT)

    # Task-space mu and p
    # Split h into Coriolis-like and gravity parts if available.
    # For the structure here we assume h = C(q,qd)@qd + g(q) and the library provides them separately.
    # Here, just demonstrate the structure:
    mu = Lambda @ (J @ Minv @ h) - Lambda @ (Jdot @ qd)

    # If you have g(q) explicitly, replace the next two lines by:
    # h_c = h_coriolis(q, qd)
    # g_vec = g(q)
    # mu = Lambda @ (J @ Minv @ h_c) - Lambda @ (Jdot @ qd)
    # p  = Lambda @ (J @ Minv @ g_vec)

    p = np.zeros_like(x_d)  # placeholder if gravity is already compensated in h

    # Task-space wrench
    F = Lambda @ xdd_ref + mu + p

    # Joint torques
    tau = J.T @ F
    return tau
      

In a real implementation, the quantities M, h, J, Jdot are obtained from a rigid-body dynamics library. For example, with pinocchio you would use pinocchio.crba for M, pinocchio.rnea for h, and pinocchio.computeJointJacobians / pinocchio.getFrameJacobian for the Jacobian.

In C++, we typically combine Eigen for linear algebra with a dynamics engine such as RBDL or Pinocchio. The snippet below focuses on the algebraic part, assuming calls to the library provide \( \mathbf{M}, \mathbf{h}, \mathbf{J}, \dot{\mathbf{J}} \).

#include <Eigen/Dense>

using Eigen::MatrixXd;
using Eigen::VectorXd;

struct OpSpaceGains {
    MatrixXd Kp;
    MatrixXd Kd;
};

VectorXd opSpaceControlStep(
    const VectorXd& q,
    const VectorXd& qd,
    const VectorXd& x,
    const VectorXd& xd,
    const VectorXd& x_d,
    const VectorXd& xd_d,
    const VectorXd& xdd_d,
    const OpSpaceGains& gains,
    const MatrixXd& M,
    const VectorXd& h,
    const MatrixXd& J,
    const MatrixXd& Jdot)
{
    const int m = x.size();

    // Errors
    VectorXd e  = x  - x_d;
    VectorXd ed = xd - xd_d;

    // Desired task acceleration
    VectorXd xdd_ref = xdd_d
                       - gains.Kd * ed
                       - gains.Kp * e;

    // Lambda = (J M^-1 J^T)^-1
    MatrixXd Minv = M.inverse();
    MatrixXd JMJT = J * Minv * J.transpose();
    MatrixXd Lambda = JMJT.inverse();

    // Task-space mu and p (gravity could be split if available)
    VectorXd mu = Lambda * (J * Minv * h) - Lambda * (Jdot * qd);
    VectorXd p  = VectorXd::Zero(m); // if gravity already in h

    VectorXd F = Lambda * xdd_ref + mu + p;

    // tau = J^T F
    VectorXd tau = J.transpose() * F;
    return tau;
}
      

With RBDL, for instance, you would call CompositeRigidBodyAlgorithm for M, InverseDynamics for h, and CalcPointJacobian6D or frame Jacobian routines for J.

Java does not have a canonical robotics library, but EJML provides efficient matrices. Below is a sketch of the controller core using EJML's SimpleMatrix.

import org.ejml.simple.SimpleMatrix;

public class OpSpaceController {

    public static SimpleMatrix step(
            SimpleMatrix q,
            SimpleMatrix qd,
            SimpleMatrix x,
            SimpleMatrix xd,
            SimpleMatrix x_d,
            SimpleMatrix xd_d,
            SimpleMatrix xdd_d,
            SimpleMatrix Kp,
            SimpleMatrix Kd,
            SimpleMatrix M,
            SimpleMatrix h,
            SimpleMatrix J,
            SimpleMatrix Jdot) {

        // Errors
        SimpleMatrix e  = x.minus(x_d);
        SimpleMatrix ed = xd.minus(xd_d);

        // Desired task acceleration
        SimpleMatrix xdd_ref = xdd_d
                .minus(Kd.mult(ed))
                .minus(Kp.mult(e));

        // Lambda = (J M^-1 J^T)^-1
        SimpleMatrix Minv = M.invert();
        SimpleMatrix JMJT = J.mult(Minv).mult(J.transpose());
        SimpleMatrix Lambda = JMJT.invert();

        // Task-space mu and p
        SimpleMatrix mu = Lambda.mult(J.mult(Minv).mult(h))
                .minus(Lambda.mult(Jdot.mult(qd)));

        SimpleMatrix p = new SimpleMatrix(mu.numRows(), 1);
        p.zero(); // assume gravity is handled in h

        SimpleMatrix F = Lambda.mult(xdd_ref).plus(mu).plus(p);

        // Joint torques tau = J^T F
        SimpleMatrix tau = J.transpose().mult(F);
        return tau;
    }
}
      

This structure can be integrated in a Java-based robotic framework or middleware (for example, via ROS nodes written in Java).

MATLAB, together with the Robotics System Toolbox, provides ready-made functions for dynamics and Jacobians. A simple function that computes operational-space torques is:

function tau = opSpaceControlStep(q, qd, x, xd, x_d, xd_d, xdd_d, Kp, Kd, robot)
% OPSPACECONTROLSTEP  One control step of operational-space control.
%
% Inputs:
%   q, qd   : joint position and velocity (n x 1)
%   x, xd   : current task position and velocity (m x 1)
%   x_d, xd_d, xdd_d : desired task trajectory signals (m x 1)
%   Kp, Kd  : task-space gains (m x m)
%   robot   : rigidBodyTree model (Robotics System Toolbox)
%
% Output:
%   tau     : joint torques (n x 1)

    % Errors
    e  = x  - x_d;
    ed = xd - xd_d;

    % Desired task acceleration
    xdd_ref = xdd_d - Kd * ed - Kp * e;

    % Joint-space inertia and nonlinear terms
    M = massMatrix(robot, q');
    % generalized forces from inverseDynamics with zero acceleration
    h = inverseDynamics(robot, q', qd', zeros(size(q')));

    % End-effector Jacobian and its derivative (approx via finite difference
    % or robot-specific routine)
    eeName = robot.BodyNames{end};
    J = geometricJacobian(robot, q', eeName); % 6 x n for full spatial
    % For a pure position task, select top 3 rows:
    Jpos = J(1:3, :);

    % Approximate Jdot * qd via numerical differentiation or compute analytically
    Jdot_qd = zeros(size(Jpos, 1), 1); % placeholder

    Minv = inv(M);
    JMJT = Jpos * Minv * Jpos';
    Lambda = inv(JMJT);

    mu = Lambda * (Jpos * Minv * h) - Lambda * Jdot_qd;
    p  = zeros(size(mu)); % if gravity already in h

    F = Lambda * xdd_ref + mu + p;

    tau = Jpos' * F;
end
      

In Simulink, a common pattern is to wrap this function in a MATLAB Function block inside a control subsystem. The block receives q, qd, x, xd, x_d, xd_d, xdd_d from upstream blocks and outputs tau to the torque input of the plant model.

Wolfram Mathematica is well suited for symbolic derivation of \( \boldsymbol{\Lambda}, \boldsymbol{\mu}, \mathbf{p} \) and numerical evaluation. A simplified sketch is:

(* Define joint and task variables *)
n = 2; (* example: planar 2R *)
q  = Array[q, n];
qd = Array[qd, n];

(* Define kinematics x = f(q): for instance, planar arm *)
l1 = 1; l2 = 1;
xPos[q1_, q2_] := l1 Cos[q1] + l2 Cos[q1 + q2];
yPos[q1_, q2_] := l1 Sin[q1] + l2 Sin[q1 + q2];
xVec = {xPos[q[[1]], q[[2]]], yPos[q[[1]], q[[2]]]};

(* Jacobian *)
J = D[xVec, {q}];

(* Assume symbolic inertia M(q) and nonlinear term h(q,qd) *)
M = { {m11[q[[1]], q[[2]]], m12[q[[1]], q[[2]]]},
     {m12[q[[1]], q[[2]]], m22[q[[1]], q[[2]]]} }
    ;
h = {h1[q[[1]], q[[2]], qd[[1]], qd[[2]]],
     h2[q[[1]], q[[2]], qd[[1]], qd[[2]]]}
    ;

(* Operational-space inertia *)
Lambda = Inverse[J . Inverse[M] . Transpose[J]] // Simplify;

(* Time derivative of J times qd *)
Jdot = D[J, {q}].qd; (* 3-tensor contracted with qd *)

(* Task-space nonlinear terms (symbolic forms) *)
mu = Lambda . (J . Inverse[M] . h) - Lambda . Jdot . qd;
(* For gravity p, separate g(q) from h if available *)

(* Control law (symbolic) *)
Kp = { {kp1, 0}, {0, kp2} };
Kd = { {kd1, 0}, {0, kd2} };

x    = xVec;
xd   = J . qd;
xDes = {x1d[t], x2d[t]};  (* user-defined trajectories *)
xdDes  = D[xDes, t];
xddDes = D[xDes, {t, 2}];

e  = x - xDes;
ed = xd - xdDes;

xddRef = xddDes - Kd . ed - Kp . e;

F = Lambda . xddRef + mu; (* ignoring p for brevity *)

(* Joint torques *)
tau = Transpose[J] . F // Simplify;
      

This environment is particularly helpful for verifying the analytical form of \( \boldsymbol{\Lambda}(\mathbf{q}) \) and exploring special configurations symbolically before implementing numerical code.

In this lesson we constructed the operational-space representation of robot dynamics, starting from the joint-space equation of motion. The key object is the task-space inertia \( \boldsymbol{\Lambda}(\mathbf{q}) \), defined as the inverse of \( \mathbf{J}\mathbf{M}^{-1}\mathbf{J}^{\top} \), which is guaranteed to be positive definite when the Jacobian has full row rank.

Using this representation, we built a model-based controller that works directly in task space: a PD-like law in \( \mathbf{x} \) combined with inverse dynamics compensation in operational space. We showed that, under perfect modeling, the task-space tracking error obeys a decoupled linear second-order ODE with tunable gains. Finally, we mapped the structure to Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica implementations, preparing the ground for redundancy handling and task hierarchies in subsequent lessons.