We consider an n-DOF rigid manipulator with joint coordinates \( \mathbf{q}(t)\in\mathbb{R}^n \) and standard dynamics (from robotics):

\[ \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{M}(\mathbf{q}) \) is the positive-definite inertia matrix, \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \) represents Coriolis/centrifugal terms, \( \mathbf{g}(\mathbf{q}) \) the gravity vector, and \( \boldsymbol{\tau} \) the vector of joint torques.

We choose a smooth, bounded reference trajectory \( \mathbf{q}_d(t) \) with derivatives \( \dot{\mathbf{q}}_d(t), \ddot{\mathbf{q}}_d(t) \). Define the tracking error:

\[ \mathbf{e}(t) = \mathbf{q}_d(t) - \mathbf{q}(t), \quad \dot{\mathbf{e}}(t) = \dot{\mathbf{q}}_d(t) - \dot{\mathbf{q}}(t). \]

Goal of the lab: use the same reference trajectory, robot model, and numerical integrator to compare:

• Pure joint-space PD control (no explicit model inversion).
• Computed-torque (inverse-dynamics) control with the same \( \mathbf{K}_p,\mathbf{K}_d \).

We will compare the controllers using quantitative metrics:

\[ J_e = \int_0^{T_f} \mathbf{e}(t)^\top \mathbf{Q}\,\mathbf{e}(t)\,dt, \quad J_{\max} = \max_{0 \le t \le T_f} \|\mathbf{e}(t)\|, \]

\[ J_{\tau} = \int_0^{T_f} \boldsymbol{\tau}(t)^\top \boldsymbol{\tau}(t)\,dt, \]

with \( \mathbf{Q} \succ 0 \) (often \( \mathbf{Q} = \mathbf{I} \)). These capture integrated error, peak error, and torque effort.

flowchart TD
  A["Specify robot (M(q), C(q,qdot), g(q))"] --> B["Define reference trajectory q_d(t)"]
  B --> C["Choose gains K_p, K_d"]
  C --> D["Implement PD controller"]
  C --> E["Implement computed torque controller"]
  D --> F["Simulate dynamics with PD"]
  E --> G["Simulate dynamics with CT"]
  F --> H["Compute metrics J_e, J_max, J_tau (PD)"]
  G --> I["Compute metrics J_e, J_max, J_tau (CT)"]
  H --> J["Compare time histories and metrics"]
  I --> J
        

The joint-space PD controller (ignoring explicit model terms) is:

\[ \boldsymbol{\tau}_{PD}(t) = \mathbf{K}_p \mathbf{e}(t) + \mathbf{K}_d \dot{\mathbf{e}}(t), \]

where \( \mathbf{K}_p, \mathbf{K}_d \in \mathbb{R}^{n\times n} \) are symmetric positive-definite gain matrices. Substituting \( \boldsymbol{\tau}_{PD} \) into the dynamics:

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{K}_p (\mathbf{q}_d - \mathbf{q}) + \mathbf{K}_d (\dot{\mathbf{q}}_d - \dot{\mathbf{q}}). \]

Using \( \mathbf{e} = \mathbf{q}_d - \mathbf{q} \) and \( \ddot{\mathbf{e}} = \ddot{\mathbf{q}}_d - \ddot{\mathbf{q}} \), we obtain:

\[ \mathbf{M}(\mathbf{q}) (\ddot{\mathbf{q}}_d - \ddot{\mathbf{e}}) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{K}_p \mathbf{e} + \mathbf{K}_d \dot{\mathbf{e}}. \]

Rearranging for \( \ddot{\mathbf{e}} \):

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{e}} = \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}}_d + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) - \mathbf{K}_p \mathbf{e} - \mathbf{K}_d \dot{\mathbf{e}}. \]

This does not simplify to a purely linear, decoupled error system because \( \mathbf{M}, \mathbf{C}, \mathbf{g} \) appear explicitly and couple joints. For small velocities/accelerations and sufficiently large gains, the PD controller still often yields acceptable tracking, but performance is sensitive to gravity and dynamic coupling.

The computed-torque controller uses the robot model to create a virtual linear system in the error coordinates. We choose a virtual acceleration input:

\[ \mathbf{v}(t) = \ddot{\mathbf{q}}_d(t) + \mathbf{K}_d \dot{\mathbf{e}}(t) + \mathbf{K}_p \mathbf{e}(t), \]

and define the control law:

