Port Pilz Industrial Motion Planner #452

henningkayser · 2021-05-05T17:59:02Z

Fixes #315

You can test the planner using the PRBT demo.launch.py

Update

~~(almost)~~ All unit tests are finally succeeding locally for me now (thanks @sjahr for all the restructuring and preparing the launch scripts)
Gripper tests are still disabled (they were implemented using a test suite with robot models for arm-only and arm+gripper). Support for that requires adding more boilerplate to the launch files. Those tests are also somewhat specific to the PRBT so IMO we should probably drop them and just prepare different launch setups for different robots to run the existing tests.
I reduced redundancy in the build structure. Some source files were compiled over and over again (different plugins, different tests) and there was even an extra library that included almost all source files just to provide test dependencies again. I broke that down into smaller libraries that can be reused and configured more specific target dependencies for most libraries and tests. There is still some cleanup that could be done, but I would expect that this reduces compile time significantly. I was planning to compare build times with the old setup, maybe this is something we want to backport to MoveIt 1.

Issues (Future work)

Some stuff I encountered while working on this. I'll file issues for these the next days.

There is a bunch of boilerplate code in the tests that could be moved into moveit_test_utils
I ran into weird runtime issues where plugins could not be loaded even though they were reported as "declared" by the ClassLoader. E.g. MoveIt capabilites were found but failed to load only when the Pilz planner has been loaded before. I was only able to fix this by letting all plugins use a plugin name (removing plugin names from all plugins also worked). @AndyZe this might very well be related to your weird issue where the smoothing plugin only didn't load inside a component node, which also is a plugin. I still want to confirm this with a reproducible example, but this is probably just a bug upstream. Since plugin names are not required anymore in ROS 2, we could also drop them from all the existing ones (or enforce adding them) if the issue persist.
RViz dies when switching planners during execution
I'm frequently running into a std::bad_alloc when loading a robot model, looking into this next

henningkayser · 2021-11-10T18:56:02Z

moveit_core/robot_model/src/joint_model_group.cpp

@@ -640,7 +640,7 @@ bool JointModelGroup::canSetStateFromIK(const std::string& tip) const
    // remove frame reference, if specified
    const std::string& tip_local = tip[0] == '/' ? tip.substr(1) : tip;
    const std::string& tip_frame_local = tip_frame[0] == '/' ? tip_frame.substr(1) : tip_frame;
-    RCLCPP_WARN(LOGGER, "comparing input tip: %s to this groups tip: %s ", tip_local.c_str(), tip_frame_local.c_str());


not sure why this was a warning, was spamming everywhere when enabling the tests. I changed it back how it was since MoveIt 1.

henningkayser · 2021-11-10T18:57:24Z

moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits.hpp

