OMPL Constrained State Space Planning

[JeroenDM]

Description

In the context of a Google Summer of Code project, I would like to add the constrained planning capabilities of OMPL to the existing interface. Previously, I split up the code in several pull requests. This was not entirely clear and impractical for testing. Hence this new approach, a single pull request that can be reviewed on a per commit basis.

Edit this pull request is quite large, but can be made smaller by first merging these tests separately:

• Add test to ompl interface for StateValidityChecker Add test to ompl interface for StateValidityChecker #2247 (already approved)
• WIP: add test for PlanningContextManager in ompl interface Add test for PlanningContextManager in ompl interface #2248 (needs some discussion)

Why

It is one of the projects on the future projects page.

It can solve the long-standing joint flip bug when planning with path constraints. It is especially useful in cases where the constrained region has zero volume in Cartesian space and the enforce_joint_model_state_space (rejection sampling) does not work. For example planning with the end-effector constrained to a plane:

Or a line:

The current default planner that uses the PoseModelStateSpace can solve these problems, but the solution will often contain large jumps in joint space because it plans in Cartesian space. (Especially when adding obstacles in the scene.) This new planner plans in joint space. The example below illustrates the difference.

With the default PoseModelStateSpace:

With OMPL's ConstrainedStateSpace:

Finally (and maybe most importantly) once this new planning approach is integrated, the possibilities for adding new constraints are endless. It would be really nice if we could pass arbitrary user-defined constraints to OMPL through some C++ interface in the future.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials.
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[JeroenDM]

The commits

This branch of a cleaned up version of my original development attempts. (See the v5 in the name :), it took me a while to get it right.)

Add new state space 7b84591

This commit adds an extra type of state space, besides the current JointModelStateSpace and PoseModelStateSpace. The state space is called ConstrainedPlanningStateSpace, as in "use this state space for constrained planning". Not to be confused with an ompl::Base::ConstrainedStateSpace, which is constrained in the sense that it generates states that satisfy the constraints.

ConstrainedPlanningStateSpace overrides the virtual methods to copy states from OMPL to MoveIt and vice-versa. It is necessary because in many places the state space is used to determine how to copy states. (Like this: spec_.state_space_->copyToRobotState(...).)

Add new state validity checker for ompl constrained planning a620442

The state validity checker operates on raw ompl::base::State* pointers. When planning, all states are of type ompl::base::ConstrainedStateSpace::StateType. This state type wraps around a "normal state". Therefore the state validity checker must implement proper casting operations to access the underling state, before the validity can be checked.

Path constraints are still checked in this new state validity checker as not all states samled by the constrained state space satisfy the constraints, unfortunately. See this comment for more details.

Add ompl position constraints model 146e5a2

To integrate OMPL's constrained state space, we need a model of the constraints that inherits from ompl::base::Constraint. In general, this can be any equality constraint. However, many interesting constraints are not equality constraints (==), but inequality constraints (< >). This pull request provides a base class BaseConstraint and a class Bounds that together model bounds on a scalar variable as an equality constraint.

A specialization of the position constraints, called EqualityPositionConstraints, does not use the bounds but models small dimensions of the constraint region as equality constraints directly. This performs much better than using small bounds. The dimensions that have a large tolerance are ignored in this class and rejection-sampled by the StateValidityChecker.

This pull request does not include orientation constraints yet. There are many unresolved issues with them. See the project issue for the full history.

Integrate new constrained planner with existing interface 26fffab
Integrate the code from the previous commits in the current interface so we can actually use it.

Add test for planning context manager

I split this test in a separate commit because it introduces a rostest dependency in this package. This test is an extended version of #2248.

Miscellaneous

• Add robot independent test util class: a generic utility class used in almost all the tests.
• Fix uninitialized memory (as reported by valgrind): initialize a robot state to its default values when creating it.
• Rename config parameter to enable constrained planning: I changed my mind in the end and made the configuration parameter a bit shorter: enforce_constrained_state_space.

As the pull request contains many changes, I think it will be useful to discuss each commits separately. Status list of reviewed commits:

• New state validity checker
• New state space
• Position constraint implementation
• Planning context manager test
• Miscellaneous
• Overall integration

[JeroenDM]

The kinetic build fails with:

/root/ros_ws/src/moveit/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/model_based_planning_context.h:48:63: fatal error: ompl/base/spaces/constraint/ConstrainedStateSpace.h: No such file or directory

I suspect this is because kinetic is using a version of OMPL from before this commit.

[felixvd]

I haven't even gotten halfway through skimming this. Just dropping some typo/style comments.

[felixvd]

?

[JeroenDM]

The whole factory method is a template code to support different constraints, but only a single option is actually returns a non-nullptr at the moment (position constraints). All the other cases spit out warnings or errors.

I guess I can remove all the unnecessary template code and only add it when needed, I never though about that.

[felixvd]

Oh. "just giving feedback"? I was having trouble parsing the sentence.

[JeroenDM]

I updated the comment and left the rest of the function as is. I feel it is pretty clear with the log messages and allows for easy integration of orientation constraints and a combination of position and orientation constraints in the future.

Now the comment reads:

// The factory method contains template code to support different constraints, but only position constraints are
// currently supported. The other options return a nullptr for now and should not be used.

[felixvd]

You can also link to a writeup with debugging instructions instead

[JeroenDM]

Good idea, it is a bit ugly to leave this in.

[JeroenDM]

I replaced the long comment with: // Developer tip: replace this with a dynamic_cast for debugging. As for debugging instructions, I think it will be useful if I write a good description of all these details in a central place somewhere. But I need to come up with a good format and a good place to put it. Maybe the MoveIt concepts page on the website?

[JeroenDM]

