Force, torque & gravity

Torque: a turning force

A door swings easily from its handle, hardly at all near its hinges. A force $F$ (newtons, N) acting a perpendicular distance $d$ (metres) from an axis makes a torque

$$\tau = F\,d$$

$\tau$ (tau) is in newton-metres (N·m); $d$ is the lever arm. Here nothing holds the arm up unless your torques do.

Gravity on the arm

Gravity pulls each link down with its weight $m g$, where $m$ is its mass (kg) and $g = 9.81$ m/s². The weight acts as if it all hung from one point, the link's centre of mass. It pulls straight down, so only horizontal distances are lever arms.

A joint carries every link beyond it. To hold them still its motor must give

$$\tau_j = -g \sum_k m_k\,(x_k - x_j)$$

where $\sum_k$ adds up over every link $k$ the joint carries, $m_k$ is that link's mass, $x_k$ the horizontal position of its centre of mass and $x_j$ that of the joint, all measured forwards from the shoulder. Gravity tips a mass in front of the joint forwards, so the motor pushes back: hence the minus sign.

Finding the positions

The shoulder, elbow and wrist angles $q_1, q_2, q_3$ are measured from straight up and add along the arm, so the links tilt by $a_1 = q_1$, $a_2 = q_1 + q_2$ and $a_3 = q_1 + q_2 + q_3$. A length $L$ at tilt $a$ reaches $L \sin a$ forwards. So the elbow is at $x_e = L_1 \sin a_1$, the wrist at $x_w = x_e + L_2 \sin a_2$, and the forearm's centre of mass at $x_e + c_2 \sin a_2$, with $c_2$ from the table.

Link	Mass $m$	Centre of mass $c$ from its joint
upper arm, $L_1 = 0.30$ m	0.6032 kg	0.0968 m
forearm, $L_2 = 0.25$ m	0.3560 kg	0.0791 m
wrist + hand	0.1519 kg	0.0295 m
payload	0.3 kg	0.06 m past the wrist

The payload is one more mass: every joint that carries it needs its weight times its lever arm too.

The PID control problem meets this torque without a model: its I term has to find it.

Your task

Write gravity_torques(q): the torques [0, τ1, τ2, τ3] that hold the arm still at q (gravity can't turn the base: its axis is vertical). The program glides to three poses and holds each for 1 s with only your torques. The arm must drift less than 1°, and your function must match the simulator within 2 % on random poses.