[WIP] Move Relative with Pilz does not reject trajectory with collision

[captain-yoshi]

It is my understanding that the MoveRelative stage min/max distance cannot be used with Pilz Industrial Motion Planner in it's default behaviour:

The min/max distance option relies on an early stop on collisions to work. Unlike MoveIt Cartesian Interpolator, the Pilz does not check for collision by default as pointed out by moveit/moveit/issues/2942. This results in the trajectory still containing waypoints that are in collision.

This could lead to potential collisions. Should the default behaviour of Pilz align more with the "early stop on collisions" ? If not, we should at least document this behavior in MTC. I don't think there is fullproof solution for this usecase...

I added 2 tests which demonstrates the issue and valid behaviour:

• cartesianCollisionMinMaxDistance: Set's a collision at the end of the trajectory. The plan will succeed because the minimum distance is cleared. Early stop on collisions. This test should be added in this package.
• cartesianPilzCollisionMinMaxDistance: Plan should have NOT succeeded because waypoints are in collision. No early stop on collisions (Default). Just for demonstration.

---

I will probably submit a PR to be able to modify the default behaviour for Pilz in MoveIt. Would these 3 modes make sense ?

1. Don't stop on collisions (default current behaviour)
2. Early stop on self-collisions
3. Early stop on collisions

[captain-yoshi]

Even if Pilz would stop early on collisions, it would not work because the time parameterization is done on the hole trajectory in the PipelinePlanner, unlike the CartesianPath solver...

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 59.02%. Comparing base (5720b83) to head (87af389).
Report is 15 commits behind head on master.

❗ Current head 87af389 differs from pull request most recent head 4769925. Consider uploading reports for the commit 4769925 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #538      +/-   ##
==========================================
- Coverage   59.09%   59.02%   -0.07%     
==========================================
  Files          90       92       +2     
  Lines        8504     8623     +119     
==========================================
+ Hits         5025     5089      +64     
- Misses       3479     3534      +55     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[rhaschke]

This is a nice new unit test illustrating the issues with the Pilz planner.

In order to resolve the issue, we need to truncate the trajectory returned by the (Pilz) pipeline planner to the valid part.
moveit/moveit#3577 suggests to do that in MoveIt's planning pipeline.
@v4hn: I would like your opinion, on which way to go.

[rhaschke]

Even if Pilz would stop early on collisions, it would not work because the time parameterization is done on the hole trajectory in the PipelinePlanner, unlike the CartesianPath solver...

Obviously, truncation of the trajectory needs to happen before time parameterization.
Having thought more about the issue, I am convinced that the issue should be tackled in the Pilz planner: it shouldn't generate invalid trajectory waypoints in first place, but stop on the last valid waypoint. Of course, this requires (additional) validity checks. On the other hand, we skip some IK computations in case the trajectories becomes invalid...

[v4hn]

Thanks for documenting the issue @captain-yoshi !

I am convinced that the issue should be tackled in the Pilz planner: it shouldn't generate invalid trajectory waypoints in first place, but stop on the last valid waypoint.

I fully agree.

Of course, this requires (additional) validity checks.

Well, the pilz planner does perform any validity checks as far as I know, so they should be added either way.
We could keep them optional (with the default ON).

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 54.16667% with 22 lines in your changes are missing coverage. Please review.

Project coverage is 56.07%. Comparing base (6b0f2c8) to head (7c7b2dc).
Report is 14 commits behind head on master.

Files	Patch %	Lines
core/test/test_move_relative.cpp	54.17%	22 Missing ⚠️

❗ Your organization needs to install the Codecov GitHub app to enable full functionality.

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #538      +/-   ##
==========================================
- Coverage   58.82%   56.07%   -2.75%     
==========================================
  Files          91      130      +39     
  Lines        8623    10771    +2148     
  Branches        0      945     +945     
==========================================
+ Hits         5072     6039     +967     
- Misses       3551     4686    +1135     
- Partials        0       46      +46     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.