Differentiating forward kinematics

Exact derivatives

Finite differences need 8 FK calls per Jacobian and are only approximate. You know the FK formula, though, so you can differentiate it by hand. The result is exact and much cheaper.

Recall the FK from Pick & Place, with link tilts $a_1 = q_1$, $a_2 = q_1+q_2$ and $a_3 = q_1+q_2+q_3$:

$$\begin{aligned} r &= L_1\sin a_1 + L_2\sin a_2 + L_3\sin a_3 \\ z &= h_0 + L_1\cos a_1 + L_2\cos a_2 + L_3\cos a_3 \\ x &= r\cos q_0, \qquad y = r\sin q_0 \end{aligned}$$

The base column

Only $x$ and $y$ depend on $q_0$, and $r$ doesn't. Turning the base swings the gripper sideways, faster the further out it is:

$$J_{:,0} = \begin{bmatrix} -r\sin q_0 & r\cos q_0 & 0 \end{bmatrix}^T$$

The pitch columns

$q_1$ appears in all three tilts, $q_2$ in $a_2$ and $a_3$, and $q_3$ only in $a_3$. By the chain rule:

$$\frac{\partial r}{\partial q_1} = L_1\cos a_1 + L_2\cos a_2 + L_3\cos a_3, \quad \frac{\partial r}{\partial q_2} = L_2\cos a_2 + L_3\cos a_3, \quad \frac{\partial r}{\partial q_3} = L_3\cos a_3$$

For $i = 1, 2, 3$ the base rotation just carries these along:

$$\frac{\partial x}{\partial q_i} = \cos q_0\,\frac{\partial r}{\partial q_i}, \qquad \frac{\partial y}{\partial q_i} = \sin q_0\,\frac{\partial r}{\partial q_i}$$

The $z$ row is yours. Differentiate $z$ the same way, remembering that $\frac{d}{da}\cos a = -\sin a$.

Your task

Implement jacobian(q) analytically, without calling arm.fk. The starter includes a working jacobian_fd from Step 1. It prints the largest difference between the two on a few poses, and how long each one takes.

The grader checks every entry of your jacobian to within $10^{-5}$ on 20 random configurations. arm.fk is switched off while it does, so a jacobian that calls it fails.