38 data[0] = arg.
data[0];
39 data[1] = arg.
data[1];
40 data[2] = arg.
data[2];
43 IMETHOD Vector::Vector(
double x,
double y,
double z)
45 data[0]=x;data[1]=y;data[2]=z;
51 data[0] = arg.
data[0];
52 data[1] = arg.
data[1];
53 data[2] = arg.
data[2];
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];
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];
75 IMETHOD double Vector::x()
const {
return data[0]; }
76 IMETHOD double Vector::y()
const {
return data[1]; }
77 IMETHOD double Vector::z()
const {
return data[2]; }
79 IMETHOD void Vector::x(
double _x ) { data[0] = _x; }
80 IMETHOD void Vector::y(
double _y ) { data[1] = _y; }
81 IMETHOD void Vector::z(
double _z ) {
data[2] = _z; }
86 tmp.data[0] = lhs.data[0]*rhs;
87 tmp.data[1] = lhs.data[1]*rhs;
88 tmp.data[2] = lhs.data[2]*rhs;
95 tmp.data[0] = lhs*rhs.data[0];
96 tmp.data[1] = lhs*rhs.data[1];
97 tmp.data[2] = lhs*rhs.data[2];
104 tmp.data[0] = lhs.data[0]/rhs;
105 tmp.data[1] = lhs.data[1]/rhs;
106 tmp.data[2] = lhs.data[2]/rhs;
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];
123 data[0]+=arg.
data[0];
124 data[1]+=arg.
data[1];
125 data[2]+=arg.
data[2];
132 data[0]-=arg.
data[0];
133 data[1]-=arg.
data[1];
134 data[2]-=arg.
data[2];
143 double Vector::operator()(
int index)
const {
148 double& Vector::operator () (
int index)
160 tmp.
force = M*arg.force;
187 return Wrench(Vector::Zero(),Vector::Zero());
191 void Wrench::ReverseSign()
193 torque.ReverseSign();
203 return Wrench(this->force,
204 this->torque+this->force*v_base_AB
223 double& Wrench::operator()(
int i)
232 double Wrench::operator()(
int i)
const
244 return Wrench(lhs.force*rhs,lhs.torque*rhs);
249 return Wrench(lhs*rhs.force,lhs*rhs.torque);
254 return Wrench(lhs.force/rhs,lhs.torque/rhs);
260 return Wrench(lhs.force+rhs.force,lhs.torque+rhs.torque);
265 return Wrench(lhs.force-rhs.force,lhs.torque-rhs.torque);
271 return Wrench(-arg.force,-arg.torque);
279 tmp.
vel = M*arg.vel+p*tmp.
rot;
285 tmp.
rot = M.Inverse(arg.
rot);
292 return Twist(Vector::Zero(),Vector::Zero());
296 void Twist::ReverseSign()
309 return Twist(this->vel+this->rot*v_base_AB,this->rot);
326 double& Twist::operator()(
int i)
335 double Twist::operator()(
int i)
const
347 return Twist(lhs.vel*rhs,lhs.rot*rhs);
352 return Twist(lhs*rhs.vel,lhs*rhs.rot);
357 return Twist(lhs.vel/rhs,lhs.rot/rhs);
363 return Twist(lhs.vel+rhs.vel,lhs.rot+rhs.rot);
368 return Twist(lhs.vel-rhs.vel,lhs.rot-rhs.rot);
374 return Twist(-arg.vel,-arg.rot);
381 return Twist(lhs.rot*rhs.vel+lhs.vel*rhs.rot,lhs.rot*rhs.rot);
385 return Wrench(lhs.rot*rhs.force,lhs.rot*rhs.torque+lhs.vel*rhs.force);
396 M = Rotation::Identity();
409 return Frame(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
419 return M.Inverse(arg-p);
424 return Frame(M.Inverse(),-M.Inverse(p));
452 tmp.data[0]=-arg.data[0];
453 tmp.data[1]=-arg.data[1];
454 tmp.data[2]=-arg.data[2];
499 double Xy,
double Yy,
double Zy,
500 double Xz,
double Yz,
double Zz)
517 while (count--)
data[count] = arg.
data[count];
522 while (count--)
data[count] = arg.
data[count];
541 return Twist((*
this)*arg.vel,(*
this)*arg.rot);
549 return Wrench((*
this)*arg.force,(*
this)*arg.torque);
558 double cs =
cos(angle);
559 double sn =
sin(angle);
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);
574 double cs =
cos(angle);
575 double sn =
sin(angle);
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);
590 double cs =
cos(angle);
591 double sn =
sin(angle);
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);
606 double cs=
cos(angle);
607 double sn=
sin(angle);
608 return Rotation(1,0,0,0,cs,-sn,0,sn,cs);
611 double cs=
cos(angle);
612 double sn=
sin(angle);
613 return Rotation(cs,0,sn,0,1,0,-sn,0,cs);
616 double cs=
cos(angle);
617 double sn=
sin(angle);
618 return Rotation(cs,-sn,0,sn,cs,0,0,0,1);
626 double n = t_this.
rot.
Norm()/samplefrequency;
628 p +=
M*(t_this.
vel/samplefrequency);
632 t_this.
vel/samplefrequency
713 tmp_XY = F_someframe_XY*(tmp_XY);
747 return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
752 return Vector2(lhs.data[0]-rhs.data[0],lhs.data[1]-rhs.data[1]);
757 return Vector2(lhs.data[0]*rhs,lhs.data[1]*rhs);
762 return Vector2(lhs*rhs.data[0],lhs*rhs.data[1]);
767 return Vector2(lhs.data[0]/rhs,lhs.data[1]/rhs);
815 return Vector2(-arg.data[0],-arg.data[1]);
871 return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
932 return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
1013 return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1017 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
1021 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
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) );
1040 return (
Equal(a.p,b.p,eps)&&
1041 Equal(a.M,b.M,eps) );
1045 return (
Equal(a.force,b.force,eps)&&
1046 Equal(a.torque,b.torque,eps) );
1050 return (
Equal(a.rot,b.rot,eps)&&
1051 Equal(a.vel,b.vel,eps) );
1055 return (
Equal(a.data[0],b.data[0],eps)&&
1056 Equal(a.data[1],b.data[1],eps) );
1060 return (
Equal(a.c,b.c,eps) &&
Equal(a.s,b.s,eps) );
1064 return (
Equal(a.p,b.p,eps)&&
1065 Equal(a.M,b.M,eps) );
1112 Vector rotvec = axis_a_b;
1113 double angle = rotvec.Normalize(1E-10);
1114 double ct =
::cos(angle);
1115 double st =
::sin(angle);
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)
1133 IMETHOD 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;
1138 IMETHOD Twist
diff(
const Frame& F_a_b1,
const Frame& F_a_b2,
double dt) {
1140 diff(F_a_b1.p,F_a_b2.p,dt),
1141 diff(F_a_b1.M,F_a_b2.M,dt)
1145 return Twist(
diff(a.vel,b.vel,dt),
diff(a.rot,b.rot,dt));
1150 diff(a.force,b.force,dt),
1151 diff(a.torque,b.torque,dt)
1161 return Rot(da*dt)*a;
1173 return Wrench(
addDelta(a.force,da.force,dt),
addDelta(a.torque,da.torque,dt));
1276 #ifdef KDL_USE_EQUAL
1279 return (a.p == b.p &&
1289 #ifdef KDL_USE_EQUAL
1292 return (a.data[0]==b.data[0]&&
1293 a.data[1]==b.data[1]&&
1294 a.data[2]==b.data[2] );
1303 #ifdef KDL_USE_EQUAL
1306 return (a.rot==b.rot &&
1316 #ifdef KDL_USE_EQUAL
1319 return (a.force==b.force &&
1320 a.torque==b.torque );
1332 #ifdef KDL_USE_EQUAL
1335 return (a.data[0]==b.data[0]&&
1336 a.data[1]==b.data[1] );