Draft: Revise OMPL planner and profile to support different types of state spaces #555
Conversation
| 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; |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
I like this change. I was using the minimum needed but you could also pass the waypoint or instruction.
| 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; | ||
| } |
There was a problem hiding this comment.
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
There was a problem hiding this comment.
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.
| @@ -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 }; | |||
There was a problem hiding this comment.
Note: segment is the same as the iterator variable i, so no need to make something separate that needs to be incremented separately
There was a problem hiding this comment.
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.
| } | ||
|
|
||
| // Parallel Plan problem | ||
| auto status = parallelPlan(*simple_setup, *solver_config, num_output_states); | ||
| auto status = parallelPlan(*simple_setup, *solver_config, 2); |
There was a problem hiding this comment.
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
| // Concatenate into results | ||
| std::copy(response.results.end(), output.begin(), output.end()); |
There was a problem hiding this comment.
This is where I need a little help understanding what exactly assignTrajectory was doing so we can replicate that behavior correctly
There was a problem hiding this comment.
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.
| #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 |
There was a problem hiding this comment.
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
|
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. |
| Eigen::Map<Eigen::VectorXd> start_joints = fromRealVectorStateSpace(s1, si_->getStateDimension()); | ||
| Eigen::Map<Eigen::VectorXd> finish_joints = fromRealVectorStateSpace(s2, si_->getStateDimension()); |
There was a problem hiding this comment.
How would this work with a constrained state space? The intent behind the extractor was to avoid multiple implementations of this class.
|
Overall, I like the direction this heading. |
The current implementation of the OMPL planner effectively limits its usage to only the real vector state space (RVSS) because of the
OMPLStateExtractorclass 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
OMPLStateExtractorconceptAll of the existing components that utilize the state extractor are effectively only valid for use with RVSS. Removing the state extractor meants:
Eigen::VectorXdtesseract_common::TrajArrayConversion 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:
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.