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