Add simple ompl profile interface

tesseract-robotics · Dec 14, 2024 · c24cb98 · c24cb98
1 parent ad73309
commit c24cb98
Show file tree

Hide file tree

Showing 12 changed files with 401 additions and 1,152 deletions.
diff --git a/tesseract_motion_planners/ompl/CMakeLists.txt b/tesseract_motion_planners/ompl/CMakeLists.txt
@@ -14,9 +14,7 @@ set(OMPL_SRC
     src/profile/ompl_default_plan_profile.cpp
     src/utils.cpp
     src/state_collision_validator.cpp
-    src/compound_state_validator.cpp
-    src/serialize.cpp
-    src/deserialize.cpp)
+    src/compound_state_validator.cpp)
 
 # if(NOT OMPL_VERSION VERSION_LESS "1.4.0") list(APPEND OMPL_SRC src/config/ompl_planner_constrained_config.cpp) endif()
 

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/deserialize.h
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_motion_planner.h
@@ -94,14 +94,6 @@ class OMPLMotionPlanner : public MotionPlanner
 protected:
   /** @brief OMPL Parallel planner */
   std::shared_ptr<ompl::tools::ParallelPlan> parallel_plan_;
-
-  OMPLProblemConfig createSubProblem(const PlannerRequest& request,
-                                     const tesseract_common::ManipulatorInfo& composite_mi,
-                                     const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                                     const MoveInstructionPoly& start_instruction,
-                                     const MoveInstructionPoly& end_instruction,
-                                     int n_output_states,
-                                     int index) const;
 };
 
 }  // namespace tesseract_planning

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/ompl_problem.h
@@ -68,18 +68,6 @@ namespace tesseract_planning
 struct OMPLProblem;
 struct OMPLPlannerConfigurator;
 
