23 #ifndef KDL_FRAMEVEL_H
24 #define KDL_FRAMEVEL_H
26 #include "utilities/utility.h"
27 #include "utilities/rall1d.h"
28 #include "utilities/traits.h"
39 return doubleVel((b.t-a.t)/dt,(b.grad-a.grad)/dt);
43 return doubleVel(a.t+da.t*dt,a.grad+da.grad*dt);
230 M(_T.
M,_t.rot),
p(_T.
p,_t.vel) {}
Definition: framevel.hpp:219
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition: framevel.hpp:232
IMETHOD friend bool operator==(const Frame &a, const FrameVel &b)
IMETHOD friend bool operator!=(const FrameVel &a, const Frame &b)
IMETHOD friend bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
Frame value() const
Definition: framevel.hpp:236
IMETHOD friend bool operator==(const FrameVel &a, const FrameVel &b)
IMETHOD FrameVel & operator=(const Frame &arg)
Definition: framevel.inl:70
FrameVel(const Frame &_T)
Definition: framevel.hpp:226
Twist deriv() const
Definition: framevel.hpp:237
FrameVel(const Frame &_T, const Twist &_t)
Definition: framevel.hpp:229
IMETHOD Twist GetTwist() const
Definition: framevel.inl:124
IMETHOD friend bool Equal(const Frame &r1, const FrameVel &r2, double eps)
IMETHOD friend bool Equal(const FrameVel &r1, const Frame &r2, double eps)
VectorVel p
Definition: framevel.hpp:222
FrameVel()
Definition: framevel.hpp:224
IMETHOD Frame GetFrame() const
Definition: framevel.inl:120
IMETHOD friend bool operator!=(const FrameVel &a, const FrameVel &b)
IMETHOD friend bool operator==(const FrameVel &a, const Frame &b)
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:46
IMETHOD FrameVel Inverse() const
Definition: framevel.inl:65
RotationVel M
Definition: framevel.hpp:221
static IMETHOD FrameVel Identity()
Definition: framevel.inl:28
IMETHOD friend bool operator!=(const Frame &a, const FrameVel &b)
Definition: frames.hpp:570
Definition: framevel.hpp:152
IMETHOD friend bool operator!=(const Rotation &r1, const RotationVel &r2)
IMETHOD VectorVel UnitY() const
Definition: framevel.inl:156
static IMETHOD RotationVel RotZ(const doubleVel &angle)
Definition: framevel.inl:224
RotationVel(const Rotation &_R, const Vector &_w)
Definition: framevel.hpp:159
IMETHOD RotationVel & operator=(const RotationVel &arg)
Definition: framevel.inl:141
IMETHOD RotationVel Inverse() const
Definition: framevel.inl:170
IMETHOD friend bool operator==(const RotationVel &r1, const RotationVel &r2)
Vector deriv() const
Definition: framevel.hpp:163
IMETHOD VectorVel UnitX() const
Definition: framevel.inl:152
static IMETHOD RotationVel Identity()
Definition: framevel.inl:166
static IMETHOD RotationVel RotX(const doubleVel &angle)
Definition: framevel.inl:208
IMETHOD friend bool Equal(const RotationVel &r1, const RotationVel &r2, double eps)
static IMETHOD RotationVel Rot(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:229
IMETHOD friend bool operator!=(const RotationVel &r1, const Rotation &r2)
IMETHOD friend bool Equal(const Rotation &r1, const RotationVel &r2, double eps)
IMETHOD VectorVel UnitZ() const
Definition: framevel.inl:160
IMETHOD friend bool operator!=(const RotationVel &r1, const RotationVel &r2)
RotationVel()
Definition: framevel.hpp:157
IMETHOD friend bool Equal(const RotationVel &r1, const Rotation &r2, double eps)
IMETHOD void DoRotX(const doubleVel &angle)
Definition: framevel.inl:204
IMETHOD friend bool operator==(const RotationVel &r1, const Rotation &r2)
IMETHOD void DoRotY(const doubleVel &angle)
Definition: framevel.inl:212
Vector w
Definition: framevel.hpp:155
static IMETHOD RotationVel Rot2(const Vector &rotvec, const doubleVel &angle)
Definition: framevel.inl:238
Rotation R
Definition: framevel.hpp:154
IMETHOD void DoRotZ(const doubleVel &angle)
Definition: framevel.inl:220
RotationVel(const Rotation &_R)
Definition: framevel.hpp:158
static IMETHOD RotationVel RotY(const doubleVel &angle)
Definition: framevel.inl:216
Rotation value() const
Definition: framevel.hpp:162
IMETHOD VectorVel operator*(const VectorVel &arg) const
Definition: framevel.inl:189
IMETHOD friend bool operator==(const Rotation &r1, const RotationVel &r2)
represents rotations in 3 dimensional space.
Definition: frames.hpp:302
Definition: framevel.hpp:278
IMETHOD friend bool operator==(const TwistVel &a, const TwistVel &b)
IMETHOD friend bool operator==(const TwistVel &a, const Twist &b)
IMETHOD friend bool Equal(const Twist &a, const TwistVel &b, double eps)
IMETHOD friend TwistVel operator/(const TwistVel &lhs, double rhs)
IMETHOD friend bool operator!=(const Twist &a, const TwistVel &b)
Twist deriv() const
Definition: framevel.hpp:293
static IMETHOD TwistVel Zero()
Definition: framevel.inl:505
IMETHOD friend void SetToZero(TwistVel &v)
IMETHOD friend bool operator!=(const TwistVel &a, const Twist &b)
TwistVel(const Twist &p, const Twist &v)
Definition: framevel.hpp:287
IMETHOD TwistVel & operator+=(const TwistVel &arg)
Definition: framevel.inl:534
IMETHOD friend TwistVel operator-(const TwistVel &lhs, const TwistVel &rhs)
IMETHOD friend TwistVel operator*(double lhs, const TwistVel &rhs)
Twist value() const
Definition: framevel.hpp:290
IMETHOD friend TwistVel operator*(const doubleVel &lhs, const TwistVel &rhs)
IMETHOD Twist GetTwist() const
Definition: framevel.inl:655
IMETHOD void ReverseSign()
Definition: framevel.inl:511
VectorVel vel
Definition: framevel.hpp:280
TwistVel(const Twist &p)
Definition: framevel.hpp:288
IMETHOD Twist GetTwistDot() const
Definition: framevel.inl:659
IMETHOD friend bool operator==(const Twist &a, const TwistVel &b)
IMETHOD friend TwistVel operator+(const TwistVel &lhs, const TwistVel &rhs)
IMETHOD TwistVel & operator-=(const TwistVel &arg)
Definition: framevel.inl:527
IMETHOD friend TwistVel operator-(const TwistVel &arg)
IMETHOD friend TwistVel operator*(const TwistVel &lhs, double rhs)
IMETHOD friend bool Equal(const TwistVel &a, const Twist &b, double eps)
IMETHOD friend bool operator!=(const TwistVel &a, const TwistVel &b)
IMETHOD TwistVel RefPoint(const VectorVel &v_base_AB)
Definition: framevel.inl:517
VectorVel rot
Definition: framevel.hpp:281
IMETHOD friend TwistVel operator*(const TwistVel &lhs, const doubleVel &rhs)
IMETHOD friend TwistVel operator/(const TwistVel &lhs, const doubleVel &rhs)
TwistVel()
Definition: framevel.hpp:285
IMETHOD friend bool Equal(const TwistVel &a, const TwistVel &b, double eps)
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition: framevel.hpp:286
represents both translational and rotational velocities.
Definition: frames.hpp:720
Definition: framevel.hpp:89
static IMETHOD VectorVel Zero()
Definition: framevel.inl:346
IMETHOD friend doubleVel dot(const VectorVel &lhs, const Vector &rhs)
VectorVel(const Vector &_p, const Vector &_v)
Definition: framevel.hpp:95
IMETHOD friend VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend void SetToZero(VectorVel &v)
Vector value() const
Definition: framevel.hpp:98
IMETHOD friend bool Equal(const Vector &r1, const VectorVel &r2, double eps)
Vector p
Definition: framevel.hpp:91
IMETHOD friend VectorVel operator*(const Rotation &R, const VectorVel &x)
Vector v
Definition: framevel.hpp:92
Vector deriv() const
Definition: framevel.hpp:99
IMETHOD friend VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
VectorVel(const Vector &_p)
Definition: framevel.hpp:96
IMETHOD friend doubleVel dot(const Vector &lhs, const VectorVel &rhs)
IMETHOD friend VectorVel operator/(const VectorVel &r1, double r2)
IMETHOD VectorVel & operator=(const VectorVel &arg)
Definition: framevel.inl:325
IMETHOD friend bool Equal(const VectorVel &r1, const Vector &r2, double eps)
IMETHOD friend bool operator!=(const Vector &r1, const VectorVel &r2)
IMETHOD friend bool operator==(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend bool operator!=(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
IMETHOD friend bool operator==(const VectorVel &r1, const Vector &r2)
IMETHOD doubleVel Norm(double eps=epsilon) const
Definition: framevel.inl:353
IMETHOD friend bool operator==(const Vector &r1, const VectorVel &r2)
IMETHOD friend VectorVel operator*(const VectorVel &r1, const VectorVel &r2)
IMETHOD friend bool Equal(const VectorVel &r1, const VectorVel &r2, double eps)
IMETHOD VectorVel & operator-=(const VectorVel &arg)
Definition: framevel.inl:340
VectorVel()
Definition: framevel.hpp:94
IMETHOD void ReverseSign()
Definition: framevel.inl:349
IMETHOD friend bool operator!=(const VectorVel &r1, const Vector &r2)
IMETHOD VectorVel & operator+=(const VectorVel &arg)
Definition: framevel.inl:335
A concrete implementation of a 3 dimensional vector class.
Definition: frames.hpp:161
Definition: articulatedbodyinertia.cpp:28
Rall1d< double > doubleVel
Definition: framevel.hpp:36
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
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)
double valueType
Definition: framevel.hpp:59
KDL::doubleVel derivType
Definition: framevel.hpp:60