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 <pr2_arm_kinematics/pr2_arm_kinematics_plugin.h>
00038 #include <geometry_msgs/PoseStamped.h>
00039 #include <kdl_parser/kdl_parser.hpp>
00040 #include <tf_conversions/tf_kdl.h>
00041 #include <ros/ros.h>
00042 #include <algorithm>
00043 #include <numeric>
00044 
00045 #include <pluginlib/class_list_macros.h>
00046 
00047 using namespace KDL;
00048 using namespace tf;
00049 using namespace std;
00050 using namespace ros;
00051 
00052 //register PR2ArmKinematics as a KinematicsBase implementation
00053 PLUGINLIB_EXPORT_CLASS(pr2_arm_kinematics::PR2ArmKinematicsPlugin, kinematics::KinematicsBase);
00054 
00055 namespace pr2_arm_kinematics {
00056 
00057 PR2ArmKinematicsPlugin::PR2ArmKinematicsPlugin():active_(false){}
00058 
00059 bool PR2ArmKinematicsPlugin::isActive()
00060 {
00061   if(active_)
00062     return true;
00063   return false;
00064 }
00065 
00066 bool PR2ArmKinematicsPlugin::initialize(const std::string& robot_description,
00067                                         const std::string& group_name,
00068                                         const std::string& base_frame,
00069                                         const std::string& tip_frame,
00070                                         double search_discretization)
00071 {
00072   setValues(robot_description, group_name, base_frame, tip_frame, search_discretization);
00073   urdf::Model robot_model;
00074   std::string xml_string;
00075   ros::NodeHandle private_handle("~/"+group_name);
00076   dimension_ = 7;
00077   while(!loadRobotModel(private_handle,robot_model,xml_string) && private_handle.ok())
00078   {
00079     ROS_ERROR("Could not load robot model. Are you sure the robot model is on the parameter server?");
00080     ros::Duration(0.5).sleep();
00081   }
00082 
00083   ROS_DEBUG("Loading KDL Tree");
00084   if(!getKDLChain(xml_string,base_frame_,tip_frame_,kdl_chain_))
00085   {
00086     active_ = false;
00087     ROS_ERROR("Could not load kdl tree");
00088   }
00089   ROS_DEBUG("Advertising services");
00090   jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
00091   private_handle.param<int>("free_angle",free_angle_,2);
00092 
00093   pr2_arm_ik_solver_.reset(new pr2_arm_kinematics::PR2ArmIKSolver(robot_model, base_frame_,tip_frame_, search_discretization_,free_angle_));
00094   if(!pr2_arm_ik_solver_->active_)
00095   {
00096     ROS_ERROR("Could not load ik");
00097     active_ = false;
00098   }
00099   else
00100   {
00101 
00102     pr2_arm_ik_solver_->getSolverInfo(ik_solver_info_);
00103     pr2_arm_kinematics::getKDLChainInfo(kdl_chain_,fk_solver_info_);
00104     fk_solver_info_.joint_names = ik_solver_info_.joint_names;
00105 
00106     for(unsigned int i=0; i < ik_solver_info_.joint_names.size(); i++)
00107     {
00108       ROS_DEBUG("PR2Kinematics:: joint name: %s",ik_solver_info_.joint_names[i].c_str());
00109     }
00110     for(unsigned int i=0; i < ik_solver_info_.link_names.size(); i++)
00111     {
00112       ROS_DEBUG("PR2Kinematics can solve IK for %s",ik_solver_info_.link_names[i].c_str());
00113     }
00114     for(unsigned int i=0; i < fk_solver_info_.link_names.size(); i++)
00115     {
00116       ROS_DEBUG("PR2Kinematics can solve FK for %s",fk_solver_info_.link_names[i].c_str());
00117     }
00118     ROS_DEBUG("PR2KinematicsPlugin::active for %s",group_name.c_str());
00119     active_ = true;
00120   }
00121   pr2_arm_ik_solver_->setFreeAngle(2);
00122   return active_;
00123 }
00124 
00125 bool PR2ArmKinematicsPlugin::getPositionIK(const geometry_msgs::Pose &ik_pose,
00126                                            const std::vector<double> &ik_seed_state,
00127                                            std::vector<double> &solution,
00128                                            moveit_msgs::MoveItErrorCodes &error_code,
00129                                            const kinematics::KinematicsQueryOptions &options) const
00130 {
00131   if(!active_)
00132   {
00133     ROS_ERROR("kinematics not active");
00134     error_code.val = error_code.NO_IK_SOLUTION;
00135     return false;
00136   }
00137 
00138   KDL::Frame pose_desired;
00139   tf::poseMsgToKDL(ik_pose, pose_desired);
00140 
00141   //Do the IK
00142   KDL::JntArray jnt_pos_in;
00143   KDL::JntArray jnt_pos_out;
00144   jnt_pos_in.resize(dimension_);
00145   for(int i=0; i < dimension_; i++)
00146   {
00147     jnt_pos_in(i) = ik_seed_state[i];
00148   }
00149 
00150   int ik_valid = pr2_arm_ik_solver_->CartToJnt(jnt_pos_in,
00151                                                pose_desired,
00152                                                jnt_pos_out);
00153   if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00154   {
00155     error_code.val = error_code.NO_IK_SOLUTION;
00156     return false;
00157   }
00158 
00159   if(ik_valid >= 0)
00160   {
00161     solution.resize(dimension_);
00162     for(int i=0; i < dimension_; i++)
00163     {
00164       solution[i] = jnt_pos_out(i);
00165     }
00166     error_code.val = error_code.SUCCESS;
00167     return true;
00168   }
00169   else
00170   {
00171     ROS_DEBUG("An IK solution could not be found");
00172     error_code.val = error_code.NO_IK_SOLUTION;
00173     return false;
00174   }
00175 }
00176 
00177 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00178                                               const std::vector<double> &ik_seed_state,
00179                                               double timeout,
00180                                               std::vector<double> &solution,
00181                                               moveit_msgs::MoveItErrorCodes &error_code,
00182                                               const kinematics::KinematicsQueryOptions &options) const
00183 {
00184   static IKCallbackFn solution_callback = 0;
00185   static std::vector<double> consistency_limits;
00186   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00187 }
00188 
00189 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00190                                               const std::vector<double> &ik_seed_state,
00191                                               double timeout,
00192                                               const std::vector<double> &consistency_limits,
00193                                               std::vector<double> &solution,
00194                                               moveit_msgs::MoveItErrorCodes &error_code,
00195                                               const kinematics::KinematicsQueryOptions &options) const
00196 {
00197   static IKCallbackFn solution_callback = 0;
00198   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00199 }
00200 
00201 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00202                                               const std::vector<double> &ik_seed_state,
00203                                               double timeout,
00204                                               std::vector<double> &solution,
00205                                               const IKCallbackFn &solution_callback,
00206                                               moveit_msgs::MoveItErrorCodes &error_code,
00207                                               const kinematics::KinematicsQueryOptions &options) const
00208 {
00209   static std::vector<double> consistency_limits;
00210   return searchPositionIK(ik_pose, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code);
00211 }
00212 
00213 bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose &ik_pose,
00214                                               const std::vector<double> &ik_seed_state,
00215                                               double timeout,
00216                                               const std::vector<double> &consistency_limits,
00217                                               std::vector<double> &solution,
00218                                               const IKCallbackFn &solution_callback,
00219                                               moveit_msgs::MoveItErrorCodes &error_code,
00220                                               const kinematics::KinematicsQueryOptions &options) const
00221 {
00222   if(!active_)
00223   {
00224     ROS_ERROR("kinematics not active");
00225     error_code.val = error_code.FAILURE;
00226     return false;
00227   }
00228   if(!consistency_limits.empty() && consistency_limits.size() != dimension_)
00229   {
00230     ROS_ERROR("Consistency limits should be of size: %d",dimension_);
00231     error_code.val = error_code.FAILURE;
00232     return false;
00233   }
00234 
00235   KDL::Frame pose_desired;
00236   tf::poseMsgToKDL(ik_pose, pose_desired);
00237 
00238   //Do the IK
00239   KDL::JntArray jnt_pos_in;
00240   KDL::JntArray jnt_pos_out;
00241   jnt_pos_in.resize(dimension_);
00242   for(int i=0; i < dimension_; i++)
00243   {
00244     jnt_pos_in(i) = ik_seed_state[i];
00245   }
00246 
00247   int ik_valid;
00248   if(consistency_limits.empty())
00249   {
00250     ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00251                                                    pose_desired,
00252                                                    jnt_pos_out,
00253                                                    timeout,
00254                                                    error_code,
00255                                                    solution_callback ?
00256                                                    boost::bind(solution_callback, _1, _2, _3):
00257                                                    IKCallbackFn());
00258   }
00259   else
00260   {
00261     ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in,
00262                                                    pose_desired,
00263                                                    jnt_pos_out,
00264                                                    timeout,
00265                                                    consistency_limits[free_angle_],
00266                                                    error_code,
00267                                                    solution_callback ?
00268                                                    boost::bind(solution_callback, _1, _2, _3):
00269                                                    IKCallbackFn());
00270   }
00271 
00272   if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION)
00273     return false;
00274 
00275   if(ik_valid >= 0)
00276   {
00277     solution.resize(dimension_);
00278     for(int i=0; i < dimension_; i++)
00279     {
00280       solution[i] = jnt_pos_out(i);
00281     }
00282     return true;
00283   }
00284   else
00285   {
00286     ROS_DEBUG("An IK solution could not be found");
00287     return false;
00288   }
00289 }
00290 
00291 bool PR2ArmKinematicsPlugin::getPositionFK(const std::vector<std::string> &link_names,
00292                                            const std::vector<double> &joint_angles,
00293                                            std::vector<geometry_msgs::Pose> &poses) const
00294 {
00295   if(!active_)
00296   {
00297     ROS_ERROR("kinematics not active");
00298     return false;
00299   }
00300 
00301   KDL::Frame p_out;
00302   KDL::JntArray jnt_pos_in;
00303   geometry_msgs::PoseStamped pose;
00304   tf::Stamped<tf::Pose> tf_pose;
00305 
00306   jnt_pos_in.resize(dimension_);
00307   for(int i=0; i < dimension_; i++)
00308   {
00309     jnt_pos_in(i) = joint_angles[i];
00310     //    ROS_DEBUG("Joint angle: %d %f",i,joint_angles[i]);
00311   }
00312 
00313   poses.resize(link_names.size());
00314 
00315   bool valid = true;
00316   for(unsigned int i=0; i < poses.size(); i++)
00317   {
00318     //    ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i]));
00319     if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,link_names[i])) >= 0)
00320     {
00321       tf::poseKDLToMsg(p_out,poses[i]);
00322     }
00323     else
00324     {
00325       ROS_ERROR("Could not compute FK for %s",link_names[i].c_str());
00326       valid = false;
00327     }
00328   }
00329   return valid;
00330 }
00331 
00332 const std::vector<std::string>& PR2ArmKinematicsPlugin::getJointNames() const
00333 {
00334   if(!active_)
00335   {
00336     ROS_ERROR("kinematics not active");
00337   }
00338   return ik_solver_info_.joint_names;
00339 }
00340 
00341 const std::vector<std::string>& PR2ArmKinematicsPlugin::getLinkNames() const
00342 {
00343   if(!active_)
00344   {
00345     ROS_ERROR("kinematics not active");
00346   }
00347   return fk_solver_info_.link_names;
00348 }
00349 
00350 } // namespace


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Thu Jun 6 2019 20:11:52