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


robotican_manipulator_h
Author(s):
autogenerated on Fri Oct 27 2017 03:03:09