M16iB20_manipulator_ikfast_plugin.cpp
Go to the documentation of this file.
00001 /*
00002  * IKFast Plugin Template for arm_kinematics_constraint_aware
00003  *
00004  * You should run create_ikfast_plugin.py to generate your own plugin.
00005  *
00006  *
00007  * This plugin and the constraint aware kinematics node can be used as a general 
00008  * kinematics service, from within the arm_navigation planning warehouse, or in 
00009  * your own ROS node.
00010  *
00011  * Tested on ROS Fuerte
00012  * with code generated by IKFast56/61 from OpenRAVE 0.8.2
00013  * using a 6 DOF manipulator
00014  *
00015  * Note: you should apply the patch for arm_kinematics_constraint_aware 
00016  * mentioned here: https://code.ros.org/trac/ros-pkg/ticket/5586
00017  * This fixes a bug in the FK routine, and also allows you to switch from using
00018  * TF by default for forward kinematics, to the FK routine from this plugin.
00019  *
00020  *
00021  * Author: David Butterworth, KAIST
00022  *         Based on original plugin by unknown author
00023  * Date: November 2012
00024  */
00025 
00026 /*
00027  * Copyright (c) 2012, David Butterworth, KAIST
00028  * All rights reserved.
00029  *
00030  * Redistribution and use in source and binary forms, with or without
00031  * modification, are permitted provided that the following conditions are met:
00032  *
00033  *     * Redistributions of source code must retain the above copyright
00034  *       notice, this list of conditions and the following disclaimer.
00035  *     * Redistributions in binary form must reproduce the above copyright
00036  *       notice, this list of conditions and the following disclaimer in the
00037  *       documentation and/or other materials provided with the distribution.
00038  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00039  *       contributors may be used to endorse or promote products derived from
00040  *       this software without specific prior written permission.
00041  *
00042  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00043  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00044  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00045  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00046  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00047  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00048  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00049  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00050  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00051  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00052  * POSSIBILITY OF SUCH DAMAGE.
00053  */
00054 
00055 /*
00056  * Add closest solution check to the getPositionIK() function
00057  * Mahisorn Wongphati, Keio University
00058  * Data 2013-01-25
00059  */
00060 
00061 // Auto-generated by create_ikfast_plugin.py in arm_kinematics_tools
00062 #include <ros/ros.h>
00063 #include <kinematics_base/kinematics_base.h>
00064 #include <urdf/model.h>
00065 #include <tf_conversions/tf_kdl.h>
00066 #include <angles/angles.h>
00067 
00068 // Need a floating point tolerance when checking joint limits, in case the joint starts at limit
00069 const double LIMIT_TOLERANCE = .0000001;
00070 
00071 namespace M16iB20_manipulator_kinematics
00072 {
00073 #define IKFAST_HAS_LIBRARY // Build IKFast with API functions
00074 #define IKFAST_NO_MAIN // Don't include main() from IKFast
00075 // Code generated by IKFast56/61
00076 #include "M16iB20_manipulator_ikfast_solver.cpp"
00077 
00078   class IKFastKinematicsPlugin : public kinematics::KinematicsBase
00079   {
00080     std::vector<std::string> joint_names_;
00081     std::vector<double> joint_min_vector_;
00082     std::vector<double> joint_max_vector_;
00083     std::vector<bool> joint_has_limits_vector_;
00084     std::vector<std::string> link_names_;
00085     size_t num_joints_;
00086     std::vector<int> free_params_;
00087 
00088     // IKFast56/61
00089     IkSolutionList<IkReal> solutions_;
00090 
00091     const std::vector<std::string>& getJointNames() const { return joint_names_; }
00092     const std::vector<std::string>& getLinkNames() const { return link_names_; }
00093 
00094     public:
00095 
00099       IKFastKinematicsPlugin() {}
00100 
00108       // This FK routine is only used if 'use_plugin_fk' is set in the 'arm_kinematics_constraint_aware' node,
00109       // otherwise ROS TF is used to calculate the forward kinematics
00110       bool getPositionFK(const std::vector<std::string> &link_names,
00111                          const std::vector<double> &joint_angles, 
00112                          std::vector<geometry_msgs::Pose> &poses);
00113     
00120       // Returns the first IK solution that is within joint limits,
00121       // this is called by get_ik() service
00122       bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00123                          const std::vector<double> &ik_seed_state,
00124                          std::vector<double> &solution,
00125                          int &error_code);
00126 
00135       bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00136                           const std::vector<double> &ik_seed_state,
00137                           const double &timeout,
00138                           std::vector<double> &solution,
00139                           int &error_code);
00140 
00150       bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00151                             const std::vector<double> &ik_seed_state,
00152                             const double &timeout,
00153                             const unsigned int& redundancy,
00154                             const double &consistency_limit,
00155                             std::vector<double> &solution,
00156                             int &error_code);
00157 
00166       // searchPositionIK #3 - used by planning scene warehouse
00167       bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00168                           const std::vector<double> &ik_seed_state,
00169                           const double &timeout,
00170                           std::vector<double> &solution,
00171                           const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00172                                                                                            int &error_code)> &desired_pose_callback,
00173                           const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00174                                                                                                int &error_code)> &solution_callback,
00175                           int &error_code);
00176 
00187         bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00188                               const std::vector<double> &ik_seed_state,
00189                               const double &timeout,
00190                               const unsigned int& redundancy,
00191                               const double &consistency_limit,
00192                               std::vector<double> &solution,
00193                               const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00194                                                                                               int &error_code)> &desired_pose_callback,
00195                               const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00196                                                                                                   int &error_code)> &solution_callback,
00197                               int &error_code);
00198 
00199     private:
00200 
00201       bool initialize(const std::string& group_name, const std::string& base_name, const std::string& tip_name, const double& search_discretization);
00202 
00207       int solve(KDL::Frame &pose_frame, const std::vector<double> &vfree);
00208 
00212       void getSolution(int i, std::vector<double>& solution);
00213 
00214       double harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution);
00215       double harmonize_old(const std::vector<double> &ik_seed_state, std::vector<double> &solution);
00216       //void getOrderedSolutions(const std::vector<double> &ik_seed_state, std::vector<std::vector<double> >& solslist);
00217       void getClosestSolution(const std::vector<double> &ik_seed_state, std::vector<double> &solution);
00218       int getClosestSolutionIndex(const std::vector<double> &ik_seed_state,
00219                                   const std::vector<std::vector<double> > &solutions);
00220       void fillFreeParams(int count, int *array);
00221       bool getCount(int &count, const int &max_count, const int &min_count);
00222 
00223   }; // end class
00224 
00225   bool IKFastKinematicsPlugin::initialize(const std::string& group_name,
00226                                           const std::string& base_name,
00227                                           const std::string& tip_name,
00228                                           const double& search_discretization) 
00229   {
00230     setValues(group_name, base_name, tip_name,search_discretization);
00231 
00232     ros::NodeHandle node_handle("~/"+group_name);
00233 
00234     std::string robot;
00235     node_handle.param("robot",robot,std::string());
00236     
00237     // IKFast56/61
00238     fillFreeParams( GetNumFreeParameters(), GetFreeParameters() );
00239     num_joints_ = GetNumJoints();
00240 
00241     if(free_params_.size()>1){
00242       ROS_FATAL("Only one free joint paramter supported!");
00243       return false;
00244     }
00245       
00246     urdf::Model robot_model;
00247     std::string xml_string;
00248 
00249     std::string urdf_xml,full_urdf_xml;
00250     node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
00251     node_handle.searchParam(urdf_xml,full_urdf_xml);
00252 
00253     ROS_DEBUG("Reading xml file from parameter server\n");
00254     if (!node_handle.getParam(full_urdf_xml, xml_string))
00255     {
00256       ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
00257       return false;
00258     }
00259 
00260     node_handle.param(full_urdf_xml,xml_string,std::string());
00261     robot_model.initString(xml_string);
00262 
00263     boost::shared_ptr<urdf::Link> link = boost::const_pointer_cast<urdf::Link>(robot_model.getLink(tip_name_));
00264     while(link->name != base_name_ && joint_names_.size() <= num_joints_){
00265       //        ROS_INFO("link %s",link->name.c_str());
00266       link_names_.push_back(link->name);
00267       boost::shared_ptr<urdf::Joint> joint = link->parent_joint;
00268       if(joint){
00269         if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
00270           joint_names_.push_back(joint->name);
00271           float lower, upper;
00272           int hasLimits;
00273           if ( joint->type != urdf::Joint::CONTINUOUS ) {
00274             if(joint->safety) {
00275               lower = joint->safety->soft_lower_limit; 
00276               upper = joint->safety->soft_upper_limit;
00277             } else {
00278               lower = joint->limits->lower;
00279               upper = joint->limits->upper;
00280             }
00281             hasLimits = 1;
00282           } else {
00283             lower = -M_PI;
00284             upper = M_PI;
00285             hasLimits = 0;
00286           }
00287           if(hasLimits) {
00288             joint_has_limits_vector_.push_back(true);
00289             joint_min_vector_.push_back(lower);
00290             joint_max_vector_.push_back(upper);
00291           } else {
00292             joint_has_limits_vector_.push_back(false);
00293             joint_min_vector_.push_back(-M_PI);
00294             joint_max_vector_.push_back(M_PI);
00295           }
00296         }
00297       } else{
00298         ROS_WARN("no joint corresponding to %s",link->name.c_str());
00299       }
00300       link = link->getParent();
00301     }
00302     
00303     if(joint_names_.size() != num_joints_){
00304       ROS_FATAL("Joints number mismatch.");
00305       return false;
00306     }
00307       
00308     std::reverse(link_names_.begin(),link_names_.end());
00309     std::reverse(joint_names_.begin(),joint_names_.end());
00310     std::reverse(joint_min_vector_.begin(),joint_min_vector_.end());
00311     std::reverse(joint_max_vector_.begin(),joint_max_vector_.end());
00312     std::reverse(joint_has_limits_vector_.begin(), joint_has_limits_vector_.end());
00313 
00314     for(size_t i=0; i <num_joints_; ++i)
00315       ROS_INFO_STREAM(joint_names_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i] << " " << joint_has_limits_vector_[i]);
00316 
00317     return true;
00318   }
00319 
00320   int IKFastKinematicsPlugin::solve(KDL::Frame &pose_frame, const std::vector<double> &vfree)
00321   {
00322     // IKFast56/61
00323     solutions_.Clear();
00324 
00325     //KDL::Rotation rot = KDL::Rotation::RotY(M_PI/2);
00326     KDL::Rotation orig = pose_frame.M;
00327     KDL::Rotation mult = orig;//*rot;
00328 
00329     double vals[9];
00330     vals[0] = mult(0,0);
00331     vals[1] = mult(0,1);
00332     vals[2] = mult(0,2);
00333     vals[3] = mult(1,0);
00334     vals[4] = mult(1,1);
00335     vals[5] = mult(1,2);
00336     vals[6] = mult(2,0);
00337     vals[7] = mult(2,1);
00338     vals[8] = mult(2,2);
00339 
00340     double trans[3];
00341     trans[0] = pose_frame.p[0];//-.18;
00342     trans[1] = pose_frame.p[1];
00343     trans[2] = pose_frame.p[2];
00344 
00345     // IKFast56/61
00346     ComputeIk(trans, vals, vfree.size() > 0 ? &vfree[0] : NULL, solutions_);
00347     return solutions_.GetNumSolutions();
00348   }
00349 
00350   void IKFastKinematicsPlugin::getSolution(int i, std::vector<double>& solution)
00351   {
00352     solution.clear();
00353     solution.resize(num_joints_);
00354 
00355     // IKFast56/61
00356     const IkSolutionBase<IkReal>& sol = solutions_.GetSolution(i);
00357     std::vector<IkReal> vsolfree( sol.GetFree().size() );
00358     sol.GetSolution(&solution[0],vsolfree.size()>0?&vsolfree[0]:NULL);
00359 
00360     // std::cout << "solution " << i << ":" ;
00361     // for(int j=0;j<num_joints_; ++j)
00362     //   std::cout << " " << solution[j];
00363     // std::cout << std::endl;
00364           
00365     //ROS_ERROR("%f %d",solution[2],vsolfree.size());
00366   }
00367 
00368   double IKFastKinematicsPlugin::harmonize_old(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00369   {
00370     double dist_sqr = 0;
00371     std::vector<double> ss = ik_seed_state;
00372     for(size_t i=0; i< ik_seed_state.size(); ++i)
00373     {
00374       while(ss[i] > 2*M_PI) {
00375         ss[i] -= 2*M_PI;
00376       }
00377       while(ss[i] < 2*M_PI) {
00378         ss[i] += 2*M_PI;
00379       }
00380       while(solution[i] > 2*M_PI) {
00381         solution[i] -= 2*M_PI;
00382       }
00383       while(solution[i] < 2*M_PI) {
00384         solution[i] += 2*M_PI;
00385       }
00386       dist_sqr += fabs(ik_seed_state[i] - solution[i]);
00387     }
00388     return dist_sqr;
00389   }
00390 
00391   double IKFastKinematicsPlugin::harmonize(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00392   {
00393     double dist_sqr = 0;
00394     for(size_t i=0; i< ik_seed_state.size(); ++i){
00395       double diff = angles::shortest_angular_distance(ik_seed_state[i], solution[i]);
00396       dist_sqr += fabs(diff);
00397     }
00398     return dist_sqr;
00399   }
00400 
00401   void IKFastKinematicsPlugin::getClosestSolution(const std::vector<double> &ik_seed_state, std::vector<double> &solution)
00402   {
00403     double mindist = DBL_MAX;
00404     int minindex = -1;
00405     std::vector<double> sol;
00406 
00407     // IKFast56/61
00408     for(size_t i=0; i < solutions_.GetNumSolutions(); ++i)
00409     {
00410       getSolution(i,sol);
00411       double dist = harmonize(ik_seed_state, sol);
00412       ROS_INFO_STREAM("Dist " << i << " dist " << dist);
00413       //std::cout << "dist[" << i << "]= " << dist << std::endl;
00414       if(minindex == -1 || dist<mindist){
00415         minindex = i;
00416         mindist = dist;
00417       }
00418     }
00419     if(minindex >= 0){
00420       getSolution(minindex,solution);
00421       harmonize(ik_seed_state, solution);
00422     }
00423   }
00424 
00425   int IKFastKinematicsPlugin::getClosestSolutionIndex(const std::vector<double> &ik_seed_state,
00426                                                       const std::vector<std::vector<double> > &solutions)
00427   {
00428     double mindist = DBL_MAX;
00429     int minindex = -1;
00430     std::vector<double> sol;
00431 
00432     // IKFast56/61
00433     double dist;
00434     for(size_t i=0; i<solutions.size();++i){
00435       sol = solutions[i];
00436       dist = harmonize(ik_seed_state, sol);
00437       ROS_DEBUG_STREAM("Dist " << i << " dist " << dist);
00438       if(minindex==-1 || dist<mindist){
00439         minindex = i;
00440         mindist = dist;
00441       }
00442     }
00443     return minindex;
00444   }
00445 
00446   void IKFastKinematicsPlugin::fillFreeParams(int count, int *array)
00447   { 
00448     free_params_.clear(); 
00449     for(int i=0; i<count;++i) free_params_.push_back(array[i]); 
00450   }
00451   
00452   bool IKFastKinematicsPlugin::getCount(int &count, const int &max_count, const int &min_count)
00453   {
00454     if(count > 0)
00455     {
00456       if(-count >= min_count)
00457       {   
00458         count = -count;
00459         return true;
00460       }
00461       else if(count+1 <= max_count)
00462       {
00463         count = count+1;
00464         return true;
00465       }
00466       else
00467       {
00468         return false;
00469       }
00470     }
00471     else
00472     {
00473       if(1-count <= max_count)
00474       {
00475         count = 1-count;
00476         return true;
00477       }
00478       else if(count-1 >= min_count)
00479       {
00480         count = count -1;
00481         return true;
00482       }
00483       else
00484         return false;
00485     }
00486   }
00487 
00488   bool IKFastKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00489                                              const std::vector<double> &joint_angles, 
00490                                              std::vector<geometry_msgs::Pose> &poses)
00491   {
00492     KDL::Frame p_out;
00493     if(link_names.size() == 0) {
00494       ROS_WARN_STREAM("Link names with nothing");
00495       return false;
00496     }
00497 
00498     if(link_names.size()!=1 || link_names[0]!=tip_name_){
00499       ROS_ERROR("Can compute FK for %s only",tip_name_.c_str());
00500       return false;
00501     }
00502         
00503     bool valid = true;
00504 
00505     IkReal eerot[9],eetrans[3];
00506     IkReal angles[joint_angles.size()];
00507     for (unsigned char i=0; i < joint_angles.size(); i++) angles[i] = joint_angles[i];
00508   
00509     // IKFast56/61
00510     ComputeFk(angles,eetrans,eerot);
00511 
00512     for(int i=0; i<3;++i) p_out.p.data[i] = eetrans[i];
00513     for(int i=0; i<9;++i) p_out.M.data[i] = eerot[i];
00514     poses.resize(1);
00515     tf::PoseKDLToMsg(p_out,poses[0]);   
00516 
00517     return valid;
00518   }  
00519 
00520   bool IKFastKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00521                                              const std::vector<double> &ik_seed_state,
00522                                              std::vector<double> &solution,
00523                                              int &error_code)
00524   {
00525     std::vector<double> vfree(free_params_.size());
00526     for(std::size_t i = 0; i < free_params_.size(); ++i){
00527       int p = free_params_[i];
00528       // ROS_ERROR("%u is %f",p,ik_seed_state[p]);
00529       vfree[i] = ik_seed_state[p];
00530     }
00531 
00532     KDL::Frame frame;
00533     tf::PoseMsgToKDL(ik_pose,frame);
00534 
00535     int numsol = solve(frame,vfree);
00536     ROS_DEBUG("Got %d solution", numsol);
00537                 
00538     std::vector<double> tmp_solution;
00539     std::vector<std::vector<double> > solutions;
00540 
00541     if(numsol){
00542       for(int s = 0; s < numsol; ++s)
00543       {
00544         std::vector<double> sol;
00545         getSolution(s,sol);
00546         //printf("Sol %d: %e   %e   %e   %e   %e   %e    \n", s, sol[0], sol[1], sol[2], sol[3], sol[4], sol[5] );
00547 
00548         bool obeys_limits = true;
00549         for(unsigned int i = 0; i < sol.size(); i++) {
00550           // Add tolerance to limit check
00551           if(joint_has_limits_vector_[i] && ( (sol[i] < (joint_min_vector_[i]-LIMIT_TOLERANCE)) || 
00552                                                                      (sol[i] > (joint_max_vector_[i]+LIMIT_TOLERANCE)) ) ) 
00553           {
00554             // One element of solution is not within limits
00555             obeys_limits = false;
00556             //ROS_INFO_STREAM("      Num " << i << " value " << sol[i] << " has limit: " << joint_has_limits_vector_[i] << "  being  " << joint_min_vector_[i] << " to " << joint_max_vector_[i] << "\n");
00557             break;
00558           }
00559         }
00560         if(obeys_limits) {
00561           // All elements of solution obey limits
00562           getSolution(s, tmp_solution);
00563           solutions.push_back(tmp_solution);
00564         }
00565       }
00566 
00567       if(solutions.size()!=0){
00568         ROS_DEBUG("Found %lu valid solution(s)", solutions.size());
00569         solution = solutions[getClosestSolutionIndex(ik_seed_state, solutions)];
00570         error_code = kinematics::SUCCESS;
00571         return true;
00572       }
00573     }else{
00574       //printf("No IK solution \n");
00575     }
00576         
00577     error_code = kinematics::NO_IK_SOLUTION; 
00578     return false;
00579   }
00580 
00581   // searchPositionIK #1
00582   bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00583                                                 const std::vector<double> &ik_seed_state,
00584                                                 const double &timeout,
00585                                                 std::vector<double> &solution,
00586                                                 int &error_code) 
00587   {
00588     if(free_params_.size()==0){
00589       return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00590     }
00591         
00592     KDL::Frame frame;
00593     tf::PoseMsgToKDL(ik_pose,frame);
00594 
00595     std::vector<double> vfree(free_params_.size());
00596 
00597     ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00598     int counter = 0;
00599 
00600     double initial_guess = ik_seed_state[free_params_[0]];
00601     vfree[0] = initial_guess;
00602 
00603     int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00604     int num_negative_increments = (initial_guess-joint_min_vector_[free_params_[0]])/search_discretization_;
00605 
00606     //ROS_INFO_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00607 
00608     while(1) {
00609       int numsol = solve(frame,vfree);
00610       //ROS_INFO_STREAM("Solutions number is " << numsol);
00611       //ROS_INFO("%f",vfree[0]);
00612             
00613       if(numsol > 0){
00614         for(int s = 0; s < numsol; ++s){
00615           std::vector<double> sol;
00616           getSolution(s,sol);
00617 
00618           bool obeys_limits = true;
00619           for(unsigned int i = 0; i < sol.size(); i++) {
00620             if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00621               obeys_limits = false;
00622               break;
00623             }
00624             //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00625           }
00626           if(obeys_limits) {
00627             getSolution(s,solution);
00628             error_code = kinematics::SUCCESS;
00629             return true;
00630           }
00631         }
00632       }
00633       
00634       // if(numsol > 0){
00635       //   for(unsigned int i = 0; i < sol.size(); i++) {
00636       //     if(i == 0) {
00637       //       //ik_solver_->getClosestSolution(ik_seed_state,solution);
00638       //       getClosestSolution(ik_seed_state,solution);
00639       //     } else {
00640       //       //ik_solver_->getSolution(s,sol);  
00641       //       getSolution(s,sol);          
00642       //     }
00643       //   }
00644       //   bool obeys_limits = true;
00645       //   for(unsigned int i = 0; i < solution.size(); i++) {
00646       //     if(joint_has_limits_vector_[i] && (solution[i] < joint_min_vector_[i] || solution[i] > joint_max_vector_[i])) {
00647       //       obeys_limits = false;
00648       //       break;
00649       //     }
00650       //     //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00651       //   }
00652       //   if(obeys_limits) {
00653       //     error_code = kinematics::SUCCESS;
00654       //     return true;
00655       //   }
00656       // }
00657       if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00658         error_code = kinematics::NO_IK_SOLUTION; 
00659         return false;
00660       }
00661       
00662       vfree[0] = initial_guess+search_discretization_*counter;
00663       ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00664     }
00665     //shouldn't ever get here
00666     error_code = kinematics::NO_IK_SOLUTION; 
00667     return false;
00668   }      
00669 
00670   // searchPositionIK #2
00671   bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00672                                                 const std::vector<double> &ik_seed_state,
00673                                                 const double &timeout,
00674                                                 const unsigned int& redundancy,
00675                                                 const double &consistency_limit,
00676                                                 std::vector<double> &solution,
00677                                                int &error_code)
00678   {
00679     if(free_params_.size()==0){
00680       //TODO - how to check consistency when there are no free params?
00681       return getPositionIK(ik_pose, ik_seed_state,solution, error_code);
00682       ROS_WARN_STREAM("No free parameters, so can't search");
00683     }
00684 
00685     if(redundancy != (unsigned int)free_params_[0]) {
00686       ROS_WARN_STREAM("Calling consistency search with wrong free param");
00687       return false;
00688     }
00689         
00690     KDL::Frame frame;
00691     tf::PoseMsgToKDL(ik_pose,frame);
00692 
00693     std::vector<double> vfree(free_params_.size());
00694 
00695     ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00696     int counter = 0;
00697 
00698     double initial_guess = ik_seed_state[free_params_[0]];
00699     vfree[0] = initial_guess;
00700 
00701     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00702     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00703 
00704     int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00705     int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00706 
00707     ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00708 
00709     while(1) {
00710       int numsol = solve(frame,vfree);
00711       //ROS_INFO_STREAM("Solutions number is " << numsol);
00712       //ROS_INFO("%f",vfree[0]);
00713             
00714       if(numsol > 0){
00715         for(int s = 0; s < numsol; ++s){
00716           std::vector<double> sol;
00717           //ik_solver_->getSolution(s,sol);
00718           getSolution(s,sol);
00719           bool obeys_limits = true;
00720           for(unsigned int i = 0; i < sol.size(); i++) {
00721             if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00722               obeys_limits = false;
00723               break;
00724             }
00725             //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00726           }
00727           if(obeys_limits) {
00728             //ik_solver_->getSolution(s,solution);
00729             getSolution(s,solution);
00730             error_code = kinematics::SUCCESS;
00731             return true;
00732           }
00733         }
00734       }
00735       
00736       // if(numsol > 0){
00737       //   for(unsigned int i = 0; i < sol.size(); i++) {
00738       //     if(i == 0) {
00739       //       //ik_solver_->getClosestSolution(ik_seed_state,solution);
00740       //       getClosestSolution(ik_seed_state,solution);
00741       //     } else {
00742       //       //ik_solver_->getSolution(s,sol);      
00743       //        getSolution(s,sol);     
00744       //     }
00745       //   }
00746       //   bool obeys_limits = true;
00747       //   for(unsigned int i = 0; i < solution.size(); i++) {
00748       //     if(joint_has_limits_vector_[i] && (solution[i] < joint_min_vector_[i] || solution[i] > joint_max_vector_[i])) {
00749       //       obeys_limits = false;
00750       //       break;
00751       //     }
00752       //     //ROS_INFO_STREAM("Num " << i << " value " << sol[i] << " has limits " << joint_has_limits_vector_[i] << " " << joint_min_vector_[i] << " " << joint_max_vector_[i]);
00753       //   }
00754       //   if(obeys_limits) {
00755       //     error_code = kinematics::SUCCESS;
00756       //     return true;
00757       //   }
00758       // }
00759       if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00760         error_code = kinematics::NO_IK_SOLUTION; 
00761         return false;
00762       }
00763       
00764       vfree[0] = initial_guess+search_discretization_*counter;
00765       ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00766     }
00767     //shouldn't ever get here
00768     error_code = kinematics::NO_IK_SOLUTION; 
00769     return false;
00770   }      
00771 
00772   // searchPositionIK #3
00773   // this is used by planning scene warehouse
00774   bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00775                                                 const std::vector<double> &ik_seed_state,
00776                                                 const double &timeout,
00777                                                 std::vector<double> &solution,
00778                                                 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00779                                                                                                              int &error_code)> &desired_pose_callback,
00780                                                 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00781                                                                                                                  int &error_code)> &solution_callback,
00782                                                 int &error_code)
00783 {
00784   // If manipulator has no free links
00785   if(free_params_.size()==0){
00786     // Find first IK solution, within joint limits
00787     if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code))    {
00788       ROS_DEBUG_STREAM("No solution whatsoever");
00789       error_code = kinematics::NO_IK_SOLUTION;
00790       return false;
00791     }
00792     // check for collisions
00793     solution_callback(ik_pose, solution, error_code);
00794     if(error_code == kinematics::SUCCESS){
00795       ROS_DEBUG_STREAM("Solution passes");
00796       return true;
00797     }else{
00798       ROS_DEBUG_STREAM("Solution has error code " << error_code);
00799       return false;
00800     }
00801   }
00802 
00803     if(!desired_pose_callback.empty())
00804       desired_pose_callback(ik_pose, ik_seed_state, error_code);
00805     if(error_code < 0){
00806       ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00807       return false;
00808     }
00809 
00810     KDL::Frame frame;
00811     tf::PoseMsgToKDL(ik_pose,frame);
00812 
00813     std::vector<double> vfree(free_params_.size());
00814 
00815     ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00816     int counter = 0;
00817 
00818     double initial_guess = ik_seed_state[free_params_[0]];
00819     vfree[0] = initial_guess;
00820 
00821     int num_positive_increments = (joint_max_vector_[free_params_[0]]-initial_guess)/search_discretization_;
00822     int num_negative_increments = (initial_guess - joint_min_vector_[free_params_[0]])/search_discretization_;
00823 
00824     ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00825 
00826     unsigned int solvecount = 0;
00827     unsigned int countsol = 0;
00828 
00829     ros::WallTime start = ros::WallTime::now();
00830 
00831     std::vector<double> sol;
00832     while(1) {
00833       int numsol = solve(frame,vfree);
00834 
00835       if(solvecount == 0) {
00836         if(numsol == 0) {
00837           ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00838         } else {
00839           ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00840         }
00841       }
00842       solvecount++;
00843       if(numsol > 0){
00844         if(solution_callback.empty()){
00845           getClosestSolution(ik_seed_state,solution);
00846           error_code = kinematics::SUCCESS;
00847           return true;
00848         }
00849         
00850         for(int s = 0; s < numsol; ++s){
00851           getSolution(s,sol);
00852           countsol++;
00853           bool obeys_limits = true;
00854           for(unsigned int i = 0; i < sol.size(); i++) {
00855             if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00856               obeys_limits = false;
00857               break;
00858             }
00859           }
00860           if(obeys_limits) {
00861             solution_callback(ik_pose,sol,error_code);
00862             if(error_code == kinematics::SUCCESS){
00863               solution = sol;
00864               ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00865               return true;
00866             }
00867           }
00868         }
00869       }
00870       if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00871         error_code = kinematics::NO_IK_SOLUTION; 
00872         ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00873         return false;
00874       }
00875       vfree[0] = initial_guess+search_discretization_*counter;
00876       ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00877     }
00878     error_code = kinematics::NO_IK_SOLUTION; 
00879     return false;
00880   }      
00881 
00882   // searchPositionIK #4
00883   bool IKFastKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00884                                                 const std::vector<double> &ik_seed_state,
00885                                                 const double &timeout,
00886                                                 const unsigned int& redundancy,
00887                                                 const double &consistency_limit,
00888                                                 std::vector<double> &solution,
00889                                                 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00890                                                                                                              int &error_code)> &desired_pose_callback,
00891                                                 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,
00892                                                                                                                  int &error_code)> &solution_callback,
00893                                                 int &error_code)
00894   {
00895     if(free_params_.size()==0){
00896       if(!getPositionIK(ik_pose, ik_seed_state,solution, error_code)) {
00897         ROS_DEBUG_STREAM("No solution whatsoever");
00898         error_code = kinematics::NO_IK_SOLUTION; 
00899         return false;
00900       } 
00901       solution_callback(ik_pose,solution,error_code);
00902       if(error_code == kinematics::SUCCESS) {
00903         ROS_DEBUG_STREAM("Solution passes");
00904         return true;
00905       } else {
00906         ROS_DEBUG_STREAM("Solution has error code " << error_code);
00907         return false;
00908       }
00909     }
00910 
00911     if(redundancy != (unsigned int) free_params_[0]) {
00912       ROS_WARN_STREAM("Calling consistency search with wrong free param");
00913       return false;
00914     }
00915 
00916     if(!desired_pose_callback.empty())
00917       desired_pose_callback(ik_pose,ik_seed_state,error_code);
00918     if(error_code < 0)
00919     {
00920       ROS_ERROR("Could not find inverse kinematics for desired end-effector pose since the pose may be in collision");
00921       return false;
00922     }
00923 
00924     KDL::Frame frame;
00925     tf::PoseMsgToKDL(ik_pose,frame);
00926 
00927     std::vector<double> vfree(free_params_.size());
00928 
00929     ros::Time maxTime = ros::Time::now() + ros::Duration(timeout);
00930     int counter = 0;
00931 
00932     double initial_guess = ik_seed_state[free_params_[0]];
00933     vfree[0] = initial_guess;
00934 
00935     double max_limit = fmin(joint_max_vector_[free_params_[0]], initial_guess+consistency_limit);
00936     double min_limit = fmax(joint_min_vector_[free_params_[0]], initial_guess-consistency_limit);
00937 
00938     int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_);
00939     int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_);
00940 
00941     ROS_DEBUG_STREAM("Free param is " << free_params_[0] << " initial guess is " << initial_guess << " " << num_positive_increments << " " << num_negative_increments);
00942 
00943     unsigned int solvecount = 0;
00944     unsigned int countsol = 0;
00945 
00946     ros::WallTime start = ros::WallTime::now();
00947 
00948     std::vector<double> sol;
00949     while(1) {
00950       int numsol = solve(frame,vfree);
00951       if(solvecount == 0) {
00952         if(numsol == 0) {
00953           ROS_DEBUG_STREAM("Bad solve time is " << ros::WallTime::now()-start);
00954         } else {
00955           ROS_DEBUG_STREAM("Good solve time is " << ros::WallTime::now()-start);
00956         }
00957       }
00958       solvecount++;
00959       if(numsol > 0){
00960         if(solution_callback.empty()){
00961           getClosestSolution(ik_seed_state,solution);
00962           error_code = kinematics::SUCCESS;
00963           return true;
00964         }
00965         
00966         for(int s = 0; s < numsol; ++s){
00967           getSolution(s,sol);
00968           countsol++;
00969           bool obeys_limits = true;
00970           for(unsigned int i = 0; i < sol.size(); i++) {
00971             if(joint_has_limits_vector_[i] && (sol[i] < joint_min_vector_[i] || sol[i] > joint_max_vector_[i])) {
00972               obeys_limits = false;
00973               break;
00974             }
00975           }
00976           if(obeys_limits) {
00977             solution_callback(ik_pose,sol,error_code);
00978             if(error_code == kinematics::SUCCESS){
00979               solution = sol;
00980               ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return true " << countsol << " " << solvecount);
00981               return true;
00982             }
00983           }
00984         }
00985       }
00986       if(!getCount(counter, num_positive_increments, num_negative_increments)) {
00987         error_code = kinematics::NO_IK_SOLUTION; 
00988         ROS_DEBUG_STREAM("Took " << (ros::WallTime::now() - start) << " to return false " << countsol << " " << solvecount);
00989         return false;
00990       }
00991       vfree[0] = initial_guess+search_discretization_*counter;
00992       ROS_DEBUG_STREAM(counter << " " << vfree[0]);
00993     }
00994     error_code = kinematics::NO_IK_SOLUTION; 
00995     return false;
00996   }      
00997 
00998 } // end namespace
00999 
01000 #include <pluginlib/class_list_macros.h>
01001 PLUGINLIB_DECLARE_CLASS(M16iB20_manipulator_kinematics, IKFastKinematicsPlugin, M16iB20_manipulator_kinematics::IKFastKinematicsPlugin, kinematics::KinematicsBase);
01002 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


m16ib20_arm_navigation
Author(s): Michael O. Blanton Jr
autogenerated on Sun Jul 7 2013 03:08:48