This file is indexed.

/usr/include/kdl/frames.hpp is in liborocos-kdl-dev 1.3.1+dfsg-1.

This file is owned by root:root, with mode 0o644.

The actual contents of the file can be viewed below.

   1
   2
   3
   4
   5
   6
   7
   8
   9
  10
  11
  12
  13
  14
  15
  16
  17
  18
  19
  20
  21
  22
  23
  24
  25
  26
  27
  28
  29
  30
  31
  32
  33
  34
  35
  36
  37
  38
  39
  40
  41
  42
  43
  44
  45
  46
  47
  48
  49
  50
  51
  52
  53
  54
  55
  56
  57
  58
  59
  60
  61
  62
  63
  64
  65
  66
  67
  68
  69
  70
  71
  72
  73
  74
  75
  76
  77
  78
  79
  80
  81
  82
  83
  84
  85
  86
  87
  88
  89
  90
  91
  92
  93
  94
  95
  96
  97
  98
  99
 100
 101
 102
 103
 104
 105
 106
 107
 108
 109
 110
 111
 112
 113
 114
 115
 116
 117
 118
 119
 120
 121
 122
 123
 124
 125
 126
 127
 128
 129
 130
 131
 132
 133
 134
 135
 136
 137
 138
 139
 140
 141
 142
 143
 144
 145
 146
 147
 148
 149
 150
 151
 152
 153
 154
 155
 156
 157
 158
 159
 160
 161
 162
 163
 164
 165
 166
 167
 168
 169
 170
 171
 172
 173
 174
 175
 176
 177
 178
 179
 180
 181
 182
 183
 184
 185
 186
 187
 188
 189
 190
 191
 192
 193
 194
 195
 196
 197
 198
 199
 200
 201
 202
 203
 204
 205
 206
 207
 208
 209
 210
 211
 212
 213
 214
 215
 216
 217
 218
 219
 220
 221
 222
 223
 224
 225
 226
 227
 228
 229
 230
 231
 232
 233
 234
 235
 236
 237
 238
 239
 240
 241
 242
 243
 244
 245
 246
 247
 248
 249
 250
 251
 252
 253
 254
 255
 256
 257
 258
 259
 260
 261
 262
 263
 264
 265
 266
 267
 268
 269
 270
 271
 272
 273
 274
 275
 276
 277
 278
 279
 280
 281
 282
 283
 284
 285
 286
 287
 288
 289
 290
 291
 292
 293
 294
 295
 296
 297
 298
 299
 300
 301
 302
 303
 304
 305
 306
 307
 308
 309
 310
 311
 312
 313
 314
 315
 316
 317
 318
 319
 320
 321
 322
 323
 324
 325
 326
 327
 328
 329
 330
 331
 332
 333
 334
 335
 336
 337
 338
 339
 340
 341
 342
 343
 344
 345
 346
 347
 348
 349
 350
 351
 352
 353
 354
 355
 356
 357
 358
 359
 360
 361
 362
 363
 364
 365
 366
 367
 368
 369
 370
 371
 372
 373
 374
 375
 376
 377
 378
 379
 380
 381
 382
 383
 384
 385
 386
 387
 388
 389
 390
 391
 392
 393
 394
 395
 396
 397
 398
 399
 400
 401
 402
 403
 404
 405
 406
 407
 408
 409
 410
 411
 412
 413
 414
 415
 416
 417
 418
 419
 420
 421
 422
 423
 424
 425
 426
 427
 428
 429
 430
 431
 432
 433
 434
 435
 436
 437
 438
 439
 440
 441
 442
 443
 444
 445
 446
 447
 448
 449
 450
 451
 452
 453
 454
 455
 456
 457
 458
 459
 460
 461
 462
 463
 464
 465
 466
 467
 468
 469
 470
 471
 472
 473
 474
 475
 476
 477
 478
 479
 480
 481
 482
 483
 484
 485
 486
 487
 488
 489
 490
 491
 492
 493
 494
 495
 496
 497
 498
 499
 500
 501
 502
 503
 504
 505
 506
 507
 508
 509
 510
 511
 512
 513
 514
 515
 516
 517
 518
 519
 520
 521
 522
 523
 524
 525
 526
 527
 528
 529
 530
 531
 532
 533
 534
 535
 536
 537
 538
 539
 540
 541
 542
 543
 544
 545
 546
 547
 548
 549
 550
 551
 552
 553
 554
 555
 556
 557
 558
 559
 560
 561
 562
 563
 564
 565
 566
 567
 568
 569
 570
 571
 572
 573
 574
 575
 576
 577
 578
 579
 580
 581
 582
 583
 584
 585
 586
 587
 588
 589
 590
 591
 592
 593
 594
 595
 596
 597
 598
 599
 600
 601
 602
 603
 604
 605
 606
 607
 608
 609
 610
 611
 612
 613
 614
 615
 616
 617
 618
 619
 620
 621
 622
 623
 624
 625
 626
 627
 628
 629
 630
 631
 632
 633
 634
 635
 636
 637
 638
 639
 640
 641
 642
 643
 644
 645
 646
 647
 648
 649
 650
 651
 652
 653
 654
 655
 656
 657
 658
 659
 660
 661
 662
 663
 664
 665
 666
 667
 668
 669
 670
 671
 672
 673
 674
 675
 676
 677
 678
 679
 680
 681
 682
 683
 684
 685
 686
 687
 688
 689
 690
 691
 692
 693
 694
 695
 696
 697
 698
 699
 700
 701
 702
 703
 704
 705
 706
 707
 708
 709
 710
 711
 712
 713
 714
 715
 716
 717
 718
 719
 720
 721
 722
 723
 724
 725
 726
 727
 728
 729
 730
 731
 732
 733
 734
 735
 736
 737
 738
 739
 740
 741
 742
 743
 744
 745
 746
 747
 748
 749
 750
 751
 752
 753
 754
 755
 756
 757
 758
 759
 760
 761
 762
 763
 764
 765
 766
 767
 768
 769
 770
 771
 772
 773
 774
 775
 776
 777
 778
 779
 780
 781
 782
 783
 784
 785
 786
 787
 788
 789
 790
 791
 792
 793
 794
 795
 796
 797
 798
 799
 800
 801
 802
 803
 804
 805
 806
 807
 808
 809
 810
 811
 812
 813
 814
 815
 816
 817
 818
 819
 820
 821
 822
 823
 824
 825
 826
 827
 828
 829
 830
 831
 832
 833
 834
 835
 836
 837
 838
 839
 840
 841
 842
 843
 844
 845
 846
 847
 848
 849
 850
 851
 852
 853
 854
 855
 856
 857
 858
 859
 860
 861
 862
 863
 864
 865
 866
 867
 868
 869
 870
 871
 872
 873
 874
 875
 876
 877
 878
 879
 880
 881
 882
 883
 884
 885
 886
 887
 888
 889
 890
 891
 892
 893
 894
 895
 896
 897
 898
 899
 900
 901
 902
 903
 904
 905
 906
 907
 908
 909
 910
 911
 912
 913
 914
 915
 916
 917
 918
 919
 920
 921
 922
 923
 924
 925
 926
 927
 928
 929
 930
 931
 932
 933
 934
 935
 936
 937
 938
 939
 940
 941
 942
 943
 944
 945
 946
 947
 948
 949
 950
 951
 952
 953
 954
 955
 956
 957
 958
 959
 960
 961
 962
 963
 964
 965
 966
 967
 968
 969
 970
 971
 972
 973
 974
 975
 976
 977
 978
 979
 980
 981
 982
 983
 984
 985
 986
 987
 988
 989
 990
 991
 992
 993
 994
 995
 996
 997
 998
 999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011
