pr2_arm_kinematics.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.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <kdl_parser/kdl_parser.hpp>
00040 #include <tf_conversions/tf_kdl.h>
00041 #include "ros/ros.h"
00042 #include <algorithm>
00043 #include <numeric>
00044 
00045 
00046 using namespace KDL;
00047 using namespace tf;
00048 using namespace std;
00049 using namespace ros;
00050 
00051 namespace pr2_arm_kinematics {
00052 
00053 static const std::string IK_SERVICE = "get_ik";
00054 static const std::string FK_SERVICE = "get_fk";
00055 static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
00056 static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
00057 
00058 PR2ArmKinematics::PR2ArmKinematics(bool create_tf_listener):  node_handle_("~"),dimension_(7)
00059 {
00060   urdf::Model robot_model;
00061   std::string tip_name, xml_string;
00062 
00063   while(!loadRobotModel(node_handle_,robot_model, xml_string) && node_handle_.ok())
00064   {
00065     ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
00066     ros::Duration(0.5).sleep();
00067   }
00068 
00069   if (!node_handle_.getParam("root_name", root_name_)){
00070     ROS_FATAL("PR2IK: No root name found on parameter server");
00071     exit(-1);
00072   }
00073   if (!node_handle_.getParam("tip_name", tip_name)){
00074     ROS_FATAL("PR2IK: No tip name found on parameter server");
00075     exit(-1);
00076   }
00077 
00078   ROS_DEBUG("Loading KDL Tree");
00079   if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
00080   {
00081     active_ = false;
00082     ROS_ERROR("Could not load kdl tree");
00083   }
00084   if(create_tf_listener) {
00085     tf_ = new TransformListener();
00086   } else {
00087     tf_ = NULL;
00088   }
00089 
00090   ROS_DEBUG("Advertising services");
00091   jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00092   node_handle_.param<int>("free_angle",free_angle_,2);
00093 
00094   node_handle_.param<double>("search_discretization",search_discretization_,0.01);
00095   pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
00096   if(!pr2_arm_ik_solver_->active_)
00097   {
00098     ROS_ERROR("Could not load ik");
00099     active_ = false;
00100   }
00101   else
00102   {
00103 
00104     pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00105     pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00106     fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00107 
00108     for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00109     {
00110       ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00111     }
00112     for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00113     {
00114       ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00115     }
00116     for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00117     {
00118       ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00119     }
00120     ROS_DEBUG("PR2Kinematics::active");
00121     active_ = true;
00122     fk_service_ = node_handle_.advertiseService(FK_SERVICE,&PR2ArmKinematics::getPositionFK,this);
00123     ik_service_ = node_handle_.advertiseService(IK_SERVICE,&PR2ArmKinematics::getPositionIK,this);
00124 
00125     ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&PR2ArmKinematics::getIKSolverInfo,this);
00126     fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&PR2ArmKinematics::getFKSolverInfo,this);
00127 
00128   }
00129 }
00130 
00131 PR2ArmKinematics::~PR2ArmKinematics()
00132 {
00133   if(tf_ != NULL) {
00134     delete tf_;
00135   }
00136 }
00137 
00138 bool PR2ArmKinematics::isActive()
00139 {
00140   if(active_)
00141     return true;
00142   return false;
00143 }
00144 
00145 bool PR2ArmKinematics::getPositionIK(moveit_msgs::GetPositionIK::Request &request,
00146                                      moveit_msgs::GetPositionIK::Response &response)
00147 {
00148   if(!active_)
00149   {
00150     ROS_ERROR("IK service not active");
00151     return false;
00152   }
00153 
00154   if(!checkIKService(request,response,ik_solver_info_))
00155     return false;
00156 
00157   geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00158   geometry_msgs::PoseStamped pose_msg_out;
00159   if(tf_ != NULL) {
00160     if(!convertPoseToRootFrame(pose_msg_in,pose_msg_out,root_name_, *tf_))
00161     {
00162       response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00163       return true;
00164     }
00165   } else {
00166     ROS_WARN_STREAM("No tf listener.  Can't transform anything");
00167     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00168     return false;
00169   }
00170   request.ik_request.pose_stamped = pose_msg_out;
00171   return getPositionIKHelper(request, response);
00172 }
00173 
00174 //this assumes that everything has been checked and is in the correct frame
00175 bool PR2ArmKinematics::getPositionIKHelper(moveit_msgs::GetPositionIK::Request &request,
00176                                            moveit_msgs::GetPositionIK::Response &response)
00177 {
00178   KDL::Frame pose_desired;
00179   tf::PoseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired);
00180 
00181   //Do the IK
00182   KDL::JntArray jnt_pos_in;
00183   KDL::JntArray jnt_pos_out;
00184   jnt_pos_in.resize(dimension_);
00185   for(int i=0; i < dimension_; i++)
00186   {
00187     int tmp_index = getJointIndex(request.ik_request.robot_state.joint_state.name[i],ik_solver_info_);
00188     if(tmp_index >=0)
00189     {
00190       jnt_pos_in(tmp_index) = request.ik_request.robot_state.joint_state.position[i];
00191     }
00192     else
00193     {
00194       ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.robot_state.joint_state.name[i].c_str());
00195     }
00196   }
00197 
00198   std::vector<KDL::JntArray> jnt_array;
00199   jnt_array.push_back(jnt_pos_out);
00200   int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00201                                                      pose_desired,
00202                                                      jnt_array,
00203                                                      (const double)(request.ik_request.timeout.toSec()));
00204   if(ik_valid == pr2_arm_kinematics::TIMED_OUT)
00205     response.error_code.val = response.error_code.TIMED_OUT;
00206   if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00207     response.error_code.val = response.error_code.NO_IK_SOLUTION;
00208 
00209   response.solution.joint_state.header = request.ik_request.pose_stamped.header;
00210 
00211   if(ik_valid >= 0)
00212   {
00213     response.solution.joint_state.name = ik_solver_info_.joint_names;
00214     response.solution.joint_state.position.resize(dimension_);
00215     for(int i=0; i < dimension_; i++)
00216     {
00217       response.solution.joint_state.position[i] = jnt_array[0](i);
00218       ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_array[0](i));
00219     }
00220     response.error_code.val = response.error_code.SUCCESS;
00221     return true;
00222   }
00223   else
00224   {
00225     ROS_DEBUG("An IK solution could not be found");
00226     return false;
00227   }
00228 }
00229 
00230 bool PR2ArmKinematics::getIKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00231                                        moveit_msgs::GetKinematicSolverInfo::Response &response)
00232 {
00233   if (active_)
00234   {
00235     response.kinematic_solver_info = ik_solver_info_;
00236     return true;
00237   }
00238   ROS_ERROR("IK node not active");
00239   return false;
00240 }
00241 
00242 bool PR2ArmKinematics::getFKSolverInfo(moveit_msgs::GetKinematicSolverInfo::Request &request,
00243                                        moveit_msgs::GetKinematicSolverInfo::Response &response)
00244 {
00245   if(active_)
00246   {
00247     response.kinematic_solver_info = fk_solver_info_;
00248     return true;
00249   }
00250   ROS_ERROR("IK node not active");
00251   return false;
00252 }
00253 
00254 bool PR2ArmKinematics::getPositionFK(moveit_msgs::GetPositionFK::Request &request,
00255                                      moveit_msgs::GetPositionFK::Response &response)
00256 {
00257   if(!active_)
00258   {
00259     ROS_ERROR("FK service not active");
00260     return false;
00261   }
00262 
00263   if(!checkFKService(request,response,fk_solver_info_))
00264     return false;
00265 
00266   KDL::Frame p_out;
00267   KDL::JntArray jnt_pos_in;
00268   geometry_msgs::PoseStamped pose;
00269   tf::Stamped<tf::Pose> tf_pose;
00270 
00271   jnt_pos_in.resize(dimension_);
00272   for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++)
00273   {
00274     int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
00275     if(tmp_index >=0)
00276       jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
00277   }
00278 
00279   response.pose_stamped.resize(request.fk_link_names.size());
00280   response.fk_link_names.resize(request.fk_link_names.size());
00281 
00282   for(unsigned int i=0; i < request.fk_link_names.size(); i++)
00283   {
00284     ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
00285     ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
00286     if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
00287     {
00288       tf_pose.frame_id_ = root_name_;
00289       tf_pose.stamp_ = ros::Time();
00290       tf::PoseKDLToTF(p_out,tf_pose);
00291       tf::poseStampedTFToMsg(tf_pose,pose);
00292       if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
00293     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00294     return false;
00295       }
00296       response.fk_link_names[i] = request.fk_link_names[i];
00297       response.error_code.val = response.error_code.SUCCESS;
00298     }
00299     else
00300     {
00301       ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
00302       response.error_code.val = response.error_code.NO_IK_SOLUTION;
00303       return false;
00304     }
00305   }
00306   return true;
00307 }
00308 bool PR2ArmKinematics::transformPose(const std::string& des_frame,
00309                                      const geometry_msgs::PoseStamped& pose_in,
00310                                      geometry_msgs::PoseStamped& pose_out)
00311 {
00312   if(tf_ != NULL) {
00313     try {
00314       tf_->transformPose(des_frame,pose_in,pose_out);
00315     }
00316     catch(...) {
00317       ROS_ERROR("Could not transform FK pose to frame: %s",des_frame.c_str());
00318       return false;
00319     }
00320   } else if(des_frame != root_name_){
00321     ROS_WARN_STREAM("No tf listener, can't transform to frame " << des_frame);
00322     return false;
00323   }
00324   return true;
00325 }
00326 } // namespace


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jun 6 2019 20:11:52