Frames & homogeneous transforms

Frames

Every position is measured from some frame: an origin and three axes. So far everything has been in the world frame, fixed to the robot's base. This step's detector is a sensor on a stand at the corner of the table, with a built-in ball finder. It reports each ball in its own frame: $z$ points out of its lens, $x$ to the right and $y$ down in its image (the usual camera convention). None of those axes lines up with the world's. It draws its frame at the start of every run.

Poses as 4×4 matrices

A frame's pose is a rotation $R$ (its axes, as columns, in world coordinates) and a translation $t$ (where its origin is). A homogeneous transform packs both into one 4×4 matrix:

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

To move a point, append a 1 and multiply:

$$\begin{bmatrix} p_{\text{world}} \\ 1 \end{bmatrix} = T_{\text{world,det}} \begin{bmatrix} p_{\text{det}} \\ 1 \end{bmatrix}$$

which is the same as $p_{\text{world}} = R\,p_{\text{det}} + t$: rotate, then shift, as in step 3, in a single multiplication.

Reading the subscripts

$T_{a,b}$ takes points written in frame $b$ and gives them in frame $a$. Chains work when the inner subscripts match and cancel: $T_{a,b}\,T_{b,c} = T_{a,c}$. detector.pose is $T_{\text{world,det}}$.

Going back

The inverse goes the other way: $T_{b,a} = T_{a,b}^{-1}$. A rotation's inverse is its transpose $R^\top$ (rows and columns swapped), so

$$T^{-1} = \begin{bmatrix} R^\top & -R^\top t \\ 0\ \ 0\ \ 0 & 1 \end{bmatrix}$$

It undoes the shift, then the rotation: $R^\top(p - t)$.

Seeing with a Camera, step 3 turns camera rays into world rays with the same $R$ and $t$.

Your task

1. Implement make_T(R, t), transform(T, p) and invert(T).
2. Read the red ball from detector.detect() and convert it to world coordinates with detector.pose.
3. Draw the world frame and an arrow from the detector to the ball, then put the ball in the cup with the starter's pick_and_drop(p).

world.balls() is switched off: the detector is your only way to see the balls. Its stand shifts a little on every run, and the balls move too.