OMPL constrained planning by bostoncleek · Pull Request #347 · moveit/moveit2

bostoncleek · 2021-01-27T19:53:38Z

OMPL for constrained cartesian planning.

Co-authored-by: JeroenDM jeroendemaeyer@live.be

Description

This is a port from PR 2273 that provides position constraints.

A demo and integration test are included.
Add the follow lines to ompl_planning.yaml in panda_config under panda_arm:

enforce_constrained_state_space: true
projection_evaluator: joints(panda_joint1,panda_joint2)

Demo

colcon build --symlink-install  --packages-up-to run_ompl_constrained_planning --mixin rel-with-deb-info compile-commands ccache
source install/setup.bash
ros2 launch run_ompl_constrained_planning run_move_group.launch.py
ros2 launch run_ompl_constrained_planning run_ompl_demo.launch.py

Unit Tests

colcon build --symlink-install  --packages-up-to moveit_planners_ompl --mixin rel-with-deb-info compile-commands ccache
colcon test --event-handlers console_direct+ --packages-select moveit_planners_ompl

Integration Test

colcon build --symlink-install  --packages-up-to moveit_ros_planning_interface --mixin rel-with-deb-info compile-commands ccache
colcon test --event-handlers console_direct+ --packages-select moveit_ros_planning_interface

The demo outputs:

Box Constraint

Plane Constraint

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

bostoncleek · 2021-01-27T20:28:54Z

@JeroenDM I was not able to achieve the same performance as your PR. Sometimes the equality constraints cannot be satisfied. Do you have any advice?

JeroenDM · 2021-01-28T14:38:19Z

@bostoncleek I cannot think of something at first glance. I can look into it in more detail this weekend.

What do you mean specifically with the constraints not being satisfied? Does it work if you increase the planning time?
Can you maybe post some example output? I have not ros2 installation at hand to quickly test it...

A question I had about the animations: is there some postprocessing involved to generate the second "cleaner" path? How does it work? It seems to work really well :)

bostoncleek · 2021-01-28T20:26:07Z

@JeroenDM For example the 45deg equality contraint will produce the following error:

[INFO] [1611865336.899738653] [moveit_kinematic_constraints.kinematic_constraints]: Differences 5.32815e-05 -0.000455496 0.299535
[ERROR] [1611865336.899771385] [moveit.ros_planning.planning_pipeline]: Completed listing of explanations for invalid states.
[INFO] [1611865336.900082039] [moveit_move_group_default_capabilities.move_action_capability]: Motion plan was found but it seems to be invalid (possibly due to postprocessing). Not executing.

For the second path I just called move(). There must be postprocessing because the error message above indicates this. Maybe there is postprocessing under the hood that prevents the planning from succeeding.

codecov · 2021-01-28T21:20:57Z

Codecov Report

Merging #347 (1903116) into main (bbe859e) will increase coverage by 5.20%.
The diff coverage is 76.41%.

@@            Coverage Diff             @@
##             main     #347      +/-   ##
==========================================
+ Coverage   48.05%   53.25%   +5.20%     
==========================================
  Files         156      209      +53     
  Lines       14941    18856    +3915     
==========================================
+ Hits         7179    10040    +2861     
- Misses       7762     8816    +1054

Impacted Files	Coverage Δ
...veit/ompl_interface/model_based_planning_context.h	`81.82% <ø> (+81.82%)`	⬆️
...ion/joint_space/constrained_planning_state_space.h	`0.00% <0.00%> (ø)`
.../moveit_servo/include/moveit_servo/pose_tracking.h	`100.00% <ø> (ø)`
moveit_ros/moveit_servo/src/servo.cpp	`70.00% <0.00%> (+13.34%)`	⬆️
...lanners/ompl/ompl_interface/src/ompl_interface.cpp	`48.15% <61.54%> (+48.15%)`	⬆️
...mpl_interface/src/model_based_planning_context.cpp	`61.54% <62.50%> (+61.54%)`	⬆️
...space/constrained_planning_state_space_factory.cpp	`66.67% <66.67%> (ø)`
...pl_interface/src/detail/state_validity_checker.cpp	`54.47% <68.09%> (+54.47%)`	⬆️
...pl/ompl_interface/src/planning_context_manager.cpp	`68.14% <71.06%> (+68.14%)`	⬆️
...mpl/ompl_interface/src/detail/ompl_constraints.cpp	`77.28% <77.28%> (ø)`
... and 109 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 2499a72...781165d. Read the comment docs.

JeroenDM · 2021-01-29T12:33:57Z

@bostoncleek I've found a possible cause. The position error is -0.000455496 whereas the error tolerance of the primitive shape (box) in the constraint message is 0.0005. But I suspect this is the total width of the box used to evaluate the position constraints, the allowed deviation from the centre is 0.0005 / 2 = 0.00025 which is smaller than the -0.000455496 .

To be clear, this would be a bug in how I wrote the tutorials or implemented the constraints. We either should lower the default constraint tolerance or increase the EQUALITY_CONSTRAINT_THRESHOLD_. (This remains a hack to specify equality constraints and is still one of the messy parts of the current implementation.)

I'm not 100% sure yet, I quickly went through the geometric_shapes package but did not get to the bottom of this.

henningkayser

Looks flawless to me. Demo and tests work great and I really didn't have a lot to criticize besides the commented code and lack of tutorials. Otherwise, lgtm.

moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt

moveit_demo_nodes/run_ompl_constrained_planning/src/run_ompl_constrained_planning.cpp

moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h

...ompl_interface/src/parameterization/joint_space/constrained_planning_state_space_factory.cpp

moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp

moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp

bostoncleek · 2021-02-05T17:51:53Z

