Lock force/compliance controller until a wrench message was recieved on the topic

[firesurfer]

System Info

ros2 iron, binary install from repository

Commit: 03057bd

Problem statement and motivation

I ran into some issues when I forgot to remap the Wrench sensor topic . When setting a Wrench target the controller will move but the action does not make sense.

It would be nice if the controller would not accept targets until it has received at least one message on the wrench topic. In the ideal case it would check if the latest received value is recent enough to be used.

Goal of this feature

Safer behavior of the controller

Approach and next steps

1. Store timestamp of latest received measurement
2. Check it in the update loop
3. If the measurement was to old / none was received yet -> Print error once. Keep current position

[stefanscherzinger]

@firesurfer

Thanks for this feature request! I think I understand the motivation.

When setting a Wrench target the controller will move but the action does not make sense.

It depends. Users might use wrench inputs to steer their robot without having a force torque sensor. I do that frequently with this teleoperation mechanism. That would no longer be possible with your proposed sanity check..

I agree that there's the risk of triggering the robot's protective stop in contact because there's no force sensor signals that balance the target wrenches..

[firesurfer]

I agree that there's the risk of triggering the robot's protective stop in contact because there's no force sensor signals that balance the target wrenches..

Or in our industrial use case it actually has the potential to damage the machine.

I suggest making this check optional by introducing an additional flag in the configuration