pr2_arm_kinematics: pr2_arm_ik_solver.cpp Source File

00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #include <pr2_arm_kinematics/pr2_arm_ik_solver.h>
00034 
00035 using namespace Eigen;
00036 using namespace pr2_arm_kinematics;
00037 
00038 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::Model &robot_model, 
00039                                const std::string &root_frame_name,
00040                                const std::string &tip_frame_name,
00041                                const double &search_discretization_angle, 
00042                                const int &free_angle):ChainIkSolverPos()
00043 {
00044   search_discretization_angle_ = search_discretization_angle;
00045   free_angle_ = free_angle;
00046   root_frame_name_ = root_frame_name;
00047   if(!pr2_arm_ik_.init(robot_model,root_frame_name,tip_frame_name))
00048     active_ = false;
00049   else
00050     active_ = true;
00051 }
00052 
00053 void PR2ArmIKSolver::getSolverInfo(kinematics_msgs::KinematicSolverInfo &response)
00054 {
00055   pr2_arm_ik_.getSolverInfo(response);
00056 }
00057 
00058 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, 
00059                               const KDL::Frame& p_in, 
00060                               KDL::JntArray &q_out)
00061 {
00062   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00063   std::vector<std::vector<double> > solution_ik;
00064   if(free_angle_ == 0)
00065   {
00066     ROS_DEBUG("Solving with %f",q_init(0)); 
00067     pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
00068   }
00069   else
00070   {
00071     pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
00072   }
00073   
00074   if(solution_ik.empty())
00075     return -1;
00076 
00077   double min_distance = 1e6;
00078   int min_index = -1;
00079 
00080   for(int i=0; i< (int) solution_ik.size(); i++)
00081   {     
00082     ROS_DEBUG("Solution : %d",(int)solution_ik.size());
00083 
00084     for(int j=0; j < (int)solution_ik[i].size(); j++)
00085     {   
00086       ROS_DEBUG("%d: %f",j,solution_ik[i][j]);
00087     }
00088     ROS_DEBUG(" ");
00089     ROS_DEBUG(" ");
00090 
00091     double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init);
00092     if(tmp_distance < min_distance)
00093     {
00094       min_distance = tmp_distance;
00095       min_index = i;
00096     }
00097   }
00098 
00099   if(min_index > -1)
00100   {
00101     q_out.resize((int)solution_ik[min_index].size());
00102     for(int i=0; i < (int)solution_ik[min_index].size(); i++)
00103     {   
00104       q_out(i) = solution_ik[min_index][i];
00105     }
00106     return 1;
00107   }
00108   else
00109     return -1;
00110 }
00111 
00112 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, 
00113                               const KDL::Frame& p_in, 
00114                               std::vector<KDL::JntArray> &q_out)
00115 {
00116   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00117   std::vector<std::vector<double> > solution_ik;
00118   KDL::JntArray q;
00119 
00120   if(free_angle_ == 0)
00121   {
00122     pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
00123   }
00124   else
00125   {
00126     pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
00127   }
00128   
00129   if(solution_ik.empty())
00130     return -1;
00131 
00132   q.resize(7);
00133   q_out.clear();
00134   for(int i=0; i< (int) solution_ik.size(); i++)
00135   {     
00136     for(int j=0; j < 7; j++)
00137     {   
00138       q(j) = solution_ik[i][j];
00139     }
00140     q_out.push_back(q);
00141   }
00142   return 1;
00143 }
00144 
00145 bool PR2ArmIKSolver::getCount(int &count, 
00146                               const int &max_count, 
00147                               const int &min_count)
00148 {
00149   if(count > 0)
00150   {
00151     if(-count >= min_count)
00152     {   
00153       count = -count;
00154       return true;
00155     }
00156     else if(count+1 <= max_count)
00157     {
00158       count = count+1;
00159       return true;
00160     }
00161     else
00162     {
00163       return false;
00164     }
00165   }
00166   else
00167   {
00168     if(1-count <= max_count)
00169     {
00170       count = 1-count;
00171       return true;
00172     }
00173     else if(count-1 >= min_count)
00174     {
00175       count = count -1;
00176       return true;
00177     }
00178     else
00179       return false;
00180   }
00181 }
00182 
00183 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, 
00184                                     const KDL::Frame& p_in, 
00185                                     std::vector<KDL::JntArray> &q_out, 
00186                                     const double &timeout)
00187 {
00188   KDL::JntArray q_init = q_in;
00189   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00190   double initial_guess = q_init(free_angle_);
00191 
00192   ros::Time start_time = ros::Time::now();
00193   double loop_time = 0;
00194   int count = 0;
00195 
00196   int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00197   int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00198   ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
00199   while(loop_time < timeout)
00200   {
00201     if(CartToJnt(q_init,p_in,q_out) > 0)
00202       return 1;
00203     if(!getCount(count,num_positive_increments,-num_negative_increments))
00204       return -1;
00205     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00206     ROS_DEBUG("%d, %f",count,q_init(free_angle_));
00207     loop_time = (ros::Time::now()-start_time).toSec();
00208   }
00209   if(loop_time >= timeout)
00210   {
00211     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00212     return TIMED_OUT;
00213   }
00214   else
00215   {
00216     ROS_DEBUG("No IK solution was found");
00217     return NO_IK_SOLUTION;
00218   }
00219   return NO_IK_SOLUTION;
00220 }
00221 
00222 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, 
00223                                     const KDL::Frame& p_in, 
00224                                     KDL::JntArray &q_out, 
00225                                     const double &timeout)
00226 {
00227   KDL::JntArray q_init = q_in;
00228   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00229   double initial_guess = q_init(free_angle_);
00230 
00231   ros::Time start_time = ros::Time::now();
00232   double loop_time = 0;
00233   int count = 0;
00234 
00235   int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00236   int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00237   ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
00238   while(loop_time < timeout)
00239   {
00240     if(CartToJnt(q_init,p_in,q_out) > 0)
00241       return 1;
00242     if(!getCount(count,num_positive_increments,-num_negative_increments))
00243       return -1;
00244     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00245     ROS_DEBUG("%d, %f",count,q_init(free_angle_));
00246     loop_time = (ros::Time::now()-start_time).toSec();
00247   }
00248   if(loop_time >= timeout)
00249   {
00250     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00251     return TIMED_OUT;
00252   }
00253   else
00254   {
00255     ROS_DEBUG("No IK solution was found");
00256     return NO_IK_SOLUTION;
00257   }
00258   return NO_IK_SOLUTION;
00259 }
00260 
00261 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, 
00262                                     const KDL::Frame& p_in, 
00263                                     KDL::JntArray &q_out, 
00264                                     const double &timeout, 
00265                                     motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00266                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
00267                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,motion_planning_msgs::ArmNavigationErrorCodes &)> &solution_callback)
00268 {
00269   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00270   KDL::JntArray q_init = q_in;
00271   double initial_guess = q_init(free_angle_);
00272 
00273   ros::Time start_time = ros::Time::now();
00274   double loop_time = 0;
00275   int count = 0;
00276 
00277   int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00278   int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00279   ROS_DEBUG("%f %f %f %d %d \n\n",initial_guess,pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,pr2_arm_ik_.solver_info_.limits[free_angle_].min_position,num_positive_increments,num_negative_increments);
00280 
00281   if(!desired_pose_callback.empty())
00282     desired_pose_callback(q_init,p_in,error_code);
00283   if(error_code.val != error_code.SUCCESS)
00284   {
00285     return -1;
00286   }
00287   bool callback_check = true;
00288   if(solution_callback.empty())
00289     callback_check = false;
00290 
00291   while(loop_time < timeout)
00292   {
00293     if(CartToJnt(q_init,p_in,q_out) > 0)
00294     {
00295       if(callback_check)
00296       {
00297         solution_callback(q_out,p_in,error_code);
00298         if(error_code.val == error_code.SUCCESS)
00299         {
00300           return 1;
00301         }
00302       }
00303       else
00304       {
00305         error_code.val = error_code.SUCCESS;
00306         return 1;
00307       }
00308     }
00309     if(!getCount(count,num_positive_increments,-num_negative_increments))
00310     {
00311       error_code.val = error_code.NO_IK_SOLUTION;
00312       return -1;
00313     }
00314     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00315     ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,q_init(free_angle_));
00316     loop_time = (ros::Time::now()-start_time).toSec();
00317   }
00318   if(loop_time >= timeout)
00319   {
00320     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00321     error_code.val = error_code.TIMED_OUT;
00322   }
00323   else
00324   {
00325     ROS_DEBUG("No IK solution was found");
00326     error_code.val = error_code.NO_IK_SOLUTION;
00327   }
00328   return -1;
00329 }
00330 
00331 std::string PR2ArmIKSolver::getFrameId()
00332 {
00333   return root_frame_name_;
00334 }