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
00035
00036
00037 #include <moveit/pr2_arm_kinematics/pr2_arm_kinematics_plugin.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 #include <pluginlib/class_list_macros.h>
00046
00047 using namespace KDL;
00048 using namespace tf;
00049 using namespace std;
00050 using namespace ros;
00051
00052
00053 PLUGINLIB_EXPORT_CLASS(pr2_arm_kinematics::PR2ArmKinematicsPlugin, kinematics::KinematicsBase);
00054
00055 namespace pr2_arm_kinematics {
00056
00057 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
00058
00059 bool PR2ArmKinematicsPlugin::isActive()
00060 {
00061 if(active_)
00062 return true;
00063 return false;
00064 }
00065
00066 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
00067 const std::string& group_name,
00068 const std::string& base_frame,
00069 const std::string& tip_frame,
00070 double search_discretization)
00071 {
00072 setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00073 urdf::Model robot_model;
00074 std::string xml_string;
00075 ros::NodeHandle private_handle("~/"+group_name);
00076 dimension_ = 7;
00077 while(!loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
00078 {
00079 ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
00080 ros::Duration(0.5).sleep();
00081 }
00082
00083 ROS_DEBUG("Loading KDL Tree");
00084 if(!getKDLChain(xml_string,base_frame_,tip_frame_,kdl_chain_))
00085 {
00086 active_ = false;
00087 ROS_ERROR("Could not load kdl tree");
00088 }
00089 ROS_DEBUG("Advertising services");
00090 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00091 private_handle.param<int>("free_angle",free_angle_,2);
00092
00093 pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model, base_frame_,tip_frame_, search_discretization_,free_angle_));
00094 if(!pr2_arm_ik_solver_->active_)
00095 {
00096 ROS_ERROR("Could not load ik");
00097 active_ = false;
00098 }
00099 else
00100 {
00101
00102 pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00103 pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00104 fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00105
00106 for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00107 {
00108 ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00109 }
00110 for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00111 {
00112 ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00113 }
00114 for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00115 {
00116 ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00117 }
00118 ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
00119 active_ = true;
00120 }
00121 pr2_arm_ik_solver_->setFreeAngle(2);
00122 return active_;
00123 }
00124
00125 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00126 const std::vector<double> &ik_seed_state,
00127 std::vector<double> &solution,
00128 moveit_msgs::MoveItErrorCodes &error_code,
00129 const kinematics::KinematicsQueryOptions &options) const
00130 {
00131 if(!active_)
00132 {
00133 ROS_ERROR("kinematics not active");
00134 error_code.val = error_code.NO_IK_SOLUTION;
00135 return false;
00136 }
00137
00138 KDL::Frame pose_desired;
00139 tf::poseMsgToKDL(ik_pose, pose_desired);
00140
00141
00142 KDL::JntArray jnt_pos_in;
00143 KDL::JntArray jnt_pos_out;
00144 jnt_pos_in.resize(dimension_);
00145 for(int i=0; i < dimension_; i++)
00146 {
00147 jnt_pos_in(i) = ik_seed_state[i];
00148 }
00149
00150 int ik_valid = pr2_arm_ik_solver_->CartToJnt(jnt_pos_in,
00151 pose_desired,
00152 jnt_pos_out);
00153 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00154 {
00155 error_code.val = error_code.NO_IK_SOLUTION;
00156 return false;
00157 }
00158
00159 if(ik_valid >= 0)
00160 {
00161 solution.resize(dimension_);
00162 for(int i=0; i < dimension_; i++)
00163 {
00164 solution[i] = jnt_pos_out(i);
00165 }
00166 error_code.val = error_code.SUCCESS;
00167 return true;
00168 }
00169 else
00170 {
00171 ROS_DEBUG("An IK solution could not be found");
00172 error_code.val = error_code.NO_IK_SOLUTION;
00173 return false;
00174 }
00175 }
00176
00177 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00178 const std::vector<double> &ik_seed_state,
00179 double timeout,
00180 std::vector<double> &solution,
00181 moveit_msgs::MoveItErrorCodes &error_code,
00182 const kinematics::KinematicsQueryOptions &options) const
00183 {
00184 static IKCallbackFn solution_callback = 0;
00185 static std::vector<double> consistency_limits;
00186 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00187 }
00188
00189 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00190 const std::vector<double> &ik_seed_state,
00191 double timeout,
00192 const std::vector<double> &consistency_limits,
00193 std::vector<double> &solution,
00194 moveit_msgs::MoveItErrorCodes &error_code,
00195 const kinematics::KinematicsQueryOptions &options) const
00196 {
00197 static IKCallbackFn solution_callback = 0;
00198 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00199 }
00200
00201 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00202 const std::vector<double> &ik_seed_state,
00203 double timeout,
00204 std::vector<double> &solution,
00205 const IKCallbackFn &solution_callback,
00206 moveit_msgs::MoveItErrorCodes &error_code,
00207 const kinematics::KinematicsQueryOptions &options) const
00208 {
00209 static std::vector<double> consistency_limits;
00210 return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00211 }
00212
00213 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00214 const std::vector<double> &ik_seed_state,
00215 double timeout,
00216 const std::vector<double> &consistency_limits,
00217 std::vector<double> &solution,
00218 const IKCallbackFn &solution_callback,
00219 moveit_msgs::MoveItErrorCodes &error_code,
00220 const kinematics::KinematicsQueryOptions &options) const
00221 {
00222 if(!active_)
00223 {
00224 ROS_ERROR("kinematics not active");
00225 error_code.val = error_code.FAILURE;
00226 return false;
00227 }
00228 if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
00229 {
00230 ROS_ERROR("Consistency limits should be of size: %d",dimension_);
00231 error_code.val = error_code.FAILURE;
00232 return false;
00233 }
00234
00235 KDL::Frame pose_desired;
00236 tf::poseMsgToKDL(ik_pose, pose_desired);
00237
00238
00239 KDL::JntArray jnt_pos_in;
00240 KDL::JntArray jnt_pos_out;
00241 jnt_pos_in.resize(dimension_);
00242 for(int i=0; i < dimension_; i++)
00243 {
00244 jnt_pos_in(i) = ik_seed_state[i];
00245 }
00246
00247 int ik_valid;
00248 if(consistency_limits.empty())
00249 {
00250 ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00251 pose_desired,
00252 jnt_pos_out,
00253 timeout,
00254 error_code,
00255 solution_callback ?
00256 boost::bind(solution_callback, _1, _2, _3):
00257 IKCallbackFn());
00258 }
00259 else
00260 {
00261 ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00262 pose_desired,
00263 jnt_pos_out,
00264 timeout,
00265 consistency_limits[free_angle_],
00266 error_code,
00267 solution_callback ?
00268 boost::bind(solution_callback, _1, _2, _3):
00269 IKCallbackFn());
00270 }
00271
00272 if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00273 return false;
00274
00275 if(ik_valid >= 0)
00276 {
00277 solution.resize(dimension_);
00278 for(int i=0; i < dimension_; i++)
00279 {
00280 solution[i] = jnt_pos_out(i);
00281 }
00282 return true;
00283 }
00284 else
00285 {
00286 ROS_DEBUG("An IK solution could not be found");
00287 return false;
00288 }
00289 }
00290
00291 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00292 const std::vector<double> &joint_angles,
00293 std::vector<geometry_msgs::Pose> &poses) const
00294 {
00295 if(!active_)
00296 {
00297 ROS_ERROR("kinematics not active");
00298 return false;
00299 }
00300
00301 KDL::Frame p_out;
00302 KDL::JntArray jnt_pos_in;
00303 geometry_msgs::PoseStamped pose;
00304 tf::Stamped<tf::Pose> tf_pose;
00305
00306 jnt_pos_in.resize(dimension_);
00307 for(int i=0; i < dimension_; i++)
00308 {
00309 jnt_pos_in(i) = joint_angles[i];
00310
00311 }
00312
00313 poses.resize(link_names.size());
00314
00315 bool valid = true;
00316 for(unsigned int i=0; i < poses.size(); i++)
00317 {
00318
00319 if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >= 0)
00320 {
00321 tf::poseKDLToMsg(p_out,poses[i]);
00322 }
00323 else
00324 {
00325 ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00326 valid = false;
00327 }
00328 }
00329 return valid;
00330 }
00331
00332 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
00333 {
00334 if(!active_)
00335 {
00336 ROS_ERROR("kinematics not active");
00337 }
00338 return ik_solver_info_.joint_names;
00339 }
00340
00341 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
00342 {
00343 if(!active_)
00344 {
00345 ROS_ERROR("kinematics not active");
00346 }
00347 return fk_solver_info_.link_names;
00348 }
00349
00350 }