1012
1013
1014
1015
1016
1017
1018
1019
1020
1021
1022
1023
1024
1025
1026
1027
1028
1029
1030
1031
1032
1033
1034
1035
1036
1037
1038
1039
1040
1041
1042
1043
1044
1045
1046
1047
1048
1049
1050
1051
1052
1053
1054
1055
1056
1057
1058
1059
1060
1061
1062
1063
1064
1065
1066
1067
1068
1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
1103
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116
1117
1118
1119
1120
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133
1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145
1146
1147
1148
1149
1150
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
1164
1165
1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
1181
1182
1183
1184
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198
1199
1200
1201
1202
1203
1204
1205
1206
1207
1208
1209
1210
1211
1212
1213
1214
1215
1216
1217
1218
1219
1220
1221
1222
1223
1224
1225
1226
1227
1228
1229
1230
1231
1232
1233
1234
1235
1236
1237
1238
1239
1240
1241
1242
1243
1244
1245
1246
1247
1248
1249
1250
1251
1252
1253
1254
1255
1256
1257
1258
1259
1260
1261
1262
1263
1264
1265
1266
1267
1268
1269
/***************************************************************************
                        frames.hpp `-  description
                       -------------------------
    begin                : June 2006
    copyright            : (C) 2006 Erwin Aertbelien
    email                : firstname.lastname@mech.kuleuven.be

 History (only major changes)( AUTHOR-Description ) :

 ***************************************************************************
 *   This library is free software; you can redistribute it and/or         *
 *   modify it under the terms of the GNU Lesser General Public            *
 *   License as published by the Free Software Foundation; either          *
 *   version 2.1 of the License, or (at your option) any later version.    *
 *                                                                         *
 *   This library is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
 *   Lesser General Public License for more details.                       *
 *                                                                         *
 *   You should have received a copy of the GNU Lesser General Public      *
 *   License along with this library; if not, write to the Free Software   *
 *   Foundation, Inc., 59 Temple Place,                                    *
 *   Suite 330, Boston, MA  02111-1307  USA                                *
 *                                                                         *
 ***************************************************************************/

/**
 * \file
 * \warning
 *       Efficienty can be improved by writing p2 = A*(B*(C*p1))) instead of
 *          p2=A*B*C*p1
 *
 * \par PROPOSED NAMING CONVENTION FOR FRAME-like OBJECTS
 *
 * \verbatim
 *      A naming convention of objects of the type defined in this file :
 *          (1) Frame : F...
 *              Rotation : R ...
 *          (2) Twist    : T ...
 *              Wrench   : W ...
 *              Vector   : V ...
 *      This prefix is followed by :
 *      for category (1) :
 *          F_A_B : w.r.t. frame A, frame B expressed
 *          ( each column of F_A_B corresponds to an axis of B,
 *            expressed w.r.t. frame A )
 *          in mathematical convention :
 *                   A
 *         F_A_B ==    F
 *                   B
 *
 *      for category (2) :
 *          V_B   : a vector expressed w.r.t. frame B
 *
 *      This can also be prepended by a name :
 *          e.g. : temporaryV_B
 *
 *      With this convention one can write :
 *
 *      F_A_B = F_B_A.Inverse();
 *      F_A_C = F_A_B * F_B_C;
 *      V_B   = F_B_C * V_C;    // both translation and rotation
 *      V_B   = R_B_C * V_C;    // only rotation
 * \endverbatim
 *
 * \par CONVENTIONS FOR WHEN USED WITH ROBOTS :
 *
 * \verbatim
 *       world : represents the frame ([1 0 0,0 1 0,0 0 1],[0 0 0]')
 *       mp    : represents mounting plate of a robot
 *               (i.e. everything before MP is constructed by robot manufacturer
 *                    everything after MP is tool )
 *       tf    : represents task frame of a robot
 *               (i.e. frame in which motion and force control is expressed)
 *       sf    : represents sensor frame of a robot
 *               (i.e. frame at which the forces measured by the force sensor
 *               are expressed )
 *
 *          Frame F_world_mp=...;
 *          Frame F_mp_sf(..)
 *          Frame F_mp_tf(,.)
 *
 *          Wrench are measured in sensor frame SF, so one could write :
 *                Wrench_tf = F_mp_tf.Inverse()* ( F_mp_sf * Wrench_sf );
 * \endverbatim
 *
 * \par CONVENTIONS REGARDING UNITS :
 *      Any consistent series of units can be used, e.g. N,mm,Nmm,..mm/sec
 *
 * \par Twist and Wrench transformations
 * 3 different types of transformations do exist for the twists
 * and wrenches.
 *
 * \verbatim
 *      1) Frame * Twist or Frame * Wrench :
 *              this transforms both the velocity/force reference point
 *             and the basis to which the twist/wrench are expressed.
 *      2) Rotation * Twist or Rotation * Wrench :
 *              this transforms the basis to which the twist/wrench are
 *              expressed, but leaves the reference point intact.
 *      3) Twist.RefPoint(v_base_AB) or Wrench.RefPoint(v_base_AB)
 *              this transforms only the reference point. v is expressed
 *              in the same base as the twist/wrench and points from the
 *              old reference point to the new reference point.
 * \endverbatim
 *
 *\par Spatial cross products
 * Let m be a 6D motion vector (Twist) and f be a 6D force vector (Wrench) 
 * attached to a rigid body moving with a certain velocity v (Twist). Then
 *\verbatim
 *     1) m_dot = v cross m or Twist=Twist*Twist
 *     2) f_dot = v cross f or Wrench=Twist*Wrench
 *\endverbatim
 *
 * \par Complexity
 *  Sometimes the amount of work is given in the documentation
 *  e.g. 6M+3A means 6 multiplications and 3 additions.
 *
 *  \author
 *      Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
 *
 ****************************************************************************/
