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> 18 ofstream os(
"tst.dat");
24 ifstream is (
"tst.dat");
27 kf2 = readKinematicFamily(is);
29 }
catch (
Error& err) {
35 std::vector<double> q(6);
36 for (
int i=0;i<q.size();++i) q[i] = i*0.2;
38 Jnt2CartPos* jnt2cartpos = kf->createJnt2CartPos();
39 Jnt2CartPos* jnt2cartpos2 = kf2->createJnt2CartPos();
41 jnt2cartpos->evaluate(q);jnt2cartpos->getFrame(F1);
42 jnt2cartpos2->evaluate(q);jnt2cartpos2->getFrame(F2);
46 cerr <<
"Results are not equal" << endl;
55 int main(
int argc,
char* argv[]) {
void test_io(KinematicFamily *kf)
void IOTraceOutput(std::ostream &os)
outputs the IO-stack to a stream to provide a better errormessage.
IMETHOD bool Equal(const FrameAcc &r1, const FrameAcc &r2, double eps=epsilon)
int main(int argc, char *argv[])
virtual const char * Description() const
represents a frame transformation in 3D space (rotation + translation)