Re: [eigen] Eigen and rigid body simulation

[Rohit Garg <rpg.314@xxxxxxxxx>]

Have you looked at using dual quaternion's to implement rigid
transformations. I have the code for them ready and they have the
advantage of providing stable, and smooth interpolation of rigid
transformations.

On Wed, Nov 25, 2009 at 9:15 PM, Mathieu Gautier <mathieu.gautier@xxxxxx> wrote:
> 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
>
>
>



-- 
Rohit Garg

http://rpg-314.blogspot.com/

Senior Undergraduate
Department of Physics
Indian Institute of Technology
Bombay