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   lookupParam("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   lookupParam("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   robot_model.initString(xml_string);
00362 
00363   ROS_DEBUG_STREAM_NAMED("ikfast", "Reading joints and links from URDF");
00364 
00365   boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(getTipFrame()));
00366   while (link->name != base_frame_ && joint_names_.size() <= num_joints_)
00367   {
00368     ROS_DEBUG_NAMED("ikfast", "Link %s", link->name.c_str());
00369     link_names_.push_back(link->name);
00370     boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00371     if (joint)
00372     {
00373       if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00374       {
00375         ROS_DEBUG_STREAM_NAMED("ikfast", "Adding joint " << joint->name);
00376 
00377         joint_names_.push_back(joint->name);
00378         float lower, upper;
00379         int hasLimits;
00380         if (joint->type != urdf::Joint::CONTINUOUS)
00381         {
00382           if (joint->safety)
00383           {
00384             lower = joint->safety->soft_lower_limit;
00385             upper = joint->safety->soft_upper_limit;
00386           }
00387           else
00388           {
00389             lower = joint->limits->lower;
00390             upper = joint->limits->upper;
00391           }
00392           hasLimits = 1;
00393         }
00394         else
00395         {
00396           lower = -M_PI;
00397           upper = M_PI;
00398           hasLimits = 0;
00399         }
00400         if (hasLimits)
00401         {
00402           joint_has_limits_vector_.push_back(true);
00403           joint_min_vector_.push_back(lower);
00404           joint_max_vector_.push_back(upper);
00405         }
00406         else
00407         {
00408           joint_has_limits_vector_.push_back(false);
00409           joint_min_vector_.push_back(-M_PI);
00410           joint_max_vector_.push_back(M_PI);
00411         }
00412       }
00413     }
00414     else
00415     {
00416       ROS_WARN_NAMED("ikfast", "no joint corresponding to %s", link->name.c_str());
00417     }
00418     link = link->getParent();
00419   }
00420 
00421   if (joint_names_.size() != num_joints_)
00422   {
00423     ROS_FATAL_STREAM_NAMED("ikfast", "Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has "
00424                                                                          << num_joints_);
00425     return false;
00426   }
00427 
00428   std::reverse(link_names_.begin(), link_names_.end());
00429   std::reverse(joint_names_.begin(), joint_names_.end());
00430   std::reverse(joint_min_vector_.begin(), joint_min_vector_.end());
00431   std::reverse(joint_max_vector_.begin(), joint_max_vector_.end());
00432   std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00433 
00434   for (size_t i = 0; i < num_joints_; ++i)
00435     ROS_DEBUG_STREAM_NAMED("ikfast", joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]
00436                                                      << " " << joint_has_limits_vector_[i]);
00437 
00438   active_ = true;
00439   return true;
00440 }
00441 
00442 void IKFastKinematicsPlugin::setSearchDiscretization(const std::map<int, double>& discretization)
00443 {
00444   if (discretization.empty())
00445   {
00446     ROS_ERROR("The 'discretization' map is empty");
00447     return;
00448   }
00449 
00450   if (redundant_joint_indices_.empty())
00451   {
00452     ROS_ERROR_STREAM("This group's solver doesn't support redundant joints");
00453     return;
00454   }
00455 
00456   if (discretization.begin()->first != redundant_joint_indices_[0])
00457   {
00458     std::string redundant_joint = joint_names_[free_params_[0]];
00459     ROS_ERROR_STREAM("Attempted to discretize a non-redundant joint "
00460                      << discretization.begin()->first << ", only joint '" << redundant_joint << "' with index "
00461                      << redundant_joint_indices_[0] << " is redundant.");
00462     return;
00463   }
00464 
00465   if (discretization.begin()->second <= 0.0)
00466   {
00467     ROS_ERROR_STREAM("Discretization can not takes values that are <= 0");
00468     return;
00469   }
00470 
00471   redundant_joint_discretization_.clear();
00472   redundant_joint_discretization_[redundant_joint_indices_[0]] = discretization.begin()->second;
00473 }
00474 
00475 bool IKFastKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
00476 {
00477   ROS_ERROR_STREAM("Changing the redundant joints isn't permitted by this group's solver ");
00478   return false;
00479 }
00480 
00481 int IKFastKinematicsPlugin::solve(KDL::Frame& pose_frame, const std::vector<double>& vfree,
00482                                   IkSolutionList<IkReal>& solutions) const
00483 {
00484   // IKFast56/61
00485   solutions.Clear();
00486 
00487   double trans[3];
00488   trans[0] = pose_frame.p[0];  //-.18;
00489   trans[1] = pose_frame.p[1];
00490   trans[2] = pose_frame.p[2];
00491 
00492   KDL::Rotation mult;
00493   KDL::Vector direction;
00494 
00495   switch (GetIkType())
00496   {
00497     case IKP_Transform6D:
00498     case IKP_Translation3D:
00499       // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
00500 
00501       mult = pose_frame.M;
00502 
00503       double vals[9];
00504       vals[0] = mult(0, 0);
00505       vals[1] = mult(0, 1);
00506       vals[2] = mult(0, 2);
00507       vals[3] = mult(1, 0);
00508       vals[4] = mult(1, 1);
00509       vals[5] = mult(1, 2);
00510       vals[6] = mult(2, 0);
00511       vals[7] = mult(2, 1);
00512       vals[8] = mult(2, 2);
00513 
00514       // IKFast56/61
00515       ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00516       return solutions.GetNumSolutions();
00517 
00518     case IKP_Direction3D:
00519     case IKP_Ray4D:
00520     case IKP_TranslationDirection5D:
00521       // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target
00522       // direction.
00523 
00524       direction = pose_frame.M * KDL::Vector(0, 0, 1);
00525       ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00526       return solutions.GetNumSolutions();
00527 
00528     case IKP_TranslationXAxisAngle4D:
00529     case IKP_TranslationYAxisAngle4D:
00530     case IKP_TranslationZAxisAngle4D:
00531       // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value
00532       // represents the angle.
00533       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00534       return 0;
00535 
00536     case IKP_TranslationLocalGlobal6D:
00537       // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end
00538       // effector coordinate system.
00539       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00540       return 0;
00541 
00542     case IKP_Rotation3D:
00543     case IKP_Lookat3D:
00544     case IKP_TranslationXY2D:
00545     case IKP_TranslationXYOrientation3D:
00546     case IKP_TranslationXAxisAngleZNorm4D:
00547     case IKP_TranslationYAxisAngleXNorm4D:
00548     case IKP_TranslationZAxisAngleYNorm4D:
00549       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00550       return 0;
00551 
00552     default:
00553       ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version "
00554                                 "of Openrave?");
00555       return 0;
00556   }
00557 }
00558 
00559 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal>& solutions, int i,
00560                                          std::vector<double>& solution) const
00561 {
00562   solution.clear();
00563   solution.resize(num_joints_);
00564 
00565   // IKFast56/61
00566   const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00567   std::vector<IkReal> vsolfree(sol.GetFree().size());
00568   sol.GetSolution(&solution[0], vsolfree.size() > 0 ? &vsolfree[0] : NULL);
00569 
00570   // std::cout << "solution " << i << ":" ;
00571   // for(int j=0;j<num_joints_; ++j)
00572   //   std::cout << " " << solution[j];
00573   // std::cout << std::endl;
00574 
00575   // ROS_ERROR("%f %d",solution[2],vsolfree.size());
00576 }
00577 
00578 double IKFastKinematicsPlugin::harmonize(const std::vector<double>& ik_seed_state, std::vector<double>& solution) const
00579 {
00580   double dist_sqr = 0;
00581   std::vector<double> ss = ik_seed_state;
00582   for (size_t i = 0; i < ik_seed_state.size(); ++i)
00583   {
00584     while (ss[i] > 2 * M_PI)
00585     {
00586       ss[i] -= 2 * M_PI;
00587     }
00588     while (ss[i] < 2 * M_PI)
00589     {
00590       ss[i] += 2 * M_PI;
00591     }
00592     while (solution[i] > 2 * M_PI)
00593     {
00594       solution[i] -= 2 * M_PI;
00595     }
00596     while (solution[i] < 2 * M_PI)
00597     {
00598       solution[i] += 2 * M_PI;
00599     }
00600     dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00601   }
00602   return dist_sqr;
00603 }
00604 
00605 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
00606 //                                  std::vector<std::vector<double> >& solslist)
00607 // {
00608 //   std::vector<double>
00609 //   double mindist = 0;
00610 //   int minindex = -1;
00611 //   std::vector<double> sol;
00612 //   for(size_t i=0;i<solslist.size();++i){
00613 //     getSolution(i,sol);
00614 //     double dist = harmonize(ik_seed_state, sol);
00615 //     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00616 //     if(minindex == -1 || dist<mindist){
00617 //       minindex = i;
00618 //       mindist = dist;
00619 //     }
00620 //   }
00621 //   if(minindex >= 0){
00622 //     getSolution(minindex,solution);
00623 //     harmonize(ik_seed_state, solution);
00624 //     index = minindex;
00625 //   }
00626 // }
00627 
00628 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal>& solutions,
00629                                                 const std::vector<double>& ik_seed_state,
00630                                                 std::vector<double>& solution) const
00631 {
00632   double mindist = DBL_MAX;
00633   int minindex = -1;
00634   std::vector<double> sol;
00635 
00636   // IKFast56/61
00637   for (size_t i = 0; i < solutions.GetNumSolutions(); ++i)
00638   {
00639     getSolution(solutions, i, sol);
00640     double dist = harmonize(ik_seed_state, sol);
00641     ROS_INFO_STREAM_NAMED("ikfast", "Dist " << i << " dist " << dist);
00642     // std::cout << "dist[" << i << "]= " << dist << std::endl;
00643     if (minindex == -1 || dist < mindist)
00644     {
00645       minindex = i;
00646       mindist = dist;
00647     }
00648   }
00649   if (minindex >= 0)
00650   {
00651     getSolution(solutions, minindex, solution);
00652     harmonize(ik_seed_state, solution);
00653   }
00654 }
00655 
00656 void IKFastKinematicsPlugin::fillFreeParams(int count, int* array)
00657 {
00658   free_params_.clear();
00659   for (int i = 0; i < count; ++i)
00660     free_params_.push_back(array[i]);
00661 }
00662 
00663 bool IKFastKinematicsPlugin::getCount(int& count, const int& max_count, const int& min_count) const
00664 {
00665   if (count > 0)
00666   {
00667     if (-count >= min_count)
00668     {
00669       count = -count;
00670       return true;
00671     }
00672     else if (count + 1 <= max_count)
00673     {
00674       count = count + 1;
00675       return true;
00676     }
00677     else
00678     {
00679       return false;
00680     }
00681   }
00682   else
00683   {
00684     if (1 - count <= max_count)
00685     {
00686       count = 1 - count;
00687       return true;
00688     }
00689     else if (count - 1 >= min_count)
00690     {
00691       count = count - 1;
00692       return true;
00693     }
00694     else
00695       return false;
00696   }
00697 }
00698 
00699 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00700                                            const std::vector<double>& joint_angles,
00701                                            std::vector<geometry_msgs::Pose>& poses) const
00702 {
00703   if (GetIkType() != IKP_Transform6D)
00704   {
00705     // ComputeFk() is the inverse function of ComputeIk(), so the format of
00706     // eerot differs depending on IK type. The Transform6D IK type is the only
00707     // one for which a 3x3 rotation matrix is returned, which means we can only
00708     // compute FK for that IK type.
00709     ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
00710     return false;
00711   }
00712 
00713   KDL::Frame p_out;
00714   if (link_names.size() == 0)
00715   {
00716     ROS_WARN_STREAM_NAMED("ikfast", "Link names with nothing");
00717     return false;
00718   }
00719 
00720   if (link_names.size() != 1 || link_names[0] != getTipFrame())
00721   {
00722     ROS_ERROR_NAMED("ikfast", "Can compute FK for %s only", getTipFrame().c_str());
00723     return false;
00724   }
00725 
00726   bool valid = true;
00727 
00728   IkReal eerot[9], eetrans[3];
00729   IkReal angles[joint_angles.size()];
00730   for (unsigned char i = 0; i < joint_angles.size(); i++)
00731     angles[i] = joint_angles[i];
00732 
00733   // IKFast56/61
00734   ComputeFk(angles, eetrans, eerot);
00735 
00736   for (int i = 0; i < 3; ++i)
00737     p_out.p.data[i] = eetrans[i];
00738 
00739   for (int i = 0; i < 9; ++i)
00740     p_out.M.data[i] = eerot[i];
00741 
00742   poses.resize(1);
00743   tf::poseKDLToMsg(p_out, poses[0]);
00744 
00745   return valid;
00746 }
00747 
00748 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00749                                               const std::vector<double>& ik_seed_state, double timeout,
00750                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00751                                               const kinematics::KinematicsQueryOptions& options) const
00752 {
00753   const IKCallbackFn solution_callback = 0;
00754   std::vector<double> consistency_limits;
00755 
00756   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00757                           options);
00758 }
00759 
00760 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00761                                               const std::vector<double>& ik_seed_state, double timeout,
00762                                               const std::vector<double>& consistency_limits,
00763                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00764                                               const kinematics::KinematicsQueryOptions& options) const
00765 {
00766   const IKCallbackFn solution_callback = 0;
00767   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00768                           options);
00769 }
00770 
00771 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00772                                               const std::vector<double>& ik_seed_state, double timeout,
00773                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00774                                               moveit_msgs::MoveItErrorCodes& error_code,
00775                                               const kinematics::KinematicsQueryOptions& options) const
00776 {
00777   std::vector<double> consistency_limits;
00778   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00779                           options);
00780 }
00781 
00782 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00783                                               const std::vector<double>& ik_seed_state, double timeout,
00784                                               const std::vector<double>& consistency_limits,
00785                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00786                                               moveit_msgs::MoveItErrorCodes& error_code,
00787                                               const kinematics::KinematicsQueryOptions& options) const
00788 {
00789   ROS_DEBUG_STREAM_NAMED("ikfast", "searchPositionIK");
00790 
00792   SEARCH_MODE search_mode = _SEARCH_MODE_;
00793 
00794   // Check if there are no redundant joints
00795   if (free_params_.size() == 0)
00796   {
00797     ROS_DEBUG_STREAM_NAMED("ikfast", "No need to search since no free params/redundant joints");
00798 
00799     // Find first IK solution, within joint limits
00800     if (!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00801     {
00802       ROS_DEBUG_STREAM_NAMED("ikfast", "No solution whatsoever");
00803       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00804       return false;
00805     }
00806 
00807     // check for collisions if a callback is provided
00808     if (!solution_callback.empty())
00809     {
00810       solution_callback(ik_pose, solution, error_code);
00811       if (error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00812       {
00813         ROS_DEBUG_STREAM_NAMED("ikfast", "Solution passes callback");
00814         return true;
00815       }
00816       else
00817       {
00818         ROS_DEBUG_STREAM_NAMED("ikfast", "Solution has error code " << error_code);
00819         return false;
00820       }
00821     }
00822     else
00823     {
00824       return true;  // no collision check callback provided
00825     }
00826   }
00827 
00828   // -------------------------------------------------------------------------------------------------
00829   // Error Checking
00830   if (!active_)
00831   {
00832     ROS_ERROR_STREAM_NAMED("ikfast", "Kinematics not active");
00833     error_code.val = error_code.NO_IK_SOLUTION;
00834     return false;
00835   }
00836 
00837   if (ik_seed_state.size() != num_joints_)
00838   {
00839     ROS_ERROR_STREAM_NAMED("ikfast", "Seed state must have size " << num_joints_ << " instead of size "
00840                                                                   << ik_seed_state.size());
00841     error_code.val = error_code.NO_IK_SOLUTION;
00842     return false;
00843   }
00844 
00845   if (!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00846   {
00847     ROS_ERROR_STREAM_NAMED("ikfast", "Consistency limits be empty or must have size "
00848                                          << num_joints_ << " instead of size " << consistency_limits.size());
00849     error_code.val = error_code.NO_IK_SOLUTION;
00850     return false;
00851   }
00852 
00853   // -------------------------------------------------------------------------------------------------
00854   // Initialize
00855 
00856   KDL::Frame frame;
00857   tf::poseMsgToKDL(ik_pose, frame);
00858 
00859   std::vector<double> vfree(free_params_.size());
00860 
00861   ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00862   int counter = 0;
00863 
00864   double initial_guess = ik_seed_state[free_params_[0]];
00865   vfree[0] = initial_guess;
00866 
00867   // -------------------------------------------------------------------------------------------------
00868   // Handle consitency limits if needed
00869   int num_positive_increments;
00870   int num_negative_increments;
00871 
00872   if (!consistency_limits.empty())
00873   {
00874     // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
00875     // Assume [0]th free_params element for now.  Probably wrong.
00876     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess + consistency_limits[free_params_[0]]);
00877     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess - consistency_limits[free_params_[0]]);
00878 
00879     num_positive_increments = (int)((max_limit - initial_guess) / search_discretization_);
00880     num_negative_increments = (int)((initial_guess - min_limit) / search_discretization_);
00881   }
00882   else  // no consitency limits provided
00883   {
00884     num_positive_increments = (joint_max_vector_[free_params_[0]] - initial_guess) / search_discretization_;
00885     num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]]) / search_discretization_;
00886   }
00887 
00888   // -------------------------------------------------------------------------------------------------
00889   // Begin searching
00890 
00891   ROS_DEBUG_STREAM_NAMED("ikfast", "Free param is " << free_params_[0] << " initial guess is " << initial_guess
00892                                                     << ", # positive increments: " << num_positive_increments
00893                                                     << ", # negative increments: " << num_negative_increments);
00894   if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
00895     ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
00896 
00897   double best_costs = -1.0;
00898   std::vector<double> best_solution;
00899   int nattempts = 0, nvalid = 0;
00900 
00901   while (true)
00902   {
00903     IkSolutionList<IkReal> solutions;
00904     int numsol = solve(frame, vfree, solutions);
00905 
00906     ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
00907 
00908     // ROS_INFO("%f",vfree[0]);
00909 
00910     if (numsol > 0)
00911     {
00912       for (int s = 0; s < numsol; ++s)
00913       {
00914         nattempts++;
00915         std::vector<double> sol;
00916         getSolution(solutions, s, sol);
00917 
00918         bool obeys_limits = true;
00919         for (unsigned int i = 0; i < sol.size(); i++)
00920         {
00921           if (joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00922           {
00923             obeys_limits = false;
00924             break;
00925           }
00926           // ROS_INFO_STREAM_NAMED("ikfast","Num " << i << " value " << sol[i] << " has limits " <<
00927           // joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00928         }
00929         if (obeys_limits)
00930         {
00931           getSolution(solutions, s, solution);
00932 
00933           // This solution is within joint limits, now check if in collision (if callback provided)
00934           if (!solution_callback.empty())
00935           {
00936             solution_callback(ik_pose, solution, error_code);
00937           }
00938           else
00939           {
00940             error_code.val = error_code.SUCCESS;
00941           }
00942 
00943           if (error_code.val == error_code.SUCCESS)
00944           {
00945             nvalid++;
00946             if (search_mode & OPTIMIZE_MAX_JOINT)
00947             {
00948               // Costs for solution: Largest joint motion
00949               double costs = 0.0;
00950               for (unsigned int i = 0; i < solution.size(); i++)
00951               {
00952                 double d = fabs(ik_seed_state[i] - solution[i]);
00953                 if (d > costs)
00954                   costs = d;
00955               }
00956               if (costs < best_costs || best_costs == -1.0)
00957               {
00958                 best_costs = costs;
00959                 best_solution = solution;
00960               }
00961             }
00962             else
00963               // Return first feasible solution
00964               return true;
00965           }
00966         }
00967       }
00968     }
00969 
00970     if (!getCount(counter, num_positive_increments, -num_negative_increments))
00971     {
00972       // Everything searched
00973       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00974       break;
00975     }
00976 
00977     vfree[0] = initial_guess + search_discretization_ * counter;
00978     // ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00979   }
00980 
00981   ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
00982 
00983   if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
00984   {
00985     solution = best_solution;
00986     error_code.val = error_code.SUCCESS;
00987     return true;
00988   }
00989 
00990   // No solution found
00991   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00992   return false;
00993 }
00994 
00995 // Used when there are no redundant joints - aka no free params
00996 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00997                                            std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00998                                            const kinematics::KinematicsQueryOptions& options) const
00999 {
01000   ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK");
01001 
01002   if (!active_)
01003   {
01004     ROS_ERROR("kinematics not active");
01005     return false;
01006   }
01007 
01008   std::vector<double> vfree(free_params_.size());
01009   for (std::size_t i = 0; i < free_params_.size(); ++i)
01010   {
01011     int p = free_params_[i];
01012     ROS_ERROR("%u is %f", p, ik_seed_state[p]);  // DTC
01013     vfree[i] = ik_seed_state[p];
01014   }
01015 
01016   KDL::Frame frame;
01017   tf::poseMsgToKDL(ik_pose, frame);
01018 
01019   IkSolutionList<IkReal> solutions;
01020   int numsol = solve(frame, vfree, solutions);
01021 
01022   ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
01023 
01024   if (numsol)
01025   {
01026     for (int s = 0; s < numsol; ++s)
01027     {
01028       std::vector<double> sol;
01029       getSolution(solutions, s, sol);
01030       ROS_DEBUG_NAMED("ikfast", "Sol %d: %e   %e   %e   %e   %e   %e", s, sol[0], sol[1], sol[2], sol[3], sol[4],
01031                       sol[5]);
01032 
01033       bool obeys_limits = true;
01034       for (unsigned int i = 0; i < sol.size(); i++)
01035       {
01036         // Add tolerance to limit check
01037         if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
01038                                             (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
01039         {
01040           // One element of solution is not within limits
01041           obeys_limits = false;
01042           ROS_DEBUG_STREAM_NAMED("ikfast", "Not in limits! " << i << " value " << sol[i] << " has limit: "
01043                                                              << joint_has_limits_vector_[i] << "  being  "
01044                                                              << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
01045           break;
01046         }
01047       }
01048       if (obeys_limits)
01049       {
01050         // All elements of solution obey limits
01051         getSolution(solutions, s, solution);
01052         error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
01053         return true;
01054       }
01055     }
01056   }
01057   else
01058   {
01059     ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
01060   }
01061 
01062   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
01063   return false;
01064 }
01065 
01066 bool IKFastKinematicsPlugin::getPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
01067                                            const std::vector<double>& ik_seed_state,
01068                                            std::vector<std::vector<double> >& solutions,
01069                                            kinematics::KinematicsResult& result,
01070                                            const kinematics::KinematicsQueryOptions& options) const
01071 {
01072   ROS_DEBUG_STREAM_NAMED("ikfast", "getPositionIK with multiple solutions");
01073 
01074   if (!active_)
01075   {
01076     ROS_ERROR("kinematics not active");
01077     result.kinematic_error = kinematics::KinematicErrors::SOLVER_NOT_ACTIVE;
01078     return false;
01079   }
01080 
01081   if (ik_poses.empty())
01082   {
01083     ROS_ERROR("ik_poses is empty");
01084     result.kinematic_error = kinematics::KinematicErrors::EMPTY_TIP_POSES;
01085     return false;
01086   }
01087 
01088   if (ik_poses.size() > 1)
01089   {
01090     ROS_ERROR("ik_poses contains multiple entries, only one is allowed");
01091     result.kinematic_error = kinematics::KinematicErrors::MULTIPLE_TIPS_NOT_SUPPORTED;
01092     return false;
01093   }
01094 
01095   if (ik_seed_state.size() < num_joints_)
01096   {
01097     ROS_ERROR_STREAM("ik_seed_state only has " << ik_seed_state.size() << " entries, this ikfast solver requires "
01098                                                << num_joints_);
01099     return false;
01100   }
01101 
01102   KDL::Frame frame;
01103   tf::poseMsgToKDL(ik_poses[0], frame);
01104 
01105   // solving ik
01106   std::vector<IkSolutionList<IkReal> > solution_set;
01107   IkSolutionList<IkReal> ik_solutions;
01108   std::vector<double> vfree;
01109   int numsol = 0;
01110   std::vector<double> sampled_joint_vals;
01111   if (!redundant_joint_indices_.empty())
01112   {
01113     // initializing from seed
01114     sampled_joint_vals.push_back(ik_seed_state[redundant_joint_indices_[0]]);
01115 
01116     // checking joint limits when using no discretization
01117     if (options.discretization_method == kinematics::DiscretizationMethods::NO_DISCRETIZATION &&
01118         joint_has_limits_vector_[redundant_joint_indices_.front()])
01119     {
01120       double joint_min = joint_min_vector_[redundant_joint_indices_.front()];
01121       double joint_max = joint_max_vector_[redundant_joint_indices_.front()];
01122 
01123       double jv = sampled_joint_vals[0];
01124       if (!((jv > (joint_min - LIMIT_TOLERANCE)) && (jv < (joint_max + LIMIT_TOLERANCE))))
01125       {
01126         result.kinematic_error = kinematics::KinematicErrors::IK_SEED_OUTSIDE_LIMITS;
01127         ROS_ERROR_STREAM("ik seed is out of bounds");
01128         return false;
01129       }
01130     }
01131 
01132     // computing all solutions sets for each sampled value of the redundant joint
01133     if (!sampleRedundantJoint(options.discretization_method, sampled_joint_vals))
01134     {
01135       result.kinematic_error = kinematics::KinematicErrors::UNSUPORTED_DISCRETIZATION_REQUESTED;
01136       return false;
01137     }
01138 
01139     for (unsigned int i = 0; i < sampled_joint_vals.size(); i++)
01140     {
01141       vfree.clear();
01142       vfree.push_back(sampled_joint_vals[i]);
01143       numsol += solve(frame, vfree, ik_solutions);
01144       solution_set.push_back(ik_solutions);
01145     }
01146   }
01147   else
01148   {
01149     // computing for single solution set
01150     numsol = solve(frame, vfree, ik_solutions);
01151     solution_set.push_back(ik_solutions);
01152   }
01153 
01154   ROS_DEBUG_STREAM_NAMED("ikfast", "Found " << numsol << " solutions from IKFast");
01155   bool solutions_found = false;
01156   if (numsol > 0)
01157   {
01158     /*
01159       Iterating through all solution sets and storing those that do not exceed joint limits.
01160     */
01161     for (unsigned int r = 0; r < solution_set.size(); r++)
01162     {
01163       ik_solutions = solution_set[r];
01164       numsol = ik_solutions.GetNumSolutions();
01165       for (int s = 0; s < numsol; ++s)
01166       {
01167         std::vector<double> sol;
01168         getSolution(ik_solutions, s, sol);
01169 
01170         bool obeys_limits = true;
01171         for (unsigned int i = 0; i < sol.size(); i++)
01172         {
01173           // Add tolerance to limit check
01174           if (joint_has_limits_vector_[i] && ((sol[i] < (joint_min_vector_[i] - LIMIT_TOLERANCE)) ||
01175                                               (sol[i] > (joint_max_vector_[i] + LIMIT_TOLERANCE))))
01176           {
01177             // One element of solution is not within limits
01178             obeys_limits = false;
01179             ROS_DEBUG_STREAM_NAMED(
01180                 "ikfast", "Not in limits! " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i]
01181                                             << "  being  " << joint_min_vector_[i] << " to " << joint_max_vector_[i]);
01182             break;
01183           }
01184         }
01185         if (obeys_limits)
01186         {
01187           // All elements of solution obey limits
01188           solutions_found = true;
01189           solutions.push_back(sol);
01190         }
01191       }
01192     }
01193 
01194     if (solutions_found)
01195     {
01196       result.kinematic_error = kinematics::KinematicErrors::OK;
01197       return true;
01198     }
01199   }
01200   else
01201   {
01202     ROS_DEBUG_STREAM_NAMED("ikfast", "No IK solution");
01203   }
01204 
01205   result.kinematic_error = kinematics::KinematicErrors::NO_SOLUTION;
01206   return false;
01207 }
01208 
01209 bool IKFastKinematicsPlugin::sampleRedundantJoint(kinematics::DiscretizationMethod method,
01210                                                   std::vector<double>& sampled_joint_vals) const
01211 {
01212   double joint_min = -M_PI;
01213   double joint_max = M_PI;
01214   int index = redundant_joint_indices_.front();
01215   double joint_dscrt = redundant_joint_discretization_.at(index);
01216 
01217   if (joint_has_limits_vector_[redundant_joint_indices_.front()])
01218   {
01219     joint_min = joint_min_vector_[index];
01220     joint_max = joint_max_vector_[index];
01221   }
01222 
01223   switch (method)
01224   {
01225     case kinematics::DiscretizationMethods::ALL_DISCRETIZED:
01226     {
01227       int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
01228       for (unsigned int i = 0; i < steps; i++)
01229       {
01230         sampled_joint_vals.push_back(joint_min + joint_dscrt * i);
01231       }
01232       sampled_joint_vals.push_back(joint_max);
01233     }
01234     break;
01235     case kinematics::DiscretizationMethods::ALL_RANDOM_SAMPLED:
01236     {
01237       int steps = std::ceil((joint_max - joint_min) / joint_dscrt);
01238       steps = steps > 0 ? steps : 1;
01239       double diff = joint_max - joint_min;
01240       for (int i = 0; i < steps; i++)
01241       {
01242         sampled_joint_vals.push_back(((diff * std::rand()) / (static_cast<double>(RAND_MAX))) + joint_min);
01243       }
01244     }
01245 
01246     break;
01247     case kinematics::DiscretizationMethods::NO_DISCRETIZATION:
01248 
01249       break;
01250     default:
01251       ROS_ERROR_STREAM("Discretization method " << method << " is not supported");
01252       return false;
01253   }
01254 
01255   return true;
01256 }
01257 
01258 }  // end namespace
01259 
01260 // register IKFastKinematicsPlugin as a KinematicsBase implementation
01261 #include <pluginlib/class_list_macros.h>
01262 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Fri Apr 20 2018 03:31:47