383 lines
12 KiB
C++
383 lines
12 KiB
C++
/*****************************************************************************
|
|
* \file
|
|
* This file contains the definition of classes for a
|
|
* Rall Algebra of (subset of) the classes defined in frames,
|
|
* i.e. classes that contain a pair (value,derivative) and define operations on that pair
|
|
* this classes are usefull for automatic differentiation ( <-> symbolic diff , <-> numeric diff)
|
|
* Defines VectorVel, RotationVel, FrameVel. Look at Frames.h for details on how to work
|
|
* with Frame objects.
|
|
* \author
|
|
* Erwin Aertbelien, Div. PMA, Dep. of Mech. Eng., K.U.Leuven
|
|
*
|
|
* \version
|
|
* ORO_Geometry V0.2
|
|
*
|
|
* \par History
|
|
* - $log$
|
|
*
|
|
* \par Release
|
|
* $Id: framevel.hpp 19905 2009-04-23 13:29:54Z ben2610 $
|
|
* $Name: $
|
|
****************************************************************************/
|
|
|
|
#ifndef KDL_FRAMEVEL_H
|
|
#define KDL_FRAMEVEL_H
|
|
|
|
#include "utilities/utility.h"
|
|
#include "utilities/rall1d.h"
|
|
#include "utilities/traits.h"
|
|
|
|
#include "frames.hpp"
|
|
|
|
|
|
|
|
namespace KDL {
|
|
|
|
typedef Rall1d<double> doubleVel;
|
|
|
|
IMETHOD doubleVel diff(const doubleVel& a,const doubleVel& b,double dt=1.0) {
|
|
return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
|
|
}
|
|
|
|
IMETHOD doubleVel addDelta(const doubleVel& a,const doubleVel&da,double dt=1.0) {
|
|
return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
|
|
}
|
|
|
|
IMETHOD void random(doubleVel& F) {
|
|
random(F.t);
|
|
random(F.grad);
|
|
}
|
|
IMETHOD void posrandom(doubleVel& F) {
|
|
posrandom(F.t);
|
|
posrandom(F.grad);
|
|
}
|
|
|
|
}
|
|
|
|
template <>
|
|
struct Traits<KDL::doubleVel> {
|
|
typedef double valueType;
|
|
typedef KDL::doubleVel derivType;
|
|
};
|
|
|
|
namespace KDL {
|
|
|
|
class TwistVel;
|
|
class VectorVel;
|
|
class FrameVel;
|
|
class RotationVel;
|
|
|
|
class VectorVel
|
|
// = TITLE
|
|
// An VectorVel is a Vector and its first derivative
|
|
// = CLASS TYPE
|
|
// Concrete
|
|
{
|
|
public:
|
|
Vector p; // position vector
|
|
Vector v; // velocity vector
|
|
public:
|
|
VectorVel():p(),v(){}
|
|
VectorVel(const Vector& _p,const Vector& _v):p(_p),v(_v) {}
|
|
explicit VectorVel(const Vector& _p):p(_p),v(Vector::Zero()) {}
|
|
|
|
Vector value() const { return p;}
|
|
Vector deriv() const { return v;}
|
|
|
|
IMETHOD VectorVel& operator = (const VectorVel& arg);
|
|
IMETHOD VectorVel& operator = (const Vector& arg);
|
|
IMETHOD VectorVel& operator += (const VectorVel& arg);
|
|
IMETHOD VectorVel& operator -= (const VectorVel& arg);
|
|
IMETHOD static VectorVel Zero();
|
|
IMETHOD void ReverseSign();
|
|
IMETHOD doubleVel Norm() const;
|
|
IMETHOD friend VectorVel operator + (const VectorVel& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator - (const VectorVel& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator + (const Vector& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator - (const Vector& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator + (const VectorVel& r1,const Vector& r2);
|
|
IMETHOD friend VectorVel operator - (const VectorVel& r1,const Vector& r2);
|
|
IMETHOD friend VectorVel operator * (const VectorVel& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator * (const VectorVel& r1,const Vector& r2);
|
|
IMETHOD friend VectorVel operator * (const Vector& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator * (const VectorVel& r1,double r2);
|
|
IMETHOD friend VectorVel operator * (double r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator * (const doubleVel& r1,const VectorVel& r2);
|
|
IMETHOD friend VectorVel operator * (const VectorVel& r2,const doubleVel& r1);
|
|
IMETHOD friend VectorVel operator*(const Rotation& R,const VectorVel& x);
|
|
|
|
IMETHOD friend VectorVel operator / (const VectorVel& r1,double r2);
|
|
IMETHOD friend VectorVel operator / (const VectorVel& r2,const doubleVel& r1);
|
|
IMETHOD friend void SetToZero(VectorVel& v);
|
|
|
|
|
|
IMETHOD friend bool Equal(const VectorVel& r1,const VectorVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const Vector& r1,const VectorVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const VectorVel& r1,const Vector& r2,double eps=epsilon);
|
|
IMETHOD friend VectorVel operator - (const VectorVel& r);
|
|
IMETHOD friend doubleVel dot(const VectorVel& lhs,const VectorVel& rhs);
|
|
IMETHOD friend doubleVel dot(const VectorVel& lhs,const Vector& rhs);
|
|
IMETHOD friend doubleVel dot(const Vector& lhs,const VectorVel& rhs);
|
|
};
|
|
|
|
|
|
|
|
class RotationVel
|
|
// = TITLE
|
|
// An RotationVel is a Rotation and its first derivative, a rotation vector
|
|
// = CLASS TYPE
|
|
// Concrete
|
|
{
|
|
public:
|
|
Rotation R; // Rotation matrix
|
|
Vector w; // rotation vector
|
|
public:
|
|
RotationVel():R(),w() {}
|
|
explicit RotationVel(const Rotation& R_):R(R_),w(Vector::Zero()){}
|
|
RotationVel(const Rotation& R_,const Vector& _w):R(R_),w(_w){}
|
|
|
|
|
|
Rotation value() const { return R;}
|
|
Vector deriv() const { return w;}
|
|
|
|
|
|
IMETHOD RotationVel& operator = (const RotationVel& arg);
|
|
IMETHOD RotationVel& operator = (const Rotation& arg);
|
|
IMETHOD VectorVel UnitX() const;
|
|
IMETHOD VectorVel UnitY() const;
|
|
IMETHOD VectorVel UnitZ() const;
|
|
IMETHOD static RotationVel Identity();
|
|
IMETHOD RotationVel Inverse() const;
|
|
IMETHOD VectorVel Inverse(const VectorVel& arg) const;
|
|
IMETHOD VectorVel Inverse(const Vector& arg) const;
|
|
IMETHOD VectorVel operator*(const VectorVel& arg) const;
|
|
IMETHOD VectorVel operator*(const Vector& arg) const;
|
|
IMETHOD void DoRotX(const doubleVel& angle);
|
|
IMETHOD void DoRotY(const doubleVel& angle);
|
|
IMETHOD void DoRotZ(const doubleVel& angle);
|
|
IMETHOD static RotationVel RotX(const doubleVel& angle);
|
|
IMETHOD static RotationVel RotY(const doubleVel& angle);
|
|
IMETHOD static RotationVel RotZ(const doubleVel& angle);
|
|
IMETHOD static RotationVel Rot(const Vector& rotvec,const doubleVel& angle);
|
|
// rotvec has arbitrary norm
|
|
// rotation around a constant vector !
|
|
IMETHOD static RotationVel Rot2(const Vector& rotvec,const doubleVel& angle);
|
|
// rotvec is normalized.
|
|
// rotation around a constant vector !
|
|
IMETHOD friend RotationVel operator* (const RotationVel& r1,const RotationVel& r2);
|
|
IMETHOD friend RotationVel operator* (const Rotation& r1,const RotationVel& r2);
|
|
IMETHOD friend RotationVel operator* (const RotationVel& r1,const Rotation& r2);
|
|
IMETHOD friend bool Equal(const RotationVel& r1,const RotationVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const Rotation& r1,const RotationVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const RotationVel& r1,const Rotation& r2,double eps=epsilon);
|
|
|
|
IMETHOD TwistVel Inverse(const TwistVel& arg) const;
|
|
IMETHOD TwistVel Inverse(const Twist& arg) const;
|
|
IMETHOD TwistVel operator * (const TwistVel& arg) const;
|
|
IMETHOD TwistVel operator * (const Twist& arg) const;
|
|
};
|
|
|
|
|
|
|
|
|
|
class FrameVel
|
|
// = TITLE
|
|
// An FrameVel is a Frame and its first derivative, a Twist vector
|
|
// = CLASS TYPE
|
|
// Concrete
|
|
// = CAVEATS
|
|
//
|
|
{
|
|
public:
|
|
RotationVel M;
|
|
VectorVel p;
|
|
public:
|
|
FrameVel(){}
|
|
|
|
explicit FrameVel(const Frame& T_):
|
|
M(T_.M),p(T_.p) {}
|
|
|
|
FrameVel(const Frame& T_,const Twist& _t):
|
|
M(T_.M,_t.rot),p(T_.p,_t.vel) {}
|
|
|
|
FrameVel(const RotationVel& _M,const VectorVel& _p):
|
|
M(_M),p(_p) {}
|
|
|
|
|
|
Frame value() const { return Frame(M.value(),p.value());}
|
|
Twist deriv() const { return Twist(p.deriv(),M.deriv());}
|
|
|
|
|
|
IMETHOD FrameVel& operator = (const Frame& arg);
|
|
IMETHOD FrameVel& operator = (const FrameVel& arg);
|
|
IMETHOD static FrameVel Identity();
|
|
IMETHOD FrameVel Inverse() const;
|
|
IMETHOD VectorVel Inverse(const VectorVel& arg) const;
|
|
IMETHOD VectorVel operator*(const VectorVel& arg) const;
|
|
IMETHOD VectorVel operator*(const Vector& arg) const;
|
|
IMETHOD VectorVel Inverse(const Vector& arg) const;
|
|
IMETHOD Frame GetFrame() const;
|
|
IMETHOD Twist GetTwist() const;
|
|
IMETHOD friend FrameVel operator * (const FrameVel& f1,const FrameVel& f2);
|
|
IMETHOD friend FrameVel operator * (const Frame& f1,const FrameVel& f2);
|
|
IMETHOD friend FrameVel operator * (const FrameVel& f1,const Frame& f2);
|
|
IMETHOD friend bool Equal(const FrameVel& r1,const FrameVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const Frame& r1,const FrameVel& r2,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const FrameVel& r1,const Frame& r2,double eps=epsilon);
|
|
|
|
IMETHOD TwistVel Inverse(const TwistVel& arg) const;
|
|
IMETHOD TwistVel Inverse(const Twist& arg) const;
|
|
IMETHOD TwistVel operator * (const TwistVel& arg) const;
|
|
IMETHOD TwistVel operator * (const Twist& arg) const;
|
|
};
|
|
|
|
|
|
|
|
|
|
|
|
//very similar to Wrench class.
|
|
class TwistVel
|
|
// = TITLE
|
|
// This class represents a TwistVel. This is a velocity and rotational velocity together
|
|
{
|
|
public:
|
|
VectorVel vel;
|
|
VectorVel rot;
|
|
public:
|
|
|
|
// = Constructors
|
|
TwistVel():vel(),rot() {};
|
|
TwistVel(const VectorVel& _vel,const VectorVel& _rot):vel(_vel),rot(_rot) {};
|
|
TwistVel(const Twist& p,const Twist& v):vel(p.vel, v.vel), rot( p.rot, v.rot) {};
|
|
TwistVel(const Twist& p):vel(p.vel), rot( p.rot) {};
|
|
|
|
Twist value() const {
|
|
return Twist(vel.value(),rot.value());
|
|
}
|
|
Twist deriv() const {
|
|
return Twist(vel.deriv(),rot.deriv());
|
|
}
|
|
// = Operators
|
|
IMETHOD TwistVel& operator-=(const TwistVel& arg);
|
|
IMETHOD TwistVel& operator+=(const TwistVel& arg);
|
|
|
|
// = External operators
|
|
IMETHOD friend TwistVel operator*(const TwistVel& lhs,double rhs);
|
|
IMETHOD friend TwistVel operator*(double lhs,const TwistVel& rhs);
|
|
IMETHOD friend TwistVel operator/(const TwistVel& lhs,double rhs);
|
|
|
|
IMETHOD friend TwistVel operator*(const TwistVel& lhs,const doubleVel& rhs);
|
|
IMETHOD friend TwistVel operator*(const doubleVel& lhs,const TwistVel& rhs);
|
|
IMETHOD friend TwistVel operator/(const TwistVel& lhs,const doubleVel& rhs);
|
|
|
|
IMETHOD friend TwistVel operator+(const TwistVel& lhs,const TwistVel& rhs);
|
|
IMETHOD friend TwistVel operator-(const TwistVel& lhs,const TwistVel& rhs);
|
|
IMETHOD friend TwistVel operator-(const TwistVel& arg);
|
|
IMETHOD friend void SetToZero(TwistVel& v);
|
|
|
|
|
|
// = Zero
|
|
static IMETHOD TwistVel Zero();
|
|
|
|
// = Reverse Sign
|
|
IMETHOD void ReverseSign();
|
|
|
|
// = Change Reference point
|
|
IMETHOD TwistVel RefPoint(const VectorVel& v_base_AB);
|
|
// Changes the reference point of the TwistVel.
|
|
// The VectorVel v_base_AB is expressed in the same base as the TwistVel
|
|
// The VectorVel v_base_AB is a VectorVel from the old point to
|
|
// the new point.
|
|
// Complexity : 6M+6A
|
|
|
|
// = Equality operators
|
|
// do not use operator == because the definition of Equal(.,.) is slightly
|
|
// different. It compares whether the 2 arguments are equal in an eps-interval
|
|
IMETHOD friend bool Equal(const TwistVel& a,const TwistVel& b,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const Twist& a,const TwistVel& b,double eps=epsilon);
|
|
IMETHOD friend bool Equal(const TwistVel& a,const Twist& b,double eps=epsilon);
|
|
|
|
// = Conversion to other entities
|
|
IMETHOD Twist GetTwist() const;
|
|
IMETHOD Twist GetTwistDot() const;
|
|
// = Friends
|
|
friend class RotationVel;
|
|
friend class FrameVel;
|
|
|
|
};
|
|
|
|
IMETHOD VectorVel diff(const VectorVel& a,const VectorVel& b,double dt=1.0) {
|
|
return VectorVel(diff(a.p,b.p,dt),diff(a.v,b.v,dt));
|
|
}
|
|
|
|
IMETHOD VectorVel addDelta(const VectorVel& a,const VectorVel&da,double dt=1.0) {
|
|
return VectorVel(addDelta(a.p,da.p,dt),addDelta(a.v,da.v,dt));
|
|
}
|
|
IMETHOD VectorVel diff(const RotationVel& a,const RotationVel& b,double dt = 1.0) {
|
|
return VectorVel(diff(a.R,b.R,dt),diff(a.w,b.w,dt));
|
|
}
|
|
|
|
IMETHOD RotationVel addDelta(const RotationVel& a,const VectorVel&da,double dt=1.0) {
|
|
return RotationVel(addDelta(a.R,da.p,dt),addDelta(a.w,da.v,dt));
|
|
}
|
|
|
|
IMETHOD TwistVel diff(const FrameVel& a,const FrameVel& b,double dt=1.0) {
|
|
return TwistVel(diff(a.M,b.M,dt),diff(a.p,b.p,dt));
|
|
}
|
|
|
|
IMETHOD FrameVel addDelta(const FrameVel& a,const TwistVel& da,double dt=1.0) {
|
|
return FrameVel(
|
|
addDelta(a.M,da.rot,dt),
|
|
addDelta(a.p,da.vel,dt)
|
|
);
|
|
}
|
|
|
|
IMETHOD void random(VectorVel& a) {
|
|
random(a.p);
|
|
random(a.v);
|
|
}
|
|
IMETHOD void random(TwistVel& a) {
|
|
random(a.vel);
|
|
random(a.rot);
|
|
}
|
|
|
|
IMETHOD void random(RotationVel& R) {
|
|
random(R.R);
|
|
random(R.w);
|
|
}
|
|
|
|
IMETHOD void random(FrameVel& F) {
|
|
random(F.M);
|
|
random(F.p);
|
|
}
|
|
IMETHOD void posrandom(VectorVel& a) {
|
|
posrandom(a.p);
|
|
posrandom(a.v);
|
|
}
|
|
IMETHOD void posrandom(TwistVel& a) {
|
|
posrandom(a.vel);
|
|
posrandom(a.rot);
|
|
}
|
|
|
|
IMETHOD void posrandom(RotationVel& R) {
|
|
posrandom(R.R);
|
|
posrandom(R.w);
|
|
}
|
|
|
|
IMETHOD void posrandom(FrameVel& F) {
|
|
posrandom(F.M);
|
|
posrandom(F.p);
|
|
}
|
|
|
|
#ifdef KDL_INLINE
|
|
#include "framevel.inl"
|
|
#endif
|
|
|
|
} // namespace
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|