00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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,root_name_,tip_name,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 ROS_DEBUG("Loading KDL Tree");
00067 if(!getKDLChain(xml_string,root_name_,tip_name,kdl_chain_))
00068 {
00069 active_ = false;
00070 ROS_ERROR("Could not load kdl tree");
00071 }
00072 if(create_tf_listener) {
00073 tf_ = new TransformListener();
00074 } else {
00075 tf_ = NULL;
00076 }
00077
00078 ROS_DEBUG("Advertising services");
00079 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00080 node_handle_.param<int>("free_angle",free_angle_,2);
00081
00082 node_handle_.param<double>("search_discretization",search_discretization_,0.01);
00083 pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
00084 if(!pr2_arm_ik_solver_->active_)
00085 {
00086 ROS_ERROR("Could not load ik");
00087 active_ = false;
00088 }
00089 else
00090 {
00091
00092 pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00093 pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00094 fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00095
00096 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00097 {
00098 ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00099 }
00100 for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00101 {
00102 ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00103 }
00104 for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00105 {
00106 ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00107 }
00108 ROS_DEBUG("PR2Kinematics::active");
00109 active_ = true;
00110 fk_service_ = node_handle_.advertiseService(FK_SERVICE,&PR2ArmKinematics::getPositionFK,this);
00111 ik_service_ = node_handle_.advertiseService(IK_SERVICE,&PR2ArmKinematics::getPositionIK,this);
00112
00113 ik_solver_info_service_ = node_handle_.advertiseService(IK_INFO_SERVICE,&PR2ArmKinematics::getIKSolverInfo,this);
00114 fk_solver_info_service_ = node_handle_.advertiseService(FK_INFO_SERVICE,&PR2ArmKinematics::getFKSolverInfo,this);
00115
00116 }
00117 }
00118
00119 PR2ArmKinematics::~PR2ArmKinematics()
00120 {
00121 if(tf_ != NULL) {
00122 delete tf_;
00123 }
00124 }
00125
00126 bool PR2ArmKinematics::isActive()
00127 {
00128 if(active_)
00129 return true;
00130 return false;
00131 }
00132
00133 bool PR2ArmKinematics::getPositionIK(kinematics_msgs::GetPositionIK::Request &request,
00134 kinematics_msgs::GetPositionIK::Response &response)
00135 {
00136 if(!active_)
00137 {
00138 ROS_ERROR("IK service not active");
00139 return true;
00140 }
00141
00142 if(!checkIKService(request,response,ik_solver_info_))
00143 return true;
00144
00145 geometry_msgs::PoseStamped pose_msg_in = request.ik_request.pose_stamped;
00146 geometry_msgs::PoseStamped pose_msg_out;
00147
00148 if(tf_ != NULL) {
00149 if(!convertPoseToRootFrame(pose_msg_in,pose_msg_out,root_name_, *tf_))
00150 {
00151 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00152 return true;
00153 }
00154 } else {
00155 ROS_WARN_STREAM("No tf listener. Can't transform anything");
00156 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00157 return true;
00158 }
00159 request.ik_request.pose_stamped = pose_msg_out;
00160 return getPositionIKHelper(request, response);
00161 }
00162
00163
00164 bool PR2ArmKinematics::getPositionIKHelper(kinematics_msgs::GetPositionIK::Request &request,
00165 kinematics_msgs::GetPositionIK::Response &response)
00166 {
00167 KDL::Frame pose_desired;
00168 tf::PoseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired);
00169
00170
00171 KDL::JntArray jnt_pos_in;
00172 KDL::JntArray jnt_pos_out;
00173 jnt_pos_in.resize(dimension_);
00174 for(int i=0; i < dimension_; i++)
00175 {
00176 int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_);
00177 if(tmp_index >=0)
00178 {
00179 jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i];
00180 }
00181 else
00182 {
00183 ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str());
00184 }
00185 }
00186
00187 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00188 pose_desired,
00189 jnt_pos_out,
00190 request.timeout.toSec());
00191 if(ik_valid == pr2_arm_kinematics::TIMED_OUT)
00192 response.error_code.val = response.error_code.TIMED_OUT;
00193 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00194 response.error_code.val = response.error_code.NO_IK_SOLUTION;
00195
00196 response.solution.joint_state.header = request.ik_request.pose_stamped.header;
00197
00198 if(ik_valid >= 0)
00199 {
00200 response.solution.joint_state.name = ik_solver_info_.joint_names;
00201 response.solution.joint_state.position.resize(dimension_);
00202 for(int i=0; i < dimension_; i++)
00203 {
00204 response.solution.joint_state.position[i] = jnt_pos_out(i);
00205 ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i));
00206 }
00207 response.error_code.val = response.error_code.SUCCESS;
00208 return true;
00209 }
00210 else
00211 {
00212 ROS_DEBUG("An IK solution could not be found");
00213 return true;
00214 }
00215 }
00216
00217 bool PR2ArmKinematics::getIKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00218 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00219 {
00220 if(!active_)
00221 {
00222 ROS_ERROR("IK node not active");
00223 return true;
00224 }
00225 response.kinematic_solver_info = ik_solver_info_;
00226 return true;
00227 }
00228
00229 bool PR2ArmKinematics::getFKSolverInfo(kinematics_msgs::GetKinematicSolverInfo::Request &request,
00230 kinematics_msgs::GetKinematicSolverInfo::Response &response)
00231 {
00232 if(!active_)
00233 {
00234 ROS_ERROR("IK node not active");
00235 return true;
00236 }
00237 response.kinematic_solver_info = fk_solver_info_;
00238 return true;
00239 }
00240
00241 bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request,
00242 kinematics_msgs::GetPositionFK::Response &response)
00243 {
00244 if(!active_)
00245 {
00246 ROS_ERROR("FK service not active");
00247 return true;
00248 }
00249
00250 if(!checkFKService(request,response,fk_solver_info_))
00251 return true;
00252
00253 KDL::Frame p_out;
00254 KDL::JntArray jnt_pos_in;
00255 geometry_msgs::PoseStamped pose;
00256 tf::Stamped<tf::Pose> tf_pose;
00257
00258 jnt_pos_in.resize(dimension_);
00259 for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++)
00260 {
00261 int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_);
00262 if(tmp_index >=0)
00263 jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i];
00264 }
00265
00266 response.pose_stamped.resize(request.fk_link_names.size());
00267 response.fk_link_names.resize(request.fk_link_names.size());
00268
00269 bool valid = true;
00270 for(unsigned int i=0; i < request.fk_link_names.size(); i++)
00271 {
00272 ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i]));
00273 ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments());
00274 if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0)
00275 {
00276 tf_pose.frame_id_ = root_name_;
00277 tf_pose.stamp_ = ros::Time();
00278 tf::PoseKDLToTF(p_out,tf_pose);
00279 tf::poseStampedTFToMsg(tf_pose,pose);
00280 if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) {
00281 response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE;
00282 return false;
00283 }
00284 response.fk_link_names[i] = request.fk_link_names[i];
00285 response.error_code.val = response.error_code.SUCCESS;
00286 }
00287 else
00288 {
00289 ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str());
00290 response.error_code.val = response.error_code.NO_FK_SOLUTION;
00291 valid = false;
00292 }
00293 }
00294 return true;
00295 }
00296 bool PR2ArmKinematics::transformPose(const std::string& des_frame,
00297 const geometry_msgs::PoseStamped& pose_in,
00298 geometry_msgs::PoseStamped& pose_out)
00299 {
00300 if(tf_ != NULL) {
00301 try {
00302 tf_->transformPose(des_frame,pose_in,pose_out);
00303 }
00304 catch(...) {
00305 ROS_ERROR("Could not transform FK pose to frame: %s",des_frame.c_str());
00306 return false;
00307 }
00308 } else if(des_frame != root_name_){
00309 ROS_WARN_STREAM("No tf listener, can't transform to frame " << des_frame);
00310 return false;
00311 }
00312 return true;
00313 }
00314 }
