fanuc_m6ib_manipulator_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 "fanuc_m6ib_manipulator_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():active_(false){}
00136 
00146   // Returns the first IK solution that is within joint limits, this is called by get_ik() service
00147   bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00148                      const std::vector<double> &ik_seed_state,
00149                      std::vector<double> &solution,
00150                      moveit_msgs::MoveItErrorCodes &error_code,
00151                      const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00152 
00161   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00162                         const std::vector<double> &ik_seed_state,
00163                         double timeout,
00164                         std::vector<double> &solution,
00165                         moveit_msgs::MoveItErrorCodes &error_code,
00166                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00167 
00177   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00178                         const std::vector<double> &ik_seed_state,
00179                         double timeout,
00180                         const std::vector<double> &consistency_limits,
00181                         std::vector<double> &solution,
00182                         moveit_msgs::MoveItErrorCodes &error_code,
00183                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00184 
00193   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00194                         const std::vector<double> &ik_seed_state,
00195                         double timeout,
00196                         std::vector<double> &solution,
00197                         const IKCallbackFn &solution_callback,
00198                         moveit_msgs::MoveItErrorCodes &error_code,
00199                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00200 
00211   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00212                         const std::vector<double> &ik_seed_state,
00213                         double timeout,
00214                         const std::vector<double> &consistency_limits,
00215                         std::vector<double> &solution,
00216                         const IKCallbackFn &solution_callback,
00217                         moveit_msgs::MoveItErrorCodes &error_code,
00218                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00219 
00228   bool getPositionFK(const std::vector<std::string> &link_names,
00229                      const std::vector<double> &joint_angles,
00230                      std::vector<geometry_msgs::Pose> &poses) const;
00231 
00232 private:
00233 
00234   bool initialize(const std::string &robot_description,
00235                   const std::string& group_name,
00236                   const std::string& base_name,
00237                   const std::string& tip_name,
00238                   double search_discretization);
00239 
00244   int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00245 
00249   void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00250 
00251   double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00252   //void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
00253   void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00254   void fillFreeParams(int count, int *array);
00255   bool getCount(int &count, const int &max_count, const int &min_count) const;
00256 
00257 }; // end class
00258 
00259 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00260                                         const std::string& group_name,
00261                                         const std::string& base_name,
00262                                         const std::string& tip_name,
00263                                         double search_discretization)
00264 {
00265   setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00266 
00267   ros::NodeHandle node_handle("~/"+group_name);
00268 
00269   std::string robot;
00270   node_handle.param("robot",robot,std::string());
00271 
00272   // IKFast56/61
00273   fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00274   num_joints_ = GetNumJoints();
00275 
00276   if(free_params_.size() > 1)
00277   {
00278     ROS_FATAL("Only one free joint paramter supported!");
00279     return false;
00280   }
00281 
00282   urdf::Model robot_model;
00283   std::string xml_string;
00284 
00285   std::string urdf_xml,full_urdf_xml;
00286   node_handle.param("urdf_xml",urdf_xml,robot_description);
00287   node_handle.searchParam(urdf_xml,full_urdf_xml);
00288 
00289   ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00290   if (!node_handle.getParam(full_urdf_xml, xml_string))
00291   {
00292     ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00293     return false;
00294   }
00295 
00296   node_handle.param(full_urdf_xml,xml_string,std::string());
00297   robot_model.initString(xml_string);
00298 
00299   ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00300 
00301   boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(getTipFrame()));
00302   while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00303   {
00304     ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00305     link_names_.push_back(link->name);
00306     boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00307     if(joint)
00308     {
00309       if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00310       {
00311         ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00312 
00313         joint_names_.push_back(joint->name);
00314         float lower, upper;
00315         int hasLimits;
00316         if ( joint->type != urdf::Joint::CONTINUOUS )
00317         {
00318           if(joint->safety)
00319           {
00320             lower = joint->safety->soft_lower_limit;
00321             upper = joint->safety->soft_upper_limit;
00322           } else {
00323             lower = joint->limits->lower;
00324             upper = joint->limits->upper;
00325           }
00326           hasLimits = 1;
00327         }
00328         else
00329         {
00330           lower = -M_PI;
00331           upper = M_PI;
00332           hasLimits = 0;
00333         }
00334         if(hasLimits)
00335         {
00336           joint_has_limits_vector_.push_back(true);
00337           joint_min_vector_.push_back(lower);
00338           joint_max_vector_.push_back(upper);
00339         }
00340         else
00341         {
00342           joint_has_limits_vector_.push_back(false);
00343           joint_min_vector_.push_back(-M_PI);
00344           joint_max_vector_.push_back(M_PI);
00345         }
00346       }
00347     } else
00348     {
00349       ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00350     }
00351     link = link->getParent();
00352   }
00353 
00354   if(joint_names_.size() != num_joints_)
00355   {
00356     ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00357     return false;
00358   }
00359 
00360   std::reverse(link_names_.begin(),link_names_.end());
00361   std::reverse(joint_names_.begin(),joint_names_.end());
00362   std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00363   std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00364   std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00365 
00366   for(size_t i=0; i <num_joints_; ++i)
00367     ROS_DEBUG_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00368 
00369   active_ = true;
00370   return true;
00371 }
00372 
00373 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00374 {
00375   // IKFast56/61
00376   solutions.Clear();
00377 
00378   double trans[3];
00379   trans[0] = pose_frame.p[0];//-.18;
00380   trans[1] = pose_frame.p[1];
00381   trans[2] = pose_frame.p[2];
00382 
00383   KDL::Rotation mult;
00384   KDL::Vector direction;
00385 
00386   switch (GetIkType())
00387   {
00388     case IKP_Transform6D:
00389     case IKP_Translation3D:
00390       // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix. For **Translation3D**, these are ignored.
00391 
00392       mult = pose_frame.M;
00393 
00394       double vals[9];
00395       vals[0] = mult(0,0);
00396       vals[1] = mult(0,1);
00397       vals[2] = mult(0,2);
00398       vals[3] = mult(1,0);
00399       vals[4] = mult(1,1);
00400       vals[5] = mult(1,2);
00401       vals[6] = mult(2,0);
00402       vals[7] = mult(2,1);
00403       vals[8] = mult(2,2);
00404 
00405       // IKFast56/61
00406       ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00407       return solutions.GetNumSolutions();
00408 
00409     case IKP_Direction3D:
00410     case IKP_Ray4D:
00411     case IKP_TranslationDirection5D:
00412       // For **Direction3D**, **Ray4D**, and **TranslationDirection5D**, the first 3 values represent the target direction.
00413 
00414       direction = pose_frame.M * KDL::Vector(0, 0, 1);
00415       ComputeIk(trans, direction.data, vfree.size() > 0 ? &vfree[0] : NULL, solutions);
00416       return solutions.GetNumSolutions();
00417 
00418     case IKP_TranslationXAxisAngle4D:
00419     case IKP_TranslationYAxisAngle4D:
00420     case IKP_TranslationZAxisAngle4D:
00421       // For **TranslationXAxisAngle4D**, **TranslationYAxisAngle4D**, and **TranslationZAxisAngle4D**, the first value represents the angle.
00422       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00423       return 0;
00424 
00425     case IKP_TranslationLocalGlobal6D:
00426       // For **TranslationLocalGlobal6D**, the diagonal elements ([0],[4],[8]) are the local translation inside the end effector coordinate system.
00427       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00428       return 0;
00429 
00430     case IKP_Rotation3D:
00431     case IKP_Lookat3D:
00432     case IKP_TranslationXY2D:
00433     case IKP_TranslationXYOrientation3D:
00434     case IKP_TranslationXAxisAngleZNorm4D:
00435     case IKP_TranslationYAxisAngleXNorm4D:
00436     case IKP_TranslationZAxisAngleYNorm4D:
00437       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00438       return 0;
00439 
00440     default:
00441       ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
00442       return 0;
00443   }
00444 }
00445 
00446 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00447 {
00448   solution.clear();
00449   solution.resize(num_joints_);
00450 
00451   // IKFast56/61
00452   const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00453   std::vector<IkReal> vsolfree( sol.GetFree().size() );
00454   sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00455 
00456   // std::cout << "solution " << i << ":" ;
00457   // for(int j=0;j<num_joints_; ++j)
00458   //   std::cout << " " << solution[j];
00459   // std::cout << std::endl;
00460 
00461   //ROS_ERROR("%f %d",solution[2],vsolfree.size());
00462 }
00463 
00464 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00465 {
00466   double dist_sqr = 0;
00467   std::vector<double> ss = ik_seed_state;
00468   for(size_t i=0; i< ik_seed_state.size(); ++i)
00469   {
00470     while(ss[i] > 2*M_PI) {
00471       ss[i] -= 2*M_PI;
00472     }
00473     while(ss[i] < 2*M_PI) {
00474       ss[i] += 2*M_PI;
00475     }
00476     while(solution[i] > 2*M_PI) {
00477       solution[i] -= 2*M_PI;
00478     }
00479     while(solution[i] < 2*M_PI) {
00480       solution[i] += 2*M_PI;
00481     }
00482     dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00483   }
00484   return dist_sqr;
00485 }
00486 
00487 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
00488 //                                  std::vector<std::vector<double> >& solslist)
00489 // {
00490 //   std::vector<double>
00491 //   double mindist = 0;
00492 //   int minindex = -1;
00493 //   std::vector<double> sol;
00494 //   for(size_t i=0;i<solslist.size();++i){
00495 //     getSolution(i,sol);
00496 //     double dist = harmonize(ik_seed_state, sol);
00497 //     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00498 //     if(minindex == -1 || dist<mindist){
00499 //       minindex = i;
00500 //       mindist = dist;
00501 //     }
00502 //   }
00503 //   if(minindex >= 0){
00504 //     getSolution(minindex,solution);
00505 //     harmonize(ik_seed_state, solution);
00506 //     index = minindex;
00507 //   }
00508 // }
00509 
00510 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00511 {
00512   double mindist = DBL_MAX;
00513   int minindex = -1;
00514   std::vector<double> sol;
00515 
00516   // IKFast56/61
00517   for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00518   {
00519     getSolution(solutions, i,sol);
00520     double dist = harmonize(ik_seed_state, sol);
00521     ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00522     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00523     if(minindex == -1 || dist<mindist){
00524       minindex = i;
00525       mindist = dist;
00526     }
00527   }
00528   if(minindex >= 0){
00529     getSolution(solutions, minindex,solution);
00530     harmonize(ik_seed_state, solution);
00531   }
00532 }
00533 
00534 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00535 {
00536   free_params_.clear();
00537   for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00538 }
00539 
00540 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00541 {
00542   if(count > 0)
00543   {
00544     if(-count >= min_count)
00545     {
00546       count = -count;
00547       return true;
00548     }
00549     else if(count+1 <= max_count)
00550     {
00551       count = count+1;
00552       return true;
00553     }
00554     else
00555     {
00556       return false;
00557     }
00558   }
00559   else
00560   {
00561     if(1-count <= max_count)
00562     {
00563       count = 1-count;
00564       return true;
00565     }
00566     else if(count-1 >= min_count)
00567     {
00568       count = count -1;
00569       return true;
00570     }
00571     else
00572       return false;
00573   }
00574 }
00575 
00576 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00577                                            const std::vector<double> &joint_angles,
00578                                            std::vector<geometry_msgs::Pose> &poses) const
00579 {
00580   if (GetIkType() != IKP_Transform6D) {
00581     // ComputeFk() is the inverse function of ComputeIk(), so the format of
00582     // eerot differs depending on IK type. The Transform6D IK type is the only
00583     // one for which a 3x3 rotation matrix is returned, which means we can only
00584     // compute FK for that IK type.
00585     ROS_ERROR_NAMED("ikfast", "Can only compute FK for Transform6D IK type!");
00586     return false;
00587   }
00588 
00589   KDL::Frame p_out;
00590   if(link_names.size() == 0) {
00591     ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
00592     return false;
00593   }
00594 
00595   if(link_names.size()!=1 || link_names[0]!=getTipFrame()){
00596     ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",getTipFrame().c_str());
00597     return false;
00598   }
00599 
00600   bool valid = true;
00601 
00602   IkReal eerot[9],eetrans[3];
00603   IkReal angles[joint_angles.size()];
00604   for (unsigned char i=0; i < joint_angles.size(); i++)
00605     angles[i] = joint_angles[i];
00606 
00607   // IKFast56/61
00608   ComputeFk(angles,eetrans,eerot);
00609 
00610   for(int i=0; i<3;++i)
00611     p_out.p.data[i] = eetrans[i];
00612 
00613   for(int i=0; i<9;++i)
00614     p_out.M.data[i] = eerot[i];
00615 
00616   poses.resize(1);
00617   tf::poseKDLToMsg(p_out,poses[0]);
00618 
00619   return valid;
00620 }
00621 
00622 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00623                                            const std::vector<double> &ik_seed_state,
00624                                            double timeout,
00625                                            std::vector<double> &solution,
00626                                            moveit_msgs::MoveItErrorCodes &error_code,
00627                                            const kinematics::KinematicsQueryOptions &options) const
00628 {
00629   const IKCallbackFn solution_callback = 0; 
00630   std::vector<double> consistency_limits;
00631 
00632   return searchPositionIK(ik_pose,
00633                           ik_seed_state,
00634                           timeout,
00635                           consistency_limits,
00636                           solution,
00637                           solution_callback,
00638                           error_code,
00639                           options);
00640 }
00641     
00642 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00643                                            const std::vector<double> &ik_seed_state,
00644                                            double timeout,
00645                                            const std::vector<double> &consistency_limits,
00646                                            std::vector<double> &solution,
00647                                            moveit_msgs::MoveItErrorCodes &error_code,
00648                                            const kinematics::KinematicsQueryOptions &options) const
00649 {
00650   const IKCallbackFn solution_callback = 0; 
00651   return searchPositionIK(ik_pose,
00652                           ik_seed_state,
00653                           timeout,
00654                           consistency_limits,
00655                           solution,
00656                           solution_callback,
00657                           error_code,
00658                           options);
00659 }
00660 
00661 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00662                                            const std::vector<double> &ik_seed_state,
00663                                            double timeout,
00664                                            std::vector<double> &solution,
00665                                            const IKCallbackFn &solution_callback,
00666                                            moveit_msgs::MoveItErrorCodes &error_code,
00667                                            const kinematics::KinematicsQueryOptions &options) const
00668 {
00669   std::vector<double> consistency_limits;
00670   return searchPositionIK(ik_pose,
00671                           ik_seed_state,
00672                           timeout,
00673                           consistency_limits,
00674                           solution,
00675                           solution_callback,
00676                           error_code,
00677                           options);
00678 }
00679 
00680 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00681                                               const std::vector<double> &ik_seed_state,
00682                                               double timeout,
00683                                               const std::vector<double> &consistency_limits,
00684                                               std::vector<double> &solution,
00685                                               const IKCallbackFn &solution_callback,
00686                                               moveit_msgs::MoveItErrorCodes &error_code,
00687                                               const kinematics::KinematicsQueryOptions &options) const
00688 {
00689   ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
00690 
00692   SEARCH_MODE search_mode = OPTIMIZE_MAX_JOINT;
00693 
00694   // Check if there are no redundant joints
00695   if(free_params_.size()==0)
00696   {
00697     ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00698 
00699     // Find first IK solution, within joint limits
00700     if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00701     {
00702       ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00703       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00704       return false;
00705     }
00706 
00707     // check for collisions if a callback is provided
00708     if( !solution_callback.empty() )
00709     {
00710       solution_callback(ik_pose, solution, error_code);
00711       if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00712       {
00713         ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00714         return true;
00715       }
00716       else
00717       {
00718         ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00719         return false;
00720       }
00721     }
00722     else
00723     {
00724       return true; // no collision check callback provided
00725     }
00726   }
00727 
00728   // -------------------------------------------------------------------------------------------------
00729   // Error Checking
00730   if(!active_)
00731   {
00732     ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00733     error_code.val = error_code.NO_IK_SOLUTION;
00734     return false;
00735   }
00736 
00737   if(ik_seed_state.size() != num_joints_)
00738   {
00739     ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00740     error_code.val = error_code.NO_IK_SOLUTION;
00741     return false;
00742   }
00743 
00744   if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00745   {
00746     ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00747     error_code.val = error_code.NO_IK_SOLUTION;
00748     return false;
00749   }
00750 
00751 
00752   // -------------------------------------------------------------------------------------------------
00753   // Initialize
00754 
00755   KDL::Frame frame;
00756   tf::poseMsgToKDL(ik_pose,frame);
00757 
00758   std::vector<double> vfree(free_params_.size());
00759 
00760   ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00761   int counter = 0;
00762 
00763   double initial_guess = ik_seed_state[free_params_[0]];
00764   vfree[0] = initial_guess;
00765 
00766   // -------------------------------------------------------------------------------------------------
00767   // Handle consitency limits if needed
00768   int num_positive_increments;
00769   int num_negative_increments;
00770 
00771   if(!consistency_limits.empty())
00772   {
00773     // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
00774     // Assume [0]th free_params element for now.  Probably wrong.
00775     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00776     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00777 
00778     num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00779     num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00780   }
00781   else // no consitency limits provided
00782   {
00783     num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00784     num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00785   }
00786 
00787   // -------------------------------------------------------------------------------------------------
00788   // Begin searching
00789 
00790   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);
00791   if ((search_mode & OPTIMIZE_MAX_JOINT) && (num_positive_increments + num_negative_increments) > 1000)
00792       ROS_WARN_STREAM_ONCE_NAMED("ikfast", "Large search space, consider increasing the search discretization");
00793   
00794   double best_costs = -1.0;
00795   std::vector<double> best_solution;
00796   int nattempts = 0, nvalid = 0;
00797 
00798   while(true)
00799   {
00800     IkSolutionList<IkReal> solutions;
00801     int numsol = solve(frame,vfree, solutions);
00802 
00803     ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00804 
00805     //ROS_INFO("%f",vfree[0]);
00806 
00807     if( numsol > 0 )
00808     {
00809       for(int s = 0; s < numsol; ++s)
00810       {
00811         nattempts++;
00812         std::vector<double> sol;
00813         getSolution(solutions,s,sol);
00814 
00815         bool obeys_limits = true;
00816         for(unsigned int i = 0; i < sol.size(); i++)
00817         {
00818           if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00819           {
00820             obeys_limits = false;
00821             break;
00822           }
00823           //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]);
00824         }
00825         if(obeys_limits)
00826         {
00827           getSolution(solutions,s,solution);
00828 
00829           // This solution is within joint limits, now check if in collision (if callback provided)
00830           if(!solution_callback.empty())
00831           {
00832             solution_callback(ik_pose, solution, error_code);
00833           }
00834           else
00835           {
00836             error_code.val = error_code.SUCCESS;
00837           }
00838 
00839           if(error_code.val == error_code.SUCCESS)
00840           {
00841             nvalid++;
00842             if (search_mode & OPTIMIZE_MAX_JOINT)
00843             {
00844               // Costs for solution: Largest joint motion
00845               double costs = 0.0;
00846               for(unsigned int i = 0; i < solution.size(); i++)
00847               {
00848                 double d = fabs(ik_seed_state[i] - solution[i]);
00849                 if (d > costs)
00850                   costs = d;
00851               }
00852               if (costs < best_costs || best_costs == -1.0)
00853               {
00854                 best_costs = costs;
00855                 best_solution = solution;
00856               }
00857             }
00858             else
00859               // Return first feasible solution
00860               return true;
00861           }
00862         }
00863       }
00864     }
00865 
00866     if(!getCount(counter, num_positive_increments, -num_negative_increments))
00867     {
00868       // Everything searched
00869       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00870       break;
00871     }
00872 
00873     vfree[0] = initial_guess+search_discretization_*counter;
00874     //ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00875   }
00876 
00877   ROS_DEBUG_STREAM_NAMED("ikfast", "Valid solutions: " << nvalid << "/" << nattempts);
00878 
00879   if ((search_mode & OPTIMIZE_MAX_JOINT) && best_costs != -1.0)
00880   {
00881     solution = best_solution;
00882     error_code.val = error_code.SUCCESS;
00883     return true;
00884   }
00885 
00886   // No solution found
00887   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00888   return false;
00889 }
00890 
00891 // Used when there are no redundant joints - aka no free params
00892 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00893                                            const std::vector<double> &ik_seed_state,
00894                                            std::vector<double> &solution,
00895                                            moveit_msgs::MoveItErrorCodes &error_code,
00896                                            const kinematics::KinematicsQueryOptions &options) const
00897 {
00898   ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00899 
00900   if(!active_)
00901   {
00902     ROS_ERROR("kinematics not active");    
00903     return false;
00904   }
00905 
00906   std::vector<double> vfree(free_params_.size());
00907   for(std::size_t i = 0; i < free_params_.size(); ++i)
00908   {
00909     int p = free_params_[i];
00910     ROS_ERROR("%u is %f",p,ik_seed_state[p]);  // DTC
00911     vfree[i] = ik_seed_state[p];
00912   }
00913 
00914   KDL::Frame frame;
00915   tf::poseMsgToKDL(ik_pose,frame);
00916 
00917   IkSolutionList<IkReal> solutions;
00918   int numsol = solve(frame,vfree,solutions);
00919 
00920   ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00921 
00922   if(numsol)
00923   {
00924     for(int s = 0; s < numsol; ++s)
00925     {
00926       std::vector<double> sol;
00927       getSolution(solutions,s,sol);
00928       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]);
00929 
00930       bool obeys_limits = true;
00931       for(unsigned int i = 0; i < sol.size(); i++)
00932       {
00933         // Add tolerance to limit check
00934         if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00935                                             (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00936         {
00937           // One element of solution is not within limits
00938           obeys_limits = false;
00939           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]);
00940           break;
00941         }
00942       }
00943       if(obeys_limits)
00944       {
00945         // All elements of solution obey limits
00946         getSolution(solutions,s,solution);
00947         error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00948         return true;
00949       }
00950     }
00951   }
00952   else
00953   {
00954     ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
00955   }
00956 
00957   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00958   return false;
00959 }
00960 
00961 
00962 
00963 } // end namespace
00964 
00965 //register IKFastKinematicsPlugin as a KinematicsBase implementation
00966 #include <pluginlib/class_list_macros.h>
00967 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);


fanuc_m6ib_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sat Jun 8 2019 20:44:00