ChronoEngine:Demo coords

From ChronoWiki

Jump to: navigation, search

Tutorial on how to perform 3D coordinate manipulation (rotation and translations of points, frames, etc.) thank to C++ objects and custom operators introduced in Chrono::Engine.

The following are excerpts from the demo_coords.cpp file, showing the main features of Chrono::Engine in the context of coordinate transformations.

You can use plain matrix algebra, or ChTrasform<> or ChChCoordsys<> or ChFrame<> functions to transform points from/to local coordinates in 3D, in ascending complexity and capabilities.

We recall the following useful concepts:

  • a ChVector<> represents a point in 3D space, as \mathbf{p}=\{p_x,p_y,p_z\}
  • a ChQuaternion<> is used to represent rotations in 3D space, as \mathbf{q}=\{q_0,q_1,q_2,q_3\}
    • a real unit quaternion \mathbf{q}=\{1,0,0,0\} represents no rotation
    • given a rotation αu about a generic unit vector \mathbf{u}, the corresponding quaternion is \mathbf{q}=\left\{
\begin{array}{c}
\cos(\alpha_u / 2)\\
{u}_x \sin(\alpha_u / 2)\\
{u}_y \sin(\alpha_u / 2)\\
{u}_z \sin(\alpha_u / 2)
\end{array}
\right\}
  • also a ChMatrix33<> can represent rotations \mathbf{A} in 3D space
    • rotation matrices are orthogonal, \mathbf{A} \in \mathsf{SO}(3), so \mathbf{A}^{-1} = \mathbf{A}^{t}
    • there are Chrono::Engine functions that allow to convert a quaternion into a 3x3 matrix \mathbf{q} \mapsto \mathbf{A} and viceversa \mathbf{A} \mapsto \mathbf{q}
  • a ChCoordsys<> represents a coordinate system in 3D space, so it embeds both a vector (coordinate translation \mathbf{p}) and a quaternion (coordinate rotation \mathbf{q}), \mathbf{c}=\{\mathbf{p},\mathbf{q}\}
  • a ChFrame<> represents a coordinate system in 3D space like a ChCoordsys<>, but includes more advanced features.
  • a ChFrameMoving<> represents a coordinate system in 3D space, like a ChFrame<>, but it stores also information about the speed and accelerations of the frame: \mathbf{c}=\{\mathbf{p},\mathbf{q},\dot\mathbf{p}, \dot\mathbf{q}, \ddot\mathbf{p}, \ddot\mathbf{q}\}


Let's begin by creating some of these objects.


// Define a  POINT  to be transformed, expressed in
 // local frame coordinate.
ChVector<> mvect1(2,3,4);

 // Define a vector representin the TRASLATION of the frame
 // respect to absolute (world) coordinates.
ChVector<>  vtraslA(5,6,7);

 // Define a quaternion representing the ROTATION of the frame
 // respect to absolute (world) coordinates. Must be normalized.
ChQuaternion<> qrotA(1, 3, 4, 5);
qrotA.Normalize();

 // ..Also create a 3x3 rotation matrix [A] from the quaternion
 // (at any time you can use mrotA.Set_A_quaternion(qrotA) );
ChMatrix33<> mrotA(qrotA);

 // ..Also create a ChCoordsys<>tem object, representing both
 // translation and rotation.
ChCoordsys<> csysA (vtraslA, qrotA);

 // Resulting (transformed) vectors will go here:
ChVector<> mvect2;
ChVector<> mvect3;

OK!!! Now we are ready to perform the transformation, like in linear algebra formula \mathbf{v}'=\mathbf{p}+\mathbf{A}\mathbf{v}, so that we will obtain the coordinates of mvect1 in absolute coordinates.

This can be achieved in many ways. Let's see them.

// TRASFORM USING ROTATION MATRIX AND LINEAR ALGEBRA

mvect2 = vtraslA + mrotA*mvect1;	// like:  v2 = t + [A]*v1


 // TRASFORM USING QUATERNION ROTATION

mvect2 = vtraslA + qrotA.Rotate(mvect1);


 // TRASFORM USING THE ChTrasform STATIC METHODS

mvect2 = ChTrasform<>::TrasformLocalToParent(mvect1, vtraslA, mrotA);  // or..

mvect2 = ChTrasform<>::TrasformLocalToParent(mvect1, vtraslA, qrotA);


 // TRASFORM USING A ChCoordys OBJECT

