Fix jerk limiter in rate_limiter

[tpoignonec]

Note

This is a duplicate of a now closed PR against ros-controls/ros2_controllers (see PR #1390) that was initially intended for the diff_drive_controller.
I was asked to move it here since the rate_limiter of the diff_drive_controller will be that of the control_toolbox (see ros2_controllers, PR #1315).

Hi,

There seem to be an error in the jerk limiter (speed_limiter.hpp #L290-L311) caused by the (as far as I can tell) incorrect factor 2 in the computation of the velocity limit resulting from jerk limits.

The consequence is an overshoot of the configured jerk limit.
To illustrate the effect of this factor, let us set the linear jerk limit as $J_{max} = 20 \text{ m.s}^{-3}$.
Before the changes, the jerk reaches $40 \text{ m.s}^{-3}$ (i.e., 2 times the configured limit):

After, it is fine

[mergify]

@tpoignonec, all pull requests must be targeted towards the master development branch.
Once merged into master, it is possible to backport to ros2-master, but it must be in master
to have these changes reflected into new distributions.

[christophfroehlich]

TBH, I just have added tests for it and haven't thought about that.
From https://en.wikipedia.org/wiki/Finite_difference

$$f''(x) \approx \frac{\nabla_h^2[f](x)}{h^2} = \frac{ \frac{f(x) - f(x-h)}{h} - \frac{f(x-h) - f(x-2h)}{h} }{h} = \frac{f(x) - 2 f(x-h) + f(x - 2h)}{h^{2}}$$

should be what you have implemented now.

Why is the jerk limit not applied when it reaches the target velocity?
And by plotting something similar to your step with the attached python script, this will oscillate if there is an unfortunate divisor of dt and the limits?

import numpy as np
import matplotlib.pyplot as plt

# Inputs
v_in = np.concatenate([np.zeros(10), np.ones(200)])  # Equivalent to [zeros(1,10), ones(1,100)]
v_out = np.zeros_like(v_in)  # Initialize output array to zeros
dt = 0.01  # Time step
t = np.arange(dt, dt * len(v_in) + dt, dt)  # Time array

# Constraints
a_max = 2.0
a_min = -5.0
j_max = 20.0
j_min = -20.0

# Constrain second derivative and velocity in the loop
for i in range(2, len(v_in)):
    v = v_in[i]
    v0 = v_out[i - 1]
    v1 = v_out[i - 2]

    # Constrain second derivative
    dv = v - v0
    dv0 = v0 - v1
    da = np.clip(dv - dv0, j_min * dt**2, j_max * dt**2)
    v = v0 + dv0 + da

    # Constrain first derivative
    v_out[i] = v0 + np.clip(v - v0, a_min * dt, a_max * dt)

# Plotting results
plt.figure(figsize=(10, 8))

# Plot v_in and v_out
plt.subplot(311)
plt.step(t, v_in, label='v_in')
plt.step(t, v_out, label='v_out')
plt.legend()
plt.title("v_in and v_out")
plt.grid()

# Plot first derivative (velocity)
plt.subplot(312)
plt.step(t[1:], np.diff(v_out) / dt, label='Velocity (First Derivative)')
plt.title("First Derivative of v_out")
plt.grid()

# Plot second derivative (acceleration)
plt.subplot(313)
plt.step(t[2:], np.diff(np.diff(v_out) / dt) / dt, label='Acceleration (Second Derivative)')
plt.title("Second Derivative of v_out")
plt.grid()

plt.tight_layout()
plt.show()

[tpoignonec]

The jerk limit is not applied in my case due to the velocity clipping being called last (with $v_{max} = 1.0$).

For the oscillatory behavior, that was already the case before.
Since it is a rate limiter and not an online trajectory generation, the velocity rate only changes when we reach the target.
At this point, it is already too late to get a smooth convergence without oscillation, at least for tight jerk limits.

I'll have a look at this tomorrow, but I doubt there is an easy fix :/

[christophfroehlich]

I'm not sure why it is not considered in your case, I don't get the argument. Your input is v=1?

You are right about the oscillatory behavior being present already on the master branch. I just never had thought about that. And I agree that there would not be an easy fix, because we would need to predict into the future..

[tpoignonec]

For context, I started looking at the implementation to check in what order the three clipping (vel / acc / jerk) should be done.
The jerk limit is not applied when the max or min velocity is reached, because the jerk is limited first and the velocity clipping overrides it later:

  limit_second_derivative(v, v0, v1, dt);
  limit_first_derivative(v, v0, dt);
  limit_value(v);

Normal case where the velocity is not reached:

The case where it is reached:

My bad I forgot to remove this part.

Conversely, we could reorder the clipping to maintain consistency:

  limit_value(v);
  limit_first_derivative(v, v0, dt);
  limit_second_derivative(v, v0, v1, dt);

[tpoignonec]

OK, so it would seem that there are 3 main options as follows.

1. remove jerk limits or ignore them (i.e., implement OTG in higher level controller. Or simply ignore jerk limits when decelerating?
2. keep the jerk limit, but check that it won't result in weird behaviors (tricky, because this will depend on the value of dt...)
3. modify the algorithm to take jerk limit into account while guaranteeing first-order closed loop response. In that case, it might be best to go all the way and do actual trajectory generation though.
   a) Using filter-like structure with a feedforward term. I found this publication TOMBUL 2021 that does something along these lines, although it does not seem to work (see bellow). There are probably better references about this topic.
   b) Cascading a velocity and/or acc limiter with a low-pass filter (see baggepinnen/TrajectoryLimiters.jl for instance)

Anything but (1) might be out of the scope of this PR thought.

Test with a feedforward term

In TOMBUL 2021 they limit the variation of a position variable:

with $\Gamma = \frac{a_{max}}{v_{max}}$ a time-varying gain for the feedforward term.

Here is a Python implementation

import matplotlib.pyplot as plt
import numpy as np

# Parameters
dt = 0.02  # Time step
time = np.arange(0, 4, dt)  # Total time

# Rate limits
v_max = 2.0  # Maximum velocity
a_max = 10.0  # Maximum acceleration
j_max = 100.0  # Maximum jerk

USE_FF = True

v_target = np.zeros_like(time)
v_target[int(0.5 / dt) : int(2 / dt)] = 1.5
v_target[int(2 / dt):] = -1.5

# Velocity, acceleration, and jerk profiles
velocity = np.zeros_like(time)
acceleration = np.zeros_like(time)
jerk = np.zeros_like(time)

# Initial velocity
velocity[0] = 0.0

# Useful variables
v_lim_v = velocity[0]

v_lim_a = velocity[0]
a_lim_a = 0.0

v_lim_j = velocity[0]
a_lim_j = 0.0
j_lim_j = 0.0

v_lim_v_log = np.zeros_like(time)
v_lim_a_log = np.zeros_like(time)

a_lim_a_log = np.zeros_like(time)

for i in range(1, len(time)):
    # Clip velocity
    v_lim_v = np.clip(v_target[i], - v_max, v_max)

    # First order limit
    a_target = (v_lim_v - v_lim_a) / dt
    a_lim_a = np.clip(a_target, - a_max, a_max)
    v_lim_a += a_lim_a * dt

    # Second order limit
    j_ff_term = 0.0
    if USE_FF:
        # Note:
        # In the paper above Eq. (5), they do something equivalent to
        #   "j_ff_term = (v_lim_v - v_lim_j) * j_max / a_max"
        # However, that does not work at all (e.g., undefined when there is not jerk limit, AKA j_max = inf)
        # Or maybe I did not implement this correctly...
        ## -> Modified so as to work
        j_ff_term = (v_lim_v - v_lim_j) * (a_max / j_max)

    j_target = (a_lim_a - a_lim_j + j_ff_term) / dt
    j_lim_j = np.clip(j_target, - j_max, j_max)
    a_lim_j += j_lim_j * dt
    v_lim_j += a_lim_j * dt

    # Store state
    jerk[i] = j_lim_j
    acceleration[i] = a_lim_j
    velocity[i] = v_lim_j

    v_lim_v_log[i] = v_lim_v
    v_lim_a_log[i] = v_lim_a
    a_lim_a_log[i] = a_lim_a

# Plot the results
plt.figure(figsize=(10, 8))
linestyle = "-"

# Plot velocity
plt.subplot(411)
plt.step(time, v_target, label="Velocity Target", color="red")
# plt.step(time, v_lim_v_log, '-', label="v_lim_v", color="blue")
# plt.step(time, v_lim_a_log, '-', label="v_lim_a", color="orange")
plt.step(time, velocity, linestyle, label="Velocity", color="black")
plt.axhline(v_max, color="black", linestyle="--", label="Velocity Limit")
plt.axhline(-v_max, color="black", linestyle="--", label="_Velocity Limit")


plt.ylabel("Velocity (m/s)")
plt.legend()
plt.grid(True)

# Plot velocity errors
plt.subplot(412)
# plt.step(time, v_target - v_lim_v_log, '-', label="v_lim_v err.", color="blue")
# plt.step(time, v_target - v_lim_a_log, '-', label="v_lim_a err.", color="orange")
plt.step(time, v_target - velocity, linestyle, label="Velocity err.", color="black")

plt.ylabel("Velocity error (m/s)")
plt.legend()
plt.grid(True)

# Plot acceleration
plt.subplot(413)
plt.step(time, acceleration, linestyle, label="Acceleration", color="black")
plt.axhline(a_max, color="black", linestyle="--", label="Accel Limit")
plt.axhline(-a_max, color="black", linestyle="--", label="_Accel Limit")
# plt.step(time, a_lim_a_log, ':', label="a_lim_a", color="blue")
plt.ylabel("Acceleration (m/s²)")
plt.legend()
plt.grid(True)

# Plot jerk
plt.subplot(414)
plt.step(time, jerk, label="Jerk", color="black")
plt.axhline(j_max, color="black", linestyle="--", label="Jerk Limit")
plt.axhline(-j_max, color="black", linestyle="--", label="_Jerk Limit")
plt.xlabel("Time (s)")
plt.ylabel("Jerk (m/s³)")
plt.legend()
plt.grid(True)

plt.tight_layout()
plt.show()

It is promising, although there is a residual error in the velocity tracking.
This error is dependent on the ratio $j_{max} / a_{max}$ and on the sampling period $dt$.

[christophfroehlich]

Thanks for the nice writeup. I would not try to implement something more sophisticated here.
Instead, let us change it to 1b): Apply jerk limits only if jerk and acceleration are in the same direction, i.e., acceleration*jerk>0?

    if (dv - dv0) * (v-v0) > 0:
        da = np.clip(dv - dv0, j_min * dt**2, j_max * dt**2)
        v = v0 + dv0 + da

And we should add this to a documentation somewhere, at least in the docstring of the class.

[codecov-commenter]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 71.05%. Comparing base (00ec401) to head (c467026).
Report is 1 commits behind head on ros2-master.

Additional details and impacted files

@@             Coverage Diff              @@
##           ros2-master     #240   +/-   ##
============================================
  Coverage        71.05%   71.05%           
============================================
  Files               20       20           
  Lines             1040     1040           
  Branches            84       84           
============================================
  Hits               739      739           
  Misses             255      255           
  Partials            46       46           

Flag	Coverage Δ
unittests	71.05% <100.00%> (ø)

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
include/control_toolbox/rate_limiter.hpp	68.25% <ø> (ø)
test/rate_limiter.cpp	100.00% <100.00%> (ø)

[tpoignonec]

Thanks for the nice writeup. I would not try to implement something more sophisticated here. Instead, let us change it to 1b): Apply jerk limits only if jerk and acceleration are in the same direction, i.e., acceleration*jerk>0?

    if (dv - dv0) * (v-v0) > 0:
        da = np.clip(dv - dv0, j_min * dt**2, j_max * dt**2)
        v = v0 + dv0 + da

