Sudden stop for critical issues, filtered deceleration otherwise

[AndyZe]

Description

A suddenHalt() function for critical issues like joint limits. Otherwise, decelerations are low-pass filtered.

Checklist

• Required by CI: Code is auto formatted using clang-format

[AndyZe]

@jschleicher give it a review, if you please

[jschleicher]

What about the check in the main loop, if a zero velocity command is received? https://github.com/ros-planning/moveit/blob/7f142291ccd8c7f203a4427e6353e3a950d09314/moveit_experimental/jog_arm/src/jog_arm/jog_calcs.cpp#L135

Recording a bag file shows several unexpected behaviours:

1. while filtering looks good for negative joint velocities, it is still hard-reset to zero for positive velocities:
   

2. during stop sometimes there is an overshoot with movement into the opposite direction:
   

[AndyZe]

Good catch on resetVelocityFilters() being out-of-place.

After moving it to a better spot, I'm not seeing anything unexpected from the filters. There is a tiny bit of overshoot at low_pass_filter_coefficient == 1. You can damp that down with a higher low_pass_filter_coefficient, if you want (compare 4th plot to 3rd plot).

I just discovered PlotJuggler for capturing this data easily. It's fantastic.

[AndyZe]

The filter type used here is known to overshoot. It could be simplified to a first-order filter that apparently doesn't have overshoot.

https://dankelley.github.io/r/2014/01/15/butterworth-filters.html

[SansoneG]

Maybe a new ros parameter could be used in the config.yaml to select the order of the butterworth filter that should be used.

[AndyZe]

I'll make an issue for that.

[jschleicher]

Just a pedantic comment: If the incoming command_timeout is lower than the time to reach zero velocity, the robot keeps moving now:

But a comment to increase the incoming_command_timeout for very high low_pass_filter_coeff (we have tested with low_pass_filter_coeff=30) in the documentation would be fine for me.

[AndyZe]

If I understand right, the last velocity command is sometimes non-zero if incoming_command_timeout is really short.

Here's a fix for that. (I did test it)

[jschleicher]

If I understand right, the last velocity command is sometimes non-zero if incoming_command_timeout is really short.

Exactly. Your fix works for me.

[AndyZe]

@henningkayser @BryceStevenWilley @felixvd Can I get a review please? Not sure where my 'Assign Reviewers' button went

[BryceStevenWilley]

LGTM, though this isn't my expertise.

[jschleicher]

@henningkayser @felixvd Ping

I'd appreciate, if this could be merged.

[rhaschke]

Approving formally, assuming that functionality was approved by @jschleicher.