Re: [eigen] Eigen and rigid body simulation

[ Thread Index | Date Index | More Archives ]


    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 :

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> {
 typedef LieAlgebra<someClass> Algebra;

 LieGroup<G> inverse() const;
 LieGroup<G> Identity();
 Algebra log() const;
 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> > {
 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);}
 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

Mail converted by MHonArc 2.6.19+