Rotations in 3-D

Rotation matrices

In 3-D an orientation is a rotation matrix $R$: a 3×3 array whose columns are a turned frame's $x$, $y$ and $z$ axes, written in world coordinates. Multiplying a vector by $R$ rotates it.

The three basic rotations turn by an angle $a$ about one axis and leave that axis alone:

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

Each is the 2-D rotation matrix placed in the two axes that turn. In $R_y$ the minus sign sits lower left, because turning about $y$ carries $z$ towards $x$.

Order matters

Two rotations in a row multiply: $R = R_1 R_2$. Unlike ordinary numbers, $R_z R_y \ne R_y R_z$ in general. Tip a book forwards and then spin it, or spin it and then tip it: it ends up facing different ways.

The gripper's orientation

The gripper has its own tool frame: $z$ runs along the fingers, $y$ is the direction they close, and $x$ completes the set. The base angle $q_0$ turns it about the vertical, and the shoulder, elbow and wrist angles $q_1$, $q_2$, $q_3$ all tilt it about the same horizontal axis, so they simply add up:

$$R_{\text{tool}} = R_z(q_0)\,R_y(s), \qquad s = q_1 + q_2 + q_3$$

Its third column is where the gripper points: $(\cos q_0 \sin s,\ \sin q_0 \sin s,\ \cos s)$. With $s = \pi$ it points straight down.

Other ways to store a rotation

A rotation has three degrees of freedom, so nine numbers are more than it needs. Euler angles store three angles about set axes; they're compact, but at some orientations two of the axes line up and one angle stops mattering (gimbal lock). Axis–angle stores an axis and how far to turn about it. Quaternions use four numbers and are what most robot software passes around. All of them convert to the same $R$.

Pick & Place, step 6 makes $s = \pi$ to keep the gripper pointing down.

Your task

1. Implement rot_x, rot_y, rot_z and tool_rotation(q). The starter draws your tool frame on the gripper at three poses; blue ($z$) should run along the fingers.
2. Point the gripper along POINT = [0.48, 0.64, -0.6], with the shoulder at 0.4 rad and the elbow at 1.2 rad. Work out the base and wrist angles from the third column.
3. Compare the two orders the starter prints and draws.