Chaining transforms: forward kinematics

One joint, one link, one matrix

Forward kinematics answers: for joint angles $q = [q_0, q_1, q_2, q_3]$ (base, shoulder, elbow, wrist), where is the gripper and which way does it face? Pick & Place, step 2 found the position with trigonometry. Transforms find both by multiplying.

Walk from the base to the gripper. Each joint turns the frame it is in: the base about $z$, then the shoulder, elbow and wrist about $y$. Each link then moves along the new $z$ axis by its length. Write $R_z(a)$ and $R_y(a)$ for the joint turns and $D(d)$ for a move of $d$ metres along $z$, all as 4×4 transforms:

$$\begin{aligned} T_{\text{world,tool}} = {} & R_z(q_0)\,D(h_0)\;R_y(q_1)\,D(L_1) \\ & R_y(q_2)\,D(L_2)\;R_y(q_3)\,D(L_3) \end{aligned}$$

$h_0$ is the shoulder's height above the table; $L_1$, $L_2$ and $L_3$ are the upper arm, the forearm and the wrist to the fingertips. All four are in arm.links. Each factor acts in the frame the ones before it built, so the product reads left to right like a walk along the arm. Stop partway and you have the frame at that joint.

The result holds both answers: its last column is the gripper's position, and its rotation is step 4's $R_z(q_0)R_y(q_1+q_2+q_3)$.

A sensor on the hand

This step's detector is bolted to the hand, so it moves with the arm. Its pose relative to the tool never changes: that's detector.T_tool_det, $T_{\text{tool,det}}$. Its pose in the world depends on the whole arm:

$$T_{\text{world,det}} = T_{\text{world,tool}}\;T_{\text{tool,det}}$$

The inner "tool"s cancel. A ball the detector sees at $p_{\text{det}}$ is at $T_{\text{world,det}}\,p_{\text{det}}$ in the world. Robots with a camera on the wrist ("eye-in-hand") do this for every image. The detector sees only balls within 60° of its $z$ axis, so the starter first lifts the arm to a look-out pose.

Jacobians, step 2 differentiates this chain to see how each joint moves the gripper.

Your task

1. Complete arm_frames(q): the frames after the shoulder, elbow and wrist joints, then the tool frame. tool_pose(q) returns the last one, and the starter draws them all on the arm.
2. At the look-out pose, combine tool_pose(arm.q) and detector.T_tool_det into the detector's world pose.
3. Convert the blue ball's reading to the world and drop the ball in the cup with pick_and_drop.

detector.pose and world.balls() are switched off here.