$search
00001 /********************************************************************* 00002 * 00003 * Software License Agreement (BSD License) 00004 * 00005 * Copyright (c) 2008, Willow Garage, Inc. 00006 * All rights reserved. 00007 * 00008 * Redistribution and use in source and binary forms, with or without 00009 * modification, are permitted provided that the following conditions 00010 * are met: 00011 * 00012 * * Redistributions of source code must retain the above copyright 00013 * notice, this list of conditions and the following disclaimer. 00014 * * Redistributions in binary form must reproduce the above 00015 * copyright notice, this list of conditions and the following 00016 * disclaimer in the documentation and/or other materials provided 00017 * with the distribution. 00018 * * Neither the name of the Willow Garage nor the names of its 00019 * contributors may be used to endorse or promote products derived 00020 * from this software without specific prior written permission. 00021 * 00022 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 * POSSIBILITY OF SUCH DAMAGE. 00034 * 00035 * Author: Sachin Chitta 00036 *********************************************************************/ 00037 00038 #ifndef PR2_ARM_IK_NODE_H 00039 #define PR2_ARM_IK_NODE_H 00040 00041 #include <ros/ros.h> 00042 #include <tf/tf.h> 00043 #include <tf/transform_listener.h> 00044 00045 #include <angles/angles.h> 00046 #include <pr2_arm_kinematics/pr2_arm_ik_solver.h> 00047 #include <tf_conversions/tf_kdl.h> 00048 00049 #include <kinematics_msgs/GetPositionFK.h> 00050 #include <kinematics_msgs/GetPositionIK.h> 00051 #include <kinematics_msgs/GetKinematicSolverInfo.h> 00052 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h> 00053 00054 #include <kdl/chainfksolverpos_recursive.hpp> 00055 00056 #include <boost/shared_ptr.hpp> 00057 00058 #include <kinematics_base/kinematics_base.h> 00059 00060 namespace pr2_arm_kinematics 00061 { 00062 class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase 00063 { 00064 public: 00065 00069 PR2ArmKinematicsPlugin(); 00070 00075 bool isActive(); 00076 00084 bool getPositionIK(const geometry_msgs::Pose &ik_pose, 00085 const std::vector<double> &ik_seed_state, 00086 std::vector<double> &solution, 00087 int &error_code); 00088 00097 bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 00098 const std::vector<double> &ik_seed_state, 00099 const double &timeout, 00100 std::vector<double> &solution, 00101 int &error_code); 00110 bool searchPositionIK(const geometry_msgs::Pose &ik_pose, 00111 const std::vector<double> &ik_seed_state, 00112 const double &timeout, 00113 std::vector<double> &solution, 00114 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &desired_pose_callback, 00115 const boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> &solution_callback, 00116 int &error_code); 00117 00124 bool getPositionFK(const std::vector<std::string> &link_names, 00125 const std::vector<double> &joint_angles, 00126 std::vector<geometry_msgs::Pose> &poses); 00127 00132 bool initialize(std::string name); 00133 00138 std::string getBaseFrame(); 00139 00143 std::string getToolFrame(); 00144 00148 std::vector<std::string> getJointNames(); 00149 00153 std::vector<std::string> getLinkNames(); 00154 00155 protected: 00156 00157 bool active_; 00158 int free_angle_; 00159 urdf::Model robot_model_; 00160 double search_discretization_; 00161 ros::NodeHandle node_handle_, root_handle_; 00162 boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_ik_solver_; 00163 ros::ServiceServer ik_service_,fk_service_,ik_solver_info_service_,fk_solver_info_service_; 00164 //tf::TransformListener tf_; 00165 std::string root_name_; 00166 int dimension_; 00167 boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_; 00168 KDL::Chain kdl_chain_; 00169 kinematics_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_; 00170 00171 boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> desiredPoseCallback_; 00172 boost::function<void(const geometry_msgs::Pose &ik_pose,const std::vector<double> &ik_solution,int &error_code)> solutionCallback_; 00173 void desiredPoseCallback(const KDL::JntArray& jnt_array, 00174 const KDL::Frame& ik_pose, 00175 arm_navigation_msgs::ArmNavigationErrorCodes& error_code); 00176 00177 void jointSolutionCallback(const KDL::JntArray& jnt_array, 00178 const KDL::Frame& ik_pose, 00179 arm_navigation_msgs::ArmNavigationErrorCodes& error_code); 00180 00181 00182 }; 00183 } 00184 00185 #endif