Computed-torque control

What it computes

PD feedback only pushes once an error has appeared, so on a fast move it always lags behind. Computed-torque control uses the arm's model to work out the torque the motion needs, and leaves feedback only the small errors that remain. It is also called inverse-dynamics control, or feedback linearization.

The maths

The arm obeys $M(q)\,\ddot q + h(q, \dot q) = \tau$, where $M$ is the 4×4 mass matrix and $h$ holds the Coriolis, centrifugal and gravity torques. Choose

Substituting it cancels the dynamics. Every joint's error $e = q_\text{des} - q$ then obeys $\ddot e + K_d \dot e + K_p e = 0$: the same stable linear system in every pose and at every speed.

a = qdd_des + KD * (qd_des - qd) + KP * (q_des - q)   # the acceleration you want
tau = M(q) @ a + h(q, qd)                             # the torque that produces it

Tools

• arm.mass_matrix(q) returns $M(q)$ as a 4×4 array (kg·m²).
• arm.bias_torques(q, qd) returns $h(q, \dot q)$ (N·m).

Pass q and qd to both. Without them they read the arm's live state, and the grader calls your function on other states.

The program

The arm is torque-controlled with no gravity compensation. It runs a 0.5 s quintic sweep (the polynomial from Trajectory Generation) twice: first with PD plus gravity compensation, then with your controller. It plots both errors. KP and KD are set at the top.

Your task

Implement computed_torque(q, qd, q_des, qd_des, qdd_des), returning the four joint torques. The grader compares it with the reference on 10 states, then checks that the arm stays within 0.5° of the sweep.