issue with acados realtime library for l4casadi #47
Comments
|
Hi, please post a minimal non working code example. Thanks |
|
Unfortunately, given that i am not able to run the code because of the error i posted in the first comment, I can only post the code i'm using which is the one that's causing issues. With this code i obtain the log error I posted initially: import torch import casadi as cs from pylab import * os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' x_start = np.array([0,0,0]) # initial state t_horizon = 5.0 nx = 3 class PyTorchModel(torch.nn.Module): class UnicycleWithLearnedDynamics: class MPC: def run(): The structure is basically identical to another code i'm using for the neural MPC but with the only difference being the type of l4casadi funcion to convert the pytorch model into a casadi affine structure (here I use l4casadi.realtime.RealTimeL4CasADi() and it doesn't work, while in the other code I use l4casadi.L4CasADi(), and the second one works great) |
RealTimeL4CasADi is not a one-to-one drop in replacement to L4CasADi. It is a different concept of using approximated functions which have to be updated by the user. Would this explain the problems? Feel free to take a look at the RealTimeL4CasADi examples and let me know if you still have questions - happy to answer them. Best |
|
It explains it partially. I have already seen and tried the example of the
realtimel4casadi application (the double integrator example), so I tried to
structure my code for the realtime neural MPC problem with the unicycle
using as a base the double integrator example. I think that the main
problem is related to the definition and update of the simulation
parameters, probably because I don’t fully understand how the parameters
are supposed to work in the code, so right now I can’t understand where
this error is. Could it be that for this application (generating the
optimal trajectory from a starting state to a final state), the
l4casadirealtime is not feasible?
…On Thu, 5 Sep 2024 at 14:31, Tim Salzmann ***@***.***> wrote:
The structure is basically identical to another code i'm using for the
neural MPC but with the only difference being the type of l4casadi funcion
to convert the pytorch model into a casadi affine structure (here I use
l4casadi.realtime.RealTimeL4CasADi()
RealTimeL4CasADi is *not* a one-to-one drop in replacement to L4CasADi.
It is a different concept of using approximated functions which have to be
updated by the user.
Would this explain the problems? Feel free to take a look at the
RealTimeL4CasADi examples and let me know if you still have questions -
happy to answer them.
Best
Tim
—
Reply to this email directly, view it on GitHub
<#47 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/AZVTUGKWYDSUVF2PLE5NKPTZVBFI3AVCNFSM6AAAAABNMBHMO2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDGMZRGQZTGNZQGA>
.
You are receiving this because you authored the thread.Message ID:
***@***.***>
|
|
Hi, please remove the following lines and see if this changes anything: |
|
Good evening Tim,
Sorry if it tok a long time to respond but I wanted to try as much as I
could and give you a proper feedback.
By removing those lines the code can at least be executed because it
doesn't go and search for the external library. Now the problem is that the
final state is not reached by a big margin of error, even though the
constraints on the inputs and the other state variables, on initial and
final state are all set. I'm also trying to work on the weights during the
trajectory and at the end of the trajectory (the matrices ocp.cost.W and
ocp.cost.W_e) to figure out if these can help to make the system converge
to the final state, but for now it doesn't seem to be the right way.
One thing that has been bothering me, and in my opinion is the thing that's
causing the problems, is the "parameter" aspect of the realtimel4casadi
library:
In the example the parameters of the learned dynamics are defined as :
p = self.learned_dyn.get_sym_params()
parameter_values = self.learned_dyn.get_params(np.array([0, 0]))
And then are updated at every step of the MPC after solving the optimal
control problem. In my case I used a similar initialization, but the
parameter_values aspect is what gives the most problems, probably because
I'm not really sure how it's supposed to work. How do the parameters
actually work and what is their main use in solving the MPC problem?
because in the standard l4casadi they are not present.
I hope I'm not bothering much with this issue, I'll wait for your response
and in the meantime I wish you a good rest of the week.
Il giorno ven 6 set 2024 alle ore 13:13 Tim Salzmann <
***@***.***> ha scritto:
… Hi,
please remove the following lines and see if this changes anything:
ocp.solver_options.model_external_shared_lib_dir = self.external_shared_lib_dir
ocp.solver_options.model_external_shared_lib_name = self.external_shared_lib_name
—
Reply to this email directly, view it on GitHub
<#47 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/AZVTUGJNFQIO2N736R2ATIDZVGE65AVCNFSM6AAAAABNMBHMO2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDGMZTHAZDQOBXGI>
.
You are receiving this because you authored the thread.Message ID:
***@***.***>
|
|
Hi Nicco, just making sure you have read my paper [1] where the Real-Time approach is developed? Think of it as an approximation of the actual model, which is accurate around the setpoint but not accurate far away. Each time you update the parameters you update the approximation around a new setpoint. Let me know if this helps or you need more details Thanks |
|
I did read the paper, yes I understood that the real-time aspect is based
on a linearization of the non linear model around the current working point
(set point). So correct me if i'm wrong, the parameters are the ones used
to describe the linearization of the model around the current working
point? Because if it's like that then I don't know where the problem in my
code could be, given that I'm updating the parameters at every step with
respect to the updated state. The neural network used by the solverto
replicate the dynamics cannot be the problem because it works very well for
the standard case, so may I ask if there's a way that I can send you my
code and when you have time you can help me fix it? I've been having these
issues for weeks now..
Il giorno mer 18 set 2024 alle ore 21:39 Tim Salzmann <
***@***.***> ha scritto:
… Hi Nicco,
just making sure you have read my paper [1] where the Real-Time approach
is developed?
Think of it as an approximation of the actual model, which is accurate
around the setpoint but not accurate far away. Each time you update the
parameters you update the approximation around a new setpoint.
Let me know if this helps or you need more details
Thanks
Tim
[1] https://arxiv.org/pdf/2203.07747
—
Reply to this email directly, view it on GitHub
<#47 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/AZVTUGMAGI7I6GZ5SOPYY2TZXHJIXAVCNFSM6AAAAABNMBHMO2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDGNJZGI3DINJVHA>
.
You are receiving this because you authored the thread.Message ID:
***@***.***>
|
|
Feel free to post your code here. However, I can not promise that I will find the time to dig into it. Best |
|
Thank you in any case for the disponiblity, I'll post the code and wait for
some kind of response while I continue to work on it:
import torch
import torch.nn as nn
import l4casadi as l4c
import casadi as cs
from acados_template import AcadosSimSolver, AcadosOcpSolver, AcadosSim,
AcadosOcp, AcadosModel
import time
import os
from pylab import *
import numpy as np
import matplotlib.pyplot as plt
os.environ['KMP_DUPLICATE_LIB_OK'] = 'True'
x_start = np.array([0,0,0]) # initial state
x_final = np.array([4,4,0]) # desired terminal state values
f_final = np.array([0,0]) # desired final control values
# MPC parameters
t_horizon = 4.0
N = 50
steps = 48
dt = t_horizon / N
# Dimensions
nx = 3
nu = 2
ny = nx + nu
class PyTorchModel(torch.nn.Module):
def __init__(self):
super(PyTorchModel, self).__init__()
self.fc1 = nn.Linear(5, 512) # 5 input nodes: x, y, theta, v, omega
self.fc2 = nn.Linear(512, 512)
self.fc3 = nn.Linear(512, 512)
self.fc4 = nn.Linear(512, 512)
self.fc5 = nn.Linear(512, 512)
self.fc6 = nn.Linear(512, 3) # 3 output nodes: x_next, y_next, theta_next
def forward(self, x):
x = nn.functional.relu(self.fc1(x))
x = nn.functional.tanh(self.fc2(x))
x = nn.functional.tanh(self.fc3(x))
x = nn.functional.tanh(self.fc4(x))
x = nn.functional.relu(self.fc5(x))
x = self.fc6(x)
return x
class UnicycleWithLearnedDynamics:
def __init__(self, learned_dyn):
self.learned_dyn = learned_dyn
def model(self):
# Definizione delle variabili di stato
x = cs.MX.sym('x', 1)
y = cs.MX.sym('y', 1)
theta = cs.MX.sym('theta', 1)
# Definizione degli ingressi
v = cs.MX.sym('v', 1)
omega = cs.MX.sym('omega', 1)
states = cs.vertcat(x, y, theta)
controls = cs.vertcat(v, omega)
# Input per la rete neurale (stati + ingressi)
inputs = cs.vertcat(states, controls)
# Derivate delle variabili di stato
x_dot = cs.MX.sym('x_dot')
y_dot = cs.MX.sym('y_dot')
theta_dot = cs.MX.sym('theta_dot')
xdot = cs.vertcat(x_dot, y_dot, theta_dot)
rand_input = torch.rand(5)
derivatives = self.learned_dyn(inputs)
p = self.learned_dyn.get_sym_params()
parameter_values = self.learned_dyn.get_params(rand_input)
# Definizione della dinamica esplicita del sistema
f_expl = derivatives
# Definizione della dinamica implicita del sistema
f_impl = f_expl - xdot
# Creazione del modello per ACADOS
model = cs.types.SimpleNamespace()
model.x = states
model.xdot = xdot
model.u = controls
model.z = cs.vertcat([])
model.p = p
model.f_expl = f_expl
model.f_impl = f_impl
model.cost_y_expr = cs.vertcat(states, controls)
model.cost_y_expr_e = cs.vertcat(states)
model.x_start = x_start
model.x_final = x_final
model.f_final = f_final
model.parameter_values = parameter_values
model.constraints = cs.vertcat([])
model.name = "unicycle_model"
return model
class MPC:
def __init__(self, model, N):
self.N = N
self.model = model
@Property
def simulator(self):
return AcadosSimSolver(self.sim())
@Property
def solver(self):
return AcadosOcpSolver(self.ocp())
def acados_model(self, model):
model_ac = AcadosModel()
model_ac.f_impl_expr = model.xdot - model.f_expl
model_ac.f_expl_expr = model.f_expl
model_ac.x = model.x
model_ac.xdot = model.xdot
model_ac.u = model.u
model_ac.p = model.p # Aggiungi questa riga per passare i parametri
model_ac.name = model.name
return model_ac
def sim(self):
model = self.model
t_horizon = 3.
N = self.N
# Get model
model_ac = self.acados_model(model=model)
model_ac.p = model.p
# Create OCP object to formulate the optimization
sim = AcadosSim()
sim.model = model_ac
sim.dims.N = N
sim.dims.nx = nx
sim.dims.nu = nu
sim.dims.ny = nx + nu
sim.solver_options.tf = t_horizon
# Solver options
sim.solver_options.Tsim = dt
sim.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
sim.solver_options.hessian_approx = 'GAUSS_NEWTON'
sim.solver_options.integrator_type = 'ERK'
# ocp.solver_options.print_level = 0
sim.solver_options.nlp_solver_type = 'SQP_RTI'
return sim
def ocp(self):
model = self.model
t_horizon = 3.
N = self.N
# Get model
model_ac = self.acados_model(model=model)
model_ac.p = model.p
# Create OCP object to formulate the optimization
ocp = AcadosOcp()
ocp.model = model_ac
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nu = nu
ocp.dims.ny = ny
ocp.parameter_values = model.parameter_values
ocp.solver_options.tf = t_horizon
# Initialize cost function
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
# Definizione pesi iniziali funzione di costo
w_x = 1.0 # Peso per x
w_y = 1.0 # Peso per y
w_theta = 1.0 # Peso maggiore per l'orientamento theta
w_vlin = 0.2 # Peso per l'input di controllo (sforzo minimo)
w_omega = 0.2 # Peso per l'input di controllo (sforzo minimo)
ocp.cost.W = np.diag([w_x, w_y, w_theta, w_vlin, w_omega])
# Definizione dei pesi parte finale della traiettoria
w_xe = 30.0 # Peso per x
w_ye = 15.0 # Peso per y
w_thetae = 35.0 # Peso maggiore per l'orientamento theta
ocp.cost.W_e = np.diag([w_xe, w_ye, w_thetae])
ocp.cost.Vx = np.zeros((5, 3))
ocp.cost.Vx[:3, :3] = np.eye(3)
ocp.cost.Vx_e = np.eye(3)
ocp.cost.Vu = np.zeros((5, 2))
ocp.cost.Vu[3:, :] = np.eye(2)
ocp.cost.Vz = np.array([[]])
# Initial reference trajectory (will be overwritten)
ocp.cost.yref = np.array([x_final[0], x_final[1], x_final[2], f_final[0],
f_final[1]])
ocp.cost.yref_e = np.array([x_final[0], x_final[1], x_final[2]])
"""
ocp.cost.yref_e = np.array([x_final[0], x_final[1], x_final[2]])
# Initial reference trajectory (will be overwritten)
ocp.cost.yref = np.array([x_final[0], x_final[1], x_final[2]])
"""
# Initial state (will be overwritten)
ocp.constraints.x0 = model.x_start
# final state
ocp.constraints.x_e = model.x_final
ocp.constraints.u_e = f_final
# Set constraints
v_max = 10
omega_max = 2*np.pi
ocp.constraints.lbu = np.array([0, -omega_max])
ocp.constraints.ubu = np.array([v_max, omega_max])
ocp.constraints.idxbu = np.array([0,1])
ocp.constraints.lbx = np.array([-np.pi])
ocp.constraints.ubx = np.array([np.pi])
ocp.constraints.idxbx = np.array([2])
# Solver options
ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'
return ocp
def run():
# Carica il modello addestrato della rete neurale tramite load_state_dict
PyTorch_model = PyTorchModel()
PyTorch_model.load_state_dict(torch.load("unicycle_model_state_3.pth"))
PyTorch_model.eval()
learned_dyn_model = l4c.realtime.RealTimeL4CasADi(PyTorch_model,
approximation_order=2)
model = UnicycleWithLearnedDynamics(learned_dyn_model)
solver = MPC(model=model.model(), N=N).solver
# Definisci il modello dinamico dell'uniciclo con la rete neurale
"""
print('Warming up model...')
x_l = []
for i in range(N):
x_l.append(solver.get(i, "x"))
for i in range(20):
learned_dyn_model.get_params(np.stack(x_l, axis=0))
print('Warmed up!')
"""
# Stato iniziale (x, y, theta)
xt = x_start
x = [xt]
# Per salvare i controlli ottimali
u_history = []
opt_times = []
for i in range(steps):
now = time.time()
for j in range(N):
solver.set(j, "yref", np.hstack((x_final,f_final)))
# Imposta lo stato finale e l'ingresso finale come riferimento per la
funzione di costo
solver.set(N, "yref", np.hstack((x_final)))
# Imposta lo stato corrente come vincolo iniziale
solver.set(0, "lbx", xt)
solver.set(0, "ubx", xt)
# Risolvi il problema di controllo ottimo
solver.solve()
# Recupera la prima azione di controllo ottima (velocità lineare e angolare)
u_opt = solver.get(0, "u")
u_history.append(u_opt)
# Usa le equazioni dell'uniciclo per calcolare il nuovo stato
dt = t_horizon / N
v, omega = u_opt
x_new = xt[0] + v * np.cos(xt[2]) * dt
y_new = xt[1] + v * np.sin(xt[2]) * dt
theta_new = xt[2] + omega * dt
theta_new = theta_new - 2 * np.pi * cs.floor((theta_new + np.pi) / (2 * np
.pi))
xt = np.array([x_new, y_new, theta_new])
x.append(xt)
# Aggiorna i parametri della rete neurale basati sui nuovi stati
x_l = []
for i in range(N):
x_state = solver.get(i, "x")
u_control = solver.get(i, "u")
x_l.append(np.hstack((x_state, u_control)))
#print(x_l)
params = learned_dyn_model.get_params(np.stack(x_l, axis=0))
#print("Params shape:", params.shape)
for i in range(N):
solver.set(i, "p", params[i])
elapsed = time.time() - now
opt_times.append(elapsed)
print(f'Mean iteration time: {1000*np.mean(opt_times):.1f}ms -- {1/np.mean(
opt_times):.0f}Hz')
# Stampa il risultato finale
print("Final state reached:", xt)
# Trasforma x e controls in array numpy per la visualizzazione
x = np.array(x)
u_history = np.array(u_history)
# Calcola gli errori rispetto allo stato finale desiderato
error_x = x[:, 0] - x_final[0]
error_y = x[:, 1] - x_final[1]
error_theta = x[:, 2] - x_final[2]
error_theta = error_theta - 2 * np.pi * cs.floor((error_theta + np.pi) / (2
* np.pi))
print(f"Final error on x: {abs(error_x[-1]):.4f} m")
print(f"Final error on y: {abs(error_y[-1]):.4f} m")
print(f"Final error on theta: {abs(error_theta[-1]):.4f} rad")
plt.figure()
plt.plot(x[:, 0], x[:, 1], label='x(t)')
plt.title('Traiettoria del robot uniciclo')
plt.xlabel('Posizione x [m]')
plt.ylabel('Posizione y [m]')
plt.grid(True)
plt.axis('scaled')
plt.show()
# Traccia la traiettoria x, y e theta rispetto al tempo
plt.figure(figsize=(12, 8))
plt.subplot(3, 1, 1)
plt.plot(x[:, 2], label='theta(t)')
plt.title('Traiettoria di theta')
plt.xlabel('Tempo [s]')
plt.ylabel('theta [rad]')
plt.subplot(3, 1, 2)
plt.plot(u_history[:, 0], label='v(t)')
plt.title('Velocità lineare v')
plt.xlabel('Tempo (s)')
plt.ylabel('v [m/s]')
plt.subplot(3, 1, 3)
plt.plot(u_history[:, 1], label='omega(t)')
plt.title('Velocità angolare omega')
plt.xlabel('Tempo (s)')
plt.ylabel('omega [rad/s]')
plt.tight_layout()
plt.show()
# Traccia gli errori rispetto allo stato finale
plt.figure(figsize=(12, 8))
plt.subplot(3, 1, 1)
plt.plot(error_x, label='Errore x(t)')
plt.title('Errore su x rispetto a posizione finale')
plt.xlabel('Tempo [s]')
plt.ylabel('Errore x [m]')
plt.subplot(3, 1, 2)
plt.plot(error_y, label='Errore y(t)')
plt.title('Errore su y rispetto a posizione finale')
plt.xlabel('Tempo [s]')
plt.ylabel('Errore y [m]')
plt.subplot(3, 1, 3)
plt.plot(error_theta, label='Errore theta(t)')
plt.title('Errore su theta rispetto a orientamento finale')
plt.xlabel('Tempo [s]')
plt.ylabel('Errore theta [rad]')
plt.tight_layout()
plt.show()
if __name__ == '__main__':
run()
Il giorno ven 20 set 2024 alle ore 20:11 Tim Salzmann <
***@***.***> ha scritto:
… Feel free to post your code here. However, I can not promise that I will
find the time to dig into it.
Best
Tim
—
Reply to this email directly, view it on GitHub
<#47 (comment)>,
or unsubscribe
<https://github.com/notifications/unsubscribe-auth/AZVTUGKDTUT5AHIQCBHJ7G3ZXRQLLAVCNFSM6AAAAABNMBHMO2VHI2DSMVQWIX3LMV43OSLTON2WKQ3PNVWWK3TUHMZDGNRUGI3DSMBYGQ>
.
You are receiving this because you authored the thread.Message ID:
***@***.***>
|
|
Hi, could you past it in a formated form? Otherwise I will have to reformat the whole text you pasted. E.g. |
|
Hello, import torch import casadi as cs from pylab import * os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' x_start = np.array([0,0,0]) # initial state t_horizon = 4.0 nx = 3 class PyTorchModel(torch.nn.Module): class UnicycleWithLearnedDynamics: class MPC: def run(): if name == 'main': Thank you again for the help, appreciate it. |
|
Hi Nicco, Can you describe the exact problem you are facing right now? I understand that with the same trained model using L4CasADi + Acados, the optimization converges to an expected solution, while with RealTimeL4CasADi + Acados, it does not. Could you upload the model you are using Thanks |
|
Hello Tim, REALTIMEL4CASADI TRAJECTORY STANDARD L4CASADI TRAJECTORY The MPC parameters for both trajectories are the following: x_start = np.array([0,0,0]) # initial state t_horizon = 5.0 Here is also the code that works: import torch import casadi as cs from pylab import * os.environ['KMP_DUPLICATE_LIB_OK'] = 'True' x_start = np.array([0,0,0]) # initial state t_horizon = 5.0 nx = 3 """ """ class UnicycleWithLearnedDynamics: class MPC: def run(): if name == 'main': Thank you again for the massive help. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Hello everybody,
I'm having issues when running a script for a Neural MPC with the RealTimeL4CasADi library, i don't know if it's a problem that regards only me because I've made mistakes in the code.
When I run the code, this is what the log I obtain:
Warning: Did not find environment variable ACADOS_SOURCE_DIR, guessed ACADOS_PATH to be /Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados.
Please export ACADOS_SOURCE_DIR to avoid this warning.
rm -f libacados_ocp_solver_unicycle_model.dylib
rm -f acados_solver_unicycle_model.o
-L /Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/_l4c_generated -l rt_l4casadi_f
ld: library not found for -lrt_l4casadi_f
clang: error: linker command failed with exit code 1 (use -v to see invocation)
make: *** [ocp_shared_lib] Error 1
acados was compiled without OpenMP.
Traceback (most recent call last):
File "/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/neuralmpc_acados_rt.py", line 470, in
run()
File "/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/neuralmpc_acados_rt.py", line 326, in run
solver = MPC(model=model.model(), N=N,
File "/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/neuralmpc_acados_rt.py", line 172, in solver
return AcadosOcpSolver(self.ocp())
File "/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/interfaces/acados_template/acados_template/acados_ocp_solver.py", line 240, in init
self.shared_lib = get_shared_lib(self.shared_lib_name, self.winmode)
File "/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/interfaces/acados_template/acados_template/utils.py", line 126, in get_shared_lib
shared_lib = DllLoader(shared_lib_name)
File "/Library/Frameworks/Python.framework/Versions/3.9/lib/python3.9/ctypes/init.py", line 374, in init
self._handle = _dlopen(self._name, mode)
OSError: dlopen(/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/c_generated_code/libacados_ocp_solver_unicycle_model.dylib, 0x0006): tried: '/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/c_generated_code/libacados_ocp_solver_unicycle_model.dylib' (no such file), '/System/Volumes/Preboot/Cryptexes/OS/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/c_generated_code/libacados_ocp_solver_unicycle_model.dylib' (no such file), '/Users/niccolobonucci/Desktop/UNI/Magistrale/TESI/acados/lib/c_generated_code/libacados_ocp_solver_unicycle_model.dylib' (no such file)
How can I solve this problem? Where can I find the lrt_l4casadi_f library?
Thank you for the help.
The text was updated successfully, but these errors were encountered: