framevel.inl
Go to the documentation of this file.
1 /*****************************************************************************
2  * \file
3  * provides inline functions of rframes.h
4  *
5  * \author
6  * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
7  *
8  * \version
9  * ORO_Geometry V0.2
10  *
11  * \par History
12  * - $log$
13  *
14  * \par Release
15  * $Id: rframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
16  * $Name: $
17  ****************************************************************************/
18 
19 
20 // Methods and operators related to FrameVelVel
21 // They all delegate most of the work to RotationVelVel and VectorVelVel
22 FrameVel& FrameVel::operator = (const FrameVel& arg) {
23  M=arg.M;
24  p=arg.p;
25  return *this;
26 }
27 
28 FrameVel FrameVel::Identity() {
29  return FrameVel(RotationVel::Identity(),VectorVel::Zero());
30 }
31 
32 
33 FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
34 {
35  return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
36 }
37 FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
38 {
39  return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
40 }
41 FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
42 {
43  return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
44 }
45 
46 VectorVel FrameVel::operator *(const VectorVel & arg) const
47 {
48  return M*arg+p;
49 }
50 VectorVel FrameVel::operator *(const Vector & arg) const
51 {
52  return M*arg+p;
53 }
54 
55 VectorVel FrameVel::Inverse(const VectorVel& arg) const
56 {
57  return M.Inverse(arg-p);
58 }
59 
60 VectorVel FrameVel::Inverse(const Vector& arg) const
61 {
62  return M.Inverse(arg-p);
63 }
64 
65 FrameVel FrameVel::Inverse() const
66 {
67  return FrameVel(M.Inverse(),-M.Inverse(p));
68 }
69 
70 FrameVel& FrameVel::operator = (const Frame& arg) {
71  M = arg.M;
72  p = arg.p;
73  return *this;
74 }
75 bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
76  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
77 }
78 bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
79  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
80 }
81 bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
82  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
83 }
84 bool operator==(const FrameVel& r1,const FrameVel& r2) {
85 #ifdef KDL_USE_EQUAL
86  return Equal(r1, r2);
87 #else
88  return (r1.p == r2.p &&
89  r1.M == r2.M );
90 #endif
91 }
92 bool operator!=(const FrameVel& r1,const FrameVel& r2) {
93  return !operator==(r1,r2);
94 }
95 bool operator==(const Frame& r1,const FrameVel& r2) {
96 #ifdef KDL_USE_EQUAL
97  return Equal(r1, r2);
98 #else
99  return (r1.p == r2.p &&
100  r1.M == r2.M );
101 #endif
102 }
103 bool operator!=(const Frame& r1,const FrameVel& r2) {
104  return !operator==(r1,r2);
105 }
106 bool operator==(const FrameVel& r1,const Frame& r2) {
107 #ifdef KDL_USE_EQUAL
108  return Equal(r1, r2);
109 #else
110  return (r1.p == r2.p &&
111  r1.M == r2.M );
112 #endif
113 }
114 bool operator!=(const FrameVel& r1,const Frame& r2) {
115  return !operator==(r1,r2);
116 }
117 
118 
119 
120 Frame FrameVel::GetFrame() const {
121  return Frame(M.R,p.p);
122 }
123 
124 Twist FrameVel::GetTwist() const {
125  return Twist(p.v,M.w);
126 }
127 
128 
129 RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
130  return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
131 }
132 
133 RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
134  return RotationVel( r1*r2.R, r1*r2.w );
135 }
136 
137 RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
138  return RotationVel( r1.R*r2, r1.w );
139 }
140 
141 RotationVel& RotationVel::operator = (const RotationVel& arg) {
142  R=arg.R;
143  w=arg.w;
144  return *this;
145  }
146 RotationVel& RotationVel::operator = (const Rotation& arg) {
147  R=arg;
148  w=Vector::Zero();
149  return *this;
150 }
151 
152 VectorVel RotationVel::UnitX() const {
153  return VectorVel(R.UnitX(),w*R.UnitX());
154 }
155 
156 VectorVel RotationVel::UnitY() const {
157  return VectorVel(R.UnitY(),w*R.UnitY());
158 }
159 
160 VectorVel RotationVel::UnitZ() const {
161  return VectorVel(R.UnitZ(),w*R.UnitZ());
162 }
163 
164 
165 
166 RotationVel RotationVel::Identity() {
167  return RotationVel(Rotation::Identity(),Vector::Zero());
168 }
169 
170 RotationVel RotationVel::Inverse() const {
171  return RotationVel(R.Inverse(),-R.Inverse(w));
172 }
173 
174 VectorVel RotationVel::Inverse(const VectorVel& arg) const {
175  Vector tmp=R.Inverse(arg.p);
176  return VectorVel(tmp,
177  R.Inverse(arg.v-w*arg.p)
178  );
179 }
180 
181 VectorVel RotationVel::Inverse(const Vector& arg) const {
182  Vector tmp=R.Inverse(arg);
183  return VectorVel(tmp,
184  R.Inverse(-w*arg)
185  );
186 }
187 
188 
189 VectorVel RotationVel::operator*(const VectorVel& arg) const {
190  Vector tmp=R*arg.p;
191  return VectorVel(tmp,w*tmp+R*arg.v);
192 }
193 
194 VectorVel RotationVel::operator*(const Vector& arg) const {
195  Vector tmp=R*arg;
196  return VectorVel(tmp,w*tmp);
197 }
198 
199 
200 // = Rotations
201 // The Rot... static functions give the value of the appropriate rotation matrix back.
202 // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
203 
204 void RotationVel::DoRotX(const doubleVel& angle) {
205  w+=R*Vector(angle.grad,0,0);
206  R.DoRotX(angle.t);
207 }
208 RotationVel RotationVel::RotX(const doubleVel& angle) {
209  return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
210 }
211 
212 void RotationVel::DoRotY(const doubleVel& angle) {
213  w+=R*Vector(0,angle.grad,0);
214  R.DoRotY(angle.t);
215 }
216 RotationVel RotationVel::RotY(const doubleVel& angle) {
217  return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
218 }
219 
220 void RotationVel::DoRotZ(const doubleVel& angle) {
221  w+=R*Vector(0,0,angle.grad);
222  R.DoRotZ(angle.t);
223 }
224 RotationVel RotationVel::RotZ(const doubleVel& angle) {
225  return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
226 }
227 
228 
229 RotationVel RotationVel::Rot(const Vector& rotvec,const doubleVel& angle)
230 // rotvec has arbitrary norm
231 // rotation around a constant vector !
232 {
233  Vector v(rotvec);
234  v.Normalize();
235  return RotationVel(Rotation::Rot2(v,angle.t),v*angle.grad);
236 }
237 
238 RotationVel RotationVel::Rot2(const Vector& rotvec,const doubleVel& angle)
239  // rotvec is normalized.
240 {
241  return RotationVel(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
242 }
243 
244 
245 VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
246  return VectorVel(r1.p+r2.p,r1.v+r2.v);
247 }
248 
249 VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
250  return VectorVel(r1.p-r2.p,r1.v-r2.v);
251 }
252 
253 VectorVel operator + (const VectorVel& r1,const Vector& r2) {
254  return VectorVel(r1.p+r2,r1.v);
255 }
256 
257 VectorVel operator - (const VectorVel& r1,const Vector& r2) {
258  return VectorVel(r1.p-r2,r1.v);
259 }
260 
261 VectorVel operator + (const Vector& r1,const VectorVel& r2) {
262  return VectorVel(r1+r2.p,r2.v);
263 }
264 
265 VectorVel operator - (const Vector& r1,const VectorVel& r2) {
266  return VectorVel(r1-r2.p,-r2.v);
267 }
268 
269 // unary -
270 VectorVel operator - (const VectorVel& r) {
271  return VectorVel(-r.p,-r.v);
272 }
273 
274 void SetToZero(VectorVel& v){
275  SetToZero(v.p);
276  SetToZero(v.v);
277 }
278 
279 // cross prod.
280 VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
281  return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
282 }
283 
284 VectorVel operator * (const VectorVel& r1,const Vector& r2) {
285  return VectorVel(r1.p*r2, r1.v*r2);
286 }
287 
288 VectorVel operator * (const Vector& r1,const VectorVel& r2) {
289  return VectorVel(r1*r2.p, r1*r2.v);
290 }
291 
292 
293 
294 // scalar mult.
295 VectorVel operator * (double r1,const VectorVel& r2) {
296  return VectorVel(r1*r2.p, r1*r2.v);
297 }
298 
299 VectorVel operator * (const VectorVel& r1,double r2) {
300  return VectorVel(r1.p*r2, r1.v*r2);
301 }
302 
303 
304 
305 VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
306  return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
307 }
308 
309 VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
310  return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
311 }
312 
313 VectorVel operator / (const VectorVel& r1,double r2) {
314  return VectorVel(r1.p/r2, r1.v/r2);
315 }
316 
317 VectorVel operator / (const VectorVel& r2,const doubleVel& r1) {
318  return VectorVel(r2.p/r1.t, r2.v/r1.t - r2.p*r1.grad/r1.t/r1.t);
319 }
320 
321 VectorVel operator*(const Rotation& R,const VectorVel& x) {
322  return VectorVel(R*x.p,R*x.v);
323 }
324 
325 VectorVel& VectorVel::operator = (const VectorVel& arg) {
326  p=arg.p;
327  v=arg.v;
328  return *this;
329 }
330 VectorVel& VectorVel::operator = (const Vector& arg) {
331  p=arg;
332  v=Vector::Zero();
333  return *this;
334 }
335 VectorVel& VectorVel::operator += (const VectorVel& arg) {
336  p+=arg.p;
337  v+=arg.v;
338  return *this;
339 }
340 VectorVel& VectorVel::operator -= (const VectorVel& arg) {
341  p-=arg.p;
342  v-=arg.v;
343  return *this;
344 }
345 
346 VectorVel VectorVel::Zero() {
347  return VectorVel(Vector::Zero(),Vector::Zero());
348 }
349 void VectorVel::ReverseSign() {
350  p.ReverseSign();
351  v.ReverseSign();
352 }
353 doubleVel VectorVel::Norm(double eps) const {
354  double n = p.Norm(eps);
355  if (n < eps) // Setting norm of p and v to 0 in case norm of p is smaller than eps
356  return doubleVel(0, 0);
357  return doubleVel(n, dot(p,v)/n);
358 }
359 
360 bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
361  return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
362 }
363 bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
364  return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
365 }
366 bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
367  return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
368 }
369 bool operator==(const VectorVel& r1,const VectorVel& r2) {
370 #ifdef KDL_USE_EQUAL
371  return Equal(r1, r2);
372 #else
373  return (r1.p == r2.p &&
374  r1.v == r2.v );
375 #endif
376 }
377 bool operator!=(const VectorVel& r1,const VectorVel& r2) {
378  return !operator==(r1,r2);
379 }
380 bool operator==(const Vector& r1,const VectorVel& r2) {
381 #ifdef KDL_USE_EQUAL
382  return Equal(r1, r2);
383 #else
384  return (r1 == r2.p &&
385  Vector::Zero() == r2.v);
386 #endif
387 }
388 bool operator!=(const Vector& r1,const VectorVel& r2) {
389  return !operator==(r1,r2);
390 }
391 bool operator==(const VectorVel& r1,const Vector& r2) {
392 #ifdef KDL_USE_EQUAL
393  return Equal(r1, r2);
394 #else
395  return (r1.p == r2 &&
396  r1.v == Vector::Zero() );
397 #endif
398 }
399 bool operator!=(const VectorVel& r1,const Vector& r2) {
400  return !operator==(r1,r2);
401 }
402 
403 
404 bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
405  return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
406 }
407 bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
408  return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
409 }
410 bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
411  return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
412 }
413 bool operator==(const RotationVel& r1,const RotationVel& r2) {
414 #ifdef KDL_USE_EQUAL
415  return Equal(r1, r2);
416 #else
417  return (r1.w == r2.w &&
418  r1.R == r2.R );
419 #endif
420 }
421 bool operator!=(const RotationVel& r1,const RotationVel& r2) {
422  return !operator==(r1,r2);
423 }
424 bool operator==(const Rotation& r1,const RotationVel& r2) {
425 #ifdef KDL_USE_EQUAL
426  return Equal(r1, r2);
427 #else
428  return (Vector::Zero() == r2.w &&
429  r1 == r2.R);
430 #endif
431 }
432 bool operator!=(const Rotation& r1,const RotationVel& r2) {
433  return !operator==(r1,r2);
434 }
435 bool operator==(const RotationVel& r1,const Rotation& r2) {
436 #ifdef KDL_USE_EQUAL
437  return Equal(r1, r2);
438 #else
439  return (r1.w == Vector::Zero() &&
440  r1.R == r2);
441 #endif
442 }
443 bool operator!=(const RotationVel& r1,const Rotation& r2) {
444  return !operator==(r1,r2);
445 }
446 
447 
448 bool Equal(const TwistVel& a,const TwistVel& b,double eps) {
449  return (Equal(a.rot,b.rot,eps)&&
450  Equal(a.vel,b.vel,eps) );
451 }
452 bool Equal(const Twist& a,const TwistVel& b,double eps) {
453  return (Equal(a.rot,b.rot,eps)&&
454  Equal(a.vel,b.vel,eps) );
455 }
456 bool Equal(const TwistVel& a,const Twist& b,double eps) {
457  return (Equal(a.rot,b.rot,eps)&&
458  Equal(a.vel,b.vel,eps) );
459 }
460 bool operator==(const TwistVel& a,const TwistVel& b) {
461 #ifdef KDL_USE_EQUAL
462  return Equal(a, b);
463 #else
464  return (a.rot == b.rot &&
465  a.vel == b.vel );
466 #endif
467 }
468 bool operator!=(const TwistVel& a,const TwistVel& b) {
469  return !operator==(a,b);
470 }
471 bool operator==(const Twist& a,const TwistVel& b) {
472 #ifdef KDL_USE_EQUAL
473  return Equal(a, b);
474 #else
475  return (a.rot == b.rot &&
476  a.vel == b.vel );
477 #endif
478 }
479 bool operator!=(const Twist& r1,const TwistVel& r2) {
480  return !operator==(r1,r2);
481 }
482 bool operator==(const TwistVel& r1,const Twist& r2) {
483 #ifdef KDL_USE_EQUAL
484  return Equal(r1, r2);
485 #else
486  return (a.rot == b.rot &&
487  a.vel == b.vel );
488 #endif
489 }
490 bool operator!=(const TwistVel& r1,const Twist& r2) {
491  return !operator==(r1,r2);
492 }
493 
494 
495 IMETHOD doubleVel dot(const VectorVel& lhs,const VectorVel& rhs) {
496  return doubleVel(dot(lhs.p,rhs.p),dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p));
497 }
498 IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
499  return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
500 }
501 IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
502  return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
503 }
504 
505 TwistVel TwistVel::Zero()
506 {
507  return TwistVel(VectorVel::Zero(),VectorVel::Zero());
508 }
509 
510 
511 void TwistVel::ReverseSign()
512 {
513  vel.ReverseSign();
514  rot.ReverseSign();
515 }
516 
517 TwistVel TwistVel::RefPoint(const VectorVel& v_base_AB)
518  // Changes the reference point of the TwistVel.
519  // The VectorVel v_base_AB is expressed in the same base as the TwistVel
520  // The VectorVel v_base_AB is a VectorVel from the old point to
521  // the new point.
522  // Complexity : 6M+6A
523 {
524  return TwistVel(this->vel+this->rot*v_base_AB,this->rot);
525 }
526 
527 TwistVel& TwistVel::operator-=(const TwistVel& arg)
528 {
529  vel-=arg.vel;
530  rot -=arg.rot;
531  return *this;
532 }
533 
534 TwistVel& TwistVel::operator+=(const TwistVel& arg)
535 {
536  vel+=arg.vel;
537  rot +=arg.rot;
538  return *this;
539 }
540 
541 
542 TwistVel operator*(const TwistVel& lhs,double rhs)
543 {
544  return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
545 }
546 
547 TwistVel operator*(double lhs,const TwistVel& rhs)
548 {
549  return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
550 }
551 
552 TwistVel operator/(const TwistVel& lhs,double rhs)
553 {
554  return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
555 }
556 
557 
558 TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
559 {
560  return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
561 }
562 
563 TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
564 {
565  return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
566 }
567 
568 TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs)
569 {
570  return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
571 }
572 
573 
574 
575 // addition of TwistVel's
576 TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
577 {
578  return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
579 }
580 
581 TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
582 {
583  return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
584 }
585 
586 // unary -
587 TwistVel operator-(const TwistVel& arg)
588 {
589  return TwistVel(-arg.vel,-arg.rot);
590 }
591 
592 void SetToZero(TwistVel& v)
593 {
594  SetToZero(v.vel);
595  SetToZero(v.rot);
596 }
597 
598 
599 
600 
601 
602 TwistVel RotationVel::Inverse(const TwistVel& arg) const
603 {
604  return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
605 }
606 
607 TwistVel RotationVel::operator * (const TwistVel& arg) const
608 {
609  return TwistVel((*this)*arg.vel,(*this)*arg.rot);
610 }
611 
612 TwistVel RotationVel::Inverse(const Twist& arg) const
613 {
614  return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
615 }
616 
617 TwistVel RotationVel::operator * (const Twist& arg) const
618 {
619  return TwistVel((*this)*arg.vel,(*this)*arg.rot);
620 }
621 
622 
623 TwistVel FrameVel::operator * (const TwistVel& arg) const
624 {
625  TwistVel tmp;
626  tmp.rot = M*arg.rot;
627  tmp.vel = M*arg.vel+p*tmp.rot;
628  return tmp;
629 }
630 
631 TwistVel FrameVel::operator * (const Twist& arg) const
632 {
633  TwistVel tmp;
634  tmp.rot = M*arg.rot;
635  tmp.vel = M*arg.vel+p*tmp.rot;
636  return tmp;
637 }
638 
639 TwistVel FrameVel::Inverse(const TwistVel& arg) const
640 {
641  TwistVel tmp;
642  tmp.rot = M.Inverse(arg.rot);
643  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
644  return tmp;
645 }
646 
647 TwistVel FrameVel::Inverse(const Twist& arg) const
648 {
649  TwistVel tmp;
650  tmp.rot = M.Inverse(arg.rot);
651  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
652  return tmp;
653 }
654 
655 Twist TwistVel::GetTwist() const {
656  return Twist(vel.p,rot.p);
657 }
658 
659 Twist TwistVel::GetTwistDot() const {
660  return Twist(vel.v,rot.v);
661 }
FrameVel operator*(const FrameVel &lhs, const FrameVel &rhs)
Definition: framevel.inl:33
void SetToZero(VectorVel &v)
Definition: framevel.inl:274
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:418
bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
Definition: framevel.inl:75
IMETHOD doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
Definition: framevel.inl:495
VectorVel operator/(const VectorVel &r1, double r2)
Definition: framevel.inl:313
bool operator!=(const FrameVel &r1, const FrameVel &r2)
Definition: framevel.inl:92
VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
Definition: framevel.inl:245
bool operator==(const FrameVel &r1, const FrameVel &r2)
Definition: framevel.inl:84
VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
Definition: framevel.inl:249
IMETHOD Rotation Rot(const Vector &axis_a_b)
Definition: frames.inl:1107
#define IMETHOD
Definition: utility.h:41
Rall1d< double > doubleVel
Definition: framevel.hpp:35


orocos_kdl
Author(s):
autogenerated on Thu Apr 13 2023 02:19:14