Removed OMPL state extractor concept

tesseract-robotics · Dec 23, 2024 · 4878e05 · 4878e05
1 parent e00ae6f
commit 4878e05
Show file tree

Hide file tree

Showing 11 changed files with 183 additions and 292 deletions.
diff --git a/...motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h b/...motion_planners/ompl/include/tesseract_motion_planners/ompl/continuous_motion_validator.h
@@ -34,7 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_environment/fwd.h>
@@ -57,8 +56,7 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
                             ompl::base::StateValidityCheckerPtr state_validator,
                             const tesseract_environment::Environment& env,
                             std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-                            const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                            OMPLStateExtractor extractor);
+                            const tesseract_collision::CollisionCheckConfig& collision_check_config);
 
   bool checkMotion(const ompl::base::State* s1, const ompl::base::State* s2) const override;
 
@@ -91,9 +89,6 @@ class ContinuousMotionValidator : public ompl::base::MotionValidator
   /** @brief A list of active links */
   std::vector<std::string> links_;
 
-  /** @brief This will extract an Eigen::VectorXd from the OMPL State */
-  OMPLStateExtractor extractor_;
-
   // The items below are to cache the contact manager based on thread ID. Currently ompl is multi
   // threaded but the methods used to implement collision checking are not thread safe. To prevent
   // reconstructing the collision environment for every check this will cache a contact manager

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
@@ -33,8 +33,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <Eigen/Geometry>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_common/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_environment/fwd.h>
@@ -45,6 +43,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP
 namespace ompl::geometric
 {
 class SimpleSetup;
+class PathGeometric;
 }
 
 namespace tesseract_planning
@@ -72,12 +71,6 @@ class OMPLPlanProfile : public Profile
    */
   virtual std::unique_ptr<OMPLSolverConfig> createSolverConfig() const = 0;
 
-  /**
-   * @brief Create the state extractor
-   * @return The OMPL state extractor
-   */
-  virtual OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const = 0;
-
   /**
    * @brief Create OMPL Simple Setup
    * @param start_instruction The start instruction
@@ -92,6 +85,13 @@ class OMPLPlanProfile : public Profile
                     const tesseract_common::ManipulatorInfo& composite_mi,
                     const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
 
+  /**
+   * @brief Convert an OMPL defined path into a composite instruction that can be returned by the planner
+   */
+  virtual CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path,
+                                           const tesseract_common::ManipulatorInfo& composite_mi,
+                                           const std::shared_ptr<const tesseract_environment::Environment>& env) const = 0;
+
 protected:
   friend class boost::serialization::access;
   template <class Archive>

diff --git a/...nners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h b/...nners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_real_vector_plan_profile.h
@@ -76,14 +76,16 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
 
   std::unique_ptr<OMPLSolverConfig> createSolverConfig() const override;
 
