Forward kinematics

What it computes

Forward kinematics (FK) maps joint angles $q$ to the pose of the gripper: where it is and which way it points. Every other arm algorithm (IK, Jacobians, planning, control) calls it.

Homogeneous transforms

A pose is a $4 \times 4$ matrix that holds a rotation $R$ and a position $p$:

$$T = \begin{bmatrix} R & p \\ 0\;0\;0 & 1 \end{bmatrix}$$

Multiplying two transforms applies one after the other, each in the frame the previous one left you in. So FK of a serial arm is a product: one rotation per joint, and one translation per link.

This arm's chain

Start at the base on the table. The base joint turns about $z$, and each pitch joint turns about the current $y$ axis. Every link runs along the current $z$ axis:

$$T(q) = R_z(q_0)\; T_z(h_0)\; R_y(q_1)\; T_z(L_1)\; R_y(q_2)\; T_z(L_2)\; R_y(q_3)\; T_z(L_3)$$

	$h_0$	$L_1$	$L_2$	$L_3$
metres	0.12	0.30	0.25	0.12

$$R_z(a) = \begin{bmatrix} \cos a & -\sin a & 0 & 0 \\ \sin a & \cos a & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}, \quad R_y(a) = \begin{bmatrix} \cos a & 0 & \sin a & 0 \\ 0 & 1 & 0 & 0 \\ -\sin a & 0 & \cos a & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

The last column of $T$ is the grasp point, and the third column of $R$ is the direction the gripper points.

See it in context

Pick & Place step 2 derives the same position with trigonometry. Transforms scale to any arm, and they give you the orientation for free.

Your task

Implement rot_z(a), rot_y(a) and fk(q), which returns the $4 \times 4$ matrix $T(q)$. The program then moves through three poses and draws your gripper frame at each one: x red, y green, z blue. The grader compares fk with the simulator on 30 random poses.