Looks flawless to me. Demo and tests work great and I really didn't have a lot to criticize besides the commented code and lack of tutorials. Otherwise, lgtm.

I added another tutorial and there are 2 extras people can run. Referring to the comments code, do you want doxygen style comments in the tutorial/test and/or more comments in the implementation?

tylerjw · 2021-02-05T18:18:28Z

I added another tutorial and there are 2 extras people can run.

I tested this. Thank you for adding this. I'll look more closely at the code early next week. This is a large one. 😄

tylerjw · 2021-02-09T17:33:18Z

The servo test failure should be fixed now if you rebase this on main. @bostoncleek

Uses OMPL for constrained cartesian planning. Co-authored-by: JeroenDM <jeroendemaeyer@live.be>

bostoncleek · 2021-02-11T16:27:08Z

The servo test failure should be fixed now if you rebase this on main. @bostoncleek

@tylerjw The PR has been rebased.

tylerjw

I read through these changes and tested them again. This will be a really awesome feature. I left comments on two small style issues. For the newline thing, you should be able to configure your editor to fix that whenever you save a file.

tylerjw · 2021-02-12T18:53:11Z

moveit_demo_nodes/run_ompl_constrained_planning/CMakeLists.txt

+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()


nit: newline at end of file

tylerjw · 2021-02-12T18:53:27Z

moveit_demo_nodes/run_ompl_constrained_planning/config/panda_controllers.yaml

+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7


tylerjw · 2021-02-12T18:53:40Z

moveit_demo_nodes/run_ompl_constrained_planning/config/start_positions.yaml

+        - -2.356
+        - 0.0
+        - 1.571
+        - 0.785


tylerjw · 2021-02-12T18:54:14Z

moveit_demo_nodes/run_ompl_constrained_planning/package.xml

+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>


tylerjw · 2021-02-12T18:55:17Z

moveit_demo_nodes/run_ompl_constrained_planning/src/run_ompl_constrained_planning.cpp

+
+  rclcpp::shutdown();
+  return 0;
+}


tylerjw · 2021-02-12T18:58:55Z

moveit_ros/planning_interface/test/config/panda_controllers.yaml

+      - panda_joint4
+      - panda_joint5
+      - panda_joint6
+      - panda_joint7


tylerjw · 2021-02-12T18:59:05Z

moveit_ros/planning_interface/test/config/start_positions.yaml

+        - -2.356
+        - 0.0
+        - 1.571
+        - 0.785


tylerjw · 2021-02-12T18:59:18Z

moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py

+         'robot_state_publisher': robot_state_publisher,
+         'fake_joint_driver_node': fake_joint_driver_node,
+         'mongodb_server_node': mongodb_server_node,
+         'ompl_constraint_test': ompl_constraint_test}


tylerjw · 2021-02-12T18:59:30Z

moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp

+  int ret = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return ret;
+}


tylerjw · 2021-02-12T18:59:53Z

moveit_ros/planning_interface/test/move_group_ompl_constraints_test.cpp

+  std::thread executor_thread_;
+  moveit::planning_interface::MoveGroupInterfacePtr move_group_;
+  std::string ref_link_, ee_link_;
+  // mutable std::mutex latest_state_mutex_;


[nit] remove commented out code

bostoncleek requested review from JafarAbdi, henningkayser, mamoll and tylerjw January 27, 2021 19:54

bostoncleek mentioned this pull request Jan 27, 2021

OMPL orientation constraints #348

Closed

henningkayser approved these changes Feb 5, 2021

View reviewed changes

OMPL constrained planning

781165d

Uses OMPL for constrained cartesian planning. Co-authored-by: JeroenDM <jeroendemaeyer@live.be>

tylerjw approved these changes Feb 12, 2021

View reviewed changes

tylerjw merged commit 20c7d33 into moveit:main Feb 15, 2021

nbbrooks mentioned this pull request Feb 24, 2021

Remove Cartesian Interpolator and point documentation to use OMPL constrained planner #373

Open

2 tasks

chiatseen mentioned this pull request Apr 8, 2021

Collision detected between 2 robot arms when planned trajectory does not collide #412

Closed

JeroenDM mentioned this pull request Jul 22, 2021

OMPL Constrained State Space Planning moveit/moveit#2273

Open

6 tasks

JeroenDM mentioned this pull request Sep 24, 2021

how to use your orientation_constrained method？ JeroenDM/elion#10

Open

andreas-botbuilt mentioned this pull request Aug 12, 2022

Position constraint violated #1507

Closed

leander-dsouza mentioned this pull request Apr 30, 2023

Backport MoveIt2 Constraints moveit/moveit#3415

Closed

2 tasks

+                      - -2.356
+                      - 0.0
+                      - 1.571
+                      - 0.785

                
                    No newline at end of file

Conversation

bostoncleek commented Jan 27, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Description

Demo

Unit Tests

Integration Test

Box Constraint

Plane Constraint

Checklist

Uh oh!

bostoncleek commented Jan 27, 2021

Uh oh!

JeroenDM commented Jan 28, 2021

Uh oh!

bostoncleek commented Jan 28, 2021

Uh oh!

codecov bot commented Jan 28, 2021 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Codecov Report

Uh oh!

JeroenDM commented Jan 29, 2021

Uh oh!

henningkayser left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

bostoncleek commented Feb 5, 2021

Uh oh!

tylerjw commented Feb 5, 2021

Uh oh!

tylerjw commented Feb 9, 2021

Uh oh!

bostoncleek commented Feb 11, 2021

Uh oh!

tylerjw left a comment

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

4 participants

bostoncleek commented Jan 27, 2021 •

edited

Loading

codecov bot commented Jan 28, 2021 •

edited

Loading