[eigen] Eigen and rigid body simulation

```Hi,

```
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
---------------

```
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
------

```
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
-----

```
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
------

```
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
------------

```
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
----------

```
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.
```
reference:
==========

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/