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
00039
00040
00041
00042 #ifndef MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_
00043 #define MOVEIT_ROS_PLANNING_SRV_KINEMATICS_PLUGIN_
00044
00045
00046 #include <ros/ros.h>
00047
00048
00049 #include <boost/shared_ptr.hpp>
00050
00051
00052 #include <geometry_msgs/PoseStamped.h>
00053 #include <moveit_msgs/GetPositionFK.h>
00054 #include <moveit_msgs/GetPositionIK.h>
00055 #include <moveit_msgs/GetKinematicSolverInfo.h>
00056 #include <moveit_msgs/MoveItErrorCodes.h>
00057
00058
00059 #include <moveit/kinematics_base/kinematics_base.h>
00060 #include <moveit/robot_model/robot_model.h>
00061 #include <moveit/robot_state/robot_state.h>
00062
00063 namespace srv_kinematics_plugin
00064 {
00069 class SrvKinematicsPlugin : public kinematics::KinematicsBase
00070 {
00071 public:
00072
00076 SrvKinematicsPlugin();
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
00120 virtual bool initialize(const std::string &robot_description,
00121 const std::string &group_name,
00122 const std::string &base_name,
00123 const std::string &tip_frame,
00124 double search_discretization)
00125 {
00126 std::vector<std::string> tip_frames;
00127 tip_frames.push_back(tip_frame);
00128 initialize(robot_description, group_name, base_name, tip_frames, search_discretization);
00129 }
00130
00131 virtual bool initialize(const std::string &robot_description,
00132 const std::string &group_name,
00133 const std::string &base_name,
00134 const std::vector<std::string> &tip_frames,
00135 double search_discretization);
00136
00140 const std::vector<std::string>& getJointNames() const;
00141
00145 const std::vector<std::string>& getLinkNames() const;
00146
00150 const std::vector<std::string>& getVariableNames() const;
00151
00152 protected:
00153
00154 virtual bool searchPositionIK(const geometry_msgs::Pose &ik_pose,
00155 const std::vector<double> &ik_seed_state,
00156 double timeout,
00157 std::vector<double> &solution,
00158 const IKCallbackFn &solution_callback,
00159 moveit_msgs::MoveItErrorCodes &error_code,
00160 const std::vector<double> &consistency_limits,
00161 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00162
00163 virtual bool searchPositionIK(const std::vector<geometry_msgs::Pose> &ik_poses,
00164 const std::vector<double> &ik_seed_state,
00165 double timeout,
00166 const std::vector<double> &consistency_limits,
00167 std::vector<double> &solution,
00168 const IKCallbackFn &solution_callback,
00169 moveit_msgs::MoveItErrorCodes &error_code,
00170 const kinematics::KinematicsQueryOptions &options = kinematics::KinematicsQueryOptions()) const;
00171
00172 virtual bool setRedundantJoints(const std::vector<unsigned int> &redundant_joint_indices);
00173
00174 private:
00175
00176 bool timedOut(const ros::WallTime &start_time, double duration) const;
00177
00178 int getJointIndex(const std::string &name) const;
00179
00180 bool isRedundantJoint(unsigned int index) const;
00181
00182 bool active_;
00184 moveit_msgs::KinematicSolverInfo ik_group_info_;
00186 unsigned int dimension_;
00188 robot_model::RobotModelPtr robot_model_;
00189 robot_model::JointModelGroup* joint_model_group_;
00190
00191 robot_state::RobotStatePtr robot_state_;
00192
00193 int num_possible_redundant_joints_;
00194
00195 boost::shared_ptr<ros::ServiceClient> ik_service_client_;
00196
00197
00198 };
00199 }
00200
00201 #endif