issue with acados realtime library for l4casadi about l4casadi HOT 10 OPEN

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?

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:

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:

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: