Computed torque

Why PD lags

Feedback only pushes once an error has appeared. On a fast move, PD + gravity has to fall behind by a sizeable error before it makes the torque needed to accelerate, so it lags by degrees unless its gains are made harshly stiff.

Inverse dynamics

Step 1 predicted motion from torques. Turned around, the equations of motion give the torques that produce a chosen acceleration $a$:

This is inverse dynamics.

Computed torque

Choose the acceleration from the trajectory, plus corrections for errors:

$q_\text{des}$, $\dot q_\text{des}$ and $\ddot q_\text{des}$ are where the trajectory wants the arm now, and how fast and how hard it is accelerating. Substitute $\tau = M(q)\,a + h$ into $M(q)\,\ddot q + h = \tau$ and the arm's own dynamics cancel: $\ddot q = a$. Every joint's error $e = q_\text{des} - q$ then obeys

the same well-damped spring in every pose, at every speed. $K_p$ (1/s²) and $K_d$ (1/s) are gains on accelerations, not torques: $M(q)$ turns them into the right torque for each joint.

Feed-forward and feedback

Split the torque in two. $M(q)\,\ddot q_\text{des} + h(q, \dot q)$ is feed-forward: what the motion needs, known before anything goes wrong. $M(q)\,(K_d\,\dot e + K_p\,e)$ is feedback: the correction. The program plots both at the shoulder; the feed-forward does nearly all the work.

The program

The arm sweeps out from A to B and back, 0.5 s each way. It runs once with PD + gravity, gains scaled for a typical 0.1 kg·m² joint, and once with your controller. Plots shows both errors.

Your task

Write inverse_dynamics(q, qd, qdd), then computed_torque(q, qd, q_des, qd_des, qdd_des) using it. Both are checked on 8 states, and your controller must keep every joint within 0.5° of the sweep.