Transform Functions

  The simulator provides a set of functions to manipulate points and geometric transforms. These functions can be useful to create inputs for the robot functions described before and to manipulate its outputs.

A point is an array of three doubles that represent the X, Y and Z coordinates of the point. Points are used, for instance, to describe leg, traces and landmarks positions. No special point type is defined since it is just a simple array. By its side, transforms are matrix for four rows and four columns. A transform can represent an arbitrary translation or rotation or combinations of them. The type t_transform can be used to define transformation variables.

Some constants are defined to easy the definition and manipulation of points and transforms:

• DIM_R3, DOF_R3: They refer to the dimensions of a point in R3 (three) and the DOF in R3 (six). DIM_R3 can be used to define points:
  double point_variable[DIM_R3];
  
    
• TX, TY, TZ, RX, RY, RZ: Used to identify each one of the six dofs of R3.        
• M_PI, M_PI_2: $\pi$ and $\pi/2$.  
• DEG2RAD, RAD2DEG: Radians in a degree and degrees in a radiant respectively. They can be used to change from degrees to radians and from radians to degrees with a simple multiplication.   
• AXIS_X, AXIS_Y, AXIS_Z, AXIS_H: They refer to axis X, Y and Z as well as to the homogeneous coordinate.     

The functions to manipulate transforms are described below.

void trs_identity (transform *t)

 

Returns the identity matrix:

$$Id=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

void trs_copy (transform *to,transform *td)

 

Sets td equal to to: td $\leftarrow$ to.

void trs_tx (double tx,transform *t)

 

Returns a matrix that represents a translation along the X axis:

$$Tx(tx)=\begin{bmatrix} 1 & 0 & 0 & tx \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

void trs_ty (double ty,transform *t)

 

Returns a matrix that represents a translation along the Y axis:

$$Ty(ty)=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & ty \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

void trs_tz (double tz,transform *t)

 

Returns a matrix that represents a translation along the Z axis:

$$Tz(tz)=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & tz \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

void trs_txy (double tx,double ty, double tz,transform *t)

 

Returns a matrix that represents a translation along axes X, Y, and Z:

$$Txyz(tx,ty,tz)=\begin{bmatrix} 1 & 0 & 0 & tx \\ 0 & 1 & 0 & ty \\ 0 & 0 & 1 & tz \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

void trs_rx (double rx,transform *t)

 

Returns a matrix that represents a rotation around the X axis:

$$Rx(rx)=\begin{bmatrix} 1 & 0 & 0 & 0 \\ 0 & \cos rx & -\sin rx & 0 \\ 0 & \sin rx & \cos rx & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

rx is given in radians. As usual, positive angles represent an anti-clockwise rotations (seen from the positive side of the axis).

void trs_ry (double ry,transform *t)

 

Returns a matrix that represents a rotation around the Y axis:

$$Ry(ry)=\begin{bmatrix} \cos ry & 0 & \sin ry & 0 \\ 0 & 1 & 0 & 0 \\ -\sin ry & 0 & \cos ry & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

rx is given in radians. As usual, positive angles represent an anti-clockwise rotations (seen from the positive side of the axis).

void trs_rz (double rz,transform *t)

 

Returns a matrix that represents a rotation around the Y axis:

$$Rz(rz)=\begin{bmatrix} \cos rz & -\sin rz & 0 & 0 \\ \sin rz & \cos rz & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

rx is given in radians. As usual, positive angles represent an anti-clockwise rotations (seen from the positive side of the axis).

void trs_symmetry (boolean sx, boolean sy, boolean sz, transform *t)

 

Returns

$$S(sx,sy,sz)=\begin{bmatrix} sign(sx) & 0 & 0 & 0 \\ 0 & sign... ... & 0 & 0 \\ 0 & 0 & sign(sz) & 0 \\ 0 & 0 & 0 & 1 \end{bmatrix}$$

where the function sign(a) returns -1 if a is TRUE and -1 if not. This transformation is useful to perform a symmetry of a point with respect to one or more axis.

void trs_create (unsigned int dof_R3,double v,transform *t)

 

Returns a transform to perform a movement in the given dof of R3 of size v. This function is just a high level interface to call the trs_tx, trs_ty, trs_tz, trs_rx, trs_ry, and trs_rz routines described before.

void trs_product (transform *t1,transform *t2,transform *t3)

 

Performs the product of two transformations: t3 $\leftarrow$ t1 t2. The effect of the transform t3 is the concatenation of the effects of t2 and t1 (in this order).

void trs_add (transform *t1,transform *t2,transform *t3)

 

Adds two transformations: t3 $\leftarrow$ t1 + t2.

void trs_transpose (transform *t,transform *tt)

 

Returns the transpose of a transformation matrix: tt $\leftarrow$ t^T. This function is specially useful since graphical packages uses transformations matrix that are the transposed of the matrixes as defined here.

void trs_inverse (transform *t,transform *ti)

 

Returns the inverse of a transformation matrix: ti $\leftarrow$ t^-1.

void trs_sequence (double tx ty tz rx ry rx, transform *t)

 

Performs the product:

$$CS=Rz(rx)\: Ry(ry) \:Rz(rz)\: Txyz(tx,ty,tz)$$

This transformation concatenates a given set of movements in the six dof of R3.

void trs_parallel (double tx ty tz rx ry rx, transform *t)

 

The above described sequential combination of transformations (CS) apply a given transformation on the frame of reference resulting from the already applied movements. This can result problematic if all movements have been calculated in parallel, in the same frame of reference. The parallel combination of transformations presented here avoids this problem.

Each transformation t produces a variation var_t over a given point p:

$$p_{new} = t\: p = p + var_t \:p= (id + var_t)\: p$$

So t=id+var_t and var_t=t-id. The accumulation of the variations proposed by some transformations $t_1, \ldots, t_n$ is:

$$accumulated\_var = \sum_{i=1}^{n}{var_{t_i}}=\sum_{i=1}^{n}{t_i}-n\: id$$

And when this accumulated variation is applied to a given point we get:
$$\begin{align*} p_{new} = & p + accumulated\_var\: p = p + (\sum_{i=1}^{n}{t_i}-n... ..._{i=1}^{n}{t_i}-n \:id)\: p = ( \sum_{i=1}^{n}{t_i}- (n-1) \:id) \:p\end{align*}$$
Consequently, the parallel combination of transformations performs the addition:

CP=Txyz(tx,ty,tz)+Rx(rx)+Ry(ry)+Rz(rz)- 3 Id

void trs_apply (transform *t, double *pi, double *ps)

 

Performs the product

$$\begin{bmatrix} ps_x\\ ps_y\\ ps_z\\ 1\end{bmatrix}=t\begin{bmatrix} pi_x\\ pi_y\\ pi_z\\ 1\end{bmatrix}$$

Observe that ps and pi only have 3 values and that the homogeneous coordinate (1) is internally added when necessary.

void trs_get_translation (double *trans,transform *t)

 

Returns the three first rows of the fourth column of the matrix t. This is equivalent to execute trs_apply(t,<0,0,0>,trans).

void trs_get_axis (unsigned int i,double *axis,transform *t)

 

Returns the three first rows of the i-th column of the matrix t. This is equivalent to execute trs_apply(t,i,trans) where i has a 1 in the i-th position and zeros in the rest of positions.

void trs_print (FILE *f,transform *t)

 

Writes the matrix t in the file f.

void trs_printT (FILE *f,transform *t)

 

Writes the transposition of the matrix t in the file f.