Bonus: Weigh the payload

Weighing with the motors

Step 3's I term found the torque the model was missing, but only for one pose, and it has to find it again whenever the arm moves. Better to work out the payload itself and put it in the model.

Gravity is linear in the payload

Take a payload of mass $m$ sitting $d$ past the wrist, along the hand. The links tilt $a_1 = q_1$, $a_2 = q_1 + q_2$ and $a_3 = q_1 + q_2 + q_3$ from vertical, so the payload sits $z = L_1\cos a_1 + L_2\cos a_2 + d\cos a_3$ above the shoulder, with $L_1$ = 0.30 m and $L_2$ = 0.25 m. Holding it still takes $m\,g\,\partial z/\partial q_j$ on joint $j$, with $g$ = 9.81 m/s²:

where $s_i = \sin a_i$. The unknowns $\theta = (m,\ m\,d)$ appear linearly, so every held pose gives three equations: shoulder, elbow and wrist (gravity can't turn the base).

Least squares

Held still at pose $q_k$, the motors supply exactly the gravity torques, so the payload's share is $\tau_k - g_\text{model}(q_k)$, where $g_\text{model}$ is arm.gravity_torques, which doesn't know the payload. Stack every pose's $Y(q_k)$ into one tall matrix and every share into one vector: np.linalg.lstsq finds the $\theta$ that fits them best (Foundations: Least squares). Then $d = \theta_2 / \theta_1$.

The program

It glides to six poses, brakes with arm.hold() and reads arm.applied_torques. After your fit it holds a test pose with PD + $g$, first with the arm's model alone and then with your payload added: $g_\text{model}(q) + Y(q)\,\theta$.

Your task

Write payload_regressor(q) and fit_payload(qs, taus). Your mass must be within 3 % of the truth, and with it in the model the test pose must hold within 0.2°.