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);
58struct Traits<
KDL::doubleVel> {
230 M(_T.
M,_t.rot),
p(_T.
p,_t.vel) {}
Definition framevel.hpp:219
IMETHOD friend FrameVel operator*(const FrameVel &f1, const FrameVel &f2)
Definition framevel.hpp:34
FrameVel(const RotationVel &_M, const VectorVel &_p)
Definition framevel.hpp:232
Frame value() const
Definition framevel.hpp:236
IMETHOD friend bool operator==(const FrameVel &a, const FrameVel &b)
Definition framevel.hpp:85
IMETHOD FrameVel & operator=(const Frame &arg)
Definition framevel.hpp:71
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.hpp:125
VectorVel p
Definition framevel.hpp:222
FrameVel()
Definition framevel.hpp:224
IMETHOD Frame GetFrame() const
Definition framevel.hpp:121
IMETHOD friend bool operator!=(const FrameVel &a, const FrameVel &b)
Definition framevel.hpp:93
IMETHOD friend bool Equal(const FrameVel &r1, const FrameVel &r2, double eps)
Definition framevel.hpp:76
IMETHOD FrameVel Inverse() const
Definition framevel.hpp:66
RotationVel M
Definition framevel.hpp:221
static IMETHOD FrameVel Identity()
Definition framevel.hpp:29
Definition frames.hpp:570
Definition framevel.hpp:152
IMETHOD VectorVel UnitY() const
Definition framevel.hpp:157
static IMETHOD RotationVel RotZ(const doubleVel &angle)
Definition framevel.hpp:225
RotationVel(const Rotation &_R, const Vector &_w)
Definition framevel.hpp:159
IMETHOD RotationVel & operator=(const RotationVel &arg)
Definition framevel.hpp:142
IMETHOD RotationVel Inverse() const
Definition framevel.hpp:171
IMETHOD friend bool operator==(const RotationVel &r1, const RotationVel &r2)
Definition framevel.hpp:414
Vector deriv() const
Definition framevel.hpp:163
IMETHOD VectorVel UnitX() const
Definition framevel.hpp:153
static IMETHOD RotationVel Identity()
Definition framevel.hpp:167
static IMETHOD RotationVel RotX(const doubleVel &angle)
Definition framevel.hpp:209
static IMETHOD RotationVel Rot(const Vector &rotvec, const doubleVel &angle)
Definition framevel.hpp:230
IMETHOD VectorVel UnitZ() const
Definition framevel.hpp:161
IMETHOD friend bool operator!=(const RotationVel &r1, const RotationVel &r2)
Definition framevel.hpp:422
RotationVel()
Definition framevel.hpp:157
IMETHOD void DoRotX(const doubleVel &angle)
Definition framevel.hpp:205
IMETHOD void DoRotY(const doubleVel &angle)
Definition framevel.hpp:213
IMETHOD friend bool Equal(const RotationVel &r1, const RotationVel &r2, double eps)
Definition framevel.hpp:405
Vector w
Definition framevel.hpp:155
static IMETHOD RotationVel Rot2(const Vector &rotvec, const doubleVel &angle)
Definition framevel.hpp:239
Rotation R
Definition framevel.hpp:154
IMETHOD void DoRotZ(const doubleVel &angle)
Definition framevel.hpp:221
IMETHOD friend RotationVel operator*(const RotationVel &r1, const RotationVel &r2)
Definition framevel.hpp:130
RotationVel(const Rotation &_R)
Definition framevel.hpp:158
static IMETHOD RotationVel RotY(const doubleVel &angle)
Definition framevel.hpp:217
Rotation value() const
Definition framevel.hpp:162
represents rotations in 3 dimensional space.
Definition frames.hpp:302
Definition framevel.hpp:278
IMETHOD friend bool operator==(const TwistVel &a, const TwistVel &b)
Definition framevel.hpp:461
IMETHOD friend TwistVel operator/(const TwistVel &lhs, double rhs)
Definition framevel.hpp:553
Twist deriv() const
Definition framevel.hpp:293
static IMETHOD TwistVel Zero()
Definition framevel.hpp:506
IMETHOD friend void SetToZero(TwistVel &v)
Definition framevel.hpp:593
TwistVel(const Twist &p, const Twist &v)
Definition framevel.hpp:287
IMETHOD TwistVel & operator+=(const TwistVel &arg)
Definition framevel.hpp:535
IMETHOD friend TwistVel operator-(const TwistVel &lhs, const TwistVel &rhs)
Definition framevel.hpp:582
Twist value() const
Definition framevel.hpp:290
IMETHOD Twist GetTwist() const
Definition framevel.hpp:656
IMETHOD void ReverseSign()
Definition framevel.hpp:512
VectorVel vel
Definition framevel.hpp:280
TwistVel(const Twist &p)
Definition framevel.hpp:288
IMETHOD Twist GetTwistDot() const
Definition framevel.hpp:660
IMETHOD friend TwistVel operator+(const TwistVel &lhs, const TwistVel &rhs)
Definition framevel.hpp:577
IMETHOD TwistVel & operator-=(const TwistVel &arg)
Definition framevel.hpp:528
IMETHOD friend TwistVel operator*(const TwistVel &lhs, double rhs)
Definition framevel.hpp:543
IMETHOD friend bool operator!=(const TwistVel &a, const TwistVel &b)
Definition framevel.hpp:469
IMETHOD TwistVel RefPoint(const VectorVel &v_base_AB)
Definition framevel.hpp:518
VectorVel rot
Definition framevel.hpp:281
TwistVel()
Definition framevel.hpp:285
TwistVel(const VectorVel &_vel, const VectorVel &_rot)
Definition framevel.hpp:286
IMETHOD friend bool Equal(const TwistVel &a, const TwistVel &b, double eps)
Definition framevel.hpp:449
represents both translational and rotational velocities.
Definition frames.hpp:720
Definition framevel.hpp:89
static IMETHOD VectorVel Zero()
Definition framevel.hpp:347
VectorVel(const Vector &_p, const Vector &_v)
Definition framevel.hpp:95
IMETHOD friend VectorVel operator+(const VectorVel &r1, const VectorVel &r2)
Definition framevel.hpp:246
IMETHOD friend void SetToZero(VectorVel &v)
Definition framevel.hpp:275
Vector value() const
Definition framevel.hpp:98
Vector p
Definition framevel.hpp:91
Vector v
Definition framevel.hpp:92
Vector deriv() const
Definition framevel.hpp:99
IMETHOD friend VectorVel operator-(const VectorVel &r1, const VectorVel &r2)
Definition framevel.hpp:250
IMETHOD friend VectorVel operator/(const VectorVel &r1, double r2)
Definition framevel.hpp:314
IMETHOD VectorVel & operator=(const VectorVel &arg)
Definition framevel.hpp:326
IMETHOD friend bool operator==(const VectorVel &r1, const VectorVel &r2)
Definition framevel.hpp:370
IMETHOD friend bool operator!=(const VectorVel &r1, const VectorVel &r2)
Definition framevel.hpp:378
IMETHOD friend doubleVel dot(const VectorVel &lhs, const VectorVel &rhs)
Definition framevel.hpp:496
IMETHOD doubleVel Norm(double eps=epsilon) const
Definition framevel.hpp:354
IMETHOD friend VectorVel operator*(const VectorVel &r1, const VectorVel &r2)
Definition framevel.hpp:281
IMETHOD VectorVel & operator-=(const VectorVel &arg)
Definition framevel.hpp:341
VectorVel()
Definition framevel.hpp:94
IMETHOD friend bool Equal(const VectorVel &r1, const VectorVel &r2, double eps)
Definition framevel.hpp:361
IMETHOD void ReverseSign()
Definition framevel.hpp:350
IMETHOD VectorVel & operator+=(const VectorVel &arg)
Definition framevel.hpp:336
A concrete implementation of a 3 dimensional vector class.
Definition frames.hpp:161
Definition articulatedbodyinertia.cpp:26
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.
Definition frames.hpp:1130
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
Definition frames.hpp:1215
IMETHOD void posrandom(Vector &a)
Definition frames.hpp:1244
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a.
Definition frames.hpp:1157
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
Definition frameacc.hpp:394
double valueType
Definition framevel.hpp:59
KDL::doubleVel derivType
Definition framevel.hpp:60