Math - SemRoCo/giskardpy GitHub Wiki

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