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
00037 #ifndef OMPL_ROS_RPY_IK_STATE_TRANSFORMER_H_
00038 #define OMPL_ROS_RPY_IK_STATE_TRANSFORMER_H_
00039
00040
00041 #include <ompl_ros_interface/state_transformers/ompl_ros_ik_state_transformer.h>
00042 #include <ompl_ros_interface/helpers/ompl_ros_conversions.h>
00043
00044
00045 #include <pluginlib/class_loader.h>
00046 #include <kinematics_base/kinematics_base.h>
00047
00048 namespace ompl_ros_interface
00049 {
00050 class OmplRosRPYIKStateTransformer : public OmplRosIKStateTransformer
00051 {
00052 public:
00053
00054
00055
00056
00057 OmplRosRPYIKStateTransformer(const ompl::base::StateSpacePtr &state_space,
00058 const planning_models::KinematicModel::JointModelGroup* physical_joint_model_group)
00059 : OmplRosIKStateTransformer(state_space,physical_joint_model_group){};
00060
00061 ~OmplRosRPYIKStateTransformer(){}
00062
00063
00064
00065 virtual bool initialize();
00066
00067
00068
00069
00070 virtual bool configureOnRequest(const arm_navigation_msgs::GetMotionPlan::Request &request,
00071 arm_navigation_msgs::GetMotionPlan::Response &response);
00072
00073
00074
00075 virtual bool inverseTransform(const ompl::base::State &ompl_state,
00076 arm_navigation_msgs::RobotState &robot_state);
00077
00078
00079
00080 virtual bool forwardTransform(const arm_navigation_msgs::RobotState &robot_state,
00081 ompl::base::State &ompl_state);
00082
00083
00084
00085
00086 virtual arm_navigation_msgs::RobotState getDefaultState();
00087
00088 private:
00089
00090 int real_vector_index_;
00091 arm_navigation_msgs::RobotState seed_state_, solution_state_;
00092 int x_index_, y_index_, z_index_, pitch_index_, roll_index_, yaw_index_;
00093 boost::shared_ptr<ompl::base::ScopedState<ompl::base::CompoundStateSpace> > scoped_state_;
00094
00095 ompl_ros_interface::OmplStateToRobotStateMapping ompl_state_to_robot_state_mapping_;
00096 ompl_ros_interface::RobotStateToOmplStateMapping robot_state_to_ompl_state_mapping_;
00097
00098 void omplStateToPose(const ompl::base::State &ompl_state,
00099 geometry_msgs::Pose &pose);
00100
00101 double generateRandomNumber(const double &min, const double &max);
00102 void generateRandomState(arm_navigation_msgs::RobotState &robot_state);
00103
00104
00105 };
00106 }
00107
00108 #endif //OMPL_ROS_RPY_IK_STATE_TRANSFORMER_H_