\[ \boldsymbol{\tau}_{CT}(t) = \mathbf{M}(\mathbf{q})\,\mathbf{v}(t) + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}). \]

Substituting into the dynamics yields

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

so that (assuming perfect model)

\[ \ddot{\mathbf{q}} = \mathbf{v} = \ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e}. \]

Using \( \ddot{\mathbf{e}} = \ddot{\mathbf{q}}_d - \ddot{\mathbf{q}} \), we obtain:

\[ \ddot{\mathbf{e}} = \ddot{\mathbf{q}}_d - \ddot{\mathbf{q}} = \ddot{\mathbf{q}}_d - \bigl(\ddot{\mathbf{q}}_d + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e}\bigr) = - \mathbf{K}_d \dot{\mathbf{e}} - \mathbf{K}_p \mathbf{e}. \]

Therefore the closed-loop error dynamics are

\[ \ddot{\mathbf{e}} + \mathbf{K}_d \dot{\mathbf{e}} + \mathbf{K}_p \mathbf{e} = \mathbf{0}, \]

which is a decoupled linear second-order system (if \( \mathbf{K}_p,\mathbf{K}_d \) are diagonal) with eigenstructure chosen by the gains. Under the usual robot property that \( \mathbf{M}(\mathbf{q}) \) is uniformly positive-definite and \( \mathbf{C} \) satisfies the skew-symmetry condition, a Lyapunov function

\[ V(\mathbf{e},\dot{\mathbf{e}}) = \tfrac{1}{2} \dot{\mathbf{e}}^\top \mathbf{M}(\mathbf{q}) \dot{\mathbf{e}} + \tfrac{1}{2} \mathbf{e}^\top \mathbf{K}_p \mathbf{e} \]

yields

\[ \dot{V} = - \dot{\mathbf{e}}^\top \mathbf{K}_d \dot{\mathbf{e}} \le 0, \]

showing global asymptotic tracking (under standard assumptions). This is why, in the lab, we expect the computed-torque controller to achieve substantially smaller tracking errors than PD for the same gains.

We consider a 2-DOF planar manipulator (shoulder and elbow) in the horizontal plane, so that gravity terms can be included or excluded to test the effect of gravity compensation. The simulation loop is conceptually identical across programming languages:

1. Initialize joint state \( \mathbf{x}(0) = [\mathbf{q}(0)^\top,\dot{\mathbf{q}}(0)^\top]^\top \).
2. At each time step \( t_k \):
   • Evaluate reference \( \mathbf{q}_d(t_k), \dot{\mathbf{q}}_d(t_k), \ddot{\mathbf{q}}_d(t_k) \).
   • Compute error \( \mathbf{e}, \dot{\mathbf{e}} \).
   • Compute \( \boldsymbol{\tau} \) via PD or computed torque.
   • Compute \( \ddot{\mathbf{q}} \) from the dynamics.
   • Integrate to obtain \( \dot{\mathbf{q}}(t_{k+1}), \mathbf{q}(t_{k+1}) \).
3. Record trajectories and evaluate \( J_e, J_{\max}, J_{\tau} \).

flowchart TD
  S["Initialize q, qdot, t = 0"] --> L["At each step: compute q_d, qd_d, qdd_d"]
  L --> E["Compute e = q_d - q, edot = qd_d - qdot"]
  E --> P["Controller: PD or CT -> tau"]
  P --> DYN["Forward dynamics: qdd = M(q)^(-1)*(tau - C(q,qdot)*qdot - g(q))"]
  DYN --> INT["Integrate qdot, q"]
  INT --> MET["Accumulate error & torque metrics"]
  MET --> TST["Check t < T_f ?"]
  TST -->|yes| L
  TST -->|no| END["Plot and compare results"]
        

In Python we can use numpy for linear algebra and optionally robot libraries (e.g., Python Robotics Toolbox) for dynamics. Here we implement a simple 2-DOF arm dynamics directly to emphasize the structure.

import numpy as np

# 2-DOF planar arm parameters (simple model)
m1, m2 = 1.0, 1.0      # link masses
l1, l2 = 1.0, 1.0      # link lengths
lc1, lc2 = 0.5, 0.5    # COM distances
I1, I2 = 0.1, 0.1      # link inertias about COM
g = 9.81