-using StateSamplerAllocator =
-    std::function<ompl::base::StateSamplerPtr(const ompl::base::StateSpace*, const OMPLProblem&)>;
-
-using OptimizationObjectiveAllocator =
-    std::function<ompl::base::OptimizationObjectivePtr(const ompl::base::SpaceInformationPtr&, const OMPLProblem&)>;
-
-using StateValidityCheckerAllocator =
-    std::function<ompl::base::StateValidityCheckerPtr(const ompl::base::SpaceInformationPtr&, const OMPLProblem&)>;
-
-using MotionValidatorAllocator =
-    std::function<ompl::base::MotionValidatorPtr(const ompl::base::SpaceInformationPtr&, const OMPLProblem&)>;
-
 enum class OMPLProblemStateSpace
 {
   REAL_STATE_SPACE,

diff --git a/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h b/..._planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_default_plan_profile.h
@@ -41,12 +41,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 #include <tesseract_command_language/fwd.h>
 #include <tesseract_common/fwd.h>
 
-namespace ompl::base
-{
-class StateValidityChecker;
-using StateValidityCheckerPtr = std::shared_ptr<StateValidityChecker>;
-}  // namespace ompl::base
-
 namespace tesseract_planning
 {
 struct OMPLPlannerConfigurator;
@@ -62,10 +56,6 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   using ConstPtr = std::shared_ptr<const OMPLDefaultPlanProfile>;
 
   OMPLDefaultPlanProfile();
-  OMPLDefaultPlanProfile(const tinyxml2::XMLElement& xml_element);
-
-  /** @brief The OMPL state space to use when planning */
-  OMPLProblemStateSpace state_space{ OMPLProblemStateSpace::REAL_STATE_SPACE };
 
   /** @brief Max planning time allowed in seconds */
   double planning_time = 5.0;
@@ -104,57 +94,64 @@ class OMPLDefaultPlanProfile : public OMPLPlanProfile
   /** @brief The collision check configuration */
   tesseract_collision::CollisionCheckConfig collision_check_config;
 
-  /** @brief The state sampler allocator. This can be null and it will use Tesseract default state sampler allocator. */
-  StateSamplerAllocator state_sampler_allocator;
+  std::unique_ptr<OMPLProblem> create(const PlannerRequest& request,
+                                      const MoveInstructionPoly& start_instruction,
+                                      const MoveInstructionPoly& end_instruction,
+                                      int n_output_states) const override;
 
-  /** @brief Set the optimization objective function allocator. Default is to minimize path length */
-  OptimizationObjectiveAllocator optimization_objective_allocator;
+protected:
+  void setup(OMPLProblem& prob) const;
 
-  /** @brief The ompl state validity checker. If nullptr and collision checking enabled it uses
-   * StateCollisionValidator */
-  StateValidityCheckerAllocator svc_allocator;
+  static void applyGoalStates(OMPLProblem& prob,
+                              const Eigen::Isometry3d& cartesian_waypoint,
+                              const MoveInstructionPoly& parent_instruction,
+                              const tesseract_common::ManipulatorInfo& manip_info);
 
-  /** @brief The ompl motion validator. If nullptr and continuous collision checking enabled it used
-   * ContinuousMotionValidator */
-  MotionValidatorAllocator mv_allocator;
+  static void applyGoalStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint);
 
-  void setup(OMPLProblem& prob) const override;
+  static void applyStartStates(OMPLProblem& prob,
+                               const Eigen::Isometry3d& cartesian_waypoint,
+                               const MoveInstructionPoly& parent_instruction,
+                               const tesseract_common::ManipulatorInfo& manip_info);
 
-  void applyGoalStates(OMPLProblem& prob,
-                       const Eigen::Isometry3d& cartesian_waypoint,
-                       const MoveInstructionPoly& parent_instruction,
-                       const tesseract_common::ManipulatorInfo& manip_info,
-                       const std::vector<std::string>& active_links,
-                       int index) const override;
+  static void applyStartStates(OMPLProblem& prob, const Eigen::VectorXd& joint_waypoint);
 
-  void applyGoalStates(OMPLProblem& prob,
-                       const Eigen::VectorXd& joint_waypoint,
-                       const MoveInstructionPoly& parent_instruction,
-                       const tesseract_common::ManipulatorInfo& manip_info,
-                       const std::vector<std::string>& active_links,
-                       int index) const override;
+  /**
+   * @brief Create state sampler allocator
+   * @param prob The OMPL problem
+   * @return OMPL state sampler allocator
+   */
+  virtual std::function<ompl::base::StateSamplerPtr(const ompl::base::StateSpace*)>
+  createStateSamplerAllocator(OMPLProblem& prob) const;
 
-  void applyStartStates(OMPLProblem& prob,
-                        const Eigen::Isometry3d& cartesian_waypoint,
-                        const MoveInstructionPoly& parent_instruction,
-                        const tesseract_common::ManipulatorInfo& manip_info,
-                        const std::vector<std::string>& active_links,
-                        int index) const override;
+  /**
+   * @brief Create state validator which should not be the collision state validator
+   * @param prob The OMPL problem
+   * @return OMPL state validator
+   */
+  virtual std::unique_ptr<ompl::base::StateValidityChecker> createStateValidator(OMPLProblem& prob) const;
 
-  void applyStartStates(OMPLProblem& prob,
-                        const Eigen::VectorXd& joint_waypoint,
-                        const MoveInstructionPoly& parent_instruction,
-                        const tesseract_common::ManipulatorInfo& manip_info,
-                        const std::vector<std::string>& active_links,
-                        int index) const override;
+  /**
+   * @brief Create collision state validator
+   * @param prob The OMPL problem
+   * @return OMPL state collision validator
+   */
+  virtual std::unique_ptr<ompl::base::StateValidityChecker> createCollisionStateValidator(OMPLProblem& prob) const;
 
-  tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const override;
+  /**
+   * @brief Create motion validator
+   * @param prob The OMPL problem
+   * @return OMPL motion validator
+   */
+  virtual std::unique_ptr<ompl::base::MotionValidator>
+  createMotionValidator(OMPLProblem& prob, const ompl::base::StateValidityCheckerPtr& svc_without_collision) const;
 
-protected:
-  ompl::base::StateValidityCheckerPtr processStateValidator(OMPLProblem& prob) const;
-  void processMotionValidator(OMPLProblem& prob,
-                              const ompl::base::StateValidityCheckerPtr& svc_without_collision) const;
-  void processOptimizationObjective(OMPLProblem& prob) const;
+  /**
+   * @brief Create OMPL optimization object
+   * @param prob The OMPL problem
+   * @return OMPL optimization objective
+   */
+  virtual std::unique_ptr<ompl::base::OptimizationObjective> createOptimizationObjective(OMPLProblem& prob) const;
 
   friend class boost::serialization::access;
   template <class Archive>

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h
@@ -34,18 +34,17 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_common/fwd.h>
+#include <tesseract_kinematics/core/fwd.h>
+#include <tesseract_environment/fwd.h>
 #include <tesseract_command_language/fwd.h>
-#include <tesseract_command_language/profile.h>
+#include <tesseract_motion_planners/core/fwd.h>
 
-namespace tinyxml2
-{
-class XMLElement;  // NOLINT
-class XMLDocument;
-}  // namespace tinyxml2
+#include <tesseract_command_language/profile.h>
 
 namespace tesseract_planning
 {
 struct OMPLProblem;
+
 class OMPLPlanProfile : public Profile
 {
 public:
@@ -60,46 +59,25 @@ class OMPLPlanProfile : public Profile
    */
   static std::size_t getStaticKey();
 
-  virtual void setup(OMPLProblem& prob) const = 0;
-
-  virtual void applyGoalStates(OMPLProblem& prob,
-                               const Eigen::Isometry3d& cartesian_waypoint,
-                               const MoveInstructionPoly& parent_instruction,
-                               const tesseract_common::ManipulatorInfo& manip_info,
-                               const std::vector<std::string>& active_links,
-                               int index) const = 0;
-
-  virtual void applyGoalStates(OMPLProblem& prob,
-                               const Eigen::VectorXd& joint_waypoint,
-                               const MoveInstructionPoly& parent_instruction,
-                               const tesseract_common::ManipulatorInfo& manip_info,
-                               const std::vector<std::string>& active_links,
-                               int index) const = 0;
-
-  virtual void applyStartStates(OMPLProblem& prob,
-                                const Eigen::Isometry3d& cartesian_waypoint,
-                                const MoveInstructionPoly& parent_instruction,
-                                const tesseract_common::ManipulatorInfo& manip_info,
-                                const std::vector<std::string>& active_links,
-                                int index) const = 0;
-
-  virtual void applyStartStates(OMPLProblem& prob,
-                                const Eigen::VectorXd& joint_waypoint,
-                                const MoveInstructionPoly& parent_instruction,
-                                const tesseract_common::ManipulatorInfo& manip_info,
-                                const std::vector<std::string>& active_links,
-                                int index) const = 0;
-
-  virtual tinyxml2::XMLElement* toXML(tinyxml2::XMLDocument& doc) const = 0;
+  /**
+   * @brief Create OMPL Planning Problem
+   * @param request The planning request
+   * @param start_instruction The start instruction
+   * @param end_instruction The goal instruction
+   * @param n_output_states The number of interpolated output states
+   * @return A OMPL Problem
+   */
+  virtual std::unique_ptr<OMPLProblem> create(const PlannerRequest& request,
+                                              const MoveInstructionPoly& start_instruction,
+                                              const MoveInstructionPoly& end_instruction,
+                                              int n_output_states) const = 0;
 
 protected:
   friend class boost::serialization::access;
   template <class Archive>
   void serialize(Archive&, const unsigned int);  // NOLINT
 };
 
-/** @todo Currently OMPL does not have support of composite profile everything is handled by the plan profile */
-
 }  // namespace tesseract_planning
 
 BOOST_CLASS_EXPORT_KEY(tesseract_planning::OMPLPlanProfile)

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/serialize.h