#ifndef KDL_FRAMES_H
#define KDL_FRAMES_H


#include "utilities/kdl-config.h"
#include "utilities/utility.h"

/////////////////////////////////////////////////////////////

namespace KDL {



class Vector;
class Rotation;
class Frame;
class Wrench;
class Twist;
class Vector2;
class Rotation2;
class Frame2;


// Equal is friend function, but default arguments for friends are forbidden (ยง8.3.6.4)
inline bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
inline bool Equal(const Frame& a,const Frame& b,double eps=epsilon);
inline bool Equal(const Twist& a,const Twist& b,double eps=epsilon);
inline bool Equal(const Wrench& a,const Wrench& b,double eps=epsilon);
inline bool Equal(const Vector2& a,const Vector2& b,double eps=epsilon);
inline bool Equal(const Rotation2& a,const Rotation2& b,double eps=epsilon);
inline bool Equal(const Frame2& a,const Frame2& b,double eps=epsilon);


/**
 * \brief A concrete implementation of a 3 dimensional vector class
 */
class Vector
{
public:
    double data[3];
     //! Does not initialise the Vector to zero. use Vector::Zero() or SetToZero for that
     inline Vector() {data[0]=data[1]=data[2] = 0.0;}

     //! Constructs a vector out of the three values x, y and z
     inline Vector(double x,double y, double z);

     //! Assignment operator. The normal copy by value semantics.
     inline Vector(const Vector& arg);

     //! Assignment operator. The normal copy by value semantics.
     inline Vector& operator = ( const Vector& arg);

     //! Access to elements, range checked when NDEBUG is not set, from 0..2
     inline double operator()(int index) const;

     //! Access to elements, range checked when NDEBUG is not set, from 0..2
     inline double& operator() (int index);

	 //! Equivalent to double operator()(int index) const
     double operator[] ( int index ) const
       {
	 return this->operator() ( index );
       }

	 //! Equivalent to double& operator()(int index)
     double& operator[] ( int index )
       {
	 return this->operator() ( index );
       }

     inline double x() const;
     inline double y() const;
     inline double z() const;
     inline void x(double);
     inline void y(double);
     inline void z(double);

     //! Reverses the sign of the Vector object itself
     inline void ReverseSign();


     //! subtracts a vector from the Vector object itself
     inline Vector& operator-=(const Vector& arg);


     //! Adds a vector from the Vector object itself
     inline Vector& operator +=(const Vector& arg);

     //! Scalar multiplication is defined
     inline friend Vector operator*(const Vector& lhs,double rhs);
     //! Scalar multiplication is defined
     inline friend Vector operator*(double lhs,const Vector& rhs);
     //! Scalar division is defined

     inline friend Vector operator/(const Vector& lhs,double rhs);
     inline friend Vector operator+(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator-(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator*(const Vector& lhs,const Vector& rhs);
     inline friend Vector operator-(const Vector& arg);
     inline friend double dot(const Vector& lhs,const Vector& rhs);

     //! To have a uniform operator to put an element to zero, for scalar values
     //! and for objects.
     inline friend void SetToZero(Vector& v);

     //! @return a zero vector
     inline static Vector Zero();

   /** Normalizes this vector and returns it norm
	* makes v a unitvector and returns the norm of v.
	* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
	* if this is not good, check the return value of this method.
	*/
     double Normalize(double eps=epsilon);

     //!    @return the norm of the vector
     double Norm() const;



     //! a 3D vector where the 2D vector v is put in the XY plane
     inline void Set2DXY(const Vector2& v);
     //! a 3D vector where the 2D vector v is put in the YZ plane
     inline void Set2DYZ(const Vector2& v);
     //! a 3D vector where the 2D vector v is put in the ZX plane
     inline void Set2DZX(const Vector2& v);
     //! a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
     inline void Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY);


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Vector& a,const Vector& b,double eps);

	 //! The literal equality operator==(), also identical.
     inline friend bool operator==(const Vector& a,const Vector& b);
	 //! The literal inequality operator!=().
     inline friend bool operator!=(const Vector& a,const Vector& b);

     friend class Rotation;
     friend class Frame;
};


/**
  \brief represents rotations in 3 dimensional space.

  This class represents a rotation matrix with the following
  conventions :
 \verbatim
     Suppose V2 = R*V,                                    (1)
     V is expressed in frame B
     V2 is expressed in frame A
     This matrix R consists of 3 collumns [ X,Y,Z ],
     X,Y, and Z contain the axes of frame B, expressed in frame A
     Because of linearity expr(1) is valid.
 \endverbatim
   This class only represents rotational_interpolation, not translation
 Two interpretations are possible for rotation angles.
 * if you rotate with angle around X frame A to have frame B,
   then the result of SetRotX is equal to frame B expressed wrt A.
     In code:
 \verbatim
      Rotation R;
      F_A_B = R.SetRotX(angle);
 \endverbatim
 * Secondly, if you take the following code :
 \verbatim
      Vector p,p2; Rotation R;
      R.SetRotX(angle);
      p2 = R*p;
 \endverbatim
   then the frame p2 is rotated around X axis with (-angle).
   Analogue reasonings can be applyd to SetRotY,SetRotZ,SetRot
 \par type
  Concrete implementation
*/
class Rotation
{
public:
    double data[9];

    inline Rotation() {
		*this = Rotation::Identity();
	}
    inline Rotation(double Xx,double Yx,double Zx,
                double Xy,double Yy,double Zy,
                double Xz,double Yz,double Zz);
    inline Rotation(const Vector& x,const Vector& y,const Vector& z);
    // default copy constructor is sufficient


     inline Rotation& operator=(const Rotation& arg);

     //!  Defines a multiplication R*V between a Rotation R and a Vector V.
     //! Complexity : 9M+6A
     inline Vector operator*(const Vector& v) const;

     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
     inline double& operator()(int i,int j);

     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
     inline double operator() (int i,int j) const;

     friend Rotation operator *(const Rotation& lhs,const Rotation& rhs);

     //! Sets the value of *this to its inverse.
     inline void SetInverse();

     //! Gives back the inverse rotation matrix of *this.
     inline Rotation Inverse() const;

