Switch controller in moveit

[Daniel Hoeltgen]

Hello everybody,

I am using the UR modern driver from Thomas Timm with ros control.

While running the application I would like to switch between two controllers which both interface with follow joint trajectory.

Therefore I listed both controllers in the moveit_config/config/controllers.yaml. Both controllers use the same joints.

I hoped that follow joint trajectory would be simply passed to the controller that is running, so I just switch between the controllers using the controller_manager/switch_controller service.

Unfortunately this doesn't work. Though the controller switches successfully, moveit keeps sending follow joint trajectory messages to the first controller.

I get this messages: 

[ INFO] [1479395259.192265251]: Received new trajectory execution service request...

[ERROR] [1479395259.192646521]: Can't accept new action goals. Controller is not running.

when I unload the controller, I get no massage at all.

How can I tell MoveIt! to use the other controller? I could not find a service like controller_manager/switch_controller.

regards

Daniel

[Daniel Hoeltgen]

Hello again,

I think moveit_controller_manager::MoveItControllerManager::switchControllers() would do what I want but the method declared in moveit/controller_manager/controller_manager.h is pure virtual.

The simple_controller_manager does not implement this functionality.

Is there really no implementation for switching controllers? Is there a workaround?

regards

Daniel

[v4hn]

Please have a look at this discussion for some background on this class:
https://github.com/ros-planning/moveit_ros/issues/725


v4hn

On Tue, Nov 22, 2016 at 08:56:02AM -0800, 'Daniel Hoeltgen' via MoveIt! Users wrote:
> Hello again,
>
> I think *moveit_controller_manager::MoveItControllerManager::switchControllers()
> *would do what I want but the method declared in
> moveit/controller_manager/controller_manager.h
> <https://github.com/ros-planning/moveit/blob/indigo-devel/moveit_core/controller_manager/include/moveit/controller_manager/controller_manager.h> is
> pure virtual.
> The simple_controller_manager
> <https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_plugins/moveit_simple_controller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h> does

> not implement this functionality.
>
> Is there really no implementation for switching controllers? Is there a
> workaround?
>
> regards
> Daniel
>
> Am Donnerstag, 17. November 2016 16:15:51 UTC+1 schrieb Daniel Hoeltgen:
> >
> > Hello everybody,
> >
> > I am using the UR modern driver from Thomas Timm with ros control.
> > While running the application I would like to switch between two

> > controllers which both interface with *follow joint trajectory.*
> > Therefore I listed both controllers in the *moveit_config/config/controllers.yaml.
> > *Both controllers use the same joints.
> > I hoped that *follow joint trajectory* would be simply passed to the

> > controller that is running, so I just switch between the controllers using

> > the *controller_manager/switch_controller *service.

> > Unfortunately this doesn't work. Though the controller switches

> > successfully, moveit keeps sending *follow joint trajectory *messages to

> > the first controller.
> > I get this messages:
> > [ INFO] [1479395259.192265251]: Received new trajectory execution service
> > request...
> > [ERROR] [1479395259.192646521]: Can't accept new action goals. Controller
> > is not running.
> >
> > when I unload the controller, I get no massage at all.
> >
> > How can I tell MoveIt! to use the other controller? I could not find a

> > service like *controller_manager/switch_controller*.
> >
> > regards
> > Daniel
> >
>
> --
> You received this message because you are subscribed to the Google Groups "MoveIt! Users" group.
> To unsubscribe from this group and stop receiving emails from it, send an email to moveit-users...@googlegroups.com.
> To view this discussion on the web visit https://groups.google.com/d/msgid/moveit-users/7509cd03-01bf-4619-88c5-14441796f41d%40googlegroups.com.

[Daniel Hoeltgen]

Hello v4hn,

thank you for the clue. I definitely agree that this has to be renamed.

My solution now was coding my own interface with execute() and switchController() methods based on the simple_controller_manager.

regards

Daniel

[v4hn]

Evening everyone,

On Fri, Nov 25, 2016 at 07:11:38AM -0800, 'Daniel Hoeltgen' via MoveIt! Users wrote:
> Am Dienstag, 22. November 2016 23:15:55 UTC+1 schrieb v4hn:
> > Please have a look at this discussion for some background on this class:
> > https://github.com/ros-planning/moveit_ros/issues/725
>

> thank you for the clue. I definitely agree that this has to be renamed.

Most development in MoveIt happens via pull-requests from users,
so feel free to change it and provide a request. :-)

> My solution now was coding my own interface with *execute()* and
> *switchController()* methods based on the *simple_controller_manager*.

This, on the other hand, is only possible because the class specifies
a Controller*Manager* and not just an *Interface*.
This sounds to me like you could provide a pull-request that enables
the simple_controller_manager to switch controllers.
This would be awsome to have upstream! Please consider contributing. :)

In the discussion I pointed you to, we argued that this class more or less
duplicates ros_control's management capabilities and that we don't need another
manager on top; an interface without management capabilities could suffice.
But at the moment I'm not sure how to interface MoveIt and ros_control if we would
go down that road. I have not yet seen a running implementation where MoveIt can make
use of a ros_control-based node that switches controllers on-the-fly (without using the
methods of the ControllerManager, that is to say).
Right now external ros_control-based nodes offer different follow_joint_trajectory actions
for different trajectory controllers and we need a way to switch these on MoveIt's side too..