def M_matrix(q):
    q1, q2 = q
    c2 = np.cos(q2)
    m11 = I1 + I2 + m1 * lc1**2 + m2 * (l1**2 + lc2**2 + 2*l1*lc2*c2)
    m12 = I2 + m2 * (lc2**2 + l1*lc2*c2)
    m22 = I2 + m2 * lc2**2
    M = np.array([[m11, m12],
                  [m12, m22]])
    return M

def C_matrix(q, qdot):
    q1, q2 = q
    q1dot, q2dot = qdot
    s2 = np.sin(q2)
    h = -m2 * l1 * lc2 * s2
    C = np.array([[h*q2dot, h*(q1dot + q2dot)],
                  [-h*q1dot, 0.0]])
    return C

def g_vector(q):
    q1, q2 = q
    g1 = (m1*lc1 + m2*l1) * g * np.cos(q1) + m2*lc2*g*np.cos(q1 + q2)
    g2 = m2*lc2*g*np.cos(q1 + q2)
    return np.array([g1, g2])

# Reference trajectory: simple joint sine motion
def qd(t):
    return np.array([0.5*np.sin(0.5*t), 0.5*np.cos(0.5*t)])

def qd_dot(t):
    return np.array([0.5*0.5*np.cos(0.5*t), -0.5*0.5*np.sin(0.5*t)])

def qd_ddot(t):
    return np.array([-0.5*0.5*0.5*np.sin(0.5*t), -0.5*0.5*0.5*np.cos(0.5*t)])

Kp = np.diag([100.0, 80.0])
Kd = np.diag([20.0, 16.0])

def controller_PD(q, qdot, t):
    e = qd(t) - q
    edot = qd_dot(t) - qdot
    tau = Kp.dot(e) + Kd.dot(edot)
    return tau

def controller_CT(q, qdot, t):
    e = qd(t) - q
    edot = qd_dot(t) - qdot
    v = qd_ddot(t) + Kd.dot(edot) + Kp.dot(e)
    M = M_matrix(q)
    C = C_matrix(q, qdot)
    g_vec = g_vector(q)
    tau = M.dot(v) + C.dot(qdot) + g_vec
    return tau

def simulate(controller, T=10.0, dt=0.001):
    steps = int(T / dt)
    q = np.array([0.0, 0.0])
    qdot = np.array([0.0, 0.0])

    Je = 0.0
    Jtau = 0.0
    e_max = 0.0

    qs = []
    qds = []
    ts = []

    for k in range(steps):
        t = k*dt
        e = qd(t) - q
        edot = qd_dot(t) - qdot
        tau = controller(q, qdot, t)

        M = M_matrix(q)
        C = C_matrix(q, qdot)
        g_vec = g_vector(q)

        qddot = np.linalg.solve(M, tau - C.dot(qdot) - g_vec)

        # simple explicit Euler (for lab usage; higher-order integrators are better)
        qdot = qdot + dt*qddot
        q = q + dt*qdot

        # metrics
        Je += e.T.dot(e) * dt
        Jtau += tau.T.dot(tau) * dt
        e_norm = np.linalg.norm(e)
        if e_norm > e_max:
            e_max = e_norm

        qs.append(q.copy())
        qds.append(qd(t))
        ts.append(t)

    return {
        "t": np.array(ts),
        "q": np.array(qs),
        "qd": np.array(qds),
        "Je": Je,
        "Jtau": Jtau,
        "e_max": e_max
    }

if __name__ == "__main__":
    res_PD = simulate(controller_PD)
    res_CT = simulate(controller_CT)

    print("PD:   J_e = {:.3f}, J_tau = {:.3f}, e_max = {:.3f}".format(
        res_PD["Je"], res_PD["Jtau"], res_PD["e_max"]))
    print("CT:   J_e = {:.3f}, J_tau = {:.3f}, e_max = {:.3f}".format(
        res_CT["Je"], res_CT["Jtau"], res_CT["e_max"]))

    # Example: plot first joint tracking (requires matplotlib)
    import matplotlib.pyplot as plt
    plt.figure()
    plt.plot(res_PD["t"], res_PD["qd"][:,0], label="q1_d")
    plt.plot(res_PD["t"], res_PD["q"][:,0], label="q1_PD")
    plt.plot(res_CT["t"], res_CT["q"][:,0], label="q1_CT")
    plt.xlabel("t [s]")
    plt.ylabel("joint 1 [rad]")
    plt.legend()
    plt.show()
      

Running this script gives quantitative metrics for PD and computed torque. You should observe significantly smaller \( J_e \) and \( e_{\max} \) for the computed-torque controller when the model is accurate and gains are the same.

