Support Chained Controllers in Moveit

[pac48]

Currently, Moveit2 can not handle the new chained controllers format in ros2_control. The reason why is because Moveit2 creates a map from controllers to joint interfaces, but does not account for chained controllers. For example, see the diagram below

A mapping would be created from JTC to Chained Controller 1 and joint_4/pos, which would cause a failure if trying to plan with joints 1-3. My changes will convert the above structure to the one shown below.

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

[codecov]

Codecov Report

Merging #1395 (58ecb15) into main (3a51feb) will decrease coverage by 0.44%.
The diff coverage is 95.75%.

@@            Coverage Diff             @@
##             main    #1395      +/-   ##
==========================================
- Coverage   61.55%   61.11%   -0.43%     
==========================================
  Files         274      275       +1     
  Lines       24977    25239     +262     
==========================================
+ Hits        15371    15422      +51     
- Misses       9606     9817     +211     

Impacted Files	Coverage Δ
...ontrol_interface/src/controller_manager_plugin.cpp	17.94% <95.75%> (ø)
...nning_scene_monitor/src/planning_scene_monitor.cpp	45.73% <0.00%> (+0.44%)	⬆️

---

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 3a51feb...58ecb15. Read the comment docs.

[henningkayser]

If the handling of the service response is so complicated, It makes me wonder if the message is designed the right way. If anything, I'd assume that other consumers (if there are any) would have to deal with the same kind of filtering. How about adding a flag to the request include_chained_controllers and move the logic into ros2_control?

[destogl]

How about adding a flag to the request include_chained_controllers and move the logic into ros2_control?

I wanted to propose something very similar. IMO we should not have logic of chained controllers here in MoveIt2, ros2_control should provide this in the most suitable format. I was thinking earlier about extending the topic, but was too focused on functionality getting working.

@pac48 What are actual data you would like to get from ros2_control? What should be the proper format of information? Would you like to get a list of controllers in the chain, or something else? You basically need mapping between controller and interfaces, I am right? (I am just doing something similar for another feature)

[pac48]

@destogl MoveIt2 creates a mapping from controller name to its claimed interface and uses it to validate whether a trajectory can be executed with the given controller. This assumption does not hold when there are chained controllers. Two possible solutions are:

1. modify the received information to meet the needs of Moveit2 (this PR)
2. or create a concept of a control system which may have one or many controller. The control system is characterized by inputs and outputs. Add another array field to this the controller list defintion called systems.

[AndyZe]

I think we need a larger discussion on how to rework the MoveIt controller managers. I'll admit, I don't know anything about chained controllers at all.

People who should be involved: @destogl @pac48 @mikeferguson @AndyZe @henningkayser @JafarAbdi @schornakj

If we do rework the controller managers, it will break some functionality in MTC and therefore MoveIt Studio. But I think it needs to be done at some point, anyway. I think there are very few people (maybe none) who understand the difference between a SimpleControllerManager, MoveItMultiControllerManager, and MoveItControllerManager -- as an example of how the current system is difficult to use.

@mikeferguson commented on the discussion here:

I'd like to pipe in with one additional note about controller stuff: not everyone uses ros2_control on their robot (there are other interfaces out there) - so ideally whatever simplify towards does not require leveraging the ros2_control configuration stuff (control_msgs/FollowJointTrajectory is really the kind of API level that should be leveraged - mapping joints to particular action servers).

[mikeferguson]

@AndyZe I think this particular part is separate from what I was commenting on - there are two different controller plugins in MoveIt2:

• One is ros2_control specific (which this PR modifies)
• The other is the simple controller manager which only knows about FollowJointTrajectory and GripperCommand actions (both from control_msgs, which is used by pretty much every controller framework out there) - in particular, it doesn't know anything about starting/stopping those controllers (it assumes they are already running)

The simple controller manager is something I added almost a decade ago - originally as an alternative to the ros_control based thing. Somewhere along the way it seems that the architecture changed a bit maybe and now we actually use the simple one in parallel with ros2_control? (at least, that's what the MSA export seems to imply, but I haven't dug too far into it).

And yes, it seems there is definitely confusion over what plugin to run - an example: #1394 (comment)

[AndyZe]

Well, here are some issues I know about:

• The SimpleControllerManager requires a namespace. The other controller mgr's don't. That's confusing.
• What's the difference between MoveItMultiControllerManager and MoveItControllerManager
• Do we even need a controller manager at all? Wouldn't one line in a yaml file to specify the controller topic for each joint group be enough?

I guess the benefit of a controller mgr is the ability to control arm + gripper at the same time

[mikeferguson]

The mapping isn't really at the joint level (conceptually, although I believe it is doing a joint-level matching in our current implementations) - the mapping is really from planning groups -> controllers, a common robot might have something like:

• arm group - which is a 7-dof arm and has a follow joint trajectory action
• arm_with_torso group - which is a 7-dof arm with a torso joint and uses a follow joint trajectory action
• gripper group - uses a gripper command action

In this case, each of those 7-dof arm joints is actually mapped to two different action interfaces, depending on which planning group was used. In theory, the MoveitSimpleControllerManager is the "minimal" implementation, and then the other ones implement ros2_control interfaces as well (I think the Multi basically adds support for mobile bases, right?)

[AndyZe]

OK - thanks.

Well, I think we should still have a discussion. To me, it sounds like we need to:

• document this stuff
• ensure the namespace is the same for all controller mgr's
• possibly delete all controller mgr's except MoveItSimpleControllerManager (This might break the Hello Stretch, though). Or, possibly delete nothing

[AndyZe]

I think your perspective is a lot different from mine because I've never used humanoids. 👍

[AndyZe]

@mikeferguson are you aware of any reason why we couldn't handle all cases with the MoveItSimpleControllerManager? Assuming the necessary controllers are running

[schornakj]

possibly delete all controller mgr's except MoveItSimpleControllerManager

I think this is not a realistic option. The MoveItSimpleControllerManager is hard-coded to handle just a few types of controllers, so it doesn't support situations where users have developed their own controllers. We shouldn't put ourselves into a situation where MoveIt has to explicitly enumerate each possible type of supported controller.

[schornakj]

One particular situation where the MoveItControllerManager is required is when multiple controllers can actuate the same joint group but cannot be running at the same time. This will come up more often as we use the admittance controller, since a user might only want to use that controller for a subset of the motion segments in a larger trajectory.

[mikeferguson]

I think you could end up with two controller managers:

• Simple - doesn't do anything fancy, works with any controller framework that doesn't require manually switching controllers
• One for ROS2_controllers - does all the start/stop logic required (I'm assuming the multi-logic for bases can be merged into the default ros2 controllers one)

At the moment, the simple controller does NOT support a mobile base AFAIK

[pac48]

I made an issue on the ros2_control
side of things: ros-controls/ros2_control#744
I think this specific problem is better solved on their end.

[AndyZe]

@mikeferguson @DLu what do you guys use for a controller manager for mobile robots?

Grepping through the Hello Stretch codebase, it looks like the MoveItSimpleControllerManager is used. So the only mobile manipulator I'm aware of in ROS2 doesn't even use the MoveItMultiControllerManager.

[AndyZe]

Ah, the MoveItMultiControllerManager is for multiple ros2_control nodes, not for mobile robots. OK, please disregard the previous question.

https://github.com/ros-planning/moveit2/tree/main/moveit_plugins/moveit_ros_control_interface