Let the MoveItControllerManager manage more controllers

[schornakj]

As currently implemented, the MoveItControllerManager plugin can only manage the activation and deactivation of controllers that have a MoveIt controller allocator plugin defined, due to this bit of code.

However, it would also be useful to allow the MoveItControllerManager to also manage controllers that are not associated with MoveIt trajectory execution. For example, MoveIt Servo is often configured to use a streaming position or velocity controller, so switching between Servo control and joint trajectory execution requires managing these controllers separately from the execution of trajectories through the TrajectoryExecutionManager. The TrajectoryExecutionManager already has a lot of information about the relationships between different available controllers and their potentially-overlapping associations with joint interfaces, and it would be great to be able to use that more flexibly.

It's fairly straightforward to allow the MoveItControllerManager to manage more controllers -- the lines of code I linked above would need to be modified to be like this so the controller is added to managed_controllers_ regardless of if a plugin is available for it:

std::string absname = getAbsName(controller.name);
auto controller_it = managed_controllers_.insert(std::make_pair(absname, controller)).first;  // with namespace
auto& claimed_interfaces = controller_it->second.claimed_interfaces;
std::transform(claimed_interfaces.cbegin(), claimed_interfaces.cend(), claimed_interfaces.begin(),
               [](const std::string& claimed_interface) {
                 return parseJointNameFromResource(claimed_interface);
               });

if (loader_.isClassAvailable(controller.type))
{
  allocate(absname, controller_it->second);
}

One way to use this expanded functionality would be to create a MoveGroup capability that activates a specified controller through the TrajectoryExecutionManager.anager.

Does anyone have thoughts about this?

[tylerjw]

My only thought is we probably should have a better name.

https://blog.codinghorror.com/i-shall-call-it-somethingmanager/

I like the idea though.

[henningkayser]

@schornakj I added #753 as a blocker for #731. Feel free to bump up the version in your PR or in a separate one.

My only thought is we probably should have a better name.

https://blog.codinghorror.com/i-shall-call-it-somethingmanager/

I like the idea though.

I somewhat agree. But this is probably more of a general discussion. Also, I'm not sure all manager classes should be renamed just because they are called managers. Often, a class redesign is required to find better names ;)

[AndyZe]

I think the code change you propose here sounds good 👍

As for this:

One way to use this expanded functionality would be to create a MoveGroup capability that activates a specified controller through the TrajectoryExecutionManager.anager.

I don't love it because the name TrajectoryExecutionManager would no longer apply. Maybe it should be called ControllerSynchronizer or something like that. Also, hopefully it would be available via C++ API too -- not just MoveGroup.

Anyway, I don't think that comment applies to the scope of work in this issue. We can work out those details later.

[AndyZe]

Using this issue as a gathering place for changes I would like...

• No more MoveIt-specific yaml controller config file. It has always been confusing to have a MoveIt config file along with a ros_control config file, and the distinction between them is getting blurred even more as MoveIt sees more realtime usage.

• MoveIt should simply query for active controllers that can actuate the specified joints for each motion. Throw an error if there is not an available controller.

• I don't think MoveIt needs to query for conflicting/overlapping controllers before each motion occurs, since ros2_control already handles that.

• Do not need to monitor trajectory execution duration since ros2_control does that already. Related issue: Moveit simple controller manager doesn't respond to changes of trajectory_execution.execution_duration_monitoring parameter #760

So it seems to me like we could really simplify controller management in MoveIt2 a lot!

[AndyZe]

@destogl what do you think about ^?

[schornakj]

I don't love it because the name TrajectoryExecutionManager would no longer apply. Maybe it should be called ControllerSynchronizer or something like that. Also, hopefully it would be available via C++ API too -- not just MoveGroup.

Since MoveGroup uses the C++ API internally this should naturally happen with any changes to the TrajectoryExecutionManager.

I don't think MoveIt needs to query for conflicting/overlapping controllers before each motion occurs, since ros2_control already handles that.

