1 #include <kdl/frames.hpp> 2 #include <kdl/frames_io.hpp> 3 #include <kdl/framevel.hpp> 4 #include <kdl/framevel_io.hpp> 5 #include <kdl/jacobianexpr.hpp> 6 #include <kdl/jacobianframe.hpp> 7 #include <kdl/jacobianframevel.hpp> 15 double adouble,bdouble;
62 SetEulerZYX(gamma,beta,alpha,R);
64 int result=GetEulerZYX(R,gamma,beta,alpha);
66 SetEulerZYX(gamma,beta,alpha,R2);
68 for (
int i=0;i<nrofcol;i++) {
73 std::cout <<
"Tests with numerical derivatives for EulerZYX " << std::endl;
74 for (
int i=0;i<nrofcol;i++) {
80 alpha.value()+ alpha.deriv(i)*dt,
81 beta.value() + beta.deriv(i)*dt,
82 gamma.value()+ gamma.deriv(i)*dt),
represents rotations in 3 dimensional space.
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
void checkEqual(const T &a, const T &b, double eps)
static Rotation EulerZYX(double Alfa, double Beta, double Gamma)
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
represents both translational and rotational velocities.
A concrete implementation of a 3 dimensional vector class.
static void check(void(*rnd)(Jacobian< A > &)=&random, double dt=1E-8, double eps=1E-4, int size=1)
static void check(double dt=1E-8, double eps=1E-4, int size=1)
represents a frame transformation in 3D space (rotation + translation)
IMETHOD Vector addDelta(const Vector &p_w_a, const Vector &p_w_da, double dt=1)
adds vector da to vector a. see also the corresponding diff() routine.
IMETHOD void random(Vector &a)
addDelta operator for displacement rotational velocity.
static void check(double dt=1E-8, double eps=1E-4, int size=1)