     //! The same as R.Inverse()*v but more efficient.
     inline Vector Inverse(const Vector& v) const;

     //! The same as R.Inverse()*arg but more efficient.
     inline Wrench Inverse(const Wrench& arg) const;

     //! The same as R.Inverse()*arg but more efficient.
     inline Twist Inverse(const Twist& arg) const;

     //! Gives back an identity rotaton matrix
     inline static Rotation Identity();


// = Rotations
    //! The Rot... static functions give the value of the appropriate rotation matrix back.
    inline static Rotation RotX(double angle);
    //! The Rot... static functions give the value of the appropriate rotation matrix back.
    inline static Rotation RotY(double angle);
    //! The Rot... static functions give the value of the appropriate rotation matrix back.
    inline static Rotation RotZ(double angle);
    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
    //! DoRot... functions are only defined when they can be executed more efficiently
    inline void DoRotX(double angle);
    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
    //! DoRot... functions are only defined when they can be executed more efficiently
    inline void DoRotY(double angle);
    //! The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot..
    //! DoRot... functions are only defined when they can be executed more efficiently
    inline void DoRotZ(double angle);

    //! Along an arbitrary axes.  It is not necessary to normalize rotvec.
    //! returns identity rotation matrix in the case that the norm of rotvec
    //! is to small to be used.
    // @see Rot2 if you want to handle this error in another way.
    static Rotation Rot(const Vector& rotvec,double angle);

    //! Along an arbitrary axes.  rotvec should be normalized.
    static Rotation Rot2(const Vector& rotvec,double angle);

    //! Returns a vector with the direction of the equiv. axis
    //! and its norm is angle
    Vector GetRot() const;

	/** Returns the rotation angle around the equiv. axis
	 * @param axis the rotation axis is returned in this variable
	 * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
	 *                                         to be +/- Z-axis
	 *               in the case of angle == PI : 2 solutions, positive Z-component
	 *                                            of the axis is choosen.
	 * @result returns the rotation angle (between [0..PI] )
	 */
	double GetRotAngle(Vector& axis,double eps=epsilon) const;


/**     Gives back a rotation matrix specified with EulerZYZ convention :
	 *       - First rotate around Z with alfa,
	 *       - then around the new Y with beta,
	 *	     - then around new Z with gamma.
	 *  Invariants:
	 *  	- EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PHI, -beta, gamma +/- PI)
	 *  	- (angle + 2*k*PI)
	 **/
    static Rotation EulerZYZ(double Alfa,double Beta,double Gamma);

	/** Gives back the EulerZYZ convention description of the rotation matrix :
	 First rotate around Z with alpha,
	 then around the new Y with beta, then around
	 new Z with gamma.

	 Variables are bound by:
	 - (-PI <  alpha  <= PI),
	 - (0   <= beta  <= PI),
	 - (-PI <  gamma <= PI)

	 if beta==0 or beta==PI, then alpha and gamma are not unique, in this case gamma is chosen to be zero.
	 Invariants:
	   - EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, -beta, gamma +/- PI)
	   - angle + 2*k*PI
	 */
    void GetEulerZYZ(double& alpha,double& beta,double& gamma) const;

    //! Gives back a rotation matrix specified with Quaternion convention
    //! the norm of (x,y,z,w) should be equal to 1
    static Rotation Quaternion(double x,double y,double z, double w);
    
    //! Get the quaternion of this matrix
    //! \post the norm of (x,y,z,w) is 1
    void GetQuaternion(double& x,double& y,double& z, double& w) const;

    /**
     *
     * Gives back a rotation matrix specified with RPY convention:
     * first rotate around X with roll, then around the
     *              old Y with pitch, then around old Z with yaw
     *
     * Invariants:
     *  - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI )
     *  - angles + 2*k*PI
     */
    static Rotation RPY(double roll,double pitch,double yaw);

/**  Gives back a vector in RPY coordinates, variables are bound by
     -  -PI <= roll <= PI
     -   -PI <= Yaw  <= PI
     -  -PI/2 <= PITCH <= PI/2

	 convention :
	 - first rotate around X with roll,
	 - then around the old Y with pitch,
	 - then around old Z with yaw

	 if pitch == PI/2 or pitch == -PI/2, multiple solutions for gamma and alpha exist.  The solution where roll==0
	 is chosen.

	 Invariants:
	 - RPY(roll,pitch,yaw) == RPY( roll +/- PI, PI-pitch, yaw +/- PI )
	 - angles + 2*k*PI

**/
    void GetRPY(double& roll,double& pitch,double& yaw) const;


    /**  EulerZYX constructs a Rotation from the Euler ZYX parameters:
     *   -  First rotate around Z with alfa,
     *   - then around the new Y with beta,
     *   - then around new X with gamma.
     *
     *  Closely related to RPY-convention.
     *
     *  Invariants:
     *  	- EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI)
     *  	- (angle + 2*k*PI)
     **/
    inline static Rotation EulerZYX(double Alfa,double Beta,double Gamma) {
        return RPY(Gamma,Beta,Alfa);
    }

    /**   GetEulerZYX gets the euler ZYX parameters of a rotation :
     *   First rotate around Z with alfa,
     *   then around the new Y with beta, then around
     *   new X with gamma.
     *
     *  Range of the results of GetEulerZYX :
     *  -  -PI <= alfa <= PI
     *  -   -PI <= gamma <= PI
     *  -  -PI/2 <= beta <= PI/2
     *
     *  if beta == PI/2 or beta == -PI/2, multiple solutions for gamma and alpha exist.  The solution where gamma==0
     *  is chosen.
     *
     *
     *  Invariants:
     *  	- EulerZYX(alpha,beta,gamma) == EulerZYX(alpha +/- PI, PI-beta, gamma +/- PI)
     *  	- and also (angle + 2*k*PI)
     *
     *  Closely related to RPY-convention.
     **/
    inline void GetEulerZYX(double& Alfa,double& Beta,double& Gamma) const {
        GetRPY(Gamma,Beta,Alfa);
    }

     //! Transformation of the base to which the twist is expressed.
     //! Complexity : 18M+12A
     //! @see Frame*Twist for a transformation that also transforms
     //! the velocity reference point.
     inline Twist operator * (const Twist& arg) const;

     //! Transformation of the base to which the wrench is expressed.
     //! Complexity : 18M+12A
     //! @see Frame*Wrench for a transformation that also transforms
     //! the force reference point.
     inline Wrench operator * (const Wrench& arg) const;

     //! Access to the underlying unitvectors of the rotation matrix
     inline Vector UnitX() const {
         return Vector(data[0],data[3],data[6]);
     }

