2 #include <kdl/error_stack.h>
4 #include <kdl/frames.hpp>
5 #include <kdl/frames_io.hpp>
6 #include <kdl/kinfam/joint.hpp>
7 #include <kdl/kinfam/serialchain.hpp>
8 #include <kdl/kinfam/kinematicfamily_io.hpp>
9 #include <kdl/kinfam/crs450.hpp>
10 #include <kdl/kinfam/jnt2cartpos.hpp>
26 kf2 = readKinematicFamily(is);
28 }
catch (
Error& err) {
35 for (
int i=0;i<q.
size();++i) q[i] = i*0.2;
37 Jnt2CartPos* jnt2cartpos = kf->createJnt2CartPos();
38 Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
40 jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1);
41 jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2);
54 int main(
int argc,
char* argv[]) {