27 #include <logging/console.h>
28 #include <plugins/openrave/environment.h>
29 #include <plugins/openrave/manipulators/katana6M180.h>
30 #include <plugins/openrave/robot.h>
31 #include <plugins/openrave/types.h>
42 printVector(vector<float> &v)
46 for (
unsigned int i = 0; i < v.size(); i++) {
47 s <<
"(" << i <<
")" << v[i] <<
" ";
50 printf(
"%s \n", s.str().c_str());
54 main(
int argc,
char **argv)
56 printf(
"++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n");
58 string robotFile = SRCDIR
"/../manipulators/katana.robot.xml";
69 robot->load(robotFile, env);
76 manip->add_motor(0, 0);
77 manip->add_motor(1, 1);
78 manip->add_motor(2, 2);
79 manip->add_motor(4, 3);
80 manip->add_motor(5, 4);
81 robot->set_manipulator(manip);
83 env->add_robot(robot);
94 manip->set_angles_device(val);
97 manip->get_angles_device(v);
104 robot->update_manipulator();
105 manip->get_angles(v);
107 manip->get_angles_device(v);