How to implement RK4 integration scheme in a DPP compliant manner
GNCGenie opened this issue · comments
While developing an MPC controller for rendezvous, I have a varying time step for the algorithm.
Treating dt as a parameter allows solving TPBVP minimising fuel use.
The formulation of problem is DPP complaint for Euler integration scheme but not for RK4.
The code looks like:
def dX(X, U):
return A @ X + B @ U
# Define optimization variables
U = cp.Variable((m, H), name='U')
X = cp.Variable((n, H+1), name='X')
# Define optimization parameters
A = cp.Parameter((n, n), name='A')
B = cp.Parameter((n, m), name='B')
x_init = cp.Parameter(n, name='x_init')
x_target = cp.Parameter(n, name='x_target')
max_velocity = cp.Parameter(name='max_velocity')
max_effort = cp.Parameter(name='max_effort')
dt = cp.Parameter(nonneg=True, name='dt') # dt declared as a parameter
# Dynamics constraints
constraints = [cp.abs(X[3:, :]) <= max_velocity,
cp.abs(U) <= max_effort]
for i in range(H):
# RK4 Timestep
k1 = dX(X[:, i], U[:, i]) # Not DPP compliant
k2 = dX(X[:, i] + 0.5*k1*dt, U[:, i])
k3 = dX(X[:, i] + 0.5*k2*dt, U[:, i])
k4 = dX(X[:, i] + k3*dt, U[:, I])
constraints += [X[:, i+1] == X[:, i] + dt*(k1 + 2*k2 + 2*k3 + k4)/6]
# Euler timestep
#constraints += [X[:, i+1] == X[:,i] + dX(X[:, i], U[:, i])] # DPP compliant
# CODE WHICH CAUSES THE ISSUE ^^^^^^
# Boundary conditions
constraints += [X[:, 0] == x_init,
X[:, -1] == x_target]
# Define objective function
objective = cp.Minimize(cp.norm(U))
# Define optimization problem
problem = cp.Problem(objective, constraints)Are there any workaround to doing this. The documentation mentions introducing variables to resolve the issue for simpler problems.
But the thing is RK4 is 4th order so that results in dt^4 expression.
Only Euler integration is convex, so regardless of DPP or anything else you can't solve MPC with RK4 using convex optimization. You should look at Casadi.
Yes!
CVX formulates the problem and solves it using solvers that support only convex optimisation.
I don't understand why it works in Python either, since it is a non-convex problem.
The controller code I have pasted above uses SCS (Splitting Cone Solver) and gives accurate outputs.
However the conversion to C code fails.
For real-time implementation you can use a zero-order hold integration scheme.
Yet, i don't see why RK4 is not convex
For real-time implementation you can use a zero-order hold integration scheme.
Yet, i don't see why RK4 is not convex
My understanding is RK4 has non-linear dynamics (i.e., non-linear equality constraints). Is that not the case?
It seems linear in the example. If the dynamics is nonlinear you need linearize it about a reference trajectory and possibile solve recursively (see the Scvx method of acickmese and Mao) Il Sab 23 Mar 2024, 00:32 Steven Diamond @.> ha scritto:
…
For real-time implementation you can use a zero-order hold integration scheme. Yet, i don't see why RK4 is not convex My understanding is RK4 has non-linear dynamics (i.e., non-linear equality constraints). Is that not the case? — Reply to this email directly, view it on GitHub <#42 (comment)>, or unsubscribe https://github.com/notifications/unsubscribe-auth/AHGBW7MM7MJ7MIYSLTABXITYZS5SHAVCNFSM6AAAAABD7XQESOVHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDAMJWGEZTCMZYGI . You are receiving this because you commented.Message ID: @.>
Ah ok, I see the actual underlying dynamics are linear here. I was confused by the example. In that case I agree that RK4 is fine and convex since it produces a linear mapping.
While developing an MPC controller for rendezvous, I have a varying time step for the algorithm. Treating dt as a parameter allows solving TPBVP minimising fuel use.
The formulation of problem is DPP complaint for Euler integration scheme but not for RK4.
The code looks like:
def dX(X, U): return A @ X + B @ U # Define optimization variables U = cp.Variable((m, H), name='U') X = cp.Variable((n, H+1), name='X') # Define optimization parameters A = cp.Parameter((n, n), name='A') B = cp.Parameter((n, m), name='B') x_init = cp.Parameter(n, name='x_init') x_target = cp.Parameter(n, name='x_target') max_velocity = cp.Parameter(name='max_velocity') max_effort = cp.Parameter(name='max_effort') dt = cp.Parameter(nonneg=True, name='dt') # dt declared as a parameter # Dynamics constraints constraints = [cp.abs(X[3:, :]) <= max_velocity, cp.abs(U) <= max_effort] for i in range(H): # RK4 Timestep k1 = dX(X[:, i], U[:, i]) # Not DPP compliant k2 = dX(X[:, i] + 0.5*k1*dt, U[:, i]) k3 = dX(X[:, i] + 0.5*k2*dt, U[:, i]) k4 = dX(X[:, i] + k3*dt, U[:, I]) constraints += [X[:, i+1] == X[:, i] + dt*(k1 + 2*k2 + 2*k3 + k4)/6] # Euler timestep #constraints += [X[:, i+1] == X[:,i] + dX(X[:, i], U[:, i])] # DPP compliant # CODE WHICH CAUSES THE ISSUE ^^^^^^ # Boundary conditions constraints += [X[:, 0] == x_init, X[:, -1] == x_target] # Define objective function objective = cp.Minimize(cp.norm(U)) # Define optimization problem problem = cp.Problem(objective, constraints)Are there any workaround to doing this. The documentation mentions introducing variables to resolve the issue for simpler problems. But the thing is RK4 is 4th order so that results in dt^4 expression.
If you are able to provide a working example, we can fix it.
The code here attached complains about several variables being not defined
Possibly the function
def dX(X, U):
return A @ X + B @ U
must be defined after A and B, but other errors are possible (not related to convex programming)
Definitely! The code below runs and throws a DPP warning, the last lines for code generation fail with the following error:
ValueError: Code generation with SCS and exponential, positive semidefinite, or power cones is not supported yet.
import cvxpy as cp
import numpy as np
################################
################################
# continuous-time dynmaics
R_Earth = 6.3781363e6
GM_Earth = 3.986004415e14
a = R_Earth + 650e3
N = np.sqrt(GM_Earth / a**3)
A_cont = np.array([[0, 0, 0, 1, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 0, 0, 0, 1],
[3*N**2, 0, 0, 0, 2*N, 0],
[0, 0, 0, -2*N, 0, 0],
[0, 0, -N**2, 0, 0, 0]])
B_cont = np.array([[0,0,0],
[0,0,0],
[0,0,0],
[1,0,0],
[0,1,0],
[0,0,1]])
# define dimensions
H, n, m = 10, 6, 3
init_state = np.array([0,1e3,0,0,0,0])
target_state = np.array([0,5e1,0,0,0,0])
Vmax = np.linalg.norm(init_state[:3] - target_state[:3]) / 1e3
Umax = 1e-1
timestep = max(2e0*np.sqrt(np.linalg.norm(init_state[:3] - target_state[:3])/Umax)/H , 1e-0)
################################
################################
def dX(X, U):
return A @ X + B @ U
# Define optimization variables
U = cp.Variable((m, H), name='U')
X = cp.Variable((n, H+1), name='X')
# Define optimization parameters
A = cp.Parameter((n, n), name='A')
B = cp.Parameter((n, m), name='B')
x_init = cp.Parameter(n, name='x_init')
x_target = cp.Parameter(n, name='x_target')
max_velocity = cp.Parameter(name='max_velocity')
max_effort = cp.Parameter(name='max_effort')
dt = cp.Parameter(nonneg=True, name='dt')
# Dynamics constraints
constraints = [cp.abs(X[3:, :]) <= max_velocity,
cp.abs(U) <= max_effort]
for i in range(H):
k1 = dX(X[:, i], U[:, i]) # Not DPP
k2 = dX(X[:, i] + 0.5*k1*dt, U[:, i])
k3 = dX(X[:, i] + 0.5*k2*dt, U[:, i])
k4 = dX(X[:, i] + k3*dt, U[:, i])
constraints += [X[:, i+1] == X[:, i] + dt*(k1 + 2*k2 + 2*k3 + k4)/6]
# Boundary conditions
constraints += [X[:, 0] == x_init,
X[:, -1] == x_target]
# Define objective function
objective = cp.Minimize(cp.norm(U))
# Define optimization problem
problem = cp.Problem(objective, constraints)
x_init.value = init_state
x_target.value = target_state
dt.value = timestep
max_velocity.value = Vmax
max_effort.value = Umax*timestep
A.value = A_cont
B.value = B_cont
# Solve the problem
val = problem.solve(solver=cp.SCS, verbose=False, max_iters=10**2)
print(f'U = {U.value.T}')
################################
################################
from cvxpygen import cpg
cpg.generate_code(problem, code_dir="MPC_RK4", solver='SCS')If the issue is the constraint type not being supported by SCS, what are some alternative solvers that handle these constraints?
My intuition is that the problem is DPC (so it is convex) but not DPP
I suspect it is breaking a rule of the form
gamma = cp.Parameter()
x = cp.Variable()
y = gamma * gamma * x; # this is not DPP, but is DPCIn fact, the line k2 = dX(X[:, i] + 0.5*k1*dt, U[:, i]) contains the product of k1 and dt, which is not DPP-compliant.
I think it is possible to find a solution, re-defining a certain number of parameters dtA, dt2A, dt3A = dtA, dtdt*A, etc by expanding the k1-- k4 terms
Let me know if you find a solution,
Best
Alessandro
The following lines should help you:
for i in range(H):
constraints += [X[:, i+1] == X[:, i] + A4h4/24. @ X[:, i] + A3Bh4/6. @ U[:,i]
+ A3h3/6. @ X[:, i] + A2Bh3/2. @ U[:,i]
+ A2h2/2. @ X[:, i] + ABh2/2. @ U[:,i]
+ Ah @ X[:, i] + Bh @ U[:,i]]
....
A4h4.value = A_cont @ A_cont @ A_cont @ A_cont * timestep**4
A3Bh4.value = A_cont @ A_cont @ A_cont @ B_cont * timestep**4
A3h3.value = A_cont @ A_cont @ A_cont * timestep**3
A2Bh3.value = A_cont @ A_cont @ B_cont * timestep**3
A2h2.value = A_cont @ A_cont * timestep**2
ABh2.value = A_cont @ B_cont * timestep**2
Ah.value = A_cont * timestep
Bh.value = B_cont * timestep
as a solver, i used CLARABEL
@GNCGenie @AlessandroZavoli @SteveDiamond here is an alternative way to make things DPP that might be a bit easier to handle when the problem becomes more involved. You should test which way gives better performance. I added only 1 auxiliary parameter Adt (meaning A times dt) and 4 auxiliary variables, k1, k2, k3, k4. This allows me to turn the RK4 expressions into separate constraints, all of which are DPP. Note that the last line problem.is_dpp() returns True.
import cvxpy as cp
H= 3
def dX(X, U):
return A @ X + B @ U
# Define optimization variables
U = cp.Variable((m, H), name='U')
X = cp.Variable((n, H+1), name='X')
# Define auxixiliary variables
k1 = cp.Variable((n, H))
k2 = cp.Variable((n, H))
k3 = cp.Variable((n, H))
k4 = cp.Variable((n, H))
# Define auxiliary parameter
Adt = cp.Parameter((n, n), name='Adt')
# Define optimization parameters
A = cp.Parameter((n, n), name='A')
B = cp.Parameter((n, m), name='B')
x_init = cp.Parameter(n, name='x_init')
x_target = cp.Parameter(n, name='x_target')
max_velocity = cp.Parameter(name='max_velocity')
max_effort = cp.Parameter(name='max_effort')
dt = cp.Parameter(nonneg=True, name='dt') # dt declared as a parameter
# Dynamics constraints
constraints = [cp.abs(X[3:, :]) <= max_velocity,
cp.abs(U) <= max_effort]
for i in range(H):
# RK4 Timestep
constraints += [
k1[:, i] == dX(X[:, i], U[:, i]),
k2[:, i] == A @ X[:, i] + 0.5 * Adt @ k1[:, i] + B @ U[:, i],
k3[:, i] == A @ X[:, i] + 0.5 * Adt @ k2[:, i] + B @ U[:, i],
k4[:, i] == A @ X[:, i] + Adt @ k3[:, i] + B @ U[:, i]
]
constraints += [
X[:, i+1] == X[:, i] + dt*(k1[:, i] + 2*k2[:, i] + 2*k3[:, i] + k4[:, i])/6
]
# Euler timestep
#constraints += [X[:, i+1] == X[:,i] + dX(X[:, i], U[:, i])] # DPP compliant
# CODE WHICH CAUSES THE ISSUE ^^^^^^
# Boundary conditions
constraints += [X[:, 0] == x_init,
X[:, -1] == x_target]
# Define objective function
objective = cp.Minimize(cp.norm(U))
# Define optimization problem
problem = cp.Problem(objective, constraints)
problem.is_dpp()
@GNCGenie to sum up, a good rule of thumb is to check whether you have products of parameters in your expressions. If yes, the formulation is not DPP.
The direct fix is usually to take every product of parameters and replace it by an auxiliary parameter. This is exactly what @AlessandroZavoli did above. In your specific case, you had a recursive structure of expressions k1, k2, k3, k4, which led to this "pyramid" of products of parameters.
In my implementation above, I broke this recursive chain of expressions by introducing k1, k2, k3, k4 not as intermediate expressions (=) but as actual variables that are constrained to what they should represent (==). It is up to you to test and decide which way works best.
Closing, feel free to re-open if you have any follow-up questions.