Servo as local planner demonstration

[AdamPettinger]

Description

• Adds Servo as a local planner (following @sjahr's PR for that over in moveit2)
• Hardcodes the MTC global planner to do a relative Cartesian plan
• Adds a joint configuration for the start of the Cartesian path
• Allows Servo local planner to execute the trajectory at constant velocity, with user input to allow rotation of the last joint on-line

How to test

Follow the updated README section Hybrid Planning Demonstration

Note this PR includes a dependency change in moveit2, so you should pull the correct version and build in order to test properly

Next Steps

This is mostly a demonstration of the hybrid planning capabilities. I am not too sure we actually want to merge this in (or at least as is now with hardcoded stuff for the demo). @henningkayser do you have any next steps for this effort?

[henningkayser]

I think this is a really useful step and we should merge this so that @christianlandgraf can reproduce this with hardware. I'd be interested to see if we see any performance differences. @AdamPettinger how about adding your temporary local solver to the demo package next to the global MTC solver? That way we can continue experimenting with it, possibly adding IK-based jogging as a next step?

[AdamPettinger]

I am working on adding it, don't quite have the pluginlib configurations correct yet so the local planner does not find the new ServoSolver plugin

[christianlandgraf]

Looks great!! Unfortunately, the hybrid_planning_container fails on my setup although the global and local goals seem to be succesfully accepted... Somehow it worked yesterday with commit 0fa9dbe but I didn't manage to get it working again and therefore I didn't tested it on real hardware.

Besides that, I had to do some minor fixes to succesfully compile everything:

• Switch to foxy branch in geometric_shapes (in upstream.repos ). I think this is one reason for the current build fail of CI
• Merge the current main branch of moveit2 into the feature/hybrid_planning branch in https://github.com/henningkayser/moveit2/tree/feature/hybrid_planning.
• Add / symbols to the end of each include directory for header files in this CMakeLists.txt

[AdamPettinger]

The latest commit I just added is following the ROS 2 Pluginlib tutorial with the local solver in our (hybrid_planning_demo) package. However, this still isn't loading the plugins correctly, see error below. If I could ask maybe @sjahr to take a look and see if there are any glaring problems, that would be great

[ERROR - local_planner_component]: Exception while loading trajectory operator 'hybrid_planning_demo::SimpleServoSampler': 'Could not find library corresponding to plugin hybrid_planning_demo::SimpleServoSampler. Make sure the plugin description XML file has the correct name of the library and that the library actually exists.'
[ERROR - local_planner_component]: Exception while loading constraint solver 'hybrid_planning_demo::ServoSolver': 'Could not find library corresponding to plugin hybrid_planning_demo::ServoSolver. Make sure the plugin description XML file has the correct name of the library and that the library actually exists.'

[christianlandgraf]

It works on the UR10e now! I would suggest to revert the changes in ipa_demo_cell_description and I think it's ready to merge aftewards.

[AdamPettinger]

It works on the UR10e now! I would suggest to revert the changes in ipa_demo_cell_description and I think it's ready to merge aftewards.

@christianlandgraf Done! Will you verify it looks ok? Note the launch now has to be with the UR10e, or the pre-welding configuration is in collision with the table

@henningkayser I didn't want to update the moveit branch to my fork again since I am leaving soon, but we might want to use your fork rebased on the foxy branch now, and include this change in hybrid planning. I hope this will make CI pass

[christianlandgraf]

@AdamPettinger It looks good to me. I think that's a minor issue since I hope we can mainly continue work with the UR10e from now on...