| [eigen] Eigen and rigid body simulation |
[ Thread Index |
Date Index
| More lists.tuxfamily.org/eigen Archives
]
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