Better OMPL error logging and fail on start/goal in collision

[marip8]

First commit from #545 that fixes issues for bad start/goal states in the OMPL planner

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 78.92%. Comparing base (40c14ab) to head (41fc0e8).
Report is 1 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #552      +/-   ##
==========================================
+ Coverage   78.91%   78.92%   +0.01%     
==========================================
  Files         253      253              
  Lines       14548    14560      +12     
==========================================
+ Hits        11480    11492      +12     
  Misses       3068     3068              

see 2 files with indirect coverage changes

[Levi-Armstrong]

Lets hold off merging this until the update ompl profile gets merged.

[marip8]

I think this should actually be merged before the OMPL profile refactor. This commit fixes a bug in the OMPL planner, and I don't think the profile refactor actually ends up touching this part of the planner.

Edit: I didn't look closely enough at this commit the first time, and I see now that this code lives in the profile (I thought it was in the planner itself). So agreed, this should probably just be addressed in #540

[rjoomen]

Should the responsibility for checking this lie with the planner, or should it lie with the user, who can, for example, add a ContactCheck in the pipeline that only checks the start and goal states (functionality to be added, as currently you can only check either start or goal, but not both)? Because that was what I am considering doing before feeding an initial trajectory to Trajopt, which does not have the checks this PR implements.

[marip8]

Closing per #552 (comment)

[marip8]

Should the responsibility for checking this lie with the planner, or should it lie with the user, who can, for example, add a ContactCheck in the pipeline that only checks the start and goal states (functionality to be added, as currently you can only check either start or goal, but not both)? Because that was what I am considering doing before feeding an initial trajectory to Trajopt, which does not have the checks this PR implements.

I haven't thought about this thoroughly, but one argument for keeping some of this capability in the planners themselves is to maintain the ability to use them independently of the task composer infrastructure. I'm not sure how strong of an argument this is, but I'll throw it out there.

Something to note about the checking the start and goal states ahead of time with a discrete contact check is that not all planners require the start/goal states to be collision-free. For example, both OMPL and Trajopt can try to change the start/goal states to make them collision free if those waypoints have non-zero tolerances. A simple contact check ahead of this planner may prevent these planners from running even though they could succeed because that simple contact check probably doesn't understand bounds on the start/end states.

Also, because Trajopt and OMPL understand the tolerances of a waypoint, they should do a contact check if they identify start/end waypoints with no tolerance because there would be no chance of success if a waypoint with no tolerance is in collision.

[rjoomen]

As you say, I'm not really sure if your first point is that relevant.

Regarding the rest, I agree with your points, I hadn't thought about the toleranced waypoints. This would imply we'd need to add this check to Trajopt as well. I think this will also improve the user friendliness, as start and goal states in collision making the planning could be confusing.

@Levi-Armstrong, what do you think?

[marrts]

In general, I am a fan of moving functionality out of planners that can be extracted and keeping planners as simple and clear as possible. I know this adds complications to taskflows, but it allows the user to have more control to optimize things for their application. I think it would likely make sense to have several default taskflows available that handle things like simple freespace motions that users could simply plug in to their application though.

I was unaware OMPL was supposed to be capable of moving start/goal states out of collision. If that is the case then we shouldn't fail if the start/goal is in collision. I've just had the experience many times where I'm using an RRT planner and my start/goal is in collision, but it takes the full timeout to fail while printing the same error over and over to the terminal and this commit was aimed to fix that problem.

The issue is that I don't know what you do for planners where that tolerance is not configured or the waypoints can't be adjusted. I guess ideally the RRT planners should still return right away with a failure if the start or goal can't be fixed, but I don't think they are set up like that currently.