In C++, we can use Eigen for matrix operations and either a robotics library (e.g., RBDL, Pinocchio) or hand-coded dynamics for simple arms. Below is a minimal sketch focusing on the control structure (2-DOF planar arm, as above).

#include <iostream>
#include <Eigen/Dense>

using Eigen::Vector2d;
using Eigen::Matrix2d;

const double m1 = 1.0, m2 = 1.0;
const double l1 = 1.0, l2 = 1.0;
const double lc1 = 0.5, lc2 = 0.5;
const double I1 = 0.1, I2 = 0.1;
const double g = 9.81;

Matrix2d M_matrix(const Vector2d& q) {
    double q1 = q(0), q2 = q(1);
    double c2 = std::cos(q2);
    double m11 = I1 + I2 + m1*lc1*lc1
                 + m2*(l1*l1 + lc2*lc2 + 2*l1*lc2*c2);
    double m12 = I2 + m2*(lc2*lc2 + l1*lc2*c2);
    double m22 = I2 + m2*lc2*lc2;
    Matrix2d M;
    M << m11, m12,
          m12, m22;
    return M;
}

Matrix2d C_matrix(const Vector2d& q, const Vector2d& qdot) {
    double q2 = q(1);
    double q1dot = qdot(0), q2dot = qdot(1);
    double s2 = std::sin(q2);
    double h = -m2*l1*lc2*s2;
    Matrix2d C;
    C << h*q2dot, h*(q1dot + q2dot),
          -h*q1dot, 0.0;
    return C;
}

Vector2d g_vector(const Vector2d& q) {
    double q1 = q(0), q2 = q(1);
    double g1 = (m1*lc1 + m2*l1)*g*std::cos(q1)
                + m2*lc2*g*std::cos(q1 + q2);
    double g2 = m2*lc2*g*std::cos(q1 + q2);
    return Vector2d(g1, g2);
}

Vector2d qd(double t) {
    return Vector2d(0.5*std::sin(0.5*t),
                    0.5*std::cos(0.5*t));
}

Vector2d qd_dot(double t) {
    return Vector2d(0.25*std::cos(0.5*t),
                    -0.25*std::sin(0.5*t));
}

Vector2d qd_ddot(double t) {
    return Vector2d(-0.125*std::sin(0.5*t),
                    -0.125*std::cos(0.5*t));
}

Matrix2d Kp = (Matrix2d() << 100.0, 0.0, 0.0, 80.0).finished();
Matrix2d Kd = (Matrix2d() << 20.0, 0.0, 0.0, 16.0).finished();

Vector2d controller_PD(const Vector2d& q,
                       const Vector2d& qdot,
                       double t) {
    Vector2d e = qd(t) - q;
    Vector2d edot = qd_dot(t) - qdot;
    return Kp*e + Kd*edot;
}

Vector2d controller_CT(const Vector2d& q,
                       const Vector2d& qdot,
                       double t) {
    Vector2d e = qd(t) - q;
    Vector2d edot = qd_dot(t) - qdot;
    Vector2d v = qd_ddot(t) + Kd*edot + Kp*e;
    Matrix2d M = M_matrix(q);
    Matrix2d C = C_matrix(q, qdot);
    Vector2d g_vec = g_vector(q);
    return M*v + C*qdot + g_vec;
}

struct Metrics {
    double Je;
    double Jtau;
    double e_max;
};

Metrics simulate(Vector2d (*controller)(const Vector2d&, const Vector2d&, double),
                 double T, double dt) {
    int steps = static_cast<int>(T / dt);
    Vector2d q(0.0, 0.0);
    Vector2d qdot(0.0, 0.0);

    double Je = 0.0, Jtau = 0.0, e_max = 0.0;

    for (int k = 0; k < steps; ++k) {
        double t = k * dt;
        Vector2d e = qd(t) - q;
        Vector2d edot = qd_dot(t) - qdot;
        Vector2d tau = controller(q, qdot, t);

        Matrix2d M = M_matrix(q);
        Matrix2d C = C_matrix(q, qdot);
        Vector2d g_vec = g_vector(q);

        Vector2d qddot = M.ldlt().solve(tau - C*qdot - g_vec);

        qdot += dt * qddot;
        q += dt * qdot;

        Je += e.dot(e) * dt;
        Jtau += tau.dot(tau) * dt;
        double e_norm = e.norm();
        if (e_norm > e_max) e_max = e_norm;
    }
    return {Je, Jtau, e_max};
}

