orocos_kdl
frameacc.inl
Go to the documentation of this file.
1 /*****************************************************************************
2  * \file
3  * provides inline functions of rrframes.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: rrframes.inl,v 1.1.1.1 2002/08/26 14:14:21 rmoreas Exp $
16  * $Name: $
17  ****************************************************************************/
18 
19 
20 
21 
23 
24 VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
25  return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
26 }
27 
28 VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) {
29  return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
30 }
31 VectorAcc operator + (const Vector& r1,const VectorAcc& r2) {
32  return VectorAcc(r1+r2.p,r2.v,r2.dv);
33 }
34 
35 VectorAcc operator - (const Vector& r1,const VectorAcc& r2) {
36  return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
37 }
38 VectorAcc operator + (const VectorAcc& r1,const Vector& r2) {
39  return VectorAcc(r1.p+r2,r1.v,r1.dv);
40 }
41 
42 VectorAcc operator - (const VectorAcc& r1,const Vector& r2) {
43  return VectorAcc(r1.p-r2, r1.v, r1.dv);
44 }
45 
46 // unary -
47 VectorAcc operator - (const VectorAcc& r) {
48  return VectorAcc(-r.p,-r.v,-r.dv);
49 }
50 
51 // cross prod.
52 VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) {
53  return VectorAcc(r1.p*r2.p,
54  r1.p*r2.v+r1.v*r2.p,
55  r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv
56  );
57 }
58 
59 VectorAcc operator * (const VectorAcc& r1,const Vector& r2) {
60  return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
61 }
62 
63 VectorAcc operator * (const Vector& r1,const VectorAcc& r2) {
64  return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
65 }
66 
67 
68 
69 // scalar mult.
70 VectorAcc operator * (double r1,const VectorAcc& r2) {
71  return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
72 }
73 
74 VectorAcc operator * (const VectorAcc& r1,double r2) {
75  return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
76 }
77 
78 VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) {
79  return VectorAcc(r1.t*r2.p,
80  r1.t*r2.v + r1.d*r2.p,
81  r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
82  );
83 }
84 
85 VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) {
86  return VectorAcc(r1.t*r2.p,
87  r1.t*r2.v + r1.d*r2.p,
88  r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p
89  );
90 }
91 
92 VectorAcc& VectorAcc::operator = (const VectorAcc& arg) {
93  p=arg.p;
94  v=arg.v;
95  dv=arg.dv;
96  return *this;
97 }
98 
99 VectorAcc& VectorAcc::operator = (const Vector& arg) {
100  p=arg;
101  v=Vector::Zero();
102  dv=Vector::Zero();
103  return *this;
104 }
105 
106 VectorAcc& VectorAcc::operator += (const VectorAcc& arg) {
107  p+=arg.p;
108  v+=arg.v;
109  dv+= arg.dv;
110  return *this;
111 }
112 VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) {
113  p-=arg.p;
114  v-=arg.v;
115  dv-=arg.dv;
116  return *this;
117 }
118 
119 VectorAcc VectorAcc::Zero() {
120  return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
121 }
122 
123 void VectorAcc::ReverseSign() {
124  p.ReverseSign();
125  v.ReverseSign();
126  dv.ReverseSign();
127 }
128 
130  doubleAcc res;
131  res.t = p.Norm(eps);
132  res.d = dot(p,v)/res.t;
133  res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t;
134  return res;
135 }
136 
137 doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) {
138  return doubleAcc( dot(lhs.p,rhs.p),
139  dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p),
140  dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p)
141  );
142 }
143 
144 doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) {
145  return doubleAcc( dot(lhs.p,rhs),
146  dot(lhs.v,rhs),
147  dot(lhs.dv,rhs)
148  );
149 }
150 
151 doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) {
152  return doubleAcc( dot(lhs,rhs.p),
153  dot(lhs,rhs.v),
154  dot(lhs,rhs.dv)
155  );
156 }
157 
158 
159 bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) {
160  return (Equal(r1.p,r2.p,eps)
161  && Equal(r1.v,r2.v,eps)
162  && Equal(r1.dv,r2.dv,eps)
163  );
164 }
165 
166 bool Equal(const Vector& r1,const VectorAcc& r2,double eps) {
167  return (Equal(r1,r2.p,eps)
168  && Equal(Vector::Zero(),r2.v,eps)
169  && Equal(Vector::Zero(),r2.dv,eps)
170  );
171 }
172 
173 bool Equal(const VectorAcc& r1,const Vector& r2,double eps) {
174  return (Equal(r1.p,r2,eps)
175  && Equal(r1.v,Vector::Zero(),eps)
176  && Equal(r1.dv,Vector::Zero(),eps)
177  );
178 }
179 
180 VectorAcc operator / (const VectorAcc& r1,double r2) {
181  return r1*(1.0/r2);
182 }
183 
184 VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) {
185  return r2*(1.0/r1);
186 }
187 
188 
189 
191 
192 RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) {
193  return RotationAcc( r1.R * r2.R,
194  r1.w + r1.R*r2.w,
195  r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw
196  );
197 }
198 
199 RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) {
200  return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
201 }
202 
203 RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) {
204  return RotationAcc( r1.R*r2, r1.w, r1.dw );
205 }
206 
207 RotationAcc& RotationAcc::operator = (const RotationAcc& arg) {
208  R=arg.R;
209  w=arg.w;
210  dw=arg.dw;
211  return *this;
212 }
213 RotationAcc& RotationAcc::operator = (const Rotation& arg) {
214  R = arg;
215  w = Vector::Zero();
216  dw = Vector::Zero();
217  return *this;
218 }
219 
220 RotationAcc RotationAcc::Identity() {
221  return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
222 }
223 
224 RotationAcc RotationAcc::Inverse() const {
225  return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
226 }
227 
228 VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const {
229  VectorAcc tmp;
230  tmp.p = R.Inverse(arg.p);
231  tmp.v = R.Inverse(arg.v - w * arg.p);
232  tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v));
233  return tmp;
234 }
235 
236 VectorAcc RotationAcc::Inverse(const Vector& arg) const {
237  VectorAcc tmp;
238  tmp.p = R.Inverse(arg);
239  tmp.v = R.Inverse(-w*arg);
240  tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v));
241  return tmp;
242 }
243 
244 
246  VectorAcc tmp;
247  tmp.p = R*arg.p;
248  tmp.dv = R*arg.v;
249  tmp.v = w*tmp.p + tmp.dv;
250  tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv;
251  return tmp;
252 }
253 
254 VectorAcc operator*(const Rotation& R,const VectorAcc& x) {
255  return VectorAcc(R*x.p,R*x.v,R*x.dv);
256 }
257 
259  VectorAcc tmp;
260  tmp.p = R*arg;
261  tmp.v = w*tmp.p;
262  tmp.dv = dw*tmp.p + w*tmp.v;
263  return tmp;
264 }
265 
266 /*
267  // = Rotations
268  // The Rot... static functions give the value of the appropriate rotation matrix back.
269  // The DoRot... functions apply a rotation R to *this,such that *this = *this * R.
270 
271  void RRotation::DoRotX(const RDouble& angle) {
272  w+=R*Vector(angle.grad,0,0);
273  R.DoRotX(angle.t);
274  }
275 RotationAcc RotationAcc::RotX(const doubleAcc& angle) {
276  return RotationAcc(Rotation::RotX(angle.t),
277  Vector(angle.d,0,0),
278  Vector(angle.dd,0,0)
279  );
280 }
281 
282  void RRotation::DoRotY(const RDouble& angle) {
283  w+=R*Vector(0,angle.grad,0);
284  R.DoRotY(angle.t);
285  }
286 RotationAcc RotationAcc::RotY(const doubleAcc& angle) {
287  return RotationAcc(
288  Rotation::RotX(angle.t),
289  Vector(0,angle.d,0),
290  Vector(0,angle.dd,0)
291  );
292 }
293 
294  void RRotation::DoRotZ(const RDouble& angle) {
295  w+=R*Vector(0,0,angle.grad);
296  R.DoRotZ(angle.t);
297  }
298 RotationAcc RotationAcc::RotZ(const doubleAcc& angle) {
299  return RotationAcc(
300  Rotation::RotZ(angle.t),
301  Vector(0,0,angle.d),
302  Vector(0,0,angle.dd)
303  );
304 }
305 
306 
307  RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle)
308  // rotvec has arbitrary norm
309  // rotation around a constant vector !
310  {
311  Vector v = rotvec.Normalize();
312  return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad);
313  }
314 
315  RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle)
316  // rotvec is normalized.
317  {
318  return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad);
319  }
320 
321 */
322 
323 bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) {
324  return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) );
325 }
326 bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) {
327  return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) &&
328  Equal(Vector::Zero(),r2.dw,eps) );
329 }
330 bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) {
331  return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) &&
332  Equal(r1.dw,Vector::Zero(),eps) );
333 }
334 
335 
336 // Methods and operators related to FrameAcc
337 // They all delegate most of the work to RotationAcc and VectorAcc
338 FrameAcc& FrameAcc::operator = (const FrameAcc& arg) {
339  M=arg.M;
340  p=arg.p;
341  return *this;
342 }
343 
344 FrameAcc FrameAcc::Identity() {
345  return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
346 }
347 
348 
349 FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs)
350 {
351  return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
352 }
353 FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs)
354 {
355  return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
356 }
357 FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs)
358 {
359  return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
360 }
361 
363 {
364  return M*arg+p;
365 }
367 {
368  return M*arg+p;
369 }
370 
371 VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const
372 {
373  return M.Inverse(arg-p);
374 }
375 
376 VectorAcc FrameAcc::Inverse(const Vector& arg) const
377 {
378  return M.Inverse(arg-p);
379 }
380 
381 FrameAcc FrameAcc::Inverse() const
382 {
383  return FrameAcc(M.Inverse(),-M.Inverse(p));
384 }
385 
386 FrameAcc& FrameAcc::operator =(const Frame & arg)
387 {
388  M = arg.M;
389  p = arg.p;
390  return *this;
391 }
392 
393 bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) {
394  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
395 }
396 bool Equal(const Frame& r1,const FrameAcc& r2,double eps) {
397  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
398 }
399 bool Equal(const FrameAcc& r1,const Frame& r2,double eps) {
400  return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
401 }
402 
403 
404 Frame FrameAcc::GetFrame() const {
405  return Frame(M.R,p.p);
406 }
407 
408 
409 Twist FrameAcc::GetTwist() const {
410  return Twist(p.v,M.w);
411 }
412 
413 
414 Twist FrameAcc::GetAccTwist() const {
415  return Twist(p.dv,M.dw);
416 }
417 
418 
419 
420 
421 
422 
423 
424 
425 
426 
427 
428 
429 
430 
431 
432 
433 
434 TwistAcc TwistAcc::Zero()
435 {
436  return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
437 }
438 
439 
440 void TwistAcc::ReverseSign()
441 {
442  vel.ReverseSign();
443  rot.ReverseSign();
444 }
445 
446 TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB)
447  // Changes the reference point of the TwistAcc.
448  // The RVector v_base_AB is expressed in the same base as the TwistAcc
449  // The RVector v_base_AB is a RVector from the old point to
450  // the new point.
451  // Complexity : 6M+6A
452 {
453  return TwistAcc(this->vel+this->rot*v_base_AB,this->rot);
454 }
455 
456 TwistAcc& TwistAcc::operator-=(const TwistAcc& arg)
457 {
458  vel-=arg.vel;
459  rot -=arg.rot;
460  return *this;
461 }
462 
463 TwistAcc& TwistAcc::operator+=(const TwistAcc& arg)
464 {
465  vel+=arg.vel;
466  rot +=arg.rot;
467  return *this;
468 }
469 
470 
471 TwistAcc operator*(const TwistAcc& lhs,double rhs)
472 {
473  return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
474 }
475 
476 TwistAcc operator*(double lhs,const TwistAcc& rhs)
477 {
478  return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
479 }
480 
481 TwistAcc operator/(const TwistAcc& lhs,double rhs)
482 {
483  return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
484 }
485 
486 
487 TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs)
488 {
489  return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
490 }
491 
492 TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs)
493 {
494  return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
495 }
496 
497 TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs)
498 {
499  return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
500 }
501 
502 
503 
504 // addition of TwistAcc's
505 TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs)
506 {
507  return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
508 }
509 
510 TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs)
511 {
512  return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
513 }
514 
515 // unary -
516 TwistAcc operator-(const TwistAcc& arg)
517 {
518  return TwistAcc(-arg.vel,-arg.rot);
519 }
520 
521 
522 
523 
524 
525 TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const
526 {
527  return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
528 }
529 
531 {
532  return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
533 }
534 
535 TwistAcc RotationAcc::Inverse(const Twist& arg) const
536 {
537  return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
538 }
539 
541 {
542  return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
543 }
544 
545 
547 {
548  TwistAcc tmp;
549  tmp.rot = M*arg.rot;
550  tmp.vel = M*arg.vel+p*tmp.rot;
551  return tmp;
552 }
553 
555 {
556  TwistAcc tmp;
557  tmp.rot = M*arg.rot;
558  tmp.vel = M*arg.vel+p*tmp.rot;
559  return tmp;
560 }
561 
562 TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const
563 {
564  TwistAcc tmp;
565  tmp.rot = M.Inverse(arg.rot);
566  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
567  return tmp;
568 }
569 
570 TwistAcc FrameAcc::Inverse(const Twist& arg) const
571 {
572  TwistAcc tmp;
573  tmp.rot = M.Inverse(arg.rot);
574  tmp.vel = M.Inverse(arg.vel-p*arg.rot);
575  return tmp;
576 }
577 
578 Twist TwistAcc::GetTwist() const {
579  return Twist(vel.p,rot.p);
580 }
581 
582 Twist TwistAcc::GetTwistDot() const {
583  return Twist(vel.v,rot.v);
584 }
585 
586 bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) {
587  return (Equal(a.rot,b.rot,eps)&&
588  Equal(a.vel,b.vel,eps) );
589 }
590 bool Equal(const Twist& a,const TwistAcc& b,double eps) {
591  return (Equal(a.rot,b.rot,eps)&&
592  Equal(a.vel,b.vel,eps) );
593 }
594 bool Equal(const TwistAcc& a,const Twist& b,double eps) {
595  return (Equal(a.rot,b.rot,eps)&&
596  Equal(a.vel,b.vel,eps) );
597 }
598 
KDL::TwistAcc::rot
VectorAcc rot
rotational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:230
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:574
KDL::Rall2d::dd
V dd
2nd derivative
Definition: rall2d.h:103
KDL::FrameAcc
Definition: frameacc.hpp:188
KDL::RotationAcc::w
Vector w
angular velocity vector
Definition: frameacc.hpp:139
KDL::RotationAcc
Definition: frameacc.hpp:135
KDL::VectorAcc::p
Vector p
position vector
Definition: frameacc.hpp:90
Equal
bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
Definition: frameacc.inl:159
KDL::Vector
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:162
KDL::RotationAcc::R
Rotation R
rotation matrix
Definition: frameacc.hpp:138
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::VectorAcc::v
Vector v
velocity vector
Definition: frameacc.hpp:91
dot
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition: frameacc.inl:137
KDL::Norm
INLINE S Norm(const Rall1d< T, V, S > &value)
Definition: rall1d.h:440
KDL::TwistAcc
Definition: frameacc.hpp:226
operator+
VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.inl:24
KDL::FrameAcc::p
VectorAcc p
Translation, velocity and acceleration of origin.
Definition: frameacc.hpp:192
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
operator*
VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.inl:52
KDL::Rall2d
Definition: rall2d.h:76
operator/
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition: frameacc.inl:180
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::RotationAcc::dw
Vector dw
angular acceration vector
Definition: frameacc.hpp:140
KDL::VectorAcc
Definition: frameacc.hpp:87
KDL::Rall2d::t
T t
value
Definition: rall2d.h:101
KDL::VectorAcc::dv
Vector dv
acceleration vector
Definition: frameacc.hpp:92
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:575
KDL::doubleAcc
Rall2d< double, double, double > doubleAcc
Definition: frameacc.hpp:65
KDL::Rall2d::d
V d
1st derivative
Definition: rall2d.h:102
KDL::TwistAcc::vel
VectorAcc vel
translational velocity and its 1st and 2nd derivative
Definition: frameacc.hpp:229
KDL::FrameAcc::M
RotationAcc M
Rotation,angular velocity, and angular acceleration of frame.
Definition: frameacc.hpp:191
operator-
VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
Definition: frameacc.inl:28
KDL::Rotation
represents rotations in 3 dimensional space.
Definition: frames.hpp:303