The homogeneous transformation matrix for 3D bodies
As in the 2D case, a homogeneous transformation matrix can be defined.
For the 3D case, a
matrix is obtained that performs the
rotation given by
, followed by a translation
given by
. The result is

(3.50) 
Once again, the order of operations is critical. The matrix in
(3.50) represents the following sequence of
transformations:
1. Roll by 

3. Yaw by 
2. Pitch by 

4. Translate by
. 
The robot primitives can be transformed to yield
. A 3D rigid body that is
capable of translation and rotation therefore has six degrees of
freedom.
Steven M LaValle
20120420