Gravity compensation

Floating

At rest ($\dot q = 0$, $\ddot q = 0$) step 1's equation leaves $\tau = g(q)$: the torques that exactly cancel gravity. Command $g(q)$ at every instant and the arm floats. It stays wherever it is, and nothing pulls it anywhere else. Add a little damping, $-K_d\,\dot q$, and a push makes it glide and stop where you leave it. Collaborative robots use this mode so that a person can guide the arm by hand.

PD alone sags

The PD controller (Foundations: P and PD control),

pulls each joint towards its target $q_\text{des}$ like a spring with stiffness $K_p$ (N·m per rad) and a damper $K_d$ (N·m·s per rad). At rest it only makes torque when there is an error $e = q_\text{des} - q$, so the arm sinks until the spring holds it up, $K_p\,e = g(q)$:

This is the steady-state error. A stiffer spring sags less, never zero, and stiff gains make a harsh arm that is dangerous to stand next to.

Add the model

Now the model supplies the holding torque (feed-forward), and PD only corrects what the model gets wrong (feedback). At rest $K_p\,e = 0$, so $e = 0$, with the same gentle gains.

The program

It floats the arm with pd_gravity(q, qd, q, True) (the target is wherever the arm is), and at t = 0.5 s the hand gets a push. Then it glides to three poses with PD alone, holding each for a second, and does the same with PD + g. Plots shows the shoulder and elbow errors.

Your task

1. In pd_gravity(q, qd, q_des, gravity), add $g(q)$ (arm.gravity_torques(q)) when gravity is True.
2. Write predicted_error(q_des): PD alone's steady-state error $g(q_\text{des})/K_p$ for each joint.

The arm must float still and come to rest after the push, PD alone must end within 15 % of your prediction, and PD + g within 0.2° of every target.