Adding a hybrid JointTrajectory/velocity-forwarding controller.

[AndyZe]

This is a new type of controller that accepts trajectory goals or real-time jogging commands. It can switch between the two smoothly. It was motivated by the annoyance of unloading and loading a new controller when I wanted to switch types. I would also like to inherit from this controller, later on, to make a compliant controller that can accept trajectories or velocity commands. This seems like the first step towards that goal.

It inherits from JointTrajectoryController and executes trajectories pretty much the same way. There is a bit of code added, on top of that, to accept real-time commands and forward them, like a JointGroupVelocityController. The real-time commands always interrupt trajectory execution if there is an active trajectory.

Here's a video of testing on a hardware UR3. It shows a couple trajectory/jog/trajectory/jog cycles.

[mathias-luedtke]

Thanks for your contribution!

I am not sure if we can merge this as-is, because the behavior is not well-defined in case the velocity command gets processed while an action is active.
In addition it is not really generic, because it only works for the velocity interface.
IMHO this behavior should be implemented by switching the controllers instead, example: cob_control_mode_adapter.

It was motivated by the annoyance of unloading and loading a new controller when I wanted to switch types.

You only need to load each controller once. Afterwards you can switch between them easily.

[AndyZe]

Thank you, @ipa-mdl. I took care of the minor comments. I think that leaves just a few more things to discuss:

1. The velocity is not well-defined when trajectory execution is interrupted.

The most recent commit adds a setHoldPosition() and a stopping() to make sure the robot halts (briefly, in a controlled way) when a trajectory is interrupted by a real-time command. Here's a video of how that looks at full speed. The trajectory is upward, the initial jogging is downward.

2. The controller is not generic.

I changed the namespace so it's under /velocity_controllers/TrajOrJogController, i.e. just another generic velocity controller. There's no reason why a position version wouldn't work, too, but it will take some work to blend the commands.

3. It makes more sense to manage controller switching with a separate, multiplexer-like node.

In my opinion that's not true, although I'm glad to have the sample code you pointed me to. That's more complexity and an extra 300+ lines of code. Ideally, you would have only one controller running and it could accept any command type, no switching needed. This moves toward the ideal solution.

[mathias-luedtke]

Thanks for your updates, I will review them later.
Just two comments for now:

Ideally, you would have only one controller running and it could accept any command type, no switching needed. This moves toward the ideal solution.

The switching(=controller managing) is the whole point of ros_control (and its predecessor pr2_mechanism).
Most of the code is dedicated to realtime-safe switching.

although I'm glad to have the sample code you pointed me to

That's no sample code. It's a released, ready-for-use node, which enables switching between trajectory, velocity or position control with a silence filter. I think 300 lines of code are quite fair ;)
In case of Care-O-bot 4, the hardware even switches to different drive modes which have been tuned independently.

[AndyZe]

@ipa-mdl OK, thanks for the comments, although I don't completely agree. I still think the management of controller switching with a separate multiplexer node introduces extra latency and complexity. Might as well put both velocity controller types together since it takes very little effort to do so and the pause during switching will be minimized. Also, why did the authors define so many functions as virtual if they did not intend for these basic controllers to be expanded?

Let me ask a question... I need to implement a compliance controller. It will probably be a node that publishes a vector of joint speeds. These compliance joint speeds should sum with the vector of joint speeds to a JointTrajectoryController or a JointGroupVelocityController. What is your vision of how something like that should be implemented? I am interested in your opinion so I have a shot at getting it merged. Let me know if I need to explain what compliance is.

Some options I can think of:

1. Inherit from JointTrajectoryController and modify it to accept compliance commands. Also inherit from ForwardCommandController and modify it to accept compliance commands. This will result in two new controller types which would need to be switched by the multiplexer.

2. Combine JointTrajectoryController and ForwardCommandController. Inherit from that combined controller and modify it to accept compliance commands. This will result in one new controller type.

Thanks! I'm interested in what @bmagyar has to say, too.

[davetcoleman]

The management of controller switching with a separate multiplexer node introduces extra latency and complexity

@AndyZe could you better motivate why you think that? I'm not sure what I think about this debate between modular controllers vs hybrid controllers that have multiple controllers within them.

[AndyZe]

Let's look at it this way. Who do you think does the best motion control in the world? Boston Dynamics, I would say.

I don't think Boston Dynamics is pausing to load and unload controllers.

[gavanderhoorn]

That's a bit of an argument by authority.