We consider a 2-DOF planar robotic arm with joint coordinates \( \mathbf{q} = [q_1\; q_2]^\top \) and end-effector Cartesian position \( \mathbf{x} = [x\; y]^\top \in \mathbb{R}^2 \). The forward kinematics is

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

where \( \mathbf{J}(\mathbf{q}) \) is the geometric Jacobian. The joint-space dynamics of the manipulator is

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

with positive definite inertia matrix \( \mathbf{M}(\mathbf{q}) \), Coriolis/centrifugal term \( \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} \), gravity vector \( \mathbf{g}(\mathbf{q}) \), and joint torques \( \boldsymbol{\tau} \).

In the lab you will:

• Generate a smooth Cartesian trajectory \( \mathbf{x}_d(t) \) from start to goal.
• Compute task-space errors and a desired task-space acceleration \( \mathbf{a}_x(t) \).
• Map \( \mathbf{a}_x(t) \) to desired joint accelerations and torques \( \boldsymbol{\tau}(t) \).
• Simulate the closed-loop system in discrete time and visualize tracking errors.

flowchart TD
  XD["Desired trajectory x_d(t)"] --> SAMP["Sample at t_k"]
  SAMP --> ERR["Compute e_x = x_d - x"]
  ERR --> CTRL["Task-space PD / op-space law"]
  CTRL --> IDYN["Inverse dynamics / J mapping"]
  IDYN --> ROB["Robot dynamics (q, qdot)"]
  ROB --> KIN["Forward kinematics & Jacobian"]
  KIN --> ERR
        

Let the desired end-effector trajectory be \( \mathbf{x}_d(t) \) with derivatives \( \dot{\mathbf{x}}_d(t) \) and \( \ddot{\mathbf{x}}_d(t) \). Define task-space error

\[ \mathbf{e}_x = \mathbf{x}_d - \mathbf{x}, \quad \dot{\mathbf{e}}_x = \dot{\mathbf{x}}_d - \dot{\mathbf{x}}. \]

A standard operational-space PD tracking law uses a desired task-space acceleration \( \mathbf{a}_x \):

\[ \mathbf{a}_x = \ddot{\mathbf{x}}_d + \mathbf{K}_d \dot{\mathbf{e}}_x + \mathbf{K}_p \mathbf{e}_x, \]

where \( \mathbf{K}_p \) and \( \mathbf{K}_d \) are symmetric positive definite task-space gain matrices. In the full operational-space formalism, the joint torques are

\[ \boldsymbol{\tau} = \mathbf{J}(\mathbf{q})^\top \big( \boldsymbol{\Lambda}(\mathbf{q}) \mathbf{a}_x + \boldsymbol{\mu}_x(\mathbf{q},\dot{\mathbf{q}}) + \mathbf{p}_x(\mathbf{q}) \big), \]

where the task-space inertia \( \boldsymbol{\Lambda}(\mathbf{q}) \), Coriolis/centripetal term \( \boldsymbol{\mu}_x \) and gravity term \( \mathbf{p}_x \) are obtained by projecting the joint-space dynamics into operational space (as derived in Lesson 2).

Under perfect modeling and nonsingular kinematics, the resulting end-effector acceleration satisfies \( \ddot{\mathbf{x}} = \mathbf{a}_x \). Substituting the control law gives

\[ \ddot{\mathbf{e}}_x = \ddot{\mathbf{x}}_d - \ddot{\mathbf{x}} = \ddot{\mathbf{x}}_d - \mathbf{a}_x = - \mathbf{K}_d \dot{\mathbf{e}}_x - \mathbf{K}_p \mathbf{e}_x, \]

i.e., the linear time-varying error dynamics

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

For constant gains with eigenvalues of \( \mathbf{K}_p \) and \( \mathbf{K}_d \) strictly positive, all scalar modes of the error satisfy a second-order stable differential equation with exponentially decaying solutions. Thus, in ideal conditions, the end-effector trajectory tracking error converges to zero.

In practice, modeling errors and discretization effects perturb this ideal behavior; the lab will reveal sensitivity to gain choice and sampling time.

