Kinematics and Dynamics for Robotics

This library defines an interface for a pose (position and orientation) of a rigid body or a (displacement) frame to enable different representations of a pose (homogeneous transformation, screw motion, etc.).

Generic Interface

The class kindr::PoseBase serves as an interface for a pose of a rigid body, i.e. the position and orientation of a rigid body. All types of representations of a pose, such as homogeneous transformations and screw motions, are derived from this base class.


Homogeneous Transformation

$\boxed{\begin{bmatrix}{}_I\mathbf{r}_{I\!P} \\ 1 \end{bmatrix} = \mathbf{T}_{I\!B} \begin{bmatrix}{}_B\mathbf{r}_{B\!P}\\ 1 \end{bmatrix}, \quad \mathbf{T}_{I\!B} = \begin{bmatrix} \mathbf{C}_{I\!B} & {}_I\mathbf{r}_{I\!B} \\ 0^T & 1 \\ \end{bmatrix}, \quad \mathbf{T}_{I\!B}^{-1} =\mathbf{T}_{B\!I} = \begin{bmatrix} \mathbf{C}_{I\!B}^T & -\mathbf{C}_{I\!B}^T {}_I\mathbf{r}_{I\!B} \\ 0^T & 1 \\ \end{bmatrix}}$

The following typedefs are provided for convenience:

Create a Homogeneous Transformation

Position3D position;
HomTransformQuatD transform1; // identity tranformation
HomTransformQuatD transform2(position, rotation); // create with position and rotation

Create a Homogeneous Transformation

Eigen::Matrix<double, 4, 4> matrix = transform1.getTransformationMatrix(); // gets the 4x4 transformation matrix
transform1.setIdentity(); // sets it to identity
transform2.invert(); // invert transformation
Eigen::Vector3d vector_transform = transform2.rotate(Eigen::Vector3d(1.0,2.0,3.0)); // tranform a vector
Eigen::Vector3d vector_invtransform = transform2.inverseTransform(Eigen::Vector3d(1.0,2.0,3.0)); // reverse tranform a vector
HomTransformQuatD transform3 = transform2*transform1; // composition of transformations