pr2_arm_kinematics.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /*
00031  * Author: Sachin Chitta
00032  */
00033 
00034 #include <pr2_arm_kinematics/pr2_arm_kinematics.h>
00035 #include <geometry_msgs/PoseStamped.h>
00036 #include <kdl_parser/kdl_parser.hpp>
00037 #include <tf_conversions/tf_kdl.h>
00038 #include "ros/ros.h"
00039 #include <algorithm>
00040 #include <numeric>
00041 
00042 
00043 using namespace KDL;
00044 using namespace tf;
00045 using namespace std;
00046 using namespace ros;
00047 
00048 namespace pr2_arm_kinematics {
00049 
00050 static const std::string IK_SERVICE = "get_ik";
00051 static const std::string FK_SERVICE = "get_fk";
00052 static const std::string IK_INFO_SERVICE = "get_ik_solver_info";
00053 static const std::string FK_INFO_SERVICE = "get_fk_solver_info";
00054 
00055 PR2ArmKinematics::PR2ArmKinematics(bool create_tf_listener):  node_handle_("~"),dimension_(7)
00056 {
00057   urdf::Model robot_model;
00058   std::string tip_name, xml_string;
00059 
00060   while(!loadRobotModel(node_handle_,robot_model, xml_string) && node_handle_.ok())
00061   {
00062     ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
00063     ros::Duration(0.5).sleep();
00064   }
00065 
00066   if (!node_handle_.getParam("root_name", root_name_)){
00067     ROS_FATAL("PR2IK: No root name found on parameter server");
00068     exit(-1);
00069   }
00070   if (!node_handle_.getParam("tip_name", tip_name)){
00071     ROS_FATAL("PR2IK: No tip name found on parameter server");
00072     exit(-1);
00073   }
00074 
00075   ROS_DEBUG("Loading KDL Tree");
00076   if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
00077   {
00078     active_ = false;
00079     ROS_ERROR("Could not load kdl tree");
00080   }
00081   if(create_tf_listener) {
00082     tf_ = new TransformListener();
00083   } else {
00084     tf_ = NULL;
00085   }
00086 
00087   ROS_DEBUG("Advertising services");
00088   jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00089   node_handle_.param<int>("free_angle",free_angle_,2);
00090 
00091   node_handle_.param<double>("search_discretization",search_discretization_,0.01);
00092   pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
00093   if(!pr2_arm_ik_solver_->active_)
00094   {
00095     ROS_ERROR("Could not load ik");
00096     active_ = false;
00097   }
00098   else
00099   {
00100 
00101     pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00102     pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00103     fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00104 
00105     for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00106     {
00107       ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00108     }
00109     for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00110     {
00111       ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00112     }
00113     for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00114     {
00115       ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00116     }
00117     ROS_DEBUG("PR2Kinematics::active");
00118     active_ = true;
00119     fk_service_ = node_handle_.advertiseService(FK_SERVICE,&PR2ArmKinematics::getPositionFK,this);
00120     ik_service_ = node_handle_.advertiseService(IK_SERVICE,&PR2ArmKinematics::getPositionIK,this);
00121 
00122     ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&PR2ArmKinematics::getIKSolverInfo,this);
00123     fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&PR2ArmKinematics::getFKSolverInfo,this);
00124 
00125   }
00126 }
00127 
00128 PR2ArmKinematics::~PR2ArmKinematics()
00129 {
00130   if(tf_ != NULL) {
00131     delete tf_;
00132   }
00133 }
00134 
00135 bool PR2ArmKinematics::isActive()
00136 {
00137   if(active_)
00138     return true;
00139   return false;
00140 }
00141 
00142 bool PR2ArmKinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request, 
00143                                      kinematics_msgs::GetPositionIK::Response &response)
00144 {
00145   if(!active_)
00146   {
00147     ROS_ERROR("IK service not active");
00148     return true;
00149   }
00150 
00151   if(!checkIKService(request,response,ik_solver_info_))
00152     return true;
00153 
00154   geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00155   geometry_msgs::PoseStamped pose_msg_out;
00156 
00157   if(tf_ != NULL) {
00158     if(!convertPoseToRootFrame(pose_msg_in,pose_msg_out,root_name_, *tf_))
00159     {
00160       response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00161       return true;
00162     }
00163   } else {
00164     ROS_WARN_STREAM("No tf listener.  Can't transform anything");
00165     response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00166     return true;
00167   }
00168   request.ik_request.pose_stamped = pose_msg_out;
00169   return getPositionIKHelper(request, response);
00170 }
00171 
00172 //this assumes that everything has been checked and is in the correct frame
00173 bool PR2ArmKinematics::getPositionIKHelper(kinematics_msgs::GetPositionIK::Request &request, 
00174                                            kinematics_msgs::GetPositionIK::Response &response)
00175 {
00176   KDL::Frame pose_desired;
00177   tf::poseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired);
00178 
00179   //Do the IK
00180   KDL::JntArray jnt_pos_in;
00181   KDL::JntArray jnt_pos_out;
00182   jnt_pos_in.resize(dimension_);
00183   for(int i=0; i < dimension_; i++)
00184   {
00185     int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
00186     if(tmp_index >=0)
00187     {
00188       jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
00189     }
00190     else
00191     {
00192       ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
00193     }
00194   }
00195   
00196   int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00197                                                      pose_desired,
00198                                                      jnt_pos_out,
00199                                                      request.timeout.toSec());
00200   if(ik_valid == pr2_arm_kinematics::TIMED_OUT)
00201     response.error_code.val = response.error_code.TIMED_OUT;
00202   if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00203     response.error_code.val = response.error_code.NO_IK_SOLUTION;
00204 
00205   response.solution.joint_state.header = request.ik_request.pose_stamped.header;
00206   
00207   if(ik_valid >= 0)
00208   {
00209     response.solution.joint_state.name = ik_solver_info_.joint_names;
00210     response.solution.joint_state.position.resize(dimension_);
00211     for(int i=0; i < dimension_; i++)
00212     {
00213       response.solution.joint_state.position[i] = jnt_pos_out(i);
00214       ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
00215     }
00216     response.error_code.val = response.error_code.SUCCESS;
00217     return true;
00218   }
00219   else
00220   {
00221     ROS_DEBUG("An IK solution could not be found");   
00222     return true;
00223   }
00224 }
00225 
00226 bool PR2ArmKinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, 
00227                                        kinematics_msgs::GetKinematicSolverInfo::Response &response)
00228 {
00229   if(!active_)
00230   {
00231     ROS_ERROR("IK node not active");
00232     return true;
00233   }
00234   response.kinematic_solver_info = ik_solver_info_;
00235   return true;
00236 }
00237 
00238 bool PR2ArmKinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request, 
00239                                        kinematics_msgs::GetKinematicSolverInfo::Response &response)
00240 {
00241   if(!active_)
00242   {
00243     ROS_ERROR("IK node not active");
00244     return true;
00245   }
00246   response.kinematic_solver_info = fk_solver_info_;
00247   return true;
00248 }
00249 
00250 bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request, 
00251                                      kinematics_msgs::GetPositionFK::Response &response)
00252 {
00253   if(!active_)
00254   {
00255     ROS_ERROR("FK service not active");
00256     return true;
00257   }
00258 
00259   if(!checkFKService(request,response,fk_solver_info_))
00260     return true;
00261 
00262   KDL::Frame p_out;
00263   KDL::JntArray jnt_pos_in;
00264   geometry_msgs::PoseStamped pose;
00265   tf::Stamped<tf::Pose> tf_pose;
00266 
00267   jnt_pos_in.resize(dimension_);
00268   for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++) 
00269   {
00270     int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
00271     if(tmp_index >=0)
00272       jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
00273   }
00274 
00275   response.pose_stamped.resize(request.fk_link_names.size());
00276   response.fk_link_names.resize(request.fk_link_names.size());
00277 
00278   bool valid = true;
00279   for(unsigned int i=0; i < request.fk_link_names.size(); i++)
00280   {
00281     ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
00282     ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
00283     if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
00284     {
00285       tf_pose.frame_id_ = root_name_;
00286       tf_pose.stamp_ = ros::Time();
00287       tf::PoseKDLToTF(p_out,tf_pose);
00288       tf::poseStampedTFToMsg(tf_pose,pose);
00289       if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
00290         response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00291         return false;
00292       }
00293       response.fk_link_names[i] = request.fk_link_names[i];
00294       response.error_code.val = response.error_code.SUCCESS;
00295     }
00296     else
00297     {
00298       ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
00299       response.error_code.val = response.error_code.NO_FK_SOLUTION;
00300       valid = false;
00301     }
00302   }
00303   return true;
00304 }
00305 bool PR2ArmKinematics::transformPose(const std::string& des_frame,
00306                                      const geometry_msgs::PoseStamped& pose_in,
00307                                      geometry_msgs::PoseStamped& pose_out)
00308 {
00309   if(tf_ != NULL) {
00310     try {
00311       tf_->transformPose(des_frame,pose_in,pose_out);
00312     }
00313     catch(...) {
00314       ROS_ERROR("Could not transform FK pose to frame: %s",des_frame.c_str());
00315       return false;
00316     }
00317   } else if(des_frame != root_name_){
00318     ROS_WARN_STREAM("No tf listener, can't transform to frame " << des_frame);
00319     return false;
00320   }
00321   return true;
00322 }
00323 } // namespace


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Tue Apr 22 2014 19:33:02