Draft: Revise OMPL planner and profile to support different types of state spaces #555

marip8 · 2024-12-23T17:39:45Z

The current implementation of the OMPL planner effectively limits its usage to only the real vector state space (RVSS) because of the OMPLStateExtractor class and because of the way the planner converts OMPL paths into composite instructions. It would be beneficial to support other types of state spaces (specifically SE(3) space and the SE(3) space with constraint projection), and this is possible with some changes to the new profile interface:

Removal of the `OMPLStateExtractor` concept

All of the existing components that utilize the state extractor are effectively only valid for use with RVSS. Removing the state extractor meants:

Adding an RVSS-specific utility for converting an OMPL state to an Eigen::VectorXd
Adding an RVSS-specific utility for converting an OMPL path to a tesseract_common::TrajArray
Replacing calls to the state extractor with calls to these RVSS-specific utilities
(TODO) renaming components (e.g., the state and motion validators) to indicate they are RVSS-specific components

Conversion of OMPL paths into composite instructions

The motion planner itself currently attempts to convert OMPL paths to composite instructions by using the state extractor to turn the path into a joint trajectory, then converting the joint trajectory into a composite instruction of state or joint waypoints. To prevent this from limiting users to only the RVSS, an OMPL profile should provide a function to convert an OMPL path into a composite instruction (given an environment and manipulator info). This PR adds the following method to the base profile:

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;

To-do

The OMPL planner currently "assigns" the resulting OMPL paths into the output composite instruction via a process that also makes the assumption that the output should be state or joint waypoints (which would not be true for SE(3) state spaces that would utilize Cartesian waypoints). If the OMPL profile becomes responsible for converting an OMPL path into a composite instruction, then this strategy won't work.

The implementation in this PR simply concatenates all of the per-segment result composite instructions into a single composite instruction. I expect that this is probably not quite sufficient, so we'll need to think about how to handle this more effectively.

marip8 · 2024-12-23T17:42:06Z

tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_profile.h

+  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;


To mirror what is currently being done in assignTrajectory, do we also need to pass in the UUIDs of the start and end waypoints or those waypoints themselves?

I like this change. I was using the minimum needed but you could also pass the waypoint or instruction.

marip8 · 2024-12-23T17:44:50Z

tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

-bool checkStartState(const ompl::base::ProblemDefinitionPtr& prob_def,
-                     const Eigen::Ref<const Eigen::VectorXd>& state,
-                     const OMPLStateExtractor& extractor)
-{
-  if (!(prob_def->getStartStateCount() >= 1))
-    return false;
-
-  for (unsigned i = 0; i < prob_def->getStartStateCount(); ++i)
-    if (extractor(prob_def->getStartState(i)).isApprox(state, 1e-5))
-      return true;
-
-  return false;
-}
-
-bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
-                    const Eigen::Ref<const Eigen::VectorXd>& state,
-                    const OMPLStateExtractor& extractor)
-{
-  ompl::base::GoalPtr goal = prob_def->getGoal();
-  if (goal->getType() == ompl::base::GoalType::GOAL_STATE)
-    return extractor(prob_def->getGoal()->as<ompl::base::GoalState>()->getState()).isApprox(state, 1e-5);
-
-  if (goal->getType() == ompl::base::GoalType::GOAL_STATES)
-  {
-    auto* goal_states = prob_def->getGoal()->as<ompl::base::GoalStates>();
-    for (unsigned i = 0; i < goal_states->getStateCount(); ++i)
-      if (extractor(goal_states->getState(i)).isApprox(state, 1e-5))
-        return true;
-  }
-  else
-  {
-    CONSOLE_BRIDGE_logWarn("checkGoalStates: Unsupported Goal Type!");
-    return true;
-  }
-  return false;
-}


Note, these functions were removed because they are RVSS-specific and don't really add any value at run-time IMO. If we're concerned that OMPL is not setting valid start and end states, we should either make a unit test to prove that this is not the case, or bake these checks in as real checks (not asserts) into the RVSS profile

I think these should only be removed if a unit test is written in its place. I typically add assets for two reasons, the first if something is not easy to write a unit test for and the second is I have not had time to write a unit test yet. They still have value in CI.

marip8 · 2024-12-23T17:45:37Z

tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

@@ -293,14 +192,9 @@ PlannerResponse OMPLMotionPlanner::solve(const PlannerRequest& request) const
    cached_simple_setups.reserve(move_instructions.size());

  // Transform plan instructions into ompl problem
-  unsigned num_output_states = 1;
-  long start_index{ 0 };
-  std::size_t segment{ 1 };


Note: segment is the same as the iterator variable i, so no need to make something separate that needs to be incremented separately

It is not the same as 'i'. The 'i' increments for every move instruction in a segment. A segment is incremented when a constrained waypoint is reached but can have any number of unconstrained waypoints in-between.

marip8 · 2024-12-23T17:46:19Z

tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

    }

    // Parallel Plan problem
-    auto status = parallelPlan(*simple_setup, *solver_config, num_output_states);
+    auto status = parallelPlan(*simple_setup, *solver_config, 2);


Note: num_output_states is initialized to 1, incremented by 1 and reset to 1 in every loop, making its value 2 every time it gets to this point. I don't think we need a separate variable to keep track of this

marip8 · 2024-12-23T17:47:16Z

tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp

+    // Concatenate into results
+    std::copy(response.results.end(), output.begin(), output.end());


This is where I need a little help understanding what exactly assignTrajectory was doing so we can replicate that behavior correctly

The intent to is to preserve as much of the structure between two constrained waypoints. There can be any number of interpolation waypoints that should be preserved.

marip8 · 2024-12-23T17:49:50Z

