Math

Transforms

Giskard uses homogeneous transformation matrices to represent transforms. The 4x4 matrix $_rT_t$ refers to the transformation matrix to transform things from frame $t$ to frame $r$, or the pose for $t$ relative to $r$, with:

$${_rT_t} =\begin{bmatrix} r_{0,0} & r_{0,1} & r_{0,2} & x \\\ r_{1,0} & r_{1,1} & r_{1,2} & y \\\ r_{2,0} & r_{2,1} & r_{2,2} & z \\\ 0 & 0 & 0 & 1 \end{bmatrix}$$

In code please use r_T_t to refer to such matrices.

Rotation matrices

Rotation matrices are also represented as 4x4 matrices, where the translation component is 0.

$${_rR_t} =\begin{bmatrix} r_{0,0} & r_{0,1} & r_{0,2} & 0 \\\ r_{1,0} & r_{1,1} & r_{1,2} & 0 \\\ r_{2,0} & r_{2,1} & r_{2,2} & 0 \\\ 0 & 0 & 0 & 1 \end{bmatrix}$$

In code please use r_R_t.

Points

Points are represented as 4d vectors:

$${_rP_t} =\begin{bmatrix} x \\\ y \\\ z \\\ 1 \end{bmatrix}$$

In code please use r_P_t.

Vectors

Vectors are represented as 4d vectors:

$${_rV_t} =\begin{bmatrix} x \\\ y \\\ z \\\ 0 \end{bmatrix}$$

In code please use r_V_t.

Properties

Using this representation, you can easily combine transforms, vectors and rotations by only making sure that the inner frame names math, e.g.

a_T_c = np.dot(a_T_b, b_T_c)

from giskardpy.utils.math import inverse_frame
c_T_a = inverse_frame(a_T_c)

# for rotation matrices, the inverse is equal to the transpose
a_R_c = np.dot(a_T_b, b_R_c)
c_R_a = a_R_c.T 

# the one at the end of the point makes the translation component of a the transformation matrix has an influence
a_P_c = np.dot(a_T_b, b_P_c) 
# the 0 at the end of vectors makes the translation component of a transformation matrix have no influence
a_V_c = np.dot(a_T_b, b_V_c)

# when subtracting points, the result is a vector. With this notation, the 1's at the end get subtracting to automatically produce a 0.
a_V_error = a_P_p2 - a_P_p1 

# adding vectors to points will keep the 1 at the end. (Adding two points makes no geometric sense.)
a_P_p3 = a_P_p1 + a_V_error