Recreating my GSoC branch.
[blender.git] / intern / itasc / kdl / frames.hpp
1 /***************************************************************************
2                         frames.hpp `-  description
3                        -------------------------
4     begin                : June 2006
5     copyright            : (C) 2006 Erwin Aertbelien
6     email                : firstname.lastname@mech.kuleuven.be
7
8  History (only major changes)( AUTHOR-Description ) :
9
10  ***************************************************************************
11  *   This library is free software; you can redistribute it and/or         *
12  *   modify it under the terms of the GNU Lesser General Public            *
13  *   License as published by the Free Software Foundation; either          *
14  *   version 2.1 of the License, or (at your option) any later version.    *
15  *                                                                         *
16  *   This library is distributed in the hope that it will be useful,       *
17  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
18  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
19  *   Lesser General Public License for more details.                       *
20  *                                                                         *
21  *   You should have received a copy of the GNU Lesser General Public      *
22  *   License along with this library; if not, write to the Free Software   *
23  *   Foundation, Inc., 51 Franklin Street,                                    *
24  *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
25  *                                                                         *
26  ***************************************************************************/
27
28 /**
29  * \file
30  * \warning
31  *       Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of
32  *          p2=A*B*C*p1
33  *
34  * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS
35  *
36  * \verbatim
37  *      A naming convention of objects of the type defined in this file :
38  *          (1) Frame : F...
39  *              Rotation : R ...
40  *          (2) Twist    : T ...
41  *              Wrench   : W ...
42  *              Vector   : V ...
43  *      This prefix is followed by :
44  *      for category (1) :
45  *          F_A_B : w.r.t. frame A, frame B expressed
46  *          ( each column of F_A_B corresponds to an axis of B,
47  *            expressed w.r.t. frame A )
48  *          in mathematical convention :
49  *                   A
50  *         F_A_B ==    F
51  *                   B
52  *
53  *      for category (2) :
54  *          V_B   : a vector expressed w.r.t. frame B
55  *
56  *      This can also be prepended by a name :
57  *          e.g. : temporaryV_B
58  *
59  *      With this convention one can write :
60  *
61  *      F_A_B = F_B_A.Inverse();
62  *      F_A_C = F_A_B * F_B_C;
63  *      V_B   = F_B_C * V_C;    // both translation and rotation
64  *      V_B   = R_B_C * V_C;    // only rotation
65  * \endverbatim
66  *
67  * \par CONVENTIONS FOR WHEN USED WITH ROBOTS :
68  *
69  * \verbatim
70  *       world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]')
71  *       mp    : represents mounting plate of a robot
72  *               (i.e. everything before MP is constructed by robot manufacturer
73  *                    everything after MP is tool )
74  *       tf    : represents task frame of a robot
75  *               (i.e. frame in which motion and force control is expressed)
76  *       sf    : represents sensor frame of a robot
77  *               (i.e. frame at which the forces measured by the force sensor
78  *               are expressed )
79  *
80  *          Frame F_world_mp=...;
81  *          Frame F_mp_sf(..)
82  *          Frame F_mp_tf(,.)
83  *
84  *          Wrench are measured in sensor frame SF, so one could write :
85  *                Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf );
86  * \endverbatim
87  *
88  * \par CONVENTIONS REGARDING UNITS :
89  *      Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec
90  *
91  * \par Twist and Wrench transformations
92  * 3 different types of transformations do exist for the twists
93  * and wrenches.
94  *
95  * \verbatim
96  *      1) Frame * Twist or Frame * Wrench :
97  *              this transforms both the velocity/force reference point
98  *             and the basis to which the twist/wrench are expressed.
99  *      2) Rotation * Twist or Rotation * Wrench :
100  *              this transforms the basis to which the twist/wrench are
101  *              expressed, but leaves the reference point intact.
102  *      3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB)
103  *              this transforms only the reference point. v is expressed
104  *              in the same base as the twist/wrench and points from the
105  *              old reference point to the new reference point.
106  * \endverbatim
107  *
108  * \par Complexity
109  *  Sometimes the amount of work is given in the documentation
110  *  e.g. 6M+3A means 6 multiplications and 3 additions.
111  *
112  *  \author
113  *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
114  *
115  ****************************************************************************/
116 #ifndef KDL_FRAMES_H
117 #define KDL_FRAMES_H
118
119
120 #include "utilities/kdl-config.h"
121 #include "utilities/utility.h"
122
123 /////////////////////////////////////////////////////////////
124
125 namespace KDL {
126
127
128
129 class Vector;
130 class Rotation;
131 class Frame;
132 class Wrench;
133 class Twist;
134 class Vector2;
135 class Rotation2;
136 class Frame2;
137
138
139
140 /**
141  * \brief A concrete implementation of a 3 dimensional vector class
142  */
143 class Vector
144 {
145 public:
146     double data[3];
147      //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that
148      inline Vector() {data[0]=data[1]=data[2] = 0.0;}
149
150      //! Constructs a vector out of the three values x, y and z
151      inline Vector(double x,double y, double z);
152
153      //! Constructs a vector out of an array of three values x, y and z
154      inline Vector(double* xyz);
155
156      //! Constructs a vector out of an array of three values x, y and z
157      inline Vector(float* xyz);
158
159          //! Assignment operator. The normal copy by value semantics.
160      inline Vector(const Vector& arg);
161
162          //! store vector components in array
163          inline void GetValue(double* xyz) const;
164
165          //! Assignment operator. The normal copy by value semantics.
166      inline Vector& operator = ( const Vector& arg);
167
168      //! Access to elements, range checked when NDEBUG is not set, from 0..2
169      inline double operator()(int index) const;
170
171      //! Access to elements, range checked when NDEBUG is not set, from 0..2
172      inline double& operator() (int index);
173
174          //! Equivalent to double operator()(int index) const
175      double operator[] ( int index ) const
176        {
177          return this->operator() ( index );
178        }
179
180          //! Equivalent to double& operator()(int index)
181      double& operator[] ( int index )
182        {
183          return this->operator() ( index );
184        }
185
186      inline double x() const;
187      inline double y() const;
188      inline double z() const;
189      inline void x(double);
190      inline void y(double);
191      inline void z(double);
192
193      //! Reverses the sign of the Vector object itself
194      inline void ReverseSign();
195
196
197      //! subtracts a vector from the Vector object itself
198      inline Vector& operator-=(const Vector& arg);
199
200
201      //! Adds a vector from the Vector object itself
202      inline Vector& operator +=(const Vector& arg);
203
204      //! Scalar multiplication is defined
205      inline friend Vector operator*(const Vector& lhs,double rhs);
206      //! Scalar multiplication is defined
207      inline friend Vector operator*(double lhs,const Vector& rhs);
208      //! Scalar division is defined
209
210      inline friend Vector operator/(const Vector& lhs,double rhs);
211      inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
212      inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
213      inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
214      inline friend Vector operator-(const Vector& arg);
215      inline friend double dot(const Vector& lhs,const Vector& rhs);
216
217      //! To have a uniform operator to put an element to zero, for scalar values
218      //! and for objects.
219      inline friend void SetToZero(Vector& v);
220
221      //! @return a zero vector
222      inline static Vector Zero();
223
224    /** Normalizes this vector and returns it norm
225         * makes v a unitvector and returns the norm of v.
226         * if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
227         * if this is not good, check the return value of this method.
228         */
229      double Normalize(double eps=epsilon);
230
231      //!    @return the norm of the vector
232      double Norm() const;
233
234
235
236      //! a 3D vector where the 2D vector v is put in the XY plane
237      inline void Set2DXY(const Vector2& v);
238      //! a 3D vector where the 2D vector v is put in the YZ plane
239      inline void Set2DYZ(const Vector2& v);
240      //! a 3D vector where the 2D vector v is put in the ZX plane
241      inline void Set2DZX(const Vector2& v);
242      //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
243      inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);
244
245
246      //! do not use operator == because the definition of Equal(.,.) is slightly
247      //! different.  It compares whether the 2 arguments are equal in an eps-interval
248      inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
249
250          //! return a normalized vector
251          inline friend Vector Normalize(const Vector& a, double eps=epsilon);
252
253          //! The literal equality operator==(), also identical.
254      inline friend bool operator==(const Vector& a,const Vector& b);
255          //! The literal inequality operator!=().
256      inline friend bool operator!=(const Vector& a,const Vector& b);
257
258      friend class Rotation;
259      friend class Frame;
260 };
261
262
263 /**
264   \brief represents rotations in 3 dimensional space.
265
266   This class represents a rotation matrix with the following
267   conventions :
268  \verbatim
269      Suppose V2 = R*V,                                    (1)
270      V is expressed in frame B
271      V2 is expressed in frame A
272      This matrix R consists of 3 collumns [ X,Y,Z ],
273      X,Y, and Z contain the axes of frame B, expressed in frame A
274      Because of linearity expr(1) is valid.
275  \endverbatim
276    This class only represents rotational_interpolation, not translation
277  Two interpretations are possible for rotation angles.
278  * if you rotate with angle around X frame A to have frame B,
279    then the result of SetRotX is equal to frame B expressed wrt A.
280      In code:
281  \verbatim
282       Rotation R;
283       F_A_B = R.SetRotX(angle);
284  \endverbatim
285  * Secondly, if you take the following code :
286  \verbatim
287       Vector p,p2; Rotation R;
288       R.SetRotX(angle);
289       p2 = R*p;
290  \endverbatim
291    then the frame p2 is rotated around X axis with (-angle).
292    Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot
293  \par type
294   Concrete implementation
295 */
296 class Rotation
297 {
298 public:
299     double data[9];
300
301     inline Rotation() {
302                 *this = Rotation::Identity();
303         }
304     inline Rotation(double Xx,double Yx,double Zx,
305                 double Xy,double Yy,double Zy,
306                 double Xz,double Yz,double Zz);
307     inline Rotation(const Vector& x,const Vector& y,const Vector& z);
308     // default copy constructor is sufficient
309
310         inline void setValue(float* oglmat);
311         inline void getValue(float* oglmat) const;
312
313      inline Rotation& operator=(const Rotation& arg);
314
315      //!  Defines a multiplication R*V between a Rotation R and a Vector V.
316      //! Complexity : 9M+6A
317      inline Vector operator*(const Vector& v) const;
318
319      //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
320      inline double& operator()(int i,int j);
321
322      //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
323      inline double operator() (int i,int j) const;
324
325      friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);
326
327      //! Sets the value of *this to its inverse.
328      inline void SetInverse();
329
330      //! Gives back the inverse rotation matrix of *this.
331      inline Rotation Inverse() const;
332
333      //! The same as R.Inverse()*v but more efficient.
334      inline Vector Inverse(const Vector& v) const;
335
336      //! The same as R.Inverse()*arg but more efficient.
337      inline Wrench Inverse(const Wrench& arg) const;
338
339      //! The same as R.Inverse()*arg but more efficient.
340      inline Twist Inverse(const Twist& arg) const;
341
342      //! Gives back an identity rotaton matrix
343      inline static Rotation Identity();
344
345
346 // = Rotations
347     //! The Rot... static functions give the value of the appropriate rotation matrix back.
348     inline static Rotation RotX(double angle);
349     //! The Rot... static functions give the value of the appropriate rotation matrix back.
350     inline static Rotation RotY(double angle);
351     //! The Rot... static functions give the value of the appropriate rotation matrix back.
352     inline static Rotation RotZ(double angle);
353     //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
354     //! DoRot... functions are only defined when they can be executed more efficiently
355     inline void DoRotX(double angle);
356     //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
357     //! DoRot... functions are only defined when they can be executed more efficiently
358     inline void DoRotY(double angle);
359     //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
360     //! DoRot... functions are only defined when they can be executed more efficiently
361     inline void DoRotZ(double angle);
362
363     //! Along an arbitrary axes.  It is not necessary to normalize rotaxis.
364     //! returns identity rotation matrix in the case that the norm of rotaxis
365     //! is to small to be used.
366     // @see Rot2 if you want to handle this error in another way.
367     static Rotation Rot(const Vector& rotaxis,double angle);
368
369         //! Along an arbitrary axes.  rotvec should be normalized.
370     static Rotation Rot2(const Vector& rotvec,double angle);
371         
372         // make sure the matrix is a pure rotation (no scaling)
373         void Ortho();
374
375     //! Returns a vector with the direction of the equiv. axis
376     //! and its norm is angle
377     Vector GetRot() const;
378
379     //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the
380     //! Y axis onto the Matrix Y axis and its norm is angle
381     Vector2 GetXZRot() const;
382
383         /** Returns the rotation angle around the equiv. axis
384          * @param axis the rotation axis is returned in this variable
385          * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
386          *                                         to be +/- Z-axis
387          *               in the case of angle == PI : 2 solutions, positive Z-component
388          *                                            of the axis is choosen.
389          * @result returns the rotation angle (between [0..PI] )
390          */
391         double GetRotAngle(Vector& axis,double eps=epsilon) const;
392
393
394     //! Gives back a rotation matrix specified with EulerZYZ convention :
395     //!  First rotate around Z with alfa,
396     //!  then around the new Y with beta, then around
397     //!  new Z with gamma.
398     static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);
399
400     //! Gives back the EulerZYZ convention description of the rotation matrix :
401     //!  First rotate around Z with alfa,
402     //!  then around the new Y with beta, then around
403     //!  new Z with gamma.
404     //!
405     //! Variables are bound by
406     //!  (-PI <= alfa <= PI),
407     //! (0 <= beta <= PI),
408     //!  (-PI <= alfa <= PI)
409     void GetEulerZYZ(double& alfa,double& beta,double& gamma) const;
410
411
412     //! Sets the value of this object to a rotation specified with RPY convention:
413     //! first rotate around X with roll, then around the
414     //!               old Y with pitch, then around old Z with alfa
415     static Rotation RPY(double roll,double pitch,double yaw);
416
417     //! Gives back a vector in RPY coordinates, variables are bound by
418     //!   -PI <= roll <= PI
419     //!    -PI <= Yaw  <= PI
420     //!   -PI/2 <= PITCH <= PI/2
421     //!
422     //!  convention : first rotate around X with roll, then around the
423     //!               old Y with pitch, then around old Z with alfa
424     void GetRPY(double& roll,double& pitch,double& yaw) const;
425
426
427     //! Gives back a rotation matrix specified with EulerZYX convention :
428     //!  First rotate around Z with alfa,
429     //!  then around the new Y with beta, then around
430     //!  new X with gamma.
431     //!
432     //! closely related to RPY-convention
433     inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
434         return RPY(Gamma,Beta,Alfa);
435     }
436
437     //! GetEulerZYX gets the euler ZYX parameters of a rotation :
438     //!  First rotate around Z with alfa,
439     //!  then around the new Y with beta, then around
440     //!  new X with gamma.
441     //!
442     //! Range of the results of GetEulerZYX :
443     //!   -PI <= alfa <= PI
444     //!    -PI <= gamma <= PI
445     //!   -PI/2 <= beta <= PI/2
446     //!
447     //! Closely related to RPY-convention.
448     inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
449         GetRPY(Gamma,Beta,Alfa);
450     }
451
452      //! Transformation of the base to which the twist is expressed.
453      //! Complexity : 18M+12A
454      //! @see Frame*Twist for a transformation that also transforms
455      //! the velocity reference point.
456      inline Twist operator * (const Twist& arg) const;
457
458      //! Transformation of the base to which the wrench is expressed.
459      //! Complexity : 18M+12A
460      //! @see Frame*Wrench for a transformation that also transforms
461      //! the force reference point.
462      inline Wrench operator * (const Wrench& arg) const;
463
464      //! Access to the underlying unitvectors of the rotation matrix
465      inline Vector UnitX() const {
466          return Vector(data[0],data[3],data[6]);
467      }
468
469      //! Access to the underlying unitvectors of the rotation matrix
470      inline void UnitX(const Vector& X) {
471         data[0] = X(0);
472         data[3] = X(1);
473         data[6] = X(2);
474      }
475
476      //! Access to the underlying unitvectors of the rotation matrix
477      inline Vector UnitY() const {
478          return Vector(data[1],data[4],data[7]);
479      }
480
481      //! Access to the underlying unitvectors of the rotation matrix
482      inline void UnitY(const Vector& X) {
483         data[1] = X(0);
484         data[4] = X(1);
485         data[7] = X(2);
486      }
487
488      //! Access to the underlying unitvectors of the rotation matrix
489      inline Vector UnitZ() const {
490          return Vector(data[2],data[5],data[8]);
491      }
492
493      //! Access to the underlying unitvectors of the rotation matrix
494      inline void UnitZ(const Vector& X) {
495         data[2] = X(0);
496         data[5] = X(1);
497         data[8] = X(2);
498      }
499
500      //! do not use operator == because the definition of Equal(.,.) is slightly
501      //! different.  It compares whether the 2 arguments are equal in an eps-interval
502      friend bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
503
504          //! The literal equality operator==(), also identical.
505      friend bool operator==(const Rotation& a,const Rotation& b);
506          //! The literal inequality operator!=()
507      friend bool operator!=(const Rotation& a,const Rotation& b);
508
509      friend class Frame;
510 };
511     bool operator==(const Rotation& a,const Rotation& b);
512
513
514
515 /**
516         \brief represents a frame transformation in 3D space (rotation + translation)
517
518     if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B)
519     then V2 = Frame.M*V1+Frame.p
520
521     Frame.M contains columns that represent the axes of frame B wrt frame A
522     Frame.p contains the origin of frame B expressed in frame A.
523 */
524 class Frame {
525 public:
526     Vector p;       //!< origine of the Frame
527     Rotation M;     //!< Orientation of the Frame
528
529 public:
530
531      inline Frame(const Rotation& R,const Vector& V);
532
533      //! The rotation matrix defaults to identity
534      explicit inline Frame(const Vector& V);
535      //! The position matrix defaults to zero
536      explicit inline Frame(const Rotation& R);
537
538          inline void setValue(float* oglmat);
539          inline void getValue(float* oglmat) const;
540
541      inline Frame() {}
542      //! The copy constructor. Normal copy by value semantics.
543      inline Frame(const Frame& arg);
544
545      //! Reads data from an double array
546      //\TODO should be formulated as a constructor
547      void Make4x4(double* d);
548
549      //!  Treats a frame as a 4x4 matrix and returns element i,j
550      //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
551      inline double operator()(int i,int j);
552
553      //!  Treats a frame as a 4x4 matrix and returns element i,j
554      //!    Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
555      inline double operator() (int i,int j) const;
556
557          // = Inverse
558      //! Gives back inverse transformation of a Frame
559      inline Frame Inverse() const;
560
561      //! The same as p2=R.Inverse()*p but more efficient.
562      inline Vector Inverse(const Vector& arg) const;
563
564      //! The same as p2=R.Inverse()*p but more efficient.
565      inline Wrench Inverse(const Wrench& arg) const;
566
567      //! The same as p2=R.Inverse()*p but more efficient.
568      inline Twist  Inverse(const Twist& arg) const;
569
570      //! Normal copy-by-value semantics.
571      inline Frame& operator = (const Frame& arg);
572
573      //! Transformation of the base to which the vector
574      //! is expressed.
575      inline Vector operator * (const Vector& arg) const;
576
577      //! Transformation of both the force reference point
578      //! and of the base to which the wrench is expressed.
579      //! look at Rotation*Wrench operator for a transformation
580      //! of only the base to which the twist is expressed.
581      //!
582      //! Complexity : 24M+18A
583      inline Wrench operator * (const Wrench& arg) const;
584
585      //! Transformation of both the velocity reference point
586      //! and of the base to which the twist is expressed.
587      //! look at Rotation*Twist for a transformation of only the
588      //! base to which the twist is expressed.
589      //!
590      //! Complexity : 24M+18A
591      inline Twist operator * (const Twist& arg) const;
592
593      //! Composition of two frames.
594      inline friend Frame operator *(const Frame& lhs,const Frame& rhs);
595
596      //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
597      inline static Frame Identity();
598
599      //! The twist <t_this> is expressed wrt the current
600      //! frame.  This frame is integrated into an updated frame with
601      //! <samplefrequency>.  Very simple first order integration rule.
602      inline void Integrate(const Twist& t_this,double frequency);
603
604     /*
605     // DH_Craig1989 : constructs a transformationmatrix
606     // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
607     // described in the Craigs book: Craig, J. J.,Introduction to
608     // Robotics: Mechanics and Control, Addison-Wesley,
609     // isbn:0-201-10326-5, 1986.
610     //
611     // Note that the frame is a redundant way to express the information
612     // in the DH-convention.
613     // \verbatim
614     // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
615     //
616     //  axis i-1 is connected by link i-1 to axis i numbering axis 1
617     //  to axis n link 0 (immobile base) to link n
618     //
619     //  link length a(i-1) length of the mutual perpendicular line
620     //  (normal) between the 2 axes.  This normal runs from (i-1) to
621     //  (i) axis.
622     //
623     //  link twist alpha(i-1): construct plane perpendicular to the
624     //  normal project axis(i-1) and axis(i) into plane angle from
625     //  (i-1) to (i) measured in the direction of the normal
626     //
627     //  link offset d(i) signed distance between normal (i-1) to (i)
628     //  and normal (i) to (i+1) along axis i joint angle theta(i)
629     //  signed angle between normal (i-1) to (i) and normal (i) to
630     //  (i+1) along axis i
631     //
632     //   First and last joints : a(0)= a(n) = 0
633     //   alpha(0) = alpha(n) = 0
634     //
635     //   PRISMATIC : theta(1) = 0 d(1) arbitrarily
636     //
637     //   REVOLUTE : theta(1) arbitrarily d(1) = 0
638     //
639     //   Not unique : if intersecting joint axis 2 choices for normal
640     //   Frame assignment of the DH convention : Z(i-1) follows axis
641     //   (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
642     //   Y(i-1) follows out of Z(i-1) and X(i-1)
643     //
644     //     a(i-1)     = distance from Z(i-1) to Z(i) along X(i-1)
645     //     alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
646     //     d(i)       = distance from X(i-1) to X(i) along Z(i)
647     //     theta(i)   = angle between X(i-1) to X(i) along X(i)
648     // \endverbatim
649     */
650      static Frame DH_Craig1989(double a,double alpha,double d,double theta);
651
652     // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
653     // the Denavit-Hartenberg convention as described in the original
654     // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
655     // notation for lower-pair mechanisms based on matrices, ASME
656     // Journal of Applied Mechanics, 23:215-221, 1955.
657
658      static Frame DH(double a,double alpha,double d,double theta);
659
660
661      //! do not use operator == because the definition of Equal(.,.) is slightly
662      //! different.  It compares whether the 2 arguments are equal in an eps-interval
663      inline friend bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
664
665          //! The literal equality operator==(), also identical.
666      inline friend bool operator==(const Frame& a,const Frame& b);
667          //! The literal inequality operator!=().
668      inline friend bool operator!=(const Frame& a,const Frame& b);
669 };
670
671 /**
672  * \brief represents both translational and rotational velocities.
673  *
674  * This class represents a twist.  A twist is the combination of translational
675  * velocity and rotational velocity applied at one point.
676 */
677 class Twist {
678 public:
679     Vector vel; //!< The velocity of that point
680     Vector rot; //!< The rotational velocity of that point.
681 public:
682
683     //! The default constructor initialises to Zero via the constructor of Vector.
684     Twist():vel(),rot() {};
685
686     Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};
687
688     inline Twist& operator-=(const Twist& arg);
689     inline Twist& operator+=(const Twist& arg);
690     //! index-based access to components, first vel(0..2), then rot(3..5)
691     inline double& operator()(int i);
692
693     //! index-based access to components, first vel(0..2), then rot(3..5)
694     //! For use with a const Twist
695     inline double operator()(int i) const;
696
697      double operator[] ( int index ) const
698        {
699          return this->operator() ( index );
700        }
701
702      double& operator[] ( int index )
703        {
704          return this->operator() ( index );
705        }
706
707      inline friend Twist operator*(const Twist& lhs,double rhs);
708      inline friend Twist operator*(double lhs,const Twist& rhs);
709      inline friend Twist operator/(const Twist& lhs,double rhs);
710      inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
711      inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
712      inline friend Twist operator-(const Twist& arg);
713      inline friend double dot(const Twist& lhs,const Wrench& rhs);
714      inline friend double dot(const Wrench& rhs,const Twist& lhs);
715      inline friend void SetToZero(Twist& v);
716
717
718      //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero())
719      static inline Twist Zero();
720
721      //! Reverses the sign of the twist
722      inline void ReverseSign();
723
724      //! Changes the reference point of the twist.
725      //! The vector v_base_AB is expressed in the same base as the twist
726      //! The vector v_base_AB is a vector from the old point to
727      //! the new point.
728      //!
729      //! Complexity : 6M+6A
730      inline Twist RefPoint(const Vector& v_base_AB) const;
731
732
733      //! do not use operator == because the definition of Equal(.,.) is slightly
734      //! different.  It compares whether the 2 arguments are equal in an eps-interval
735      inline friend bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
736
737          //! The literal equality operator==(), also identical.
738      inline friend bool operator==(const Twist& a,const Twist& b);
739          //! The literal inequality operator!=().
740      inline friend bool operator!=(const Twist& a,const Twist& b);
741
742 // = Friends
743     friend class Rotation;
744     friend class Frame;
745
746 };
747
748 /**
749  *      \brief represents both translational and rotational acceleration.
750  *
751  *      This class represents an acceleration twist.  A acceleration twist is
752  *      the combination of translational
753  *      acceleration and rotational acceleration applied at one point.
754 */
755 /*
756 class AccelerationTwist {
757 public:
758     Vector trans; //!< The translational acceleration of that point
759     Vector rot; //!< The rotational acceleration of that point.
760 public:
761
762     //! The default constructor initialises to Zero via the constructor of Vector.
763     AccelerationTwist():trans(),rot() {};
764
765     AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};
766
767     inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
768     inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
769     //! index-based access to components, first vel(0..2), then rot(3..5)
770     inline double& operator()(int i);
771
772     //! index-based access to components, first vel(0..2), then rot(3..5)
773     //! For use with a const AccelerationTwist
774     inline double operator()(int i) const;
775
776     double operator[] ( int index ) const
777     {
778         return this->operator() ( index );
779         }
780
781      double& operator[] ( int index )
782      {
783          return this->operator() ( index );
784      }
785
786      inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
787      inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
788      inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
789      inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
790      inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
791      inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
792      //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
793      //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
794      inline friend void SetToZero(AccelerationTwist& v);
795
796
797      //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero())
798      static inline AccelerationTwist Zero();
799
800      //! Reverses the sign of the AccelerationTwist
801      inline void ReverseSign();
802
803      //! Changes the reference point of the AccelerationTwist.
804      //! The vector v_base_AB is expressed in the same base as the AccelerationTwist
805      //! The vector v_base_AB is a vector from the old point to
806      //! the new point.
807      //!
808      //! Complexity : 6M+6A
809      inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;
810
811
812      //! do not use operator == because the definition of Equal(.,.) is slightly
813      //! different.  It compares whether the 2 arguments are equal in an eps-interval
814      inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);
815
816          //! The literal equality operator==(), also identical.
817      inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
818          //! The literal inequality operator!=().
819      inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);
820
821 // = Friends
822     friend class Rotation;
823     friend class Frame;
824
825 };
826 */
827 /**
828  * \brief represents the combination of a force and a torque.
829  *
830  * This class represents a Wrench.  A Wrench is the force and torque applied at a point
831  */
832 class Wrench
833 {
834 public:
835     Vector force;       //!< Force that is applied at the origin of the current ref frame
836     Vector torque;      //!< Torque that is applied at the origin of the current ref frame
837 public:
838
839     //! Does  initialise force and torque to zero via the underlying constructor of Vector
840     Wrench():force(),torque() {};
841     Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};
842
843 // = Operators
844      inline Wrench& operator-=(const Wrench& arg);
845      inline Wrench& operator+=(const Wrench& arg);
846
847      //! index-based access to components, first force(0..2), then torque(3..5)
848      inline double& operator()(int i);
849
850      //! index-based access to components, first force(0..2), then torque(3..5)
851      //! for use with a const Wrench
852      inline double operator()(int i) const;
853
854      double operator[] ( int index ) const
855        {
856          return this->operator() ( index );
857        }
858
859      double& operator[] ( int index )
860        {
861          return this->operator() ( index );
862        }
863
864      //! Scalar multiplication
865      inline friend Wrench operator*(const Wrench& lhs,double rhs);
866      //! Scalar multiplication
867      inline friend Wrench operator*(double lhs,const Wrench& rhs);
868      //! Scalar division
869      inline friend Wrench operator/(const Wrench& lhs,double rhs);
870
871      inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
872      inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);
873
874      //! An unary - operator
875      inline friend Wrench operator-(const Wrench& arg);
876
877      //! Sets the Wrench to Zero, to have a uniform function that sets an object or
878      //! double to zero.
879      inline friend void SetToZero(Wrench& v);
880
881      //! @return a zero Wrench
882      static inline Wrench Zero();
883
884      //! Reverses the sign of the current Wrench
885      inline void ReverseSign();
886
887      //! Changes the reference point of the wrench.
888      //! The vector v_base_AB is expressed in the same base as the twist
889      //! The vector v_base_AB is a vector from the old point to
890      //! the new point.
891      //!
892      //! Complexity : 6M+6A
893      inline Wrench RefPoint(const Vector& v_base_AB) const;
894
895
896      //! do not use operator == because the definition of Equal(.,.) is slightly
897      //! different.  It compares whether the 2 arguments are equal in an eps-interval
898      inline friend bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
899
900          //! The literal equality operator==(), also identical.
901      inline friend bool operator==(const Wrench& a,const Wrench& b);
902          //! The literal inequality operator!=().
903      inline friend bool operator!=(const Wrench& a,const Wrench& b);
904
905     friend class Rotation;
906     friend class Frame;
907
908
909 };
910
911
912 //! 2D version of Vector
913 class Vector2
914 {
915     double data[2];
916 public:
917      //! Does not initialise to Zero().
918      Vector2() {data[0]=data[1] = 0.0;}
919      inline Vector2(double x,double y);
920      inline Vector2(const Vector2& arg);
921      inline Vector2(double* xyz);
922      inline Vector2(float* xyz);
923
924      inline Vector2& operator = ( const Vector2& arg);
925
926      //! Access to elements, range checked when NDEBUG is not set, from 0..1
927      inline double operator()(int index) const;
928
929      //! Access to elements, range checked when NDEBUG is not set, from 0..1
930      inline double& operator() (int index);
931
932          //! store vector components in array
933          inline void GetValue(double* xy) const;
934
935      inline void ReverseSign();
936      inline Vector2& operator-=(const Vector2& arg);
937      inline Vector2& operator +=(const Vector2& arg);
938
939
940      inline friend Vector2 operator*(const Vector2& lhs,double rhs);
941      inline friend Vector2 operator*(double lhs,const Vector2& rhs);
942      inline friend Vector2 operator/(const Vector2& lhs,double rhs);
943      inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
944      inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
945      inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
946      inline friend Vector2 operator-(const Vector2& arg);
947      inline friend void SetToZero(Vector2& v);
948
949      //! @return a zero 2D vector.
950      inline static Vector2 Zero();
951
952    /** Normalizes this vector and returns it norm
953         * makes v a unitvector and returns the norm of v.
954         * if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
955         * if this is not good, check the return value of this method.
956         */
957      double Normalize(double eps=epsilon);
958
959      //!  @return the norm of the vector
960      inline double Norm() const;
961
962      //! projects v in its XY plane, and sets *this to these values
963      inline void Set3DXY(const Vector& v);
964
965      //! projects v in its YZ plane, and sets *this to these values
966      inline void Set3DYZ(const Vector& v);
967
968      //! projects v in its ZX plane, and sets *this to these values
969      inline void Set3DZX(const Vector& v);
970
971      //! projects v_someframe in the XY plane of F_someframe_XY,
972      //! and sets *this to these values
973      //! expressed wrt someframe.
974      inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);
975
976
977      //! do not use operator == because the definition of Equal(.,.) is slightly
978      //! different.  It compares whether the 2 arguments are equal in an eps-interval
979      inline friend bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
980
981     friend class Rotation2;
982 };
983
984
985 //! A 2D Rotation class, for conventions see Rotation. For further documentation
986 //! of the methods see Rotation class.
987 class Rotation2
988 {
989     double s,c;
990     //! c,s represent  cos(angle), sin(angle), this also represents first col. of rot matrix
991     //! from outside, this class behaves as if it would store the complete 2x2 matrix.
992 public:
993     //! Default constructor does NOT initialise to Zero().
994     Rotation2() {c=1.0;s=0.0;}
995
996     explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}
997
998     Rotation2(double ca,double sa):s(sa),c(ca){}
999
1000      inline Rotation2& operator=(const Rotation2& arg);
1001      inline Vector2 operator*(const Vector2& v) const;
1002      //!    Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set
1003      inline double operator() (int i,int j) const;
1004
1005      inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);
1006
1007      inline void SetInverse();
1008      inline Rotation2 Inverse() const;
1009      inline Vector2 Inverse(const Vector2& v) const;
1010
1011      inline void SetIdentity();
1012      inline static Rotation2 Identity();
1013
1014
1015      //! The SetRot.. functions set the value of *this to the appropriate rotation matrix.
1016      inline void SetRot(double angle);
1017
1018      //! The Rot... static functions give the value of the appropriate rotation matrix bac
1019      inline static Rotation2 Rot(double angle);
1020
1021      //! Gets the angle (in radians)
1022      inline double GetRot() const;
1023
1024      //! do not use operator == because the definition of Equal(.,.) is slightly
1025      //! different.  It compares whether the 2 arguments are equal in an eps-interval
1026      inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
1027 };
1028
1029 //! A 2D frame class, for further documentation see the Frames class
1030 //! for methods with unchanged semantics.
1031 class Frame2
1032  {
1033 public:
1034     Vector2 p;          //!< origine of the Frame
1035     Rotation2 M;        //!< Orientation of the Frame
1036
1037 public:
1038
1039      inline Frame2(const Rotation2& R,const Vector2& V);
1040      explicit inline Frame2(const Vector2& V);
1041      explicit inline Frame2(const Rotation2& R);
1042      inline Frame2(void);
1043      inline Frame2(const Frame2& arg);
1044      inline void Make4x4(double* d);
1045
1046      //!  Treats a frame as a 3x3 matrix and returns element i,j
1047      //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
1048      inline double operator()(int i,int j);
1049
1050      //!  Treats a frame as a 4x4 matrix and returns element i,j
1051      //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
1052      inline double operator() (int i,int j) const;
1053
1054      inline void SetInverse();
1055      inline Frame2 Inverse() const;
1056      inline Vector2 Inverse(const Vector2& arg) const;
1057      inline Frame2& operator = (const Frame2& arg);
1058      inline Vector2 operator * (const Vector2& arg);
1059      inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
1060      inline void SetIdentity();
1061      inline void Integrate(const Twist& t_this,double frequency);
1062      inline static Frame2 Identity() {
1063         Frame2 tmp;
1064         tmp.SetIdentity();
1065         return tmp;
1066      }
1067      inline friend bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);
1068 };
1069
1070 IMETHOD Vector diff(const Vector& a,const Vector& b,double dt=1);
1071 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);
1072 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);
1073 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);
1074 IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);
1075 IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt=1);
1076 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt=1);
1077 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt=1);
1078 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);
1079 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);
1080 #ifdef KDL_INLINE
1081 //    #include "vector.inl"
1082 //   #include "wrench.inl"
1083     //#include "rotation.inl"
1084     //#include "frame.inl"
1085     //#include "twist.inl"
1086     //#include "vector2.inl"
1087     //#include "rotation2.inl"
1088     //#include "frame2.inl"
1089 #include "frames.inl"
1090 #endif
1091
1092
1093
1094 }
1095
1096
1097 #endif