int main() {
    double T = 10.0, dt = 0.001;
    Metrics mPD = simulate(controller_PD, T, dt);
    Metrics mCT = simulate(controller_CT, T, dt);

    std::cout << "PD: J_e=" << mPD.Je
              << " J_tau=" << mPD.Jtau
              << " e_max=" << mPD.e_max << std::endl;

    std::cout << "CT: J_e=" << mCT.Je
              << " J_tau=" << mCT.Jtau
              << " e_max=" << mCT.e_max << std::endl;
    return 0;
}
      

This code can be compiled with, e.g., g++ and linked with Eigen headers. For more complex robots, replace M_matrix, C_matrix, and g_vector with calls to a dynamics library.

In Java, we can use small helper methods and 2D arrays for matrices. Robotics-specific Java libraries are less common, so hand-coding simple dynamics is typical in teaching environments.

public class TwoDofLab {

    static final double m1 = 1.0, m2 = 1.0;
    static final double l1 = 1.0, l2 = 1.0;
    static final double lc1 = 0.5, lc2 = 0.5;
    static final double I1 = 0.1, I2 = 0.1;
    static final double g = 9.81;

    static double[][] M(double[] q) {
        double q1 = q[0], q2 = q[1];
        double c2 = Math.cos(q2);
        double m11 = I1 + I2 + m1*lc1*lc1
                     + m2*(l1*l1 + lc2*lc2 + 2*l1*lc2*c2);
        double m12 = I2 + m2*(lc2*lc2 + l1*lc2*c2);
        double m22 = I2 + m2*lc2*lc2;
        return new double[][] { {m11, m12},
                               {m12, m22} };
    }

    static double[][] C(double[] q, double[] qdot) {
        double q2 = q[1];
        double q1dot = qdot[0], q2dot = qdot[1];
        double s2 = Math.sin(q2);
        double h = -m2*l1*lc2*s2;
        return new double[][] {
            {h*q2dot, h*(q1dot + q2dot)},
            {-h*q1dot, 0.0}
        };
    }

    static double[] gvec(double[] q) {
        double q1 = q[0], q2 = q[1];
        double g1 = (m1*lc1 + m2*l1)*g*Math.cos(q1)
                    + m2*lc2*g*Math.cos(q1 + q2);
        double g2 = m2*lc2*g*Math.cos(q1 + q2);
        return new double[] {g1, g2};
    }

    static double[] qd(double t) {
        return new double[] {0.5*Math.sin(0.5*t),
                             0.5*Math.cos(0.5*t)};
    }

    static double[] qdDot(double t) {
        return new double[] {0.25*Math.cos(0.5*t),
                             -0.25*Math.sin(0.5*t)};
    }

    static double[] qdDdot(double t) {
        return new double[] {-0.125*Math.sin(0.5*t),
                             -0.125*Math.cos(0.5*t)};
    }

    static double[][] Kp = {
        {100.0, 0.0},
        {0.0, 80.0}
    };
    static double[][] Kd = {
        {20.0, 0.0},
        {0.0, 16.0}
    };

    static double[] matVec(double[][] A, double[] x) {
        return new double[] {
            A[0][0]*x[0] + A[0][1]*x[1],
            A[1][0]*x[0] + A[1][1]*x[1]
        };
    }

    static double[] vecAdd(double[] a, double[] b) {
        return new double[] {a[0] + b[0], a[1] + b[1]};
    }

    static double[] vecSub(double[] a, double[] b) {
        return new double[] {a[0] - b[0], a[1] - b[1]};
    }

    static double dot(double[] a, double[] b) {
        return a[0]*b[0] + a[1]*b[1];
    }

    // 2x2 linear solve using explicit formula
    static double[] solve2x2(double[][] A, double[] b) {
        double a11 = A[0][0], a12 = A[0][1];
        double a21 = A[1][0], a22 = A[1][1];
        double det = a11*a22 - a12*a21;
        double inv11 = a22 / det;
        double inv12 = -a12 / det;
        double inv21 = -a21 / det;
        double inv22 = a11 / det;
        return new double[] {
            inv11*b[0] + inv12*b[1],
            inv21*b[0] + inv22*b[1]
        };
    }

