[eigen] Eigen and rigid body simulation

[ Thread Index | Date Index | More lists.tuxfamily.org/eigen Archives ]


I plan to use eigen in one of our library to simulate rigid body in 3D. I have several basic elements (Wrench, Twist, Displacement, etc.) based on Eigen matrix and quaternion which are used to manipulate the rigid bodies velocities, forces, positions, etc. I think that this elements could be added to Eigen as a new module, although, they are quite tied to the geometry module. So, if you are interested I can submit a patch.

I still have to write the documentation and the unitary tests.

I have a description of these elements and a remark about the quaternion at the end of the mail. The mail is quite long, but I try to be as claer and detailed as possible.

Solid mechanism module

This module will provide elements of Lie Group and Lie algebra to represents and manipulate positions, velocities (linear and angular velocities) and forces (linear force and torque) of 3D objects.

6 elements :

* Quaternion : representing a 3D Rotation (Eigen quaternion with two additionnal methods)

* Displacement : representing a 6D Position (3D translation and 3D rotation)

* AngularVelocity : represention a 3D Angular velocity
* Twist : representing a 6D Velocity (3D linear velocity and 3D angular velocity)

* Torque	  : representing a 3D Angular Force
* Wrench	  : representing a 6D Force (3D linear force and 3D angular torque)

All elements are designed using the same pattern :

These elements are not directly matrices but may inherit from MatrixBase and store their coefficients as either a Matrix or a Map<Matrix> although they are mathematically in a vector field, something like Quaternion classes in the geometry module.

Each element are implemented through three classes like Quaternion (I skip scalar type and alignement template argument), for instance for Twist:

template<class Derived> class TwistBase;

class Twist: public TwistBase<Twist>;
class Map<Twist> : public TwistBase<Map<Twist> >;

Elements :

AngularVelocity is an element of a 3 dimensions vector field (so(3)) representing an angular velocity of a rigid body, so AngularVelocityBase inherits from MatrixBase. AngularVelocity and Map<AngularVelocity> store their coefficients through Matrix<Scalar,1, 3> or Map<Scalar, 1, 3>.

This class has 5 specific methods :

* AngularVelocity bracket(AngularVelocity ang) const
   - return *this cross ang

* AngularVelocity adjoint(AngularVelocity ang) const
   - return *this  cross ang (it applies the adjoint of *this to ang,
     see adjoint() )

* Torque coadjoint(Torque tor) const
   - return - *this cross tor (it applies the adjoint of *this to ang,
   see coadjoint() )

* Matrix3x3 adjoint() const;
   - return an antisymetric matrix representing the angular velocity
* Matrix3x3 coadjoint() const;
- return the opposite of the antisymetric matrix representing the angular velocity

for an angular velocity with vx, vy, vz coefficients, the adjoint is the 3x3 matrix :
      0 -vz  vy
     vz   0 -vx
    -vy  vx   0

NB: This class is optional, and is only needed for strong typing.


Torque is an element of a 3 dimensions vector field (so*(3)) representing an angular force applied to a rigid body. It has no specific methods, it's only used to give strong typing to adjoint() and coadjoint() function, described in AngularVelocity.

NB: This class is optional, and is only needed for strong typing.

Remark : These two classes Torque and AngularVelocity are optionnal and could be replaced by Matrix<Scalar, 1, 3> if the strong typing is to cumbersome to maintain and if a method returning the antisymmetric matrix associated to a vector of 3 dimensions is added to MatrixBase.


