1 #include <ros/ros.h>
   2 #include <ros/console.h>
   3 #include <iostream>
   4 #include <kdl_wrapper/kdl_wrapper.h>
   5 
   6 
   7 int main(int argc, char *argv[])
   8 {
   9     /// initialize ROS, specify name of node
  10     ros::init(argc, argv, "pr2_kdl_wrapper_example");
  11 
  12 
  13     KDLWrapper pr2_kdl_wrapper;
  14 
  15     if(!pr2_kdl_wrapper.init("torso_lift_link", "r_gripper_tool_frame"))
  16     {
  17         ROS_ERROR("Error initiliazing pr2_kdl_wrapper");
  18     }
  19 
  20     KDL::JntArray q_in(7);
  21     q_in(0) = 0.0;
  22     q_in(1) = M_PI/2;
  23     q_in(2) = 0.0;
  24     q_in(3) = M_PI/4;
  25     q_in(4) = 0.0;
  26     q_in(5) = 0.0;
  27     q_in(6) = M_PI/3;
  28 
  29 
  30     KDL::Twist v_in;
  31     v_in.vel = KDL::Vector(0.0, 0.2, 0.2);
  32     v_in.rot = KDL::Vector(0.0, 0.0, 0.0);
  33 
  34     KDL::JntArray q_dot_out;
  35     ROS_INFO("Calculating inverse kinematics");
  36     pr2_kdl_wrapper.ik_solver_vel->setLambda(0.3);
  37     pr2_kdl_wrapper.ik_solver_vel->CartToJnt(q_in, v_in, q_dot_out);
  38 
  39     ROS_INFO("Output q_dot: (%f, %f, %f, %f, %f, %f, %f)",
  40              q_dot_out(0),
  41              q_dot_out(1),
  42              q_dot_out(2),
  43              q_dot_out(3),
  44              q_dot_out(4),
  45              q_dot_out(5),
  46              q_dot_out(6));
  47 
  48     return 0;
  49 }