Re: [eigen] Eigen and rigid body simulation

[Maxime Tournier <maxime.tournier@xxxxxxxxxxxx>]

On December 2, 2009 03:42:49 pm Gael Guennebaud wrote:
> On Wed, Dec 2, 2009 at 8:19 PM, Maxime Tournier <
> 
> maxime.tournier@xxxxxxxxxxxx> wrote:
> > On December 2, 2009 01:11:49 pm Gael Guennebaud wrote:
> > > On Wed, Dec 2, 2009 at 4:11 PM, Maxime Tournier <
> > >
> > > maxime.tournier@xxxxxxxxxxxx> wrote:
> > > > Wow, lots of questions to answer :)
> > > >
> > > > On Wednesday 02 December 2009 11:31:03 Gael Guennebaud wrote:
> > > > > I also like Maxime's idea, but I'm wondering whether the Lie<G>
> > > > > group
> > > >
> > > > class
> > > >
> > > > > should not be a decorator to G rather than a traits class. The main
> > > >
> > > > raisons
> > > >
> > > > > for that are 1) to make it less tedious to use, 2) the current
> > > > > design of the geometry module already goes to that direction since
> > > > > all our transformation classes already implement inverse, identity
> > > > > and
> > > >
> > > > composition
> > > >
> > > > >  via operator* (e.g., Translation::operator* actually performs a
> > > > > +).
> > > > >
> > > > > So basically, a Lie object should implement the following
> > > > > interface:
> > > > >
> > > > > template<class G>
> > > > >   struct Lie {
> > > > >   typedef some_type Algebra;
> > > > >   typedef some_type CoAlgebra;
> > > > >
> > > > >   static G Identity();
> > > > >   G inverse() const;
> > > > >   G operator*(const G&) const;
> > > > >
> > > > >   G exp() const;
> > > > >   G log() const;
> > > > >  };
> > > >
> > > > yep, you could do it like this, but then you have to take care about
> > > > storage
> > > > in each specialization, and struggle with name clashes introduced by
> > > > operators, exp/log, and so on.
> > >
> > > yes that's why I suggested to use lieExp, lieLog, though that's
> > > obviously not an ideal solution.
> > >
> > > > Also, and this is really important, one *has* to derive from Lie<G>
> > > > in order
> > > > to use the Lie structure. This is a strong constraint, 'cause now
> > > > it's impossible to say that a pair of group elements is actually an
> > > > element
> >
> > of
> >
> > > > some
> > > > product group, unless you find a way to make std::pair<A, B> derive
> >
> > from
> >
> > > > Lie<
> > > > std::pair< A, B > > which is of course impossible.
> > >
> > > Not really, you can still do:
> > >
> > > template<A,B> class Lie<std::pair<A,B> > {
> > > public:
> > >
> > >   Lie operator*(const Lie& other) const { return something using m_pair
> >
> > and
> >
> > > other.m_pair; }
> > >
> > > protected:
> > >   std::pair<A,B>& m_pair; // note that it only stores a ref
> > > };
> > >
> > > and then:
> > >
> > > typedef std::pair<X,Y> MyPair;
> > > MyPair my_pair;
> > >
> > > fast_exp(Lie<MyPair>(my_pair));
> > >
> > > that left your "shiny" hierarchy unchanged ;) Another solution is to
> > > make class Lie<std::pair<A,B> > inherits std::pair<A,B>. This Lie
> >
> > specialization
> >
> > > will behave like a std::pair, but with additional features. The
> > > drawback, is that the user has to declare a Lie<std::pair<A,B>> in the
> > > first
> >
> > place,
> >
> > >  otherwise he will have to pay for an extra copy. And now I remark that
> > >  none of these two solutions are fully satisfactory because they would
> >
> > not
> >
> > >  be consistent with a Lie<Translation> as I described before.
> > >
> > > So to keep something clean and consistent I see two options, the
> > > concept map approach, or a true decorator/wrapper approach (I mean like
> > > the
> >
> > first
> >
> > >  example above for std::pair). Actually if you think about it, these
> > > two approaches mainly differ from a syntaxic point of view. So here is
> > > a new proposal which should make everybody happy:
> > >
> > > We keep the initial concept map approach, and to ease the writing of
> > > algorithms we add a trivial and generic LieWrapper<> class allowing to
> > > temporarily use a Lie element with a nicer API:
> > >
> > > template<class G>
> > > struct LieWrapper {
> > >    typedef Lie<G> L;
> > >
> > >    G inverse() const { return L::inverse(m_element); }
> > >    G operator*(const G& other) const { return
> > > L::comp(m_element,other.element()); }
> > >
> > >    G exp() const { return L::exp()(m_element); }
> > >
> > >    // ...
> > >
> > >   protected:
> > >     G& m_element;
> > > };
> >
> > ok i see
> >
> > > and fast_exp can now be implemented like this:
> > >
> > > template<G>
> > > G fast_exp(const G& _g, int n)
> > > {
> > >   LieWrapper<G> g(_g);
> > >
> > >   if( n == 0 )
> > >     return Lie<G>::Identity();
> > >
> > >   if( n % 2 )
> > >     return x * fast_exp(g*g, n/2);
> > >   else
> > >     return fast_exp( g*g, n/2);
> > > }
> >
> > yes, it seems much nicer indeed. You can even call g.identity() if you
> > want to.
> >
> > > The main drawback compared to my first proposal is that now we cannot
> > > (easily) specialize a function for Lie objects,
> >
> > you could do it with SFINAE by checking the existence of type
> > Lie<G>::Algebra
> > for example, but it's a bit involved when you also have a fallback
> > template,
> > and not just a function that's only valid for Lie groups. See
> > boost::enable_if
> > documentation for more information.
> 
> yes that's why I added "easily" between parenthesizes.
> 
> > I agree: it's not exactly easy.
> >
> > > or write a function which
> > > explicitely only accepts object inheriting Lie<something>...
> >
> > if you call a function:
> >
> > template<class G>
> > void foo(const LieWrap<G>&, int n);
> >
> > with a G element, there should be a conversion from G to LieWrap<G>
> > provided
> > that LieWrap<G> has a constructor that takes a G. All you have to do is
> > make
> > sure LieWrap doesnt have such constructor when for example no
> > Lie<G>::Algebra
> > type is defined.
> >
> > the problem then is that G can no longer be inferred automatically by the
> > compiler in the foo template, so one has to write foo<G>(g, 0); instead
> > of just foo(g, 0); when calling it.
> 
> I was going to tell you that, but I see you already know all the c++
> template subtilities :)
> 
> > maybe this isn't so bad ?
> 

