PID when the model is wrong

The model is never perfect

The gripper now holds a tool of unknown mass. arm.gravity_torques(q) still describes the arm as designed, without the tool, so gravity compensation falls short by the tool's weight. With PD on top, step 2 says the arm settles $\Delta g / K_p$ off target, where $\Delta g$ is the torque the model is missing.

The I term

A PID controller adds the integral of the error $e = q_\text{des} - q$:

While any error is left, the integral keeps growing, until $K_i \int e\,dt$ supplies the missing torque and $e = 0$. The motors get $\tau = g(q) + u$: the model does what it can and the PID finds the rest. Every quantity is a numpy array with one entry per joint, so the code is the same as for a single joint.

Windup

Each output is clamped to $\pm$limit. During a big move it sits at the limit, but the integral keeps growing (winding up) without changing $u$. When the arm arrives, that stored-up integral throws it past the target. Conditional integration fixes it: a joint whose output is clamped keeps its old integral.

integral_new = integral + error * dt
derivative = (error - prev_error) / dt        # zeros on the first call
u = kp * error + ki * integral_new + kd * derivative
u_out = np.clip(u, -limit, limit)
integral = np.where(u_out == u, integral_new, integral)   # only joints that aren't clamped
prev_error = error

The program

At t = 0 the shoulder steps from 0.2 to 0.8 rad and the elbow from 1.2 to 1.0 rad; the base and wrist are held for you. At 2.5 s the hand is pushed down with 20 N for 0.1 s. The PID runs at 500 Hz, and Plots shows the errors and $u$. Leave pid_response under your class: the grader tests PID through it.

Your task

Complete PID: its memory in __init__ and update(error, dt). The grader feeds it fixed error sequences, one with a long saturation. Live, both joints must settle within ±0.3° by t = 2 s, overshoot by less than 13 %, and be back within ±0.5° within 0.8 s of the push.