diff options
Diffstat (limited to 'intern/itasc/kdl/frameacc.inl')
-rw-r--r-- | intern/itasc/kdl/frameacc.inl | 598 |
1 files changed, 598 insertions, 0 deletions
diff --git a/intern/itasc/kdl/frameacc.inl b/intern/itasc/kdl/frameacc.inl new file mode 100644 index 00000000000..a8ea35ad436 --- /dev/null +++ b/intern/itasc/kdl/frameacc.inl @@ -0,0 +1,598 @@ +/***************************************************************************** + * \file + * provides inline functions of rrframes.h + * + * \author + * Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven + * + * \version + * ORO_Geometry V0.2 + * + * \par History + * - $log$ + * + * \par Release + * $Id: frameacc.inl 19905 2009-04-23 13:29:54Z ben2610 $ + * $Name: $ + ****************************************************************************/ + + + + +/////////////////// VectorAcc ///////////////////////////////////// + +VectorAcc operator + (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p+r2.p,r1.v+r2.v,r1.dv+r2.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p-r2.p, r1.v-r2.v, r1.dv-r2.dv); +} +VectorAcc operator + (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1+r2.p,r2.v,r2.dv); +} + +VectorAcc operator - (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1-r2.p, -r2.v, -r2.dv); +} +VectorAcc operator + (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p+r2,r1.v,r1.dv); +} + +VectorAcc operator - (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p-r2, r1.v, r1.dv); +} + +// unary - +VectorAcc operator - (const VectorAcc& r) { + return VectorAcc(-r.p,-r.v,-r.dv); +} + +// cross prod. +VectorAcc operator * (const VectorAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.p*r2.p, + r1.p*r2.v+r1.v*r2.p, + r1.dv*r2.p+2*r1.v*r2.v+r1.p*r2.dv + ); +} + +VectorAcc operator * (const VectorAcc& r1,const Vector& r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const Vector& r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + + + +// scalar mult. +VectorAcc operator * (double r1,const VectorAcc& r2) { + return VectorAcc(r1*r2.p, r1*r2.v, r1*r2.dv ); +} + +VectorAcc operator * (const VectorAcc& r1,double r2) { + return VectorAcc(r1.p*r2, r1.v*r2, r1.dv*r2 ); +} + +VectorAcc operator * (const doubleAcc& r1,const VectorAcc& r2) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc operator * (const VectorAcc& r2,const doubleAcc& r1) { + return VectorAcc(r1.t*r2.p, + r1.t*r2.v + r1.d*r2.p, + r1.t*r2.dv + 2*r1.d*r2.v + r1.dd*r2.p + ); +} + +VectorAcc& VectorAcc::operator = (const VectorAcc& arg) { + p=arg.p; + v=arg.v; + dv=arg.dv; + return *this; +} + +VectorAcc& VectorAcc::operator = (const Vector& arg) { + p=arg; + v=Vector::Zero(); + dv=Vector::Zero(); + return *this; +} + +VectorAcc& VectorAcc::operator += (const VectorAcc& arg) { + p+=arg.p; + v+=arg.v; + dv+= arg.dv; + return *this; +} +VectorAcc& VectorAcc::operator -= (const VectorAcc& arg) { + p-=arg.p; + v-=arg.v; + dv-=arg.dv; + return *this; +} + +VectorAcc VectorAcc::Zero() { + return VectorAcc(Vector::Zero(),Vector::Zero(),Vector::Zero()); +} + +void VectorAcc::ReverseSign() { + p.ReverseSign(); + v.ReverseSign(); + dv.ReverseSign(); +} + +doubleAcc VectorAcc::Norm() { + doubleAcc res; + res.t = p.Norm(); + res.d = dot(p,v)/res.t; + res.dd = (dot(p,dv)+dot(v,v)-res.d*res.d)/res.t; + return res; +} + +doubleAcc dot(const VectorAcc& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs.p,rhs.p), + dot(lhs.p,rhs.v)+dot(lhs.v,rhs.p), + dot(lhs.p,rhs.dv)+2*dot(lhs.v,rhs.v)+dot(lhs.dv,rhs.p) + ); +} + +doubleAcc dot(const VectorAcc& lhs,const Vector& rhs) { + return doubleAcc( dot(lhs.p,rhs), + dot(lhs.v,rhs), + dot(lhs.dv,rhs) + ); +} + +doubleAcc dot(const Vector& lhs,const VectorAcc& rhs) { + return doubleAcc( dot(lhs,rhs.p), + dot(lhs,rhs.v), + dot(lhs,rhs.dv) + ); +} + + +bool Equal(const VectorAcc& r1,const VectorAcc& r2,double eps) { + return (Equal(r1.p,r2.p,eps) + && Equal(r1.v,r2.v,eps) + && Equal(r1.dv,r2.dv,eps) + ); +} + +bool Equal(const Vector& r1,const VectorAcc& r2,double eps) { + return (Equal(r1,r2.p,eps) + && Equal(Vector::Zero(),r2.v,eps) + && Equal(Vector::Zero(),r2.dv,eps) + ); +} + +bool Equal(const VectorAcc& r1,const Vector& r2,double eps) { + return (Equal(r1.p,r2,eps) + && Equal(r1.v,Vector::Zero(),eps) + && Equal(r1.dv,Vector::Zero(),eps) + ); +} + +VectorAcc operator / (const VectorAcc& r1,double r2) { + return r1*(1.0/r2); +} + +VectorAcc operator / (const VectorAcc& r2,const doubleAcc& r1) { + return r2*(1.0/r1); +} + + + +/////////////////// RotationAcc ///////////////////////////////////// + +RotationAcc operator* (const RotationAcc& r1,const RotationAcc& r2) { + return RotationAcc( r1.R * r2.R, + r1.w + r1.R*r2.w, + r1.dw + r1.w*(r1.R*r2.w) + r1.R*r2.dw + ); +} + +RotationAcc operator* (const Rotation& r1,const RotationAcc& r2) { + return RotationAcc( r1*r2.R, r1*r2.w, r1*r2.dw); +} + +RotationAcc operator* (const RotationAcc& r1,const Rotation& r2) { + return RotationAcc( r1.R*r2, r1.w, r1.dw ); +} + +RotationAcc& RotationAcc::operator = (const RotationAcc& arg) { + R=arg.R; + w=arg.w; + dw=arg.dw; + return *this; +} +RotationAcc& RotationAcc::operator = (const Rotation& arg) { + R = arg; + w = Vector::Zero(); + dw = Vector::Zero(); + return *this; +} + +RotationAcc RotationAcc::Identity() { + return RotationAcc(Rotation::Identity(),Vector::Zero(),Vector::Zero()); +} + +RotationAcc RotationAcc::Inverse() const { + return RotationAcc(R.Inverse(),-R.Inverse(w),-R.Inverse(dw)); +} + +VectorAcc RotationAcc::Inverse(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg.p); + tmp.v = R.Inverse(arg.v - w * arg.p); + tmp.dv = R.Inverse(arg.dv - dw*arg.p - w*(arg.v+R*tmp.v)); + return tmp; +} + +VectorAcc RotationAcc::Inverse(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R.Inverse(arg); + tmp.v = R.Inverse(-w*arg); + tmp.dv = R.Inverse(-dw*arg - w*(R*tmp.v)); + return tmp; +} + + +VectorAcc RotationAcc::operator*(const VectorAcc& arg) const { + VectorAcc tmp; + tmp.p = R*arg.p; + tmp.dv = R*arg.v; + tmp.v = w*tmp.p + tmp.dv; + tmp.dv = dw*tmp.p + w*(tmp.v + tmp.dv) + R*arg.dv; + return tmp; +} + +VectorAcc operator*(const Rotation& R,const VectorAcc& x) { + return VectorAcc(R*x.p,R*x.v,R*x.dv); +} + +VectorAcc RotationAcc::operator*(const Vector& arg) const { + VectorAcc tmp; + tmp.p = R*arg; + tmp.v = w*tmp.p; + tmp.dv = dw*tmp.p + w*tmp.v; + return tmp; +} + +/* + // = Rotations + // The Rot... static functions give the value of the appropriate rotation matrix back. + // The DoRot... functions apply a rotation R to *this,such that *this = *this * R. + + void RRotation::DoRotX(const RDouble& angle) { + w+=R*Vector(angle.grad,0,0); + R.DoRotX(angle.t); + } +RotationAcc RotationAcc::RotX(const doubleAcc& angle) { + return RotationAcc(Rotation::RotX(angle.t), + Vector(angle.d,0,0), + Vector(angle.dd,0,0) + ); +} + + void RRotation::DoRotY(const RDouble& angle) { + w+=R*Vector(0,angle.grad,0); + R.DoRotY(angle.t); + } +RotationAcc RotationAcc::RotY(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotX(angle.t), + Vector(0,angle.d,0), + Vector(0,angle.dd,0) + ); +} + + void RRotation::DoRotZ(const RDouble& angle) { + w+=R*Vector(0,0,angle.grad); + R.DoRotZ(angle.t); + } +RotationAcc RotationAcc::RotZ(const doubleAcc& angle) { + return RotationAcc( + Rotation::RotZ(angle.t), + Vector(0,0,angle.d), + Vector(0,0,angle.dd) + ); +} + + + RRotation RRotation::Rot(const Vector& rotvec,const RDouble& angle) + // rotvec has arbitrary norm + // rotation around a constant vector ! + { + Vector v = rotvec.Normalize(); + return RRotation(Rotation::Rot2(v,angle.t),v*angle.grad); + } + + RRotation RRotation::Rot2(const Vector& rotvec,const RDouble& angle) + // rotvec is normalized. + { + return RRotation(Rotation::Rot2(rotvec,angle.t),rotvec*angle.grad); + } + +*/ + +bool Equal(const RotationAcc& r1,const RotationAcc& r2,double eps) { + return (Equal(r1.w,r2.w,eps) && Equal(r1.R,r2.R,eps) && Equal(r1.dw,r2.dw,eps) ); +} +bool Equal(const Rotation& r1,const RotationAcc& r2,double eps) { + return (Equal(Vector::Zero(),r2.w,eps) && Equal(r1,r2.R,eps) && + Equal(Vector::Zero(),r2.dw,eps) ); +} +bool Equal(const RotationAcc& r1,const Rotation& r2,double eps) { + return (Equal(r1.w,Vector::Zero(),eps) && Equal(r1.R,r2,eps) && + Equal(r1.dw,Vector::Zero(),eps) ); +} + + +// Methods and operators related to FrameAcc +// They all delegate most of the work to RotationAcc and VectorAcc +FrameAcc& FrameAcc::operator = (const FrameAcc& arg) { + M=arg.M; + p=arg.p; + return *this; +} + +FrameAcc FrameAcc::Identity() { + return FrameAcc(RotationAcc::Identity(),VectorAcc::Zero()); +} + + +FrameAcc operator *(const FrameAcc& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const FrameAcc& lhs,const Frame& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} +FrameAcc operator *(const Frame& lhs,const FrameAcc& rhs) +{ + return FrameAcc(lhs.M*rhs.M,lhs.M*rhs.p+lhs.p); +} + +VectorAcc FrameAcc::operator *(const VectorAcc & arg) const +{ + return M*arg+p; +} +VectorAcc FrameAcc::operator *(const Vector & arg) const +{ + return M*arg+p; +} + +VectorAcc FrameAcc::Inverse(const VectorAcc& arg) const +{ + return M.Inverse(arg-p); +} + +VectorAcc FrameAcc::Inverse(const Vector& arg) const +{ + return M.Inverse(arg-p); +} + +FrameAcc FrameAcc::Inverse() const +{ + return FrameAcc(M.Inverse(),-M.Inverse(p)); +} + +FrameAcc& FrameAcc::operator =(const Frame & arg) +{ + M = arg.M; + p = arg.p; + return *this; +} + +bool Equal(const FrameAcc& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const Frame& r1,const FrameAcc& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} +bool Equal(const FrameAcc& r1,const Frame& r2,double eps) { + return (Equal(r1.M,r2.M,eps) && Equal(r1.p,r2.p,eps)); +} + + +Frame FrameAcc::GetFrame() const { + return Frame(M.R,p.p); +} + + +Twist FrameAcc::GetTwist() const { + return Twist(p.v,M.w); +} + + +Twist FrameAcc::GetAccTwist() const { + return Twist(p.dv,M.dw); +} + + + + + + + + + + + + + + + + + +TwistAcc TwistAcc::Zero() +{ + return TwistAcc(VectorAcc::Zero(),VectorAcc::Zero()); +} + + +void TwistAcc::ReverseSign() +{ + vel.ReverseSign(); + rot.ReverseSign(); +} + +TwistAcc TwistAcc::RefPoint(const VectorAcc& v_base_AB) + // Changes the reference point of the TwistAcc. + // The RVector v_base_AB is expressed in the same base as the TwistAcc + // The RVector v_base_AB is a RVector from the old point to + // the new point. + // Complexity : 6M+6A +{ + return TwistAcc(this->vel+this->rot*v_base_AB,this->rot); +} + +TwistAcc& TwistAcc::operator-=(const TwistAcc& arg) +{ + vel-=arg.vel; + rot -=arg.rot; + return *this; +} + +TwistAcc& TwistAcc::operator+=(const TwistAcc& arg) +{ + vel+=arg.vel; + rot +=arg.rot; + return *this; +} + + +TwistAcc operator*(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(double lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,double rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + +TwistAcc operator*(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel*rhs,lhs.rot*rhs); +} + +TwistAcc operator*(const doubleAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs*rhs.vel,lhs*rhs.rot); +} + +TwistAcc operator/(const TwistAcc& lhs,const doubleAcc& rhs) +{ + return TwistAcc(lhs.vel/rhs,lhs.rot/rhs); +} + + + +// addition of TwistAcc's +TwistAcc operator+(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel+rhs.vel,lhs.rot+rhs.rot); +} + +TwistAcc operator-(const TwistAcc& lhs,const TwistAcc& rhs) +{ + return TwistAcc(lhs.vel-rhs.vel,lhs.rot-rhs.rot); +} + +// unary - +TwistAcc operator-(const TwistAcc& arg) +{ + return TwistAcc(-arg.vel,-arg.rot); +} + + + + + +TwistAcc RotationAcc::Inverse(const TwistAcc& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const TwistAcc& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + +TwistAcc RotationAcc::Inverse(const Twist& arg) const +{ + return TwistAcc(Inverse(arg.vel),Inverse(arg.rot)); +} + +TwistAcc RotationAcc::operator * (const Twist& arg) const +{ + return TwistAcc((*this)*arg.vel,(*this)*arg.rot); +} + + +TwistAcc FrameAcc::operator * (const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::operator * (const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M*arg.rot; + tmp.vel = M*arg.vel+p*tmp.rot; + return tmp; +} + +TwistAcc FrameAcc::Inverse(const TwistAcc& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +TwistAcc FrameAcc::Inverse(const Twist& arg) const +{ + TwistAcc tmp; + tmp.rot = M.Inverse(arg.rot); + tmp.vel = M.Inverse(arg.vel-p*arg.rot); + return tmp; +} + +Twist TwistAcc::GetTwist() const { + return Twist(vel.p,rot.p); +} + +Twist TwistAcc::GetTwistDot() const { + return Twist(vel.v,rot.v); +} + +bool Equal(const TwistAcc& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const Twist& a,const TwistAcc& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} +bool Equal(const TwistAcc& a,const Twist& b,double eps) { + return (Equal(a.rot,b.rot,eps)&& + Equal(a.vel,b.vel,eps) ); +} + |