     //! Access to the underlying unitvectors of the rotation matrix
     inline void UnitX(const Vector& X) {
        data[0] = X(0);
        data[3] = X(1);
        data[6] = X(2);
     }

     //! Access to the underlying unitvectors of the rotation matrix
     inline Vector UnitY() const {
         return Vector(data[1],data[4],data[7]);
     }

     //! Access to the underlying unitvectors of the rotation matrix
     inline void UnitY(const Vector& X) {
        data[1] = X(0);
        data[4] = X(1);
        data[7] = X(2);
     }

     //! Access to the underlying unitvectors of the rotation matrix
     inline Vector UnitZ() const {
         return Vector(data[2],data[5],data[8]);
     }

     //! Access to the underlying unitvectors of the rotation matrix
     inline void UnitZ(const Vector& X) {
        data[2] = X(0);
        data[5] = X(1);
        data[8] = X(2);
     }

     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     friend bool Equal(const Rotation& a,const Rotation& b,double eps);

	 //! The literal equality operator==(), also identical.
     friend bool operator==(const Rotation& a,const Rotation& b);
	 //! The literal inequality operator!=()
     friend bool operator!=(const Rotation& a,const Rotation& b);

     friend class Frame;
};
    bool operator==(const Rotation& a,const Rotation& b);
    bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);



/**
	\brief represents a frame transformation in 3D space (rotation + translation)

    if V2 = Frame*V1 (V2 expressed in frame A, V1 expressed in frame B)
    then V2 = Frame.M*V1+Frame.p

    Frame.M contains columns that represent the axes of frame B wrt frame A
    Frame.p contains the origin of frame B expressed in frame A.
*/
class Frame {
public:
    Vector p;       //!< origine of the Frame
    Rotation M;     //!< Orientation of the Frame

public:

     inline Frame(const Rotation& R,const Vector& V);

     //! The rotation matrix defaults to identity
     explicit inline Frame(const Vector& V);
     //! The position matrix defaults to zero
     explicit inline Frame(const Rotation& R);

     inline Frame() {}
     //! The copy constructor. Normal copy by value semantics.
     inline Frame(const Frame& arg);

     //! Reads data from an double array
     //\TODO should be formulated as a constructor
     void Make4x4(double* d);

     //!  Treats a frame as a 4x4 matrix and returns element i,j
     //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
     inline double operator()(int i,int j);

     //!  Treats a frame as a 4x4 matrix and returns element i,j
     //!    Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
     inline double operator() (int i,int j) const;

// = Inverse
     //! Gives back inverse transformation of a Frame
     inline Frame Inverse() const;

     //! The same as p2=R.Inverse()*p but more efficient.
     inline Vector Inverse(const Vector& arg) const;

     //! The same as p2=R.Inverse()*p but more efficient.
     inline Wrench Inverse(const Wrench& arg) const;

     //! The same as p2=R.Inverse()*p but more efficient.
     inline Twist  Inverse(const Twist& arg) const;

     //! Normal copy-by-value semantics.
     inline Frame& operator = (const Frame& arg);

     //! Transformation of the base to which the vector
     //! is expressed.
     inline Vector operator * (const Vector& arg) const;

     //! Transformation of both the force reference point
     //! and of the base to which the wrench is expressed.
     //! look at Rotation*Wrench operator for a transformation
     //! of only the base to which the twist is expressed.
     //!
     //! Complexity : 24M+18A
     inline Wrench operator * (const Wrench& arg) const;

     //! Transformation of both the velocity reference point
     //! and of the base to which the twist is expressed.
     //! look at Rotation*Twist for a transformation of only the
     //! base to which the twist is expressed.
     //!
     //! Complexity : 24M+18A
     inline Twist operator * (const Twist& arg) const;

     //! Composition of two frames.
     inline friend Frame operator *(const Frame& lhs,const Frame& rhs);

     //! @return the identity transformation Frame(Rotation::Identity(),Vector::Zero()).
     inline static Frame Identity();

     //! The twist <t_this> is expressed wrt the current
     //! frame.  This frame is integrated into an updated frame with
     //! <samplefrequency>.  Very simple first order integration rule.
     inline void Integrate(const Twist& t_this,double frequency);

    /*
    // DH_Craig1989 : constructs a transformationmatrix
    // T_link(i-1)_link(i) with the Denavit-Hartenberg convention as
    // described in the Craigs book: Craig, J. J.,Introduction to
    // Robotics: Mechanics and Control, Addison-Wesley,
    // isbn:0-201-10326-5, 1986.
    //
    // Note that the frame is a redundant way to express the information
    // in the DH-convention.
    // \verbatim
    // Parameters in full : a(i-1),alpha(i-1),d(i),theta(i)
    //
    //  axis i-1 is connected by link i-1 to axis i numbering axis 1
    //  to axis n link 0 (immobile base) to link n
    //
    //  link length a(i-1) length of the mutual perpendicular line
    //  (normal) between the 2 axes.  This normal runs from (i-1) to
    //  (i) axis.
    //
    //  link twist alpha(i-1): construct plane perpendicular to the
    //  normal project axis(i-1) and axis(i) into plane angle from
    //  (i-1) to (i) measured in the direction of the normal
    //
    //  link offset d(i) signed distance between normal (i-1) to (i)
    //  and normal (i) to (i+1) along axis i joint angle theta(i)
    //  signed angle between normal (i-1) to (i) and normal (i) to
    //  (i+1) along axis i
    //
    //   First and last joints : a(0)= a(n) = 0
    //   alpha(0) = alpha(n) = 0
    //
    //   PRISMATIC : theta(1) = 0 d(1) arbitrarily
    //
    //   REVOLUTE : theta(1) arbitrarily d(1) = 0
    //
    //   Not unique : if intersecting joint axis 2 choices for normal
    //   Frame assignment of the DH convention : Z(i-1) follows axis
    //   (i-1) X(i-1) is the normal between axis(i-1) and axis(i)
    //   Y(i-1) follows out of Z(i-1) and X(i-1)
    //
    //     a(i-1)     = distance from Z(i-1) to Z(i) along X(i-1)
    //     alpha(i-1) = angle between Z(i-1) to Z(i) along X(i-1)
    //     d(i)       = distance from X(i-1) to X(i) along Z(i)
    //     theta(i)   = angle between X(i-1) to X(i) along X(i)
    // \endverbatim
    */
     static Frame DH_Craig1989(double a,double alpha,double d,double theta);