    static double[] controllerPD(double[] q, double[] qdot, double t) {
        double[] e = vecSub(qd(t), q);
        double[] edot = vecSub(qdDot(t), qdot);
        double[] term1 = matVec(Kp, e);
        double[] term2 = matVec(Kd, edot);
        return vecAdd(term1, term2);
    }

    static double[] controllerCT(double[] q, double[] qdot, double t) {
        double[] e = vecSub(qd(t), q);
        double[] edot = vecSub(qdDot(t), qdot);
        double[] v = vecAdd(qdDdot(t),
                            vecAdd(matVec(Kd, edot), matVec(Kp, e)));
        double[][] M = M(q);
        double[][] Cmat = C(q, qdot);
        double[] g_vec = gvec(q);
        double[] Cv = matVec(Cmat, qdot);
        double[] tmp = vecAdd(matVec(M, v), vecAdd(Cv, g_vec));
        return tmp;
    }

    static class Metrics {
        double Je, Jtau, emax;
    }

    static Metrics simulate(boolean useCT, double T, double dt) {
        int steps = (int)(T / dt);
        double[] q = {0.0, 0.0};
        double[] qdot = {0.0, 0.0};

        double Je = 0.0, Jtau = 0.0, emax = 0.0;

        for (int k = 0; k < steps; ++k) {
            double t = k*dt;
            double[] e = vecSub(qd(t), q);
            double[] edot = vecSub(qdDot(t), qdot);
            double[] tau = useCT ? controllerCT(q, qdot, t)
                                 : controllerPD(q, qdot, t);

            double[][] M = M(q);
            double[][] Cmat = C(q, qdot);
            double[] g_vec = gvec(q);
            double[] rhs = vecSub(tau, vecAdd(matVec(Cmat, qdot), g_vec));
            double[] qddot = solve2x2(M, rhs);

            qdot[0] += dt*qddot[0];
            qdot[1] += dt*qddot[1];
            q[0] += dt*qdot[0];
            q[1] += dt*qdot[1];

            Je += dot(e, e)*dt;
            Jtau += dot(tau, tau)*dt;
            double en = Math.sqrt(dot(e, e));
            if (en > emax) emax = en;
        }

        Metrics m = new Metrics();
        m.Je = Je; m.Jtau = Jtau; m.emax = emax;
        return m;
    }

    public static void main(String[] args) {
        double T = 10.0, dt = 0.001;
        Metrics mPD = simulate(false, T, dt);
        Metrics mCT = simulate(true, T, dt);
        System.out.println("PD: J_e=" + mPD.Je + " J_tau=" + mPD.Jtau + " e_max=" + mPD.emax);
        System.out.println("CT: J_e=" + mCT.Je + " J_tau=" + mCT.Jtau + " e_max=" + mCT.emax);
    }
}
      

This Java code mirrors the structure of the Python and C++ implementations and can be extended with plotting via, e.g., a plotting library or CSV export for offline analysis.

In MATLAB, we can implement the dynamics and controllers as functions and use ode45 for integration, or build a Simulink model where:

• A block computes the reference trajectory \( \mathbf{q}_d,\dot{\mathbf{q}}_d,\ddot{\mathbf{q}}_d \).
• A block computes \( \mathbf{e},\dot{\mathbf{e}} \) and outputs PD or computed-torque torques.
• A dynamics block computes joint accelerations and integrates them.

function lab_compare_PD_CT
    T = 10.0;
    dt = 0.001;
    tspan = 0:dt:T;

    % Initial state [q1; q2; q1dot; q2dot]
    x0 = [0; 0; 0; 0];

    Kp = diag([100, 80]);
    Kd = diag([20, 16]);

    % Simulate PD
    [t_PD, x_PD] = ode45(@(t, x) dyn_closed_loop(t, x, Kp, Kd, false), tspan, x0);
    [Je_PD, Jtau_PD, emax_PD] = compute_metrics(t_PD, x_PD, Kp, Kd, false);

    % Simulate CT
    [t_CT, x_CT] = ode45(@(t, x) dyn_closed_loop(t, x, Kp, Kd, true), tspan, x0);
    [Je_CT, Jtau_CT, emax_CT] = compute_metrics(t_CT, x_CT, Kp, Kd, true);

    fprintf('PD: J_e = %.3f, J_tau = %.3f, e_max = %.3f\n', Je_PD, Jtau_PD, emax_PD);
    fprintf('CT: J_e = %.3f, J_tau = %.3f, e_max = %.3f\n', Je_CT, Jtau_CT, emax_CT);

    % Plot joint 1 tracking
    figure; hold on;
    plot(t_PD, qd(t_PD, 1), 'k', 'DisplayName', 'q1_d');
    plot(t_PD, x_PD(:,1), 'r', 'DisplayName', 'q1 PD');
    plot(t_CT, x_CT(:,1), 'b', 'DisplayName', 'q1 CT');
    xlabel('t [s]'); ylabel('q1 [rad]');
    legend show;
