I know for position controlled devices the general wisdom is
feedforward + feedback with a trapezoid (or exponential) motion profile.
With velocity based control the general wisdom is
feedforward + fedback without a trapezoid profile
Looking at CTRE’s documentation for Phoenix 6 I noticed that they allow for the possibility of a motion profile constraining Acceleration and Jerk.
Is there a need for this in general?
Is it possible to do with sysId and WPILib
Using motion magic on velocity has roughly the same advantages as using it for position. You can use more aggressive PID gains, you don’t have to tune the same gains to both spin up from zero and make small speed adjustments
The general-general wisdom 
- Pick a software/controls strategy which respects the physical limits of the mechanism.
Specifically for velocity/flywheel control, consider stepping your shooter wheel on from 0 to 1000RPM. For the moment you turn it on, the commanded acceleration is infinite, and you’ll have to spend time waiting for the system to catch up to that unreasonable command.
Better, use constant acceleration - you’ll ramp the velocity command from 0 to 1000RPM linearly, with a rate picked to be at or under the mechanism’s maximum acceleration.
This will still have some “unrealisticness”, since now the acceleration steps from 0 to a max value at the start and end of the ramp. A further step is to ramp the acceleration, which gets into constant-jerk profile.
However, with each step, you’re taking the level of “not realistic” down by orders of magnitude. For FRC motors and mechanisms, constant acceleration is usually more than sufficient.
I’m assuming the WPILib equivalent for motion profiling would be to use a trapezoid profile with velocity as acceleration and acceleration as jerk for the constraints.
Theoretically could the Simple DC Motor Balance Equation be extended to handle Jerk. Like by introducing a kJ constant.
Think hard about what would physically determine that constant, and you’ll realize why no one does this.
You would in a sense have to measure the Rate of Change of the current over time.
Yes and - more specifically - what physical property of a motor and flywheel prevents current from changing instantaneously?
All models are wrong, some models are useful.
I’m assuming the rotor.
I might be a little dense at the moment.
I can hit up more later tonight with specifics, but I’ll keep hinting at my answer if you have time
Draw out the circuit model for the motor and the battery, and walk through each component. One of them will limit the time rate of change of current.
With the fairly significant asterisks that you aren’t approaching the free-speed of the mechanism. As the motor approaches free speed, the maximum possible acceleration approaches zero which will cause difficulty following motion profiles at very high speeds, espeically if they are not generated or updated real-time.
There is one step further in which you can use motion profiles derived directly from the dynamic equations to generate a motion profile which will maximize the motor’s output given the constrains, but in practice, it takes a significant amount of time to implement for reduced benefits which is generally not worth it for FRC teams to implement.
In practice, jerk is caused by motor inductance which reacts extremely fast, in milliseconds which is faster than the FRC control system can respond to. Accounting for this also requires a second differentiator in your system which amplifies noise, which is extremely undesirable. Inductance effects are so small in practice that even if you could account for them (which you can’t), you’d gain almost no benefit for doing so.
WPILib and CTRE have implemented it. The model is \frac{dx}{dt} = Ax + Bu where A = -k_v/k_a, B = 1/k_a, x = \text{velocity}, and u = \text{voltage}.
The exponential profile is much more limited compared to working in the differential equation form directly, which can account for other non-linearities such as dry friction, current limits, arms or anything with variable gravity gains or any system dynamics, which the differential equation form can depending on how its formulated.
So I’m relatively convinced that you can use a trap profile with velocity control. You would be constraining with acceleration and jerk and basically experimenting until you find values of each that would work hence the diminishing returns and why most teams and examples don’t bother with a profile for velocity control. Just curious can you use an expo profiles with velocity control or would those be limited to position only in The current WPILib setup
Or am do I have this turned around in my head?
It is limiting, but FRC only hooks up motors to masses in a few ways: flywheels, elevators (vertical and horizontal), single-jointed arms (vertical and horizontal), and drivetrains. The motor dynamics are linear (and handled by the exponential profile), dry friction and current limits don’t affect the profile much at all in practice if you gear the mechanism right, and elevator gravity just reduces the max voltage bound by a constant (feedforward adds it to the control input to cancel out gravity).
Single-jointed arm gravity is trickier if the goal is time optimality throughout the whole range of motion, so I agree something extra is needed there. MPC is an option, though the downside is the amount of CPU time required:
#!/usr/bin/env python3
# Uses https://github.com/SleipnirGroup/Sleipnir/
from jormungandr.autodiff import VariableMatrix, cos
from jormungandr.optimization import OptimizationProblem
import math
import matplotlib.pyplot as plt
import numpy as np
T = 2
dt = 0.01
N = int(T / dt)
# Model constants
# 12 V per (π/2 radians per 0.5 seconds)
k_v = 12.0 / ((math.pi / 2.0) / 0.5)
k_a = 0.5
g = 9.8
L = 2.0
def rk4(f, x, u, dt):
"""
Performs 4th order Runge-Kutta integration of dx/dt = f(x, u) for dt.
Parameter ``f``:
The function to integrate. It must take two arguments x and u.
Parameter ``x``:
The initial value of x.
Parameter ``u``:
The value u held constant over the integration period.
Parameter ``dt``:
The time over which to integrate.
"""
h = dt
k1 = f(x, u)
k2 = f(x + h * 0.5 * k1, u)
k3 = f(x + h * 0.5 * k2, u)
k4 = f(x + h * k3, u)
return x + h / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4)
def dynamics(x, u):
# x = [θ, ω]ᵀ = [angle, angular velocity]ᵀ
# u = [voltage]ᵀ
#
# The torque on the arm is given by τ = F⋅r, where F is the force applied by
# gravity and r the distance from pivot to center of mass. Recall from
# dynamics that the sum of torques for a rigid body is τ = J⋅α, were τ is
# torque on the arm, J is the mass-moment of inertia about the pivot axis,
# and α is the angular acceleration in rad/s². Rearranging yields: α = F⋅r/J
#
# We substitute in F = m⋅g⋅cos(θ), where θ is the angle from horizontal:
#
# α = (m⋅g⋅cos(θ))⋅r/J
#
# Multiply RHS by cos(θ) to account for the arm angle. Further, we know the
# arm mass-moment of inertia J of our arm is given by J=1/3 mL², modeled as
# a rod rotating about it's end, where L is the overall rod length. The mass
# distribution is assumed to be uniform. Substitute r=L/2 to find:
#
# α = (m⋅g⋅cos(θ))⋅r/(1/3 mL²)
# α = (m⋅g⋅cos(θ))⋅(L/2)/(1/3 mL²)
# α = 3/2⋅g⋅cos(θ)/L
#
# This acceleration is next added to the linear system dynamics ẋ=Ax+Bu
#
# f(x, u) = Ax + Bu + [0 α]ᵀ
# f(x, u) = Ax + Bu + [0 −3/2⋅g⋅cos(θ)/L]ᵀ
#
# .
# [θ] = [ ω ]
# [ω] [−kᵥ/kₐ ω + 1/kₐ u − 3/2⋅g⋅cos(θ)/L]
θ = x[0, 0]
ω = x[1, 0]
return VariableMatrix(
[
[ω],
[-k_v / k_a * ω + 1.0 / k_a * u[0, 0] - 1.5 * g * cos(θ) / L],
]
)
# Initial and final states
x_0 = np.array([[0.0], [0.0]])
r_f = np.array([[math.pi], [0.0]])
problem = OptimizationProblem()
# 2x1 state vector with N + 1 timesteps (includes last state)
X = problem.decision_variable(2, N + 1)
# 1x1 input vector with N timesteps (input at last state doesn't matter)
U = problem.decision_variable(1, N)
# Dynamics constraint
for k in range(N):
problem.subject_to(X[:, k + 1] == rk4(dynamics, X[:, k], U[:, k], dt))
# Initial state
problem.subject_to(X[:, 0] == x_0)
# Limit voltage
problem.subject_to(-12 <= U)
problem.subject_to(U <= 12)
# Cost function
J = 0.0
for k in range(N + 1):
# Penalize sum of squares angle error
J += (r_f[0, 0] - X[0, k]) ** 2
for k in range(N - 1):
# Penalize change in voltage over time to reduce input chattering
J += 1e-9 * ((U[0, k + 1] - U[0, k]) / dt) ** 2
problem.minimize(J)
problem.solve(diagnostics=True)
ts = np.linspace(0, T, N + 1)
plt.figure()
ax = plt.gca()
ax.set_title("Angle vs Time")
ax.set_xlabel("Time (s)")
ax.set_ylabel("Angle (rad)")
ax.plot(ts, X[0, :].value().T)
plt.figure()
ax = plt.gca()
ax.set_title("Angular velocity vs Time")
ax.set_xlabel("Time (s)")
ax.set_ylabel("Angular velocity (rad/s)")
plt.plot(ts, X[1, :].value().T)
plt.figure()
ax = plt.gca()
ax.set_title("Voltage vs Time")
ax.set_xlabel("Time (s)")
ax.set_ylabel("Voltage (V)")
plt.plot(ts[:-1], U.value().T)
plt.show()
Solver diagnostics:
[tav@myriad python]$ ./single_jointed_arm_mpc.py
The cost function is quadratic.
The equality constraints are nonlinear.
The inequality constraints are linear.
Number of decision variables: 602
Number of equality constraints: 402
Number of inequality constraints: 400
Error tolerance: 1e-08
iter time (ms) error cost infeasibility
=============================================================
0 2.967 8.283900e+00 2.774858e+03 3.010425e+00
1 2.956 5.695855e+00 1.603034e+03 7.758558e+00
2 2.886 3.764500e+00 8.311582e+02 9.842311e+00
3 3.165 2.426376e-01 5.678307e+02 3.030038e+00
4 2.862 6.690453e-02 5.590189e+02 4.844520e-02
5 2.708 3.925000e-02 5.560594e+02 6.830626e-03
6 2.677 5.152591e-03 5.556606e+02 7.434953e-04
7 2.787 4.531113e-03 5.553002e+02 3.181957e-04
8 2.767 3.999824e-04 5.552808e+02 4.742002e-05
9 2.752 8.528682e-04 5.552624e+02 1.425035e-05
10 3.009 4.420123e-04 5.552593e+02 1.825874e-06
11 3.359 8.630707e-06 5.552591e+02 1.112538e-07
12 8.766 2.220707e-06 5.552589e+02 1.630995e-08
13 2.880 9.860224e-07 5.552589e+02 1.492666e-09
14 2.706 1.305631e-07 5.552589e+02 2.625046e-10
15 2.806 2.728217e-08 5.552589e+02 5.185460e-11
16 2.854 3.776894e-09 5.552589e+02 6.536133e-12
Solve time: 64.550 ms
↳ 8.473 ms (solver setup)
↳ 56.077 ms (17 solver iterations; 3.298 ms average)
autodiff setup (ms) avg solve (ms) solves
===============================================
∇f(x) 0.056 0.033 18
∇²ₓₓL 1.841 0.763 18
∂cₑ/∂x 0.864 0.328 18
∂cᵢ/∂x 0.101 0.000 0
Exit condition: solved to desired tolerance
Plots:
If your profile doesn’t have a position goal, you can just use SlewRateLimiter (WPILib API 2024.3.2).
You’d probably have to fake a position goal, even if you don’t use it.
Okay. And If I recall correctly the SlewRateLimiter is the TrapProfile reduced by a degree of freedom?
Yep.