Let the control loop run with sampling time \( T_s \) and discrete instants \( t_k = k T_s \). At time step \( k \), we approximate the continuous control law by

\[ \begin{aligned} \mathbf{e}_x[k] &= \mathbf{x}_d[t_k] - \mathbf{x}[t_k], \\ \dot{\mathbf{e}}_x[k] &\approx \frac{\mathbf{e}_x[k] - \mathbf{e}_x[k-1]}{T_s}, \\ \mathbf{a}_x[k] &= \ddot{\mathbf{x}}_d[t_k] + \mathbf{K}_d \dot{\mathbf{e}}_x[k] + \mathbf{K}_p \mathbf{e}_x[k]. \end{aligned} \]

For a non-redundant 2-DOF arm with \( \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{2\times 2} \) invertible away from singularities, we compute the desired joint accelerations as

\[ \ddot{\mathbf{q}}_d[k] = \mathbf{J}(\mathbf{q}[k])^{-1} \Big( \mathbf{a}_x[k] - \dot{\mathbf{J}}(\mathbf{q}[k],\dot{\mathbf{q}}[k])\dot{\mathbf{q}}[k] \Big). \]

The joint torques follow from inverse dynamics:

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

The joint states are integrated forward (e.g., via explicit Euler):

\[ \begin{aligned} \dot{\mathbf{q}}[k+1] &= \dot{\mathbf{q}}[k] + T_s \ddot{\mathbf{q}}[k], \\ \mathbf{q}[k+1] &= \mathbf{q}[k] + T_s \dot{\mathbf{q}}[k+1]. \end{aligned} \]

In simulation we can either treat the plant as ideal (directly applying \( \ddot{\mathbf{q}}_d \)) or numerically integrate the full dynamics with \( \boldsymbol{\tau}[k] \) as input using the same dynamic model.

To avoid discontinuities in velocities and accelerations, we construct trajectories via polynomial time-scaling between an initial end-effector pose \( \mathbf{x}_0 \) and a final pose \( \mathbf{x}_f \) over time horizon \( T > 0 \). Let \( s(t) \in [0,1] \) be a scalar time-scaling function satisfying \( s(0)=0 \), \( s(T)=1 \), \( \dot{s}(0)=\dot{s}(T)=0 \), and \( \ddot{s}(0)=\ddot{s}(T)=0 \). A standard quintic choice is

\[ \tilde{t} = \frac{t}{T}, \quad s(t) = 10 \tilde{t}^3 - 15 \tilde{t}^4 + 6 \tilde{t}^5. \]

Then the desired position, velocity, and acceleration are

\[ \begin{aligned} \mathbf{x}_d(t) &= \mathbf{x}_0 + s(t)(\mathbf{x}_f - \mathbf{x}_0), \\ \dot{\mathbf{x}}_d(t) &= \dot{s}(t)(\mathbf{x}_f - \mathbf{x}_0), \\ \ddot{\mathbf{x}}_d(t) &= \ddot{s}(t)(\mathbf{x}_f - \mathbf{x}_0), \end{aligned} \]

where \( \dot{s}(t) \) and \( \ddot{s}(t) \) are obtained by differentiating the quintic polynomial. More complex trajectories (e.g., circles or Lissajous curves) can be generated by choosing \( \mathbf{x}_d(t) \) directly as a time-parametrized curve and analytically differentiating it.

flowchart TD
  X0["x_0 (start pose)"] --> SETT["Select T, quintic s(t)"]
  XF["x_f (goal pose)"] --> SETT
  SETT --> TRAJ["Compute x_d(t), xdot_d(t), xddot_d(t)"]
  TRAJ --> SAMPL["Sample at t_k for control loop"]
        

We now implement the end-effector trajectory tracking loop in Python for a 2-DOF planar arm. We use numpy for linear algebra and implement the kinematics and dynamics explicitly. This code is suitable for offline simulation.

import numpy as np

