KDL 1.5.1
Loading...
Searching...
No Matches
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
24VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) {
25 return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv);
26}
27
28VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) {
29 return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv);
30}
31VectorAcc operator + (const Vector& r1,const VectorAcc& r2) {
32 return VectorAcc(r1+r2.p,r2.v,r2.dv);
33}
34
35VectorAcc operator - (const Vector& r1,const VectorAcc& r2) {
36 return VectorAcc(r1-r2.p, -r2.v, -r2.dv);
37}
38VectorAcc operator + (const VectorAcc& r1,const Vector& r2) {
39 return VectorAcc(r1.p+r2,r1.v,r1.dv);
40}
41
42VectorAcc operator - (const VectorAcc& r1,const Vector& r2) {
43 return VectorAcc(r1.p-r2, r1.v, r1.dv);
44}
45
46// unary -
47VectorAcc operator - (const VectorAcc& r) {
48 return VectorAcc(-r.p,-r.v,-r.dv);
49}
50
51// cross prod.
52VectorAcc 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
59VectorAcc operator * (const VectorAcc& r1,const Vector& r2) {
60 return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
61}
62
63VectorAcc 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.
70VectorAcc operator * (double r1,const VectorAcc& r2) {
71 return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv );
72}
73
74VectorAcc operator * (const VectorAcc& r1,double r2) {
75 return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 );
76}
77
78VectorAcc 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
85VectorAcc 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
92VectorAcc& VectorAcc::operator = (const VectorAcc& arg) {
93 p=arg.p;
94 v=arg.v;
95 dv=arg.dv;
96 return *this;
97}
98
99VectorAcc& VectorAcc::operator = (const Vector& arg) {
100 p=arg;
101 v=Vector::Zero();
102 dv=Vector::Zero();
103 return *this;
104}
105
106VectorAcc& VectorAcc::operator += (const VectorAcc& arg) {
107 p+=arg.p;
108 v+=arg.v;
109 dv+= arg.dv;
110 return *this;
111}
112VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) {
113 p-=arg.p;
114 v-=arg.v;
115 dv-=arg.dv;
116 return *this;
117}
118
119VectorAcc VectorAcc::Zero() {
120 return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero());
121}
122
123void VectorAcc::ReverseSign() {
124 p.ReverseSign();
125 v.ReverseSign();
126 dv.ReverseSign();
127}
128
129doubleAcc VectorAcc::Norm(double eps) {
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
137doubleAcc 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
144doubleAcc 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
151doubleAcc 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
159bool 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
166bool 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
173bool 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
180VectorAcc operator / (const VectorAcc& r1,double r2) {
181 return r1*(1.0/r2);
182}
183
184VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) {
185 return r2*(1.0/r1);
186}
187
188
189
191
192RotationAcc 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
199RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) {
200 return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw);
201}
202
203RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) {
204 return RotationAcc( r1.R*r2, r1.w, r1.dw );
205}
206
207RotationAcc& RotationAcc::operator = (const RotationAcc& arg) {
208 R=arg.R;
209 w=arg.w;
210 dw=arg.dw;
211 return *this;
212}
213RotationAcc& RotationAcc::operator = (const Rotation& arg) {
214 R = arg;
215 w = Vector::Zero();
216 dw = Vector::Zero();
217 return *this;
218}
219
220RotationAcc RotationAcc::Identity() {
221 return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero());
222}
223
224RotationAcc RotationAcc::Inverse() const {
225 return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw));
226}
227
228VectorAcc 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
236VectorAcc 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
245VectorAcc RotationAcc::operator*(const VectorAcc& arg) const {
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
254VectorAcc operator*(const Rotation& R,const VectorAcc& x) {
255 return VectorAcc(R*x.p,R*x.v,R*x.dv);
256}
257
258VectorAcc RotationAcc::operator*(const Vector& arg) const {
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 }
275RotationAcc 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 }
286RotationAcc 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 }
298RotationAcc 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
323bool 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}
326bool 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}
330bool 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
338FrameAcc& FrameAcc::operator = (const FrameAcc& arg) {
339 M=arg.M;
340 p=arg.p;
341 return *this;
342}
343
344FrameAcc FrameAcc::Identity() {
345 return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero());
346}
347
348
349FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs)
350{
351 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
352}
353FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs)
354{
355 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
356}
357FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs)
358{
359 return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
360}
361
362VectorAcc FrameAcc::operator *(const VectorAcc & arg) const
363{
364 return M*arg+p;
365}
366VectorAcc FrameAcc::operator *(const Vector & arg) const
367{
368 return M*arg+p;
369}
370
371VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const
372{
373 return M.Inverse(arg-p);
374}
375
376VectorAcc FrameAcc::Inverse(const Vector& arg) const
377{
378 return M.Inverse(arg-p);
379}
380
381FrameAcc FrameAcc::Inverse() const
382{
383 return FrameAcc(M.Inverse(),-M.Inverse(p));
384}
385
386FrameAcc& FrameAcc::operator =(const Frame & arg)
387{
388 M = arg.M;
389 p = arg.p;
390 return *this;
391}
392
393bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) {
394 return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
395}
396bool Equal(const Frame& r1,const FrameAcc& r2,double eps) {
397 return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps));
398}
399bool 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
404Frame FrameAcc::GetFrame() const {
405 return Frame(M.R,p.p);
406}
407
408
409Twist FrameAcc::GetTwist() const {
410 return Twist(p.v,M.w);
411}
412
413
414Twist 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
434TwistAcc TwistAcc::Zero()
435{
436 return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero());
437}
438
439
440void TwistAcc::ReverseSign()
441{
442 vel.ReverseSign();
443 rot.ReverseSign();
444}
445
446TwistAcc 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
456TwistAcc& TwistAcc::operator-=(const TwistAcc& arg)
457{
458 vel-=arg.vel;
459 rot -=arg.rot;
460 return *this;
461}
462
463TwistAcc& TwistAcc::operator+=(const TwistAcc& arg)
464{
465 vel+=arg.vel;
466 rot +=arg.rot;
467 return *this;
468}
469
470
471TwistAcc operator*(const TwistAcc& lhs,double rhs)
472{
473 return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
474}
475
476TwistAcc operator*(double lhs,const TwistAcc& rhs)
477{
478 return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
479}
480
481TwistAcc operator/(const TwistAcc& lhs,double rhs)
482{
483 return TwistAcc(lhs.vel/rhs,lhs.rot/rhs);
484}
485
486
487TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs)
488{
489 return TwistAcc(lhs.vel*rhs,lhs.rot*rhs);
490}
491
492TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs)
493{
494 return TwistAcc(lhs*rhs.vel,lhs*rhs.rot);
495}
496
497TwistAcc 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
505TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs)
506{
507 return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
508}
509
510TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs)
511{
512 return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
513}
514
515// unary -
516TwistAcc operator-(const TwistAcc& arg)
517{
518 return TwistAcc(-arg.vel,-arg.rot);
519}
520
521
522
523
524
525TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const
526{
527 return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
528}
529
530TwistAcc RotationAcc::operator * (const TwistAcc& arg) const
531{
532 return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
533}
534
535TwistAcc RotationAcc::Inverse(const Twist& arg) const
536{
537 return TwistAcc(Inverse(arg.vel),Inverse(arg.rot));
538}
539
540TwistAcc RotationAcc::operator * (const Twist& arg) const
541{
542 return TwistAcc((*this)*arg.vel,(*this)*arg.rot);
543}
544
545
546TwistAcc FrameAcc::operator * (const TwistAcc& arg) const
547{
548 TwistAcc tmp;
549 tmp.rot = M*arg.rot;
550 tmp.vel = M*arg.vel+p*tmp.rot;
551 return tmp;
552}
553
554TwistAcc FrameAcc::operator * (const Twist& arg) const
555{
556 TwistAcc tmp;
557 tmp.rot = M*arg.rot;
558 tmp.vel = M*arg.vel+p*tmp.rot;
559 return tmp;
560}
561
562TwistAcc 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
570TwistAcc 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
578Twist TwistAcc::GetTwist() const {
579 return Twist(vel.p,rot.p);
580}
581
582Twist TwistAcc::GetTwistDot() const {
583 return Twist(vel.v,rot.v);
584}
585
586bool 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}
590bool 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}
594bool 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
bool Equal(const VectorAcc &r1, const VectorAcc &r2, double eps)
Definition frameacc.inl:159
VectorAcc operator*(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:52
VectorAcc operator-(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:28
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition frameacc.inl:180
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition frameacc.inl:137
VectorAcc operator+(const VectorAcc &r1, const VectorAcc &r2)
Definition frameacc.inl:24
Rall2d< double, double, double > doubleAcc
Definition frameacc.hpp:41