    // DH : constructs a transformationmatrix T_link(i-1)_link(i) with
    // the Denavit-Hartenberg convention as described in the original
    // publictation: Denavit, J. and Hartenberg, R. S., A kinematic
    // notation for lower-pair mechanisms based on matrices, ASME
    // Journal of Applied Mechanics, 23:215-221, 1955.

     static Frame DH(double a,double alpha,double d,double theta);


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Frame& a,const Frame& b,double eps);

	 //! The literal equality operator==(), also identical.
     inline friend bool operator==(const Frame& a,const Frame& b);
	 //! The literal inequality operator!=().
     inline friend bool operator!=(const Frame& a,const Frame& b);
};

/**
 * \brief represents both translational and rotational velocities.
 *
 * This class represents a twist.  A twist is the combination of translational
 * velocity and rotational velocity applied at one point.
*/
class Twist {
public:
    Vector vel; //!< The velocity of that point
    Vector rot; //!< The rotational velocity of that point.
public:

    //! The default constructor initialises to Zero via the constructor of Vector.
    Twist():vel(),rot() {};

    Twist(const Vector& _vel,const Vector& _rot):vel(_vel),rot(_rot) {};

    inline Twist& operator-=(const Twist& arg);
    inline Twist& operator+=(const Twist& arg);
    //! index-based access to components, first vel(0..2), then rot(3..5)
    inline double& operator()(int i);

    //! index-based access to components, first vel(0..2), then rot(3..5)
    //! For use with a const Twist
    inline double operator()(int i) const;

     double operator[] ( int index ) const
       {
	 return this->operator() ( index );
       }

     double& operator[] ( int index )
       {
	 return this->operator() ( index );
       }

     inline friend Twist operator*(const Twist& lhs,double rhs);
     inline friend Twist operator*(double lhs,const Twist& rhs);
     inline friend Twist operator/(const Twist& lhs,double rhs);
     inline friend Twist operator+(const Twist& lhs,const Twist& rhs);
     inline friend Twist operator-(const Twist& lhs,const Twist& rhs);
     inline friend Twist operator-(const Twist& arg);
     inline friend double dot(const Twist& lhs,const Wrench& rhs);
     inline friend double dot(const Wrench& rhs,const Twist& lhs);
     inline friend void SetToZero(Twist& v);
    /// Spatial cross product for 6d motion vectors, beware all of them have to be expressed in the same reference frame/point
    inline friend Twist operator*(const Twist& lhs,const Twist& rhs);
    /// Spatial cross product for 6d force vectors, beware all of them have to be expressed in the same reference frame/point
    inline friend Wrench operator*(const Twist& lhs,const Wrench& rhs);

     //! @return a zero Twist : Twist(Vector::Zero(),Vector::Zero())
     static inline Twist Zero();

     //! Reverses the sign of the twist
     inline void ReverseSign();

     //! Changes the reference point of the twist.
     //! The vector v_base_AB is expressed in the same base as the twist
     //! The vector v_base_AB is a vector from the old point to
     //! the new point.
     //!
     //! Complexity : 6M+6A
     inline Twist RefPoint(const Vector& v_base_AB) const;


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Twist& a,const Twist& b,double eps);

	 //! The literal equality operator==(), also identical.
     inline friend bool operator==(const Twist& a,const Twist& b);
	 //! The literal inequality operator!=().
     inline friend bool operator!=(const Twist& a,const Twist& b);

// = Friends
    friend class Rotation;
    friend class Frame;

};

/**
 * 	\brief represents both translational and rotational acceleration.
 *
 * 	This class represents an acceleration twist.  A acceleration twist is
 * 	the combination of translational
 *	acceleration and rotational acceleration applied at one point.
*/
/*
class AccelerationTwist {
public:
    Vector trans; //!< The translational acceleration of that point
    Vector rot; //!< The rotational acceleration of that point.
public:

    //! The default constructor initialises to Zero via the constructor of Vector.
    AccelerationTwist():trans(),rot() {};

    AccelerationTwist(const Vector& _trans,const Vector& _rot):trans(_trans),rot(_rot) {};

    inline AccelerationTwist& operator-=(const AccelerationTwist& arg);
    inline AccelerationTwist& operator+=(const AccelerationTwist& arg);
    //! index-based access to components, first vel(0..2), then rot(3..5)
    inline double& operator()(int i);

    //! index-based access to components, first vel(0..2), then rot(3..5)
    //! For use with a const AccelerationTwist
    inline double operator()(int i) const;

    double operator[] ( int index ) const
    {
    	return this->operator() ( index );
	}

     double& operator[] ( int index )
     {
    	 return this->operator() ( index );
     }

     inline friend AccelerationTwist operator*(const AccelerationTwist& lhs,double rhs);
     inline friend AccelerationTwist operator*(double lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator/(const AccelerationTwist& lhs,double rhs);
     inline friend AccelerationTwist operator+(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator-(const AccelerationTwist& lhs,const AccelerationTwist& rhs);
     inline friend AccelerationTwist operator-(const AccelerationTwist& arg);
     //inline friend double dot(const AccelerationTwist& lhs,const Wrench& rhs);
     //inline friend double dot(const Wrench& rhs,const AccelerationTwist& lhs);
     inline friend void SetToZero(AccelerationTwist& v);


     //! @return a zero AccelerationTwist : AccelerationTwist(Vector::Zero(),Vector::Zero())
     static inline AccelerationTwist Zero();

     //! Reverses the sign of the AccelerationTwist
     inline void ReverseSign();

     //! Changes the reference point of the AccelerationTwist.
     //! The vector v_base_AB is expressed in the same base as the AccelerationTwist
     //! The vector v_base_AB is a vector from the old point to
     //! the new point.
     //!
     //! Complexity : 6M+6A
     inline AccelerationTwist RefPoint(const Vector& v_base_AB) const;


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const AccelerationTwist& a,const AccelerationTwist& b,double eps=epsilon);

	 //! The literal equality operator==(), also identical.
     inline friend bool operator==(const AccelerationTwist& a,const AccelerationTwist& b);
	 //! The literal inequality operator!=().
     inline friend bool operator!=(const AccelerationTwist& a,const AccelerationTwist& b);

// = Friends
    friend class Rotation;
    friend class Frame;

};
*/
/**
 * \brief represents the combination of a force and a torque.
 *
 * This class represents a Wrench.  A Wrench is the force and torque applied at a point
 */
class Wrench
{
public:
    Vector force;       //!< Force that is applied at the origin of the current ref frame
    Vector torque;      //!< Torque that is applied at the origin of the current ref frame
public:

