orocos_kdl
frames.inl
Go to the documentation of this file.
1 /***************************************************************************
2  frames.inl - description
3  -------------------------
4  begin : June 2006
5  copyright : (C) 2006 Erwin Aertbelien
6  email : firstname.lastname@mech.kuleuven.ac.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., 59 Temple Place, *
24  * Suite 330, Boston, MA 02111-1307 USA *
25  * *
26  ***************************************************************************/
27 
36 IMETHOD Vector::Vector(const Vector & arg)
37 {
38  data[0] = arg.data[0];
39  data[1] = arg.data[1];
40  data[2] = arg.data[2];
41 }
42 
43 IMETHOD Vector::Vector(double x,double y, double z)
44 {
45  data[0]=x;data[1]=y;data[2]=z;
46 }
47 
48 
49 IMETHOD Vector& Vector::operator =(const Vector & arg)
50 {
51  data[0] = arg.data[0];
52  data[1] = arg.data[1];
53  data[2] = arg.data[2];
54  return *this;
55 }
56 
57 IMETHOD Vector operator +(const Vector & lhs,const Vector& rhs)
58 {
59  Vector tmp;
60  tmp.data[0] = lhs.data[0]+rhs.data[0];
61  tmp.data[1] = lhs.data[1]+rhs.data[1];
62  tmp.data[2] = lhs.data[2]+rhs.data[2];
63  return tmp;
64 }
65 
66 IMETHOD Vector operator -(const Vector & lhs,const Vector& rhs)
67 {
68  Vector tmp;
69  tmp.data[0] = lhs.data[0]-rhs.data[0];
70  tmp.data[1] = lhs.data[1]-rhs.data[1];
71  tmp.data[2] = lhs.data[2]-rhs.data[2];
72  return tmp;
73 }
74 
75 IMETHOD double Vector::x() const { return data[0]; }
76 IMETHOD double Vector::y() const { return data[1]; }
77 IMETHOD double Vector::z() const { return data[2]; }
78 
79 IMETHOD void Vector::x( double _x ) { data[0] = _x; }
80 IMETHOD void Vector::y( double _y ) { data[1] = _y; }
81 IMETHOD void Vector::z( double _z ) { data[2] = _z; }
82 
83 Vector operator *(const Vector& lhs,double rhs)
84 {
85  Vector tmp;
86  tmp.data[0] = lhs.data[0]*rhs;
87  tmp.data[1] = lhs.data[1]*rhs;
88  tmp.data[2] = lhs.data[2]*rhs;
89  return tmp;
90 }
91 
92 Vector operator *(double lhs,const Vector& rhs)
93 {
94  Vector tmp;
95  tmp.data[0] = lhs*rhs.data[0];
96  tmp.data[1] = lhs*rhs.data[1];
97  tmp.data[2] = lhs*rhs.data[2];
98  return tmp;
99 }
100 
101 Vector operator /(const Vector& lhs,double rhs)
102 {
103  Vector tmp;
104  tmp.data[0] = lhs.data[0]/rhs;
105  tmp.data[1] = lhs.data[1]/rhs;
106  tmp.data[2] = lhs.data[2]/rhs;
107  return tmp;
108 }
109 
110 Vector operator *(const Vector & lhs,const Vector& rhs)
111 // Complexity : 6M+3A
112 {
113  Vector tmp;
114  tmp.data[0] = lhs.data[1]*rhs.data[2]-lhs.data[2]*rhs.data[1];
115  tmp.data[1] = lhs.data[2]*rhs.data[0]-lhs.data[0]*rhs.data[2];
116  tmp.data[2] = lhs.data[0]*rhs.data[1]-lhs.data[1]*rhs.data[0];
117  return tmp;
118 }
119 
120 Vector& Vector::operator +=(const Vector & arg)
121 // Complexity : 3A
122 {
123  data[0]+=arg.data[0];
124  data[1]+=arg.data[1];
125  data[2]+=arg.data[2];
126  return *this;
127 }
128 
129 Vector& Vector::operator -=(const Vector & arg)
130 // Complexity : 3A
131 {
132  data[0]-=arg.data[0];
133  data[1]-=arg.data[1];
134  data[2]-=arg.data[2];
135  return *this;
136 }
137 
138 Vector Vector::Zero()
139 {
140  return Vector(0,0,0);
141 }
142 
143 double Vector::operator()(int index) const {
144  FRAMES_CHECKI((0<=index)&&(index<=2));
145  return data[index];
146 }
147 
148 double& Vector::operator () (int index)
149 {
150  FRAMES_CHECKI((0<=index)&&(index<=2));
151  return data[index];
152 }
153 
154 
155 
157 // Complexity : 24M+18A
158 {
159  Wrench tmp;
160  tmp.force = M*arg.force;
161  tmp.torque = M*arg.torque + p*tmp.force;
162  return tmp;
163 }
164 
165 Wrench Frame::Inverse(const Wrench& arg) const
166 {
167  Wrench tmp;
168  tmp.force = M.Inverse(arg.force);
169  tmp.torque = M.Inverse(arg.torque-p*arg.force);
170  return tmp;
171 }
172 
173 
174 
175 Wrench Rotation::Inverse(const Wrench& arg) const
176 {
177  return Wrench(Inverse(arg.force),Inverse(arg.torque));
178 }
179 
180 Twist Rotation::Inverse(const Twist& arg) const
181 {
182  return Twist(Inverse(arg.vel),Inverse(arg.rot));
183 }
184 
185 Wrench Wrench::Zero()
186 {
187  return Wrench(Vector::Zero(),Vector::Zero());
188 }
189 
190 
191 void Wrench::ReverseSign()
192 {
193  torque.ReverseSign();
194  force.ReverseSign();
195 }
196 
197 Wrench Wrench::RefPoint(const Vector& v_base_AB) const
198  // Changes the reference point of the Wrench.
199  // The vector v_base_AB is expressed in the same base as the twist
200  // The vector v_base_AB is a vector from the old point to
201  // the new point.
202 {
203  return Wrench(this->force,
204  this->torque+this->force*v_base_AB
205  );
206 }
207 
208 
209 Wrench& Wrench::operator-=(const Wrench& arg)
210 {
211  torque-=arg.torque;
212  force -=arg.force;
213  return *this;
214 }
215 
216 Wrench& Wrench::operator+=(const Wrench& arg)
217 {
218  torque+=arg.torque;
219  force +=arg.force;
220  return *this;
221 }
222 
223 double& Wrench::operator()(int i)
224 {
225  // assert((0<=i)&&(i<6)); done by underlying routines
226  if (i<3)
227  return force(i);
228  else
229  return torque(i-3);
230 }
231 
232 double Wrench::operator()(int i) const
233 {
234  // assert((0<=i)&&(i<6)); done by underlying routines
235  if (i<3)
236  return force(i);
237  else
238  return torque(i-3);
239 }
240 
241 
242 Wrench operator*(const Wrench& lhs,double rhs)
243 {
244  return Wrench(lhs.force*rhs,lhs.torque*rhs);
245 }
246 
247 Wrench operator*(double lhs,const Wrench& rhs)
248 {
249  return Wrench(lhs*rhs.force,lhs*rhs.torque);
250 }
251 
252 Wrench operator/(const Wrench& lhs,double rhs)
253 {
254  return Wrench(lhs.force/rhs,lhs.torque/rhs);
255 }
256 
257 // addition of Wrench's
258 Wrench operator+(const Wrench& lhs,const Wrench& rhs)
259 {
260  return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
261 }
262 
263 Wrench operator-(const Wrench& lhs,const Wrench& rhs)
264 {
265  return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
266 }
267 
268 // unary -
269 Wrench operator-(const Wrench& arg)
270 {
271  return Wrench(-arg.force,-arg.torque);
272 }
273 
274 Twist Frame::operator * (const Twist& arg) const
275 // Complexity : 24M+18A
276 {
277  Twist tmp;
278  tmp.rot = M*arg.rot;
279  tmp.vel = M*arg.vel+p*tmp.rot;
280  return tmp;
281 }
282 Twist Frame::Inverse(const Twist& arg) const
283 {
284  Twist tmp;
285  tmp.rot = M.Inverse(arg.rot);
286  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
287  return tmp;
288 }
289 
290 Twist Twist::Zero()
291 {
292  return Twist(Vector::Zero(),Vector::Zero());
293 }
294 
295 
296 void Twist::ReverseSign()
297 {
298  vel.ReverseSign();
299  rot.ReverseSign();
300 }
301 
302 Twist Twist::RefPoint(const Vector& v_base_AB) const
303  // Changes the reference point of the twist.
304  // The vector v_base_AB is expressed in the same base as the twist
305  // The vector v_base_AB is a vector from the old point to
306  // the new point.
307  // Complexity : 6M+6A
308 {
309  return Twist(this->vel+this->rot*v_base_AB,this->rot);
310 }
311 
312 Twist& Twist::operator-=(const Twist& arg)
313 {
314  vel-=arg.vel;
315  rot -=arg.rot;
316  return *this;
317 }
318 
319 Twist& Twist::operator+=(const Twist& arg)
320 {
321  vel+=arg.vel;
322  rot +=arg.rot;
323  return *this;
324 }
325 
326 double& Twist::operator()(int i)
327 {
328  // assert((0<=i)&&(i<6)); done by underlying routines
329  if (i<3)
330  return vel(i);
331  else
332  return rot(i-3);
333 }
334 
335 double Twist::operator()(int i) const
336 {
337  // assert((0<=i)&&(i<6)); done by underlying routines
338  if (i<3)
339  return vel(i);
340  else
341  return rot(i-3);
342 }
343 
344 
345 Twist operator*(const Twist& lhs,double rhs)
346 {
347  return Twist(lhs.vel*rhs,lhs.rot*rhs);
348 }
349 
350 Twist operator*(double lhs,const Twist& rhs)
351 {
352  return Twist(lhs*rhs.vel,lhs*rhs.rot);
353 }
354 
355 Twist operator/(const Twist& lhs,double rhs)
356 {
357  return Twist(lhs.vel/rhs,lhs.rot/rhs);
358 }
359 
360 // addition of Twist's
361 Twist operator+(const Twist& lhs,const Twist& rhs)
362 {
363  return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
364 }
365 
366 Twist operator-(const Twist& lhs,const Twist& rhs)
367 {
368  return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
369 }
370 
371 // unary -
372 Twist operator-(const Twist& arg)
373 {
374  return Twist(-arg.vel,-arg.rot);
375 }
376 
377 
378 //Spatial products for twists
379 Twist operator*(const Twist& lhs,const Twist& rhs)
380 {
381  return Twist(lhs.rot*rhs.vel+lhs.vel*rhs.rot,lhs.rot*rhs.rot);
382 }
383 Wrench operator*(const Twist& lhs,const Wrench& rhs)
384 {
385  return Wrench(lhs.rot*rhs.force,lhs.rot*rhs.torque+lhs.vel*rhs.force);
386 }
387 
388 Frame::Frame(const Rotation & R)
389 {
390  M=R;
391  p=Vector::Zero();
392 }
393 
394 Frame::Frame(const Vector & V)
395 {
396  M = Rotation::Identity();
397  p = V;
398 }
399 
400 Frame::Frame(const Rotation & R, const Vector & V)
401 {
402  M = R;
403  p = V;
404 }
405 
406  Frame operator *(const Frame& lhs,const Frame& rhs)
407 // Complexity : 36M+36A
408 {
409  return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
410 }
411 
412 Vector Frame::operator *(const Vector & arg) const
413 {
414  return M*arg+p;
415 }
416 
417 Vector Frame::Inverse(const Vector& arg) const
418 {
419  return M.Inverse(arg-p);
420 }
421 
422 Frame Frame::Inverse() const
423 {
424  return Frame(M.Inverse(),-M.Inverse(p));
425 }
426 
427 
428 Frame& Frame::operator =(const Frame & arg)
429 {
430  M = arg.M;
431  p = arg.p;
432  return *this;
433 }
434 
435 Frame::Frame(const Frame & arg) :
436  p(arg.p),M(arg.M)
437 {}
438 
439 
441 {
442  data[0] = -data[0];
443  data[1] = -data[1];
444  data[2] = -data[2];
445 }
446 
447 
448 
449 Vector operator-(const Vector & arg)
450 {
451  Vector tmp;
452  tmp.data[0]=-arg.data[0];
453  tmp.data[1]=-arg.data[1];
454  tmp.data[2]=-arg.data[2];
455  return tmp;
456 }
457 
458 void Vector::Set2DXY(const Vector2& v)
459 // a 3D vector where the 2D vector v is put in the XY plane
460 {
461  data[0]=v(0);
462  data[1]=v(1);
463  data[2]=0;
464 
465 }
466 void Vector::Set2DYZ(const Vector2& v)
467 // a 3D vector where the 2D vector v is put in the YZ plane
468 {
469  data[1]=v(0);
470  data[2]=v(1);
471  data[0]=0;
472 
473 }
474 
475 void Vector::Set2DZX(const Vector2& v)
476 // a 3D vector where the 2D vector v is put in the ZX plane
477 {
478  data[2]=v(0);
479  data[0]=v(1);
480  data[1]=0;
481 
482 }
483 
484 
485 
486 
487 
488 double& Rotation::operator()(int i,int j) {
489  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
490  return data[i*3+j];
491 }
492 
493 double Rotation::operator()(int i,int j) const {
494  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
495  return data[i*3+j];
496 }
497 
498 Rotation::Rotation( double Xx,double Yx,double Zx,
499  double Xy,double Yy,double Zy,
500  double Xz,double Yz,double Zz)
501 {
502  data[0] = Xx;data[1]=Yx;data[2]=Zx;
503  data[3] = Xy;data[4]=Yy;data[5]=Zy;
504  data[6] = Xz;data[7]=Yz;data[8]=Zz;
505 }
506 
507 
508 Rotation::Rotation(const Vector& x,const Vector& y,const Vector& z)
509 {
510  data[0] = x.data[0];data[3] = x.data[1];data[6] = x.data[2];
511  data[1] = y.data[0];data[4] = y.data[1];data[7] = y.data[2];
512  data[2] = z.data[0];data[5] = z.data[1];data[8] = z.data[2];
513 }
514 
516  int count=9;
517  while (count--) data[count] = arg.data[count];
518 }
519 
521  int count=9;
522  while (count--) data[count] = arg.data[count];
523  return *this;
524 }
525 
527 // Complexity : 9M+6A
528  return Vector(
529  data[0]*v.data[0] + data[1]*v.data[1] + data[2]*v.data[2],
530  data[3]*v.data[0] + data[4]*v.data[1] + data[5]*v.data[2],
531  data[6]*v.data[0] + data[7]*v.data[1] + data[8]*v.data[2]
532  );
533 }
534 
536  // Transformation of the base to which the twist is expressed.
537  // look at Frame*Twist for a transformation that also transforms
538  // the velocity reference point.
539  // Complexity : 18M+12A
540 {
541  return Twist((*this)*arg.vel,(*this)*arg.rot);
542 }
543 
545  // Transformation of the base to which the wrench is expressed.
546  // look at Frame*Twist for a transformation that also transforms
547  // the force reference point.
548 {
549  return Wrench((*this)*arg.force,(*this)*arg.torque);
550 }
551 
553  return Rotation(1,0,0,0,1,0,0,0,1);
554 }
555 // *this = *this * ROT(X,angle)
556 void Rotation::DoRotX(double angle)
557 {
558  double cs = cos(angle);
559  double sn = sin(angle);
560  double x1,x2,x3;
561  x1 = cs* (*this)(0,1) + sn* (*this)(0,2);
562  x2 = cs* (*this)(1,1) + sn* (*this)(1,2);
563  x3 = cs* (*this)(2,1) + sn* (*this)(2,2);
564  (*this)(0,2) = -sn* (*this)(0,1) + cs* (*this)(0,2);
565  (*this)(1,2) = -sn* (*this)(1,1) + cs* (*this)(1,2);
566  (*this)(2,2) = -sn* (*this)(2,1) + cs* (*this)(2,2);
567  (*this)(0,1) = x1;
568  (*this)(1,1) = x2;
569  (*this)(2,1) = x3;
570 }
571 
572 void Rotation::DoRotY(double angle)
573 {
574  double cs = cos(angle);
575  double sn = sin(angle);
576  double x1,x2,x3;
577  x1 = cs* (*this)(0,0) - sn* (*this)(0,2);
578  x2 = cs* (*this)(1,0) - sn* (*this)(1,2);
579  x3 = cs* (*this)(2,0) - sn* (*this)(2,2);
580  (*this)(0,2) = sn* (*this)(0,0) + cs* (*this)(0,2);
581  (*this)(1,2) = sn* (*this)(1,0) + cs* (*this)(1,2);
582  (*this)(2,2) = sn* (*this)(2,0) + cs* (*this)(2,2);
583  (*this)(0,0) = x1;
584  (*this)(1,0) = x2;
585  (*this)(2,0) = x3;
586 }
587 
588 void Rotation::DoRotZ(double angle)
589 {
590  double cs = cos(angle);
591  double sn = sin(angle);
592  double x1,x2,x3;
593  x1 = cs* (*this)(0,0) + sn* (*this)(0,1);
594  x2 = cs* (*this)(1,0) + sn* (*this)(1,1);
595  x3 = cs* (*this)(2,0) + sn* (*this)(2,1);
596  (*this)(0,1) = -sn* (*this)(0,0) + cs* (*this)(0,1);
597  (*this)(1,1) = -sn* (*this)(1,0) + cs* (*this)(1,1);
598  (*this)(2,1) = -sn* (*this)(2,0) + cs* (*this)(2,1);
599  (*this)(0,0) = x1;
600  (*this)(1,0) = x2;
601  (*this)(2,0) = x3;
602 }
603 
604 
605 Rotation Rotation::RotX(double angle) {
606  double cs=cos(angle);
607  double sn=sin(angle);
608  return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
609 }
610 Rotation Rotation::RotY(double angle) {
611  double cs=cos(angle);
612  double sn=sin(angle);
613  return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
614 }
615 Rotation Rotation::RotZ(double angle) {
616  double cs=cos(angle);
617  double sn=sin(angle);
618  return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
619 }
620 
621 
622 
623 
624 void Frame::Integrate(const Twist& t_this,double samplefrequency)
625 {
626  double n = t_this.rot.Norm()/samplefrequency;
627  if (n<epsilon) {
628  p += M*(t_this.vel/samplefrequency);
629  } else {
630  (*this) = (*this) *
631  Frame ( Rotation::Rot( t_this.rot, n ),
632  t_this.vel/samplefrequency
633  );
634  }
635 }
636 
638 {
639  Rotation tmp(*this);
640  tmp.SetInverse();
641  return tmp;
642 }
643 
644 Vector Rotation::Inverse(const Vector& v) const {
645  return Vector(
646  data[0]*v.data[0] + data[3]*v.data[1] + data[6]*v.data[2],
647  data[1]*v.data[0] + data[4]*v.data[1] + data[7]*v.data[2],
648  data[2]*v.data[0] + data[5]*v.data[1] + data[8]*v.data[2]
649  );
650 }
651 
652 
654 {
655  double tmp;
656  tmp = data[1];data[1]=data[3];data[3]=tmp;
657  tmp = data[2];data[2]=data[6];data[6]=tmp;
658  tmp = data[5];data[5]=data[7];data[7]=tmp;
659 }
660 
661 
662 
663 
664 
665 
666 
667 double Frame::operator()(int i,int j) {
668  FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
669  if (i==3) {
670  if (j==3)
671  return 1.0;
672  else
673  return 0.0;
674  } else {
675  if (j==3)
676  return p(i);
677  else
678  return M(i,j);
679 
680  }
681 }
682 
683 double Frame::operator()(int i,int j) const {
684  FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
685  if (i==3) {
686  if (j==3)
687  return 1;
688  else
689  return 0;
690  } else {
691  if (j==3)
692  return p(i);
693  else
694  return M(i,j);
695 
696  }
697 }
698 
699 
702 }
703 
704 
705 
706 
707 void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY)
708 // a 3D vector where the 2D vector v is put in the XY plane of the frame
709 // F_someframe_XY.
710 {
711 Vector tmp_XY;
712 tmp_XY.Set2DXY(v_XY);
713 tmp_XY = F_someframe_XY*(tmp_XY);
714 }
715 
716 
717 
718 
719 
720 
721 
722 
723 
724 //============ 2 dimensional version of the frames objects =============
726 {
727  data[0] = arg.data[0];
728  data[1] = arg.data[1];
729 }
730 
731 IMETHOD Vector2::Vector2(double x,double y)
732 {
733  data[0]=x;data[1]=y;
734 }
735 
736 
738 {
739  data[0] = arg.data[0];
740  data[1] = arg.data[1];
741  return *this;
742 }
743 
744 
745 IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
746 {
747  return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
748 }
749 
750 IMETHOD Vector2 operator -(const Vector2 & lhs,const Vector2& rhs)
751 {
752  return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
753 }
754 
755 IMETHOD Vector2 operator *(const Vector2& lhs,double rhs)
756 {
757  return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
758 }
759 
760 IMETHOD Vector2 operator *(double lhs,const Vector2& rhs)
761 {
762  return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
763 }
764 
765 IMETHOD Vector2 operator /(const Vector2& lhs,double rhs)
766 {
767  return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
768 }
769 
771 {
772  data[0]+=arg.data[0];
773  data[1]+=arg.data[1];
774  return *this;
775 }
776 
778 {
779  data[0]-=arg.data[0];
780  data[1]-=arg.data[1];
781  return *this;
782 }
783 
785  return Vector2(0,0);
786 }
787 
788 IMETHOD double Vector2::operator()(int index) const {
789  FRAMES_CHECKI((0<=index)&&(index<=1));
790  return data[index];
791 }
792 
793 IMETHOD double& Vector2::operator () (int index)
794 {
795  FRAMES_CHECKI((0<=index)&&(index<=1));
796  return data[index];
797 }
798 
799 IMETHOD double Vector2::x() const { return data[0]; }
800 IMETHOD double Vector2::y() const { return data[1]; }
801 
802 IMETHOD void Vector2::x( double _x ) { data[0] = _x; }
803 IMETHOD void Vector2::y( double _y ) { data[1] = _y; }
804 
805 
807 {
808  data[0] = -data[0];
809  data[1] = -data[1];
810 }
811 
812 
813 IMETHOD Vector2 operator-(const Vector2 & arg)
814 {
815  return Vector2(-arg.data[0],-arg.data[1]);
816 }
817 
818 
820 // projects v in its XY plane, and sets *this to these values
821 {
822  data[0]=v(0);
823  data[1]=v(1);
824 }
826 // projects v in its XY plane, and sets *this to these values
827 {
828  data[0]=v(1);
829  data[1]=v(2);
830 }
832 // projects v in its XY plane, and sets *this to these values
833 {
834  data[0]=v(2);
835  data[1]=v(0);
836 }
837 
838 IMETHOD void Vector2::Set3DPlane(const Frame& F_someframe_XY,const Vector& v_someframe)
839 // projects v in the XY plane of F_someframe_XY, and sets *this to these values
840 // expressed wrt someframe.
841 {
842  Vector tmp = F_someframe_XY.Inverse(v_someframe);
843  data[0]=tmp(0);
844  data[1]=tmp(1);
845 }
846 
848  c=arg.c;s=arg.s;
849 }
850 
852  c=arg.c;s=arg.s;
853  return *this;
854 }
855 
857  return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c);
858 }
859 
860 IMETHOD double Rotation2::operator()(int i,int j) const {
861  FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1));
862  if (i==j) return c;
863  if (i==0)
864  return s;
865  else
866  return -s;
867 }
868 
869 
870 IMETHOD Rotation2 operator *(const Rotation2& lhs,const Rotation2& rhs) {
871  return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
872 }
873 
875  s=-s;
876 }
877 
879  return Rotation2(c,-s);
880 }
881 
883  return Vector2(v.data[0]*c+v.data[1]*s,-v.data[0]*s+v.data[1]*c);
884 }
885 
887  return Rotation2(1,0);
888 }
889 
891 {
892  c = 1;
893  s = 0;
894 }
895 
896 IMETHOD void Rotation2::SetRot(double angle) {
897  c=cos(angle);s=sin(angle);
898 }
899 
901  return Rotation2(cos(angle),sin(angle));
902 }
903 
904 IMETHOD double Rotation2::GetRot() const {
905  return atan2(s,c);
906 }
907 
908 
910 }
911 
913 {
914  M=R;
915  p=Vector2::Zero();
916 }
917 
919 {
921  p = V;
922 }
923 
925 {
926  M = R;
927  p = V;
928 }
929 
930 IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs)
931 {
932  return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
933 }
934 
936 {
937  return M*arg+p;
938 }
939 
941 {
942  return M.Inverse(arg-p);
943 }
944 
946 {
947  M.SetIdentity();
948  p = Vector2::Zero();
949 }
950 
952 {
953  M.SetInverse();
954  p = M*p;
955  p.ReverseSign();
956 }
957 
958 
960 {
961  Frame2 tmp(*this);
962  tmp.SetInverse();
963  return tmp;
964 }
965 
967 {
968  M = arg.M;
969  p = arg.p;
970  return *this;
971 }
972 
974  p(arg.p), M(arg.M)
975 {}
976 
977 
978 IMETHOD double Frame2::operator()(int i,int j) {
979  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
980  if (i==2) {
981  if (j==2)
982  return 1;
983  else
984  return 0;
985  } else {
986  if (j==2)
987  return p(i);
988  else
989  return M(i,j);
990 
991  }
992 }
993 
994 IMETHOD double Frame2::operator()(int i,int j) const {
995  FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
996  if (i==2) {
997  if (j==2)
998  return 1;
999  else
1000  return 0;
1001  } else {
1002  if (j==2)
1003  return p(i);
1004  else
1005  return M(i,j);
1006 
1007  }
1008 }
1009 
1010 // Scalar products.
1011 
1012 IMETHOD double dot(const Vector& lhs,const Vector& rhs) {
1013  return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1014 }
1015 
1016 IMETHOD double dot(const Twist& lhs,const Wrench& rhs) {
1017  return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1018 }
1019 
1020 IMETHOD double dot(const Wrench& rhs,const Twist& lhs) {
1021  return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1022 }
1023 
1024 
1025 
1026 
1027 
1028 // Equality operators
1029 
1030 
1031 
1032 IMETHOD bool Equal(const Vector& a,const Vector& b,double eps) {
1033  return (Equal(a.data[0],b.data[0],eps)&&
1034  Equal(a.data[1],b.data[1],eps)&&
1035  Equal(a.data[2],b.data[2],eps) );
1036  }
1037 
1038 
1039 IMETHOD bool Equal(const Frame& a,const Frame& b,double eps) {
1040  return (Equal(a.p,b.p,eps)&&
1041  Equal(a.M,b.M,eps) );
1042 }
1043 
1044 IMETHOD bool Equal(const Wrench& a,const Wrench& b,double eps) {
1045  return (Equal(a.force,b.force,eps)&&
1046  Equal(a.torque,b.torque,eps) );
1047 }
1048 
1049 IMETHOD bool Equal(const Twist& a,const Twist& b,double eps) {
1050  return (Equal(a.rot,b.rot,eps)&&
1051  Equal(a.vel,b.vel,eps) );
1052 }
1053 
1054 IMETHOD bool Equal(const Vector2& a,const Vector2& b,double eps) {
1055  return (Equal(a.data[0],b.data[0],eps)&&
1056  Equal(a.data[1],b.data[1],eps) );
1057  }
1058 
1059 IMETHOD bool Equal(const Rotation2& a,const Rotation2& b,double eps) {
1060  return ( Equal(a.c,b.c,eps) && Equal(a.s,b.s,eps) );
1061 }
1062 
1063 IMETHOD bool Equal(const Frame2& a,const Frame2& b,double eps) {
1064  return (Equal(a.p,b.p,eps)&&
1065  Equal(a.M,b.M,eps) );
1066 }
1067 
1068 IMETHOD void SetToZero(Vector& v) {
1069  v=Vector::Zero();
1070 }
1071 IMETHOD void SetToZero(Twist& v) {
1072  SetToZero(v.rot);
1073  SetToZero(v.vel);
1074 }
1075 IMETHOD void SetToZero(Wrench& v) {
1076  SetToZero(v.force);
1077  SetToZero(v.torque);
1078 }
1079 
1080 IMETHOD void SetToZero(Vector2& v) {
1081  v = Vector2::Zero();
1082 }
1083 
1084 
1086 // The following defines the operations
1087 // diff
1088 // addDelta
1089 // random
1090 // posrandom
1091 // on all the types defined in this library.
1092 // (mostly for uniform integration, differentiation and testing).
1093 // Defined as functions because double is not a class and a method
1094 // would brake uniformity when defined for a double.
1096 
1097 
1098 
1099 
1100 
1101 
1107 IMETHOD Rotation Rot(const Vector& axis_a_b) {
1108  // The formula is
1109  // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
1110  // can be found by multiplying it with an arbitrary vector p
1111  // and noting that this vector is rotated.
1112  Vector rotvec = axis_a_b;
1113  double angle = rotvec.Normalize(1E-10);
1114  double ct = ::cos(angle);
1115  double st = ::sin(angle);
1116  double vt = 1-ct;
1117  return Rotation(
1118  ct + vt*rotvec(0)*rotvec(0),
1119  -rotvec(2)*st + vt*rotvec(0)*rotvec(1),
1120  rotvec(1)*st + vt*rotvec(0)*rotvec(2),
1121  rotvec(2)*st + vt*rotvec(1)*rotvec(0),
1122  ct + vt*rotvec(1)*rotvec(1),
1123  -rotvec(0)*st + vt*rotvec(1)*rotvec(2),
1124  -rotvec(1)*st + vt*rotvec(2)*rotvec(0),
1125  rotvec(0)*st + vt*rotvec(2)*rotvec(1),
1126  ct + vt*rotvec(2)*rotvec(2)
1127  );
1128  }
1129 IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
1130  return (b-a)/dt;
1131 }
1132 
1133 IMETHOD Vector diff(const Rotation& R_a_b1,const Rotation& R_a_b2,double dt) {
1134  Rotation R_b1_b2(R_a_b1.Inverse()*R_a_b2);
1135  return R_a_b1 * R_b1_b2.GetRot() / dt;
1136 }
1137 
1138 IMETHOD Twist diff(const Frame& F_a_b1,const Frame& F_a_b2,double dt) {
1139  return Twist(
1140  diff(F_a_b1.p,F_a_b2.p,dt),
1141  diff(F_a_b1.M,F_a_b2.M,dt)
1142  );
1143 }
1144 IMETHOD Twist diff(const Twist& a,const Twist& b,double dt) {
1145  return Twist(diff(a.vel,b.vel,dt),diff(a.rot,b.rot,dt));
1146 }
1147 
1148 IMETHOD Wrench diff(const Wrench& a,const Wrench& b,double dt) {
1149  return Wrench(
1150  diff(a.force,b.force,dt),
1151  diff(a.torque,b.torque,dt)
1152  );
1153 }
1154 
1155 
1156 IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
1157  return a+da*dt;
1158 }
1159 
1160 IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
1161  return Rot(da*dt)*a;
1162 }
1163 IMETHOD Frame addDelta(const Frame& a,const Twist& da,double dt) {
1164  return Frame(
1165  addDelta(a.M,da.rot,dt),
1166  addDelta(a.p,da.vel,dt)
1167  );
1168 }
1169 IMETHOD Twist addDelta(const Twist& a,const Twist&da,double dt) {
1170  return Twist(addDelta(a.vel,da.vel,dt),addDelta(a.rot,da.rot,dt));
1171 }
1172 IMETHOD Wrench addDelta(const Wrench& a,const Wrench&da,double dt) {
1173  return Wrench(addDelta(a.force,da.force,dt),addDelta(a.torque,da.torque,dt));
1174 }
1175 
1176 
1214 IMETHOD void random(Vector& a) {
1215  random(a[0]);
1216  random(a[1]);
1217  random(a[2]);
1218 }
1219 IMETHOD void random(Twist& a) {
1220  random(a.rot);
1221  random(a.vel);
1222 }
1223 IMETHOD void random(Wrench& a) {
1224  random(a.torque);
1225  random(a.force);
1226 }
1227 
1228 IMETHOD void random(Rotation& R) {
1229  double alfa;
1230  double beta;
1231  double gamma;
1232  random(alfa);
1233  random(beta);
1234  random(gamma);
1235  R = Rotation::EulerZYX(alfa,beta,gamma);
1236 }
1237 
1238 IMETHOD void random(Frame& F) {
1239  random(F.M);
1240  random(F.p);
1241 }
1242 
1243 IMETHOD void posrandom(Vector& a) {
1244  posrandom(a[0]);
1245  posrandom(a[1]);
1246  posrandom(a[2]);
1247 }
1248 IMETHOD void posrandom(Twist& a) {
1249  posrandom(a.rot);
1250  posrandom(a.vel);
1251 }
1252 IMETHOD void posrandom(Wrench& a) {
1253  posrandom(a.torque);
1254  posrandom(a.force);
1255 }
1256 
1257 IMETHOD void posrandom(Rotation& R) {
1258  double alfa;
1259  double beta;
1260  double gamma;
1261  posrandom(alfa);
1262  posrandom(beta);
1263  posrandom(gamma);
1264  R = Rotation::EulerZYX(alfa,beta,gamma);
1265 }
1266 
1267 IMETHOD void posrandom(Frame& F) {
1268  random(F.M);
1269  random(F.p);
1270 }
1271 
1272 
1273 
1274 
1275 IMETHOD bool operator==(const Frame& a,const Frame& b ) {
1276 #ifdef KDL_USE_EQUAL
1277  return Equal(a,b);
1278 #else
1279  return (a.p == b.p &&
1280  a.M == b.M );
1281 #endif
1282 }
1283 
1284 IMETHOD bool operator!=(const Frame& a,const Frame& b) {
1285  return !operator==(a,b);
1286 }
1287 
1288 IMETHOD bool operator==(const Vector& a,const Vector& b) {
1289 #ifdef KDL_USE_EQUAL
1290  return Equal(a,b);
1291 #else
1292  return (a.data[0]==b.data[0]&&
1293  a.data[1]==b.data[1]&&
1294  a.data[2]==b.data[2] );
1295 #endif
1296  }
1297 
1298 IMETHOD bool operator!=(const Vector& a,const Vector& b) {
1299  return !operator==(a,b);
1300 }
1301 
1302 IMETHOD bool operator==(const Twist& a,const Twist& b) {
1303 #ifdef KDL_USE_EQUAL
1304  return Equal(a,b);
1305 #else
1306  return (a.rot==b.rot &&
1307  a.vel==b.vel );
1308 #endif
1309 }
1310 
1311 IMETHOD bool operator!=(const Twist& a,const Twist& b) {
1312  return !operator==(a,b);
1313 }
1314 
1315 IMETHOD bool operator==(const Wrench& a,const Wrench& b ) {
1316 #ifdef KDL_USE_EQUAL
1317  return Equal(a,b);
1318 #else
1319  return (a.force==b.force &&
1320  a.torque==b.torque );
1321 #endif
1322 }
1323 
1324 IMETHOD bool operator!=(const Wrench& a,const Wrench& b) {
1325  return !operator==(a,b);
1326 }
1327 IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
1328  return !operator==(a,b);
1329 }
1330 
1331 IMETHOD bool operator==(const Vector2& a,const Vector2& b) {
1332 #ifdef KDL_USE_EQUAL
1333  return Equal(a,b);
1334 #else
1335  return (a.data[0]==b.data[0]&&
1336  a.data[1]==b.data[1] );
1337 #endif
1338  }
1339 
1340 IMETHOD bool operator!=(const Vector2& a,const Vector2& b) {
1341  return !operator==(a,b);
1342 }
1343 
KDL::Rotation::SetInverse
void SetInverse()
Sets the value of *this to its inverse.
Definition: frames.inl:653
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
KDL::Rotation2::operator()
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition: frames.inl:860
KDL::Frame::Identity
static Frame Identity()
Definition: frames.inl:700
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Frame2
Definition: frames.hpp:1098
KDL::Frame::Inverse
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition: frames.inl:422
KDL::Vector::Set2DZX
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition: frames.inl:475
KDL::Vector2::Set3DYZ
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition: frames.inl:825
KDL::operator*
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Definition: articulatedbodyinertia.cpp:49
KDL::Frame2::Frame2
Frame2(void)
KDL::posrandom
IMETHOD void posrandom(doubleVel &F)
Definition: framevel.hpp:69
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:637
KDL::Vector::Zero
static Vector Zero()
Definition: frames.inl:138
KDL::Vector2::operator-=
Vector2 & operator-=(const Vector2 &arg)
Definition: frames.inl:777
KDL::Frame::Frame
Frame()
Definition: frames.hpp:586
KDL::Frame::Integrate
void Integrate(const Twist &t_this, double frequency)
Definition: frames.inl:624
KDL::Rotation::DoRotZ
void DoRotZ(double angle)
Definition: frames.inl:588
KDL::Rotation::operator()
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition: frames.inl:488
FRAMES_CHECKI
#define FRAMES_CHECKI(a)
Definition: utility.h:52
KDL::random
IMETHOD void random(doubleVel &F)
Definition: framevel.hpp:65
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::Rotation::Rotation
Rotation()
Definition: frames.hpp:308
KDL::Vector2::operator=
Vector2 & operator=(const Vector2 &arg)
Definition: frames.inl:737
KDL::Rotation2::c
double c
Definition: frames.hpp:1053
KDL::Vector::Norm
double Norm(double eps=epsilon) const
Definition: frames.cpp:142
KDL::Rotation::RotY
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:610
KDL::Rotation::DoRotX
void DoRotX(double angle)
Definition: frames.inl:556
KDL::sin
INLINE Rall1d< T, V, S > sin(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:335
KDL::Vector::Set2DYZ
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition: frames.inl:466
KDL::operator/
INLINE Rall1d< T, V, S > operator/(const Rall1d< T, V, S > &lhs, const Rall1d< T, V, S > &rhs)
Definition: rall1d.h:239
KDL::Frame2::SetInverse
void SetInverse()
Definition: frames.inl:951
KDL::Rotation2::Rotation2
Rotation2()
Default constructor does NOT initialise to Zero().
Definition: frames.hpp:1058
KDL::addDelta
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
KDL::Rotation2::s
double s
Definition: frames.hpp:1053
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::operator==
bool operator==(const Rotation &a, const Rotation &b)
Definition: frames.cpp:455
KDL::Frame2::operator*
Vector2 operator*(const Vector2 &arg) const
Definition: frames.inl:935
KDL::cos
INLINE Rall1d< T, V, S > cos(const Rall1d< T, V, S > &arg)
Definition: rall1d.h:343
KDL::Vector::Set2DPlane
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
Definition: frames.inl:707
operator-
IMETHOD Vector operator-(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:66
KDL::epsilon
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cxx:21
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::Rotation::DoRotY
void DoRotY(double angle)
Definition: frames.inl:572
KDL::atan2
INLINE Rall1d< T, V, S > atan2(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:453
KDL::Vector2::y
double y() const
Definition: frames.inl:800
KDL::Rotation2::operator*
Vector2 operator*(const Vector2 &v) const
Definition: frames.inl:856
KDL::Wrench::torque
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition: frames.hpp:885
dot
IMETHOD double dot(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:1012
KDL::Frame2::SetIdentity
void SetIdentity()
Definition: frames.inl:945
KDL::Frame2::operator=
Frame2 & operator=(const Frame2 &arg)
Definition: frames.inl:966
KDL::Vector2::ReverseSign
void ReverseSign()
Definition: frames.inl:806
KDL::Rotation::RotZ
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:615
KDL::Rotation::Identity
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition: frames.inl:552
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::Rotation2::Inverse
Rotation2 Inverse() const
Definition: frames.inl:878
KDL::operator-
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:60
KDL::Rotation2::SetIdentity
void SetIdentity()
Definition: frames.inl:890
KDL::Vector2::Set3DZX
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition: frames.inl:831
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::Rotation::operator*
Vector operator*(const Vector &v) const
Definition: frames.inl:526
KDL::Vector2::x
double x() const
Definition: frames.inl:799
KDL::Rotation2
Definition: frames.hpp:1051
KDL::operator!=
INLINE bool operator!=(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
Definition: rall1d.h:507
operator*
Vector operator*(const Vector &lhs, double rhs)
Definition: frames.inl:83
KDL::Vector::Set2DXY
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition: frames.inl:458
KDL::Vector2::Set3DXY
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition: frames.inl:819
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::Frame2::operator()
double operator()(int i, int j)
Definition: frames.inl:978
KDL::Frame2::p
Vector2 p
origine of the Frame
Definition: frames.hpp:1101
KDL::Rotation::data
double data[9]
Definition: frames.hpp:306
KDL::Vector2::Set3DPlane
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
Definition: frames.inl:838
Rot
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.inl:1107
KDL::Rotation::EulerZYX
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
Definition: frames.hpp:471
KDL::Rotation::Rot
static Rotation Rot(const Vector &rotvec, double angle)
Definition: frames.cpp:318
KDL::Equal
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
KDL::Vector2::Zero
static Vector2 Zero()
Definition: frames.inl:784
KDL::Rotation2::GetRot
double GetRot() const
Gets the angle (in radians)
Definition: frames.inl:904
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
operator+
IMETHOD Vector operator+(const Vector &lhs, const Vector &rhs)
Definition: frames.inl:57
KDL::Vector::ReverseSign
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition: frames.inl:440
KDL::Frame2::Inverse
Frame2 Inverse() const
Definition: frames.inl:959
operator/
Vector operator/(const Vector &lhs, double rhs)
Definition: frames.inl:101
KDL::Rotation2::operator=
Rotation2 & operator=(const Rotation2 &arg)
Definition: frames.inl:851
KDL::Rotation::operator=
Rotation & operator=(const Rotation &arg)
Definition: frames.inl:520
KDL::Rotation::RotX
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition: frames.inl:605
KDL::Vector2
2D version of Vector
Definition: frames.hpp:960
KDL::Vector2::Vector2
Vector2()
Does not initialise to Zero().
Definition: frames.hpp:965
KDL::Frame::operator()
double operator()(int i, int j)
Definition: frames.inl:667
KDL::Rotation2::SetRot
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition: frames.inl:896
KDL::Vector2::operator+=
Vector2 & operator+=(const Vector2 &arg)
Definition: frames.inl:770
KDL::Rotation2::Identity
static Rotation2 Identity()
Definition: frames.inl:886
KDL::operator+
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition: articulatedbodyinertia.cpp:53
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:79
KDL::Vector2::data
double data[2]
Definition: frames.hpp:962
KDL::diff
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
KDL::Rotation2::SetInverse
void SetInverse()
Definition: frames.inl:874
KDL::Wrench::force
Vector force
Force that is applied at the origin of the current ref frame.
Definition: frames.hpp:884
KDL::Vector2::operator()
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition: frames.inl:788
IMETHOD
#define IMETHOD
Definition: utility.h:41
KDL::Rotation2::Rot
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition: frames.inl:900
std::data
T data(T... args)
KDL::Frame2::M
Rotation2 M
Orientation of the Frame.
Definition: frames.hpp:1102
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303