As far as I can see, currently ros_control does not allow to switch the controller behind
a follow_joint_trajectory action without changing the whole action server. This might be
an alternative way to go.
Can anybody confirm this is not possible at the moment?


v4hn

[Daniel Hoeltgen]

Hello v4hn,

This, on the other hand, is only possible because the class specifies 
a Controller*Manager* and not just an *Interface*. 
This sounds to me like you could provide a pull-request that enables 
the simple_controller_manager to switch controllers. 
This would be awsome to have upstream! Please consider contributing. :)  

 

My first approach was to use or improve the existing functionality, but I couldn't really get behind how all this is working. It seems that the move_group::execute() method forwards the trajectory to the execute_kinematic_path service but I couldn't find where exactly this service is started and where the received trajectory message is passed to sendTrajectory(). 

I interpret it like this: The service is somehow provided by the simple_controller_manager plugin. In general we would just have to implement a message for switching controllers and change the handle object. But if it's really so easy I can't see why this is not implemented yet.

At least coding your own execute() and switchController() methods is an easy workaround.

As far as I can see, currently ros_control does not allow to switch the controller behind 
a follow_joint_trajectory action without changing the whole action server. This might be 
an alternative way to go. 
Can anybody confirm this is not possible at the moment? 

You can use a different controller manager plugin like the ros_control_interface. This switches your ros controllers automatically and handles them correctly. However it works only with standard velocity and position controllers but probably suffices for most users. I coded an own controller and thus couldn't use it. Moreover I read that it reacts slowly.

regards

Daniel

[v4hn]

On Fri, Dec 02, 2016 at 06:46:28AM -0800, 'Daniel Hoeltgen' via MoveIt! Users wrote:
> My first approach was to use or improve the existing functionality, but I
> couldn't really get behind how all this is working.

Welcome to the murky waters of middleware-engineering,
But I suppose you're somewhat used to it, otherwise you wouldn't send mails to this list. :)

> It seems that the move_group::execute() method forwards the trajectory to the
> execute_kinematic_path service but I couldn't find where exactly this
> service is started and where the received trajectory message is passed to
> sendTrajectory().

You missed one level of abstraction. MoveGroupInterface (until recently MoveGroup)
is the client side that calls a service (by recent addition alternatively an Action)
of the move_group node. This service is provided by a capability, that receives the plan
and pushes it to the execution manager here:
https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.cpp#L82

This manager then chooses a set of applicable controllers that together can execute the trajectory,
switches them to active as required here https://github.com/ros-planning/moveit/blob/kinetic-devel/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp#L1576 ,
and eventually pushes the trajectories (or segments) to the selected controllers.
To decide applicability of controllers, it compares some characteristics of the controllers
and (among other things) prefers active controllers.

> I interpret it like this: The service is somehow provided by the
> simple_controller_manager plugin. In general we would just have to
> implement a message for switching controllers and change the handle object.
> But if it's really so easy I can't see why this is not implemented yet.

Well, it's a bit more complicated as you probably see from the pointers above. (:
One could implement logic in the movit_simple_controller_manager to make it support
"active" controllers with a simple bitmask stored in the controller manager,
implement switchControllers, add a forward method for this method to the
TrajectoryExecutionManager interface (not sure if this would be needed,
but it seems one can't directly unload a controller within moveit)
and then add a MoveGroupCapability that provides a ROS service to call that method
on the loaded manager. That's some thoughts off the top of my head...

> You can use a different controller manager plugin like the
> ros_control_interface

> <https://github.com/ros-planning/moveit/tree/kinetic-devel/moveit_plugins/moveit_ros_control_interface>.

> This switches your ros controllers automatically and handles them correctly.

You can switch controllers on the ros-control side.
As this is forwarded to MoveIt by moveit_ros_control_interface,
the trajectory execution manager should prefer the now-active controller
according to the logic I described above (if they are otherwise equal).
So that way it probably is possible to change controllers without
having to tell MoveIt explicitly. No idea why I didn't see that when I wrote the previous mail.

Even so, this whole apparatus makes it pretty complex to turn the current concept
of a Controller*Manager* in MoveIt into one of a Controller*Interface* (this is what Dave proposed in the issue).


v4hn
--
Michael Görner, M.Sc. Cognitive Science, PhD Student
Universität Hamburg
Faculty of Mathematics, Informatics and Natural Sciences
Department of Informatics
Group Technical Aspects of Multimodal Systems
Vogt-Kölln-Straße 30
D-22527 Hamburg
Room: F-315
Phone: +49 40 42883-2342

[Daniel Hoeltgen]

You can switch controllers on the ros-control side. 
As this is forwarded to MoveIt by moveit_ros_control_interface, 
the trajectory execution manager should prefer the now-active controller 
according to the logic I described above (if they are otherwise equal). 
So that way it probably is possible to change controllers without 
having to tell MoveIt explicitly.

this is what didn't work for me. After switching the controller in ros-control MoveIt still sent the trajectories to the same controller. I even unloaded the first controller but nothing changed.

I understand the moveit_ros_control_interface like this: It can automatically chose between velocity based and position based controllers depending on the trajectory. Then it tells ros-control to switch controllers (not vice versa). At least I know that in my tests it didn't chose the now-active-controller.