Velocity kinematics

From positions to velocities

In Pick & Place you used forward kinematics, $p = \mathrm{fk}(q)$, to find the gripper from the joint angles. Controllers often need the next question: if the joints turn at speeds $\dot q$, how fast does the gripper move?

Differentiate FK with the chain rule and a matrix appears, the Jacobian:

$$\dot p = J(q)\,\dot q, \qquad J_{ij} = \frac{\partial p_i}{\partial q_j}$$

The gripper moves in 3-D and the arm has 4 joints, so $J$ is $3\times4$. Column $j$ is the gripper velocity you get when joint $j$ turns at 1 rad/s and the others stay still. It depends on $q$: the same joint speed moves the gripper differently in different poses.

Measuring a column

You don't need calculus to get $J$. Nudge one joint by a small $h$ both ways and see how the gripper moves:

$$J_{:,j} \approx \frac{\mathrm{fk}(q + h\,e_j) - \mathrm{fk}(q - h\,e_j)}{2h}$$

Here $e_j$ is 1 in slot $j$ and 0 elsewhere. This central difference has an error of order $h^2$, so a smaller $h$ is better, up to a point. Make $h$ too small and you subtract two almost equal numbers, and floating-point round-off takes over. $h \approx 10^{-5}$ rad balances the two.

arm.fk(q) is unlocked in this lesson. It returns the grasp point for any q without moving the arm.

Your task

Implement jacobian_fd(q) so it returns a $3\times4$ np.array.

The starter program sweeps every joint with a sine wave. In the Plots tab it compares two gripper speeds: one measured from arm.ee_position(), one predicted as $\lVert J(q)\,\dot q\rVert$ using the joint velocities arm.qd. When your Jacobian is right, the curves lie on top of each other.

The grader compares your function with the exact Jacobian on 20 random configurations.