And we should add this to a documentation somewhere, at least in the docstring of the class.

That sounds great!
Haven't had time to look at this today, but I'll test ASAP and add it to the PR.

[tpoignonec]

I added the acceleration condition on the jerk limiter to the PR.
Also, I reverted the limiter order commit, as it is less relevant now that the jerk problem is solved.

In the simulation with

def limit_jerk(v, v_0, v_1, j_min, j_max):
    dv = v - v_0
    dv_0 = v_0 - v_1

    if (dv - dv_0) * (v - v_0) > 0:
        dt2 = dt**2
        da_min = j_min * dt2
        da_max = j_max * dt2
        da = np.clip(dv - dv_0, da_min, da_max)
        return v_0 + dv_0 + da
    else:
        return v

[tpoignonec]

My only remaining concern at this point is the robustness to noise.

For instance, even small noise on the command signal is enough to make the close-loop system oscillate (a bit, but still) as it was before:

For reference, that is the same plot, but without a jerk limiter:

@christophfroehlich What do you think? Is the current state of this PR sufficient to close, maybe with a "be careful with jerk limits, only use at your own risk" type of disclaimer?

The python script to generate the figures (toggle USE_JERK_LIMIT True / False)

import matplotlib.pyplot as plt
import numpy as np
from copy import deepcopy

