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


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Wed Aug 26 2015 15:36:29