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_IK_RPY_TASK_SPACE_PLANNER_H_
00038 #define OMPL_ROS_IK_RPY_TASK_SPACE_PLANNER_H_
00039
00040
00041 #include <ompl_ros_interface/planners/ompl_ros_task_space_planner.h>
00042 #include <ompl_ros_interface/state_validity_checkers/ompl_ros_task_space_validity_checker.h>
00043 #include <ompl_ros_interface/state_transformers/ompl_ros_rpy_ik_state_transformer.h>
00044
00045 #include <arm_navigation_msgs/ArmNavigationErrorCodes.h>
00046
00047 namespace ompl_ros_interface
00048 {
00049 class OmplRosRPYIKTaskSpacePlanner: public OmplRosTaskSpacePlanner
00050 {
00051 protected:
00052 virtual bool initializeStateValidityChecker(ompl_ros_interface::OmplRosStateValidityCheckerPtr &state_validity_checker);
00053
00054 virtual arm_navigation_msgs::RobotTrajectory getSolutionPath();
00055
00056 virtual bool constraintsToOmplState(const arm_navigation_msgs::Constraints &constraints,
00057 ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal);
00058
00059 private:
00060
00061 bool poseStampedToOmplState(const geometry_msgs::PoseStamped &pose_stamped,
00062 ompl::base::ScopedState<ompl::base::CompoundStateSpace> &goal,
00063 const bool &return_if_outside_constraints = true);
00064
00065 geometry_msgs::PoseStamped getEndEffectorPose(const arm_navigation_msgs::RobotState &robot_state);
00066
00067 virtual bool setStart(arm_navigation_msgs::GetMotionPlan::Request &request,
00068 arm_navigation_msgs::GetMotionPlan::Response &response);
00069
00070 bool checkAndCorrectForWrapAround(double &value,
00071 const double &min_v,
00072 const double &max_v);
00073
00074 boost::shared_ptr<ompl::base::RealVectorBounds> original_real_vector_bounds_;
00075
00076 bool getEndEffectorConstraints(const arm_navigation_msgs::Constraints &constraints,
00077 arm_navigation_msgs::PositionConstraint &position_constraint,
00078 arm_navigation_msgs::OrientationConstraint &orientation_constraint,
00079 const bool &need_both_constraints);
00080
00081 bool positionConstraintToOmplStateBounds(const arm_navigation_msgs::PositionConstraint &position_constraint,
00082 ompl::base::StateSpacePtr &goal);
00083
00084 bool orientationConstraintToOmplStateBounds(const arm_navigation_msgs::OrientationConstraint &orientation_constraint,
00085 ompl::base::StateSpacePtr &goal);
00086
00087 std::string end_effector_name_;
00088
00089 };
00090 }
00091 #endif //OMPL_ROS_IK_RPY_TASK_SPACE_PLANNER_H_