KDL 1.5.1
Loading...
Searching...
No Matches
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
22FrameVel& FrameVel::operator = (const FrameVel& arg) {
23 M=arg.M;
24 p=arg.p;
25 return *this;
26}
27
28FrameVel FrameVel::Identity() {
29 return FrameVel(RotationVel::Identity(),VectorVel::Zero());
30}
31
32
33FrameVel operator *(const FrameVel& lhs,const FrameVel& rhs)
34{
35 return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
36}
37FrameVel operator *(const FrameVel& lhs,const Frame& rhs)
38{
39 return FrameVel(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
40}
41FrameVel operator *(const Frame& lhs,const FrameVel& rhs)
42{
43 return FrameVel(lhs.M*rhs.M , lhs.M*rhs.p+lhs.p );
44}
45
46VectorVel FrameVel::operator *(const VectorVel & arg) const
47{
48 return M*arg+p;
49}
50VectorVel FrameVel::operator *(const Vector & arg) const
51{
52 return M*arg+p;
53}
54
55VectorVel FrameVel::Inverse(const VectorVel& arg) const
56{
57 return M.Inverse(arg-p);
58}
59
60VectorVel FrameVel::Inverse(const Vector& arg) const
61{
62 return M.Inverse(arg-p);
63}
64
65FrameVel FrameVel::Inverse() const
66{
67 return FrameVel(M.Inverse(),-M.Inverse(p));
68}
69
70FrameVel& FrameVel::operator = (const Frame& arg) {
71 M = arg.M;
72 p = arg.p;
73 return *this;
74}
75bool Equal(const FrameVel& r1,const FrameVel& r2,double eps) {
76 return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
77}
78bool Equal(const Frame& r1,const FrameVel& r2,double eps) {
79 return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
80}
81bool Equal(const FrameVel& r1,const Frame& r2,double eps) {
82 return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
83}
84bool 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}
92bool operator!=(const FrameVel& r1,const FrameVel& r2) {
93 return !operator==(r1,r2);
94}
95bool 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}
103bool operator!=(const Frame& r1,const FrameVel& r2) {
104 return !operator==(r1,r2);
105}
106bool 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}
114bool operator!=(const FrameVel& r1,const Frame& r2) {
115 return !operator==(r1,r2);
116}
117
118
119
120Frame FrameVel::GetFrame() const {
121 return Frame(M.R,p.p);
122}
123
124Twist FrameVel::GetTwist() const {
125 return Twist(p.v,M.w);
126}
127
128
129RotationVel operator* (const RotationVel& r1,const RotationVel& r2) {
130 return RotationVel( r1.R*r2.R, r1.w + r1.R*r2.w );
131}
132
133RotationVel operator* (const Rotation& r1,const RotationVel& r2) {
134 return RotationVel( r1*r2.R, r1*r2.w );
135}
136
137RotationVel operator* (const RotationVel& r1,const Rotation& r2) {
138 return RotationVel( r1.R*r2, r1.w );
139}
140
141RotationVel& RotationVel::operator = (const RotationVel& arg) {
142 R=arg.R;
143 w=arg.w;
144 return *this;
145 }
146RotationVel& RotationVel::operator = (const Rotation& arg) {
147 R=arg;
148 w=Vector::Zero();
149 return *this;
150}
151
152VectorVel RotationVel::UnitX() const {
153 return VectorVel(R.UnitX(),w*R.UnitX());
154}
155
156VectorVel RotationVel::UnitY() const {
157 return VectorVel(R.UnitY(),w*R.UnitY());
158}
159
160VectorVel RotationVel::UnitZ() const {
161 return VectorVel(R.UnitZ(),w*R.UnitZ());
162}
163
164
165
166RotationVel RotationVel::Identity() {
167 return RotationVel(Rotation::Identity(),Vector::Zero());
168}
169
170RotationVel RotationVel::Inverse() const {
171 return RotationVel(R.Inverse(),-R.Inverse(w));
172}
173
174VectorVel 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
181VectorVel 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
189VectorVel RotationVel::operator*(const VectorVel& arg) const {
190 Vector tmp=R*arg.p;
191 return VectorVel(tmp,w*tmp+R*arg.v);
192}
193
194VectorVel 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
204void RotationVel::DoRotX(const doubleVel& angle) {
205 w+=R*Vector(angle.grad,0,0);
206 R.DoRotX(angle.t);
207}
208RotationVel RotationVel::RotX(const doubleVel& angle) {
209 return RotationVel(Rotation::RotX(angle.t),Vector(angle.grad,0,0));
210}
211
212void RotationVel::DoRotY(const doubleVel& angle) {
213 w+=R*Vector(0,angle.grad,0);
214 R.DoRotY(angle.t);
215}
216RotationVel RotationVel::RotY(const doubleVel& angle) {
217 return RotationVel(Rotation::RotX(angle.t),Vector(0,angle.grad,0));
218}
219
220void RotationVel::DoRotZ(const doubleVel& angle) {
221 w+=R*Vector(0,0,angle.grad);
222 R.DoRotZ(angle.t);
223}
224RotationVel RotationVel::RotZ(const doubleVel& angle) {
225 return RotationVel(Rotation::RotZ(angle.t),Vector(0,0,angle.grad));
226}
227
228
229RotationVel 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
238RotationVel 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
245VectorVel operator + (const VectorVel& r1,const VectorVel& r2) {
246 return VectorVel(r1.p+r2.p,r1.v+r2.v);
247}
248
249VectorVel operator - (const VectorVel& r1,const VectorVel& r2) {
250 return VectorVel(r1.p-r2.p,r1.v-r2.v);
251}
252
253VectorVel operator + (const VectorVel& r1,const Vector& r2) {
254 return VectorVel(r1.p+r2,r1.v);
255}
256
257VectorVel operator - (const VectorVel& r1,const Vector& r2) {
258 return VectorVel(r1.p-r2,r1.v);
259}
260
261VectorVel operator + (const Vector& r1,const VectorVel& r2) {
262 return VectorVel(r1+r2.p,r2.v);
263}
264
265VectorVel operator - (const Vector& r1,const VectorVel& r2) {
266 return VectorVel(r1-r2.p,-r2.v);
267}
268
269// unary -
270VectorVel operator - (const VectorVel& r) {
271 return VectorVel(-r.p,-r.v);
272}
273
274void SetToZero(VectorVel& v){
275 SetToZero(v.p);
276 SetToZero(v.v);
277}
278
279// cross prod.
280VectorVel operator * (const VectorVel& r1,const VectorVel& r2) {
281 return VectorVel(r1.p*r2.p, r1.p*r2.v+r1.v*r2.p);
282}
283
284VectorVel operator * (const VectorVel& r1,const Vector& r2) {
285 return VectorVel(r1.p*r2, r1.v*r2);
286}
287
288VectorVel operator * (const Vector& r1,const VectorVel& r2) {
289 return VectorVel(r1*r2.p, r1*r2.v);
290}
291
292
293
294// scalar mult.
295VectorVel operator * (double r1,const VectorVel& r2) {
296 return VectorVel(r1*r2.p, r1*r2.v);
297}
298
299VectorVel operator * (const VectorVel& r1,double r2) {
300 return VectorVel(r1.p*r2, r1.v*r2);
301}
302
303
304
305VectorVel operator * (const doubleVel& r1,const VectorVel& r2) {
306 return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
307}
308
309VectorVel operator * (const VectorVel& r2,const doubleVel& r1) {
310 return VectorVel(r1.t*r2.p, r1.t*r2.v + r1.grad*r2.p);
311}
312
313VectorVel operator / (const VectorVel& r1,double r2) {
314 return VectorVel(r1.p/r2, r1.v/r2);
315}
316
317VectorVel 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
321VectorVel operator*(const Rotation& R,const VectorVel& x) {
322 return VectorVel(R*x.p,R*x.v);
323}
324
325VectorVel& VectorVel::operator = (const VectorVel& arg) {
326 p=arg.p;
327 v=arg.v;
328 return *this;
329}
330VectorVel& VectorVel::operator = (const Vector& arg) {
331 p=arg;
332 v=Vector::Zero();
333 return *this;
334}
335VectorVel& VectorVel::operator += (const VectorVel& arg) {
336 p+=arg.p;
337 v+=arg.v;
338 return *this;
339}
340VectorVel& VectorVel::operator -= (const VectorVel& arg) {
341 p-=arg.p;
342 v-=arg.v;
343 return *this;
344}
345
346VectorVel VectorVel::Zero() {
347 return VectorVel(Vector::Zero(),Vector::Zero());
348}
349void VectorVel::ReverseSign() {
350 p.ReverseSign();
351 v.ReverseSign();
352}
353doubleVel 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
360bool Equal(const VectorVel& r1,const VectorVel& r2,double eps) {
361 return (Equal(r1.p,r2.p,eps) && Equal(r1.v,r2.v,eps));
362}
363bool Equal(const Vector& r1,const VectorVel& r2,double eps) {
364 return (Equal(r1,r2.p,eps) && Equal(Vector::Zero(),r2.v,eps));
365}
366bool Equal(const VectorVel& r1,const Vector& r2,double eps) {
367 return (Equal(r1.p,r2,eps) && Equal(r1.v,Vector::Zero(),eps));
368}
369bool 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}
377bool operator!=(const VectorVel& r1,const VectorVel& r2) {
378 return !operator==(r1,r2);
379}
380bool 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}
388bool operator!=(const Vector& r1,const VectorVel& r2) {
389 return !operator==(r1,r2);
390}
391bool 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}
399bool operator!=(const VectorVel& r1,const Vector& r2) {
400 return !operator==(r1,r2);
401}
402
403
404bool Equal(const RotationVel& r1,const RotationVel& r2,double eps) {
405 return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps));
406}
407bool Equal(const Rotation& r1,const RotationVel& r2,double eps) {
408 return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps));
409}
410bool Equal(const RotationVel& r1,const Rotation& r2,double eps) {
411 return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps));
412}
413bool 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}
421bool operator!=(const RotationVel& r1,const RotationVel& r2) {
422 return !operator==(r1,r2);
423}
424bool 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}
432bool operator!=(const Rotation& r1,const RotationVel& r2) {
433 return !operator==(r1,r2);
434}
435bool 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}
443bool operator!=(const RotationVel& r1,const Rotation& r2) {
444 return !operator==(r1,r2);
445}
446
447
448bool 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}
452bool 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}
456bool 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}
460bool 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}
468bool operator!=(const TwistVel& a,const TwistVel& b) {
469 return !operator==(a,b);
470}
471bool 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}
479bool operator!=(const Twist& r1,const TwistVel& r2) {
480 return !operator==(r1,r2);
481}
482bool 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}
490bool operator!=(const TwistVel& r1,const Twist& r2) {
491 return !operator==(r1,r2);
492}
493
494
495IMETHOD 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}
498IMETHOD doubleVel dot(const VectorVel& lhs,const Vector& rhs) {
499 return doubleVel(dot(lhs.p,rhs),dot(lhs.v,rhs));
500}
501IMETHOD doubleVel dot(const Vector& lhs,const VectorVel& rhs) {
502 return doubleVel(dot(lhs,rhs.p),dot(lhs,rhs.v));
503}
504
505TwistVel TwistVel::Zero()
506{
507 return TwistVel(VectorVel::Zero(),VectorVel::Zero());
508}
509
510
511void TwistVel::ReverseSign()
512{
513 vel.ReverseSign();
514 rot.ReverseSign();
515}
516
517TwistVel 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
527TwistVel& TwistVel::operator-=(const TwistVel& arg)
528{
529 vel-=arg.vel;
530 rot -=arg.rot;
531 return *this;
532}
533
534TwistVel& TwistVel::operator+=(const TwistVel& arg)
535{
536 vel+=arg.vel;
537 rot +=arg.rot;
538 return *this;
539}
540
541
542TwistVel operator*(const TwistVel& lhs,double rhs)
543{
544 return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
545}
546
547TwistVel operator*(double lhs,const TwistVel& rhs)
548{
549 return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
550}
551
552TwistVel operator/(const TwistVel& lhs,double rhs)
553{
554 return TwistVel(lhs.vel/rhs,lhs.rot/rhs);
555}
556
557
558TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs)
559{
560 return TwistVel(lhs.vel*rhs,lhs.rot*rhs);
561}
562
563TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs)
564{
565 return TwistVel(lhs*rhs.vel,lhs*rhs.rot);
566}
567
568TwistVel 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
576TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs)
577{
578 return TwistVel(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
579}
580
581TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs)
582{
583 return TwistVel(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
584}
585
586// unary -
587TwistVel operator-(const TwistVel& arg)
588{
589 return TwistVel(-arg.vel,-arg.rot);
590}
591
592void SetToZero(TwistVel& v)
593{
594 SetToZero(v.vel);
595 SetToZero(v.rot);
596}
597
598
599
600
601
602TwistVel RotationVel::Inverse(const TwistVel& arg) const
603{
604 return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
605}
606
607TwistVel RotationVel::operator * (const TwistVel& arg) const
608{
609 return TwistVel((*this)*arg.vel,(*this)*arg.rot);
610}
611
612TwistVel RotationVel::Inverse(const Twist& arg) const
613{
614 return TwistVel(Inverse(arg.vel),Inverse(arg.rot));
615}
616
617TwistVel RotationVel::operator * (const Twist& arg) const
618{
619 return TwistVel((*this)*arg.vel,(*this)*arg.rot);
620}
621
622
623TwistVel 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
631TwistVel 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
639TwistVel 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
647TwistVel 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
655Twist TwistVel::GetTwist() const {
656 return Twist(vel.p,rot.p);
657}
658
659Twist TwistVel::GetTwistDot() const {
660 return Twist(vel.v,rot.v);
661}
Definition framevel.hpp:219
VectorVel p
Definition framevel.hpp:222
RotationVel M
Definition framevel.hpp:221
Definition frames.hpp:570
Rotation M
Orientation of the Frame.
Definition frames.hpp:573
Vector p
origine of the Frame
Definition frames.hpp:572
Definition framevel.hpp:152
Vector w
Definition framevel.hpp:155
Rotation R
Definition framevel.hpp:154
represents rotations in 3 dimensional space.
Definition frames.hpp:302
Definition framevel.hpp:278
VectorVel vel
Definition framevel.hpp:280
VectorVel rot
Definition framevel.hpp:281
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
Definition framevel.hpp:89
Vector p
Definition framevel.hpp:91
Vector v
Definition framevel.hpp:92
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
double Normalize(double eps=epsilon)
Normalizes this vector and returns it norm makes v a unitvector and returns the norm of v.
Definition frames.cpp:147
void SetToZero(VectorVel &v)
Definition framevel.inl:274
FrameVel operator*(const FrameVel &lhs, const FrameVel &rhs)
Definition framevel.inl:33
VectorVel operator/(const VectorVel &r1, double r2)
Definition framevel.inl:313
VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
Definition framevel.inl:249
IMETHOD doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
Definition framevel.inl:495
VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
Definition framevel.inl:245
bool operator==(const FrameVel &r1, const FrameVel &r2)
Definition framevel.inl:84
bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
Definition framevel.inl:75
bool operator!=(const FrameVel &r1, const FrameVel &r2)
Definition framevel.inl:92
Rall1d< double > doubleVel
Definition framevel.hpp:36