trac_ik_kinematics_plugin.cpp
Go to the documentation of this file.
00001 /********************************************************************************
00002 Copyright (c) 2015, TRACLabs, Inc.
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without modification,
00006  are permitted provided that the following conditions are met:
00007 
00008     1. Redistributions of source code must retain the above copyright notice,
00009        this list of conditions and the following disclaimer.
00010 
00011     2. Redistributions in binary form must reproduce the above copyright notice,
00012        this list of conditions and the following disclaimer in the documentation
00013        and/or other materials provided with the distribution.
00014 
00015     3. Neither the name of the copyright holder nor the names of its contributors
00016        may be used to endorse or promote products derived from this software
00017        without specific prior written permission.
00018 
00019 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00020 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
00022 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
00023 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00024 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00025 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
00026 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
00027 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
00028 OF THE POSSIBILITY OF SUCH DAMAGE.
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;  //Same as MoveIt's KDL plugin
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     // check for collisions if a callback is provided
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; // no collision check callback provided
00403   }
00404 
00405   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00406   return false;
00407 }
00408 
00409 
00410 
00411 } // end namespace
00412 
00413 //register TRAC_IKKinematicsPlugin as a KinematicsBase implementation
00414 #include <pluginlib/class_list_macros.h>
00415 PLUGINLIB_EXPORT_CLASS(trac_ik_kinematics_plugin::TRAC_IKKinematicsPlugin, kinematics::KinematicsBase);


trac_ik_kinematics_plugin
Author(s): Patrick Beeson
autogenerated on Thu Apr 25 2019 03:39:28