pr2_arm_kinematics_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Sachin Chitta */
00036 
00037 #include <geometry_msgs/PoseStamped.h>
00038 #include <kdl_parser/kdl_parser.hpp>
00039 #include <eigen_conversions/eigen_msg.h>
00040 #include <eigen_conversions/eigen_kdl.h>
00041 #include <algorithm>
00042 #include <numeric>
00043 
00044 #include "pr2_arm_kinematics_plugin.h"
00045 
00046 using namespace KDL;
00047 using namespace std;
00048 
00049 namespace pr2_arm_kinematics
00050 {
00051 bool PR2ArmIKSolver::getCount(int& count, const int& max_count, const int& min_count)
00052 {
00053   if (count > 0)
00054   {
00055     if (-count >= min_count)
00056     {
00057       count = -count;
00058       return true;
00059     }
00060     else if (count + 1 <= max_count)
00061     {
00062       count = count + 1;
00063       return true;
00064     }
00065     else
00066     {
00067       return false;
00068     }
00069   }
00070   else
00071   {
00072     if (1 - count <= max_count)
00073     {
00074       count = 1 - count;
00075       return true;
00076     }
00077     else if (count - 1 >= min_count)
00078     {
00079       count = count - 1;
00080       return true;
00081     }
00082     else
00083       return false;
00084   }
00085 }
00086 
00087 PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name,
00088                                const std::string& tip_frame_name, const double& search_discretization_angle,
00089                                const int& free_angle)
00090   : ChainIkSolverPos()
00091 {
00092   search_discretization_angle_ = search_discretization_angle;
00093   free_angle_ = free_angle;
00094   root_frame_name_ = root_frame_name;
00095   if (!pr2_arm_ik_.init(robot_model, root_frame_name, tip_frame_name))
00096     active_ = false;
00097   else
00098     active_ = true;
00099 }
00100 
00101 int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out)
00102 {
00103   const bool verbose = false;
00104   Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
00105   std::vector<std::vector<double> > solution_ik;
00106   if (free_angle_ == 0)
00107   {
00108     if (verbose)
00109       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solving with %f", q_init(0));
00110     pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
00111   }
00112   else
00113   {
00114     pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
00115   }
00116 
00117   if (solution_ik.empty())
00118     return -1;
00119 
00120   double min_distance = 1e6;
00121   int min_index = -1;
00122 
00123   for (int i = 0; i < (int)solution_ik.size(); i++)
00124   {
00125     if (verbose)
00126     {
00127       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Solution : %d", (int)solution_ik.size());
00128 
00129       for (int j = 0; j < (int)solution_ik[i].size(); j++)
00130       {
00131         ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d: %f", j, solution_ik[i][j]);
00132       }
00133       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
00134       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", " ");
00135     }
00136     double tmp_distance = computeEuclideanDistance(solution_ik[i], q_init);
00137     if (tmp_distance < min_distance)
00138     {
00139       min_distance = tmp_distance;
00140       min_index = i;
00141     }
00142   }
00143 
00144   if (min_index > -1)
00145   {
00146     q_out.resize((int)solution_ik[min_index].size());
00147     for (int i = 0; i < (int)solution_ik[min_index].size(); i++)
00148     {
00149       q_out(i) = solution_ik[min_index][i];
00150     }
00151     return 1;
00152   }
00153   else
00154     return -1;
00155 }
00156 
00157 int PR2ArmIKSolver::CartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out,
00158                                     const double& timeout)
00159 {
00160   const bool verbose = false;
00161   KDL::JntArray q_init = q_in;
00162   double initial_guess = q_init(free_angle_);
00163 
00164   ros::Time start_time = ros::Time::now();
00165   double loop_time = 0;
00166   int count = 0;
00167 
00168   int num_positive_increments =
00169       (int)((pr2_arm_ik_.solver_info_.limits[free_angle_].max_position - initial_guess) / search_discretization_angle_);
00170   int num_negative_increments =
00171       (int)((initial_guess - pr2_arm_ik_.solver_info_.limits[free_angle_].min_position) / search_discretization_angle_);
00172   if (verbose)
00173     ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%f %f %f %d %d \n\n", initial_guess,
00174                     pr2_arm_ik_.solver_info_.limits[free_angle_].max_position,
00175                     pr2_arm_ik_.solver_info_.limits[free_angle_].min_position, num_positive_increments,
00176                     num_negative_increments);
00177   while (loop_time < timeout)
00178   {
00179     if (CartToJnt(q_init, p_in, q_out) > 0)
00180       return 1;
00181     if (!getCount(count, num_positive_increments, -num_negative_increments))
00182       return -1;
00183     q_init(free_angle_) = initial_guess + search_discretization_angle_ * count;
00184     if (verbose)
00185       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "%d, %f", count, q_init(free_angle_));
00186     loop_time = (ros::Time::now() - start_time).toSec();
00187   }
00188   if (loop_time >= timeout)
00189   {
00190     ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "IK Timed out in %f seconds", timeout);
00191     return TIMED_OUT;
00192   }
00193   else
00194   {
00195     ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "No IK solution was found");
00196     return NO_IK_SOLUTION;
00197   }
00198   return NO_IK_SOLUTION;
00199 }
00200 
00201 bool getKDLChain(const urdf::ModelInterface& model, const std::string& root_name, const std::string& tip_name,
00202                  KDL::Chain& kdl_chain)
00203 {
00204   // create robot chain from root to tip
00205   KDL::Tree tree;
00206   if (!kdl_parser::treeFromUrdfModel(model, tree))
00207   {
00208     ROS_ERROR("Could not initialize tree object");
00209     return false;
00210   }
00211   if (!tree.getChain(root_name, tip_name, kdl_chain))
00212   {
00213     ROS_ERROR_STREAM("Could not initialize chain object for base " << root_name << " tip " << tip_name);
00214     return false;
00215   }
00216   return true;
00217 }
00218 
00219 Eigen::Matrix4f KDLToEigenMatrix(const KDL::Frame& p)
00220 {
00221   Eigen::Matrix4f b = Eigen::Matrix4f::Identity();
00222   for (int i = 0; i < 3; i++)
00223   {
00224     for (int j = 0; j < 3; j++)
00225     {
00226       b(i, j) = p.M(i, j);
00227     }
00228     b(i, 3) = p.p(i);
00229   }
00230   return b;
00231 }
00232 
00233 double computeEuclideanDistance(const std::vector<double>& array_1, const KDL::JntArray& array_2)
00234 {
00235   double distance = 0.0;
00236   for (int i = 0; i < (int)array_1.size(); i++)
00237   {
00238     distance += (array_1[i] - array_2(i)) * (array_1[i] - array_2(i));
00239   }
00240   return sqrt(distance);
00241 }
00242 
00243 void getKDLChainInfo(const KDL::Chain& chain, moveit_msgs::KinematicSolverInfo& chain_info)
00244 {
00245   int i = 0;  // segment number
00246   while (i < (int)chain.getNrOfSegments())
00247   {
00248     chain_info.link_names.push_back(chain.getSegment(i).getName());
00249     i++;
00250   }
00251 }
00252 
00253 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin() : active_(false)
00254 {
00255 }
00256 
00257 bool PR2ArmKinematicsPlugin::isActive()
00258 {
00259   if (active_)
00260     return true;
00261   return false;
00262 }
00263 
00264 void PR2ArmKinematicsPlugin::setRobotModel(boost::shared_ptr<urdf::ModelInterface>& robot_model)
00265 {
00266   robot_model_ = robot_model;
00267 }
00268 
00269 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
00270                                         const std::string& base_name, const std::string& tip_name,
00271                                         double search_discretization)
00272 {
00273   setValues(robot_description, group_name, base_name, tip_name, search_discretization);
00274   const bool verbose = false;
00275   std::string xml_string;
00276   dimension_ = 7;
00277 
00278   ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "Loading KDL Tree");
00279   if (!getKDLChain(*robot_model_.get(), base_frame_, tip_frame_, kdl_chain_))
00280   {
00281     active_ = false;
00282     ROS_ERROR("Could not load kdl tree");
00283   }
00284   jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00285   free_angle_ = 2;
00286 
00287   pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(*robot_model_.get(), base_frame_, tip_frame_,
00288                                                                   search_discretization_, free_angle_));
00289   if (!pr2_arm_ik_solver_->active_)
00290   {
00291     ROS_ERROR("Could not load ik");
00292     active_ = false;
00293   }
00294   else
00295   {
00296     pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00297     pr2_arm_kinematics::getKDLChainInfo(kdl_chain_, fk_solver_info_);
00298     fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00299 
00300     if (verbose)
00301     {
00302       for (unsigned int i = 0; i < ik_solver_info_.joint_names.size(); i++)
00303       {
00304         ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics:: joint name: %s",
00305                         ik_solver_info_.joint_names[i].c_str());
00306       }
00307       for (unsigned int i = 0; i < ik_solver_info_.link_names.size(); i++)
00308       {
00309         ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve IK for %s",
00310                         ik_solver_info_.link_names[i].c_str());
00311       }
00312       for (unsigned int i = 0; i < fk_solver_info_.link_names.size(); i++)
00313       {
00314         ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2Kinematics can solve FK for %s",
00315                         fk_solver_info_.link_names[i].c_str());
00316       }
00317       ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "PR2KinematicsPlugin::active for %s", group_name.c_str());
00318     }
00319     active_ = true;
00320   }
00321   return active_;
00322 }
00323 
00324 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00325                                            std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00326                                            const kinematics::KinematicsQueryOptions& options) const
00327 {
00328   return false;
00329 }
00330 
00331 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00332                                               const std::vector<double>& ik_seed_state, double timeout,
00333                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00334                                               const kinematics::KinematicsQueryOptions& options) const
00335 {
00336   if (!active_)
00337   {
00338     ROS_ERROR("kinematics not active");
00339     error_code.val = error_code.PLANNING_FAILED;
00340     return false;
00341   }
00342   KDL::Frame pose_desired;
00343   Eigen::Affine3d tp;
00344   tf::poseMsgToEigen(ik_pose, tp);
00345   tf::transformEigenToKDL(tp, pose_desired);
00346 
00347   // Do the IK
00348   KDL::JntArray jnt_pos_in;
00349   KDL::JntArray jnt_pos_out;
00350   jnt_pos_in.resize(dimension_);
00351   for (int i = 0; i < dimension_; i++)
00352   {
00353     jnt_pos_in(i) = ik_seed_state[i];
00354   }
00355 
00356   int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, timeout);
00357   if (ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00358   {
00359     error_code.val = error_code.NO_IK_SOLUTION;
00360     return false;
00361   }
00362 
00363   if (ik_valid >= 0)
00364   {
00365     solution.resize(dimension_);
00366     for (int i = 0; i < dimension_; i++)
00367     {
00368       solution[i] = jnt_pos_out(i);
00369     }
00370     error_code.val = error_code.SUCCESS;
00371     return true;
00372   }
00373   else
00374   {
00375     ROS_DEBUG_NAMED("pr2_arm_kinematics_plugin", "An IK solution could not be found");
00376     error_code.val = error_code.NO_IK_SOLUTION;
00377     return false;
00378   }
00379 }
00380 
00381 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00382                                               const std::vector<double>& ik_seed_state, double timeout,
00383                                               const std::vector<double>& consistency_limit,
00384                                               std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00385                                               const kinematics::KinematicsQueryOptions& options) const
00386 {
00387   return false;
00388 }
00389 
00390 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00391                                               const std::vector<double>& ik_seed_state, double timeout,
00392                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00393                                               moveit_msgs::MoveItErrorCodes& error_code,
00394                                               const kinematics::KinematicsQueryOptions& options) const
00395 {
00396   return false;
00397 }
00398 
00399 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose,
00400                                               const std::vector<double>& ik_seed_state, double timeout,
00401                                               const std::vector<double>& consistency_limit,
00402                                               std::vector<double>& solution, const IKCallbackFn& solution_callback,
00403                                               moveit_msgs::MoveItErrorCodes& error_code,
00404                                               const kinematics::KinematicsQueryOptions& options) const
00405 {
00406   return false;
00407 }
00408 
00409 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00410                                            const std::vector<double>& joint_angles,
00411                                            std::vector<geometry_msgs::Pose>& poses) const
00412 {
00413   return false;
00414 }
00415 
00416 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
00417 {
00418   if (!active_)
00419   {
00420     ROS_ERROR("kinematics not active");
00421   }
00422   return ik_solver_info_.joint_names;
00423 }
00424 
00425 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
00426 {
00427   if (!active_)
00428   {
00429     ROS_ERROR("kinematics not active");
00430   }
00431   return fk_solver_info_.link_names;
00432 }
00433 
00434 }  // namespace


moveit_core
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley
autogenerated on Wed Jun 19 2019 19:23:49