Exercise $4$: Homogeneous Transformations

Transformations between coordinate frames play an important role in robotics.

The $2$D pose of a robot w$.$r$.$t$.$ a global coordinate frame is commonly written as $=(x,y,{)}^T$ where $(x,y)$ denotes its position in the $xy-$plane and $$ its orientation. The homogeneous transformation matrix that represents a pose $x=(x,y,{)}^T$ w$.$r$.$t$.$ to the origin $(0,0,{)}^T$ of the global coordinate system is given by

$T=\left[\begin{array}{ccc}cos& -sin& x\\ sin& cos& y\\ 0& 0& 1\end{array}\right]$

a$)$ Implement the homogeneous transformation matrix as a function in Python. The function takes $3$ arguments: $x,y$ and $.$

Returns a $33$ homogeneous transformation matrix.

b$)$ While being at pose $x_1=(5,6,30{)}^T,$ the robot senses a landmark $\frac{?}{?}$ at position $(3,4)$ w$.$r$.$t$.$ to its local frame.

Use $T$ to calculate the coordinates of I w$.$r$.$t$.$ the global frame using NumPy.

c$)$ Now imagine that you are given the landmark's coordinates w$.$r$.$t$.$ the global frame. Calculate the coordinates that will be sensed in its local frame? That is$,$ show that the landmark is at coordinates $x=3$ and $y=4$ w$.$r$.$t$.$ the robot.