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++) {
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),