@felixvd Thank you for the review! I addressed most of the small comments and fixes. I left some comments unresolved as I added a change that does not necessarily reflect you're proposed change but still resolves it in a reasonable way I think.

[felixvd]

I hope I can get back to it soon, I didn't get even get through half!

[JeroenDM]

Yeah, it's a lot. I still think my original approach to split the pull request into smaller parts was easier to review. But it's difficult to keep everything organized that way.

Anyway, I would ignore the test code for now and focus on the functional code. (Although the tests can serve a bit as documentation.)

edit but to end on a more encouraging note: my code does not touch much of the existing functionality, so merging this should be very possible!

[lorepieri8]

Is this problem present also in Moveit2?

[JeroenDM]

@lorepieri8 Yes. As far as I know, MoveIt2 uses exactly the same OMPL interface code as MoveIt1.

[mamoll]

Looking good, Jeroen! I have made several small suggestions. The only one that might be a bit more work is the one related to refactoring the Bounds class.

[JeroenDM]

@mamoll thank you for the detailed review! I'll do my best to find some time in the coming weeks to move this forward.

[JeroenDM]

I created a public Trello board to follow progress on this. Hopefully, this planning helps me to spend the little time I have to work on this effectively.

[tylerjw]

I left some comments through this. I didn't test running it as I didn't have time and don't know where to begin to do that. Most of my comments are just C++ stuff that should be easy enough to address. Thank you again for doing this, I look forward to getting this merged and seeing it work.

[JeroenDM]

I just did the scariest rebase so far :) Now I can continue with addressing the review comments. The biggest chunk left is refactoring the bounds class. Also, there are some smaller things that I can clean up in the tests based on the feedback on my other pull requests that got merged recently.

[JeroenDM]

Update I am slowly working my way through the review comments and the Bounds refactor is in progress. (I'm working on it a little bit in the morning before work.) In any case, thank you already for the many in-depth review comments and patience, I will get there! :)

[JeroenDM]

@mamoll The bound refactor is done. There is still some less ideal code with many temporary objects:

void BaseConstraint::jacobian(const Eigen::Ref<const Eigen::VectorXd>& joint_values,
                              Eigen::Ref<Eigen::MatrixXd> out) const
{
  Eigen::VectorXd constraint_error = calcError(joint_values);
  Eigen::VectorXd constraint_derivative = bounds_.derivative(constraint_error);
  Eigen::MatrixXd robot_jacobian = calcErrorJacobian(joint_values);
  for (std::size_t i{ 0 }; i < bounds_.size(); ++i)
  {
    out.row(i) = constraint_derivative[i] * robot_jacobian.row(i);
  }
}

The loop could potentially be replaced with Eigen magic, but I'm not sure it's worth it performance or readability wise.

[gautz]

I looked a bit at the moveit2 PR, the core functionalities (/ompl_interface*) are just copied with some small modification. I feel like most of the changes are just related to the port to ROS2, changing namespaces. Also clang was run which makes the PR much bigger than this one. The only visible review/modifications/bugfixes are also mostly ROS2 specific, e.g. moveit/moveit2#347 (review).
So porting back a port does sound like a lots more overhead than maybe erasing small diffs from each core files.

I did not check what was done with the tests and the demos though. What is your opinion @JeroenDM , especially on the tests?

Would be nice to have thoughts of @bostoncleek on this also. Were there any bug fixes independent from the ROS2 migration?

[JeroenDM]

I'm sorry I have so little time and did not really add anything useful in the maintainer meeting.

But if @gautz analysis is correct

I looked a bit at the moveit2 PR, the core functionalities (/ompl_interface*) are just copied with some small modifications. I feel like most of the changes are just related to the port to ROS2, changing namespaces.

and given that backporting from moveit2 to moveit1 does not seem hassle-free anyway, I cannot see major disadvantages in merging this PR instead of someone trying to backport the moveit2 one.

That is, given that I fix this recent issue that should have been an innocent merge conflict fix. I will look into this tomorrow.
It turns out it is difficult to revert the specific changes that I did when resolving the merge conflict, except manually changing the lines.

[gautz]

@JeroenDM could you check the tests and the demos from the MoveIt2 PR to see if there are important things we should backport to this PR?

[gautz]

#2861

[fmessmer]

@JeroenDM @gautz
I wonder whether there is still effort going on in getting this merged into moveit(1) or whether focus is on extending this feature in moveit2 (ref moveit/moveit2#347)
it's a bit tricky from the outside to assess the current state and what's missing for the merge

[gautz]

The last idea from MoveIt maintainers was to backport the MoveIt2 PR. Which I did not agree (see previous message):

porting back a port does sound like a lots more overhead

So remaining ToDos may be:

• erasing small diffs (between this PR and moveit2#347) from each core files.
• Backport the tests, for this I asked @JeroenDM to have a look

But my general feeling was that no MoveIt1 maintainer has time to work on this.
Thus if I find time in the last 3 months of my current position, I'll try

extending this feature in moveit2 (ref ros-planning/moveit2#347)

[v4hn]

I believe there is nothing to see "from the inside" of the maintainer group that you can't see in the issues. It would be great to see this feature working in MoveIt out of the box and I agree that backporting the MoveIt2 PR is not a good idea as multiple people stated it was merged prematurely. I don't have a concrete use-case for this myself at the moment and it is too involved to look into it on the side. This seems to be the case for other maintainers as well. Any work towards merging and/or improving these features is very welcome!

[captain-yoshi]

@v4hn @gautz Would this project be a good candidate for GSoC22 ? Maybe as a Medium project. Don't really know how much work is left...

[v4hn]

this was a gsoc project already. thus I do not consider this as a viable candidate unless the scope is significant extended.

[knorth55]

we have this issue in our Fetch robot.
what is the status of this issue?

Screencast.2022-10-13.20.38.42.mp4