Robot control part 1: Forward transformation matrices

Operational Space

E.g. this 3 DOF arm with 2 joints

System state q is best represented by joint angle space made up by q_1 and q_2

$q = [q_1, q_2]$

But this is usually not that helpful, given that we want to command the robot to a certain x, y position instead of a certain angle most of the time!!

While the left case as such might be quite obvious, you can see below how this can get complicated real fast

Changing reference frames

Rotation

Say we have a point in reference frame 1 and want to find the corrdinates in frame 0

We can calculate $x_0 = cos(q)p_{x_1} + cos(90+q)p_{y_1}$, which can be rewritten as $cos(q)p_{x_1} -sin(q)p_{y_1}$

We can find $y_0 = sin(q)p_{x_1} + cos(q)p_{y_1}$ in a similar fashion.

In rotation matrix form, where $^1_0\textbf{R}$ is the rotation matrix,

$$ ^1_0\textbf{R} \; \textbf{p}1 = \left[ \begin{array}{cc} cos(q_0) & -sin(q_0) \\ sin(q_0) & cos(q_0) \end{array} \right] \left[ \begin{array}{c} p{x_1} \\ p_{y_1} \end{array} \right] $$

$^1_0\textbf{R}$ denotes that a multiplication will transform a pointn from reference frame 1 to 0

The above example is 2 dimensional.

When working with 3 dimensional, we need to extend the matrix 1 column and 1 row more

Rotations around z axis looks most similar:

$$ \left[\begin{array}{cccc} \textrm{cos}(q_i) & -\textrm{sin}(q_i) & 0 & 0 \\ \textrm{sin}(q_i) & \textrm{cos}(q_i) & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1\end{array}\right] $$