KDL 1.5.1
Loading...
Searching...
No Matches
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
36IMETHOD 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
43IMETHOD Vector::Vector(double x,double y, double z)
44{
45 data[0]=x;data[1]=y;data[2]=z;
46}
47
48
49IMETHOD 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
57IMETHOD 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
66IMETHOD 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
75IMETHOD double Vector::x() const { return data[0]; }
76IMETHOD double Vector::y() const { return data[1]; }
77IMETHOD double Vector::z() const { return data[2]; }
78
79IMETHOD void Vector::x( double _x ) { data[0] = _x; }
80IMETHOD void Vector::y( double _y ) { data[1] = _y; }
81IMETHOD void Vector::z( double _z ) { data[2] = _z; }
82
83Vector 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
92Vector 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
101Vector 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
110Vector 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
120Vector& 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
129Vector& 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
138Vector Vector::Zero()
139{
140 return Vector(0,0,0);
141}
142
143double Vector::operator()(int index) const {
144 FRAMES_CHECKI((0<=index)&&(index<=2));
145 return data[index];
146}
147
148double& Vector::operator () (int index)
149{
150 FRAMES_CHECKI((0<=index)&&(index<=2));
151 return data[index];
152}
153
154
155
156Wrench Frame::operator * (const Wrench& arg) const
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
165Wrench 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
175Wrench Rotation::Inverse(const Wrench& arg) const
176{
177 return Wrench(Inverse(arg.force),Inverse(arg.torque));
178}
179
180Twist Rotation::Inverse(const Twist& arg) const
181{
182 return Twist(Inverse(arg.vel),Inverse(arg.rot));
183}
184
185Wrench Wrench::Zero()
186{
187 return Wrench(Vector::Zero(),Vector::Zero());
188}
189
190
191void Wrench::ReverseSign()
192{
193 torque.ReverseSign();
194 force.ReverseSign();
195}
196
197Wrench 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
209Wrench& Wrench::operator-=(const Wrench& arg)
210{
211 torque-=arg.torque;
212 force -=arg.force;
213 return *this;
214}
215
216Wrench& Wrench::operator+=(const Wrench& arg)
217{
218 torque+=arg.torque;
219 force +=arg.force;
220 return *this;
221}
222
223double& 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
232double 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
242Wrench operator*(const Wrench& lhs,double rhs)
243{
244 return Wrench(lhs.force*rhs,lhs.torque*rhs);
245}
246
247Wrench operator*(double lhs,const Wrench& rhs)
248{
249 return Wrench(lhs*rhs.force,lhs*rhs.torque);
250}
251
252Wrench operator/(const Wrench& lhs,double rhs)
253{
254 return Wrench(lhs.force/rhs,lhs.torque/rhs);
255}
256
257// addition of Wrench's
258Wrench operator+(const Wrench& lhs,const Wrench& rhs)
259{
260 return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
261}
262
263Wrench operator-(const Wrench& lhs,const Wrench& rhs)
264{
265 return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
266}
267
268// unary -
269Wrench operator-(const Wrench& arg)
270{
271 return Wrench(-arg.force,-arg.torque);
272}
273
274Twist 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}
282Twist 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
290Twist Twist::Zero()
291{
292 return Twist(Vector::Zero(),Vector::Zero());
293}
294
295
296void Twist::ReverseSign()
297{
298 vel.ReverseSign();
299 rot.ReverseSign();
300}
301
302Twist 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
312Twist& Twist::operator-=(const Twist& arg)
313{
314 vel-=arg.vel;
315 rot -=arg.rot;
316 return *this;
317}
318
319Twist& Twist::operator+=(const Twist& arg)
320{
321 vel+=arg.vel;
322 rot +=arg.rot;
323 return *this;
324}
325
326double& 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
335double 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
345Twist operator*(const Twist& lhs,double rhs)
346{
347 return Twist(lhs.vel*rhs,lhs.rot*rhs);
348}
349
350Twist operator*(double lhs,const Twist& rhs)
351{
352 return Twist(lhs*rhs.vel,lhs*rhs.rot);
353}
354
355Twist operator/(const Twist& lhs,double rhs)
356{
357 return Twist(lhs.vel/rhs,lhs.rot/rhs);
358}
359
360// addition of Twist's
361Twist operator+(const Twist& lhs,const Twist& rhs)
362{
363 return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
364}
365
366Twist operator-(const Twist& lhs,const Twist& rhs)
367{
368 return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
369}
370
371// unary -
372Twist operator-(const Twist& arg)
373{
374 return Twist(-arg.vel,-arg.rot);
375}
376
377
378//Spatial products for twists
379Twist operator*(const Twist& lhs,const Twist& rhs)
380{
381 return Twist(lhs.rot*rhs.vel+lhs.vel*rhs.rot,lhs.rot*rhs.rot);
382}
383Wrench 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
388Frame::Frame(const Rotation & R)
389{
390 M=R;
391 p=Vector::Zero();
392}
393
394Frame::Frame(const Vector & V)
395{
396 M = Rotation::Identity();
397 p = V;
398}
399
400Frame::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
412Vector Frame::operator *(const Vector & arg) const
413{
414 return M*arg+p;
415}
416
417Vector Frame::Inverse(const Vector& arg) const
418{
419 return M.Inverse(arg-p);
420}
421
422Frame Frame::Inverse() const
423{
424 return Frame(M.Inverse(),-M.Inverse(p));
425}
426
427
428Frame& Frame::operator =(const Frame & arg)
429{
430 M = arg.M;
431 p = arg.p;
432 return *this;
433}
434
435Frame::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
449Vector 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
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}
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
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
488double& 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
493double 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
498Rotation::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
508Rotation::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)
556void 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
572void 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
588void 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
606 double cs=cos(angle);
607 double sn=sin(angle);
608 return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
609}
611 double cs=cos(angle);
612 double sn=sin(angle);
613 return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
614}
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
624void 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
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
667double 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
683double 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
707void 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{
711Vector tmp_XY;
712tmp_XY.Set2DXY(v_XY);
713tmp_XY = F_someframe_XY*(tmp_XY);
714}
715
716
717
718
719
720
721
722
723
724//============ 2 dimensional version of the frames objects =============
725IMETHOD Vector2::Vector2(const Vector2 & arg)
726{
727 data[0] = arg.data[0];
728 data[1] = arg.data[1];
729}
730
731IMETHOD 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
745IMETHOD 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
750IMETHOD 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
755IMETHOD Vector2 operator *(const Vector2& lhs,double rhs)
756{
757 return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
758}
759
760IMETHOD Vector2 operator *(double lhs,const Vector2& rhs)
761{
762 return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
763}
764
765IMETHOD 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
788IMETHOD double Vector2::operator()(int index) const {
789 FRAMES_CHECKI((0<=index)&&(index<=1));
790 return data[index];
791}
792
793IMETHOD double& Vector2::operator () (int index)
794{
795 FRAMES_CHECKI((0<=index)&&(index<=1));
796 return data[index];
797}
798
799IMETHOD double Vector2::x() const { return data[0]; }
800IMETHOD double Vector2::y() const { return data[1]; }
801
802IMETHOD void Vector2::x( double _x ) { data[0] = _x; }
803IMETHOD void Vector2::y( double _y ) { data[1] = _y; }
804
805
807{
808 data[0] = -data[0];
809 data[1] = -data[1];
810}
811
812
813IMETHOD Vector2 operator-(const Vector2 & arg)
814{
815 return Vector2(-arg.data[0],-arg.data[1]);
816}
817
818
819IMETHOD void Vector2::Set3DXY(const Vector& v)
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}
825IMETHOD void Vector2::Set3DYZ(const Vector& v)
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}
831IMETHOD void Vector2::Set3DZX(const Vector& v)
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
838IMETHOD 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
847IMETHOD Rotation2::Rotation2(const Rotation2& arg) {
848 c=arg.c;s=arg.s;
849}
850
852 c=arg.c;s=arg.s;
853 return *this;
854}
855
856IMETHOD Vector2 Rotation2::operator*(const Vector2& v) const {
857 return Vector2(v.data[0]*c-v.data[1]*s,v.data[0]*s+v.data[1]*c);
858}
859
860IMETHOD 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
870IMETHOD 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
874IMETHOD void Rotation2::SetInverse() {
875 s=-s;
876}
877
879 return Rotation2(c,-s);
880}
881
882IMETHOD Vector2 Rotation2::Inverse(const Vector2& v) const {
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
896IMETHOD void Rotation2::SetRot(double angle) {
897 c=cos(angle);s=sin(angle);
898}
899
900IMETHOD Rotation2 Rotation2::Rot(double angle) {
901 return Rotation2(cos(angle),sin(angle));
902}
903
904IMETHOD double Rotation2::GetRot() const {
905 return atan2(s,c);
906}
907
908
909IMETHOD Frame2::Frame2() {
910}
911
912IMETHOD Frame2::Frame2(const Rotation2 & R)
913{
914 M=R;
916}
917
918IMETHOD Frame2::Frame2(const Vector2 & V)
919{
921 p = V;
922}
923
924IMETHOD Frame2::Frame2(const Rotation2 & R, const Vector2 & V)
925{
926 M = R;
927 p = V;
928}
929
930IMETHOD Frame2 operator *(const Frame2& lhs,const Frame2& rhs)
931{
932 return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
933}
934
935IMETHOD Vector2 Frame2::operator *(const Vector2 & arg) const
936{
937 return M*arg+p;
938}
939
940IMETHOD Vector2 Frame2::Inverse(const Vector2& arg) const
941{
942 return M.Inverse(arg-p);
943}
944
946{
947 M.SetIdentity();
948 p = Vector2::Zero();
949}
950
951IMETHOD void Frame2::SetInverse()
952{
953 M.SetInverse();
954 p = M*p;
955 p.ReverseSign();
956}
957
958
959IMETHOD Frame2 Frame2::Inverse() const
960{
961 Frame2 tmp(*this);
962 tmp.SetInverse();
963 return tmp;
964}
965
966IMETHOD Frame2& Frame2::operator =(const Frame2 & arg)
967{
968 M = arg.M;
969 p = arg.p;
970 return *this;
971}
972
973IMETHOD Frame2::Frame2(const Frame2 & arg) :
974 p(arg.p), M(arg.M)
975{}
976
977
978IMETHOD 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
994IMETHOD 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
1012IMETHOD double dot(const Vector& lhs,const Vector& rhs) {
1013 return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1014}
1015
1016IMETHOD double dot(const Twist& lhs,const Wrench& rhs) {
1017 return dot(lhs.vel,rhs.force)+dot(lhs.rot,rhs.torque);
1018}
1019
1020IMETHOD 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
1032IMETHOD 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
1039IMETHOD 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
1044IMETHOD 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
1049IMETHOD 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
1054IMETHOD 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
1059IMETHOD 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
1063IMETHOD 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
1068IMETHOD void SetToZero(Vector& v) {
1069 v=Vector::Zero();
1070}
1071IMETHOD void SetToZero(Twist& v) {
1072 SetToZero(v.rot);
1073 SetToZero(v.vel);
1074}
1075IMETHOD void SetToZero(Wrench& v) {
1076 SetToZero(v.force);
1077 SetToZero(v.torque);
1078}
1079
1080IMETHOD 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
1107IMETHOD 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 }
1129IMETHOD Vector diff(const Vector& a,const Vector& b,double dt) {
1130 return (b-a)/dt;
1131}
1132
1133IMETHOD 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
1138IMETHOD 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}
1144IMETHOD 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
1148IMETHOD 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
1156IMETHOD Vector addDelta(const Vector& a,const Vector&da,double dt) {
1157 return a+da*dt;
1158}
1159
1160IMETHOD Rotation addDelta(const Rotation& a,const Vector&da,double dt) {
1161 return Rot(da*dt)*a;
1162}
1163IMETHOD 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}
1169IMETHOD 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}
1172IMETHOD 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
1214IMETHOD void random(Vector& a) {
1215 random(a[0]);
1216 random(a[1]);
1217 random(a[2]);
1218}
1219IMETHOD void random(Twist& a) {
1220 random(a.rot);
1221 random(a.vel);
1222}
1223IMETHOD void random(Wrench& a) {
1224 random(a.torque);
1225 random(a.force);
1226}
1227
1228IMETHOD 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
1238IMETHOD void random(Frame& F) {
1239 random(F.M);
1240 random(F.p);
1241}
1242
1243IMETHOD void posrandom(Vector& a) {
1244 posrandom(a[0]);
1245 posrandom(a[1]);
1246 posrandom(a[2]);
1247}
1248IMETHOD void posrandom(Twist& a) {
1249 posrandom(a.rot);
1250 posrandom(a.vel);
1251}
1252IMETHOD void posrandom(Wrench& a) {
1253 posrandom(a.torque);
1254 posrandom(a.force);
1255}
1256
1257IMETHOD 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
1267IMETHOD void posrandom(Frame& F) {
1268 random(F.M);
1269 random(F.p);
1270}
1271
1272
1273
1274
1275IMETHOD 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
1284IMETHOD bool operator!=(const Frame& a,const Frame& b) {
1285 return !operator==(a,b);
1286}
1287
1288IMETHOD 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
1298IMETHOD bool operator!=(const Vector& a,const Vector& b) {
1299 return !operator==(a,b);
1300}
1301
1302IMETHOD 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
1311IMETHOD bool operator!=(const Twist& a,const Twist& b) {
1312 return !operator==(a,b);
1313}
1314
1315IMETHOD 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
1324IMETHOD bool operator!=(const Wrench& a,const Wrench& b) {
1325 return !operator==(a,b);
1326}
1327IMETHOD bool operator!=(const Rotation& a,const Rotation& b) {
1328 return !operator==(a,b);
1329}
1330
1331IMETHOD 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
1340IMETHOD bool operator!=(const Vector2& a,const Vector2& b) {
1341 return !operator==(a,b);
1342}
1343
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition frames.hpp:1097
void SetIdentity()
Definition frames.inl:945
double operator()(int i, int j)
Treats a frame as a 3x3 matrix and returns element i,j Access to elements 0..2,0.....
Definition frames.inl:978
friend Frame2 operator*(const Frame2 &lhs, const Frame2 &rhs)
Frame2 & operator=(const Frame2 &arg)
Definition frames.inl:966
Rotation2 M
Orientation of the Frame.
Definition frames.hpp:1100
Vector2 p
origine of the Frame
Definition frames.hpp:1099
void SetInverse()
Definition frames.inl:951
Frame2 Inverse() const
Definition frames.inl:959
Definition frames.hpp:570
static Frame Identity()
Definition frames.inl:700
Rotation M
Orientation of the Frame.
Definition frames.hpp:573
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition frames.inl:624
Vector p
origine of the Frame
Definition frames.hpp:572
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition frames.inl:422
double operator()(int i, int j)
Treats a frame as a 4x4 matrix and returns element i,j Access to elements 0..3,0.....
Definition frames.inl:667
Frame()
Definition frames.hpp:584
A 2D Rotation class, for conventions see Rotation.
Definition frames.hpp:1051
double GetRot() const
Gets the angle (in radians)
Definition frames.inl:904
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
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition frames.inl:900
void SetInverse()
Definition frames.inl:874
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition frames.hpp:1057
static Rotation2 Identity()
Definition frames.inl:886
friend Rotation2 operator*(const Rotation2 &lhs, const Rotation2 &rhs)
double s
Definition frames.hpp:1052
double c
Definition frames.hpp:1052
void SetIdentity()
Definition frames.inl:890
Rotation2 Inverse() const
Definition frames.inl:878
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition frames.inl:896
Rotation2 & operator=(const Rotation2 &arg)
Definition frames.inl:851
represents rotations in 3 dimensional space.
Definition frames.hpp:302
void DoRotZ(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:588
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition frames.inl:637
Rotation & operator=(const Rotation &arg)
Definition frames.inl:520
Rotation()
Definition frames.hpp:306
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition frames.hpp:469
friend Rotation operator*(const Rotation &lhs, const Rotation &rhs)
Definition frames.cpp:173
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition frames.inl:552
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:605
void DoRotX(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:556
double data[9]
Definition frames.hpp:304
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:610
void SetInverse()
Sets the value of *this to its inverse.
Definition frames.inl:653
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition frames.cpp:293
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:615
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
void DoRotY(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:572
represents both translational and rotational velocities.
Definition frames.hpp:720
Vector rot
The rotational velocity of that point.
Definition frames.hpp:723
Vector vel
The velocity of that point.
Definition frames.hpp:722
2D version of Vector
Definition frames.hpp:960
static Vector2 Zero()
Definition frames.inl:784
void ReverseSign()
Definition frames.inl:806
double x() const
Definition frames.inl:799
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition frames.inl:831
Vector2 & operator=(const Vector2 &arg)
Definition frames.inl:737
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition frames.inl:819
Vector2()
Does not initialise to Zero().
Definition frames.hpp:964
Vector2 & operator+=(const Vector2 &arg)
Definition frames.inl:770
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition frames.inl:825
Vector2 & operator-=(const Vector2 &arg)
Definition frames.inl:777
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition frames.inl:788
double y() const
Definition frames.inl:800
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
projects v_someframe in the XY plane of F_someframe_XY, and sets *this to these values expressed wrt ...
Definition frames.inl:838
double data[2]
Definition frames.hpp:961
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
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
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition frames.inl:440
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition frames.inl:475
double data[3]
Definition frames.hpp:163
static Vector Zero()
Definition frames.inl:138
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition frames.inl:458
double Norm(double eps=epsilon) const
Definition frames.cpp:117
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition frames.inl:466
represents both translational and rotational acceleration.
Definition frames.hpp:879
Vector force
Force that is applied at the origin of the current ref frame.
Definition frames.hpp:881
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition frames.hpp:882
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition frameacc.inl:180
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition frameacc.inl:137
Vector operator/(const Vector &lhs, double rhs)
Definition frames.inl:101
IMETHOD Vector operator-(const Vector &lhs, const Vector &rhs)
Definition frames.inl:66
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition frames.inl:1284
IMETHOD Vector operator+(const Vector &lhs, const Vector &rhs)
Definition frames.inl:57
Vector operator*(const Vector &lhs, double rhs)
Definition frames.inl:83
IMETHOD Rotation Rot(const Vector &axis_a_b)
axis_a_b is a rotation vector, its norm is a rotation angle axis_a_b rotates the a frame towards the ...
Definition frames.inl:1107
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition articulatedbodyinertia.cpp:60
bool operator==(const Rotation &a, const Rotation &b)
Definition frames.cpp:430
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
addition I: I_new = I_old1 + I_old2, make sure that I_old1 and I_old2 are expressed in the same refer...
Definition articulatedbodyinertia.cpp:53
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
IMETHOD void random(doubleVel &F)
Definition framevel.hpp:46
void SetToZero(Jacobian &jac)
Definition jacobian.cpp:79
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Scalar product: I_new = double * I_old.
Definition articulatedbodyinertia.cpp:49
IMETHOD void posrandom(doubleVel &F)
Definition framevel.hpp:50
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)