ikfast61_moveit_plugin_template.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  * Copyright (c) 2013, Dave Coleman, CU Boulder; Jeremy Zoss, SwRI; David Butterworth, KAIST; Mathias L├╝dtke, Fraunhofer
00006  *IPA
00007  * All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *     * Redistributions of source code must retain the above copyright
00014  *       notice, this list of conditions and the following disclaimer.
00015  *     * Redistributions in binary form must reproduce the above copyright
00016  *       notice, this list of conditions and the following disclaimer in the
00017  *       documentation and/or other materials provided with the distribution.
00018  *     * Neither the name of the all of the author's companies nor the names of its
00019  *       contributors may be used to endorse or promote products derived from
00020  *       this software without specific prior written permission.
00021  *
00022  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00023  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00024  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00025  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00026  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00027  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00028  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00029  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00030  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00031  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  *********************************************************************/
00035 
00036 /*
00037  * IKFast Plugin Template for moveit
00038  *
00039  * AUTO-GENERATED by create_ikfast_moveit_plugin.py in arm_kinematics_tools
00040  * You should run create_ikfast_moveit_plugin.py to generate your own plugin.
00041  *
00042  * Creates a kinematics plugin using the output of IKFast from OpenRAVE.
00043  * This plugin and the move_group node can be used as a general
00044  * kinematics service, from within the moveit planning environment, or in
00045  * your own ROS node.
00046  *
00047  */
00048 
00049 #include <ros/ros.h>
00050 #include <moveit/kinematics_base/kinematics_base.h>
00051 #include <urdf/model.h>
00052 #include <tf_conversions/tf_kdl.h>
00053 
00054 // Need a floating point tolerance when checking joint limits, in case the joint starts at limit
00055 const double LIMIT_TOLERANCE = .0000001;
00057 enum SEARCH_MODE
00058 {
00059   OPTIMIZE_FREE_JOINT = 1,
00060   OPTIMIZE_MAX_JOINT = 2
00061 };
00062 
00063 namespace ikfast_kinematics_plugin
00064 {
00065 #define IKFAST_NO_MAIN  // Don't include main() from IKFast
00066 
00072 enum IkParameterizationType
00073 {
00074   IKP_None = 0,
00075   IKP_Transform6D = 0x67000001,    
00076   IKP_Rotation3D = 0x34000002,     
00077   IKP_Translation3D = 0x33000003,  
00078   IKP_Direction3D = 0x23000004,    
00079   IKP_Ray4D = 0x46000005,          
00080   IKP_Lookat3D = 0x23000006,       
00081   IKP_TranslationDirection5D = 0x56000007,  
00082 
00083 
00084   IKP_TranslationXY2D = 0x22000008,             
00085   IKP_TranslationXYOrientation3D = 0x33000009,  
00086 
00087 
00088   IKP_TranslationLocalGlobal6D = 0x3600000a,  
00089 
00090   IKP_TranslationXAxisAngle4D = 0x4400000b,  
00091 
00092 
00093   IKP_TranslationYAxisAngle4D = 0x4400000c,  
00094 
00095 
00096   IKP_TranslationZAxisAngle4D = 0x4400000d,  
00097 
00098 
00099 
00100   IKP_TranslationXAxisAngleZNorm4D = 0x4400000e,  
00101 
00102 
00103 
00104   IKP_TranslationYAxisAngleXNorm4D = 0x4400000f,  
00105 
00106 
00107 
00108   IKP_TranslationZAxisAngleYNorm4D = 0x44000010,  
00109 
00110 
00111 
00112 
00113   IKP_NumberOfParameterizations = 16,  
00114 
00115   IKP_VelocityDataBit =
00116       0x00008000,  
00117   IKP_Transform6DVelocity = IKP_Transform6D | IKP_VelocityDataBit,
00118   IKP_Rotation3DVelocity = IKP_Rotation3D | IKP_VelocityDataBit,
00119   IKP_Translation3DVelocity = IKP_Translation3D | IKP_VelocityDataBit,
00120   IKP_Direction3DVelocity = IKP_Direction3D | IKP_VelocityDataBit,
00121   IKP_Ray4DVelocity = IKP_Ray4D | IKP_VelocityDataBit,
00122   IKP_Lookat3DVelocity = IKP_Lookat3D | IKP_VelocityDataBit,
00123   IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D | IKP_VelocityDataBit,
00124   IKP_TranslationXY2DVelocity = IKP_TranslationXY2D | IKP_VelocityDataBit,
00125   IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D | IKP_VelocityDataBit,
00126   IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D | IKP_VelocityDataBit,
00127   IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D | IKP_VelocityDataBit,
00128   IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D | IKP_VelocityDataBit,
00129   IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D | IKP_VelocityDataBit,
00130   IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D | IKP_VelocityDataBit,
00131   IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D | IKP_VelocityDataBit,
00132   IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D | IKP_VelocityDataBit,
00133 
00134   IKP_UniqueIdMask = 0x0000ffff,   
00135   IKP_CustomDataBit = 0x00010000,  
00136 
00137 };
00138 
00139 // Code generated by IKFast56/61
00140 #include "_ROBOT_NAME___GROUP_NAME__ikfast_solver.cpp"
00141 
00142 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00143 {
00144   std::vector<std::string> joint_names_;
00145   std::vector<double> joint_min_vector_;
00146   std::vector<double> joint_max_vector_;
00147   std::vector<bool> joint_has_limits_vector_;
00148   std::vector<std::string> link_names_;
00149   size_t num_joints_;
00150   std::vector<int> free_params_;
00151   bool active_;  // Internal variable that indicates whether solvers are configured and ready
00152 
00153   const std::vector<std::string>& getJointNames() const
00154   {
00155     return joint_names_;
00156   }
00157   const std::vector<std::string>& getLinkNames() const
00158   {
00159     return link_names_;
00160   }
00161 
00162 public:
00166   IKFastKinematicsPlugin() : active_(false)
00167   {
00168     srand(time(NULL));
00169     supported_methods_.push_back(kinematics::DiscretizationMethods::NO_DISCRETIZATION);
00170     supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_DISCRETIZED);
00171     supported_methods_.push_back(kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED);
00172   }
00173 
00183   // Returns the first IK solution that is within joint limits, this is called by get_ik() service
00184   bool getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00185                      std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00186                      const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
00187 
00203   bool getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
00204                      std::vector<std::vector<double> >& solutions, kinematics::KinematicsResult& result,
00205                      const kinematics::KinematicsQueryOptions& options) const;
00206 
00215   bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00216                         std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00217                         const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
00218 
00228   bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00229                         const std::vector<double>& consistency_limits, std::vector<double>& solution,
00230                         moveit_msgs::MoveItErrorCodes& error_code,
00231                         const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
00232 
00241   bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00242                         std::vector<double>& solution, const IKCallbackFn& solution_callback,
00243                         moveit_msgs::MoveItErrorCodes& error_code,
00244                         const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
00245 
00256   bool searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state, double timeout,
00257                         const std::vector<double>& consistency_limits, std::vector<double>& solution,
00258                         const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code,
00259                         const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions()) const;
00260 
00269   bool getPositionFK(const std::vector<std::string>& link_names, const std::vector<double>& joint_angles,
00270                      std::vector<geometry_msgs::Pose>& poses) const;
00271 
00281   void setSearchDiscretization(const std::map<int, double>& discretization);
00282 
00286   bool setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices);
00287 
00288 private:
00289   bool initialize(const std::string& robot_description, const std::string& group_name, const std::string& base_name,
00290                   const std::string& tip_name, double search_discretization);
00291 
00296   int solve(KDL::Frame& pose_frame, const std::vector<double>& vfree, IkSolutionList<IkReal>& solutions) const;
00297 
00301   void getSolution(const IkSolutionList<IkReal>& solutions, int i, std::vector<double>& solution) const;
00302 
00303   double harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const;
00304   // void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
00305   void getClosestSolution(const IkSolutionList<IkReal>& solutions, const std::vector<double>& ik_seed_state,
00306                           std::vector<double>& solution) const;
00307   void fillFreeParams(int count, int* array);
00308   bool getCount(int& count, const int& max_count, const int& min_count) const;
00309 
00316   bool sampleRedundantJoint(kinematics::DiscretizationMethod method, std::vector<double>& sampled_joint_vals) const;
00317 
00318 };  // end class
00319 
00320 bool IKFastKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
00321                                         const std::string& base_name, const std::string& tip_name,
00322                                         double search_discretization)
00323 {
00324   setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00325 
00326   ros::NodeHandle node_handle("~/" + group_name);
00327 
00328   std::string robot;
00329   node_handle.param("robot", robot, std::string());
00330 
00331   // IKFast56/61
00332   fillFreeParams(GetNumFreeParameters(), GetFreeParameters());
00333   num_joints_ = GetNumJoints();
00334 
00335   if (free_params_.size() > 1)
00336   {
00337     ROS_FATAL("Only one free joint parameter supported!");
00338     return false;
00339   }
00340   else if (free_params_.size() == 1)
00341   {
00342     redundant_joint_indices_.clear();
00343     redundant_joint_indices_.push_back(free_params_[0]);
00344     KinematicsBase::setSearchDiscretization(DEFAULT_SEARCH_DISCRETIZATION);
00345   }
00346 
00347   urdf::Model robot_model;
00348   std::string xml_string;
00349 
00350   std::string urdf_xml, full_urdf_xml;
00351   node_handle.param("urdf_xml", urdf_xml, robot_description);
00352   node_handle.searchParam(urdf_xml, full_urdf_xml);
00353 
00354   ROS_DEBUG_NAMED("ikfast", "Reading xml file from parameter server");
00355   if (!node_handle.getParam(full_urdf_xml, xml_string))
00356   {
00357     ROS_FATAL_NAMED("ikfast", "Could not load the xml from parameter server: %s", urdf_xml.c_str());
00358     return false;
00359   }
00360 
00361   node_handle.param(full_urdf_xml, xml_string, std::string());
00362   robot_model.initString(xml_string);
00363 
00364   ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF");
00365 
00366   urdf::LinkConstSharedPtr link = robot_model.getLink(getTipFrame());
00367   while (link->name != base_frame_ && joint_names_.size() <= num_joints_)
00368   {
00369     ROS_DEBUG_NAMED("ikfast", "Link %s", link->name.c_str());
00370     link_names_.push_back(link->name);
00371     urdf::JointSharedPtr joint = link->parent_joint;
00372     if (joint)
00373     {
00374       if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00375       {
00376         ROS_DEBUG_STREAM_NAMED("ikfast", "Adding joint " << joint->name);
00377 
00378         joint_names_.push_back(joint->name);
00379         float lower, upper;
00380         int hasLimits;
00381         if (joint->type != urdf::Joint::CONTINUOUS)
00382         {
00383           if (joint->safety)
00384           {
00385             lower = joint->safety->soft_lower_limit;
00386             upper = joint->safety->soft_upper_limit;
00387           }
00388           else
00389           {
00390             lower = joint->limits->lower;
00391             upper = joint->limits->upper;
00392           }
00393           hasLimits = 1;
00394         }
00395         else
00396         {
00397           lower = -M_PI;
00398           upper = M_PI;
00399           hasLimits = 0;
00400         }
00401         if (hasLimits)
00402         {
00403           joint_has_limits_vector_.push_back(true);
00404           joint_min_vector_.push_back(lower);
00405           joint_max_vector_.push_back(upper);
00406         }
00407         else
00408         {
00409           joint_has_limits_vector_.push_back(false);
00410           joint_min_vector_.push_back(-M_PI);
00411           joint_max_vector_.push_back(M_PI);
00412         }
00413       }
00414     }
00415     else
00416     {
00417       ROS_WARN_NAMED("ikfast", "no joint corresponding to %s", link->name.c_str());
00418     }
00419     link = link->getParent();
00420   }
00421 
00422   if (joint_names_.size() != num_joints_)
00423   {
00424     ROS_FATAL_STREAM_NAMED("ikfast", "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has "
00425                                                                          << num_joints_);
00426     return false;
00427   }
00428 
00429   std::reverse(link_names_.begin(), link_names_.end());
00430   std::reverse(joint_names_.begin(), joint_names_.end());
00431   std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
00432   std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
00433   std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00434 
00435   for (size_t i = 0; i < num_joints_; ++i)
00436     ROS_DEBUG_STREAM_NAMED("ikfast", joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]
00437                                                      << " " << joint_has_limits_vector_[i]);
00438 
00439   active_ = true;
00440   return true;
00441 }
00442 
00443 void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<int, double>& discretization)
00444 {
00445   if (discretization.empty())
00446   {
00447     ROS_ERROR("The 'discretization' map is empty");
00448     return;
00449   }
00450 
00451   if (redundant_joint_indices_.empty())
00452   {
00453     ROS_ERROR_STREAM("This group's solver doesn't support redundant joints");
00454     return;
00455   }
00456 
00457   if (discretization.begin()->first != redundant_joint_indices_[0])
00458   {
00459     std::string redundant_joint = joint_names_[free_params_[0]];
00460     ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "
00461                      << discretization.begin()->first << ", only joint '" << redundant_joint << "' with index "
00462                      << redundant_joint_indices_[0] << " is redundant.");
00463     return;
00464   }
00465 
00466   if (discretization.begin()->second <= 0.0)
00467   {
00468     ROS_ERROR_STREAM("Discretization can not takes values that are <= 0");
00469     return;
00470   }
00471 
00472   redundant_joint_discretization_.clear();
00473   redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
00474 }
00475 
00476 bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
00477 {
00478   ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver ");
00479   return false;
00480 }
00481 
00482 int IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector<double>& vfree,
00483                                   IkSolutionList<IkReal>& solutions) const
00484 {
00485   // IKFast56/61
00486   solutions.Clear();
00487 
00488   double trans[3];
00489   trans[0] = pose_frame.p[0];  //-.18;
00490   trans[1] = pose_frame.p[1];
00491   trans[2] = pose_frame.p[2];
00492 
00493   KDL::Rotation mult;
00494   KDL::Vector direction;
00495 
00496   switch (GetIkType())
00497   {
00498     case IKP_Transform6D:
00499     case IKP_Translation3D:
00500       // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
00501 
00502       mult = pose_frame.M;
00503 
00504       double vals[9];
00505       vals[0] = mult(0, 0);
00506       vals[1] = mult(0, 1);
00507       vals[2] = mult(0, 2);
00508       vals[3] = mult(1, 0);
00509       vals[4] = mult(1, 1);
00510       vals[5] = mult(1, 2);
00511       vals[6] = mult(2, 0);
00512       vals[7] = mult(2, 1);
00513       vals[8] = mult(2, 2);
00514 
00515       // IKFast56/61
00516       ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00517       return solutions.GetNumSolutions();
00518 
00519     case IKP_Direction3D:
00520     case IKP_Ray4D:
00521     case IKP_TranslationDirection5D:
00522       // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
00523       // direction.
00524 
00525       direction = pose_frame.M * KDL::Vector(0, 0, 1);
00526       ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00527       return solutions.GetNumSolutions();
00528 
00529     case IKP_TranslationXAxisAngle4D:
00530     case IKP_TranslationYAxisAngle4D:
00531     case IKP_TranslationZAxisAngle4D:
00532       // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value
00533       // represents the angle.
00534       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00535       return 0;
00536 
00537     case IKP_TranslationLocalGlobal6D:
00538       // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
00539       // effector coordinate system.
00540       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00541       return 0;
00542 
00543     case IKP_Rotation3D:
00544     case IKP_Lookat3D:
00545     case IKP_TranslationXY2D:
00546     case IKP_TranslationXYOrientation3D:
00547     case IKP_TranslationXAxisAngleZNorm4D:
00548     case IKP_TranslationYAxisAngleXNorm4D:
00549     case IKP_TranslationZAxisAngleYNorm4D:
00550       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00551       return 0;
00552 
00553     default:
00554       ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version "
00555                                 "of Openrave?");
00556       return 0;
00557   }
00558 }
00559 
00560 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
00561                                          std::vector<double>& solution) const
00562 {
00563   solution.clear();
00564   solution.resize(num_joints_);
00565 
00566   // IKFast56/61
00567   const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00568   std::vector<IkReal> vsolfree(sol.GetFree().size());
00569   sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
00570 
00571   // std::cout << "solution " << i << ":" ;
00572   // for(int j=0;j<num_joints_; ++j)
00573   //   std::cout << " " << solution[j];
00574   // std::cout << std::endl;
00575 
00576   // ROS_ERROR("%f %d",solution[2],vsolfree.size());
00577 }
00578 
00579 double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
00580 {
00581   double dist_sqr = 0;
00582   std::vector<double> ss = ik_seed_state;
00583   for (size_t i = 0; i < ik_seed_state.size(); ++i)
00584   {
00585     while (ss[i] > 2 * M_PI)
00586     {
00587       ss[i] -= 2 * M_PI;
00588     }
00589     while (ss[i] < 2 * M_PI)
00590     {
00591       ss[i] += 2 * M_PI;
00592     }
00593     while (solution[i] > 2 * M_PI)
00594     {
00595       solution[i] -= 2 * M_PI;
00596     }
00597     while (solution[i] < 2 * M_PI)
00598     {
00599       solution[i] += 2 * M_PI;
00600     }
00601     dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00602   }
00603   return dist_sqr;
00604 }
00605 
00606 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
00607 //                                  std::vector<std::vector<double> >& solslist)
00608 // {
00609 //   std::vector<double>
00610 //   double mindist = 0;
00611 //   int minindex = -1;
00612 //   std::vector<double> sol;
00613 //   for(size_t i=0;i<solslist.size();++i){
00614 //     getSolution(i,sol);
00615 //     double dist = harmonize(ik_seed_state, sol);
00616 //     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00617 //     if(minindex == -1 || dist<mindist){
00618 //       minindex = i;
00619 //       mindist = dist;
00620 //     }
00621 //   }
00622 //   if(minindex >= 0){
00623 //     getSolution(minindex,solution);
00624 //     harmonize(ik_seed_state, solution);
00625 //     index = minindex;
00626 //   }
00627 // }
00628 
00629 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal>& solutions,
00630                                                 const std::vector<double>& ik_seed_state,
00631                                                 std::vector<double>& solution) const
00632 {
00633   double mindist = DBL_MAX;
00634   int minindex = -1;
00635   std::vector<double> sol;
00636 
00637   // IKFast56/61
00638   for (size_t i = 0; i < solutions.GetNumSolutions(); ++i)
00639   {
00640     getSolution(solutions, i, sol);
00641     double dist = harmonize(ik_seed_state, sol);
00642     ROS_INFO_STREAM_NAMED("ikfast", "Dist " << i << " dist " << dist);
00643     // std::cout << "dist[" << i << "]= " << dist << std::endl;
00644     if (minindex == -1 || dist < mindist)
00645     {
00646       minindex = i;
00647       mindist = dist;
00648     }
00649   }
00650   if (minindex >= 0)
00651   {
00652     getSolution(solutions, minindex, solution);
00653     harmonize(ik_seed_state, solution);
00654   }
00655 }
00656 
00657 void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
00658 {
00659   free_params_.clear();
00660   for (int i = 0; i < count; ++i)
00661     free_params_.push_back(array[i]);
00662 }
00663 
00664 bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
00665 {
00666   if (count > 0)
00667   {
00668     if (-count >= min_count)
00669     {
00670       count = -count;
00671       return true;
00672     }
00673     else if (count + 1 <= max_count)
00674     {
00675       count = count + 1;
00676       return true;
00677     }
00678     else
00679     {
00680       return false;
00681     }
00682   }
00683   else
00684   {
00685     if (1 - count <= max_count)
00686     {
00687       count = 1 - count;
00688       return true;
00689     }
00690     else if (count - 1 >= min_count)
00691     {
00692       count = count - 1;
00693       return true;
00694     }
00695     else
00696       return false;
00697   }
00698 }
00699 
00700 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00701                                            const std::vector<double>& joint_angles,
00702                                            std::vector<geometry_msgs::Pose>& poses) const
00703 {
00704   if (GetIkType() != IKP_Transform6D)
00705   {
00706     // ComputeFk() is the inverse function of ComputeIk(), so the format of
00707     // eerot differs depending on IK type. The Transform6D IK type is the only
00708     // one for which a 3x3 rotation matrix is returned, which means we can only
00709     // compute FK for that IK type.
00710     ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
00711     return false;
00712   }
00713 
00714   KDL::Frame p_out;
00715   if (link_names.size() == 0)
00716   {
00717     ROS_WARN_STREAM_NAMED("ikfast", "Link names with nothing");
00718     return false;
00719   }
00720 
00721   if (link_names.size() != 1 || link_names[0] != getTipFrame())
00722   {
00723     ROS_ERROR_NAMED("ikfast", "Can compute FK for %s only", getTipFrame().c_str());
00724     return false;
00725   }
00726 
00727   bool valid = true;
00728 
00729   IkReal eerot[9], eetrans[3];
00730   IkReal angles[joint_angles.size()];
00731   for (unsigned char i = 0; i < joint_angles.size(); i++)
00732     angles[i] = joint_angles[i];
00733 
00734   // IKFast56/61
00735   ComputeFk(angles, eetrans, eerot);
00736 
00737   for (int i = 0; i < 3; ++i)
00738     p_out.p.data[i] = eetrans[i];
00739 
00740   for (int i = 0; i < 9; ++i)
00741     p_out.M.data[i] = eerot[i];
00742 
00743   poses.resize(1);
00744   tf::poseKDLToMsg(p_out, poses[0]);
00745 
00746   return valid;
00747 }
00748 
00749 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00750                                               const std::vector<double>& ik_seed_state, double timeout,
00751                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00752                                               const kinematics::KinematicsQueryOptions& options) const
00753 {
00754   const IKCallbackFn solution_callback = 0;
00755   std::vector<double> consistency_limits;
00756 
00757   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00758                           options);
00759 }
00760 
00761 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00762                                               const std::vector<double>& ik_seed_state, double timeout,
00763                                               const std::vector<double>& consistency_limits,
00764                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00765                                               const kinematics::KinematicsQueryOptions& options) const
00766 {
00767   const IKCallbackFn solution_callback = 0;
00768   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00769                           options);
00770 }
00771 
00772 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00773                                               const std::vector<double>& ik_seed_state, double timeout,
00774                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00775                                               moveit_msgs::MoveItErrorCodes& error_code,
00776                                               const kinematics::KinematicsQueryOptions& options) const
00777 {
00778   std::vector<double> consistency_limits;
00779   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00780                           options);
00781 }
00782 
00783 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00784                                               const std::vector<double>& ik_seed_state, double timeout,
00785                                               const std::vector<double>& consistency_limits,
00786                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00787                                               moveit_msgs::MoveItErrorCodes& error_code,
00788                                               const kinematics::KinematicsQueryOptions& options) const
00789 {
00790   ROS_DEBUG_STREAM_NAMED("ikfast", "searchPositionIK");
00791 
00793   SEARCH_MODE search_mode = _SEARCH_MODE_;
00794 
00795   // Check if there are no redundant joints
00796   if (free_params_.size() == 0)
00797   {
00798     ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints");
00799 
00800     // Find first IK solution, within joint limits
00801     if (!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00802     {
00803       ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever");
00804       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00805       return false;
00806     }
00807 
00808     // check for collisions if a callback is provided
00809     if (!solution_callback.empty())
00810     {
00811       solution_callback(ik_pose, solution, error_code);
00812       if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00813       {
00814         ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback");
00815         return true;
00816       }
00817       else
00818       {
00819         ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code);
00820         return false;
00821       }
00822     }
00823     else
00824     {
00825       return true;  // no collision check callback provided
00826     }
00827   }
00828 
00829   // -------------------------------------------------------------------------------------------------
00830   // Error Checking
00831   if (!active_)
00832   {
00833     ROS_ERROR_STREAM_NAMED("ikfast", "Kinematics not active");
00834     error_code.val = error_code.NO_IK_SOLUTION;
00835     return false;
00836   }
00837 
00838   if (ik_seed_state.size() != num_joints_)
00839   {
00840     ROS_ERROR_STREAM_NAMED("ikfast", "Seed state must have size " << num_joints_ << " instead of size "
00841                                                                   << ik_seed_state.size());
00842     error_code.val = error_code.NO_IK_SOLUTION;
00843     return false;
00844   }
00845 
00846   if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00847   {
00848     ROS_ERROR_STREAM_NAMED("ikfast", "Consistency limits be empty or must have size "
00849                                          << num_joints_ << " instead of size " << consistency_limits.size());
00850     error_code.val = error_code.NO_IK_SOLUTION;
00851     return false;
00852   }
00853 
00854   // -------------------------------------------------------------------------------------------------
00855   // Initialize
00856 
00857   KDL::Frame frame;
00858   tf::poseMsgToKDL(ik_pose, frame);
00859 
00860   std::vector<double> vfree(free_params_.size());
00861 
00862   ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00863   int counter = 0;
00864 
00865   double initial_guess = ik_seed_state[free_params_[0]];
00866   vfree[0] = initial_guess;
00867 
00868   // -------------------------------------------------------------------------------------------------
00869   // Handle consitency limits if needed
00870   int num_positive_increments;
00871   int num_negative_increments;
00872 
00873   if (!consistency_limits.empty())
00874   {
00875     // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
00876     // Assume [0]th free_params element for now.  Probably wrong.
00877     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
00878     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
00879 
00880     num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
00881     num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
00882   }
00883   else  // no consitency limits provided
00884   {
00885     num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
00886     num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
00887   }
00888 
00889   // -------------------------------------------------------------------------------------------------
00890   // Begin searching
00891 
00892   ROS_DEBUG_STREAM_NAMED("ikfast", "Free param is " << free_params_[0] << " initial guess is " << initial_guess
00893                                                     << ", # positive increments: " << num_positive_increments
00894                                                     << ", # negative increments: " << num_negative_increments);
00895   if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
00896     ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
00897 
00898   double best_costs = -1.0;
00899   std::vector<double> best_solution;
00900   int nattempts = 0, nvalid = 0;
00901 
00902   while (true)
00903   {
00904     IkSolutionList<IkReal> solutions;
00905     int numsol = solve(frame, vfree, solutions);
00906 
00907     ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
00908 
00909     // ROS_INFO("%f",vfree[0]);
00910 
00911     if (numsol > 0)
00912     {
00913       for (int s = 0; s < numsol; ++s)
00914       {
00915         nattempts++;
00916         std::vector<double> sol;
00917         getSolution(solutions, s, sol);
00918 
00919         bool obeys_limits = true;
00920         for (unsigned int i = 0; i < sol.size(); i++)
00921         {
00922           if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00923           {
00924             obeys_limits = false;
00925             break;
00926           }
00927           // ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " <<
00928           // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00929         }
00930         if (obeys_limits)
00931         {
00932           getSolution(solutions, s, solution);
00933 
00934           // This solution is within joint limits, now check if in collision (if callback provided)
00935           if (!solution_callback.empty())
00936           {
00937             solution_callback(ik_pose, solution, error_code);
00938           }
00939           else
00940           {
00941             error_code.val = error_code.SUCCESS;
00942           }
00943 
00944           if (error_code.val == error_code.SUCCESS)
00945           {
00946             nvalid++;
00947             if (search_mode & OPTIMIZE_MAX_JOINT)
00948             {
00949               // Costs for solution: Largest joint motion
00950               double costs = 0.0;
00951               for (unsigned int i = 0; i < solution.size(); i++)
00952               {
00953                 double d = fabs(ik_seed_state[i] - solution[i]);
00954                 if (d > costs)
00955                   costs = d;
00956               }
00957               if (costs < best_costs || best_costs == -1.0)
00958               {
00959                 best_costs = costs;
00960                 best_solution = solution;
00961               }
00962             }
00963             else
00964               // Return first feasible solution
00965               return true;
00966           }
00967         }
00968       }
00969     }
00970 
00971     if (!getCount(counter, num_positive_increments, -num_negative_increments))
00972     {
00973       // Everything searched
00974       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00975       break;
00976     }
00977 
00978     vfree[0] = initial_guess + search_discretization_ * counter;
00979     // ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00980   }
00981 
00982   ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
00983 
00984   if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
00985   {
00986     solution = best_solution;
00987     error_code.val = error_code.SUCCESS;
00988     return true;
00989   }
00990 
00991   // No solution found
00992   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00993   return false;
00994 }
00995 
00996 // Used when there are no redundant joints - aka no free params
00997 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00998                                            std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00999                                            const kinematics::KinematicsQueryOptions& options) const
01000 {
01001   ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK");
01002 
01003   if (!active_)
01004   {
01005     ROS_ERROR("kinematics not active");
01006     return false;
01007   }
01008 
01009   std::vector<double> vfree(free_params_.size());
01010   for (std::size_t i = 0; i < free_params_.size(); ++i)
01011   {
01012     int p = free_params_[i];
01013     ROS_ERROR("%u is %f", p, ik_seed_state[p]);  // DTC
01014     vfree[i] = ik_seed_state[p];
01015   }
01016 
01017   KDL::Frame frame;
01018   tf::poseMsgToKDL(ik_pose, frame);
01019 
01020   IkSolutionList<IkReal> solutions;
01021   int numsol = solve(frame, vfree, solutions);
01022 
01023   ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
01024 
01025   if (numsol)
01026   {
01027     for (int s = 0; s < numsol; ++s)
01028     {
01029       std::vector<double> sol;
01030       getSolution(solutions, s, sol);
01031       ROS_DEBUG_NAMED("ikfast", "Sol %d: %e   %e   %e   %e   %e   %e", s, sol[0], sol[1], sol[2], sol[3], sol[4],
01032                       sol[5]);
01033 
01034       bool obeys_limits = true;
01035       for (unsigned int i = 0; i < sol.size(); i++)
01036       {
01037         // Add tolerance to limit check
01038         if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
01039                                             (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
01040         {
01041           // One element of solution is not within limits
01042           obeys_limits = false;
01043           ROS_DEBUG_STREAM_NAMED("ikfast", "Not in limits! " << i << " value " << sol[i] << " has limit: "
01044                                                              << joint_has_limits_vector_[i] << "  being  "
01045                                                              << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
01046           break;
01047         }
01048       }
01049       if (obeys_limits)
01050       {
01051         // All elements of solution obey limits
01052         getSolution(solutions, s, solution);
01053         error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
01054         return true;
01055       }
01056     }
01057   }
01058   else
01059   {
01060     ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
01061   }
01062 
01063   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
01064   return false;
01065 }
01066 
01067 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
01068                                            const std::vector<double>& ik_seed_state,
01069                                            std::vector<std::vector<double> >& solutions,
01070                                            kinematics::KinematicsResult& result,
01071                                            const kinematics::KinematicsQueryOptions& options) const
01072 {
01073   ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK with multiple solutions");
01074 
01075   if (!active_)
01076   {
01077     ROS_ERROR("kinematics not active");
01078     result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE;
01079     return false;
01080   }
01081 
01082   if (ik_poses.empty())
01083   {
01084     ROS_ERROR("ik_poses is empty");
01085     result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
01086     return false;
01087   }
01088 
01089   if (ik_poses.size() > 1)
01090   {
01091     ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
01092     result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
01093     return false;
01094   }
01095 
01096   if (ik_seed_state.size() < num_joints_)
01097   {
01098     ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
01099                                                << num_joints_);
01100     return false;
01101   }
01102 
01103   KDL::Frame frame;
01104   tf::poseMsgToKDL(ik_poses[0], frame);
01105 
01106   // solving ik
01107   std::vector<IkSolutionList<IkReal> > solution_set;
01108   IkSolutionList<IkReal> ik_solutions;
01109   std::vector<double> vfree;
01110   int numsol = 0;
01111   std::vector<double> sampled_joint_vals;
01112   if (!redundant_joint_indices_.empty())
01113   {
01114     // initializing from seed
01115     sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
01116 
01117     // checking joint limits when using no discretization
01118     if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION &&
01119         joint_has_limits_vector_[redundant_joint_indices_.front()])
01120     {
01121       double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
01122       double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
01123 
01124       double jv = sampled_joint_vals[0];
01125       if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
01126       {
01127         result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS;
01128         ROS_ERROR_STREAM("ik seed is out of bounds");
01129         return false;
01130       }
01131     }
01132 
01133     // computing all solutions sets for each sampled value of the redundant joint
01134     if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
01135     {
01136       result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
01137       return false;
01138     }
01139 
01140     for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
01141     {
01142       vfree.clear();
01143       vfree.push_back(sampled_joint_vals[i]);
01144       numsol += solve(frame, vfree, ik_solutions);
01145       solution_set.push_back(ik_solutions);
01146     }
01147   }
01148   else
01149   {
01150     // computing for single solution set
01151     numsol = solve(frame, vfree, ik_solutions);
01152     solution_set.push_back(ik_solutions);
01153   }
01154 
01155   ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
01156   bool solutions_found = false;
01157   if (numsol > 0)
01158   {
01159     /*
01160       Iterating through all solution sets and storing those that do not exceed joint limits.
01161     */
01162     for (unsigned int r = 0; r < solution_set.size(); r++)
01163     {
01164       ik_solutions = solution_set[r];
01165       numsol = ik_solutions.GetNumSolutions();
01166       for (int s = 0; s < numsol; ++s)
01167       {
01168         std::vector<double> sol;
01169         getSolution(ik_solutions, s, sol);
01170 
01171         bool obeys_limits = true;
01172         for (unsigned int i = 0; i < sol.size(); i++)
01173         {
01174           // Add tolerance to limit check
01175           if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
01176                                               (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
01177           {
01178             // One element of solution is not within limits
01179             obeys_limits = false;
01180             ROS_DEBUG_STREAM_NAMED(
01181                 "ikfast", "Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i]
01182                                             << "  being  " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
01183             break;
01184           }
01185         }
01186         if (obeys_limits)
01187         {
01188           // All elements of solution obey limits
01189           solutions_found = true;
01190           solutions.push_back(sol);
01191         }
01192       }
01193     }
01194 
01195     if (solutions_found)
01196     {
01197       result.kinematic_error = kinematics::KinematicErrors::OK;
01198       return true;
01199     }
01200   }
01201   else
01202   {
01203     ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
01204   }
01205 
01206   result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
01207   return false;
01208 }
01209 
01210 bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method,
01211                                                   std::vector<double>& sampled_joint_vals) const
01212 {
01213   double joint_min = -M_PI;
01214   double joint_max = M_PI;
01215   int index = redundant_joint_indices_.front();
01216   double joint_dscrt = redundant_joint_discretization_.at(index);
01217 
01218   if (joint_has_limits_vector_[redundant_joint_indices_.front()])
01219   {
01220     joint_min = joint_min_vector_[index];
01221     joint_max = joint_max_vector_[index];
01222   }
01223 
01224   switch (method)
01225   {
01226     case kinematics::DiscretizationMethods::ALL_DISCRETIZED:
01227     {
01228       int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
01229       for (unsigned int i = 0; i < steps; i++)
01230       {
01231         sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
01232       }
01233       sampled_joint_vals.push_back(joint_max);
01234     }
01235     break;
01236     case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED:
01237     {
01238       int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
01239       steps = steps > 0 ? steps : 1;
01240       double diff = joint_max - joint_min;
01241       for (int i = 0; i < steps; i++)
01242       {
01243         sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
01244       }
01245     }
01246 
01247     break;
01248     case kinematics::DiscretizationMethods::NO_DISCRETIZATION:
01249 
01250       break;
01251     default:
01252       ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
01253       return false;
01254   }
01255 
01256   return true;
01257 }
01258 
01259 }  // end namespace
01260 
01261 // register IKFastKinematicsPlugin as a KinematicsBase implementation
01262 #include <pluginlib/class_list_macros.h>
01263 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Mon Jul 24 2017 02:21:29