# --------------------------
# Robot parameters (2-link planar)
# --------------------------
l1, l2 = 1.0, 1.0      # link lengths
m1, m2 = 1.0, 1.0      # link masses
lc1, lc2 = 0.5, 0.5    # CoM distances
I1, I2 = 0.05, 0.05    # link inertias
g = 9.81

def forward_kinematics(q):
    q1, q2 = q
    x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
    y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
    return np.array([x, y])

def jacobian(q):
    q1, q2 = q
    s1 = np.sin(q1); c1 = np.cos(q1)
    s12 = np.sin(q1 + q2); c12 = np.cos(q1 + q2)
    j11 = -l1 * s1 - l2 * s12
    j12 = -l2 * s12
    j21 =  l1 * c1 + l2 * c12
    j22 =  l2 * c12
    return np.array([[j11, j12],
                     [j21, j22]])

def jacobian_dot(q, qdot):
    # simple numerical approximation for lab purposes
    eps = 1e-6
    J0 = jacobian(q)
    Jdot = np.zeros((2, 2))
    for i in range(2):
        dq = np.zeros(2)
        dq[i] = eps
        J_plus = jacobian(q + dq)
        J_minus = jacobian(q - dq)
        # approximate dJ/dq_i * qdot_i
        Jdot += (J_plus - J_minus) / (2.0 * eps) * qdot[i]
    return Jdot

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

def C_vector(q, qdot):
    q1, q2 = q
    q1dot, q2dot = qdot
    s2 = np.sin(q2)
    h = -m2 * l1 * lc2 * s2
    c1 = h * (2.0 * q1dot * q2dot + q2dot**2)
    c2 = h * q1dot**2
    return np.array([c1, c2])

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])

# --------------------------
# Quintic time scaling
# --------------------------
def s_quintic(t, T):
    tau = t / T
    return 10 * tau**3 - 15 * tau**4 + 6 * tau**5

def s_quintic_dot(t, T):
    tau = t / T
    return (30 * tau**2 - 60 * tau**3 + 30 * tau**4) / T

def s_quintic_ddot(t, T):
    tau = t / T
    return (60 * tau - 180 * tau**2 + 120 * tau**3) / (T**2)

# --------------------------
# Desired Cartesian motion: line from x0 to xf
# --------------------------
x0 = np.array([0.8, 0.4])
xf = np.array([1.2, 0.6])
T = 4.0    # duration [s]

def x_des(t):
    s = s_quintic(t, T)
    return x0 + s * (xf - x0)

def xdot_des(t):
    sd = s_quintic_dot(t, T)
    return sd * (xf - x0)

def xddot_des(t):
    sdd = s_quintic_ddot(t, T)
    return sdd * (xf - x0)

# --------------------------
# Control gains and simulation setup
# --------------------------
Kp = np.diag([100.0, 100.0])
Kd = np.diag([20.0, 20.0])

dt = 0.001
t_final = 5.0
N = int(t_final / dt)

q = np.array([0.0, 0.0])
qdot = np.array([0.0, 0.0])

xs_log = []
xd_log = []
err_log = []

for k in range(N):
    t = k * dt

    # forward kinematics
    x = forward_kinematics(q)
    J = jacobian(q)
    xdot = J @ qdot
    Jdot = jacobian_dot(q, qdot)

    # desired trajectory
    xd = x_des(t)
    xdd = xddot_des(t)
    xd_dot = xdot_des(t)

    # errors
    e = xd - x
    e_dot = xd_dot - xdot

    # task-space PD law
    a_x = xdd + Kd @ e_dot + Kp @ e

    # joint accelerations via kinematic relation
    J_inv = np.linalg.inv(J)
    qdd_des = J_inv @ (a_x - Jdot @ qdot)

    # inverse dynamics (computed torque)
    M = M_matrix(q)
    C = C_vector(q, qdot)
    g_vec = g_vector(q)
    tau = M @ qdd_des + C + g_vec

    # plant: here we assume ideal dynamics = inverse of the same model
    qdd = np.linalg.solve(M, tau - C - g_vec)

    # integrate
    qdot = qdot + dt * qdd
    q = q + dt * qdot

    xs_log.append(x.copy())
    xd_log.append(xd.copy())
    err_log.append(e.copy())

