Re: [eigen] Eigen and rigid body simulation

[Mathieu Gautier <mathieu.gautier@xxxxxx>]

Hi,

>     When I have time enough I'll post a first version of this module. So
>     we can discuss about it.

It took way more time than I first expected. I didn't have time last 
month to work on it.

But, I have finally a first version which build with g++ 4.4 et vc++ 
2008 without warnings. The generated assembly seems fine for simple 
operations (composition and assignation).

The module is in the attached file (lgsm.7z)

You can see a quick (and dirty) usecase in SE3CubicInterpolator.h.
Which is a cubic interpolator in SE(3) (taken from : 
http://en.wikipedia.org/wiki/Spline_%28mathematics%29#Algorithm_for_computing_Clamped_Cubic_Splines)

There's part in this module (I'll have to split them) :

* the first one is about lie groups and algebras. It tries to address 
Maxime wishes and my issues with :
  LieGroup.h, LieAlgebra.h, LieGroup_SO3.h, LieGroup_SE3.h,
  LieAlgebra_so3.h and LieAlgebra_se3.h

* the second one is a set of classes that encapsulates the previous ones 
and provides names and accessors that are commonly used in the robotics 
and rigid body simulation communities with :
  Displacement.h, Twist.h, Wrench.h, Rotation3D.h, AngularVelocity.h
  and Torque.h

Since it's the first version many things may (or have to) change. I'll 
just summarize my requirements and the choices I made (It's quite long 
for a summary :) ).

My requirements :

* Wrap existing eigen class (do not add any methods to the common classes)
* Reuse functions/methods provided by Eigen.
* Mimic the Map<> mecanism to handle array of scalars.
* Easily implement functions that work for any instance of a lie group 
or algebra
* Provide names that can be understood easily for people unfamiliar with 
the Lie group/algebra formalism.

The choices I made :

1) class specialization instead of methods specialization
2) Implementing the Map<> design
3) MatrixBase inheritance
4) Strong typing the dual of the Lie Algebra
5) Providing names for rigid bodies simulation

In details :

1) class specialization instead of method specialization

To address these requirements, we agreed on the previous mail to use a 
wrapper pattern :

template<class G>
LieGroup<G> {
public:
 typedef LieAlgebra<someClass> Algebra;

 LieGroup<G> inverse() const;
 LieGroup<G> Identity();
 Algebra log() const;
protected:
 G m_coeffs;
}

where G is an Eigen object (Matrix, Array, etc.)

The easy way is then to implement the generic definition for inverse, 
log, etc and specializing specifics methods such as Identity. There are 
two problems with this approach.

First, the generic definitions generate a code with poor performance 
and, secondly, we may want to add specific methods or constructors for 
some specialization of LieGroup. So, I choose to specialize LieGroup 
class for the 2 groups I use (SO(3) and SE(3)).

for exemple for SE(3) which is the semi direct product of SO(3) and R^3, 
we want to get the SO(3) part or R^3 part. It will help in the 
implementation of some methods :

template<class Scalar>
LieGroup<Eigen::Array<Scalar, 7, 1> > {
public:
 typedef LieAlgebra<someClass> Algebra;
 typedef LieGroup<...> LieGroupSO3;

 LieGroup<G> inverse() const;
 LieGroup<G> Identity();
 Algebra log() const;

 Map<LieGroupSO3> getSO3Element() {return Map<Map<LieGroupSO3>(m_coeffs);}
protected:
 G m_coeffs;
}

So I introduce a specific accessor for this group. I also wish to 
provide specific constructor for example :

LieGroup<Eigen::Array<Scalar, 7, 1> >(const LieGroupSO3&, const Vector3&).

2) Implementing the Map<> design

To mimic the Map<> design used in Eigen, I have to introduce the notion 
of Base object, plain object and map object, so I have a LieGroupBase 
which defines the methods, a LieGroup which defines some constructors 
and a Map<LieGroup> for the mapping. This leads to a fairly complex 
class prototype for the Base class :

template<class G, class Derived> class LieGroupBase

and the specialization :

template<class Derived>
class LieGroupBase<Eigen::Quaternion<typename 
Eigen::ei_traits<Derived>::Scalar>, Derived>

The first argument of the template indicates the group (SO(3), the group 
of 3D rotation which wraps a quaternion in this case). The second 
argument is used to implement the Map<> concept where the type of 
Derived is either LieGroup<G> or Map<LieGroup<G> >.
This desing seems a little complex and the method prototype are hard to 
read, but I didn't manage to find a simpler solution.

The positive aspect is that Map<LieGroup> and Map<LieAlgebra> are not 
specialized for specific algebras or groups. The constructors of the 
generic classes are enough.

3) MatrixBase inheritance

It's an issue that's bothering me. A Lie algebra is a vector field, so I 
want to reuse the methods provided by Eigen for the vector. To avoid 
many lines of code, I let LieAlgebraBase (and it's dual) to inherit from 
a MatrixBase. Using a MatrixBase instead of a matrix allow me to keep 
the Map<> design.

So a LieAlgebra<> wraps a Matrix or a Map<Matrix> and is a MatrixBase. I 
don't know if it may provoke some issues.

I use a Macro (in utils/macros.h) to add the methods that must be 
implemented in a class inheriting from MatrixBase. I'm not sure if I did 
it correctly. I implemented :

cols() and rows(),
coeffs() and coeffRef(),
packet() and writePacket(),
innerStride() and outerStride()
and data().

4) Strong typing the dual of the Lie Algebra

The dual of the Lie algebra is a plain vector field. I choose to 
introduce a LieAlgebraDual instead of using a Eigen Matrix to provide 
strong typing on some functions (adjointTr for example). This strong 
typing is really usefull when I introduce classes for the rigid body 
simulations. For example a Wrench is the dual of a Twist which is a Lie 
algebra, and I want to call getForce or getTorque on a wrench.

5) Providing names for rigid bodies simulation :

The names I choose may not be perfect (conflict between Rotation3D and 
other Rotation in Eigen or Displacement versus Frame).

Since I want to provide specific accessors and constructors, some 
typedefs are not enough. I have to inherits from the previous class, and 
reimplement the Map<> design. It's led to copy/paste a part of the code 
and replacing LieGroup<> by appropriate names. The objects which don't 
need new constructors or accessor are juste some typedef (see Rotation3D 
or Torque)

--
Mathieu Gautier

Attachment: lgsm.7z
Description: Binary data