pr2_arm_ik_solver.cpp
Go to the documentation of this 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                                     const double& consistency_limit,
00264                                     KDL::JntArray &q_out, 
00265                                     const double &timeout)
00266 {
00267   KDL::JntArray q_init = q_in;
00268   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00269   double initial_guess = q_init(free_angle_);
00270 
00271   ros::Time start_time = ros::Time::now();
00272   double loop_time = 0;
00273   int count = 0;
00274 
00275   double max_limit = fmin(pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, initial_guess+consistency_limit);
00276   double min_limit = fmax(pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, initial_guess-consistency_limit);
00277     
00278   int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_angle_);
00279   int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_angle_);
00280 
00281   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);
00282   while(loop_time < timeout)
00283   {
00284     if(CartToJnt(q_init,p_in,q_out) > 0)
00285       return 1;
00286     if(!getCount(count,num_positive_increments,-num_negative_increments))
00287       return -1;
00288     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00289     ROS_DEBUG("%d, %f",count,q_init(free_angle_));
00290     loop_time = (ros::Time::now()-start_time).toSec();
00291   }
00292   if(loop_time >= timeout)
00293   {
00294     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00295     return TIMED_OUT;
00296   }
00297   else
00298   {
00299     ROS_DEBUG("No IK solution was found");
00300     return NO_IK_SOLUTION;
00301   }
00302   return NO_IK_SOLUTION;
00303 }
00304 
00305 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, 
00306                                     const KDL::Frame& p_in, 
00307                                     KDL::JntArray &q_out, 
00308                                     const double &timeout, 
00309                                     arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00310                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
00311                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback)
00312 {
00313   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00314   KDL::JntArray q_init = q_in;
00315   double initial_guess = q_init(free_angle_);
00316 
00317   ros::Time start_time = ros::Time::now();
00318   double loop_time = 0;
00319   int count = 0;
00320 
00321   int num_positive_increments = (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position-initial_guess)/search_discretization_angle_);
00322   int num_negative_increments = (int)((initial_guess-pr2_arm_ik_.solver_info_.limits[free_angle_].min_position)/search_discretization_angle_);
00323   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);
00324   unsigned int testnum = 0;
00325   if(!desired_pose_callback.empty())
00326     desired_pose_callback(q_init,p_in,error_code);
00327   if(error_code.val != error_code.SUCCESS)
00328   {
00329     return -1;
00330   }
00331   bool callback_check = true;
00332   if(solution_callback.empty())
00333     callback_check = false;
00334 
00335   ros::WallTime s = ros::WallTime::now();
00336 
00337   while(loop_time < timeout)
00338   {
00339     testnum++;
00340     if(CartToJnt(q_init,p_in,q_out) > 0)
00341     {
00342       if(callback_check)
00343       {
00344         solution_callback(q_out,p_in,error_code);
00345         if(error_code.val == error_code.SUCCESS)
00346         {
00347           ROS_DEBUG_STREAM("Success with " << testnum << " in " << (ros::WallTime::now()-s));
00348           return 1;
00349         }
00350       }
00351       else
00352       {
00353         error_code.val = error_code.SUCCESS;
00354         return 1;
00355       }
00356     }
00357     if(!getCount(count,num_positive_increments,-num_negative_increments))
00358     {
00359       ROS_DEBUG_STREAM("Failure with " << testnum << " in " << (ros::WallTime::now()-s));
00360       error_code.val = error_code.NO_IK_SOLUTION;
00361       return -1;
00362     }
00363     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00364     ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,q_init(free_angle_));
00365     loop_time = (ros::Time::now()-start_time).toSec();
00366   }
00367   if(loop_time >= timeout)
00368   {
00369     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00370     error_code.val = error_code.TIMED_OUT;
00371   }
00372   else
00373   {
00374     ROS_DEBUG("No IK solution was found");
00375     error_code.val = error_code.NO_IK_SOLUTION;
00376   }
00377   return -1;
00378 }
00379 
00380 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, 
00381                                     const KDL::Frame& p_in, 
00382                                     KDL::JntArray &q_out, 
00383                                     const double &timeout, 
00384                                     const double &max_consistency,
00385                                     arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00386                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &desired_pose_callback,
00387                                     const boost::function<void(const KDL::JntArray&,const KDL::Frame&,arm_navigation_msgs::ArmNavigationErrorCodes &)> &solution_callback)
00388 {
00389   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00390   KDL::JntArray q_init = q_in;
00391   double initial_guess = q_init(free_angle_);
00392 
00393   ros::Time start_time = ros::Time::now();
00394   double loop_time = 0;
00395   int count = 0;
00396 
00397   double max_limit = fmin(pr2_arm_ik_.solver_info_.limits[free_angle_].max_position, initial_guess+max_consistency);
00398   double min_limit = fmax(pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, initial_guess-max_consistency);
00399  
00400   ROS_DEBUG_STREAM("Initial guess " << initial_guess << " max " << max_consistency);
00401   ROS_DEBUG_STREAM("Max limit " << max_limit << " " << pr2_arm_ik_.solver_info_.limits[free_angle_].max_position << " " << initial_guess+max_consistency);
00402   ROS_DEBUG_STREAM("Min limit " << min_limit << " " << pr2_arm_ik_.solver_info_.limits[free_angle_].min_position << " " << initial_guess-max_consistency);
00403   
00404   int num_positive_increments = (int)((max_limit-initial_guess)/search_discretization_angle_);
00405   int num_negative_increments = (int)((initial_guess-min_limit)/search_discretization_angle_);
00406   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);
00407   unsigned int testnum = 0;
00408   if(!desired_pose_callback.empty())
00409     desired_pose_callback(q_init,p_in,error_code);
00410   if(error_code.val != error_code.SUCCESS)
00411   {
00412     return -1;
00413   }
00414   bool callback_check = true;
00415   if(solution_callback.empty())
00416     callback_check = false;
00417 
00418   ros::WallTime s = ros::WallTime::now();
00419 
00420   while(loop_time < timeout)
00421   {
00422     testnum++;
00423     if(CartToJnt(q_init,p_in,q_out) > 0)
00424     {
00425       if(callback_check)
00426       {
00427         solution_callback(q_out,p_in,error_code);
00428         if(error_code.val == error_code.SUCCESS)
00429         {
00430           ROS_DEBUG_STREAM("Difference is " << abs(q_in(free_angle_)-q_out(free_angle_)));
00431           ROS_DEBUG_STREAM("Success with " << testnum << " in " << (ros::WallTime::now()-s));
00432           return 1;
00433         }
00434       }
00435       else
00436       {
00437         error_code.val = error_code.SUCCESS;
00438         return 1;
00439       }
00440     }
00441     if(!getCount(count,num_positive_increments,-num_negative_increments))
00442     {
00443       ROS_DEBUG_STREAM("Failure with " << testnum << " in " << (ros::WallTime::now()-s));
00444       error_code.val = error_code.NO_IK_SOLUTION;
00445       return -1;
00446     }
00447     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00448     ROS_DEBUG("Redundancy search, index:%d, free angle value: %f",count,q_init(free_angle_));
00449     loop_time = (ros::Time::now()-start_time).toSec();
00450   }
00451   if(loop_time >= timeout)
00452   {
00453     ROS_DEBUG("IK Timed out in %f seconds",timeout);
00454     error_code.val = error_code.TIMED_OUT;
00455   }
00456   else
00457   {
00458     ROS_DEBUG("No IK solution was found");
00459     error_code.val = error_code.NO_IK_SOLUTION;
00460   }
00461   return -1;
00462 }
00463 
00464 
00465 std::string PR2ArmIKSolver::getFrameId()
00466 {
00467   return root_frame_name_;
00468 }


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jan 2 2014 11:40:43