# TrajectoryLimiters

Contains an implementation of

Nonlinear filters for the generation of smooth trajectories R. Zanasi, C. Guarino Lo Bianco, A. Tonielli

This nonlinear trajectory filter takes a pre-defined reference trajectory

What is this good for? Some applications call for a dynamically feasible reference trajectory, i.e., a trajectory with bounded velocity and acceleration, but all you have access to is an instantaneous reference

## Usage

To filter an entire trajectory, create a `TrajectoryLimiter`

and call it like a function:

```
using TrajectoryLimiters
ẍM = 50 # Maximum acceleration
ẋM = 10 # Maximum velocity
Ts = 0.005 # Sample time
r(t) = 2.5 + 3 * (t - floor(t)) # Reference to be smoothed
t = 0:Ts:3 # Time vector
R = r.(t) # An array of sampled position references
limiter = TrajectoryLimiter(Ts, ẋM, ẍM)
X, Ẋ, Ẍ = limiter(R)
plot(
t,
[X Ẋ Ẍ],
plotu = true,
c = :black,
title = ["Position \$x(t)\$" "Velocity \$ẋ(t)\$" "Acceleration \$u(t)\$"],
ylabel = "",
layout = (3,1),
)
plot!(r, extrema(t)..., sp = 1, lab = "", l = (:black, :dashdot))
```

The figure above reproduces figure 10 from the reference, except that we did not increase the acceleration bound (which we call

The figure indicates that the limited (solid lines) trajectory follows the original reference trajectory (dashed line) whenever possible, but deviates whenever the original trajectory violates the velocity or acceleration constraints. When it has deviated, the limited trajectory converges to the original reference trajectory again with a time-optimal behavior whenever the velocity and acceleration profiles allow.

Since the trajectory limiter outputs position, velocity and acceleration, it is easy to use inverse-based feedforward models to improve the trajectory tracking compared to purely feedback-based controllers (*always* use some form of feedforward if trajectory-tracking performance is important).

To limit a trajectory online, i.e., one step at a time, call the limiter like so

`state, ẍ = limiter(state, r(t))`

this outputs a new state, containing

One can also call the lower-level function

`state, ẍ = TrajectoryLimiter.trajlim(state, rt, Ts, ẋM, ẍM)`

directly in case one would like to change any of the parameters online.

To set the initial state of the trajectory limiter, create a

`TrajectoryLimiters.State(x, ẋ, r, ṙ)`

manually. The default choice if no initial state is given when batch filtering an array `R`

is `TrajectoryLimiters.State(0, 0, r, 0)`

where `r`

is the first value in the array `R`

.

### Performance

On a laptop from 2021, filtering a trajectory `R`

of length 601 samples takes

```
julia> length(R)
601
julia> @btime $limiter($R);
23.745 μs (3 allocations: 14.62 KiB)
```

With preallocated output arrays, you can avoid the allocations completely:

```
julia> X, Ẋ, Ẍ = similar.((R,R,R));
julia> @btime $limiter($X, $Ẋ, $Ẍ, $R);
20.813 μs (0 allocations: 0 bytes)
```

Taking a single step takes

```
julia> @btime $limiter(TrajectoryLimiters.State(0.0), 0.0);
17.372 ns (0 allocations: 0 bytes)
```