I don't think ros2_control handles switching between conflicting controllers -- it can provide the info needed to figure out where conflicts exist, but it doesn't activate or deactivate the controllers to resolve the conflict. When I use the /controller_manager/switch_controller service to set the active controller in advance of executing a trajectory, I need to specifically set controllers to stop and start, which requires me to know what active controllers are claiming the joints I need. Finding conflicts between controllers is one of the useful features already implemented in the MoveIt controller manager.

So it seems to me like we could really simplify controller management in MoveIt2 a lot!

I definitely agree. In particular it would be great to get more info about the system "live" while it's running rather than relying on various YAML config files.

[AndyZe]

Finding conflicts between controllers is one of the useful features already implemented in the MoveIt controller manager.

ros2_control does ensure that only one controller claims each interface, I believe. So it will not allow conflicts.

When it comes to MoveIt managing the controllers, and switching between streaming and trajectory control, MoveIt doesn't have any way to know which controller is used for streaming (and vice versa). So I don't think we should attempt to even tackle that problem.

Yeha, we could maybe make a new yaml tag to define whether a controller is valid for streaming, or trajectories, or both, or neither, but... that seems like a lot of complexity that doesn't add much value.

[AndyZe]

Futhermore, it could be possible to have 3+ controllers for any given joint group! Then it will be really tricky for MoveIt to select the right one.

[schornakj]

ros2_control does ensure that only one controller claims each interface, I believe. So it will not allow conflicts.

That's true, but I think the result if there's a conflict is just that the trajectory will fail to execute. Some additional logic is needed to make sure the trajectory can be successfully executed if possible.

When it comes to MoveIt managing the controllers, and switching between streaming and trajectory control, MoveIt doesn't have any way to know which controller is used for streaming (and vice versa). So I don't think we should attempt to even tackle that problem.

Yeah, there are so many levels of application-specificity here that it makes more sense to leave it to the application developer to pick the right controller for some use cases. One issue I ran into is that there isn't much support in MoveGroup or MTC for assigning a specific controller to a given trajectory, so that'll be an additional PR.

[v4hn]

Yeah, there are so many levels of application-specificity here that it makes more sense to leave it to the application developer to pick the right controller for some use cases. One issue I ran into is that there isn't much support in MoveGroup or MTC for assigning a specific controller to a given trajectory, so that'll be an additional PR.

Yes to all of that! For MTC my plan is to specify specific controllers via [external properties](moveit/moveit_task_constructor#194), but that support is not ready for merge yet. Also, when working with compliant controllers, it's far from clear when trajectory execution should finish or abort during execution.

[tylerjw]

Hey team! Please add your planning poker estimate with ZenHub @abake48 @Abishalini @AndyZe @DLu @henningkayser @JafarAbdi @vatanaksoytezer

[schornakj]

Yes to all of that! For MTC my plan is to specify specific controllers via external properties, but that support is not ready for merge yet.

Can you provide some detail about how you picture specifying and using specific controllers through an MTC task? Currently I use the ExecuteTaskSolutionCapability in my project, but since this ties me to the MoveGroup action interface I'm limited by the absence of a message field to describe my desired controller configuration.

Also, when working with compliant controllers, it's far from clear when trajectory execution should finish or abort during execution.

One thing I was thinking about today was switching between compliant and non-compliant controllers that both control the same arm while executing a multi-segment trajectory. It's likely that at the end of a motion the compliant controller would leave the robot at a different joint state than originally planned, which would cause problems for the non-compliant controller since the start start of its trajectory would now be different than what was planned. This might necessitate a replan, and I don't have a sense for if there is existing support for this kind of runtime replanning in MoveIt. Hybrid planning could help solve this, though.

[sjahr]

@schornakj I think Hybrid Planning could help with this problem because you could use the local planner to decouple global planning from control and the planning logic for replanning

[JaisonJose241]

Any updates? Has it been added onto MTC?