    //! Does  initialise force and torque to zero via the underlying constructor of Vector
    Wrench():force(),torque() {};
    Wrench(const Vector& _force,const Vector& _torque):force(_force),torque(_torque) {};

// = Operators
     inline Wrench& operator-=(const Wrench& arg);
     inline Wrench& operator+=(const Wrench& arg);

     //! index-based access to components, first force(0..2), then torque(3..5)
     inline double& operator()(int i);

     //! index-based access to components, first force(0..2), then torque(3..5)
     //! for use with a const Wrench
     inline double operator()(int i) const;

     double operator[] ( int index ) const
       {
	 return this->operator() ( index );
       }

     double& operator[] ( int index )
       {
	 return this->operator() ( index );
       }

     //! Scalar multiplication
     inline friend Wrench operator*(const Wrench& lhs,double rhs);
     //! Scalar multiplication
     inline friend Wrench operator*(double lhs,const Wrench& rhs);
     //! Scalar division
     inline friend Wrench operator/(const Wrench& lhs,double rhs);

     inline friend Wrench operator+(const Wrench& lhs,const Wrench& rhs);
     inline friend Wrench operator-(const Wrench& lhs,const Wrench& rhs);

     //! An unary - operator
     inline friend Wrench operator-(const Wrench& arg);

     //! Sets the Wrench to Zero, to have a uniform function that sets an object or
     //! double to zero.
     inline friend void SetToZero(Wrench& v);

     //! @return a zero Wrench
     static inline Wrench Zero();

     //! Reverses the sign of the current Wrench
     inline void ReverseSign();

     //! Changes the reference point of the wrench.
     //! The vector v_base_AB is expressed in the same base as the twist
     //! The vector v_base_AB is a vector from the old point to
     //! the new point.
     //!
     //! Complexity : 6M+6A
     inline Wrench RefPoint(const Vector& v_base_AB) const;


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Wrench& a,const Wrench& b,double eps);

	 //! The literal equality operator==(), also identical.
     inline friend bool operator==(const Wrench& a,const Wrench& b);
	 //! The literal inequality operator!=().
     inline friend bool operator!=(const Wrench& a,const Wrench& b);

    friend class Rotation;
    friend class Frame;


};


//! 2D version of Vector
class Vector2
{
    double data[2];
public:
     //! Does not initialise to Zero().
     Vector2() {data[0]=data[1] = 0.0;}
     inline Vector2(double x,double y);
     inline Vector2(const Vector2& arg);

     inline Vector2& operator = ( const Vector2& arg);

     //! Access to elements, range checked when NDEBUG is not set, from 0..1
     inline double operator()(int index) const;

     //! Access to elements, range checked when NDEBUG is not set, from 0..1
     inline double& operator() (int index);

    //! Equivalent to double operator()(int index) const
	double operator[] ( int index ) const
	{
		return this->operator() ( index );
	}

	//! Equivalent to double& operator()(int index)
	double& operator[] ( int index )
	{
		return this->operator() ( index );
	}

	inline double x() const;
	inline double y() const;
	inline void x(double);
	inline void y(double);

     inline void ReverseSign();
     inline Vector2& operator-=(const Vector2& arg);
     inline Vector2& operator +=(const Vector2& arg);


     inline friend Vector2 operator*(const Vector2& lhs,double rhs);
     inline friend Vector2 operator*(double lhs,const Vector2& rhs);
     inline friend Vector2 operator/(const Vector2& lhs,double rhs);
     inline friend Vector2 operator+(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator-(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator*(const Vector2& lhs,const Vector2& rhs);
     inline friend Vector2 operator-(const Vector2& arg);
     inline friend void SetToZero(Vector2& v);

     //! @return a zero 2D vector.
     inline static Vector2 Zero();

   /** Normalizes this vector and returns it norm
	* makes v a unitvector and returns the norm of v.
	* if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
	* if this is not good, check the return value of this method.
	*/
     double Normalize(double eps=epsilon);

     //!  @return the norm of the vector
     double Norm() const;

     //! projects v in its XY plane, and sets *this to these values
     inline void Set3DXY(const Vector& v);

     //! projects v in its YZ plane, and sets *this to these values
     inline void Set3DYZ(const Vector& v);

     //! projects v in its ZX plane, and sets *this to these values
     inline void Set3DZX(const Vector& v);

     //! projects v_someframe in the XY plane of F_someframe_XY,
     //! and sets *this to these values
     //! expressed wrt someframe.
     inline void Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe);


     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Vector2& a,const Vector2& b,double eps);

	//! The literal equality operator==(), also identical.
	inline friend bool operator==(const Vector2& a,const Vector2& b);
	//! The literal inequality operator!=().
	inline friend bool operator!=(const Vector2& a,const Vector2& b);

    friend class Rotation2;
};


//! A 2D Rotation class, for conventions see Rotation. For further documentation
//! of the methods see Rotation class.
class Rotation2
{
    double s,c;
    //! c,s represent  cos(angle), sin(angle), this also represents first col. of rot matrix
    //! from outside, this class behaves as if it would store the complete 2x2 matrix.
public:
    //! Default constructor does NOT initialise to Zero().
    Rotation2() {c=1.0;s=0.0;}

    explicit Rotation2(double angle_rad):s(sin(angle_rad)),c(cos(angle_rad)) {}

    Rotation2(double ca,double sa):s(sa),c(ca){}

     inline Rotation2& operator=(const Rotation2& arg);
     inline Vector2 operator*(const Vector2& v) const;
     //!    Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set
     inline double operator() (int i,int j) const;

     inline friend Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs);

     inline void SetInverse();
     inline Rotation2 Inverse() const;
     inline Vector2 Inverse(const Vector2& v) const;

     inline void SetIdentity();
     inline static Rotation2 Identity();


     //! The SetRot.. functions set the value of *this to the appropriate rotation matrix.
     inline void SetRot(double angle);

     //! The Rot... static functions give the value of the appropriate rotation matrix bac
     inline static Rotation2 Rot(double angle);

     //! Gets the angle (in radians)
     inline double GetRot() const;

     //! do not use operator == because the definition of Equal(.,.) is slightly
     //! different.  It compares whether the 2 arguments are equal in an eps-interval
     inline friend bool Equal(const Rotation2& a,const Rotation2& b,double eps);
};

