pr2_arm_kinematics_utils.cpp
Go to the documentation of this file.
00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #include <pr2_arm_kinematics/pr2_arm_kinematics_utils.h>
00034 #include <pr2_arm_kinematics/pr2_arm_kinematics_constants.h>
00035 
00036 namespace pr2_arm_kinematics
00037 {
00038   static const double IK_DEFAULT_TIMEOUT = 10.0;
00039   bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &xml_string)
00040   {
00041     std::string urdf_xml,full_urdf_xml;
00042     node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00043     node_handle.searchParam(urdf_xml,full_urdf_xml);
00044     TiXmlDocument xml;
00045     ROS_DEBUG("Reading xml file from parameter server\n");
00046     std::string result;
00047     if (node_handle.getParam(full_urdf_xml, result))
00048       xml.Parse(result.c_str());
00049     else
00050     {
00051       ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00052       return false;
00053     }
00054     xml_string = result;
00055     TiXmlElement *root_element = xml.RootElement();
00056     TiXmlElement *root = xml.FirstChildElement("robot");
00057     if (!root || !root_element)
00058     {
00059       ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
00060       exit(1);
00061     }
00062     robot_model.initXml(root);
00063     return true;
00064   }
00065 
00066   bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
00067   {
00068     // create robot chain from root to tip
00069     KDL::Tree tree;
00070     if (!kdl_parser::treeFromString(xml_string, tree))
00071     {
00072       ROS_ERROR("Could not initialize tree object");
00073       return false;
00074     }
00075     if (!tree.getChain(root_name, tip_name, kdl_chain))
00076     {
00077       ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
00078       return false;
00079     }
00080     return true;
00081   }
00082 
00083   bool getKDLTree(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Tree &kdl_tree)
00084   {
00085     // create robot chain from root to tip
00086     if (!kdl_parser::treeFromString(xml_string, kdl_tree))
00087     {
00088       ROS_ERROR("Could not initialize tree object");
00089       return false;
00090     }
00091     return true;
00092   }
00093 
00094 
00095   Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
00096   {
00097     Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
00098     for(int i=0; i < 3; i++)
00099     {
00100       for(int j=0; j<3; j++)
00101       {
00102         b(i,j) = p.M(i,j);
00103       }
00104       b(i,3) = p.p(i);
00105     }
00106     return b;
00107   }
00108 
00109   double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2)
00110   {
00111     double distance = 0.0;
00112     for(int i=0; i< (int) array_1.size(); i++)
00113     {
00114       distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
00115     }
00116     return sqrt(distance);
00117   }
00118 
00119 
00120   double distance(const urdf::Pose &transform)
00121   {
00122     return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
00123   }
00124 
00125 
00126   bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00127   {
00128     double discriminant = b*b-4*a*c;
00129     if(fabs(a) < IK_EPS)
00130     {
00131       *x1 = -c/b;
00132       *x2 = *x1;
00133       return true;
00134     }
00135     //ROS_DEBUG("Discriminant: %f\n",discriminant);
00136     if (discriminant >= 0)
00137     {      
00138       *x1 = (-b + sqrt(discriminant))/(2*a); 
00139       *x2 = (-b - sqrt(discriminant))/(2*a);
00140       return true;
00141     } 
00142     else if(fabs(discriminant) < IK_EPS)
00143     {
00144       *x1 = -b/(2*a);
00145       *x2 = -b/(2*a);
00146       return true;
00147     }
00148     else
00149     {
00150       *x1 = -b/(2*a);
00151       *x2 = -b/(2*a);
00152       return false;
00153     }
00154   }
00155 
00156   Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g)
00157   {
00158     Eigen::Matrix4f result = g;
00159     Eigen::Matrix3f Rt = Eigen::Matrix3f::Identity();
00160 
00161     Eigen::Vector3f p = Eigen::Vector3f::Zero(3);
00162     Eigen::Vector3f pinv = Eigen::Vector3f::Zero(3);
00163 
00164     Rt(0,0) = g(0,0);
00165     Rt(1,1) = g(1,1);
00166     Rt(2,2) = g(2,2);
00167 
00168     Rt(0,1) = g(1,0);
00169     Rt(1,0) = g(0,1);
00170 
00171     Rt(0,2) = g(2,0);
00172     Rt(2,0) = g(0,2);
00173 
00174     Rt(1,2) = g(2,1);
00175     Rt(2,1) = g(1,2);
00176 
00177     p(0) = g(0,3);
00178     p(1) = g(1,3);
00179     p(2) = g(2,3);
00180 
00181     pinv = -Rt*p;
00182 
00183     result(0,0) = g(0,0);
00184     result(1,1) = g(1,1);
00185     result(2,2) = g(2,2);
00186 
00187     result(0,1) = g(1,0);
00188     result(1,0) = g(0,1);
00189 
00190     result(0,2) = g(2,0);
00191     result(2,0) = g(0,2);
00192 
00193     result(1,2) = g(2,1);
00194     result(2,1) = g(1,2);
00195 
00196     result(0,3) = pinv(0);
00197     result(1,3) = pinv(1);
00198     result(2,3) = pinv(2);
00199   
00200     return result;
00201   }
00202 
00203 
00204   bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
00205   {
00206     double theta1 = atan2(b,a);
00207     double denom  = sqrt(a*a+b*b);
00208 
00209     if(fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
00210     {
00211 #ifdef DEBUG
00212       std::cout << "denom: " << denom << std::endl;
00213 #endif
00214       return false;
00215     }
00216     double rhs_ratio = c/denom;
00217     if(rhs_ratio < -1 || rhs_ratio > 1)
00218     {
00219 #ifdef DEBUG
00220       std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00221 #endif
00222       return false;
00223     }
00224     double acos_term = acos(rhs_ratio);
00225     soln1 = theta1 + acos_term;
00226     soln2 = theta1 - acos_term;
00227 
00228     return true;
00229   }
00230 
00231 
00232 bool checkJointNames(const std::vector<std::string> &joint_names,
00233                        const kinematics_msgs::KinematicSolverInfo &chain_info)
00234   {    
00235     for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00236     {
00237       int index = -1;
00238       for(unsigned int j=0; j < joint_names.size(); j++)
00239       {
00240         if(chain_info.joint_names[i] == joint_names[j])
00241         {
00242           index = j;
00243           break;
00244         }
00245       }
00246       if(index < 0)
00247       {
00248         ROS_ERROR("Joint state does not contain joint state for %s.",chain_info.joint_names[i].c_str());
00249         return false;
00250       }
00251     }
00252     return true;
00253   }
00254 
00255   bool checkLinkNames(const std::vector<std::string> &link_names,
00256                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00257   {
00258     if(link_names.empty())
00259       return false;
00260     for(unsigned int i=0; i < link_names.size(); i++)
00261     {
00262       if(!checkLinkName(link_names[i],chain_info))
00263         return false;
00264     }
00265     return true;   
00266   }
00267 
00268   bool checkLinkName(const std::string &link_name,
00269                    const kinematics_msgs::KinematicSolverInfo &chain_info)
00270   {
00271     for(unsigned int i=0; i < chain_info.link_names.size(); i++)
00272     {
00273       if(link_name == chain_info.link_names[i])
00274         return true;
00275     }
00276     return false;   
00277   }
00278 
00279   bool checkRobotState(arm_navigation_msgs::RobotState &robot_state,
00280                      const kinematics_msgs::KinematicSolverInfo &chain_info)
00281   {
00282     if((int) robot_state.joint_state.position.size() != (int) robot_state.joint_state.name.size())
00283     {
00284       ROS_ERROR("Number of joints in robot_state.joint_state does not match number of positions in robot_state.joint_state");
00285       return false;
00286     }    
00287     if(!checkJointNames(robot_state.joint_state.name,chain_info))
00288     {
00289       ROS_ERROR("Robot state must contain joint state for every joint in the kinematic chain");
00290       return false;
00291     }
00292     return true;
00293   }
00294 
00295   bool checkFKService(kinematics_msgs::GetPositionFK::Request &request, 
00296                       kinematics_msgs::GetPositionFK::Response &response, 
00297                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00298   {
00299     if(!checkLinkNames(request.fk_link_names,chain_info))
00300     {
00301       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
00302       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00303       return false;
00304     }
00305     if(!checkRobotState(request.robot_state,chain_info))
00306     {
00307       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00308       return false;
00309     }
00310     return true;
00311   }
00312 
00313   bool checkIKService(kinematics_msgs::GetPositionIK::Request &request, 
00314                       kinematics_msgs::GetPositionIK::Response &response,
00315                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00316   {
00317     if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00318     {
00319       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");      
00320       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00321       return false;
00322     }
00323     if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00324     {
00325       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00326       return false;
00327     }
00328     if(request.timeout <= ros::Duration(0.0))
00329       {
00330         response.error_code.val = response.error_code.INVALID_TIMEOUT;
00331         return false;
00332       }
00333     return true;
00334   }
00335 
00336   bool checkConstraintAwareIKService(kinematics_msgs::GetConstraintAwarePositionIK::Request &request, 
00337                       kinematics_msgs::GetConstraintAwarePositionIK::Response &response,
00338                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00339   {
00340     if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00341     {
00342       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");      
00343       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00344       return false;
00345     }
00346     if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00347     {
00348       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00349       return false;
00350     }
00351     if(request.timeout <= ros::Duration(0.0))
00352       {
00353         response.error_code.val = response.error_code.INVALID_TIMEOUT;
00354         return false;
00355       }
00356     return true;
00357   }
00358 
00359   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00360                               KDL::Frame &pose_kdl, 
00361                               const std::string &root_frame,
00362                               tf::TransformListener& tf)
00363   {
00364     geometry_msgs::PoseStamped pose_stamped;
00365     if(!convertPoseToRootFrame(pose_msg, pose_stamped, root_frame,tf))
00366       return false;
00367     tf::poseMsgToKDL(pose_stamped.pose, pose_kdl);
00368     return true;
00369   }
00370 
00371 
00372   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00373                               geometry_msgs::PoseStamped &pose_msg_out, 
00374                               const std::string &root_frame,
00375                               tf::TransformListener& tf)
00376   {
00377     geometry_msgs::PoseStamped pose_msg_in = pose_msg;
00378     ROS_DEBUG("Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
00379               pose_msg_in.header.frame_id.c_str(),
00380               pose_msg_in.pose.position.x,
00381               pose_msg_in.pose.position.y,
00382               pose_msg_in.pose.position.z,
00383               pose_msg_in.pose.orientation.x,
00384               pose_msg_in.pose.orientation.y,
00385               pose_msg_in.pose.orientation.z,
00386               pose_msg_in.pose.orientation.w);
00387     pose_msg_out = pose_msg;
00388     tf::Stamped<tf::Pose> pose_stamped;
00389     poseStampedMsgToTF(pose_msg_in, pose_stamped);
00390     
00391     if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
00392     {
00393       std::string err;    
00394       if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
00395       {
00396         ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'. TF said: %s",pose_stamped.frame_id_.c_str(),root_frame.c_str(), err.c_str());
00397         return false;
00398       }
00399     }    
00400     try
00401     {
00402       tf.transformPose(root_frame, pose_stamped, pose_stamped);
00403     }
00404     catch(...)
00405     {
00406       ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
00407       return false;
00408     } 
00409     tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);   
00410     return true;
00411   }
00412 
00413 
00414 
00415   int getJointIndex(const std::string &name,
00416                   const kinematics_msgs::KinematicSolverInfo &chain_info)
00417   {
00418     for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00419     {
00420       if(chain_info.joint_names[i] == name)
00421       {
00422           return i;
00423       }
00424     }
00425     return -1;
00426   }
00427 
00428   void getKDLChainInfo(const KDL::Chain &chain,
00429                        kinematics_msgs::KinematicSolverInfo &chain_info)
00430   {
00431     int i=0; // segment number
00432     while(i < (int)chain.getNrOfSegments())
00433     {
00434       chain_info.link_names.push_back(chain.getSegment(i).getName());
00435       i++;
00436     }
00437   }
00438 
00439   int getKDLSegmentIndex(const KDL::Chain &chain, 
00440                          const std::string &name)
00441   {
00442     int i=0; // segment number
00443     while(i < (int)chain.getNrOfSegments())
00444     {
00445       if(chain.getSegment(i).getName() == name)
00446       {
00447         return i+1;
00448       }
00449       i++;
00450     }
00451     return -1;   
00452   }
00453 
00454 
00455 }


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jan 2 2014 11:40:43