pr2_arm_kinematics_plugin.h
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 #ifndef PR2_ARM_IK_NODE_H
00038 #define PR2_ARM_IK_NODE_H
00039 
00040 #include <ros/ros.h>
00041 #include <tf/tf.h>
00042 #include <tf/transform_listener.h>
00043 
00044 #include <angles/angles.h>
00045 #include <moveit/pr2_arm_kinematics/pr2_arm_ik_solver.h>
00046 #include <tf_conversions/tf_kdl.h>
00047 
00048 #include <moveit_msgs/GetPositionFK.h>
00049 #include <moveit_msgs/GetPositionIK.h>
00050 #include <moveit_msgs/GetKinematicSolverInfo.h>
00051 #include <moveit_msgs/MoveItErrorCodes.h>
00052 
00053 #include <kdl/chainfksolverpos_recursive.hpp>
00054 
00055 #include <boost/shared_ptr.hpp>
00056 
00057 #include <moveit/kinematics_base/kinematics_base.h>
00058 
00060 namespace pr2_arm_kinematics
00061 {
00062 class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase
00063 {
00064 public:
00065 
00069   PR2ArmKinematicsPlugin();
00070 
00075   bool isActive();
00076 
00077   virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00078                              const std::vector<double> &ik_seed_state,
00079                              std::vector<double> &solution,
00080                              moveit_msgs::MoveItErrorCodes &error_code,
00081                              const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00082 
00083   virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00084                                 const std::vector<double> &ik_seed_state,
00085                                 double timeout,
00086                                 std::vector<double> &solution,
00087                                 moveit_msgs::MoveItErrorCodes &error_code,
00088                                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00089 
00090   virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00091                                 const std::vector<double> &ik_seed_state,
00092                                 double timeout,
00093                                 const std::vector<double> &consistency_limits,
00094                                 std::vector<double> &solution,
00095                                 moveit_msgs::MoveItErrorCodes &error_code,
00096                                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00097 
00098   virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00099                                 const std::vector<double> &ik_seed_state,
00100                                 double timeout,
00101                                 std::vector<double> &solution,
00102                                 const IKCallbackFn &solution_callback,
00103                                 moveit_msgs::MoveItErrorCodes &error_code,
00104                                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00105 
00106   virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00107                                 const std::vector<double> &ik_seed_state,
00108                                 double timeout,
00109                                 const std::vector<double> &consistency_limits,
00110                                 std::vector<double> &solution,
00111                                 const IKCallbackFn &solution_callback,
00112                                 moveit_msgs::MoveItErrorCodes &error_code,
00113                                 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00114 
00115   virtual bool getPositionFK(const std::vector<std::string> &link_names,
00116                              const std::vector<double> &joint_angles,
00117                              std::vector<geometry_msgs::Pose> &poses) const;
00118 
00123   virtual bool initialize(const std::string& robot_description,
00124                           const std::string& group_name,
00125                           const std::string& base_frame,
00126                           const std::string& tip_frame,
00127                           double search_discretization);
00128 
00132   const std::vector<std::string>& getJointNames() const;
00133 
00137   const std::vector<std::string>& getLinkNames() const;
00138 
00139 protected:
00140 
00141   bool active_;
00142   int free_angle_;
00143   urdf::Model robot_model_;
00144   ros::NodeHandle node_handle_, root_handle_;
00145   boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_ik_solver_;
00146   ros::ServiceServer ik_service_,fk_service_,ik_solver_info_service_,fk_solver_info_service_;
00147   //tf::TransformListener tf_;
00148   std::string root_name_;
00149   int dimension_;
00150   boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
00151   KDL::Chain kdl_chain_;
00152   moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
00153 
00154 };
00155 }
00156 
00157 #endif


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Sep 14 2015 14:17:49