Feedback: P and PD control

Open loop can't react

Step 3's timed torques worked because nothing got in the way. If something bumps the arm, a timed plan carries on regardless. Feedback (a closed loop) measures what actually happened and corrects it, hundreds of times a second.

Proportional control

The error is how far the joint angle $\theta$ is from where you want it: $e = \theta_{\text{target}} - \theta$. A P controller pushes in proportion to it:

$$\tau = k_p\,e$$

$k_p$, the proportional gain (N·m per rad), acts like a spring pulling the joint to its target. A spring with nothing to slow it bounces for ever, and this base has no friction at all: it swings past the target, back, and past again.

Add damping

A PD controller adds a push against the angular velocity $\omega$:

$$\tau = k_p\,(\theta_{\text{target}} - \theta) - k_d\,\omega$$

$k_d$, the derivative gain (N·m·s/rad), works like a shock absorber. Too little and the joint still overshoots; too much and it creeps. The sweet spot, critical damping, is

$$k_d = 2\sqrt{k_p\,I}$$

where $I \approx 0.133$ kg·m² is the base's moment of inertia in this pose (you measured it in step 3).

Saturation

The base motor gives at most 10 N·m. With $k_p = 40$, a 1 rad error asks for 40 N·m, so at first the motor is flat out (saturated), and the PD law only takes over near the target. Bigger gains react harder, but they can't make the motor any stronger.

Pick & Place's position servos run this same PD law for you: see Pick & Place, step 1.

Your task

1. Write pd_torque(q, qd, target, kp, kd): the PD torque for one joint, with q for $\theta$ and qd for $\omega$.
2. The program runs P only (kd = 0) for 3 s: watch it swing in Plots. Then it steps the base from 0 to 1 rad with your KP and KD, and pushes the hand sideways 1.5 s in.
3. Choose KD. The step must settle within ±1° in 1 s and overshoot (go past the target) by under 5 % of the step, and after the push the base must be back within ±1° in under 0.8 s.