WIP: WMD 2021 Cartesian Planning

[bostoncleek]

This PR instantiates a BaseConstraint-derived class called, say, ToolPathConstraint from a CartesianTrajectory.msg that represents a tool path. The class finely interpolates points along the tool path and inserts these points in a nearest neighbor data structure. For any random joint state, we can use FK to compute end effector pose and the nearest point on the tool path.

Checklist

• [ ] Required by CI: Code is auto formatted using clang-format
• [ ] Extend the tutorials / documentation reference
• [ ] 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

[mamoll]

Thanks for taking this on, @bostoncleek!

[codecov[bot]]

Codecov Report

Merging #377 (a26d13d) into main (e97122c) will decrease coverage by 0.92%. The diff coverage is 0.00%.

:exclamation: Current head a26d13d differs from pull request most recent head 0cb84e7. Consider uploading reports for the commit 0cb84e7 to get more accurate results

@@            Coverage Diff             @@
##             main     #377      +/-   ##
==========================================
- Coverage   53.36%   52.45%   -0.91%     
==========================================
  Files         210      213       +3     
  Lines       18814    19136     +322     
==========================================
- Hits        10038    10035       -3     
- Misses       8776     9101     +325     

Impacted Files	Coverage Δ
...lude/moveit/ompl_interface/detail/GreedyKCenters.h	0.00% <0.00%> (ø)
...de/moveit/ompl_interface/detail/NearestNeighbors.h	0.00% <0.00%> (ø)
...oveit/ompl_interface/detail/NearestNeighborsGNAT.h	0.00% <0.00%> (ø)
...de/moveit/ompl_interface/detail/ompl_constraints.h	100.00% <ø> (ø)
...mpl/ompl_interface/src/detail/ompl_constraints.cpp	53.69% <ø> (-23.58%)	:arrow_down:
.../ompl_interface/src/detail/constrained_sampler.cpp	42.86% <0.00%> (-17.14%)	:arrow_down:
... and 2 more

---

Continue to review full report at Codecov.

Legend - Click here to learn more Δ = absolute <relative> (impact), ø = not affected, ? = missing data Powered by Codecov. Last update e97122c...0cb84e7. Read the comment docs.

[bostoncleek]

@mamoll Thank you this sort of feedback is very beneficial.

[gautz]

If I understand well, your goal is to use the constrained planner to plan e.g. a spraying path? The constraints would be defined by discrete trajectory points? Do you plan to use the Cartesian Trajectory proposed interface? https://fzi-robot-interface-proposal.readthedocs.io/en/latest/proposal/index.html#tldr-proposed-interface

I am familiar with ompl_constraints.cpp but not with the classical cartesian planning. I never used the CartesianTrajectory.msg, thus it would be nice if you could explain the idea here.. Where would your input come from? A 3D path generated on a CAD file in a CAM system? In the form of linear, circular and spline segment?

I am working on possibly a similar problem, just want to avoid dual work.

[bostoncleek]

@gautz The idea is that the end effector would follow a path (spraying, welding, drawing, etc). The CartesianTrajectory.msg should contain a list of waypoints for the end effector to follow.

As of now the user must specify the waypoints. But as you can imagine the waypoints could come from a CAM system or another path generator. You could even run canny edge detection on an imagine and convert the pixels representing contours to Cartesian space. The robot would then ideally be able to draw the picture.

[gautz]

@bostoncleek any plans to implement a jacobian() with this generic Cartesian path approach? Or should the function() be sufficient for performance?

[bostoncleek]

@bostoncleek any plans to implement a jacobian() with this generic Cartesian path approach? Or should the function() be sufficient for performance?

Ompl can compose the derivative numerically. Of course the jacobian will allows provide an increase in performance.

[mergify[bot]]

This pull request is in conflict. Could you fix it @bostoncleek?

[mergify[bot]]

This pull request is in conflict. Could you fix it @bostoncleek?