USE_JERK_LIMIT = True

ADD_COMMAND_NOISE = True
CMD_NOISE_STD = 0.05
np.random.seed(0)

# Parameters
dt = 0.02  # Time step
time = np.arange(0, 8, dt)  # Total time

# Target velocity
v_target = np.zeros_like(time)
v_target[int(2 / dt) : int(6 / dt)] = 1.0

# Rate limits
#   --> Maximum velocity
v_max = 1.5
v_min = -v_max
#   --> Maximum acceleration
a_max = 5.0
a_min = -a_max
#   --> Maximum jerk
j_max = 50.0
j_min = -j_max


def limit_velocity(v, v_min, v_max):
    return np.clip(v, v_min, v_max)


def limit_acceleration(v, v_0, a_min, a_max):
    dv_min = a_min * dt
    dv_max = a_max * dt
    dv = np.clip(v - v_0, dv_min, dv_max)
    return v_0 + dv


def limit_jerk(v, v_0, v_1, j_min, j_max):
    dv = v - v_0
    dv_0 = v_0 - v_1

    if (dv - dv_0) * (v - v_0) > 0:
        dt2 = dt**2
        da_min = j_min * dt2
        da_max = j_max * dt2
        da = np.clip(dv - dv_0, da_min, da_max)
        return v_0 + dv_0 + da
    else:
        return v


