Re: [eigen] Eigen and rigid body simulation

[Benoit Jacob <jacob.benoit.1@xxxxxxxxx>]

2009/11/30 Maxime Tournier <maxime.tournier@xxxxxxxxxxxx>:
> Hi everyone,
>
> As Mathieu said, I have some generic code that allows one to abstract
> the Lie group structure of a type, so that one can write any Lie group
> algorithm no matter the group. I told Gael about it few days ago, and
> since it could be of interest to others, here's an overview of what I
> did.
>
> The approach somehow follows c++0x's concepts proposal, in that the
> Lie group structure for a type T is encoded in a separate template
> class, Lie<T>, containing all the types and operations that define the
> Lie group structure.
>
> One can describe the Lie group structure for a type T by specializing
> the template Lie for this type, giving the associated types and
> implementing required operations.  Every specialization should conform
> to the same "interface".
>
> Here are two basic examples with only the algebraic structure:
>
> // here is S^3 with quaternion multiplication
> template<class Real>
> struct Lie< Quaternion<Real> > {
>
>  static Quaternion<Real> id() { return Quaternion<Real>::Identity(); }
>  static Quaternion<Real> inv(const Quaternion<Real>& q) { return q.inverse();
> }
>  static Quaternion<Real> comp(const Quaternion<Real>& q1,
>                               const Quaternion<Real>& q2) {
>    return Quaternion<Real>::Identity();
>  }
>
> };
>
> // here is R^3 with addition
> template<>
> struct Lie< Vector3d > {
>
>  static Vector3d id() { return Vector3d::Zero(); }
>  static Vector3d inv(const Vector3d& v) {
>    return -v;
>  }
>  static Vector3d comp(const Vector3d& a,
>                       const Vector3d& b) {
>    return a + b;
>  }
>
> };
>
> You can then write an algorithm for any kind of group like this:
>
> template<class G>
> G fast_exponentiation(const G& g, int n) {
>  if( n == 0 ) {
>    return Lie<G>::id();
>  }
>
>  if( n % 2 ) {
>    return Lie<G>::comp(  fast_exponentiation( Lie<G>::comp(g, g), n/2), g );
>  } else {
>    return fast_exponentiation( Lie<G>::comp(g, g), n/2) );
>  }
>
> }
>
> Now I know it's a bit tedious to write, but:
>
> - No modification to exisiting types is needed whatsoever. This is A
>  Good Thing. No curious inheritance pattern is introduced in
>  particular. Actually, underlying types are completely independant
>  from this new structure. This should make Eigen types developers
>  happy since we're not touching anything in existing Eigen code :-)
>
> - We can quite easily construct new groups from existing ones:
>
> template<class G1, class G2>
> struct Lie< std::pair<G1, G2> > {
>
>  // ...
>
> };
>
> ...which essentialy says that the product of two groups is a group for
> the product law. This is particularly useful when using std::tuple and
> variadic templates, since product-groups are automatically constructed
> at compilation.

I understand, your design seems very good.

> The complete Lie template specification in my code is the following:
>
> template<class G>
> struct Lie {
>  typedef some_type algebra;
>  typedef some_type coalgebra;
>
>  static G id();
>  static G inv(const G&);
>  static G comp(const G&, const G& );
>
>  // default-constructible
>  typedef some_functor exp;
>  typedef some_functor log;
>
>  // G-constructible
>  typedef some_functor ad;
>  typedef some_functor ad_T;
>
> };

OK I understand. ad is inner automorphisms? and ad_T is the inverse?

>
> The only problem with it is the identity for dynamic-sized types, for
> example VectorXd: what should be the size of the result ?

Do you mean as a compile time value or as a runtime value?

At compile time:

Throughout Eigen we use a special value, Dynamic, to mean that. For
example, VectorXd is just:
matrix<double,Dynamic,1>.
Is that what you were looking for?

At run time: in the dynamic-size case, you could require id() to take
a size argument. So id(void) would use a static assertion that the
size is fixed at compile time. See what is already done in
MatrixBase::Identity().

> So maybe it
> should accept some optional contextual hint for the dimension.

in Eigen, all types inheriting MatrixBase (hence VectorXd) have a
member enum SizeAtCompileTime telling the number of coefficients
(rows*cols)

so:
   VectorXd::SizeAtCompileTime == Dynamic

Other types could be harmonized if that is useful to you, e.g. we
could set Quaternion::SizeAtCompileTime==4 if that's useful.


> Once this is done, it is quite straightforward to implement the
> tangent bundle, then differentiable functions, and then automatically
> compute differentials for complicated function compositions using
> expression templates. When dealing with complicated functions on
> complicated non-flat spaces, this is a real joy to use.
>
> I have the Lie group structure implemented for quaternions, Rohit
> Garg's rigid transforms, row/column vectors, as well as any std::tuple
> combination of these. Based on this I have code that can interpolate
> (linear/~spline), compute geometric mean, and do statistical analysis
> while preserving the Lie group (hence manifold) structure.
>
> So if you need to spline interpolate homographies
> in a non degenerate way for example, all you have to do is to
> implement the Lie group structure for homographies and the algorithm
> is already coded for you :-)
>
> So my question is: would anyone be interested in me porting all/parts
> of this stuff into Eigen ? If so where/how should I start ?

It seems that this would be nice content for a new module, "Lie". Does
that sound good?

You would start by forking the eigen repository at bitbucket, and
creating a new Lie module in unsupported/.

If you prefer it's always possible to develop directly in our
repository, it's just that forking gives you that extra flexibility.

I would also like to understand how this interplays with Mathieu's
proposal. If you create a new Lie module, should Mathieu's classes go
into it? Or should only Torque/Twist/Wrench go into it while "so(3)"
is considered so important for geometry that it goes into the Geometry
module? If Mathieu's stuff gets split between Geometry and Lie, does
that mean that Lie should #include Geometry? Is that OK? Is that
something you wanted to do anyway?

Benoit