end

function xdot = dyn_closed_loop(t, x, Kp, Kd, useCT)
    q = x(1:2);
    qdot = x(3:4);

    % Reference
    [qd_vec, qd_dot_vec, qd_ddot_vec] = ref_traj(t);

    e = qd_vec - q;
    edot = qd_dot_vec - qdot;

    if useCT
        v = qd_ddot_vec + Kd*edot + Kp*e;
        M = M_matrix(q);
        C = C_matrix(q, qdot);
        g_vec = g_vector(q);
        tau = M*v + C*qdot + g_vec;
    else
        tau = Kp*e + Kd*edot;
    end

    M = M_matrix(q);
    C = C_matrix(q, qdot);
    g_vec = g_vector(q);

    qddot = M \ (tau - C*qdot - g_vec);

    xdot = [qdot; qddot];
end

function [Je, Jtau, emax] = compute_metrics(t, x, Kp, Kd, useCT)
    Je = 0; Jtau = 0; emax = 0;
    for k = 1:length(t)
        q = x(k,1:2)';
        qdot = x(k,3:4)';
        [qd_vec, qd_dot_vec, qd_ddot_vec] = ref_traj(t(k));
        e = qd_vec - q;
        edot = qd_dot_vec - qdot;
        if useCT
            v = qd_ddot_vec + Kd*edot + Kp*e;
            M = M_matrix(q);
            C = C_matrix(q, qdot);
            g_vec = g_vector(q);
            tau = M*v + C*qdot + g_vec;
        else
            tau = Kp*e + Kd*edot;
        end
        Je = Je + (e'*e) * (t(2)-t(1));
        Jtau = Je + (tau'*tau) * (t(2)-t(1));
        en = norm(e);
        if en > emax
            emax = en;
        end
    end
end

function [qd_vec, qd_dot_vec, qd_ddot_vec] = ref_traj(t)
    qd_vec = [0.5*sin(0.5*t); 0.5*cos(0.5*t)];
    qd_dot_vec = [0.25*cos(0.5*t); -0.25*sin(0.5*t)];
    qd_ddot_vec = [-0.125*sin(0.5*t); -0.125*cos(0.5*t)];
end

% M, C, g as in Python/C++ versions (2-DOF planar arm)
function M = M_matrix(q)
    m1 = 1.0; m2 = 1.0;
    l1 = 1.0; lc1 = 0.5; lc2 = 0.5; I1 = 0.1; I2 = 0.1;
    q1 = q(1); q2 = q(2);
    c2 = cos(q2);
    m11 = I1 + I2 + m1*lc1^2 + m2*(l1^2 + lc2^2 + 2*l1*lc2*c2);
    m12 = I2 + m2*(lc2^2 + l1*lc2*c2);
    m22 = I2 + m2*lc2^2;
    M = [m11, m12; m12, m22];
end

function C = C_matrix(q, qdot)
    m2 = 1.0; l1 = 1.0; lc2 = 0.5;
    q2 = q(2);
    q1dot = qdot(1); q2dot = qdot(2);
    s2 = sin(q2);
    h = -m2*l1*lc2*s2;
    C = [h*q2dot, h*(q1dot + q2dot);
         -h*q1dot, 0];
end

function g_vec = g_vector(q)
    m1 = 1.0; m2 = 1.0;
    l1 = 1.0; lc1 = 0.5; lc2 = 0.5;
    g = 9.81;
    q1 = q(1); q2 = q(2);
    g1 = (m1*lc1 + m2*l1)*g*cos(q1) + m2*lc2*g*cos(q1 + q2);
    g2 = m2*lc2*g*cos(q1 + q2);
    g_vec = [g1; g2];
end
      

In Simulink, the same equations can be implemented using blocks: a reference generator, a controller block (switchable between PD and CT), and a dynamics block. Data logging blocks can compute \( J_e, J_{\max}, J_{\tau} \).

Mathematica is convenient for symbolic verification of error dynamics as well as numerical simulation. Below is a numerical example with a switch between PD and computed torque.

(* Parameters *)
m1 = 1.; m2 = 1.;
l1 = 1.; l2 = 1.;
lc1 = 0.5; lc2 = 0.5;
I1 = 0.1; I2 = 0.1;
g = 9.81;

Mmat[{q1_, q2_}] := Module[{c2 = Cos[q2]},
  {
    {I1 + I2 + m1 lc1^2 + m2 (l1^2 + lc2^2 + 2 l1 lc2 c2),
     I2 + m2 (lc2^2 + l1 lc2 c2)},
    {I2 + m2 (lc2^2 + l1 lc2 c2),
     I2 + m2 lc2^2}
  }
];

Cmat[{q1_, q2_}, {q1dot_, q2dot_}] := Module[{s2 = Sin[q2], h},
  h = -m2 l1 lc2 s2;
  {
    {h q2dot, h (q1dot + q2dot)},
    {-h q1dot, 0}
  }
];

gvec[{q1_, q2_}] :=
  {
    (m1 lc1 + m2 l1) g Cos[q1] + m2 lc2 g Cos[q1 + q2],
    m2 lc2 g Cos[q1 + q2]
  };

qd[t_] := {0.5 Sin[0.5 t], 0.5 Cos[0.5 t]};
qdDot[t_] := {0.25 Cos[0.5 t], -0.25 Sin[0.5 t]};
qdDdot[t_] := {-0.125 Sin[0.5 t], -0.125 Cos[0.5 t]};

Kp = DiagonalMatrix[{100., 80.}];
Kd = DiagonalMatrix[{20., 16.}];

controller[t_, q_, qdot_, mode_] := Module[
  {e, edot, v, tau},
  e = qd[t] - q;
  edot = qdDot[t] - qdot;
  If[mode == "PD",
    tau = Kp.e + Kd.edot,
    (* Computed torque *)
    v = qdDdot[t] + Kd.edot + Kp.e;
    tau = Mmat[q].v + Cmat[q, qdot].qdot + gvec[q];
  ];
  tau
];

(* Closed-loop dynamics ODE system *)
mode = "CT"; (* or "PD" *)

eqs = {
  q1'[t] == q1dot[t],
  q2'[t] == q2dot[t],
  {q1ddot[t], q2ddot[t]} ==
    LinearSolve[
      Mmat[{q1[t], q2[t]}],
      controller[t, {q1[t], q2[t]}, {q1dot[t], q2dot[t]}, mode]
        - Cmat[{q1[t], q2[t]}, {q1dot[t], q2dot[t]}].{q1dot[t], q2dot[t]}
        - gvec[{q1[t], q2[t]}]
    ]
};

ics = {q1[0] == 0, q2[0] == 0, q1dot[0] == 0, q2dot[0] == 0};

solCT = NDSolve[
  Flatten[{eqs, ics}] /. mode -> "CT",
  {q1, q2, q1dot, q2dot},
  {t, 0, 10}
][[1]];

solPD = NDSolve[
  Flatten[{eqs, ics}] /. mode -> "PD",
  {q1, q2, q1dot, q2dot},
  {t, 0, 10}
][[1]];

Plot[
  Evaluate[
    {
      qd[t][[1]],
      q1[t] /. solPD,
      q1[t] /. solCT
    }
  ],
  {t, 0, 10},
  PlotLegends -> {"q1_d", "q1_PD", "q1_CT"},
  AxesLabel -> {"t", "q1"}
]
      

You can also symbolically verify the linear error dynamics for the computed-torque law by substituting the controller into the dynamics and simplifying \( \ddot{\mathbf{e}} \).

In this lab we implemented and compared joint-space PD control and computed-torque inverse-dynamics control on a 2-DOF manipulator. We used the robot dynamics model to derive linear error dynamics for the computed-torque controller and showed that it effectively decouples joints and shapes the tracking response through \( \mathbf{K}_p,\mathbf{K}_d \).

Python, C++, Java, MATLAB/Simulink, and Mathematica implementations illustrated that the core structure of the lab is language-independent: the difference between PD and computed torque lies in how torques are computed from the tracking errors and the model, not in the numerical integrator. Quantitative metrics \( J_e, J_{\max}, J_{\tau} \) provide a systematic way to compare the two controllers, and you should observe substantially improved tracking for computed torque under accurate modeling assumptions.