[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 ros-planning/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[bot]]

Codecov Report

All modified and coverable lines are covered by tests :white_check_mark:

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

:exclamation: 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

```diff @@ 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 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

[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 :warning:

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

Additional details and impacted files

```diff @@ 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 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.