fanuc_m20ia10l_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;
00055 
00056 namespace ikfast_kinematics_plugin
00057 {
00058 
00059 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00060 
00066 enum IkParameterizationType {
00067     IKP_None=0,
00068     IKP_Transform6D=0x67000001,     
00069     IKP_Rotation3D=0x34000002,     
00070     IKP_Translation3D=0x33000003,     
00071     IKP_Direction3D=0x23000004,     
00072     IKP_Ray4D=0x46000005,     
00073     IKP_Lookat3D=0x23000006,     
00074     IKP_TranslationDirection5D=0x56000007,     
00075     IKP_TranslationXY2D=0x22000008,     
00076     IKP_TranslationXYOrientation3D=0x33000009,     
00077     IKP_TranslationLocalGlobal6D=0x3600000a,     
00078 
00079     IKP_TranslationXAxisAngle4D=0x4400000b, 
00080     IKP_TranslationYAxisAngle4D=0x4400000c, 
00081     IKP_TranslationZAxisAngle4D=0x4400000d, 
00082 
00083     IKP_TranslationXAxisAngleZNorm4D=0x4400000e, 
00084     IKP_TranslationYAxisAngleXNorm4D=0x4400000f, 
00085     IKP_TranslationZAxisAngleYNorm4D=0x44000010, 
00086 
00087     IKP_NumberOfParameterizations=16,     
00088 
00089     IKP_VelocityDataBit = 0x00008000, 
00090     IKP_Transform6DVelocity = IKP_Transform6D|IKP_VelocityDataBit,
00091     IKP_Rotation3DVelocity = IKP_Rotation3D|IKP_VelocityDataBit,
00092     IKP_Translation3DVelocity = IKP_Translation3D|IKP_VelocityDataBit,
00093     IKP_Direction3DVelocity = IKP_Direction3D|IKP_VelocityDataBit,
00094     IKP_Ray4DVelocity = IKP_Ray4D|IKP_VelocityDataBit,
00095     IKP_Lookat3DVelocity = IKP_Lookat3D|IKP_VelocityDataBit,
00096     IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D|IKP_VelocityDataBit,
00097     IKP_TranslationXY2DVelocity = IKP_TranslationXY2D|IKP_VelocityDataBit,
00098     IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D|IKP_VelocityDataBit,
00099     IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D|IKP_VelocityDataBit,
00100     IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D|IKP_VelocityDataBit,
00101     IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D|IKP_VelocityDataBit,
00102     IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D|IKP_VelocityDataBit,
00103     IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNorm4D|IKP_VelocityDataBit,
00104     IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNorm4D|IKP_VelocityDataBit,
00105     IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNorm4D|IKP_VelocityDataBit,
00106 
00107     IKP_UniqueIdMask = 0x0000ffff, 
00108     IKP_CustomDataBit = 0x00010000, 
00109 };
00110 
00111 // Code generated by IKFast56/61
00112 #include "fanuc_m20ia10l_manipulator_ikfast_solver.cpp"
00113 
00114 class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00115 {
00116   std::vector<std::string> joint_names_;
00117   std::vector<double> joint_min_vector_;
00118   std::vector<double> joint_max_vector_;
00119   std::vector<bool> joint_has_limits_vector_;
00120   std::vector<std::string> link_names_;
00121   size_t num_joints_;
00122   std::vector<int> free_params_;
00123   bool active_; // Internal variable that indicates whether solvers are configured and ready
00124 
00125   const std::vector<std::string>& getJointNames() const { return joint_names_; }
00126   const std::vector<std::string>& getLinkNames() const { return link_names_; }
00127 
00128 public:
00129 
00133   IKFastKinematicsPlugin():active_(false){}
00134 
00144   // Returns the first IK solution that is within joint limits, this is called by get_ik() service
00145   bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00146                      const std::vector<double> &ik_seed_state,
00147                      std::vector<double> &solution,
00148                      moveit_msgs::MoveItErrorCodes &error_code,
00149                      const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00150 
00159   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00160                         const std::vector<double> &ik_seed_state,
00161                         double timeout,
00162                         std::vector<double> &solution,
00163                         moveit_msgs::MoveItErrorCodes &error_code,
00164                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00165 
00175   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00176                         const std::vector<double> &ik_seed_state,
00177                         double timeout,
00178                         const std::vector<double> &consistency_limits,
00179                         std::vector<double> &solution,
00180                         moveit_msgs::MoveItErrorCodes &error_code,
00181                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00182 
00191   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00192                         const std::vector<double> &ik_seed_state,
00193                         double timeout,
00194                         std::vector<double> &solution,
00195                         const IKCallbackFn &solution_callback,
00196                         moveit_msgs::MoveItErrorCodes &error_code,
00197                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00198 
00209   bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00210                         const std::vector<double> &ik_seed_state,
00211                         double timeout,
00212                         const std::vector<double> &consistency_limits,
00213                         std::vector<double> &solution,
00214                         const IKCallbackFn &solution_callback,
00215                         moveit_msgs::MoveItErrorCodes &error_code,
00216                         const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00217 
00229   bool getPositionFK(const std::vector<std::string> &link_names,
00230                      const std::vector<double> &joint_angles,
00231                      std::vector<geometry_msgs::Pose> &poses) const;
00232 
00233 private:
00234 
00235   bool initialize(const std::string &robot_description,
00236                   const std::string& group_name,
00237                   const std::string& base_name,
00238                   const std::string& tip_name,
00239                   double search_discretization);
00240 
00245   int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const;
00246 
00250   void getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const;
00251 
00252   double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00253   //void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
00254   void getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const;
00255   void fillFreeParams(int count, int *array);
00256   bool getCount(int &count, const int &max_count, const int &min_count) const;
00257 
00258 }; // end class
00259 
00260 bool IKFastKinematicsPlugin::initialize(const std::string &robot_description,
00261                                         const std::string& group_name,
00262                                         const std::string& base_name,
00263                                         const std::string& tip_name,
00264                                         double search_discretization)
00265 {
00266   setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00267 
00268   ros::NodeHandle node_handle("~/"+group_name);
00269 
00270   std::string robot;
00271   node_handle.param("robot",robot,std::string());
00272 
00273   // IKFast56/61
00274   fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00275   num_joints_ = GetNumJoints();
00276 
00277   if(free_params_.size() > 1)
00278   {
00279     ROS_FATAL("Only one free joint paramter supported!");
00280     return false;
00281   }
00282 
00283   urdf::Model robot_model;
00284   std::string xml_string;
00285 
00286   std::string urdf_xml,full_urdf_xml;
00287   node_handle.param("urdf_xml",urdf_xml,robot_description);
00288   node_handle.searchParam(urdf_xml,full_urdf_xml);
00289 
00290   ROS_DEBUG_NAMED("ikfast","Reading xml file from parameter server");
00291   if (!node_handle.getParam(full_urdf_xml, xml_string))
00292   {
00293     ROS_FATAL_NAMED("ikfast","Could not load the xml from parameter server: %s", urdf_xml.c_str());
00294     return false;
00295   }
00296 
00297   node_handle.param(full_urdf_xml,xml_string,std::string());
00298   robot_model.initString(xml_string);
00299 
00300   ROS_DEBUG_STREAM_NAMED("ikfast","Reading joints and links from URDF");
00301 
00302   boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_frame_));
00303   while(link->name != base_frame_ && joint_names_.size() <= num_joints_)
00304   {
00305     ROS_DEBUG_NAMED("ikfast","Link %s",link->name.c_str());
00306     link_names_.push_back(link->name);
00307     boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00308     if(joint)
00309     {
00310       if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
00311       {
00312         ROS_DEBUG_STREAM_NAMED("ikfast","Adding joint " << joint->name );
00313 
00314         joint_names_.push_back(joint->name);
00315         float lower, upper;
00316         int hasLimits;
00317         if ( joint->type != urdf::Joint::CONTINUOUS )
00318         {
00319           if(joint->safety)
00320           {
00321             lower = joint->safety->soft_lower_limit;
00322             upper = joint->safety->soft_upper_limit;
00323           } else {
00324             lower = joint->limits->lower;
00325             upper = joint->limits->upper;
00326           }
00327           hasLimits = 1;
00328         }
00329         else
00330         {
00331           lower = -M_PI;
00332           upper = M_PI;
00333           hasLimits = 0;
00334         }
00335         if(hasLimits)
00336         {
00337           joint_has_limits_vector_.push_back(true);
00338           joint_min_vector_.push_back(lower);
00339           joint_max_vector_.push_back(upper);
00340         }
00341         else
00342         {
00343           joint_has_limits_vector_.push_back(false);
00344           joint_min_vector_.push_back(-M_PI);
00345           joint_max_vector_.push_back(M_PI);
00346         }
00347       }
00348     } else
00349     {
00350       ROS_WARN_NAMED("ikfast","no joint corresponding to %s",link->name.c_str());
00351     }
00352     link = link->getParent();
00353   }
00354 
00355   if(joint_names_.size() != num_joints_)
00356   {
00357     ROS_FATAL_STREAM_NAMED("ikfast","Joint numbers mismatch: URDF has " << joint_names_.size() << " and IKFast has " << num_joints_);
00358     return false;
00359   }
00360 
00361   std::reverse(link_names_.begin(),link_names_.end());
00362   std::reverse(joint_names_.begin(),joint_names_.end());
00363   std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00364   std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00365   std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00366 
00367   for(size_t i=0; i <num_joints_; ++i)
00368     ROS_INFO_STREAM_NAMED("ikfast",joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00369 
00370   active_ = true;
00371   return true;
00372 }
00373 
00374 int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree, IkSolutionList<IkReal> &solutions) const
00375 {
00376   // IKFast56/61
00377   solutions.Clear();
00378 
00379   double trans[3];
00380   trans[0] = pose_frame.p[0];//-.18;
00381   trans[1] = pose_frame.p[1];
00382   trans[2] = pose_frame.p[2];
00383 
00384   KDL::Rotation mult;
00385   KDL::Vector direction;
00386 
00387   switch (GetIkType())
00388   {
00389     case IKP_Transform6D:
00390       // For **Transform6D**, eerot is 9 values for the 3x3 rotation matrix.
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_Translation3D:
00432     case IKP_Lookat3D:
00433     case IKP_TranslationXY2D:
00434     case IKP_TranslationXYOrientation3D:
00435     case IKP_TranslationXAxisAngleZNorm4D:
00436     case IKP_TranslationYAxisAngleXNorm4D:
00437     case IKP_TranslationZAxisAngleYNorm4D:
00438       ROS_ERROR_NAMED("ikfast", "IK for this IkParameterizationType not implemented yet.");
00439       return 0;
00440 
00441     default:
00442       ROS_ERROR_NAMED("ikfast", "Unknown IkParameterizationType! Was the solver generated with an incompatible version of Openrave?");
00443       return 0;
00444   }
00445 }
00446 
00447 void IKFastKinematicsPlugin::getSolution(const IkSolutionList<IkReal> &solutions, int i, std::vector<double>& solution) const
00448 {
00449   solution.clear();
00450   solution.resize(num_joints_);
00451 
00452   // IKFast56/61
00453   const IkSolutionBase<IkReal>& sol = solutions.GetSolution(i);
00454   std::vector<IkReal> vsolfree( sol.GetFree().size() );
00455   sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00456 
00457   // std::cout << "solution " << i << ":" ;
00458   // for(int j=0;j<num_joints_; ++j)
00459   //   std::cout << " " << solution[j];
00460   // std::cout << std::endl;
00461 
00462   //ROS_ERROR("%f %d",solution[2],vsolfree.size());
00463 }
00464 
00465 double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00466 {
00467   double dist_sqr = 0;
00468   std::vector<double> ss = ik_seed_state;
00469   for(size_t i=0; i< ik_seed_state.size(); ++i)
00470   {
00471     while(ss[i] > 2*M_PI) {
00472       ss[i] -= 2*M_PI;
00473     }
00474     while(ss[i] < 2*M_PI) {
00475       ss[i] += 2*M_PI;
00476     }
00477     while(solution[i] > 2*M_PI) {
00478       solution[i] -= 2*M_PI;
00479     }
00480     while(solution[i] < 2*M_PI) {
00481       solution[i] += 2*M_PI;
00482     }
00483     dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00484   }
00485   return dist_sqr;
00486 }
00487 
00488 // void IKFastKinematicsPlugin::getOrderedSolutions(const std::vector<double> &ik_seed_state,
00489 //                                  std::vector<std::vector<double> >& solslist)
00490 // {
00491 //   std::vector<double>
00492 //   double mindist = 0;
00493 //   int minindex = -1;
00494 //   std::vector<double> sol;
00495 //   for(size_t i=0;i<solslist.size();++i){
00496 //     getSolution(i,sol);
00497 //     double dist = harmonize(ik_seed_state, sol);
00498 //     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00499 //     if(minindex == -1 || dist<mindist){
00500 //       minindex = i;
00501 //       mindist = dist;
00502 //     }
00503 //   }
00504 //   if(minindex >= 0){
00505 //     getSolution(minindex,solution);
00506 //     harmonize(ik_seed_state, solution);
00507 //     index = minindex;
00508 //   }
00509 // }
00510 
00511 void IKFastKinematicsPlugin::getClosestSolution(const IkSolutionList<IkReal> &solutions, const std::vector<double> &ik_seed_state, std::vector<double> &solution) const
00512 {
00513   double mindist = DBL_MAX;
00514   int minindex = -1;
00515   std::vector<double> sol;
00516 
00517   // IKFast56/61
00518   for(size_t i=0; i < solutions.GetNumSolutions(); ++i)
00519   {
00520     getSolution(solutions, i,sol);
00521     double dist = harmonize(ik_seed_state, sol);
00522     ROS_INFO_STREAM_NAMED("ikfast","Dist " << i << " dist " << dist);
00523     //std::cout << "dist[" << i << "]= " << dist << std::endl;
00524     if(minindex == -1 || dist<mindist){
00525       minindex = i;
00526       mindist = dist;
00527     }
00528   }
00529   if(minindex >= 0){
00530     getSolution(solutions, minindex,solution);
00531     harmonize(ik_seed_state, solution);
00532   }
00533 }
00534 
00535 void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00536 {
00537   free_params_.clear();
00538   for(int i=0; i<count;++i) free_params_.push_back(array[i]);
00539 }
00540 
00541 bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count) const
00542 {
00543   if(count > 0)
00544   {
00545     if(-count >= min_count)
00546     {
00547       count = -count;
00548       return true;
00549     }
00550     else if(count+1 <= max_count)
00551     {
00552       count = count+1;
00553       return true;
00554     }
00555     else
00556     {
00557       return false;
00558     }
00559   }
00560   else
00561   {
00562     if(1-count <= max_count)
00563     {
00564       count = 1-count;
00565       return true;
00566     }
00567     else if(count-1 >= min_count)
00568     {
00569       count = count -1;
00570       return true;
00571     }
00572     else
00573       return false;
00574   }
00575 }
00576 
00577 bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00578                                            const std::vector<double> &joint_angles,
00579                                            std::vector<geometry_msgs::Pose> &poses) const
00580 {
00581 #ifndef IKTYPE_TRANSFORM_6D
00582   ROS_ERROR_NAMED("ikfast", "Can only compute FK for IKTYPE_TRANSFORM_6D!");
00583   return false;
00584 #endif
00585 
00586   KDL::Frame p_out;
00587   if(link_names.size() == 0) {
00588     ROS_WARN_STREAM_NAMED("ikfast","Link names with nothing");
00589     return false;
00590   }
00591 
00592   if(link_names.size()!=1 || link_names[0]!=tip_frame_){
00593     ROS_ERROR_NAMED("ikfast","Can compute FK for %s only",tip_frame_.c_str());
00594     return false;
00595   }
00596 
00597   bool valid = true;
00598 
00599   IkReal eerot[9],eetrans[3];
00600   IkReal angles[joint_angles.size()];
00601   for (unsigned char i=0; i < joint_angles.size(); i++)
00602     angles[i] = joint_angles[i];
00603 
00604   // IKFast56/61
00605   ComputeFk(angles,eetrans,eerot);
00606 
00607   for(int i=0; i<3;++i)
00608     p_out.p.data[i] = eetrans[i];
00609 
00610   for(int i=0; i<9;++i)
00611     p_out.M.data[i] = eerot[i];
00612 
00613   poses.resize(1);
00614   tf::poseKDLToMsg(p_out,poses[0]);
00615 
00616   return valid;
00617 }
00618 
00619 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00620                                            const std::vector<double> &ik_seed_state,
00621                                            double timeout,
00622                                            std::vector<double> &solution,
00623                                            moveit_msgs::MoveItErrorCodes &error_code,
00624                                            const kinematics::KinematicsQueryOptions &options) const
00625 {
00626   const IKCallbackFn solution_callback = 0; 
00627   std::vector<double> consistency_limits;
00628 
00629   return searchPositionIK(ik_pose,
00630                           ik_seed_state,
00631                           timeout,
00632                           consistency_limits,
00633                           solution,
00634                           solution_callback,
00635                           error_code,
00636                           options);
00637 }
00638     
00639 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00640                                            const std::vector<double> &ik_seed_state,
00641                                            double timeout,
00642                                            const std::vector<double> &consistency_limits,
00643                                            std::vector<double> &solution,
00644                                            moveit_msgs::MoveItErrorCodes &error_code,
00645                                            const kinematics::KinematicsQueryOptions &options) const
00646 {
00647   const IKCallbackFn solution_callback = 0; 
00648   return searchPositionIK(ik_pose,
00649                           ik_seed_state,
00650                           timeout,
00651                           consistency_limits,
00652                           solution,
00653                           solution_callback,
00654                           error_code,
00655                           options);
00656 }
00657 
00658 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00659                                            const std::vector<double> &ik_seed_state,
00660                                            double timeout,
00661                                            std::vector<double> &solution,
00662                                            const IKCallbackFn &solution_callback,
00663                                            moveit_msgs::MoveItErrorCodes &error_code,
00664                                            const kinematics::KinematicsQueryOptions &options) const
00665 {
00666   std::vector<double> consistency_limits;
00667   return searchPositionIK(ik_pose,
00668                           ik_seed_state,
00669                           timeout,
00670                           consistency_limits,
00671                           solution,
00672                           solution_callback,
00673                           error_code,
00674                           options);
00675 }
00676 
00677 bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00678                                               const std::vector<double> &ik_seed_state,
00679                                               double timeout,
00680                                               const std::vector<double> &consistency_limits,
00681                                               std::vector<double> &solution,
00682                                               const IKCallbackFn &solution_callback,
00683                                               moveit_msgs::MoveItErrorCodes &error_code,
00684                                               const kinematics::KinematicsQueryOptions &options) const
00685 {
00686   ROS_DEBUG_STREAM_NAMED("ikfast","searchPositionIK");
00687 
00688   // Check if there are no redundant joints
00689   if(free_params_.size()==0)
00690   {
00691     ROS_DEBUG_STREAM_NAMED("ikfast","No need to search since no free params/redundant joints");
00692 
00693     // Find first IK solution, within joint limits
00694     if(!getPositionIK(ik_pose, ik_seed_state, solution, error_code))
00695     {
00696       ROS_DEBUG_STREAM_NAMED("ikfast","No solution whatsoever");
00697       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00698       return false;
00699     }
00700 
00701     // check for collisions if a callback is provided
00702     if( !solution_callback.empty() )
00703     {
00704       solution_callback(ik_pose, solution, error_code);
00705       if(error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
00706       {
00707         ROS_DEBUG_STREAM_NAMED("ikfast","Solution passes callback");
00708         return true;
00709       }
00710       else
00711       {
00712         ROS_DEBUG_STREAM_NAMED("ikfast","Solution has error code " << error_code);
00713         return false;
00714       }
00715     }
00716     else
00717     {
00718       return true; // no collision check callback provided
00719     }
00720   }
00721 
00722   // -------------------------------------------------------------------------------------------------
00723   // Error Checking
00724   if(!active_)
00725   {
00726     ROS_ERROR_STREAM_NAMED("ikfast","Kinematics not active");
00727     error_code.val = error_code.NO_IK_SOLUTION;
00728     return false;
00729   }
00730 
00731   if(ik_seed_state.size() != num_joints_)
00732   {
00733     ROS_ERROR_STREAM_NAMED("ikfast","Seed state must have size " << num_joints_ << " instead of size " << ik_seed_state.size());
00734     error_code.val = error_code.NO_IK_SOLUTION;
00735     return false;
00736   }
00737 
00738   if(!consistency_limits.empty() && consistency_limits.size() != num_joints_)
00739   {
00740     ROS_ERROR_STREAM_NAMED("ikfast","Consistency limits be empty or must have size " << num_joints_ << " instead of size " << consistency_limits.size());
00741     error_code.val = error_code.NO_IK_SOLUTION;
00742     return false;
00743   }
00744 
00745 
00746   // -------------------------------------------------------------------------------------------------
00747   // Initialize
00748 
00749   KDL::Frame frame;
00750   tf::poseMsgToKDL(ik_pose,frame);
00751 
00752   std::vector<double> vfree(free_params_.size());
00753 
00754   ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00755   int counter = 0;
00756 
00757   double initial_guess = ik_seed_state[free_params_[0]];
00758   vfree[0] = initial_guess;
00759 
00760   // -------------------------------------------------------------------------------------------------
00761   // Handle consitency limits if needed
00762   int num_positive_increments;
00763   int num_negative_increments;
00764 
00765   if(!consistency_limits.empty())
00766   {
00767     // moveit replaced consistency_limit (scalar) w/ consistency_limits (vector)
00768     // Assume [0]th free_params element for now.  Probably wrong.
00769     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limits[free_params_[0]]);
00770     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limits[free_params_[0]]);
00771 
00772     num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00773     num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00774   }
00775   else // no consitency limits provided
00776   {
00777     num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00778     num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00779   }
00780 
00781   // -------------------------------------------------------------------------------------------------
00782   // Begin searching
00783 
00784   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);
00785 
00786   while(true)
00787   {
00788     IkSolutionList<IkReal> solutions;
00789     int numsol = solve(frame,vfree, solutions);
00790 
00791     ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00792 
00793     //ROS_INFO("%f",vfree[0]);
00794 
00795     if( numsol > 0 )
00796     {
00797       for(int s = 0; s < numsol; ++s)
00798       {
00799         std::vector<double> sol;
00800         getSolution(solutions,s,sol);
00801 
00802         bool obeys_limits = true;
00803         for(unsigned int i = 0; i < sol.size(); i++)
00804         {
00805           if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i]))
00806           {
00807             obeys_limits = false;
00808             break;
00809           }
00810           //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]);
00811         }
00812         if(obeys_limits)
00813         {
00814           getSolution(solutions,s,solution);
00815 
00816           // This solution is within joint limits, now check if in collision (if callback provided)
00817           if(!solution_callback.empty())
00818           {
00819             solution_callback(ik_pose, solution, error_code);
00820           }
00821           else
00822           {
00823             error_code.val = error_code.SUCCESS;
00824           }
00825 
00826           if(error_code.val == error_code.SUCCESS)
00827           {
00828             return true;
00829           }
00830         }
00831       }
00832     }
00833 
00834     if(!getCount(counter, num_positive_increments, num_negative_increments))
00835     {
00836       error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00837       return false;
00838     }
00839 
00840     vfree[0] = initial_guess+search_discretization_*counter;
00841     ROS_DEBUG_STREAM_NAMED("ikfast","Attempt " << counter << " with 0th free joint having value " << vfree[0]);
00842   }
00843 
00844   // not really needed b/c shouldn't ever get here
00845   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00846   return false;
00847 }
00848 
00849 // Used when there are no redundant joints - aka no free params
00850 bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00851                                            const std::vector<double> &ik_seed_state,
00852                                            std::vector<double> &solution,
00853                                            moveit_msgs::MoveItErrorCodes &error_code,
00854                                            const kinematics::KinematicsQueryOptions &options) const
00855 {
00856   ROS_DEBUG_STREAM_NAMED("ikfast","getPositionIK");
00857 
00858   if(!active_)
00859   {
00860     ROS_ERROR("kinematics not active");    
00861     return false;
00862   }
00863 
00864   std::vector<double> vfree(free_params_.size());
00865   for(std::size_t i = 0; i < free_params_.size(); ++i)
00866   {
00867     int p = free_params_[i];
00868     ROS_ERROR("%u is %f",p,ik_seed_state[p]);  // DTC
00869     vfree[i] = ik_seed_state[p];
00870   }
00871 
00872   KDL::Frame frame;
00873   tf::poseMsgToKDL(ik_pose,frame);
00874 
00875   IkSolutionList<IkReal> solutions;
00876   int numsol = solve(frame,vfree,solutions);
00877 
00878   ROS_DEBUG_STREAM_NAMED("ikfast","Found " << numsol << " solutions from IKFast");
00879 
00880   if(numsol)
00881   {
00882     for(int s = 0; s < numsol; ++s)
00883     {
00884       std::vector<double> sol;
00885       getSolution(solutions,s,sol);
00886       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]);
00887 
00888       bool obeys_limits = true;
00889       for(unsigned int i = 0; i < sol.size(); i++)
00890       {
00891         // Add tolerance to limit check
00892         if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) ||
00893                                             (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) )
00894         {
00895           // One element of solution is not within limits
00896           obeys_limits = false;
00897           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]);
00898           break;
00899         }
00900       }
00901       if(obeys_limits)
00902       {
00903         // All elements of solution obey limits
00904         getSolution(solutions,s,solution);
00905         error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00906         return true;
00907       }
00908     }
00909   }
00910   else
00911   {
00912     ROS_DEBUG_STREAM_NAMED("ikfast","No IK solution");
00913   }
00914 
00915   error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00916   return false;
00917 }
00918 
00919 
00920 
00921 } // end namespace
00922 
00923 //register IKFastKinematicsPlugin as a KinematicsBase implementation
00924 #include <pluginlib/class_list_macros.h>
00925 PLUGINLIB_EXPORT_CLASS(ikfast_kinematics_plugin::IKFastKinematicsPlugin, kinematics::KinematicsBase);


fanuc_m20ia_moveit_plugins
Author(s): G.A. vd. Hoorn (TU Delft Robotics Institute)
autogenerated on Sat Jun 8 2019 20:43:53