Setting command to feedback in controller before enabling drives

[John Morris]

We're using the `joint_trajectory_controller`.  I'm trying to figure
out how to handle cases where the robot is disabled, and commanded
position from the controller is different from the actual robot
position, such as when hitting e-stop in mid-trajectory or updating
joint offsets while mastering.  Unless commanded position is reset to
feedback position in the controller, the robot will make an
undesirable move to commanded position when the robot is reenabled.

It looks like stopping and starting the trajectory controller does the
trick with the following commands; afterwards, command is reset to
feedback and there's no motion when enabling the robot:

    rosrun controller_manager controller_manager stop position_trajectory_controller
    rosrun controller_manager controller_manager start position_trajectory_controller

IIUC, these are calling the controller's `stopping()` and `starting()`
methods, described respectively as "Cancels the active action goal, if
any" and "Holds the current position" in
`joint_trajectory_controller/joint_trajectory_controller.h`.

My first idea was to call these from within the RT controller update
loop when the hardware sees the drives go online or offline.  This
doesn't require external coordination with the higher-level
application to respond correctly to lower-level events coming from the
hardware.  However, the closest interface to these exposed from the
`controller_manager::ControllerManager` object appears to be
`update([...], reset_controllers=true)`, a bit blunt since it calls
both `stopping()` and `starting()` for all controllers.

If that `reset_controllers` argument is the best way to go, how is it
typically used?  Could I set it `true` at each falling and rising edge
of e.g. an `enable` signal?

Or should I perhaps be thinking about this in a completely different
way?  For example, we'll also have a simple python "drive_manager" ROS
node that connects down to the hardware and provides a higher-level
interface up to the application for reporting status of the drive,
e-stop, etc., and for servicing requests to enable/disable drives, set
offsets, etc.; would this be a typical place to stop and start
controllers?

Thanks-

    John

[Denis Štogl]

Hello John,

 

I would try do to the following. In your launch file you can just load controller and not start them. Then you start the controller manually with your application. Then you should get correct data in the controller.

 

Would this be sensible approach for your application? A one drawback is that you always need to enable controllers "manually". Nevertheless this would be more classical approach with robot controllers where you need to confirm starting of your application/program.

 

P.S. I didn't test this on the real robot, so it can be I missed something.

 

Best regards

 

Denis

> John
--

Karlsruher Institut für Technologie (KIT)
Institut für Anthropomatik und Robotik (IAR) – Intelligente Prozessautomation und Robotik (IPR)

 

Denis Štogl, M.Sc.

Wissenschaftlicher Mitarbeiter

Intelligente Industrieroboter Gruppe (IIROB)

Engler-Bunte-Ring 8,

Gebäude 40.28, Raum 002.1

76131 Karlsruhe

Tel: +49 721 608-46903

Fax: +49 721 608-47141

E-Mail: denis...@kit.edu
Web: rob.ipr.kit.edu

KIT – Die Forschungsuniversität in der Helmholtz-Gemeinschaft

Das KIT ist seit 2010 als familiengerechte Hochschule zertifiziert.

[John Morris]

On Thursday, January 10, 2019 at 4:32:44 PM UTC+8, Denis Štogl wrote:

 

I would try do to the following. In your launch file you can just load controller and not start them. Then you start the controller manually with your application. Then you should get correct data in the controller.

Thanks, an interesting idea.  The specific cases we know of so far when we want to reset command to feedback probably all require application intervention anyway, so this could make sense.

In the meantime, I've been experimenting with `update([...], reset_controllers=true)`, and am able to reset command from within the ROS control loop.  I programmed the PLC (the Machinekit ROS control project mentioned in a previous post) to reset the controllers once whenever (enable == false and cmd != fb and velocity ~= 0).  While this feels like a hack for testing purposes, it does accomplish what we need for the moment.

    John