36IMETHOD Vector::Vector(
const Vector & arg)
38 data[0] = arg.
data[0];
39 data[1] = arg.
data[1];
40 data[2] = arg.
data[2];
43IMETHOD 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];
57IMETHOD Vector
operator +(
const Vector & lhs,
const Vector& rhs)
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];
66IMETHOD Vector
operator -(
const Vector & lhs,
const Vector& rhs)
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];
75IMETHOD
double Vector::x()
const {
return data[0]; }
76IMETHOD
double Vector::y()
const {
return data[1]; }
77IMETHOD
double Vector::z()
const {
return data[2]; }
79IMETHOD
void Vector::x(
double _x ) { data[0] = _x; }
80IMETHOD
void Vector::y(
double _y ) { data[1] = _y; }
81IMETHOD
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];
143double Vector::operator()(
int index)
const {
144 FRAMES_CHECKI((0<=index)&&(index<=2));
148double& Vector::operator () (
int index)
150 FRAMES_CHECKI((0<=index)&&(index<=2));
160 tmp.
force = M*arg.force;
187 return Wrench(Vector::Zero(),Vector::Zero());
191void Wrench::ReverseSign()
193 torque.ReverseSign();
203 return Wrench(this->force,
204 this->torque+this->force*v_base_AB
223double& Wrench::operator()(
int i)
232double 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());
296void Twist::ReverseSign()
309 return Twist(this->vel+this->rot*v_base_AB,this->rot);
326double& Twist::operator()(
int i)
335double 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];
489 FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
494 FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=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
668 FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
684 FRAMES_CHECKI((0<=i)&&(i<=3)&&(0<=j)&&(j<=3));
713tmp_XY = F_someframe_XY*(tmp_XY);
745IMETHOD Vector2
operator +(
const Vector2 & lhs,
const Vector2& rhs)
747 return Vector2(lhs.data[0]+rhs.data[0],lhs.data[1]+rhs.data[1]);
750IMETHOD Vector2
operator -(
const Vector2 & lhs,
const Vector2& rhs)
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);
789 FRAMES_CHECKI((0<=index)&&(index<=1));
795 FRAMES_CHECKI((0<=index)&&(index<=1));
815 return Vector2(-arg.data[0],-arg.data[1]);
861 FRAMES_CHECKI((0<=i)&&(i<=1)&&(0<=j)&&(j<=1));
870IMETHOD Rotation2
operator *(
const Rotation2& lhs,
const Rotation2& rhs) {
871 return Rotation2(lhs.c*rhs.c-lhs.s*rhs.s,lhs.s*rhs.c+lhs.c*rhs.s);
897 c=cos(angle);
s=sin(angle);
930IMETHOD Frame2
operator *(
const Frame2& lhs,
const Frame2& rhs)
932 return Frame2(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p);
979 FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
995 FRAMES_CHECKI((0<=i)&&(i<=2)&&(0<=j)&&(j<=2));
1012IMETHOD
double dot(
const Vector& lhs,
const Vector& rhs) {
1013 return rhs(0)*lhs(0)+rhs(1)*lhs(1)+rhs(2)*lhs(2);
1016IMETHOD
double dot(
const Twist& lhs,
const Wrench& rhs) {
1017 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
1020IMETHOD
double dot(
const Wrench& rhs,
const Twist& lhs) {
1021 return dot(lhs.vel,rhs.force)+
dot(lhs.rot,rhs.torque);
1032IMETHOD
bool Equal(
const Vector& a,
const Vector& b,
double eps) {
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) );
1039IMETHOD
bool Equal(
const Frame& a,
const Frame& b,
double eps) {
1040 return (
Equal(a.p,b.p,eps)&&
1041 Equal(a.M,b.M,eps) );
1044IMETHOD
bool Equal(
const Wrench& a,
const Wrench& b,
double eps) {
1045 return (
Equal(a.force,b.force,eps)&&
1046 Equal(a.torque,b.torque,eps) );
1049IMETHOD
bool Equal(
const Twist& a,
const Twist& b,
double eps) {
1050 return (
Equal(a.rot,b.rot,eps)&&
1051 Equal(a.vel,b.vel,eps) );
1054IMETHOD
bool Equal(
const Vector2& a,
const Vector2& b,
double eps) {
1055 return (
Equal(a.data[0],b.data[0],eps)&&
1056 Equal(a.data[1],b.data[1],eps) );
1059IMETHOD
bool Equal(
const Rotation2& a,
const Rotation2& b,
double eps) {
1060 return (
Equal(a.c,b.c,eps) &&
Equal(a.s,b.s,eps) );
1063IMETHOD
bool Equal(
const Frame2& a,
const Frame2& b,
double eps) {
1064 return (
Equal(a.p,b.p,eps)&&
1065 Equal(a.M,b.M,eps) );
1107IMETHOD Rotation
Rot(
const Vector& axis_a_b) {
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)
1129IMETHOD Vector
diff(
const Vector& a,
const Vector& b,
double dt) {
1133IMETHOD 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;
1138IMETHOD 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)
1144IMETHOD Twist
diff(
const Twist& a,
const Twist& b,
double dt) {
1145 return Twist(
diff(a.vel,b.vel,dt),
diff(a.rot,b.rot,dt));
1148IMETHOD Wrench
diff(
const Wrench& a,
const Wrench& b,
double dt) {
1150 diff(a.force,b.force,dt),
1151 diff(a.torque,b.torque,dt)
1156IMETHOD Vector
addDelta(
const Vector& a,
const Vector&da,
double dt) {
1160IMETHOD Rotation
addDelta(
const Rotation& a,
const Vector&da,
double dt) {
1161 return Rot(da*dt)*a;
1163IMETHOD Frame
addDelta(
const Frame& a,
const Twist& da,
double dt) {
1169IMETHOD Twist
addDelta(
const Twist& a,
const Twist&da,
double dt) {
1172IMETHOD Wrench
addDelta(
const Wrench& a,
const Wrench&da,
double dt) {
1173 return Wrench(
addDelta(a.force,da.force,dt),
addDelta(a.torque,da.torque,dt));
1279 return (a.p == b.p &&
1292 return (a.data[0]==b.data[0]&&
1293 a.data[1]==b.data[1]&&
1294 a.data[2]==b.data[2] );
1306 return (a.rot==b.rot &&
1319 return (a.force==b.force &&
1320 a.torque==b.torque );
1335 return (a.data[0]==b.data[0]&&
1336 a.data[1]==b.data[1] );
A 2D frame class, for further documentation see the Frames class for methods with unchanged semantics...
Definition frames.hpp:1097
void SetIdentity()
Definition frames.inl:945
double operator()(int i, int j)
Treats a frame as a 3x3 matrix and returns element i,j Access to elements 0..2,0.....
Definition frames.inl:978
friend Frame2 operator*(const Frame2 &lhs, const Frame2 &rhs)
Frame2 & operator=(const Frame2 &arg)
Definition frames.inl:966
Rotation2 M
Orientation of the Frame.
Definition frames.hpp:1100
Vector2 p
origine of the Frame
Definition frames.hpp:1099
void SetInverse()
Definition frames.inl:951
Frame2 Inverse() const
Definition frames.inl:959
Definition frames.hpp:570
static Frame Identity()
Definition frames.inl:700
Rotation M
Orientation of the Frame.
Definition frames.hpp:573
void Integrate(const Twist &t_this, double frequency)
The twist <t_this> is expressed wrt the current frame.
Definition frames.inl:624
Vector p
origine of the Frame
Definition frames.hpp:572
Frame Inverse() const
Gives back inverse transformation of a Frame.
Definition frames.inl:422
double operator()(int i, int j)
Treats a frame as a 4x4 matrix and returns element i,j Access to elements 0..3,0.....
Definition frames.inl:667
Frame()
Definition frames.hpp:584
A 2D Rotation class, for conventions see Rotation.
Definition frames.hpp:1051
double GetRot() const
Gets the angle (in radians)
Definition frames.inl:904
double operator()(int i, int j) const
Access to elements 0..1,0..1, bounds are checked when NDEBUG is not set.
Definition frames.inl:860
static Rotation2 Rot(double angle)
The Rot... static functions give the value of the appropriate rotation matrix bac.
Definition frames.inl:900
void SetInverse()
Definition frames.inl:874
Rotation2()
c,s represent cos(angle), sin(angle), this also represents first col.
Definition frames.hpp:1057
static Rotation2 Identity()
Definition frames.inl:886
friend Rotation2 operator*(const Rotation2 &lhs, const Rotation2 &rhs)
double s
Definition frames.hpp:1052
double c
Definition frames.hpp:1052
void SetIdentity()
Definition frames.inl:890
Rotation2 Inverse() const
Definition frames.inl:878
void SetRot(double angle)
The SetRot.. functions set the value of *this to the appropriate rotation matrix.
Definition frames.inl:896
Rotation2 & operator=(const Rotation2 &arg)
Definition frames.inl:851
represents rotations in 3 dimensional space.
Definition frames.hpp:302
void DoRotZ(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:588
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition frames.inl:637
Rotation & operator=(const Rotation &arg)
Definition frames.inl:520
Rotation()
Definition frames.hpp:306
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
EulerZYX constructs a Rotation from the Euler ZYX parameters:
Definition frames.hpp:469
friend Rotation operator*(const Rotation &lhs, const Rotation &rhs)
Definition frames.cpp:173
static Rotation Identity()
Gives back an identity rotaton matrix.
Definition frames.inl:552
static Rotation RotX(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:605
void DoRotX(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:556
double data[9]
Definition frames.hpp:304
static Rotation RotY(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:610
void SetInverse()
Sets the value of *this to its inverse.
Definition frames.inl:653
static Rotation Rot(const Vector &rotvec, double angle)
Along an arbitrary axes.
Definition frames.cpp:293
static Rotation RotZ(double angle)
The Rot... static functions give the value of the appropriate rotation matrix back.
Definition frames.inl:615
double & operator()(int i, int j)
Access to elements 0..2,0..2, bounds are checked when NDEBUG is not set.
Definition frames.inl:488
void DoRotY(double angle)
The DoRot... functions apply a rotation R to *this,such that *this = *this * Rot.
Definition frames.inl:572
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
2D version of Vector
Definition frames.hpp:960
static Vector2 Zero()
Definition frames.inl:784
void ReverseSign()
Definition frames.inl:806
double x() const
Definition frames.inl:799
void Set3DZX(const Vector &v)
projects v in its ZX plane, and sets *this to these values
Definition frames.inl:831
Vector2 & operator=(const Vector2 &arg)
Definition frames.inl:737
void Set3DXY(const Vector &v)
projects v in its XY plane, and sets *this to these values
Definition frames.inl:819
Vector2()
Does not initialise to Zero().
Definition frames.hpp:964
Vector2 & operator+=(const Vector2 &arg)
Definition frames.inl:770
void Set3DYZ(const Vector &v)
projects v in its YZ plane, and sets *this to these values
Definition frames.inl:825
Vector2 & operator-=(const Vector2 &arg)
Definition frames.inl:777
double operator()(int index) const
Access to elements, range checked when NDEBUG is not set, from 0..1.
Definition frames.inl:788
double y() const
Definition frames.inl:800
void Set3DPlane(const Frame &F_someframe_XY, const Vector &v_someframe)
projects v_someframe in the XY plane of F_someframe_XY, and sets *this to these values expressed wrt ...
Definition frames.inl:838
double data[2]
Definition frames.hpp:961
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
void Set2DPlane(const Frame &F_someframe_XY, const Vector2 &v_XY)
a 3D vector where the 2D vector v_XY is put in the XY plane of the frame F_someframe_XY.
Definition frames.inl:707
void ReverseSign()
Reverses the sign of the Vector object itself.
Definition frames.inl:440
void Set2DZX(const Vector2 &v)
a 3D vector where the 2D vector v is put in the ZX plane
Definition frames.inl:475
double data[3]
Definition frames.hpp:163
static Vector Zero()
Definition frames.inl:138
void Set2DXY(const Vector2 &v)
a 3D vector where the 2D vector v is put in the XY plane
Definition frames.inl:458
double Norm(double eps=epsilon) const
Definition frames.cpp:117
void Set2DYZ(const Vector2 &v)
a 3D vector where the 2D vector v is put in the YZ plane
Definition frames.inl:466
represents both translational and rotational acceleration.
Definition frames.hpp:879
Vector force
Force that is applied at the origin of the current ref frame.
Definition frames.hpp:881
Vector torque
Torque that is applied at the origin of the current ref frame.
Definition frames.hpp:882
VectorAcc operator/(const VectorAcc &r1, double r2)
Definition frameacc.inl:180
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
Definition frameacc.inl:137
Vector operator/(const Vector &lhs, double rhs)
Definition frames.inl:101
IMETHOD Vector operator-(const Vector &lhs, const Vector &rhs)
Definition frames.inl:66
IMETHOD bool operator!=(const Frame &a, const Frame &b)
Definition frames.inl:1284
IMETHOD Vector operator+(const Vector &lhs, const Vector &rhs)
Definition frames.inl:57
Vector operator*(const Vector &lhs, double rhs)
Definition frames.inl:83
IMETHOD Rotation Rot(const Vector &axis_a_b)
axis_a_b is a rotation vector, its norm is a rotation angle axis_a_b rotates the a frame towards the ...
Definition frames.inl:1107
ArticulatedBodyInertia operator-(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
Definition articulatedbodyinertia.cpp:60
bool operator==(const Rotation &a, const Rotation &b)
Definition frames.cpp:430
ArticulatedBodyInertia operator+(const ArticulatedBodyInertia &Ia, const ArticulatedBodyInertia &Ib)
addition I: I_new = I_old1 + I_old2, make sure that I_old1 and I_old2 are expressed in the same refer...
Definition articulatedbodyinertia.cpp:53
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
determines the difference of vector b with vector a.
IMETHOD void random(doubleVel &F)
Definition framevel.hpp:46
void SetToZero(Jacobian &jac)
Definition jacobian.cpp:79
ArticulatedBodyInertia operator*(double a, const ArticulatedBodyInertia &I)
Scalar product: I_new = double * I_old.
Definition articulatedbodyinertia.cpp:49
IMETHOD void posrandom(doubleVel &F)
Definition framevel.hpp:50
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)