sr_kinematics.cpp
Go to the documentation of this file.
00001 // bsd license blah blah
00002 // majority of this code comes from package urdf_tool/arm_kinematics
00003 // written by David Lu!!
00004 // the IK is our own computation the FK will be in a near future.
00005 
00006 #include <cstring>
00007 #include <cmath>
00008 #include <ros/ros.h>
00009 #include <tf/transform_listener.h>
00010 #include <tf_conversions/tf_kdl.h>
00011 #include <tf/transform_datatypes.h>
00012 #include <kdl_parser/kdl_parser.hpp>
00013 #include <kdl/jntarray.hpp>
00014 #include <kdl/chainfksolverpos_recursive.hpp>
00015 #include <moveit_msgs/GetPositionFK.h>
00016 #include <moveit_msgs/GetPositionIK.h>
00017 #include <moveit_msgs/GetKinematicSolverInfo.h>
00018 #include <moveit_msgs/KinematicSolverInfo.h>
00019 #include <urdf/model.h>
00020 #include <string>
00021 
00022 using std::string;
00023 
00024 static const std::string IK_SERVICE = "get_ik";
00025 static const std::string FK_SERVICE = "get_fk";
00026 static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
00027 static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
00028 
00029 #define IK_EPS  1e-5
00030 
00031 //taken from PR2_kinematics_utils... should we link to this or ?
00032 bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00033 {
00034   double discriminant = b * b - 4 * a * c;
00035   if (fabs(a) < IK_EPS)
00036   {
00037     *x1 = -c / b;
00038     *x2 = *x1;
00039     return true;
00040   }
00041   //ROS_DEBUG("Discriminant: %f\n",discriminant);
00042   if (discriminant >= 0)
00043   {
00044     *x1 = (-b + sqrt(discriminant)) / (2 * a);
00045     *x2 = (-b - sqrt(discriminant)) / (2 * a);
00046     return true;
00047   }
00048   else if (fabs(discriminant) < IK_EPS)
00049   {
00050     *x1 = -b / (2 * a);
00051     *x2 = -b / (2 * a);
00052     return true;
00053   }
00054   else
00055   {
00056     *x1 = -b / (2 * a);
00057     *x2 = -b / (2 * a);
00058     return false;
00059   }
00060 }
00061 
00062 class Kinematics
00063 {
00064 public:
00065   Kinematics();
00066   bool init();
00067 
00068 private:
00069   ros::NodeHandle nh, nh_private;
00070   std::string root_name, finger_base_name, tip_name;
00071   KDL::JntArray joint_min, joint_max;
00072   KDL::Chain chain;
00073   unsigned int num_joints;
00074   std::vector<urdf::Pose> link_offset;
00075   std::vector<std::string> link_offset_name;
00076   std::vector<urdf::Vector3> knuckle_axis;
00077 
00078   KDL::ChainFkSolverPos_recursive* fk_solver;
00079 
00080   double epsilon;
00081   double length_middle;
00082   double length_proximal;
00083   unsigned int J5_idx_offset; //LF has an additional joint (1 when LF, 0 otherwise)
00084 
00085   ros::ServiceServer ik_service, ik_solver_info_service;
00086   ros::ServiceServer fk_service, fk_solver_info_service;
00087 
00088   tf::TransformListener tf_listener;
00089 
00090   moveit_msgs::KinematicSolverInfo info;
00091 
00092   bool loadModel(const std::string xml);
00093   bool readJoints(urdf::Model &robot_model);
00094   int getJointIndex(const std::string &name);
00095   int getKDLSegmentIndex(const std::string &name);
00096 
00102   bool getPositionIK(moveit_msgs::GetPositionIK::Request &request, moveit_msgs::GetPositionIK::Response &response);
00103 
00109   bool getIKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00110                        moveit_msgs::GetKinematicSolverInfo::Response &response);
00111 
00117   bool getFKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00118                        moveit_msgs::GetKinematicSolverInfo::Response &response);
00119 
00125   bool getPositionFK(moveit_msgs::GetPositionFK::Request &request, moveit_msgs::GetPositionFK::Response &response);
00126 };
00127 
00128 Kinematics::Kinematics() :
00129     nh_private("~"), epsilon(1e-2), length_proximal(.0), length_middle(.0), num_joints(0),
00130     J5_idx_offset(0), fk_solver(NULL)
00131 {
00132 }
00133 
00134 bool Kinematics::init()
00135 {
00136   // Get URDF XML
00137   std::string urdf_xml, full_urdf_xml;
00138   nh.param("urdf_xml", urdf_xml, std::string("robot_description"));
00139   nh.searchParam(urdf_xml, full_urdf_xml);
00140   ROS_DEBUG("Reading xml file from parameter server");
00141   std::string result;
00142   if (!nh.getParam(full_urdf_xml, result))
00143   {
00144     ROS_FATAL("Could not load the xml from parameter server: %s", urdf_xml.c_str());
00145     return false;
00146   }
00147 
00148   // Get Root and Tip From Parameter Service
00149   if (!nh_private.getParam("root_name", root_name))
00150   {
00151     ROS_FATAL("GenericIK: No root name found on parameter server");
00152     return false;
00153   }
00154   if (root_name != "palm")
00155   {
00156     ROS_FATAL("Current solver can only resolve to root frame = palm");
00157     return false;
00158   }
00159 
00160   if (!nh_private.getParam("tip_name", tip_name))
00161   {
00162     ROS_FATAL("GenericIK: No tip name found on parameter server");
00163     return false;
00164   }
00165 
00166   if (tip_name.find("distal") == string::npos)
00167   {
00168     ROS_FATAL("Current solver can only resolve to one of the distal frames");
00169     return false;
00170   }
00171 
00172   if (tip_name.find("lfdistal") != string::npos)
00173   {
00174     ROS_INFO("Computing LF IK not considering J5");
00175     J5_idx_offset = 1;
00176   }
00177   else
00178     J5_idx_offset = 0;
00179 
00180   if (tip_name.find("thdistal") != string::npos)
00181   {
00182     ROS_FATAL("Current solver cannot resolve to the thdistal frame");
00183     return false;
00184   }
00185 
00186   finger_base_name = tip_name.substr(0, 2);
00187   finger_base_name.append("knuckle");
00188   ROS_INFO("base_finger name %s", finger_base_name.c_str());
00189 
00190   // Load and Read Models
00191   if (!loadModel(result))
00192   {
00193     ROS_FATAL("Could not load models!");
00194     return false;
00195   }
00196 
00197   // Get Solver Parameters
00198   int maxIterations;
00199 
00200   nh_private.param("maxIterations", maxIterations, 1000);
00201   nh_private.param("epsilon", epsilon, 1e-2);
00202 
00203   // Build Solvers
00204   fk_solver = new KDL::ChainFkSolverPos_recursive(chain); //keep the standard arm_kinematics fk_solver although we have ours.
00205 
00206   ROS_INFO("Advertising services");
00207   fk_service = nh_private.advertiseService(FK_SERVICE, &Kinematics::getPositionFK, this);
00208   ik_service = nh_private.advertiseService(IK_SERVICE, &Kinematics::getPositionIK, this);
00209   ik_solver_info_service = nh_private.advertiseService(IK_INFO_SERVICE, &Kinematics::getIKSolverInfo, this);
00210   fk_solver_info_service = nh_private.advertiseService(FK_INFO_SERVICE, &Kinematics::getFKSolverInfo, this);
00211 
00212   return true;
00213 }
00214 
00215 bool Kinematics::loadModel(const std::string xml)
00216 {
00217   urdf::Model robot_model;
00218   KDL::Tree tree;
00219 
00220   if (!robot_model.initString(xml))
00221   {
00222     ROS_FATAL("Could not initialize robot model");
00223     return -1;
00224   }
00225   if (!kdl_parser::treeFromString(xml, tree))
00226   {
00227     ROS_ERROR("Could not initialize tree object");
00228     return false;
00229   }
00230   if (!tree.getChain(root_name, tip_name, chain))
00231   {
00232     ROS_ERROR("Could not initialize chain object");
00233     return false;
00234   }
00235 
00236   if (!readJoints(robot_model))
00237   {
00238     ROS_FATAL("Could not read information about the joints");
00239     return false;
00240   }
00241 
00242   return true;
00243 }
00244 
00245 bool Kinematics::readJoints(urdf::Model &robot_model)
00246 {
00247   num_joints = 0;
00248   // get joint maxs and mins
00249   boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
00250   boost::shared_ptr<const urdf::Joint> joint;
00251 
00252   bool length_proximal_done = false;
00253   bool length_middle_done = false;
00254   urdf::Vector3 length;
00255 
00256   while (link && link->name != root_name)
00257   {
00258     //extract knuckle to palm_offset.
00259     if (link->name.find("knuckle") != string::npos)
00260     {
00261       if (link->getParent()->name == "palm") // FF,MF,RF
00262       {
00263         link_offset.push_back(link->parent_joint->parent_to_joint_origin_transform);
00264         link_offset_name.push_back(link->name);
00265         knuckle_axis.push_back(link->parent_joint->axis);
00266       }
00267       else // LF
00268       {
00269         //temp store the first offset
00270         urdf::Pose first_offset = link->parent_joint->parent_to_joint_origin_transform;
00271         std::string link_name = link->name;
00272         //check for palm in parent of parent
00273         if (link->getParent()->getParent()->name == "palm")
00274         {
00275           urdf::Pose second_offset = link->getParent()->parent_joint->parent_to_joint_origin_transform;
00276           urdf::Pose combined_offset(first_offset);
00277           //combine only position (so the pose between palm and lfknuckle may be wrong regarding rotations)
00278           combined_offset.position.x += second_offset.position.x;
00279           combined_offset.position.y += second_offset.position.y;
00280           combined_offset.position.z += second_offset.position.z;
00281           link_offset.push_back(combined_offset);
00282           link_offset_name.push_back(link_name);
00283           knuckle_axis.push_back(link->parent_joint->axis);
00284         }
00285         else
00286         { //stop at level2; structure must be false.
00287           ROS_ERROR("Could not find palm parent for this finger");
00288           return false;
00289         }
00290       }
00291     }
00292 
00293     // extract proximal length
00294     if (!length_proximal_done)
00295     {
00296       if (link->name.find("middle") != string::npos)
00297       {
00298         length = link->parent_joint->parent_to_joint_origin_transform.position;
00299         length_proximal = sqrt(length.x * length.x + length.y * length.y + length.z * length.z);
00300         length_proximal_done = true;
00301       }
00302     }
00303 
00304     // extract middle length
00305     if (!length_middle_done)
00306     {
00307       if (link->name.find("distal") != string::npos)
00308       {
00309         length = link->parent_joint->parent_to_joint_origin_transform.position;
00310         length_middle = sqrt(length.x * length.x + length.y * length.y + length.z * length.z);
00311         length_middle_done = true;
00312       }
00313     }
00314 
00315     joint = robot_model.getJoint(link->parent_joint->name);
00316     if (!joint)
00317     {
00318       ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
00319       return false;
00320     }
00321     if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00322     {
00323       ROS_INFO("adding joint: [%s]", joint->name.c_str());
00324       num_joints++;
00325     }
00326     link = robot_model.getLink(link->getParent()->name);
00327   }
00328 
00329   joint_min.resize(num_joints);
00330   joint_max.resize(num_joints);
00331   info.joint_names.resize(num_joints);
00332   info.link_names.resize(num_joints);
00333   info.limits.resize(num_joints);
00334 
00335   link = robot_model.getLink(tip_name);
00336   unsigned int i = 0;
00337   while (link && i < num_joints)
00338   {
00339     joint = robot_model.getJoint(link->parent_joint->name);
00340     if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00341     {
00342       ROS_INFO("getting bounds for joint: [%s]", joint->name.c_str());
00343 
00344       float lower, upper;
00345       int hasLimits;
00346       if (joint->type != urdf::Joint::CONTINUOUS)
00347       {
00348         lower = joint->limits->lower;
00349         upper = joint->limits->upper;
00350         hasLimits = 1;
00351       }
00352       else
00353       {
00354         lower = -M_PI;
00355         upper = M_PI;
00356         hasLimits = 0;
00357       }
00358       int index = num_joints - i - 1;
00359 
00360       joint_min.data[index] = lower;
00361       joint_max.data[index] = upper;
00362       info.joint_names[index] = joint->name;
00363       info.link_names[index] = link->name;
00364       info.limits[index].joint_name = joint->name;
00365       info.limits[index].has_position_limits = hasLimits;
00366       info.limits[index].min_position = lower;
00367       info.limits[index].max_position = upper;
00368       i++;
00369     }
00370     link = robot_model.getLink(link->getParent()->name);
00371   }
00372   return true;
00373 }
00374 
00375 int Kinematics::getJointIndex(const std::string &name)
00376 {
00377   for (unsigned int i = 0; i < info.joint_names.size(); i++)
00378   {
00379     if (info.joint_names[i] == name)
00380       return i;
00381   }
00382   return -1;
00383 }
00384 
00385 int Kinematics::getKDLSegmentIndex(const std::string &name)
00386 {
00387   int i = 0;
00388   while (i < (int)chain.getNrOfSegments())
00389   {
00390     if (chain.getSegment(i).getName() == name)
00391     {
00392       return i + 1;
00393     }
00394     i++;
00395   }
00396   return -1;
00397 }
00398 
00399 bool Kinematics::getPositionIK(moveit_msgs::GetPositionIK::Request &request,
00400                                moveit_msgs::GetPositionIK::Response &response)
00401 {
00402 
00403   // get the 3D position of requested goal (forget about orientation, which is not relevant)
00404   geometry_msgs::PointStamped point_msg_in;
00405   point_msg_in.header.frame_id = request.ik_request.pose_stamped.header.frame_id;
00406   point_msg_in.header.stamp = ros::Time::now() - ros::Duration(1);
00407   point_msg_in.point = request.ik_request.pose_stamped.pose.position;
00408 
00409   tf::Stamped<tf::Point> transform;
00410   tf::Stamped<tf::Point> transform_root;
00411   tf::Stamped<tf::Point> transform_finger_base;
00412   tf::pointStampedMsgToTF(point_msg_in, transform);
00413 
00414   urdf::Pose knuckle_offset;
00415   urdf::Vector3 J4_axis;
00416   float J4_dir;
00417   tf::Point p; //local req_point coordinates
00418   tf::Point pbis; //with different coordinate system
00419 
00420   // IK computation variables
00421   double l1 = length_proximal, l2 = length_middle;
00422   double L = 0.0;
00423   double x1, x2;
00424   double thetap, thetam, l2p;
00425   double alpha2;
00426   float b, c, delta = 0.01;
00427 
00428   //Do the IK
00429   KDL::JntArray jnt_pos_in;
00430   KDL::JntArray jnt_pos_out;
00431   jnt_pos_in.resize(num_joints);
00432   jnt_pos_out.resize(num_joints);
00433 
00434   //Convert F to our root_frame
00435   ROS_DEBUG("sr_kin: Get point in root frame");
00436   try
00437   {
00438     tf_listener.transformPoint(root_name, transform, transform_root);
00439   }
00440   catch (...)
00441   {
00442     ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str());
00443     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00444     return false;
00445   }
00446   ROS_DEBUG("root x,y,z:%f,%f,%f", transform_root.x(), transform_root.y(), transform_root.z());
00447 
00448   //look for knuckle corresponding to distal phalanx
00449   ROS_DEBUG("sr_kin: Looking for J4_axis direction and knuckle offset");
00450   for (unsigned int i = 0; i < link_offset_name.size(); i++)
00451   {
00452     if (link_offset_name[i] == finger_base_name)
00453     {
00454       knuckle_offset = link_offset[i];
00455       ROS_DEBUG("knuckle offset is %f,%f,%f", knuckle_offset.position.x, knuckle_offset.position.y,
00456                 knuckle_offset.position.z);
00457       J4_axis = knuckle_axis[i];
00458       //ROS_DEBUG("J4 axis is %f,%f,%f",J4_axis.x,J4_axis.y,J4_axis.z);
00459       ROS_DEBUG("J4 is %s", J4_axis.y > 0 ? "positive" : "negative");
00460       break;
00461     }
00462   }
00463 
00464   // Change to a local computation frame (at knuckle pos but with palm orientation)
00465   ROS_DEBUG("sr_kin: Convert Frame to local computation frame");
00466   p.setValue(-knuckle_offset.position.x, -knuckle_offset.position.y, -knuckle_offset.position.z);
00467   p += transform_root;
00468   ROS_DEBUG("x,y,z:%f,%f,%f", p.x(), p.y(), p.z());
00469 
00470   // Change frame to the one where maths have been done :-)
00471   pbis = p;
00472   pbis.setValue(p.z(), p.x(), -p.y());
00473   ROS_DEBUG("p2bis %f,%f,%f", pbis.x(), pbis.y(), pbis.z());
00474 
00475   // because J4 are not the same direction for each finger
00476   // we need to change it according to knuckle axis direction
00477   ROS_DEBUG("sr_kin: Compute J4");
00478   J4_dir = J4_axis.y > 0 ? 1.0 : -1.0;
00479 
00480   jnt_pos_out(0) = 0; //to solve the case of LF;
00481   if (fabs(pbis.x()) - epsilon > 0.0)
00482   {
00483     jnt_pos_out(0 + J5_idx_offset) = J4_dir * atan(pbis.y() / pbis.x());
00484   }
00485   else
00486   {
00487     if (fabs(pbis.y()) - epsilon > 0.0)
00488     {
00489       float sign = pbis.y() >= 0 ? 1.0 : -1.0;
00490       jnt_pos_out(0 + J5_idx_offset) = J4_dir * sign * M_PI_2;
00491     }
00492     else
00493       jnt_pos_out(0 + J5_idx_offset) = 0;
00494   }
00495 
00496   L = pbis.length2();
00497   ROS_DEBUG("L %f", L);
00498   ROS_DEBUG("sr_kin: Solve IK quadratic system");
00499   if (!solveQuadratic(l1 * l2, l2 * l2 - 3 * l1 * l2, -(L) + l1 * l1, &x2, &x1)) //x1 x2 inversed is normal
00500   {
00501     ROS_DEBUG("No solution to quadratic eqn");
00502     return false;
00503   }
00504   ROS_DEBUG("x1,x2 %f,%f", x1, x2);
00505 
00506   // thetap et thetam are the 2 possibilities for each x1 or x2 in the isoceles triangle
00507   ROS_DEBUG("sr_kin: Check for acceptable solutions");
00508   if (x2 > 0)
00509   {
00510     thetam = acos(sqrt(x2) / 2);
00511     thetap = acos(-sqrt(x2) / 2);
00512     l2p = 2 * l2 * (sqrt(x2) / 2);
00513   }
00514   else
00515   {
00516     if (x1 > 0)
00517     {
00518       thetam = acos(sqrt(x1) / 2);
00519       thetap = acos(-sqrt(x1) / 2);
00520       l2p = 2 * l2 * (sqrt(x1) / 2);
00521     }
00522     else
00523     {
00524       thetap = 0;
00525       thetam = 0;
00526       l2p = 2 * l2;
00527     }
00528   }
00529 
00530   // alpha2 is the virtual angle of the second phalanx in a system with only 2 phalanxes
00531   ROS_DEBUG("sr_kin: Compute virtual ALPHA");
00532   ROS_DEBUG("thetap,thetam, l2p %f,%f,%f", thetap, thetam, l2p);
00533   if (thetap >= 0 && thetap <= M_PI_2)
00534   {
00535     alpha2 = 3 * thetap;
00536   }
00537   else
00538   {
00539     if (thetam <= M_PI_2)
00540       alpha2 = 3 * thetam;
00541     else
00542       alpha2 = 0;
00543   }
00544 
00545   b = l1 + l2p * cos(alpha2);
00546   c = l2p * sin(alpha2);
00547 
00548   ROS_DEBUG("sr_kin: Compute J3");
00549   ROS_DEBUG("alpha2,b,c %f,%f,%f", alpha2, b, c);
00550   if (pbis.x() < 0)
00551     jnt_pos_out(1 + J5_idx_offset) = M_PI_2 + atan2(sqrt(pbis.x() * pbis.x() + pbis.y() * pbis.y()), pbis.z())
00552         - atan2(c, b);
00553   else
00554     jnt_pos_out(1 + J5_idx_offset) = atan2(b * pbis.z() - c * sqrt(pbis.x() * pbis.x() + pbis.y() * pbis.y()),
00555                                            b * sqrt(pbis.x() * pbis.x() + pbis.y() * pbis.y()) + c * pbis.z());
00556 
00557   ROS_DEBUG("sr_kin: Compute J0=J1+J2");
00558   jnt_pos_out(2 + J5_idx_offset) = 2.0 * alpha2 / 3.0; // sum of the angles of the last 2 phalanxes divided by 2 to get individual phalanx angle in the coupled joint
00559   jnt_pos_out(3 + J5_idx_offset) = jnt_pos_out(2 + J5_idx_offset);
00560 
00561   for (unsigned int i = 0; i < num_joints; i++)
00562   {
00563     ROS_DEBUG("limit min-max %d, %f , %f", i, joint_min(i), joint_max(i));
00564     ROS_DEBUG("pos %d,%f", i, jnt_pos_out(i));
00565     if (jnt_pos_out(i) > joint_max(i) + delta || jnt_pos_out(i) < joint_min(i) - delta || isnan(jnt_pos_out(i)))
00566     {
00567       jnt_pos_out(0 + J5_idx_offset) = 0;
00568       jnt_pos_out(1 + J5_idx_offset) = 0;
00569       jnt_pos_out(2 + J5_idx_offset) = 0;
00570       jnt_pos_out(3 + J5_idx_offset) = 0;
00571       return 0;
00572     }
00573   }
00574 
00575   int ik_valid = 1;
00576 
00577   if (ik_valid >= 0)
00578   {
00579     response.solution.joint_state.name = info.joint_names;
00580     response.solution.joint_state.position.resize(num_joints);
00581     for (unsigned int i = 0; i < num_joints; i++)
00582     {
00583       response.solution.joint_state.position[i] = jnt_pos_out(i);
00584       ROS_DEBUG("IK Solution: %s %d: %f", response.solution.joint_state.name[i].c_str(), i, jnt_pos_out(i));
00585     }
00586     response.error_code.val = response.error_code.SUCCESS;
00587     return true;
00588   }
00589   else
00590   {
00591     ROS_DEBUG("An IK solution could not be found");
00592     response.error_code.val = response.error_code.NO_IK_SOLUTION;
00593     return true;
00594   }
00595   return true;
00596 }
00597 
00598 bool Kinematics::getIKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00599                                  moveit_msgs::GetKinematicSolverInfo::Response &response)
00600 {
00601   response.kinematic_solver_info = info;
00602   return true;
00603 }
00604 
00605 bool Kinematics::getFKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00606                                  moveit_msgs::GetKinematicSolverInfo::Response &response)
00607 {
00608   response.kinematic_solver_info = info;
00609   return true;
00610 }
00611 
00612 bool Kinematics::getPositionFK(moveit_msgs::GetPositionFK::Request &request,
00613                                moveit_msgs::GetPositionFK::Response &response)
00614 {
00615   KDL::Frame p_out;
00616   KDL::JntArray jnt_pos_in;
00617   geometry_msgs::PoseStamped pose;
00618   tf::Stamped<tf::Pose> tf_pose;
00619 
00620   jnt_pos_in.resize(num_joints);
00621   for (unsigned int i = 0; i < num_joints; i++)
00622   {
00623     int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]);
00624     if (tmp_index >= 0)
00625       jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
00626   }
00627 
00628   response.pose_stamped.resize(request.fk_link_names.size());
00629   response.fk_link_names.resize(request.fk_link_names.size());
00630 
00631   bool valid = true;
00632   for (unsigned int i = 0; i < request.fk_link_names.size(); i++)
00633   {
00634     int segmentIndex = getKDLSegmentIndex(request.fk_link_names[i]);
00635     ROS_DEBUG("End effector index: %d", segmentIndex);
00636     ROS_DEBUG("Chain indices: %d", chain.getNrOfSegments());
00637     if (fk_solver->JntToCart(jnt_pos_in, p_out, segmentIndex) >= 0)
00638     {
00639       tf_pose.frame_id_ = root_name;
00640       tf_pose.stamp_ = ros::Time();
00641       tf::poseKDLToTF(p_out, tf_pose);
00642       try
00643       {
00644         tf_listener.transformPose(request.header.frame_id, tf_pose, tf_pose);
00645       }
00646       catch (...)
00647       {
00648         ROS_ERROR("Could not transform FK pose to frame: %s", request.header.frame_id.c_str());
00649         response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00650         return false;
00651       }
00652       tf::poseStampedTFToMsg(tf_pose, pose);
00653       response.pose_stamped[i] = pose;
00654       response.fk_link_names[i] = request.fk_link_names[i];
00655       response.error_code.val = response.error_code.SUCCESS;
00656     }
00657     else
00658     {
00659       ROS_ERROR("Could not compute FK for %s", request.fk_link_names[i].c_str());
00660       response.error_code.val = response.error_code.NO_IK_SOLUTION;
00661       valid = false;
00662     }
00663   }
00664   return true;
00665 }
00666 
00667 int main(int argc, char **argv)
00668 {
00669   ros::init(argc, argv, "sr_kinematics");
00670   Kinematics k;
00671   if (k.init() < 0)
00672   {
00673     ROS_ERROR("Could not initialize kinematics node");
00674     return -1;
00675   }
00676 
00677   ros::spin();
00678   return 0;
00679 }
00680 


sr_kinematics
Author(s): David LU, adapted for ShadowHand by Guillaume WALCK
autogenerated on Fri Aug 28 2015 13:09:49