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_INFO("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_INFO("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_INFO("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_INFO("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_INFO("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00110 }
00111 ROS_INFO("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 KDL::Frame pose_desired;
00129 tf::PoseMsgToKDL(ik_pose, pose_desired);
00130
00131
00132 KDL::JntArray jnt_pos_in;
00133 KDL::JntArray jnt_pos_out;
00134 jnt_pos_in.resize(dimension_);
00135 for(int i=0; i < dimension_; i++)
00136 {
00137 jnt_pos_in(i) = ik_seed_state[i];
00138 }
00139
00140 int ik_valid = pr2_arm_ik_solver_->CartToJnt(jnt_pos_in,
00141 pose_desired,
00142 jnt_pos_out);
00143 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00144 {
00145 error_code = kinematics::NO_IK_SOLUTION;
00146 return false;
00147 }
00148
00149 if(ik_valid >= 0)
00150 {
00151 solution.resize(dimension_);
00152 for(int i=0; i < dimension_; i++)
00153 {
00154 solution[i] = jnt_pos_out(i);
00155 }
00156 error_code = kinematics::SUCCESS;
00157 return true;
00158 }
00159 else
00160 {
00161 ROS_DEBUG("An IK solution could not be found");
00162 error_code = kinematics::NO_IK_SOLUTION;
00163 return false;
00164 }
00165 }
00166
00167 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00168 const std::vector<double> &ik_seed_state,
00169 const double &timeout,
00170 std::vector<double> &solution,
00171 int &error_code)
00172 {
00173 if(!active_)
00174 {
00175 ROS_ERROR("kinematics not active");
00176 error_code = kinematics::INACTIVE;
00177 return false;
00178 }
00179 KDL::Frame pose_desired;
00180 tf::PoseMsgToKDL(ik_pose, pose_desired);
00181
00182
00183 KDL::JntArray jnt_pos_in;
00184 KDL::JntArray jnt_pos_out;
00185 jnt_pos_in.resize(dimension_);
00186 for(int i=0; i < dimension_; i++)
00187 {
00188 jnt_pos_in(i) = ik_seed_state[i];
00189 }
00190
00191 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00192 pose_desired,
00193 jnt_pos_out,
00194 timeout);
00195 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00196 {
00197 error_code = kinematics::NO_IK_SOLUTION;
00198 return false;
00199 }
00200
00201 if(ik_valid >= 0)
00202 {
00203 solution.resize(dimension_);
00204 for(int i=0; i < dimension_; i++)
00205 {
00206 solution[i] = jnt_pos_out(i);
00207 }
00208 error_code = kinematics::SUCCESS;
00209 return true;
00210 }
00211 else
00212 {
00213 ROS_DEBUG("An IK solution could not be found");
00214 error_code = kinematics::NO_IK_SOLUTION;
00215 return false;
00216 }
00217 }
00218
00219 void PR2ArmKinematicsPlugin::desiredPoseCallback(const KDL::JntArray& jnt_array,
00220 const KDL::Frame& ik_pose,
00221 motion_planning_msgs::ArmNavigationErrorCodes& error_code)
00222 {
00223 std::vector<double> ik_seed_state;
00224 ik_seed_state.resize(dimension_);
00225 int int_error_code;
00226 for(int i=0; i < dimension_; i++)
00227 ik_seed_state[i] = jnt_array(i);
00228
00229 geometry_msgs::Pose ik_pose_msg;
00230 tf::PoseKDLToMsg(ik_pose,ik_pose_msg);
00231
00232 desiredPoseCallback_(ik_pose_msg,ik_seed_state,int_error_code);
00233 if(int_error_code)
00234 error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS;
00235 else
00236 error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;
00237 }
00238
00239
00240 void PR2ArmKinematicsPlugin::jointSolutionCallback(const KDL::JntArray& jnt_array,
00241 const KDL::Frame& ik_pose,
00242 motion_planning_msgs::ArmNavigationErrorCodes& error_code)
00243 {
00244 std::vector<double> ik_seed_state;
00245 ik_seed_state.resize(dimension_);
00246 int int_error_code;
00247 for(int i=0; i < dimension_; i++)
00248 ik_seed_state[i] = jnt_array(i);
00249
00250 geometry_msgs::Pose ik_pose_msg;
00251 tf::PoseKDLToMsg(ik_pose,ik_pose_msg);
00252
00253 solutionCallback_(ik_pose_msg,ik_seed_state,int_error_code);
00254 if(int_error_code > 0)
00255 error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS;
00256 else
00257 error_code.val = motion_planning_msgs::ArmNavigationErrorCodes::NO_IK_SOLUTION;
00258 }
00259
00260 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00261 const std::vector<double> &ik_seed_state,
00262 const double &timeout,
00263 std::vector<double> &solution,
00264 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback,
00265 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback,
00266 int &error_code_int)
00267 {
00268 if(!active_)
00269 {
00270 ROS_ERROR("kinematics not active");
00271 error_code_int = kinematics::INACTIVE;
00272 return false;
00273 }
00274 KDL::Frame pose_desired;
00275 tf::PoseMsgToKDL(ik_pose, pose_desired);
00276
00277 desiredPoseCallback_ = desired_pose_callback;
00278 solutionCallback_ = solution_callback;
00279
00280
00281 KDL::JntArray jnt_pos_in;
00282 KDL::JntArray jnt_pos_out;
00283 jnt_pos_in.resize(dimension_);
00284 for(int i=0; i < dimension_; i++)
00285 {
00286 jnt_pos_in(i) = ik_seed_state[i];
00287 }
00288
00289 motion_planning_msgs::ArmNavigationErrorCodes error_code;
00290 int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00291 pose_desired,
00292 jnt_pos_out,
00293 timeout,
00294 error_code,
00295 boost::bind(&PR2ArmKinematicsPlugin::desiredPoseCallback, this, _1, _2, _3),
00296 boost::bind(&PR2ArmKinematicsPlugin::jointSolutionCallback, this, _1, _2, _3));
00297 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00298 return false;
00299
00300 if(ik_valid >= 0)
00301 {
00302 solution.resize(dimension_);
00303 for(int i=0; i < dimension_; i++)
00304 {
00305 solution[i] = jnt_pos_out(i);
00306 }
00307 error_code_int = kinematics::SUCCESS;
00308 return true;
00309 }
00310 else
00311 {
00312 ROS_DEBUG("An IK solution could not be found");
00313 error_code_int = error_code.val;
00314 return false;
00315 }
00316 }
00317
00318 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00319 const std::vector<double> &joint_angles,
00320 std::vector<geometry_msgs::Pose> &poses)
00321 {
00322 if(!active_)
00323 {
00324 ROS_ERROR("kinematics not active");
00325 return false;
00326 }
00327
00328 KDL::Frame p_out;
00329 KDL::JntArray jnt_pos_in;
00330 geometry_msgs::PoseStamped pose;
00331 tf::Stamped<tf::Pose> tf_pose;
00332
00333 jnt_pos_in.resize(dimension_);
00334 for(int i=0; i < dimension_; i++)
00335 {
00336 jnt_pos_in(i) = joint_angles[i];
00337 }
00338
00339 poses.resize(link_names.size());
00340
00341 bool valid = true;
00342 for(unsigned int i=0; i < poses.size(); i++)
00343 {
00344 ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i]));
00345 if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >=0)
00346 {
00347 tf::PoseKDLToMsg(p_out,poses[i]);
00348 }
00349 else
00350 {
00351 ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00352 valid = false;
00353 }
00354 }
00355 return valid;
00356 }
00357
00358 std::string PR2ArmKinematicsPlugin::getBaseFrame()
00359 {
00360 if(!active_)
00361 {
00362 ROS_ERROR("kinematics not active");
00363 return std::string("");
00364 }
00365 return root_name_;
00366 }
00367
00368 std::string PR2ArmKinematicsPlugin::getToolFrame()
00369 {
00370 if(!active_ || ik_solver_info_.link_names.empty())
00371 {
00372 ROS_ERROR("kinematics not active");
00373 return std::string("");
00374 }
00375 return ik_solver_info_.link_names[0];
00376 }
00377
00378 std::vector<std::string> PR2ArmKinematicsPlugin::getJointNames()
00379 {
00380 if(!active_)
00381 {
00382 std::vector<std::string> empty;
00383 ROS_ERROR("kinematics not active");
00384 return empty;
00385 }
00386 return ik_solver_info_.joint_names;
00387 }
00388
00389 std::vector<std::string> PR2ArmKinematicsPlugin::getLinkNames()
00390 {
00391 if(!active_)
00392 {
00393 std::vector<std::string> empty;
00394 ROS_ERROR("kinematics not active");
00395 return empty;
00396 }
00397 return fk_solver_info_.link_names;
00398 }
00399
00400 }
