Safety feedback and cobots

[simonschmeisser]

We're currently discussing internally how to integrate external safety features with MoveIt. What we aim for is a very basic integration of area sensors that report something along the lines of 100% 50% 10% 0% depending on proximity of humans. The sensor is of course directly coupled (PROFIsafe or similar) to the hardware and MoveIt/ROS is just an observer.

This could be handled by having TrajectoryExecutionManager listen to a new topic with such information. I'm wondering if this has been done previously and if there are message types defined somewhere already?

The message would include

• int safety_state
• double resulting velocity override

@gavanderhoorn do you know of previous efforts? Should this be included in industrial_msgs::RobotStatus? (I personally would rather have it separately)

[v4hn]

What features would you like to retain in MoveIt if the hardware driver takes care of the slowdown? Essentially, if you disable duration monitoring, there is no need to modify MoveIt at all. Trajectories will simply take longer than anticipated. If you want to have online path validation, you would have to extend the monitor. The two options I see: - you can monitor the safety state and update the expected runtime of the remaining trajectory. You are probably already aware of https://github.com/ros-industrial/industrial_core/tree/kinetic-devel/industrial_msgs These messages do not seem to contain information like that yet, but I would expect it there. This approach will end up much more fragile than the current time-based solution because delays can accumulate and you might have to model the dynamic response of the controller to changes in safety state if you want to stay close to the real trajectory. - You can directly monitor joint states and map them to the executing trajectory by hand. This also allows to verify execution tolerances. Personally, I would like to see an upstream patch for the second option. 😎

[simonschmeisser]

• You can directly monitor joint states and map them to the executing trajectory by hand. This also allows to verify execution tolerances.

Thanks for your feedback @v4hn , can you elaborate a bit more?

Turning off the duration monitoring would unfortunately mean that our application "hangs" as there are multiple components without a feedback channel. (E.g. upload to robot failed, robot rejected trajectory, ...) We should modify our application such that one can still cancel such trajectories but we also need to notify higher level system or maintenance personal in case of errors.

[v4hn]

You can directly monitor joint states and map them to the executing trajectory by hand. This also allows to verify execution tolerances.

can you elaborate a bit more?

You send trajectories to the controller and expect it to follow that trajectory in joint-space, though the time profile can change from the computed one.
For this use case, you can monitor the joint_state topic and try to infer the next upcoming waypoint from the current state. This would enable a much more reliable estimate of the current state of execution, in terms of progress through the trajectory.
The current default estimation is entirely based on the time since execution started.

Turning off the duration monitoring would unfortunately mean that our application "hangs" as there are multiple components without a feedback channel.

Does that mean all these components currently only rely on the time since you sent the trajectory for execution? I really would not build on that value at all...

(E.g. upload to robot failed, robot rejected trajectory, ...)

These things should be reported by the TrajectoryExecutionAction itself. This feedback is independent of duration monitoring.

We should modify our application such that one can still cancel such trajectories

If "such" refers to cases of action failure: this should already be the case?
If "such" refers to trajectories with very long execution times due to the safety states:
It can make a lot of sense to provide information on safety states via a topic and have higher-level logic subscribe to that topic if a change of safety state can change the system behavior and not just its time scaling.
If you want to reason about the remaining execution time in MoveIt though, I would monitor joint states, as discussed above, and introduce triggers if external time limits are exceeded.

[simonschmeisser]

See ros-industrial/industrial_core#187 and ros-industrial/industrial_core#131 and a few others for examples where the action is not cancelled cleanly

[v4hn]

You are pointing to a number of actual issues in driver implementations that should really be fixed! They are really old too, wow...

Of course you can build a monitoring system around the whole system to validate its behavior, but that's (theoretically) not the idea of the functional frameworks.
Monitoring shouldn't affect behavior during normal operation.

If I understand your intention, you are basically looking for a way to extend the monitoring system you already have (duration-based) to support safety states and dynamic external velocity scaling.

I would still argue that this dynamic component justifies joint state monitoring.
It also means you can define better criteria for monitoring alerts/failures, like

• some epsilon after sending the trajectory, the joints should start moving and have velocities going in the correct direction
• if the next way point is not passed within a maximum time, defined as either some (rather high) constant times the expected duration for the trajectory segment, report stuck
  Alternatively, the same check could use the reported safety state factor and a smaller constant >1 to account for delays and dynamic effects
• if the last way point is not reached up to some accuracy at the end, report failure to reach goal