1 #include <kdl/framevel.hpp> 2 #include <kdl/frameacc.hpp> 3 #include <kdl/framevel_io.hpp> 4 #include <kdl/frameacc_io.hpp> 5 #include <kdl/test_macros.h> 17 KDL_DIFF( v+v+v-2*v, v );
26 KDL_DIFF( v*vt,-vt*v);
28 KDL_DIFF( v*v2,-v2*v);
38 KDL_DIFF( v+v+v-2*v, v );
47 KDL_DIFF( v*vt,-vt*v);
49 KDL_DIFF( v*v2,-v2*v);
59 KDL_DIFF( v+v+v-2*v, v );
68 KDL_DIFF( v*vt,-vt*v);
70 KDL_DIFF( v*v2,-v2*v);
89 KDL_DIFF( (R*v).
Norm(), v.Norm() );
94 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
95 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
98 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
120 KDL_DIFF( (R*v).
Norm(), v.Norm() );
125 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
126 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
129 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
150 KDL_DIFF( (R*v).
Norm(), v.Norm() );
155 KDL_DIFF( R*(R*(R*v)), (R*R*R)*v );
156 KDL_DIFF( R*(R*(R*vt)), (R*R*R)*vt );
159 KDL_DIFF( R.Inverse()*v, R.Inverse(v) );
176 KDL_DIFF( F.
Inverse(F*vt), vt );
178 KDL_DIFF( F*F.
Inverse(vt), vt );
181 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
182 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
185 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
202 KDL_DIFF( F.
Inverse(F*vt), vt );
204 KDL_DIFF( F*F.
Inverse(vt), vt );
207 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
208 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
211 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
229 KDL_DIFF( F.
Inverse(F*vt), vt );
231 KDL_DIFF( F*F.
Inverse(vt), vt );
234 KDL_DIFF( F*(F*(F*v)), (F*F*F)*v );
235 KDL_DIFF( F*(F*(F*vt)), (F*F*F)*vt );
238 KDL_DIFF( F.Inverse()*vt, F.Inverse(vt) );
267 KDL_DIFF(t+t+t-2.0*t,t);
277 KDL_DIFF(R*t,R*R.
Inverse(R*t));
279 KDL_DIFF(F*t,F*F.
Inverse(F*t));
282 KDL_DIFF(t/3.14,t*(1.0/3.14));
286 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
287 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
309 KDL_DIFF(t+t+t-2.0*t,t);
319 KDL_DIFF(R*t,R*R.
Inverse(R*t));
321 KDL_DIFF(F*t,F*F.
Inverse(F*t));
324 KDL_DIFF(t/3.14,t*(1.0/3.14));
328 KDL_DIFF(t.RefPoint(p1+p2),t.RefPoint(p1).RefPoint(p2));
329 KDL_DIFF(t,t.RefPoint(-p1).RefPoint(p1));
IMETHOD void ReverseSign()
represents rotations in 3 dimensional space.
static IMETHOD RotationAcc Identity()
INLINE S Norm(const Rall1d< T, V, S > &value)
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
static IMETHOD RotationVel Identity()
IMETHOD void ReverseSign()
IMETHOD void ReverseSign()
void ReverseSign()
Reverses the sign of the Vector object itself.
doubleAcc dot(const VectorAcc &lhs, const VectorAcc &rhs)
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
represents both translational and rotational velocities.
IMETHOD RotationAcc Inverse() const
A concrete implementation of a 3 dimensional vector class.
static Rotation RPY(double roll, double pitch, double yaw)
static IMETHOD FrameVel Identity()
IMETHOD RotationVel Inverse() const
static Rotation Identity()
Gives back an identity rotaton matrix.
static IMETHOD FrameAcc Identity()
INLINE Rall1d< T, V, S > sqrt(const Rall1d< T, V, S > &arg)
Frame Inverse() const
Gives back inverse transformation of a Frame.
Rall2d< double, double, double > doubleAcc
represents a frame transformation in 3D space (rotation + translation)
IMETHOD FrameVel Inverse() const
IMETHOD FrameAcc Inverse() const
const double deg2rad
the value pi/180
IMETHOD void ReverseSign()
Rall1d< double > doubleVel