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_plugin.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 #include <pluginlib/class_list_macros.h>
00043
00044 using namespace KDL;
00045 using namespace tf;
00046 using namespace std;
00047 using namespace ros;
00048
00049
00050 PLUGINLIB_DECLARE_CLASS(pr2_arm_kinematics,PR2ArmKinematicsPlugin, pr2_arm_kinematics::PR2ArmKinematicsPlugin, kinematics::KinematicsBase)
00051
00052 namespace pr2_arm_kinematics {
00053
00054 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
00055
00056 bool PR2ArmKinematicsPlugin::isActive()
00057 {
00058 if(active_)
00059 return true;
00060 return false;
00061 }
00062
00063 bool PR2ArmKinematicsPlugin::initialize(std::string name)
00064 {
00065 urdf::Model robot_model;
00066 std::string tip_name, xml_string;
00067 ros::NodeHandle private_handle("~/"+name);
00068 dimension_ = 7;
00069 while(!loadRobotModel(private_handle,robot_model,root_name_,tip_name,xml_string) && private_handle.ok())
00070 {
00071 ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
00072 ros::Duration(0.5).sleep();
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 ROS_DEBUG("Advertising services");
00082 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00083 private_handle.param<int>("free_angle",free_angle_,2);
00084
00085 private_handle.param<double>("search_discretization",search_discretization_,0.01);
00086 pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model,root_name_,tip_name, search_discretization_,free_angle_));
00087 if(!pr2_arm_ik_solver_->active_)
00088 {
00089 ROS_ERROR("Could not load ik");
00090 active_ = false;
00091 }
00092 else
00093 {
00094
00095 pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00096 pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00097 fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00098
00099 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00100 {
00101 ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00102 }
00103 for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00104 {
00105 ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00106 }
00107 for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00108 {
00109 ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00110 }
00111 ROS_DEBUG("PR2KinematicsPlugin::active for %s",name.c_str());
00112 active_ = true;
00113 }
00114 return active_;
00115 }
00116
00117 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00118 const std::vector<double> &ik_seed_state,
00119 std::vector<double> &solution,
00120 int &error_code)
00121 {
00122 if(!active_)
00123 {
00124 ROS_ERROR("kinematics not active");
00125 error_code = kinematics::NO_IK_SOLUTION;
00126 return false;
00127 }
00128
00129 KDL::Frame pose_desired;
00130 tf::PoseMsgToKDL(ik_pose, pose_desired);
00131
00132
00133 KDL::JntArray jnt_pos_in;
00134 KDL::JntArray jnt_pos_out;
00135 jnt_pos_in.resize(dimension_);
00136 for(int i=0; i < dimension_; i++)
00137 {
00138 jnt_pos_in(i) = ik_seed_state[i];
00139 }
00140
00141 int ik_valid = pr2_arm_ik_solver_->CartToJnt(jnt_pos_in,
00142 pose_desired,
00143 jnt_pos_out);
00144 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00145 {
00146 error_code = kinematics::NO_IK_SOLUTION;
00147 return false;
00148 }
00149
00150 if(ik_valid >= 0)
00151 {
00152 solution.resize(dimension_);
00153 for(int i=0; i < dimension_; i++)
00154 {
00155 solution[i] = jnt_pos_out(i);
00156 }
00157 error_code = kinematics::SUCCESS;
00158 return true;
00159 }
00160 else
00161 {
00162 ROS_DEBUG("An IK solution could not be found");
00163 error_code = kinematics::NO_IK_SOLUTION;
00164 return false;
00165 }
00166 }
00167
00168 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00169 const std::vector<double> &ik_seed_state,
00170 const double &timeout,
00171 std::vector<double> &solution,
00172 int &error_code)
00173 {
00174 if(!active_)
00175 {
00176 ROS_ERROR("kinematics not active");
00177 error_code = kinematics::INACTIVE;
00178 return false;
00179 }
00180 KDL::Frame pose_desired;
00181 tf::PoseMsgToKDL(ik_pose, pose_desired);
00182
00183
00184 KDL::JntArray jnt_pos_in;
00185 KDL::JntArray jnt_pos_out;
00186 jnt_pos_in.resize(dimension_);
00187 for(int i=0; i < dimension_; i++)
00188 {
00189 jnt_pos_in(i) = ik_seed_state[i];
00190 }
00191
00192 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00193 pose_desired,
00194 jnt_pos_out,
00195 timeout);
00196 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00197 {
00198 error_code = kinematics::NO_IK_SOLUTION;
00199 return false;
00200 }
00201
00202 if(ik_valid >= 0)
00203 {
00204 solution.resize(dimension_);
00205 for(int i=0; i < dimension_; i++)
00206 {
00207 solution[i] = jnt_pos_out(i);
00208 }
00209 error_code = kinematics::SUCCESS;
00210 return true;
00211 }
00212 else
00213 {
00214 ROS_DEBUG("An IK solution could not be found");
00215 error_code = kinematics::NO_IK_SOLUTION;
00216 return false;
00217 }
00218 }
00219
00220 void PR2ArmKinematicsPlugin::desiredPoseCallback(const KDL::JntArray& jnt_array,
00221 const KDL::Frame& ik_pose,
00222 arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
00223 {
00224 std::vector<double> ik_seed_state;
00225 ik_seed_state.resize(dimension_);
00226 int int_error_code;
00227 for(int i=0; i < dimension_; i++)
00228 ik_seed_state[i] = jnt_array(i);
00229
00230 geometry_msgs::Pose ik_pose_msg;
00231 tf::PoseKDLToMsg(ik_pose,ik_pose_msg);
00232
00233 desiredPoseCallback_(ik_pose_msg,ik_seed_state,int_error_code);
00234 if(int_error_code)
00235 error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
00236 else
00237 error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;
00238 }
00239
00240
00241 void PR2ArmKinematicsPlugin::jointSolutionCallback(const KDL::JntArray& jnt_array,
00242 const KDL::Frame& ik_pose,
00243 arm_navigation_msgs::ArmNavigationErrorCodes& error_code)
00244 {
00245 std::vector<double> ik_seed_state;
00246 ik_seed_state.resize(dimension_);
00247 int int_error_code;
00248 for(int i=0; i < dimension_; i++)
00249 ik_seed_state[i] = jnt_array(i);
00250
00251 geometry_msgs::Pose ik_pose_msg;
00252 tf::PoseKDLToMsg(ik_pose,ik_pose_msg);
00253
00254 solutionCallback_(ik_pose_msg,ik_seed_state,int_error_code);
00255 if(int_error_code > 0)
00256 error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
00257 else
00258 error_code.val = arm_navigation_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;
00259 }
00260
00261 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00262 const std::vector<double> &ik_seed_state,
00263 const double &timeout,
00264 std::vector<double> &solution,
00265 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00266 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00267 int &error_code_int)
00268 {
00269 if(!active_)
00270 {
00271 ROS_ERROR("kinematics not active");
00272 error_code_int = kinematics::INACTIVE;
00273 return false;
00274 }
00275 KDL::Frame pose_desired;
00276 tf::PoseMsgToKDL(ik_pose, pose_desired);
00277
00278 desiredPoseCallback_ = desired_pose_callback;
00279 solutionCallback_ = solution_callback;
00280
00281
00282 KDL::JntArray jnt_pos_in;
00283 KDL::JntArray jnt_pos_out;
00284 jnt_pos_in.resize(dimension_);
00285 for(int i=0; i < dimension_; i++)
00286 {
00287 jnt_pos_in(i) = ik_seed_state[i];
00288 }
00289
00290 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00291 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00292 pose_desired,
00293 jnt_pos_out,
00294 timeout,
00295 error_code,
00296 boost::bind(&PR2ArmKinematicsPlugin::desiredPoseCallback, this, _1, _2, _3),
00297 boost::bind(&PR2ArmKinematicsPlugin::jointSolutionCallback, this, _1, _2, _3));
00298 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00299 return false;
00300
00301 if(ik_valid >= 0)
00302 {
00303 solution.resize(dimension_);
00304 for(int i=0; i < dimension_; i++)
00305 {
00306 solution[i] = jnt_pos_out(i);
00307 }
00308 error_code_int = kinematics::SUCCESS;
00309 return true;
00310 }
00311 else
00312 {
00313 ROS_DEBUG("An IK solution could not be found");
00314 error_code_int = error_code.val;
00315 return false;
00316 }
00317 }
00318
00319 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00320 const std::vector<double> &joint_angles,
00321 std::vector<geometry_msgs::Pose> &poses)
00322 {
00323 if(!active_)
00324 {
00325 ROS_ERROR("kinematics not active");
00326 return false;
00327 }
00328
00329 KDL::Frame p_out;
00330 KDL::JntArray jnt_pos_in;
00331 geometry_msgs::PoseStamped pose;
00332 tf::Stamped<tf::Pose> tf_pose;
00333
00334 jnt_pos_in.resize(dimension_);
00335 for(int i=0; i < dimension_; i++)
00336 {
00337 jnt_pos_in(i) = joint_angles[i];
00338 }
00339
00340 poses.resize(link_names.size());
00341
00342 bool valid = true;
00343 for(unsigned int i=0; i < poses.size(); i++)
00344 {
00345 ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i]));
00346 if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >=0)
00347 {
00348 tf::PoseKDLToMsg(p_out,poses[i]);
00349 }
00350 else
00351 {
00352 ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00353 valid = false;
00354 }
00355 }
00356 return valid;
00357 }
00358
00359 std::string PR2ArmKinematicsPlugin::getBaseFrame()
00360 {
00361 if(!active_)
00362 {
00363 ROS_ERROR("kinematics not active");
00364 return std::string("");
00365 }
00366 return root_name_;
00367 }
00368
00369 std::string PR2ArmKinematicsPlugin::getToolFrame()
00370 {
00371 if(!active_ || ik_solver_info_.link_names.empty())
00372 {
00373 ROS_ERROR("kinematics not active");
00374 return std::string("");
00375 }
00376 return ik_solver_info_.link_names[0];
00377 }
00378
00379 std::vector<std::string> PR2ArmKinematicsPlugin::getJointNames()
00380 {
00381 if(!active_)
00382 {
00383 std::vector<std::string> empty;
00384 ROS_ERROR("kinematics not active");
00385 return empty;
00386 }
00387 return ik_solver_info_.joint_names;
00388 }
00389
00390 std::vector<std::string> PR2ArmKinematicsPlugin::getLinkNames()
00391 {
00392 if(!active_)
00393 {
00394 std::vector<std::string> empty;
00395 ROS_ERROR("kinematics not active");
00396 return empty;
00397 }
00398 return fk_solver_info_.link_names;
00399 }
00400
00401 }
