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


sr_kinematics
Author(s): David LU, adapted for ShadowHand by Guillaume WALCK
autogenerated on Tue Jun 10 2014 15:13:32