Feature/port orientation constraints

[mipark56]

Description

Please explain the changes you made, including a reference to the related issue if applicable

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

[mipark56]

@vatanaksoytezer
Hi Vatan, tried to add you as a reviewer but couldn't seem to so tagging you here. Feel free to add yourself. If this PR was intended to be local within my fork (i.e. in this feature branch back to https://github.com/mipark56/moveit2), let me know and can close this and make one there.

Here's a quick summary of what I changed and build / test to see that the changes worked (I believe).

1. From this ticket OMPL orientation constraints #348 , ported this PR, as outined / requested: https://github.com/ros-planning/moveit/pull/2402/files
2. From that diff, all orientation constraint parameterization changes, I simply copied over.
3. However, a few changes needed to be made, specific to ROS2, for example a.) Updated logging calls, e.g., ROS_ERROR to RCLCPP_ERROR, and b.) Updated messages namespace, e.g. moveit_msgs::OrientationConstraint to moveit_msgs::msg::OrientationConstraint
4. After installing ROS 2 Foxy Fitzroy (side note: Galactic I could install but the rosdeps failed, preventing colcon build--if you're interested I can provide the name of the packages that halted the flow) from this guide: https://moveit.ros.org/install-moveit2/source/, I built the MoveIt2 suite with colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release
5. The colcon build seemed pretty computationally intensive--my gaming laptop fan was spinning full blast!
6. For the pre-formatting checks https://moveit.ros.org//documentation/contributing/code/ I found some trailing whitespace that I used VSCode settings to resolve. Screenshot of the pre-commit checks below.
7. Lastly, I ran the demo setup https://github.com/ros-planning/moveit2/tree/main/moveit_demo_nodes/run_moveit_cpp to see the demo behavior in the RViz UI. Screenshot on that below as well.

Thanks for reviewing this and if you've any follow up comments to things to look into, please comment here and I'll look into it.

[codecov]

Codecov Report

Merging #550 (feaad87) into main (50bf408) will increase coverage by 0.13%.
The diff coverage is 94.88%.

@@            Coverage Diff             @@
##             main     #550      +/-   ##
==========================================
+ Coverage   54.09%   54.22%   +0.13%     
==========================================
  Files         190      190              
  Lines       20037    20063      +26     
==========================================
+ Hits        10838    10878      +40     
+ Misses       9199     9185      -14     

Impacted Files	Coverage Δ
...raint_samplers/src/default_constraint_samplers.cpp	72.71% <88.89%> (+0.20%)	⬆️
...kinematic_constraints/src/kinematic_constraint.cpp	74.68% <96.43%> (+0.34%)	⬆️
...oveit/kinematic_constraints/kinematic_constraint.h	92.31% <100.00%> (+0.25%)	⬆️
moveit_core/robot_model/src/robot_model.cpp	73.19% <0.00%> (+0.26%)	⬆️
...bot_state/include/moveit/robot_state/robot_state.h	87.37% <0.00%> (+0.36%)	⬆️
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.42% <0.00%> (+0.51%)	⬆️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	75.85% <0.00%> (+1.14%)	⬆️
moveit_core/utils/src/robot_model_test_utils.cpp	76.78% <0.00%> (+3.32%)	⬆️

---

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 50bf408...feaad87. Read the comment docs.

[vatanaksoytezer]

Hey @mipark56 thanks for the PR! Great work! Sorry for the delayed review. Overall the port looks great, but to comment / clarify on a few points:

If this PR was intended to be local within my fork (i.e. in this feature branch back to https://github.com/mipark56/moveit2), let me know and can close this and make one there.

This PR is intended for here. No worries on that.

• From that diff, all orientation constraint parameterization changes, I simply copied over.

Can you squash this PR into 2 commits, so your changes and differences from the original PR will be more understandable:

1. Commit from the original PR (New orientation constraint parameterization moveit#2402)
2. ROS2 changes

After installing ROS 2 Foxy Fitzroy (side note: Galactic I could install but the rosdeps failed, preventing colcon build--if you're interested I can provide the name of the packages that halted the flow) from this guide: https://moveit.ros.org/install-moveit2/source/, I built the MoveIt2 suite with colcon build --event-handlers desktop_notification- status- --cmake-args -DCMAKE_BUILD_TYPE=Release

Galactic should work right now, we were waiting for the latest Galactic sync that would enable MoveIt 2 to be built without additional repos (see #558 and moveit/moveit.ros.org#617 for more context.) So that was missing documentation, on galactic compilation, but the extra repos are not needed now. Your code is builded against Rolling, Galactic and Foxy anyway by CI, so also no worries on this.

The colcon build seemed pretty computationally intensive--my gaming laptop fan was spinning full blast!

It is, we recommend using ccache for consecutive builds to speed up the process (https://moveit.ros.org/install/source/). That is undocumented for ROS2 but should work the same way with ROS.

• For the pre-formatting checks https://moveit.ros.org//documentation/contributing/code/ I found some trailing whitespace that I used VSCode settings to resolve.

pre-commit will fix and format things for you, you should not need any extra steps or clean things manually. So should pre-commit run -a or git commit -m "...." after pre-commit hooks are installed should automatically fix your formatting.

I appreciate your work and it looks pretty clean. I would be happy if you can squash this into 2 commits as I've mentioned above, otherwise looks good to me!

[mipark56]

Hi @vatanaksoytezer, thanks for the review! I updated the PR to combine the changes into the 2 commits you suggested. I agree this makes the port cleaner while also pulling in the changes directly from the original PR links this change directly to that one within the git log (which is a good thing). Let me know if you see anything else here that might need my attention.

[vatanaksoytezer]

Thanks for the commit cleanup, great work @mipark56. I think this can be merged after a maintainer approval.

[gautz]

@henningkayser The name of this PR is misleading. This is currently only the port of the new orientation parametrization which is already merged in MoveIt1. This is a prerequisite for adding Orientation constraints support to OMPL constrained planning. But there is currently no open PR on this. I could work on a PR in MoveIt1 that you could then port to MoveIt2. But for this I need this PR to be merged: moveit/moveit#2273

@mipark56 @vatanaksoytezer do you plan to work on OMPL orientation constrained planning after this one is merged?

[vatanaksoytezer]

@gautz I don't think we have a clear plan to work on orientation constrain right now, I would appreciate if you can work on porting that and I would be happy to help getting it reviewed