After trying to implement this, it seems we don't need both a concept map and 
a wrapper since one is going to copy the other anyway. So only the wrapper 
seems fine. It's quite convenient to use so far. 

Also I came across a (minor) problem with this approach: what if you need to 
modify the wrapped element ? 

We cannot only wrap lvalues, so we have to have two flavours of LieWrapper 
depending on constness and well, this gets complicated. Or am I missing 
something ?

If not, this means we can only specialize for Lie groups when parameters are 
const (otherwise we need a G& reference). Does not look like a big deal to me.

Best regards,



max.


> indeed.
> 
> gael.
> 
> > max.
> >
> > > New opinions ?
> > >
> > >
> > > gael.
> > >
> > > > This is the very motivation of using "trait classes": it leaves
> >
> > existing
> >
> > > > types
> > > > unchanged. This will make it much easier for people to benefit from
> > > > the framework since all they have to do is to provide a concept_map
> > > > (in c++0x's wording) for the concept Lie, and everything should work.
> > > > They wont have to mess with their oh-so-shiny class hierarchy to make
> > > > it
> >
> > work.
> >
> > > > It'll just stay
> > > > shiny.
> > > >
> > > > Of course, the burden is somehow shifted towards library developpers
> > > > (=us) but
> > > > hey, this is gonna be a template nightmare anyways :-)
> > > >
> > > > > and then, e.g., Translation<S,D> could inherit Lie<Translation<S,D>
> > >
> > >,
> > >
> > > > > NonUniformScaling<S,D> would inherit Lie<NonUniformScaling<S,D> >,
> > > > > Quaternion<S> would inherit Lie<Quaternion<S>>, etc.
> > > > >
> > > > >
> > > > > Each of these specializations of Lie<> would also provide the
> > > > > storage and respective ctors. This way one can still directly
> > > > > instanciate a Lie<Translation<S,D> > if he really wants a "pure"
> > > > > Lie group object, and more importantly, write generic algorithms
> > > > > taking only objects
> > > >
> > > > implementing
> > > >
> > > > > the Lie group concept:
> > > > >
> > > > > template<G>
> > > > > Lie<G> fast_exp(const Lie<G>& x, int n)
> > > > > {
> > > > >   if( n == 0 )
> > > > >     return Lie<G>::Identity();
> > > > >
> > > > >   if( n % 2 )
> > > > >     return x * fast_exp(g*g, n/2);
> > > > >   else
> > > > >     return fast_exp( g*g, n/2);
> > > > > }
> > > > >
> > > > > that is much more readable.
> > > >
> > > > totally agreed.
> > > >
> > > > and let's not even mention those spurious "typename" compiler hints.
> > > >
> > > > > However, the exp/log functions could now become ambiguous,
> > > > > especially for Quaternion. So if you think the above proposal makes
> > > > > sens, we
> >
> > could
> >
> > > > > use lieExp/lieLog to make it more explicit they belong to the Lie
> > > > > algebra.
> > > > >
> > > > >
> > > > > Now I have to acknowledge that your design has so advantages too.
> > > > > In particular it really well separate the Lie algebra, and make it
> > > > > clear
> > > >
> > > > when
> > > >
> > > > > you are using operations belonging to the Lie algebra. In that
> > > > > sense
> >
> > it
> >
> > > > > looks a bit cleaner. Are there any other advantages that I'm
> > > > > missing
> >
> > ?
> >
> > > > in a nutshell: it doesnt touch existing types.
> > > >
> > > > > Also I don't understand why do you use functors for the log/exp
> > > > > functions and not static functions ?
> > > >
> > > > i could have done it with static functions as well (it used to be the
> > > > case actually), but i also happen to derive these functions which is
> >
> > far
> >
> > > > easier with functors: the functors have a .diff method from the
> > > > domain tangent bundle
> > > > to the range tangent bundle.
> > > >
> > > > Actually i found functors a lot simpler to deal with than plain
> > > > functions, once you get used to the (small) extra coding.
> > > >
> > > > Benoit, as for the relationships between this and what Mathieu is
> >
> > doing,
> >
> > > > from
> > > > my point of view it is the very same thing in the end (after all, the
> > > > goal is
> > > > to simulate rigid bodies in both cases right ?), using two different
> > > > approaches.
> > > >
> > > > Mathieu's one is perhaps more user-friendly while mine seems more
> > > > developper
> > > > friendly (at least to me).
> > > >
> > > > For example I don't see why one should introduce (and maintain) a
> > > > Twist class
> > > > since this essentially boils down to a 3-vector, same with Torque
> > > > (3-covector)
> > > > or Wrench (6-covector). All you basically need is an
> > > > adjoint/coadjoint operator on these spaces.
> > > >
> > > >
> > > >
> > > > max.
> > > >
> > > > > gael.
> > > > >
> > > > > On Tue, Dec 1, 2009 at 11:24 PM, Benoit Jacob
> > > >
> > > > <jacob.benoit.1@xxxxxxxxx>wrote:
> > > > > > 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
> > > >
> > > > --
> > > > Maxime Tournier
> > > >
> > > > PhD Student - EVASION Team
> > > > Grenoble Universities / LJK / INRIA Rhône-Alpes
> > > > FRANCE
> > > > Tel: +33 4 76 61 54 45
>