Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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 <moveit/pr2_arm_kinematics/pr2_arm_ik_solver.h>
00047 #include <tf_conversions/tf_kdl.h>
00048
00049 #include <moveit_msgs/GetPositionFK.h>
00050 #include <moveit_msgs/GetPositionIK.h>
00051 #include <moveit_msgs/GetKinematicSolverInfo.h>
00052 #include <moveit_msgs/MoveItErrorCodes.h>
00053
00054 #include <kdl/chainfksolverpos_recursive.hpp>
00055
00056 #include <boost/shared_ptr.hpp>
00057
00058 #include <moveit/kinematics_base/kinematics_base.h>
00059
00061 namespace pr2_arm_kinematics
00062 {
00063 class PR2ArmKinematicsPlugin : public kinematics::KinematicsBase
00064 {
00065 public:
00066
00070 PR2ArmKinematicsPlugin();
00071
00076 bool isActive();
00077
00078 virtual bool getPositionIK(const geometry_msgs::Pose &ik_pose,
00079 const std::vector<double> &ik_seed_state,
00080 std::vector<double> &solution,
00081 moveit_msgs::MoveItErrorCodes &error_code,
00082 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00083
00084 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00085 const std::vector<double> &ik_seed_state,
00086 double timeout,
00087 std::vector<double> &solution,
00088 moveit_msgs::MoveItErrorCodes &error_code,
00089 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00090
00091 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00092 const std::vector<double> &ik_seed_state,
00093 double timeout,
00094 const std::vector<double> &consistency_limits,
00095 std::vector<double> &solution,
00096 moveit_msgs::MoveItErrorCodes &error_code,
00097 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00098
00099 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00100 const std::vector<double> &ik_seed_state,
00101 double timeout,
00102 std::vector<double> &solution,
00103 const IKCallbackFn &solution_callback,
00104 moveit_msgs::MoveItErrorCodes &error_code,
00105 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00106
00107 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00108 const std::vector<double> &ik_seed_state,
00109 double timeout,
00110 const std::vector<double> &consistency_limits,
00111 std::vector<double> &solution,
00112 const IKCallbackFn &solution_callback,
00113 moveit_msgs::MoveItErrorCodes &error_code,
00114 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00115
00116 virtual bool getPositionFK(const std::vector<std::string> &link_names,
00117 const std::vector<double> &joint_angles,
00118 std::vector<geometry_msgs::Pose> &poses) const;
00119
00124 virtual bool initialize(const std::string& robot_description,
00125 const std::string& group_name,
00126 const std::string& base_name,
00127 const std::string& tip_name,
00128 double search_discretization);
00129
00133 const std::vector<std::string>& getJointNames() const;
00134
00138 const std::vector<std::string>& getLinkNames() const;
00139
00140 protected:
00141
00142 bool active_;
00143 int free_angle_;
00144 urdf::Model robot_model_;
00145 ros::NodeHandle node_handle_, root_handle_;
00146 boost::shared_ptr<pr2_arm_kinematics::PR2ArmIKSolver> pr2_arm_ik_solver_;
00147 ros::ServiceServer ik_service_,fk_service_,ik_solver_info_service_,fk_solver_info_service_;
00148
00149 std::string root_name_;
00150 int dimension_;
00151 boost::shared_ptr<KDL::ChainFkSolverPos_recursive> jnt_to_pose_solver_;
00152 KDL::Chain kdl_chain_;
00153 moveit_msgs::KinematicSolverInfo ik_solver_info_, fk_solver_info_;
00154
00155 };
00156 }
00157
00158 #endif