xs_log = np.array(xs_log)
xd_log = np.array(xd_log)
err_log = np.array(err_log)

print("Final end-effector error:", err_log[-1])
      

This script simulates an operational-space tracking controller along a straight-line Cartesian path. You can easily visualize tracking with matplotlib by plotting xs_log[:,0] vs xs_log[:,1] together with xd_log, and inspect error time series from err_log.

In C++, we typically use Eigen for matrix operations and run the same loop in real time on a controller. Below is a minimal skeleton of the control loop; details of kinematics and dynamics follow the Python code.

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

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

Vector2d forwardKinematics(const Vector2d& q);
Matrix2d jacobian(const Vector2d& q);
Matrix2d jacobianDot(const Vector2d& q, const Vector2d& qdot);
Matrix2d M_matrix(const Vector2d& q);
Vector2d C_vector(const Vector2d& q, const Vector2d& qdot);
Vector2d g_vector(const Vector2d& q);

Vector2d x_des(double t);
Vector2d xdot_des(double t);
Vector2d xddot_des(double t);

int main() {
    double dt = 0.001;
    double t_final = 5.0;

    Matrix2d Kp = Matrix2d::Zero();
    Matrix2d Kd = Matrix2d::Zero();
    Kp(0,0) = 100.0; Kp(1,1) = 100.0;
    Kd(0,0) = 20.0;  Kd(1,1) = 20.0;

    Vector2d q(0.0, 0.0);
    Vector2d qdot(0.0, 0.0);

    int N = static_cast<int>(t_final / dt);
    for (int k = 0; k < N; ++k) {
        double t = k * dt;

        Vector2d x   = forwardKinematics(q);
        Matrix2d J   = jacobian(q);
        Vector2d xdot = J * qdot;
        Matrix2d Jdot = jacobianDot(q, qdot);

        Vector2d xd    = x_des(t);
        Vector2d xd_dot = xdot_des(t);
        Vector2d xdd    = xddot_des(t);

        Vector2d e    = xd - x;
        Vector2d e_dot = xd_dot - xdot;

        Vector2d a_x = xdd + Kd * e_dot + Kp * e;

        Matrix2d Jinv = J.inverse();
        Vector2d qdd_des = Jinv * (a_x - Jdot * qdot);

        Matrix2d M = M_matrix(q);
        Vector2d C = C_vector(q, qdot);
        Vector2d g = g_vector(q);

        Vector2d tau = M * qdd_des + C + g;

        // plant integration (explicit Euler using same model)
        Vector2d qdd = M.ldlt().solve(tau - C - g);
        qdot += dt * qdd;
        q    += dt * qdot;
    }

    std::cout << "Simulation finished." << std::endl;
    return 0;
}
      

For deployment on real hardware, this loop would run inside a real-time thread at period \( T_s \), with state estimates coming from encoders and velocity observers instead of the simulated model.

In Java-based environments (e.g., when building a GUI or a higher-level coordination layer), a common design is to compute the desired joint accelerations or velocities in Java and delegate torque-level control to the robot controller. Below is a simplified kinematic-level controller that computes qdd_des but leaves dynamics to a lower layer.

public class TaskSpaceController2DOF {

    private double dt;
    private double[][] Kp;
    private double[][] Kd;

    public TaskSpaceController2DOF(double dt) {
        this.dt = dt;
        this.Kp = new double[][]{  {100.0, 0.0}, {0.0, 100.0}  };
        this.Kd = new double[][]{  {20.0, 0.0}, {0.0, 20.0}  };
    }

    // Implement kinematics, Jacobian, etc. similarly to the Python code
    public double[] forwardKinematics(double[] q) { /* ... */ return new double[2]; }
    public double[][] jacobian(double[] q) { /* ... */ return new double[2][2]; }
    public double[][] jacobianDot(double[] q, double[] qdot) { /* ... */ return new double[2][2]; }