Twist is an element of a 6 dimensions vector field (se(3)) which is an assembly of a angular velocity (AngularVelocity) and a linear velocity (Vector3). Twist is similar to AngularVelocity, since it inherits from MatrixBase and store its coefficents through Matrix or Map<Matrix>. The angular velocity and linear velocity are in fact map objects, so sizeof(Twist<Scalar>) = 6*sizeof(Scalar) :

 inline Map<AngularVelocity<Scalar> > getAngularVelocity(){
    return Map<AngularVelocity<Scalar> >(this->derived().data().data());
 inline VectorBlock<TwistVectorType, 3> getLinearVelocity(){
    return this->derived().data().template start<3>();

where derived().data() return either a Matrix<Scalar, 1,6> or a Map<Matrix<...> >

other method such as intergrator, etc. could be added to this class.


Wrench is an element of a 6 dimensions vector field (se*(3)) which is an assembly of an angular torque (Torque) and a linear force (Vector3). It's very similar to Twist. Force and Torque are also mapped objects.

There is a method power() which takes a twist and a wrench and returns a scalar (in fact the mechanical power associated to the velocity and force) which can be implemented either in Wrench or in Twist.


Displacement is a member of the special euclidian group SE(3) which represents a rigid body position or a direct isometry (translations and rotations). It's an assembly of a Unitary quaternion representing a 3D rotation and a position (vector3). These two elements are also mapped object. The coefficients of Displacement are stored either in an array of 7 Scalars for Displacement and a pointer to an array of Scalar for Map<Displacement>.

There are several methods :

Displacement inverse() const;
  - opposite transformation
Displacement operator* (const Displacement& d) const;
  - transformation composition
Vector3 operator* (const MatrixBase<OtherDerived>& d) const;
  - transformation of a Vector3
Twist log() const;	
  - Twist associated to the Displacement cf. [1]

Twist adjoint(const Twist&) const;	
  - Return the twist express in an other frame through the
  transformation hold in Displacement
Wrench adjointTr(const WrenchBase<OtherDerived>&) const;
  - Similar to adjoint but for Wrench

Matrix<Scalar, 6, 6> adjoint() const;
 - return the adjoint which is a 6x6 matrix. This matrix (which is
 implicitly used in the previously decribed method adjoint(Twist&) is
 used to move a Twist from a frame to another frame.
Matrix<Scalar, 6, 6> adjointInv() const;
 - similar to adjoint() but for wrench

To simplify the explanation, adjoint and coadjoint are used to apply a rotation and a translation. To apply only the translation or the rotation part of the transformation described by Displacement, the following methods are used :

Twist changeBase(const Twist&) const;   <- apply rotation
Wrench changeBase(const Wrench&) const;

Twist changeReductionPoint (const Twist&) const;  <- apply translation
Twist changeReductionPoint (const Wrench&) const;

remarks :
Displacement class could used the Array class when implemented to store its coefficients. Some other methods, such a interpolation, etc. could be added to the Displacement class.

Displacement can also return a transform object or a homogeneous object. It also could be build from these objects. Like Quaternion, Displacement has a compact storage, is efficient to compose and can implement stable interpolation compared to other representations.


Quaternion is relatively similar to the Quaternion class already present in Eigen, but it has to be considered unitary.

there are two methods :

AngularVelocity log() const;
static Quaternion Exp(const AngularVelocity& ang);

these two methods link the rotation represented by the quaternion to the angular velocity. The exponential function, Exp, is different from the exponential defined for a quaternion which is a generalisation for quaternions of the complex exponential. Indeed our exponential return an angular velocity (Vector3) and the classical quaternion exponential return a quaternion.

Actually, we think that the quaternion used to represent a 3D rotation may have to be different from the generic quaternion. Maybe, creating a class Rotation3D like Rotation2D whose coefficients could be stored as a Quaternion and which defines specific functions associated to rotations. For example the operator == is different if the quaternion represent a rotation or if it's "only" a quaternion. Since the unitary quaternion space map the rotation space exaclty two times, this operator must reflect this behavior.


The mathematical background to these elements can be found in :
[1] R. Murray, Z. Li, and S. Sastry, "A mathematical introduction to robotic manipulation", (book) CRC Press, Boca Raton, FL, 1994. (http://www.cds.caltech.edu/~murray/books/MLS/pdf/mls94-complete.pdf)

Mathieu Gautier

Mail converted by MHonArc 2.6.19+ http://listengine.tuxfamily.org/