[cob_base_velocity_smoother] sawtooth behavior

[fmessmer]

I (still) think this is worth creating an issue: @ipa-bnm @ipa-fmw

When investigating the base command pipeline (twist_mux -> velocity_smoother -> control_plugin -> odometry_plugin), I found the following behavior for the "new" velocity smoother.

Tested with a cpp-node that publishes a sine signal onto a geometry_msgs::Twist topic (see #93)
roslaunch cob_bringup velocity_smoother.launch robot:=cob4-1

100Hz

50 Hz

30Hz

Maybe it's just a matter of improving the configuration....

@fmessmer FYI

[fmessmer]

What it does well is smoothing back to stop in case the publisher stops publishing

30Hz

[fmessmer]

For comparison this is the behavor of the "old" cob_base_velocity_smoother (in it's default configuration):
rosrun cob_base_velocity_smoother cob_base_velocity_smoother

100Hz

50Hz

30Hz

[fmessmer]

What the "old" smoother doesn't do well is smoothing back to 0.0:

[fmessmer]

The "new" smoother's publish rate is set to 100.0 Hz
The "old" smoother's publish rate is set to 30.0 Hz

[fmessmer]

After some discussions with @ipa-bnm and @ipa-mdl, I see the point in the current implementation of cob_base_velocity_smoother as it is not really supposed to "smooth-out" steps/jumps in the raw input signal but limit the input only in case it's surpassing either the velocity limit or the acceleration limit.
Otherwise it tries to follow the input signal as fast as possible - and as soon as the desired (input) velocity is reached keep publishing them.

Quoting @ipa-bnm:

Repeated publishing of the target velocity should not make a difference for the underlying base_twist_controller, as the input for the base_twist_controller would be the same if only published once...only that with the cob_base_velocity_smoother velocity and acceleration limits can be guaranteed on the Cartesian velocity!

If a real smoothing is desired/needed, the only possible way would be to introduce some lag, e.g. PT1 controller, as the "direction" of the input cannot be predicted.

Still, https://github.com/ipa320/cob_control/pull/133 introduces some cleanups and consitency fixes for the cob_base_velocity_smoother

[fmessmer]

Basically the question is:
Are we ok with the described non-continuous "sawtooth" behaviour in the input signal of the base_twist_controller?
The cob_base_velocity_smoother then would rather act as a limiter interface operating on the Cartesian velocity level.
Maybe we could add this input limiting (i.e. acceleration limiting) to the base_twist_controller directly - saving one subscribe-publish-cycle

Or do we need something continuous, i.e. "stetig", as input for the base_twist_controller?
I guess atm only input Cartesian and output joint velocities are limitable in base_twist_controller.

[mathias-luedtke]

I guess atm only input Cartesian and output joint velocities are limitable in base_twist_controller.

Only output velocities get limited.
Drive velolcity by velocity limit, steer velocity by velocity and acceleration limit.

Are we ok with the described non-continuous "sawtooth" behaviour in the input signal of the base_twist_controller?

Can you prepare plots of the controllers response?

[fmessmer]

I think we definitely need to solve this together with "tuning" the configuration of the base_twist_controller...
I will prepare configurability (see https://github.com/ipa320/cob_control/issues/91) and plots tomorrow...

[fmessmer]

Only output velocities get limited.
Drive velocity by velocity limit, steer velocity by velocity and acceleration limit.

@ipa-mdl @ipa-mig @ipa-bnm FYI

Here is some information about limiting I found in the base_twist_controller:

• limitValue function is used throughout all the following limiting...
  It's "croping" the value to the interval [-limit, limit] in case limit!=0.
• Input Cartesian Velocities
  • translational velocity, i.e. twist.linear.x, twist.linear.y is limited here using max_trans_velocity which defaults to 0.0, i.e. unlimited (currently this parameter is commented in the base_controller.yaml
  • rotational velocity, i.e. twist.angular.z is limited here using max_rot_velocity which defaults to 0.0, i.e. unlimited (currently this parameter is commented in the base_controller.yaml
  • Note that the defaults for max_trans_velocity and max_rot_velocity are different in old undercarriage_ctrl and new undercarriage_ctrl_node
• Output Joint Velocities/Acceleration
  • steer/drive velocity is limited here using max_steer_rate/max_drive_rate which defaults to 0.0, i.e. unlimited (currently this parameter is not set in the base_controller.yaml
  • steer velocity is also limited limited here using d_phi_max which is required to be set and currently set to 10.0 in the [base_controller.yaml]https://github.com/ipa320/cob_robots/blob/indigo_dev/cob_hardware_config/cob4-2/config/base_controller.yaml#L22)
  • steer acceleration is limited here using dd_phi_max which is required to be set and currently set to 40.0 in the [base_controller.yaml]https://github.com/ipa320/cob_robots/blob/indigo_dev/cob_hardware_config/cob4-2/config/base_controller.yaml#L23)

[mathias-luedtke]

Not sure why we have d_phi_max AND max_steer_rate..

[fmessmer]

Here is a comparison of using the cob_base_velocity_smoother vs. not using the cob_base_velocity_smoother (ipa320/indigo_dev implementation/configuration):

1. Step Linear 10Hz (wheels oriented in rotational direction) published to base_twist_controller directly
   
2. Step Linear 10Hz (wheels oriented in rotational direction) published to velocity_smoother 50Hz
   
3. Step Rotation 10Hz (wheels oriented in linear direction) published to base_twist_controller directly
   
4. Step Rotation 10Hz (wheels oriented in linear direction) published to velocity_smoother 50Hz
   

From the plots, it seems that the smoother rather "slows down" the controller, for one due the additional pub-sub-cycle for another due to acceleration limiting, i.e. the ramp of the odometry is steeper, i.e. more reactive, without the smoother...and it seems that this higher ramp is allowed in the base_twist_controller...

But we need to repeat this experiment on real hardware