Forward kinematics

Where is my gripper?

Encoders tell a robot its joint angles. To grab something, though, it needs to know where the gripper is in the world. The map from joint angles to gripper position is called forward kinematics (FK):

$$\mathbf{p} = \operatorname{FK}(q)$$

Deriving FK for this arm

First look at the arm from the side, in the vertical plane it swings in. The three pitch joints add up. Each link's absolute tilt from vertical is the sum of the joints before it:

$$\alpha_1 = q_1, \qquad \alpha_2 = q_1 + q_2, \qquad \alpha_3 = q_1 + q_2 + q_3$$

Each link then adds a vector of its own length at its tilt. That gives the horizontal reach $r$ and the height $z$:

$$\begin{aligned} r &= L_1\sin\alpha_1 + L_2\sin\alpha_2 + L_3\sin\alpha_3 \\ z &= h_0 + L_1\cos\alpha_1 + L_2\cos\alpha_2 + L_3\cos\alpha_3 \end{aligned}$$

Now look from above. The base yaw $q_0$ swings that whole plane around the vertical axis:

$$x = r\cos q_0, \qquad y = r\sin q_0$$

The link lengths are in arm.links: h0 is the table to the shoulder, L1 is the upper arm, L2 is the forearm, and L3 is the wrist to the point between the fingertips.

Your task

Implement forward_kinematics(q) so it returns np.array([x, y, z]).

The starter program moves the arm through a few poses. At each pose it draws your prediction as a pink dot and prints it next to arm.ee_position(), which is the simulator's ground truth. When they agree, the dot sits right between the fingertips.

The grader then tries your function on 20 random configurations.