# Velocity, acceleration, and jerk profiles
position = np.zeros_like(time)
velocity = np.zeros_like(time)
acceleration = np.zeros_like(time)
jerk = np.zeros_like(time)

# Simulation
v_initial = 0  # m/s
v_k_1 = 0.0  # v[k-1]
v_k_2 = 0.0  # v[k-2]
a_k_1 = 0.0  # a[k-1]

for i in range(1, len(time)):
    # Ideal velocity without limits
    if ADD_COMMAND_NOISE:
        v_target[i] += np.random.normal(0, CMD_NOISE_STD)
    v_new = deepcopy(v_target[i])

    # Apply jerk limits
    if USE_JERK_LIMIT:
        v_new = limit_jerk(v_new, v_k_1, v_k_2, -j_max, j_max)

    # Apply acceleration limits
    v_new = limit_acceleration(v_new, v_k_1, -a_max, a_max)

    # Apply velocity limits
    v_new = limit_velocity(v_new, -v_max, v_max)

    # Store values for plot
    position[i] = position[i - 1] + v_new * dt
    velocity[i] = deepcopy(v_new)
    acceleration[i] = (v_new - v_k_1) / dt
    jerk[i] = (acceleration[i] - a_k_1) / dt

    # Update previous values
    v_k_2 = deepcopy(v_k_1)
    v_k_1 = deepcopy(v_new)
    a_k_1 = deepcopy(acceleration[i])

# Plot the results
fig, axs = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
linestyle = "-"

# Plot velocity
axs[0].step(time, velocity, linestyle, label="Velocity", color="black")
axs[0].step(time, v_target, label="Velocity Target", color="red")
axs[0].axhline(v_max, color="black", linestyle="--", label="Velocity Limit")
axs[0].axhline(-v_max, color="black", linestyle="--", label="_Velocity Limit")
axs[0].set_ylabel("Velocity (m/s)")
axs[0].legend()
axs[0].grid(True)

# Plot acceleration
axs[1].step(time, acceleration, linestyle, label="Acceleration", color="black")
axs[1].axhline(a_max, color="black", linestyle="--", label="Accel Limit")
axs[1].axhline(a_min, color="black", linestyle="--", label="_Accel Limit")
axs[1].set_ylabel("Acceleration (m/s²)")
axs[1].legend()
axs[1].grid(True)

# Plot jerk
axs[2].step(time, jerk, label="Jerk", color="black")
axs[2].axhline(j_max, color="black", linestyle="--", label="Jerk Limit")
axs[2].axhline(j_min, color="black", linestyle="--", label="_Jerk Limit")
axs[2].set_xlabel("Time (s)")
axs[2].set_ylabel("Jerk (m/s³)")
axs[2].legend()
axs[2].grid(True)

plt.tight_layout()
plt.show()

[christophfroehlich]

I don't think that the noisy example is something that we need take much care about. Until now, this was only used for input filtering of the diff_drive_controller, and the twist typically is generated offline from a path planner -> no sensor feedback -> no noise.

But please add the comment you added in the code also to the docstring of the constructor and limit_second_der methods:

https://github.com/ros-controls/control_toolbox/blob/ros2-master/include/control_toolbox/rate_limiter.hpp#L46-L49
https://github.com/ros-controls/control_toolbox/blob/ros2-master/include/control_toolbox/rate_limiter.hpp#L96