    public double[] xDes(double t) { /* quintic line */ return new double[2]; }
    public double[] xdotDes(double t) { return new double[2]; }
    public double[] xddotDes(double t) { return new double[2]; }

    public double[] step(double t, double[] q, double[] qdot) {
        double[] x = forwardKinematics(q);
        double[][] J = jacobian(q);
        double[] xdot = matVec(J, qdot);
        double[][] Jdot = jacobianDot(q, qdot);

        double[] xd = xDes(t);
        double[] xdDot = xdotDes(t);
        double[] xdd = xddotDes(t);

        double[] e = new double[]{xd[0] - x[0], xd[1] - x[1]};
        double[] eDot = new double[]{xdDot[0] - xdot[0], xdDot[1] - xdot[1]};

        double[] Kp_e = matVec(Kp, e);
        double[] Kd_eDot = matVec(Kd, eDot);

        double[] a_x = new double[]{
            xdd[0] + Kd_eDot[0] + Kp_e[0],
            xdd[1] + Kd_eDot[1] + Kp_e[1]
        };

        double[][] Jinv = invert2x2(J);
        double[] Jdot_qdot = matVec(Jdot, qdot);
        double[] rhs = new double[]{
            a_x[0] - Jdot_qdot[0],
            a_x[1] - Jdot_qdot[1]
        };

        double[] qdd_des = matVec(Jinv, rhs);
        return qdd_des; // to be consumed by lower-level torque or velocity controller
    }

    private double[] matVec(double[][] A, double[] x) { /* ... */ return new double[x.length]; }
    private double[][] invert2x2(double[][] A) { /* closed-form inverse */ return new double[2][2]; }
}
      

This approach keeps Java responsible for task-space control logic while leveraging existing libraries or firmware for low-level actuation and real-time scheduling.

In MATLAB, the same control law can be coded as an m-file and embedded in a Simulink block. When using the Robotics System Toolbox, it is convenient to define the robot as a rigidBodyTree and use forwardKinematics and geometricJacobian utilities.

function dx = planar2dof_opspace_ode(t, x, robot, Kp, Kd, x0, xf, T)

% x = [q1; q2; q1dot; q2dot]
q    = x(1:2);
qdot = x(3:4);

eeName = 'tool';
baseName = robot.BaseName;

% kinematics and Jacobian from rigidBodyTree
T_ee = getTransform(robot, q, eeName, baseName);
p = T_ee(1:2, 4);
J = geometricJacobian(robot, q, eeName);
J = J(1:2, 1:2); % planar subset

xd    = x_des(t, x0, xf, T);
xdDot = xdot_des(t, x0, xf, T);
xdd   = xddot_des(t, x0, xf, T);

e    = xd - p;
eDot = xdDot - J * qdot;

a_x = xdd + Kd * eDot + Kp * e;

% numerical Jdot
eps = 1e-6;
Jdot = zeros(2, 2);
for i = 1:2
    dq = zeros(2, 1);
    dq(i) = eps;
    Jp = geometricJacobian(robot, q + dq, eeName);
    Jm = geometricJacobian(robot, q - dq, eeName);
    Jp = Jp(1:2, 1:2);
    Jm = Jm(1:2, 1:2);
    Jdot = Jdot + (Jp - Jm) / (2 * eps) * qdot(i);
end

qdd_des = J \ (a_x - Jdot * qdot); % 2x2 solve

% inverse dynamics using rigidBodyTree
tau = inverseDynamics(robot, q, qdot, qdd_des);

M = massMatrix(robot, q);
C = velocityProduct(robot, q, qdot);
G = gravityTorque(robot, q);

qdd = M \ (tau - C - G);

dx = zeros(4,1);
dx(1:2) = qdot;
dx(3:4) = qdd;
end
      

In Simulink, you can encapsulate the above differential equation inside an ODE block or implement the discrete-time control law directly using blocks for kinematics, Jacobian evaluation, gain matrices, and inverse dynamics. This graphically exposes the structure of the operational-space control loop discussed earlier.