-  OMPLStateExtractor createStateExtractor(const tesseract_kinematics::JointGroup& manip) const override;
-
   std::unique_ptr<ompl::geometric::SimpleSetup>
   createSimpleSetup(const MoveInstructionPoly& start_instruction,
                     const MoveInstructionPoly& end_instruction,
                     const tesseract_common::ManipulatorInfo& composite_mi,
                     const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
 
+  CompositeInstruction convertPath(const ompl::geometric::PathGeometric& path,
+                                   const tesseract_common::ManipulatorInfo& composite_mi,
+                                   const std::shared_ptr<const tesseract_environment::Environment>& env) const override;
+
 protected:
   static void applyGoalStates(ompl::geometric::SimpleSetup& simple_setup,
                               const tesseract_kinematics::KinGroupIKInput& ik_input,
@@ -126,8 +128,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::StateValidityChecker>
   createStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
                        const std::shared_ptr<const tesseract_environment::Environment>& env,
-                       const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                       const OMPLStateExtractor& state_extractor) const;
+                       const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   /**
    * @brief Create collision state validator
@@ -140,8 +141,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::StateValidityChecker>
   createCollisionStateValidator(const ompl::geometric::SimpleSetup& simple_setup,
                                 const std::shared_ptr<const tesseract_environment::Environment>& env,
-                                const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                                const OMPLStateExtractor& state_extractor) const;
+                                const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   /**
    * @brief Create motion validator
@@ -155,7 +155,6 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   createMotionValidator(const ompl::geometric::SimpleSetup& simple_setup,
                         const std::shared_ptr<const tesseract_environment::Environment>& env,
                         const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                        const OMPLStateExtractor& state_extractor,
                         const std::shared_ptr<ompl::base::StateValidityChecker>& svc_without_collision) const;
 
   /**
@@ -169,8 +168,7 @@ class OMPLRealVectorPlanProfile : public OMPLPlanProfile
   virtual std::unique_ptr<ompl::base::OptimizationObjective>
   createOptimizationObjective(const ompl::geometric::SimpleSetup& simple_setup,
                               const std::shared_ptr<const tesseract_environment::Environment>& env,
-                              const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip,
-                              const OMPLStateExtractor& state_extractor) const;
+                              const std::shared_ptr<const tesseract_kinematics::JointGroup>& manip) const;
 
   friend class boost::serialization::access;
   template <class Archive>

diff --git a/...t_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h b/...t_motion_planners/ompl/include/tesseract_motion_planners/ompl/state_collision_validator.h
@@ -34,8 +34,6 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <mutex>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_environment/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
 #include <tesseract_collision/core/fwd.h>
@@ -55,8 +53,7 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
   StateCollisionValidator(const ompl::base::SpaceInformationPtr& space_info,
                           const tesseract_environment::Environment& env,
                           std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-                          const tesseract_collision::CollisionCheckConfig& collision_check_config,
-                          OMPLStateExtractor extractor);
+                          const tesseract_collision::CollisionCheckConfig& collision_check_config);
 
   bool isValid(const ompl::base::State* state) const override;
 
@@ -70,9 +67,6 @@ class StateCollisionValidator : public ompl::base::StateValidityChecker
   /** @brief A list of active links */
   std::vector<std::string> links_;
 
-  /** @brief This will extract an Eigen::VectorXd from the OMPL State */
-  OMPLStateExtractor extractor_;
-
   // The items below are to cache the contact manager based on thread ID. Currently ompl is multi
   // threaded but the methods used to implement collision checking are not thread safe. To prevent
   // reconstructing the collision environment for every check this will cache a contact manager

diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/types.h
diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/utils.h
@@ -33,11 +33,10 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 #include <memory>
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
-#include <tesseract_motion_planners/ompl/types.h>
-
 #include <tesseract_common/eigen_types.h>
 #include <tesseract_collision/core/fwd.h>
 #include <tesseract_kinematics/core/fwd.h>
+#include <tesseract_command_language/composite_instruction.h>
 
 namespace ompl::base
 {
@@ -55,21 +54,6 @@ class PathGeometric;
 
 namespace tesseract_planning
 {
-Eigen::Map<Eigen::VectorXd> RealVectorStateSpaceExtractor(const ompl::base::State* s1, unsigned dimension);
-
-#ifndef OMPL_LESS_1_4_0
-Eigen::Map<Eigen::VectorXd> ConstrainedStateSpaceExtractor(const ompl::base::State* s1);
-#endif
-
-/**
- * @brief Convert an ompl path to tesseract TrajArray
- * @param path OMPL Path
- * @param extractor This function understands the type of state space and converts it to an eigen vector.
- * @return Tesseract TrajArray
- */
-tesseract_common::TrajArray toTrajArray(const ompl::geometric::PathGeometric& path,
-                                        const OMPLStateExtractor& extractor);
-
 /**
  * @brief Given longest valid fraction and length it will set the correct information of the state space
  * @param state_space_ptr OMPL State Space
@@ -111,6 +95,29 @@ ompl::base::StateSamplerPtr allocWeightedRealVectorStateSampler(const ompl::base
                                                                 const Eigen::VectorXd& weights,
                                                                 const Eigen::MatrixX2d& limits);
 
+/**
+ * @brief Converts an OMPL state into a vector of doubles
+ * @param s1 OMPL state
+ * @param dimension Size of the state (e.g., number of joints)
+ * @return
+ */
+Eigen::Map<Eigen::VectorXd> fromRealVectorStateSpace(const ompl::base::State* s1, unsigned dimension);
+
+/**
+ * @brief Converts
+ * @param path
+ * @return
+ */
+tesseract_common::TrajArray fromRealVectorStateSpace(const ompl::geometric::PathGeometric& path);
+
+// long assignTrajectory(tesseract_planning::CompositeInstruction& output,
+//                       boost::uuids::uuid start_uuid,
+//                       boost::uuids::uuid end_uuid,
+//                       long start_index,
+//                       const std::vector<std::string>& joint_names,
+//                       const tesseract_common::TrajArray& traj,
+//                       const bool format_result_as_input);
+
 }  // namespace tesseract_planning
 
 #endif  // TESSERACT_MOTION_PLANNERS_OMPL_UTILS_H
diff --git a/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp b/tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp
@@ -31,6 +31,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
 TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
 #include <tesseract_motion_planners/ompl/continuous_motion_validator.h>
+#include <tesseract_motion_planners/ompl/utils.h>
 
 #include <tesseract_environment/environment.h>
 #include <tesseract_kinematics/core/joint_group.h>
@@ -44,13 +45,11 @@ ContinuousMotionValidator::ContinuousMotionValidator(
     ompl::base::StateValidityCheckerPtr state_validator,
     const tesseract_environment::Environment& env,
     std::shared_ptr<const tesseract_kinematics::JointGroup> manip,
-    const tesseract_collision::CollisionCheckConfig& collision_check_config,
-    OMPLStateExtractor extractor)
+    const tesseract_collision::CollisionCheckConfig& collision_check_config)
   : MotionValidator(space_info)
   , state_validator_(std::move(state_validator))
   , manip_(std::move(manip))
   , continuous_contact_manager_(env.getContinuousContactManager())
-  , extractor_(std::move(extractor))
 {
   links_ = manip_->getActiveLinkNames();
 
@@ -139,8 +138,8 @@ bool ContinuousMotionValidator::continuousCollisionCheck(const ompl::base::State
   }
   mutex_.unlock();
 
-  Eigen::Map<Eigen::VectorXd> start_joints = extractor_(s1);
-  Eigen::Map<Eigen::VectorXd> finish_joints = extractor_(s2);
+  Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension());
+  Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension());
 
   tesseract_common::TransformMap state0 = manip_->calcFwdKin(start_joints);
   tesseract_common::TransformMap state1 = manip_->calcFwdKin(finish_joints);