//! A 2D frame class, for further documentation see the Frames class
//! for methods with unchanged semantics.
class Frame2
 {
public:
    Vector2 p;          //!< origine of the Frame
    Rotation2 M;        //!< Orientation of the Frame

public:

     inline Frame2(const Rotation2& R,const Vector2& V);
     explicit inline Frame2(const Vector2& V);
     explicit inline Frame2(const Rotation2& R);
     inline Frame2(void);
     inline Frame2(const Frame2& arg);
     inline void Make4x4(double* d);

     //!  Treats a frame as a 3x3 matrix and returns element i,j
     //!    Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set
     inline double operator()(int i,int j);

     //!  Treats a frame as a 4x4 matrix and returns element i,j
     //!  Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
     inline double operator() (int i,int j) const;

     inline void SetInverse();
     inline Frame2 Inverse() const;
     inline Vector2 Inverse(const Vector2& arg) const;
     inline Frame2& operator = (const Frame2& arg);
     inline Vector2 operator * (const Vector2& arg);
     inline friend Frame2 operator *(const Frame2& lhs,const Frame2& rhs);
     inline void SetIdentity();
     inline void Integrate(const Twist& t_this,double frequency);
     inline static Frame2 Identity() {
        Frame2 tmp;
        tmp.SetIdentity();
        return tmp;
     }
     inline friend bool Equal(const Frame2& a,const Frame2& b,double eps);
};

/**
 * determines the difference of vector b with vector a.
 *
 * see diff for Rotation matrices for further background information.
 *
 * \param p_w_a start vector a expressed to some frame w
 * \param p_w_b end vector   b expressed to some frame w .
 * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place.
 * \return the difference (b-a) expressed to the frame w.
 */
IMETHOD Vector diff(const Vector& p_w_a,const Vector& p_w_b,double dt=1);


/**
 * determines the (scaled) rotation axis necessary to rotate from b1 to b2.  
 *
 * This rotation axis is expressed w.r.t. frame a.  The rotation axis is scaled
 * by the necessary rotation angle. The rotation angle is always in the 
 * (inclusive) interval \f$ [0 , \pi] \f$.
 *
 * This definition is chosen in this way to facilitate numerical differentiation.
 * With this definition diff(a,b) == -diff(b,a).
 *
 * The diff() function is overloaded for all classes in frames.hpp and framesvel.hpp, such that 
 * numerical differentiation, equality checks with tolerances, etc.  can be performed 
 * without caring exactly on which type the operation is performed.  
 *  
 * \param R_a_b1: The rotation matrix \f$ _a^{b1} R  \f$ of b1 with respect to frame a. 
 * \param R_a_b2: The Rotation matrix \f$ _a^{b2} R \f$ of b2 with respect to frame a. 
 * \param dt [optional][obsolete] time interval over which the numerical differentiation takes place. By default this is set to 1.0.
 * \return rotation axis to rotate from b1 to b2, scaled by the rotation angle, expressed in frame a.
 * \warning - The result is not a rotational vector, i.e. it is not a mathematical vector.
 *          (no communitative addition). 
 *
 * \warning - When used in the context of numerical differentiation, with the frames b1 and b2 very
 *           close to each other, the semantics correspond to the twist, scaled by the time. 
 *
 * \warning - For angles equal to \f$ \pi \f$, The negative of the
 *          return value is equally valid. 
 */
IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt=1);

/**
 * determines the rotation axis necessary to rotate the frame b1 to the same orientation as frame b2 and the vector
 * necessary to translate the origin of b1 to the origin of b2, and stores the result in a Twist datastructure.   
 * \param F_a_b1 frame b1 expressed with respect to some frame a. 
 * \param F_a_b2 frame b2 expressed with respect to some frame a. 
 * \warning The result is not a Twist!  
 * see diff() for Rotation and Vector arguments for further detail on the semantics.
 */
IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt=1);

/**
 * determines the difference between two twists i.e. the difference between
 * the underlying velocity vectors and rotational velocity vectors. 
 */
IMETHOD Twist diff(const Twist& a,const Twist& b,double dt=1);

/**
 * determines the difference between two wrenches i.e. the difference between
 * the underlying torque vectors and force vectors. 
 */
IMETHOD Wrench diff(const Wrench& W_a_p1,const Wrench& W_a_p2,double dt=1);

/**
 * \brief adds vector da to vector a.
 * see also the corresponding diff() routine.
 * \param p_w_a vector a expressed to some frame w.
 * \param p_w_da vector da expressed to some frame w.
 * \returns the vector resulting from the displacement of vector a by vector da, expressed in the frame w.
 */
IMETHOD Vector addDelta(const Vector& p_w_a,const Vector& p_w_da,double dt=1);

/**
 * returns the rotation matrix resulting from the rotation of frame a by the axis and angle
 * specified with da_w.  
 *
 * see also the corresponding diff() routine.
 *
 * \param R_w_a Rotation matrix of frame a expressed to some frame w.
 * \param da_w  axis and angle of the rotation expressed to some frame w.
 * \returns the rotation matrix resulting from the rotation of frame a by the axis and angle
 *          specified with da.   The resulting rotation matrix is expressed with respect to
 *          frame w.
 */
IMETHOD Rotation addDelta(const Rotation& R_w_a,const Vector& da_w,double dt=1);

/**
 * returns the frame resulting from the rotation of frame a by the axis and angle
 * specified in da_w and the translation of the origin (also specified in da_w). 
 *
 * see also the corresponding diff() routine.
 * \param R_w_a Rotation matrix of frame a expressed to some frame w.
 * \param da_w  axis and angle of the rotation (da_w.rot), together with a displacement vector for the origin (da_w.vel),  expressed to some frame w.
 * \returns the frame resulting from the rotation of frame a by the axis and angle
 *          specified with da.rot, and the translation of the origin da_w.vel .  The resulting frame is expressed with respect to frame w.
 */
IMETHOD Frame addDelta(const Frame& F_w_a,const Twist& da_w,double dt=1);

/**
 * \brief adds the twist da to the twist a.
 * see also the corresponding diff() routine.
 * \param a a twist wrt some frame
 * \param da a twist difference wrt some frame
 * \returns The twist (a+da) wrt the corresponding frame.
 */
IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt=1);


/**
 * \brief adds the wrench da to the wrench w.
 * see also the corresponding diff() routine.
 * see also the corresponding diff() routine.
 * \param a a wrench wrt some frame
 * \param da a wrench difference wrt some frame
 * \returns the wrench (a+da) wrt the corresponding frame.
 */
IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt=1);

#ifdef KDL_INLINE
//    #include "vector.inl"
//   #include "wrench.inl"
    //#include "rotation.inl"
    //#include "frame.inl"
    //#include "twist.inl"
    //#include "vector2.inl"
    //#include "rotation2.inl"
    //#include "frame2.inl"
#include "frames.inl"
#endif



}


#endif