Mathematica is convenient for symbolic derivation and numerical integration of the closed-loop equations. The snippet below outlines a simulation with the same planar arm and task-space PD control.

(* Parameters *)
l1 = 1.0; l2 = 1.0;
m1 = 1.0; m2 = 1.0;
lc1 = 0.5; lc2 = 0.5;
I1 = 0.05; I2 = 0.05;
g = 9.81;

Kp = DiagonalMatrix[{100.0, 100.0}];
Kd = DiagonalMatrix[{20.0, 20.0}];

x0 = {0.8, 0.4};
xf = {1.2, 0.6};
T = 4.0;

s[t_] := 10 (t/T)^3 - 15 (t/T)^4 + 6 (t/T)^5;
sd[t_] := D[s[t], t];
sdd[t_] := D[sd[t], t];

xdes[t_] := x0 + s[t] (xf - x0);
xdotdes[t_] := sd[t] (xf - x0);
xddotdes[t_] := sdd[t] (xf - x0);

(* Kinematics and dynamics *)
xpos[q1_, q2_] := {
  l1 Cos[q1] + l2 Cos[q1 + q2],
  l1 Sin[q1] + l2 Sin[q1 + q2]
};

Jmat[q1_, q2_] := {
  {-l1 Sin[q1] - l2 Sin[q1 + q2], -l2 Sin[q1 + q2]},
  { l1 Cos[q1] + l2 Cos[q1 + q2],  l2 Cos[q1 + q2]}
};

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} }
];

Cvec[q1_, q2_, q1d_, q2d_] := Module[{s2 = Sin[q2]},
  Module[{h = -m2 l1 lc2 s2},
    {
      h (2 q1d q2d + q2d^2),
      h q1d^2
    }
  ]
];

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

(* Closed-loop dynamics *)
eqns = {
  q1'[t] == q1d[t],
  q2'[t] == q2d[t],
  {q1d'[t], q2d'[t]} ==
    Module[{q1t = q1[t], q2t = q2[t], q1dt = q1d[t], q2dt = q2d[t],
            x, J, xdot, xd, xdd, xdDot, e, eDot, ax, M, C, G, tau},
      x = xpos[q1t, q2t];
      J = Jmat[q1t, q2t];
      xdot = J . {q1dt, q2dt};
      xd = xdes[t]; xdDot = xdotdes[t]; xdd = xddotdes[t];
      e = xd - x; eDot = xdDot - xdot;
      ax = xdd + Kd . eDot + Kp . e;
      (* numerical Jdot omitted; small velocities assumed *)
      M = Mmat[q1t, q2t];
      C = Cvec[q1t, q2t, q1dt, q2dt];
      G = Gvec[q1t, q2t];
      (* approximate qdd = M^-1 (tau - C - G) with tau = M qdd_des + C + G *)
      LinearSolve[M, (M . LinearSolve[J, ax]) - C - G]
    }
};

ics = {q1[0] == 0.0, q2[0] == 0.0, q1d[0] == 0.0, q2d[0] == 0.0};

sol = NDSolve[Join[eqns, ics], {q1, q2, q1d, q2d}, {t, 0, 5.0}][[1]];

ParametricPlot[
  Evaluate[xpos[q1[t] /. sol, q2[t] /. sol]],
  {t, 0, 5.0},
  PlotRange -> All,
  AxesLabel -> {"x", "y"}
]
      

Mathematica allows you to symbolically inspect the error dynamics and verify exponential convergence under ideal assumptions, complementing the numeric simulations in other languages.

In this lab we translated the abstract operational-space control law into a concrete implementation for a 2-DOF planar manipulator. We constructed smooth Cartesian trajectories using quintic time-scaling, computed task-space errors and accelerations, mapped them into desired joint accelerations via the Jacobian, and generated torques using inverse dynamics. Implementations in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica illustrated how the same mathematical structure underlies a variety of software environments. The theoretical analysis of error dynamics and sampling effects complements the numerical experiments, preparing you for more complex task hierarchies and constrained control in subsequent chapters.