+struct JointLimits
+{
+  JointLimits()
+    : min_position(std::numeric_limits<double>::quiet_NaN())


I'm not convinced that nan is a good default, in ROS 1 it was 0. But I left it like this for now to match the WIP PR.

henningkayser · 2021-11-10T18:58:27Z

moveit_planners/pilz_industrial_motion_planner/include/joint_limits/joint_limits_rosparam.hpp

+
+namespace joint_limits
+{
+inline bool declare_parameters(const std::string& joint_name, const rclcpp::Node::SharedPtr& node,


This API had to be changed from upstream. @destogl what do you think about adding a related change to ros-controls/ros2_control#462?

henningkayser · 2021-11-10T19:00:17Z

moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp

+    // NOTE: declareParameters fails (=returns false) if the parameters have already been declared.
+    // The function should be checked in the if condition below when we disable
+    // 'NodeOptions::automatically_declare_parameters_from_overrides(true)'
+    joint_limits_interface::declareParameters(joint_model->getName(), param_namespace, node);


We should make a decision if we want to enforce declaring parameters everywhere or if we are fine with allowing automatic overrides moving forward.

codecov · 2021-11-11T13:24:47Z

Codecov Report

Merging #452 (a06f7b9) into main (cf62f75) will increase coverage by 4.00%.
The diff coverage is 90.71%.

❗ Current head a06f7b9 differs from pull request most recent head 05fa5a2. Consider uploading reports for the commit 05fa5a2 to get more accurate results

@@            Coverage Diff             @@
##             main     #452      +/-   ##
==========================================
+ Coverage   56.81%   60.81%   +4.00%     
==========================================
  Files         200      271      +71     
  Lines       21659    23815    +2156     
==========================================
+ Hits        12304    14481    +2177     
+ Misses       9355     9334      -21

Impacted Files	Coverage Δ
...strial_motion_planner/cartesian_trajectory_point.h	`100.00% <ø> (ø)`
...ndustrial_motion_planner/joint_limits_aggregator.h	`100.00% <ø> (ø)`
...industrial_motion_planner/joint_limits_extension.h	`100.00% <ø> (ø)`
...al_motion_planner/pilz_industrial_motion_planner.h	`100.00% <ø> (ø)`
..._industrial_motion_planner/planning_context_circ.h	`100.00% <ø> (ø)`
...z_industrial_motion_planner/planning_context_lin.h	`100.00% <ø> (ø)`
...z_industrial_motion_planner/planning_context_ptp.h	`100.00% <ø> (ø)`
...ustrial_motion_planner/trajectory_blend_response.h	`100.00% <ø> (ø)`
...ion_planner/trajectory_blender_transition_window.h	`100.00% <ø> (ø)`
..._motion_planner/trajectory_generation_exceptions.h	`100.00% <ø> (ø)`
... and 117 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 42afeea...05fa5a2. Read the comment docs.

henningkayser · 2021-11-11T14:40:46Z

clang-tidy doesn't like the copied joint_limits headers and the included ikfast plugin for the PRBT. Both should be ignored. Otherwise, CI seems happy with the current version.

mergify · 2021-11-19T11:06:45Z

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

henningkayser · 2021-11-29T18:35:38Z

Update:

All unit tests pass for the PRBT package
I think the migration is ready to get merged before we branch off rolling

However, I don't think that it makes sense to enable the PRBT in moveit_resources and enabling the tests for a different robot is a bigger undertaking. For now, I switched to the Panda robot, but obviously a lot of the tests fail since there are still plenty of hardcoded values in the tests and the Panda doesn't provide deterministic IK at this point. The failing tests are disabled with the last commit.
I still think that we should merge this before the branch-off and then work on fixing all the tests for a different robot as suggested in this issue moveit/moveit#2947.

henningkayser · 2021-11-29T19:06:09Z

Based on @tylerjw's feedback, I switched this back to use PRBT to keep the tests enabled. We still need to migrate to a different robot, but keeping the old setup around is worth it in order to also keep the tests enabled.

mergify · 2021-11-30T15:47:13Z

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

moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt

moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp

henningkayser · 2021-12-03T15:45:35Z

@JafarAbdi thanks for your review, all requests are fixed. I already merged the moveit_resources PR and updated the installed repo here as well. IMO, this is ready to get merged if CI succeeds, but please don't squash-merge.

tylerjw · 2021-12-03T16:24:07Z

.github/workflows/ci.yaml

@@ -27,6 +27,7 @@ jobs:
      CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"
      DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
      UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f)
+      TARGET_WORKSPACE: $TARGET_REPO_PATH github:henningkayser/moveit_resources#pr-port_prbt_packages


Shouldn't you drop this change now that it is merged into moveit_resources?

I thought I just did this... I must have failed to commit it

Should be fixed now. We still need moveit_resources in the target workspace because of the circular dependency (see #885). It's the same issue with MoveIt 1

tylerjw · 2021-12-06T15:17:25Z

I pushed a small change to fix a clang-tidy warning in one of the tests.

* Port to rolling * Update source documentation * Port joint and cartesian limit unittests * Fix cartesian and joint limit handling * Port pure gtest based unittests * Update unittest launch files * Add Joint Limits Container and Joint Limits validator unittest * Cleanup and use correct cmake macros * Port launch_testing depended unittests

* Fix deprecated tf2 includes and conversion functions * Include latest changes to ros2_control joint_limits API * Move joint_limits headers into separate files * Silence warnings about unused variadic macros * Remove dependency to moveit_configs_utils * Fixup dependency and runtime issues in tests * Make all tests pass * Disable failing linters

henningkayser force-pushed the pr-port_pilz_planner branch from 62a2020 to fe91433 Compare July 16, 2021 15:43

henningkayser force-pushed the pr-port_pilz_planner branch from fe91433 to e7ed187 Compare October 5, 2021 15:48

henningkayser self-assigned this Oct 22, 2021

henningkayser force-pushed the pr-port_pilz_planner branch from 26b7be6 to 1908031 Compare November 8, 2021 18:42

henningkayser changed the title ~~WIP: Port Pilz Industrial Motion Planner~~ Port Pilz Industrial Motion Planner Nov 10, 2021

henningkayser force-pushed the pr-port_pilz_planner branch from 0ee9d5c to ac798fe Compare November 10, 2021 18:55

henningkayser commented Nov 10, 2021

View reviewed changes

henningkayser mentioned this pull request Nov 10, 2021

Port PRBT packages moveit/moveit_resources#101

Merged

henningkayser commented Nov 10, 2021

View reviewed changes

henningkayser force-pushed the pr-port_pilz_planner branch 4 times, most recently from be006a5 to d2d0391 Compare November 22, 2021 09:58

henningkayser force-pushed the pr-port_pilz_planner branch 2 times, most recently from 32f794a to 32b228a Compare November 29, 2021 18:30

henningkayser force-pushed the pr-port_pilz_planner branch from 32b228a to da9dc4b Compare November 29, 2021 19:02

JafarAbdi mentioned this pull request Nov 29, 2021

Sync with MoveIt #834

Merged

henningkayser force-pushed the pr-port_pilz_planner branch 3 times, most recently from 0a16433 to 1406b10 Compare November 30, 2021 16:47

henningkayser force-pushed the pr-port_pilz_planner branch 4 times, most recently from cb9acac to df3fcb7 Compare December 1, 2021 19:06

JafarAbdi reviewed Dec 3, 2021

View reviewed changes

henningkayser force-pushed the pr-port_pilz_planner branch from df3fcb7 to 40176d7 Compare December 3, 2021 14:48

henningkayser mentioned this pull request Dec 3, 2021

Revert debug warning #884

Merged

henningkayser force-pushed the pr-port_pilz_planner branch from 40176d7 to def10ba Compare December 3, 2021 15:28

tylerjw reviewed Dec 3, 2021

View reviewed changes

henningkayser force-pushed the pr-port_pilz_planner branch from def10ba to bf951ea Compare December 3, 2021 17:47

henningkayser and others added 2 commits December 6, 2021 08:17

PILZ: Migrate planner and testutils packages to ROS 2

25cc636

tylerjw force-pushed the pr-port_pilz_planner branch from a06f7b9 to 58c2d05 Compare December 6, 2021 15:17

henningkayser added 3 commits December 6, 2021 08:18

Use CallbackGroup for MoveGroupSequenceAction

b923910

Temporarily install moveit_resources in workspace

05fa5a2

tylerjw force-pushed the pr-port_pilz_planner branch from 58c2d05 to 05fa5a2 Compare December 6, 2021 15:18

tylerjw approved these changes Dec 6, 2021

View reviewed changes

tylerjw merged commit 2830dd8 into moveit:main Dec 6, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Port Pilz Industrial Motion Planner #452

Port Pilz Industrial Motion Planner #452

henningkayser commented May 5, 2021 •

edited

Loading

henningkayser Nov 10, 2021

henningkayser Nov 10, 2021

henningkayser Nov 10, 2021

henningkayser Nov 10, 2021

codecov bot commented Nov 11, 2021 •

edited

Loading

henningkayser commented Nov 11, 2021

mergify bot commented Nov 19, 2021

henningkayser commented Nov 29, 2021

henningkayser commented Nov 29, 2021

mergify bot commented Nov 30, 2021

henningkayser commented Dec 3, 2021

tylerjw Dec 3, 2021

henningkayser Dec 3, 2021

henningkayser Dec 3, 2021

tylerjw commented Dec 6, 2021

Port Pilz Industrial Motion Planner #452

Port Pilz Industrial Motion Planner #452

Conversation

henningkayser commented May 5, 2021 • edited Loading

henningkayser Nov 10, 2021

Choose a reason for hiding this comment

henningkayser Nov 10, 2021

Choose a reason for hiding this comment

henningkayser Nov 10, 2021

Choose a reason for hiding this comment

henningkayser Nov 10, 2021

Choose a reason for hiding this comment

codecov bot commented Nov 11, 2021 • edited Loading

Codecov Report

henningkayser commented Nov 11, 2021

mergify bot commented Nov 19, 2021

henningkayser commented Nov 29, 2021

henningkayser commented Nov 29, 2021

mergify bot commented Nov 30, 2021

henningkayser commented Dec 3, 2021

tylerjw Dec 3, 2021

Choose a reason for hiding this comment

henningkayser Dec 3, 2021

Choose a reason for hiding this comment

henningkayser Dec 3, 2021

Choose a reason for hiding this comment

tylerjw commented Dec 6, 2021

henningkayser commented May 5, 2021 •

edited

Loading

codecov bot commented Nov 11, 2021 •

edited

Loading