## Is your feature request related to a problem? Please describe.
Trapezoid pro…files assume the available acceleration is constant for every velocity. For a DC motor, the available acceleration decreases linearly with the velocity. This means that for higher profile max velocities, the max acceleration must be set more conservatively to make it achievable. This wastes performance at the lower end of the velocity profile.
## Describe the solution you'd like
DC motor dynamics are exponential in the time domain, so an exponential velocity profile would be a much better fit. The constructor inputs would be Kv and Ka system ID constants and a max voltage (ideally below 12 V so the feedback controller can react to disturbances). The calculate() inputs would be the current state (position and velocity), unconstrained goal state, and the timestep duration. The calculate() output would be a voltage.
## Implementation details
This class would essentially solve a DC motor minimum time optimization problem. Simulating a 2x1 LinearSystem forward isn't enough because the switching points in the phase space need to be determined to make it speed up and slow down at the right times.
### Possible solution: Dynamic programming
The minimum time problem for a double integrator (position and velocity states, acceleration input, no velocity decay) can be solved with dynamic programming like so.
```python
#!/usr/bin/env python3
import matplotlib.pyplot as plt
import numpy as np
import numpy.typing as npt
from typing import List
def plot_data(
times: List[float],
data: npt.NDArray[np.float64],
labels: List[str],
units: List[str],
) -> None:
"""Plots data (e.g., states, inputs) in time domain with one figure per
unit.
Keyword arguments:
times -- list of times
data -- matrix of data (states or inputs x times)
labels -- list of data label strings
units -- list of data unit strings
"""
# Build mapping from unit to data that have that unit
unit_to_data = {}
for i, unit in enumerate(units):
try:
unit_to_data[unit].append(i)
except KeyError:
unit_to_data[unit] = [i]
for unit, indices in unit_to_data.items():
plt.figure()
plt.title(f"{unit[:unit.find('(')].rstrip()} vs Time")
plt.xlabel("Time (s)")
plt.ylabel(unit)
for i in indices:
if len(data.shape) > 1:
d = data[i, :]
else:
d = data
if len(indices) > 1:
plt.plot(times, d, label=labels[i])
else:
plt.plot(times, d)
if len(indices) > 1:
plt.legend()
class TrapezoidProfileFilter:
"""A time optimal controller for a double integrator.
This is essentially a trapezoid profile parameterized by state instead of by
time.
https://underactuated.mit.edu/dp.html#minimum_time_double_integrator
"""
class Constraints:
def __init__(self, max_velocity: float = None, max_acceleration: float = None):
if max_velocity != None:
self.max_velocity = max_velocity
else:
self.max_velocity = 0.0
if max_acceleration != None:
self.max_acceleration = max_acceleration
else:
self.max_acceleration = 0.0
class State:
def __init__(self, position: float = None, velocity: float = None):
if position != None:
self.position = position
else:
self.position = 0.0
if velocity != None:
self.velocity = velocity
else:
self.velocity = 0.0
def __init__(self, constraints: Constraints):
"""Construct a TrapezoidProfileFilter.
Keyword arguments:
constraints -- the constraints on the profile, like maximum velocity
"""
self.constraints = constraints
def calculate(self, state: State, goal: State, dt: float):
"""Returns the next acceleration of the double integrator.
Keyword arguments:
state -- the current position and velocity
goal -- the desired position and velocity
dt -- timestep duration in seconds
"""
q = state.position
q_ref = goal.position
qdot = state.velocity
qdot_ref = goal.velocity
u_max = self.constraints.max_acceleration
# If the position and velocity are within the distance and velocity the
# controller can travel in one timestep respectively, turn the
# controller off
if (
abs(q_ref - q) <= 0.5 * u_max * dt**2
and abs(qdot_ref - qdot) <= u_max * dt
):
return TrapezoidProfileFilter.State(q_ref, qdot_ref)
# c₊ = qᵣ − 0.5 / u_max q̇ᵣ²
# c₋ = qᵣ + 0.5 / u_max q̇ᵣ²
#
# if (q̇ < q̇ᵣ and q ≤ 0.5 / u_max q̇² + c₊) or
# (q̇ ≥ q̇ᵣ and q < −0.5 / u_max q̇² + c₋)
#
# if (q̇ < q̇ᵣ and q ≤ 0.5 / u_max q̇² + qᵣ − 0.5 / u_max q̇ᵣ²) or
# (q̇ ≥ q̇ᵣ and q < −0.5 / u_max q̇² + qᵣ + 0.5 / u_max q̇ᵣ²)
#
# if (q̇ < q̇ᵣ and q − qᵣ ≤ 0.5 / u_max q̇² − 0.5 / u_max q̇ᵣ²) or
# (q̇ ≥ q̇ᵣ and q − qᵣ < −0.5 / u_max q̇² + 0.5 / u_max q̇ᵣ²)
#
# if (q̇ < q̇ᵣ and q − qᵣ ≤ 0.5 / u_max q̇² − 0.5 / u_max q̇ᵣ²) or
# (q̇ ≥ q̇ᵣ and q − qᵣ < −(0.5 / u_max q̇² − 0.5 / u_max q̇ᵣ²))
#
# if (q̇ < q̇ᵣ and q − qᵣ ≤ 0.5 / u_max (q̇² − q̇ᵣ²)) or
# (q̇ ≥ q̇ᵣ and q − qᵣ < −0.5 / u_max (q̇² − q̇ᵣ²))
if (
qdot < qdot_ref and q - q_ref <= 0.5 / u_max * (qdot**2 - qdot_ref**2)
) or (
qdot >= qdot_ref and q - q_ref < -0.5 / u_max * (qdot**2 - qdot_ref**2)
):
# Enforce qdot maximum
if qdot < self.constraints.max_velocity:
x = [q, qdot, u_max]
else:
x = [q, qdot, 0.0]
else:
# Enforce qdot minimum
if qdot > -self.constraints.max_velocity:
x = [q, qdot, -u_max]
else:
x = [q, qdot, 0.0]
# qₖ₊₁ = qₖ + Tq̇ₖ + 1/2 T²q̈ₖ
# q̇ₖ₊₁ = q̇ₖ + Tq̈ₖ
return TrapezoidProfileFilter.State(
x[0] + dt * x[1] + 0.5 * dt * dt * x[2], x[1] + dt * x[2]
)
def main():
dt = 0.005
T = 1.75
N = int(T / dt)
controller = TrapezoidProfileFilter(TrapezoidProfileFilter.Constraints(2, 12))
x = TrapezoidProfileFilter.State(1, 0)
r = TrapezoidProfileFilter.State(2, 0)
times = np.linspace(0, (N + 1) * dt, N + 1)
states = np.zeros((2, N + 1))
for k in range(N):
states[:, k : k + 1] = np.array([[x.position], [x.velocity]])
x = controller.calculate(x, r, dt)
states[:, k + 1 : k + 2] = np.array([[x.position], [x.velocity]])
plot_data(
times,
states,
["Position", "Velocity"],
["Position (m)", "Velocity (m/s)"],
)
plt.figure()
plt.title("Velocity vs Position")
plt.xlabel("Position (m)")
plt.ylabel("Velocity (m/s)")
plt.plot(states[0, :], states[1, :])
plt.show()
if __name__ == "__main__":
main()
```
The minimum time problem for a DC motor is more difficult to solve though due to the velocity decay dynamics complicating the derivation. The other problem with dynamic programming is the phase switches in the discrete solution aren't the same as those for the continuous solution, so the discrete solution can oscillate near the goal due to switching too early or too late:
The overshoot/undershoot disappears as the timestep duration approaches zero.
### Possible solution: MPC
Another option would be setting up a linear MPC problem with some time horizon and solving it with https://github.com/SleipnirGroup/Sleipnir/ (Sleipnir is integrated as a thirdparty dependency [here](https://github.com/calcmogul/allwpilib/tree/wpimath-add-sleipnir)). This would be slower than an analytical solution, but it might end up being more feasible to derive and implement.