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 <motion_planning_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 motion_planning_msgs::RobotTrajectory getSolutionPath();
00055
00056 virtual bool constraintsToOmplState(const motion_planning_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 motion_planning_msgs::RobotState &robot_state);
00066
00067 tf::TransformListener tf_;
00068
00069 virtual bool setStart(motion_planning_msgs::GetMotionPlan::Request &request,
00070 motion_planning_msgs::GetMotionPlan::Response &response);
00071
00072 bool checkAndCorrectForWrapAround(double &value,
00073 const double &min_v,
00074 const double &max_v);
00075
00076 boost::shared_ptr<ompl::base::RealVectorBounds> original_real_vector_bounds_;
00077
00078 bool getEndEffectorConstraints(const motion_planning_msgs::Constraints &constraints,
00079 motion_planning_msgs::PositionConstraint &position_constraint,
00080 motion_planning_msgs::OrientationConstraint &orientation_constraint,
00081 const bool &need_both_constraints);
00082
00083 bool positionConstraintToOmplStateBounds(const motion_planning_msgs::PositionConstraint &position_constraint,
00084 ompl::base::StateSpacePtr &goal);
00085
00086 bool orientationConstraintToOmplStateBounds(const motion_planning_msgs::OrientationConstraint &orientation_constraint,
00087 ompl::base::StateSpacePtr &goal);
00088
00089 std::string end_effector_name_;
00090
00091 };
00092 }
00093 #endif //OMPL_ROS_IK_RPY_TASK_SPACE_PLANNER_H_