pr2_arm_kinematics: pr2_arm_kinematics: pr2_arm_kinematics

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 &root_name, std::string &tip_name, 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     if (!node_handle.getParam("root_name", root_name)){
00064       ROS_FATAL("PR2IK: No root name found on parameter server");
00065       return false;
00066     }
00067     if (!node_handle.getParam("tip_name", tip_name)){
00068       ROS_FATAL("PR2IK: No tip name found on parameter server");
00069       return false;
00070     }
00071     return true;
00072   }
00073 
00074   bool getKDLChain(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Chain &kdl_chain)
00075   {
00076     // create robot chain from root to tip
00077     KDL::Tree tree;
00078     if (!kdl_parser::treeFromString(xml_string, tree))
00079     {
00080       ROS_ERROR("Could not initialize tree object");
00081       return false;
00082     }
00083     if (!tree.getChain(root_name, tip_name, kdl_chain))
00084     {
00085       ROS_ERROR("Could not initialize chain object");
00086       return false;
00087     }
00088     return true;
00089   }
00090 
00091   bool getKDLTree(const std::string &xml_string, const std::string &root_name, const std::string &tip_name, KDL::Tree &kdl_tree)
00092   {
00093     // create robot chain from root to tip
00094     if (!kdl_parser::treeFromString(xml_string, kdl_tree))
00095     {
00096       ROS_ERROR("Could not initialize tree object");
00097       return false;
00098     }
00099     return true;
00100   }
00101 
00102 
00103   Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame &p)
00104   {
00105     Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
00106     for(int i=0; i < 3; i++)
00107     {
00108       for(int j=0; j<3; j++)
00109       {
00110         b(i,j) = p.M(i,j);
00111       }
00112       b(i,3) = p.p(i);
00113     }
00114     return b;
00115   }
00116 
00117   double computeEuclideanDistance(const std::vector<double> &array_1, const KDL::JntArray &array_2)
00118   {
00119     double distance = 0.0;
00120     for(int i=0; i< (int) array_1.size(); i++)
00121     {
00122       distance += (array_1[i] - array_2(i))*(array_1[i] - array_2(i));
00123     }
00124     return sqrt(distance);
00125   }
00126 
00127 
00128   double distance(const urdf::Pose &transform)
00129   {
00130     return sqrt(transform.position.x*transform.position.x+transform.position.y*transform.position.y+transform.position.z*transform.position.z);
00131   }
00132 
00133 
00134   bool solveQuadratic(const double &a, const double &b, const double &c, double *x1, double *x2)
00135   {
00136     double discriminant = b*b-4*a*c;
00137     if(fabs(a) < IK_EPS)
00138     {
00139       *x1 = -c/b;
00140       *x2 = *x1;
00141       return true;
00142     }
00143     //ROS_DEBUG("Discriminant: %f\n",discriminant);
00144     if (discriminant >= 0)
00145     {      
00146       *x1 = (-b + sqrt(discriminant))/(2*a); 
00147       *x2 = (-b - sqrt(discriminant))/(2*a);
00148       return true;
00149     } 
00150     else if(fabs(discriminant) < IK_EPS)
00151     {
00152       *x1 = -b/(2*a);
00153       *x2 = -b/(2*a);
00154       return true;
00155     }
00156     else
00157     {
00158       *x1 = -b/(2*a);
00159       *x2 = -b/(2*a);
00160       return false;
00161     }
00162   }
00163 
00164   Eigen::Matrix4f matrixInverse(const Eigen::Matrix4f &g)
00165   {
00166     Eigen::Matrix4f result = g;
00167     Eigen::Matrix3f Rt = Eigen::Matrix3f::Identity();
00168 
00169     Eigen::Vector3f p = Eigen::Vector3f::Zero(3);
00170     Eigen::Vector3f pinv = Eigen::Vector3f::Zero(3);
00171 
00172     Rt(0,0) = g(0,0);
00173     Rt(1,1) = g(1,1);
00174     Rt(2,2) = g(2,2);
00175 
00176     Rt(0,1) = g(1,0);
00177     Rt(1,0) = g(0,1);
00178 
00179     Rt(0,2) = g(2,0);
00180     Rt(2,0) = g(0,2);
00181 
00182     Rt(1,2) = g(2,1);
00183     Rt(2,1) = g(1,2);
00184 
00185     p(0) = g(0,3);
00186     p(1) = g(1,3);
00187     p(2) = g(2,3);
00188 
00189     pinv = -Rt*p;
00190 
00191     result(0,0) = g(0,0);
00192     result(1,1) = g(1,1);
00193     result(2,2) = g(2,2);
00194 
00195     result(0,1) = g(1,0);
00196     result(1,0) = g(0,1);
00197 
00198     result(0,2) = g(2,0);
00199     result(2,0) = g(0,2);
00200 
00201     result(1,2) = g(2,1);
00202     result(2,1) = g(1,2);
00203 
00204     result(0,3) = pinv(0);
00205     result(1,3) = pinv(1);
00206     result(2,3) = pinv(2);
00207   
00208     return result;
00209   }
00210 
00211 
00212   bool solveCosineEqn(const double &a, const double &b, const double &c, double &soln1, double &soln2)
00213   {
00214     double theta1 = atan2(b,a);
00215     double denom  = sqrt(a*a+b*b);
00216 
00217     if(fabs(denom) < IK_EPS) // should never happen, wouldn't make sense but make sure it is checked nonetheless
00218     {
00219 #ifdef DEBUG
00220       std::cout << "denom: " << denom << std::endl;
00221 #endif
00222       return false;
00223     }
00224     double rhs_ratio = c/denom;
00225     if(rhs_ratio < -1 || rhs_ratio > 1)
00226     {
00227 #ifdef DEBUG
00228       std::cout << "rhs_ratio: " << rhs_ratio << std::endl;
00229 #endif
00230       return false;
00231     }
00232     double acos_term = acos(rhs_ratio);
00233     soln1 = theta1 + acos_term;
00234     soln2 = theta1 - acos_term;
00235 
00236     return true;
00237   }
00238 
00239 
00240 bool checkJointNames(const std::vector<std::string> &joint_names,
00241                        const kinematics_msgs::KinematicSolverInfo &chain_info)
00242   {    
00243     for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00244     {
00245       int index = -1;
00246       for(unsigned int j=0; j < joint_names.size(); j++)
00247       {
00248         if(chain_info.joint_names[i] == joint_names[j])
00249         {
00250           index = j;
00251           break;
00252         }
00253       }
00254       if(index < 0)
00255       {
00256         ROS_ERROR("Joint state does not contain joint state for %s.",chain_info.joint_names[i].c_str());
00257         return false;
00258       }
00259     }
00260     return true;
00261   }
00262 
00263   bool checkLinkNames(const std::vector<std::string> &link_names,
00264                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00265   {
00266     if(link_names.empty())
00267       return false;
00268     for(unsigned int i=0; i < link_names.size(); i++)
00269     {
00270       if(!checkLinkName(link_names[i],chain_info))
00271         return false;
00272     }
00273     return true;   
00274   }
00275 
00276   bool checkLinkName(const std::string &link_name,
00277                    const kinematics_msgs::KinematicSolverInfo &chain_info)
00278   {
00279     for(unsigned int i=0; i < chain_info.link_names.size(); i++)
00280     {
00281       if(link_name == chain_info.link_names[i])
00282         return true;
00283     }
00284     return false;   
00285   }
00286 
00287   bool checkRobotState(arm_navigation_msgs::RobotState &robot_state,
00288                      const kinematics_msgs::KinematicSolverInfo &chain_info)
00289   {
00290     if((int) robot_state.joint_state.position.size() != (int) robot_state.joint_state.name.size())
00291     {
00292       ROS_ERROR("Number of joints in robot_state.joint_state does not match number of positions in robot_state.joint_state");
00293       return false;
00294     }    
00295     if(!checkJointNames(robot_state.joint_state.name,chain_info))
00296     {
00297       ROS_ERROR("Robot state must contain joint state for every joint in the kinematic chain");
00298       return false;
00299     }
00300     return true;
00301   }
00302 
00303   bool checkFKService(kinematics_msgs::GetPositionFK::Request &request, 
00304                       kinematics_msgs::GetPositionFK::Response &response, 
00305                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00306   {
00307     if(!checkLinkNames(request.fk_link_names,chain_info))
00308     {
00309       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");
00310       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00311       return false;
00312     }
00313     if(!checkRobotState(request.robot_state,chain_info))
00314     {
00315       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00316       return false;
00317     }
00318     return true;
00319   }
00320 
00321   bool checkIKService(kinematics_msgs::GetPositionIK::Request &request, 
00322                       kinematics_msgs::GetPositionIK::Response &response,
00323                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00324   {
00325     if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00326     {
00327       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");      
00328       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00329       return false;
00330     }
00331     if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00332     {
00333       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00334       return false;
00335     }
00336     if(request.timeout <= ros::Duration(0.0))
00337       {
00338         response.error_code.val = response.error_code.INVALID_TIMEOUT;
00339         return false;
00340       }
00341     return true;
00342   }
00343 
00344   bool checkConstraintAwareIKService(kinematics_msgs::GetConstraintAwarePositionIK::Request &request, 
00345                       kinematics_msgs::GetConstraintAwarePositionIK::Response &response,
00346                       const kinematics_msgs::KinematicSolverInfo &chain_info)
00347   {
00348     if(!checkLinkName(request.ik_request.ik_link_name,chain_info))
00349     {
00350       ROS_ERROR("Link name in service request does not match links that kinematics can provide solutions for.");      
00351       response.error_code.val = response.error_code.INVALID_LINK_NAME;
00352       return false;
00353     }
00354     if(!checkRobotState(request.ik_request.ik_seed_state,chain_info))
00355     {
00356       response.error_code.val = response.error_code.INVALID_ROBOT_STATE;
00357       return false;
00358     }
00359     if(request.timeout <= ros::Duration(0.0))
00360       {
00361         response.error_code.val = response.error_code.INVALID_TIMEOUT;
00362         return false;
00363       }
00364     return true;
00365   }
00366 
00367   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00368                               KDL::Frame &pose_kdl, 
00369                               const std::string &root_frame,
00370                               tf::TransformListener& tf)
00371   {
00372     geometry_msgs::PoseStamped pose_stamped;
00373     if(!convertPoseToRootFrame(pose_msg, pose_stamped, root_frame,tf))
00374       return false;
00375     tf::PoseMsgToKDL(pose_stamped.pose, pose_kdl);
00376     return true;
00377   }
00378 
00379 
00380   bool convertPoseToRootFrame(const geometry_msgs::PoseStamped &pose_msg, 
00381                               geometry_msgs::PoseStamped &pose_msg_out, 
00382                               const std::string &root_frame,
00383                               tf::TransformListener& tf)
00384   {
00385     geometry_msgs::PoseStamped pose_msg_in = pose_msg;
00386     ROS_DEBUG("Request:\nframe_id: %s\nPosition: %f %f %f\n:Orientation: %f %f %f %f\n",
00387               pose_msg_in.header.frame_id.c_str(),
00388               pose_msg_in.pose.position.x,
00389               pose_msg_in.pose.position.y,
00390               pose_msg_in.pose.position.z,
00391               pose_msg_in.pose.orientation.x,
00392               pose_msg_in.pose.orientation.y,
00393               pose_msg_in.pose.orientation.z,
00394               pose_msg_in.pose.orientation.w);
00395     pose_msg_out = pose_msg;
00396     tf::Stamped<tf::Pose> pose_stamped;
00397     poseStampedMsgToTF(pose_msg_in, pose_stamped);
00398     
00399     if (!tf.canTransform(root_frame, pose_stamped.frame_id_, pose_stamped.stamp_))
00400     {
00401       std::string err;    
00402       if (tf.getLatestCommonTime(pose_stamped.frame_id_, root_frame, pose_stamped.stamp_, &err) != tf::NO_ERROR)
00403       {
00404         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());
00405         return false;
00406       }
00407     }    
00408     try
00409     {
00410       tf.transformPose(root_frame, pose_stamped, pose_stamped);
00411     }
00412     catch(...)
00413     {
00414       ROS_ERROR("pr2_arm_ik:: Cannot transform from '%s' to '%s'",pose_stamped.frame_id_.c_str(),root_frame.c_str());
00415       return false;
00416     } 
00417     tf::poseStampedTFToMsg(pose_stamped,pose_msg_out);   
00418     return true;
00419   }
00420 
00421 
00422 
00423   int getJointIndex(const std::string &name,
00424                   const kinematics_msgs::KinematicSolverInfo &chain_info)
00425   {
00426     for(unsigned int i=0; i < chain_info.joint_names.size(); i++)
00427     {
00428       if(chain_info.joint_names[i] == name)
00429       {
00430           return i;
00431       }
00432     }
00433     return -1;
00434   }
00435 
00436   void getKDLChainInfo(const KDL::Chain &chain,
00437                        kinematics_msgs::KinematicSolverInfo &chain_info)
00438   {
00439     int i=0; // segment number
00440     while(i < (int)chain.getNrOfSegments())
00441     {
00442       chain_info.link_names.push_back(chain.getSegment(i).getName());
00443       i++;
00444     }
00445   }
00446 
00447   int getKDLSegmentIndex(const KDL::Chain &chain, 
00448                          const std::string &name)
00449   {
00450     int i=0; // segment number
00451     while(i < (int)chain.getNrOfSegments())
00452     {
00453       if(chain.getSegment(i).getName() == name)
00454       {
00455         return i+1;
00456       }
00457       i++;
00458     }
00459     return -1;   
00460   }
00461 
00462 
00463 }
pr2_arm_kinematics_utils.cpp