mvect2 = csysA.TrasformLocalToParent(mvect1);


 // TRASFORM USING A ChFrame OBJECT

ChFrame<> mframeA(vtraslA, qrotA);  // or ChFrame<> mframeA(vtraslA, mrotA);

mvect2 = mframeA.TrasformLocalToParent(mvect1); // using TrasformLocalToParent()

mvect2 = mvect1 >> mframeA;                     // or using the >> operator

mvect2 = mframeA * mvect1;                      // or using the * operator

Now perform transformations in a chain of frames, in sequence. This means that one can have a concatenation of coordinate frames, such as in a robot arm where for each joint one has a coordsystem. Thank to operator overloading, these concatenated transformations are quite easy.

ChVector<>	   v10 (5,6,7);
ChQuaternion<> q10 (1, 3, 4, 5);	q10.Normalize();
ChMatrix33<>   m10 (q10);

ChVector<>	   v21 (4,1,3);
ChQuaternion<> q21 (3, 2, 1, 5);	q21.Normalize();
ChMatrix33<>   m21 (q21);

ChVector<>	   v32 (1,5,1);
ChQuaternion<> q32 (4, 1, 3, 1);	q32.Normalize();
ChMatrix33<>   m32 (q32);

 // ...with linear algebra:

mvect3 =  v10 + m10 * (v21 + m21 * (v32 + m32 * mvect1));


 // ...with ChFrame '>>' operator or "*" operator
 // is by far much simplier!

ChFrame<> f10 (v10, q10);
ChFrame<> f21 (v21, q21);
ChFrame<> f32 (v32, q32);

mvect3 = mvect1 >> f32 >> f21 >> f10;   // with the >> left-to-right concatenation

mvect3 =  f10 * f21 * f32 * mvect1;     // or with the * right-to-left concatenation

ChFrame<> tempf(f10 * f21 * f32);       // or other example
mvect3 = tempf * mvect1;

Not only vectors, but also frames can be transformed with ">>" or "*" operators.

ChFrame<> f_3 (mvect1);
ChFrame<> f_0;

f_0 = f_3 >> f32 >> f21 >> f10;  // with the >> left-to-right concatenation

f_0 = f10 *  f21 *  f32 *  f_3;  // or with the * right-to-left concatenation

Now test inverse transformations too. From the low-level to the higher level methods, here are some ways to accomplish this.

// TRASFORM USING ROTATION MATRIX AND LINEAR ALGEBRA


mvect1 = mrotA.MatrT_x_Vect(mvect2 - vtraslA);		 // like:  v1 = [A]'*(v2-t)


 // TRASFORM USING QUATERNION ROTATION

mvect1 = qrotA.RotateBack(mvect2 - vtraslA);


 // TRASFORM USING THE ChTrasform STATIC METHODS

mvect1 = ChTrasform<>::TrasformParentToLocal(mvect2, vtraslA, mrotA);  // or..

mvect1 = ChTrasform<>::TrasformParentToLocal(mvect2, vtraslA, qrotA);


 // TRASFORM USING A ChCoordys OBJECT

mvect1 = csysA.TrasformParentToLocal(mvect2);


 // TRASFORM USING A ChFrame OBJECT

mvect1 = mframeA.TrasformParentToLocal(mvect2);  // with the  TrasformParentToLocal()

mvect1 = mvect2 >> mframeA.GetInverse(); // or with the >> left-to-right concatenation

mvect1 = mframeA.GetInverse() * mvect2;  // or with the * right-to-left concatenation

mvect1 = mframeA / mvect2;

ChFrame<> mframeAinv (mframeA);
mframeAinv.Invert();
mvect1 = mframeAinv * mvect2;



 // ... also for inverting chain of transformations...
 //  The following are equivalent:

//mvect3 =  f10 * f21 * f32 * mvect1;				// direct transf..

mvect1 =  (f10 * f21 * f32).GetInverse() * mvect3;	// inverse transf.

mvect1 =  f32.GetInverse() * f21.GetInverse() * f10.GetInverse() * mvect3;

mvect1 =  mvect3 >> (f32 >> f21 >> f10).GetInverse();

mvect1 =  mvect3 >> f10.GetInverse() >> f21.GetInverse() >> f32.GetInverse();
Personal tools