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 #include <ros/ros.h>
00033 #include <urdf/model.h>
00034 #include <tf_conversions/tf_kdl.h>
00035 #include <algorithm>
00036 #include <kdl/tree.hpp>
00037 #include <kdl_parser/kdl_parser.hpp>
00038 #include <trac_ik/trac_ik.hpp>
00039 #include <trac_ik/trac_ik_kinematics_plugin.hpp>
00040 #include <limits>
00041
00042 namespace trac_ik_kinematics_plugin
00043 {
00044
00045 bool TRAC_IKKinematicsPlugin::initialize(const std::string &robot_description,
00046 const std::string& group_name,
00047 const std::string& base_name,
00048 const std::string& tip_name,
00049 double search_discretization)
00050 {
00051 setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00052
00053 ros::NodeHandle node_handle("~");
00054
00055 urdf::Model robot_model;
00056 std::string xml_string;
00057
00058 std::string urdf_xml,full_urdf_xml;
00059 node_handle.param("urdf_xml",urdf_xml,robot_description);
00060 node_handle.searchParam(urdf_xml,full_urdf_xml);
00061
00062 ROS_DEBUG_NAMED("trac_ik","Reading xml file from parameter server");
00063 if (!node_handle.getParam(full_urdf_xml, xml_string))
00064 {
00065 ROS_FATAL_NAMED("trac_ik","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00066 return false;
00067 }
00068
00069 node_handle.param(full_urdf_xml,xml_string,std::string());
00070 robot_model.initString(xml_string);
00071
00072 ROS_DEBUG_STREAM_NAMED("trac_ik","Reading joints and links from URDF");
00073
00074 KDL::Tree tree;
00075
00076 if (!kdl_parser::treeFromUrdfModel(robot_model, tree)) {
00077 ROS_FATAL("Failed to extract kdl tree from xml robot description");
00078 return false;
00079 }
00080
00081 if(!tree.getChain(base_name, tip_name, chain)) {
00082 ROS_FATAL("Couldn't find chain %s to %s",base_name.c_str(),tip_name.c_str());
00083 return false;
00084 }
00085
00086 num_joints_=chain.getNrOfJoints();
00087
00088 std::vector<KDL::Segment> chain_segs = chain.segments;
00089
00090 boost::shared_ptr<const urdf::Joint> joint;
00091
00092 std::vector<double> l_bounds, u_bounds;
00093
00094 joint_min.resize(num_joints_);
00095 joint_max.resize(num_joints_);
00096
00097 uint joint_num=0;
00098 for(unsigned int i = 0; i < chain_segs.size(); ++i) {
00099
00100 link_names_.push_back(chain_segs[i].getName());
00101 joint = robot_model.getJoint(chain_segs[i].getJoint().getName());
00102 if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00103 joint_num++;
00104 assert(joint_num<=num_joints_);
00105 float lower, upper;
00106 int hasLimits;
00107 joint_names_.push_back(joint->name);
00108 if ( joint->type != urdf::Joint::CONTINUOUS ) {
00109 if(joint->safety) {
00110 lower = std::max(joint->limits->lower, joint->safety->soft_lower_limit);
00111 upper = std::min(joint->limits->upper, joint->safety->soft_upper_limit);
00112 } else {
00113 lower = joint->limits->lower;
00114 upper = joint->limits->upper;
00115 }
00116 hasLimits = 1;
00117 }
00118 else {
00119 hasLimits = 0;
00120 }
00121 if(hasLimits) {
00122 joint_min(joint_num-1)=lower;
00123 joint_max(joint_num-1)=upper;
00124 }
00125 else {
00126 joint_min(joint_num-1)=std::numeric_limits<float>::lowest();
00127 joint_max(joint_num-1)=std::numeric_limits<float>::max();
00128 }
00129 ROS_INFO_STREAM("IK Using joint "<<chain_segs[i].getName()<<" "<<joint_min(joint_num-1)<<" "<<joint_max(joint_num-1));
00130 }
00131 }
00132
00133 ROS_INFO_NAMED("trac-ik plugin","Looking in private handle: %s for param name: %s",
00134 node_handle.getNamespace().c_str(),
00135 (group_name+"/position_only_ik").c_str());
00136
00137 node_handle.param(group_name+"/position_only_ik", position_ik_, false);
00138
00139 ROS_INFO_NAMED("trac-ik plugin","Looking in private handle: %s for param name: %s",
00140 node_handle.getNamespace().c_str(),
00141 (group_name+"/solve_type").c_str());
00142
00143 node_handle.param(group_name+"/solve_type", solve_type, std::string("Speed"));
00144 ROS_INFO_NAMED("trac_ik plugin","Using solve type %s",solve_type.c_str());
00145
00146 active_ = true;
00147 return true;
00148 }
00149
00150
00151 int TRAC_IKKinematicsPlugin::getKDLSegmentIndex(const std::string &name) const
00152 {
00153 int i=0;
00154 while (i < (int)chain.getNrOfSegments()) {
00155 if (chain.getSegment(i).getName() == name) {
00156 return i+1;
00157 }
00158 i++;
00159 }
00160 return -1;
00161 }
00162
00163
00164 bool TRAC_IKKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00165 const std::vector<double> &joint_angles,
00166 std::vector<geometry_msgs::Pose> &poses) const
00167 {
00168 if(!active_)
00169 {
00170 ROS_ERROR_NAMED("trac_ik","kinematics not active");
00171 return false;
00172 }
00173 poses.resize(link_names.size());
00174 if(joint_angles.size() != num_joints_)
00175 {
00176 ROS_ERROR_NAMED("trac_ik","Joint angles vector must have size: %d",num_joints_);
00177 return false;
00178 }
00179
00180 KDL::Frame p_out;
00181 geometry_msgs::PoseStamped pose;
00182 tf::Stamped<tf::Pose> tf_pose;
00183
00184 KDL::JntArray jnt_pos_in(num_joints_);
00185 for(unsigned int i=0; i < num_joints_; i++)
00186 {
00187 jnt_pos_in(i) = joint_angles[i];
00188 }
00189
00190 KDL::ChainFkSolverPos_recursive fk_solver(chain);
00191
00192 bool valid = true;
00193 for(unsigned int i=0; i < poses.size(); i++)
00194 {
00195 ROS_DEBUG_NAMED("trac_ik","End effector index: %d",getKDLSegmentIndex(link_names[i]));
00196 if(fk_solver.JntToCart(jnt_pos_in,p_out,getKDLSegmentIndex(link_names[i])) >=0)
00197 {
00198 tf::poseKDLToMsg(p_out,poses[i]);
00199 }
00200 else
00201 {
00202 ROS_ERROR_NAMED("trac_ik","Could not compute FK for %s",link_names[i].c_str());
00203 valid = false;
00204 }
00205 }
00206
00207 return valid;
00208 }
00209
00210
00211 bool TRAC_IKKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00212 const std::vector<double> &ik_seed_state,
00213 std::vector<double> &solution,
00214 moveit_msgs::MoveItErrorCodes &error_code,
00215 const kinematics::KinematicsQueryOptions &options) const
00216 {
00217 const IKCallbackFn solution_callback = 0;
00218 std::vector<double> consistency_limits;
00219
00220 return searchPositionIK(ik_pose,
00221 ik_seed_state,
00222 default_timeout_,
00223 solution,
00224 solution_callback,
00225 error_code,
00226 consistency_limits,
00227 options);
00228 }
00229
00230 bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00231 const std::vector<double> &ik_seed_state,
00232 double timeout,
00233 std::vector<double> &solution,
00234 moveit_msgs::MoveItErrorCodes &error_code,
00235 const kinematics::KinematicsQueryOptions &options) const
00236 {
00237 const IKCallbackFn solution_callback = 0;
00238 std::vector<double> consistency_limits;
00239
00240 return searchPositionIK(ik_pose,
00241 ik_seed_state,
00242 timeout,
00243 solution,
00244 solution_callback,
00245 error_code,
00246 consistency_limits,
00247 options);
00248 }
00249
00250 bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00251 const std::vector<double> &ik_seed_state,
00252 double timeout,
00253 const std::vector<double> &consistency_limits,
00254 std::vector<double> &solution,
00255 moveit_msgs::MoveItErrorCodes &error_code,
00256 const kinematics::KinematicsQueryOptions &options) const
00257 {
00258 const IKCallbackFn solution_callback = 0;
00259 return searchPositionIK(ik_pose,
00260 ik_seed_state,
00261 timeout,
00262 solution,
00263 solution_callback,
00264 error_code,
00265 consistency_limits,
00266 options);
00267 }
00268
00269 bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00270 const std::vector<double> &ik_seed_state,
00271 double timeout,
00272 std::vector<double> &solution,
00273 const IKCallbackFn &solution_callback,
00274 moveit_msgs::MoveItErrorCodes &error_code,
00275 const kinematics::KinematicsQueryOptions &options) const
00276 {
00277 std::vector<double> consistency_limits;
00278 return searchPositionIK(ik_pose,
00279 ik_seed_state,
00280 timeout,
00281 solution,
00282 solution_callback,
00283 error_code,
00284 consistency_limits,
00285 options);
00286 }
00287
00288 bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00289 const std::vector<double> &ik_seed_state,
00290 double timeout,
00291 const std::vector<double> &consistency_limits,
00292 std::vector<double> &solution,
00293 const IKCallbackFn &solution_callback,
00294 moveit_msgs::MoveItErrorCodes &error_code,
00295 const kinematics::KinematicsQueryOptions &options) const
00296 {
00297 return searchPositionIK(ik_pose,
00298 ik_seed_state,
00299 timeout,
00300 solution,
00301 solution_callback,
00302 error_code,
00303 consistency_limits,
00304 options);
00305 }
00306
00307 bool TRAC_IKKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00308 const std::vector<double> &ik_seed_state,
00309 double timeout,
00310 std::vector<double> &solution,
00311 const IKCallbackFn &solution_callback,
00312 moveit_msgs::MoveItErrorCodes &error_code,
00313 const std::vector<double> &consistency_limits,
00314 const kinematics::KinematicsQueryOptions &options) const
00315 {
00316 ROS_DEBUG_STREAM_NAMED("trac_ik","getPositionIK");
00317
00318 if(!active_) {
00319 ROS_ERROR("kinematics not active");
00320 error_code.val = error_code.NO_IK_SOLUTION;
00321 return false;
00322 }
00323
00324 if (ik_seed_state.size() != num_joints_) {
00325 ROS_ERROR_STREAM_NAMED("trac_ik","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00326 error_code.val = error_code.NO_IK_SOLUTION;
00327 return false;
00328 }
00329
00330 KDL::Frame frame;
00331 tf::poseMsgToKDL(ik_pose,frame);
00332
00333 KDL::JntArray in(num_joints_), out(num_joints_);
00334
00335 for (uint z=0; z< num_joints_; z++)
00336 in(z)=ik_seed_state[z];
00337
00338 KDL::Twist bounds=KDL::Twist::Zero();
00339
00340 if (position_ik_) {
00341 bounds.rot.x(std::numeric_limits<float>::max());
00342 bounds.rot.y(std::numeric_limits<float>::max());
00343 bounds.rot.z(std::numeric_limits<float>::max());
00344 }
00345
00346 double epsilon = 1e-5;
00347
00348 TRAC_IK::SolveType solvetype;
00349
00350 if (solve_type == "Manipulation1")
00351 solvetype = TRAC_IK::Manip1;
00352 else if (solve_type == "Manipulation2")
00353 solvetype = TRAC_IK::Manip2;
00354 else if (solve_type == "Distance")
00355 solvetype = TRAC_IK::Distance;
00356 else {
00357 if (solve_type != "Speed") {
00358 ROS_WARN_STREAM_NAMED("trac_ik", solve_type << " is not a valid solve_type; setting to default: Speed");
00359 }
00360 solvetype = TRAC_IK::Speed;
00361 }
00362
00363 TRAC_IK::TRAC_IK ik_solver(chain, joint_min, joint_max, timeout, epsilon, solvetype);
00364
00365 int rc = ik_solver.CartToJnt(in, frame, out, bounds);
00366
00367
00368 solution.resize(num_joints_);
00369
00370 if (rc >=0) {
00371 for (uint z=0; z< num_joints_; z++)
00372 solution[z]=out(z);
00373
00374
00375 if( !solution_callback.empty() )
00376 {
00377 solution_callback(ik_pose, solution, error_code);
00378 if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00379 {
00380 ROS_DEBUG_STREAM_NAMED("trac_ik","Solution passes callback");
00381 return true;
00382 }
00383 else
00384 {
00385 ROS_DEBUG_STREAM_NAMED("trac_ik","Solution has error code " << error_code);
00386 return false;
00387 }
00388 }
00389 else
00390 return true;
00391 }
00392
00393 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00394 return false;
00395 }
00396
00397
00398
00399 }
00400
00401
00402 #include <pluginlib/class_list_macros.h>
00403 PLUGINLIB_EXPORT_CLASS(trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin, kinematics::KinematicsBase);