tesseract_motion_planners/ompl/src/utils.cpp

-#ifndef OMPL_LESS_1_4_0
-Eigen::Map<Eigen::VectorXd> ConstrainedStateSpaceExtractor(const ompl::base::State* s1)
-{
-  assert(dynamic_cast<const ompl::base::ProjectedStateSpace::StateType*>(s1) != nullptr);
-  const Eigen::Map<Eigen::VectorXd>& s = *(s1->template as<ompl::base::ProjectedStateSpace::StateType>());
-  return s;
-}
-#endif


Note: constrained RVSS should eventually become it's own profile for those who want to use it. When we get to the point of implementing it, I think we should be able to re-use most of the RVSS components (e.g., state and motion validators, etc.) without having to write too much additional code

Levi-Armstrong · 2024-12-23T23:50:00Z

I think it is important that all motion planners preserve as much of the original program state. The program may include other instructions that need to be preserved. If it needs to insert more interpolation instructions that is fine but it should not ever reduce.

Levi-Armstrong · 2024-12-24T20:42:14Z

tesseract_motion_planners/ompl/src/continuous_motion_validator.cpp

+  Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension());
+  Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension());


How would this work with a constrained state space? The intent behind the extractor was to avoid multiple implementations of this class.

Check out 74d7dda, 8cbdbce, 27f376e, and 792626f, which add a constrained planner profile that inherits from the existing RVSS plan profile with minimal additional code. It at least compiles on my machine, but I haven't tested it yet.

I think we still need the concept of an OMPLStateExtractor, but it only applies to real vector state spaces, not all state spaces in general. I effectively added this back to the RVSS state and motion validators and plan profile

Levi-Armstrong · 2024-12-24T20:46:42Z

Overall, I like the direction this heading.

…pendently of the simple setup

marip8 · 2024-12-26T18:38:17Z

It would be beneficial to support other types of state spaces (specifically SE(3) space and the SE(3) space with constraint projection), and this is possible with some changes to the new profile interface:

Note: per the constrained planning documentation, the constrained state space requires that the state be contiguous in memory, which means that we cannot directly use the OMPL SE(3) state space with constraints. We would have to make our own real vector SE(3) space (e.g., position and angle-axis) to support constrained planning in Cartesian space.

Removed OMPL state extractor concept

4878e05

marip8 commented Dec 23, 2024

View reviewed changes

Levi-Armstrong reviewed Dec 24, 2024

View reviewed changes

marip8 added 6 commits December 26, 2024 11:24

Moved RVSS components to separate subdirectory

4684840

Added state converter function back to RVSS motion and state validators

74d7dda

Remove unused include

e2a756d

Added function to OMPL profile for generating state space object inde…

8cbdbce

…pendently of the simple setup

Added state converter object in RVSS profile

27f376e

Added a constrained RVSS plan profile

792626f

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Draft: Revise OMPL planner and profile to support different types of state spaces #555

Draft: Revise OMPL planner and profile to support different types of state spaces #555

marip8 commented Dec 23, 2024 •

edited

Loading

marip8 Dec 23, 2024

Levi-Armstrong Dec 24, 2024

marip8 Dec 23, 2024

Levi-Armstrong Dec 24, 2024 •

edited

Loading

marip8 Dec 23, 2024 •

edited

Loading

Levi-Armstrong Dec 24, 2024

marip8 Dec 23, 2024 •

edited

Loading

marip8 Dec 23, 2024

Levi-Armstrong Dec 24, 2024

marip8 Dec 23, 2024

Levi-Armstrong commented Dec 23, 2024

Levi-Armstrong Dec 24, 2024 •

edited

Loading

marip8 Dec 26, 2024 •

edited

Loading

Levi-Armstrong commented Dec 24, 2024

marip8 commented Dec 26, 2024

		// Concatenate into results
		std::copy(response.results.end(), output.begin(), output.end());

		Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension());
		Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension());

Draft: Revise OMPL planner and profile to support different types of state spaces #555

Are you sure you want to change the base?

Draft: Revise OMPL planner and profile to support different types of state spaces #555

Conversation

marip8 commented Dec 23, 2024 • edited Loading

Removal of the OMPLStateExtractor concept

Conversion of OMPL paths into composite instructions

To-do

marip8 Dec 23, 2024

Choose a reason for hiding this comment

Levi-Armstrong Dec 24, 2024

Choose a reason for hiding this comment

marip8 Dec 23, 2024

Choose a reason for hiding this comment

Levi-Armstrong Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

marip8 Dec 23, 2024 • edited Loading

Choose a reason for hiding this comment

Levi-Armstrong Dec 24, 2024

Choose a reason for hiding this comment

marip8 Dec 23, 2024 • edited Loading

Choose a reason for hiding this comment

marip8 Dec 23, 2024

Choose a reason for hiding this comment

Levi-Armstrong Dec 24, 2024

Choose a reason for hiding this comment

marip8 Dec 23, 2024

Choose a reason for hiding this comment

Levi-Armstrong commented Dec 23, 2024

Levi-Armstrong Dec 24, 2024 • edited Loading

Choose a reason for hiding this comment

marip8 Dec 26, 2024 • edited Loading

Choose a reason for hiding this comment

Levi-Armstrong commented Dec 24, 2024

marip8 commented Dec 26, 2024

marip8 commented Dec 23, 2024 •

edited

Loading

Removal of the `OMPLStateExtractor` concept

Levi-Armstrong Dec 24, 2024 •

edited

Loading

marip8 Dec 23, 2024 •

edited

Loading

marip8 Dec 23, 2024 •

edited

Loading

Levi-Armstrong Dec 24, 2024 •

edited

Loading

marip8 Dec 26, 2024 •

edited

Loading