def construct_upd_l(self): # create parameters x_i = struct_symSX(self.q_i_struct) z_i = struct_symSX(self.q_i_struct) z_ij = struct_symSX(self.q_ij_struct) l_i = struct_symSX(self.q_i_struct) l_ij = struct_symSX(self.q_ij_struct) x_j = struct_symSX(self.q_ij_struct) t = SX.sym('t') T = SX.sym('T') rho = SX.sym('rho') t0 = t/T inp = [x_i, z_i, z_ij, l_i, l_ij, x_j, t, T, rho] # put symbols in SX structs (necessary for transformation) x_i = self.q_i_struct(x_i) z_i = self.q_i_struct(z_i) z_ij = self.q_ij_struct(z_ij) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) x_j = self.q_ij_struct(x_j) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, z_i, l_i], tf, self.q_i) self._transform_spline([x_j, z_ij, l_ij], tf, self.q_ij) # update lambda l_i_new = self.q_i_struct(l_i.cat + rho*(x_i.cat - z_i.cat)) l_ij_new = self.q_ij_struct(l_ij.cat + rho*(x_j.cat - z_ij.cat)) tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(l_i_new, tf, self.q_i) self._transform_spline(l_ij_new, tf, self.q_ij) out = [l_i_new, l_ij_new] # create problem prob, compile_time = self.father.create_function( 'upd_l', inp, out, self.options) self.problem_upd_l = prob
def create_nlp(self, var, par, obj, con, options): jit = options['codegen'] if options['verbose'] >= 2: print 'Building nlp ... ', if jit['jit']: print('[jit compilation with flags %s]' % (','.join(jit['jit_options']['flags']))), t0 = time.time() nlp = MXFunction('nlp', nlpIn(x=var, p=par), nlpOut(f=obj, g=con)) solver = NlpSolver('solver', 'ipopt', nlp, options['solver']) grad_f, jac_g = nlp.gradient('x', 'f'), nlp.jacobian('x', 'g') hess_lag = solver.hessLag() var, par = struct_symSX(var), struct_symSX(par) nlp = SXFunction('nlp', [var, par], nlp([var, par]), jit) grad_f = SXFunction('grad_f', [var, par], grad_f([var, par]), jit) jac_g = SXFunction('jac_g', [var, par], jac_g([var, par]), jit) lam_f, lam_g = SX.sym('lam_f'), SX.sym('lam_g', con.size) hess_lag = SXFunction('hess_lag', [var, par, lam_f, lam_g], hess_lag([var, par, lam_f, lam_g]), jit) options['solver'].update({'grad_f': grad_f, 'jac_g': jac_g, 'hess_lag': hess_lag}) problem = NlpSolver('solver', 'ipopt', nlp, options['solver']) t1 = time.time() if options['verbose'] >= 2: print 'in %5f s' % (t1-t0) return problem, (t1-t0)
def chen_model(): """ The following ODE model comes from Chen1998. """ nx, nu = (2, 1) x = SX.sym('x', nx) u = SX.sym('u', nu) mu = 0.5 rhs = vertcat(x[1] + u*(mu + (1.-mu)*x[0]), x[0] + u*(mu - 4.*(1.-mu)*x[1])) return Function('chen', [x, u], [rhs]), nx, nu
def shift_knot1_bwd(cfs, basis, t_shift): if isinstance(cfs, (SX, MX)): cfs_sym = SX.sym('cfs', cfs.shape) t_shift_sym = SX.sym('t_shift') T, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True) cfs2_sym = mul(Tinv, cfs_sym) fun = SXFunction('fun', [cfs_sym, t_shift_sym], [cfs2_sym]) return fun([cfs, t_shift])[0] else: T, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True) return Tinv.dot(cfs)
def shift_knot1_bwd(cfs, basis, t_shift): if isinstance(cfs, (SX, MX)): cfs_sym = SX.sym('cfs', cfs.shape) t_shift_sym = SX.sym('t_shift') _, Tinv = shiftfirstknot_T(basis, t_shift_sym, inverse=True) cfs2_sym = mtimes(Tinv, cfs_sym) fun = Function('fun', [cfs_sym, t_shift_sym], [cfs2_sym]).expand() return fun(cfs, t_shift) else: _, Tinv = shiftfirstknot_T(basis, t_shift, inverse=True) return Tinv.dot(cfs)
def construct_upd_z(self): # check if we have linear equality constraints self._lineq_updz, A, b = self._check_for_lineq() if not self._lineq_updz: self._construct_upd_z_nlp() x_i = struct_symSX(self.q_i_struct) x_j = struct_symSX(self.q_ij_struct) l_i = struct_symSX(self.q_i_struct) l_ij = struct_symSX(self.q_ij_struct) t = SX.sym('t') T = SX.sym('T') rho = SX.sym('rho') par = struct_symSX(self.par_struct) inp = [x_i.cat, l_i.cat, l_ij.cat, x_j.cat, t, T, rho, par.cat] t0 = t/T # put symbols in SX structs (necessary for transformation) x_i = self.q_i_struct(x_i) x_j = self.q_ij_struct(x_j) l_i = self.q_i_struct(l_i) l_ij = self.q_ij_struct(l_ij) # transform spline variables: only consider future piece of spline tf = lambda cfs, basis: shift_knot1_fwd(cfs, basis, t0) self._transform_spline([x_i, l_i], tf, self.q_i) self._transform_spline([x_j, l_ij], tf, self.q_ij) # fill in parameters A = A([par])[0] b = b([par])[0] # build KKT system E = rho*SX.eye(A.shape[1]) l, x = vertcat([l_i.cat, l_ij.cat]), vertcat([x_i.cat, x_j.cat]) f = -(l + rho*x) G = vertcat([horzcat([E, A.T]), horzcat([A, SX.zeros(A.shape[0], A.shape[0])])]) h = vertcat([-f, b]) z = solve(G, h) l_qi = self.q_i_struct.shape[0] l_qij = self.q_ij_struct.shape[0] z_i_new = self.q_i_struct(z[:l_qi]) z_ij_new = self.q_ij_struct(z[l_qi:l_qi+l_qij]) # transform back tf = lambda cfs, basis: shift_knot1_bwd(cfs, basis, t0) self._transform_spline(z_i_new, tf, self.q_i) self._transform_spline(z_ij_new, tf, self.q_ij) out = [z_i_new.cat, z_ij_new.cat] # create problem prob, compile_time = self.father.create_function( 'upd_z', inp, out, self.options) self.problem_upd_z = prob
def pendulum_model(): """ Nonlinear inverse pendulum model. """ M = 1 # mass of the cart [kg] m = 0.1 # mass of the ball [kg] g = 9.81 # gravity constant [m/s^2] l = 0.8 # length of the rod [m] p = SX.sym('p') # horizontal displacement [m] theta = SX.sym('theta') # angle with the vertical [rad] v = SX.sym('v') # horizontal velocity [m/s] omega = SX.sym('omega') # angular velocity [rad/s] F = SX.sym('F') # horizontal force [N] ode_rhs = vertcat(v, omega, (- l*m*sin(theta)*omega**2 + F + g*m*cos(theta)*sin(theta))/(M + m - m*cos(theta)**2), (- l*m*cos(theta)*sin(theta)*omega**2 + F*cos(theta) + g*m*sin(theta) + M*g*sin(theta))/(l*(M + m - m*cos(theta)**2))) nx = 4 # for IRK xdot = SX.sym('xdot', nx, 1) z = SX.sym('z',0,1) return (Function('pendulum', [vertcat(p, theta, v, omega), F], [ode_rhs], ['x', 'u'], ['xdot']), nx, # number of states 1, # number of controls Function('impl_pendulum', [vertcat(p, theta, v, omega), F, xdot, z], [ode_rhs-xdot], ['x', 'u','xdot','z'], ['rhs']))
def deform( vertices_val, adjacency, target_to_base_indices, targets_val, ): n_vertices = vertices_val.shape[1] n_targets = targets_val.shape[1] assert vertices_val.shape[0] == 3 assert targets_val.shape[0] == 3 assert len(adjacency) == n_vertices assert len(target_to_base_indices) == n_targets start = time.time() old_vertices = SX.sym("old_vertices", 3, n_vertices) targets = SX.sym("targets", 3, n_targets) vertices = SX.sym("vertices", 3, n_vertices) rotations = SX.sym("rotations", 3, 3 * n_vertices) vertices_to_fit = vertices[:, target_to_base_indices] start_data = time.time() data = (vertices_to_fit - targets).reshape((-1, 1)) print(f"data elapsed {time.time() - start_data}") start_arap = time.time() arap_residual = arap.make_rot_arap_residuals(adjacency, old_vertices, rotations, vertices) print(f"arap elapsed {time.time() - start_arap}") start_rigid = time.time() rigid = arap.make_rigid_residuals(rotations) print(f"rigid elapsed {time.time() - start_rigid}") residuals = casadi.vertcat( data, arap_residual, 10.0 * rigid ) variables = casadi.vertcat( rotations.reshape((-1, 1)), vertices.reshape((-1, 1)) ) start_jac = time.time() jac = casadi.jacobian(residuals, variables) print(f"jac elapsed {time.time() - start_jac}") print("jac.shape", jac.shape, "jac.nnz()", jac.nnz()) fixed_values = [ old_vertices, targets, ] start_res = time.time() residual_func = Function("res_func", [variables] + fixed_values, [residuals]) print(f"res elapsed {time.time() - start_res}") start_jac_func = time.time() jac_func = Function("jac_func", [variables] + fixed_values, [jac]) print(f"jac_func elapsed {time.time() - start_jac_func}") print(f"construction elapsed {time.time() - start}") exit(0) def compute_residuals_and_jac(x): start = time.time() residuals_val = residual_func(x, vertices_val, targets_val).toarray() print(f"Residual elapsed {time.time() - start}") start = time.time() jacobian_val = jac_func(x, vertices_val, targets_val) print(f"Jacobian elapsed {time.time() - start}") jacobian_val = jacobian_val.sparse() return residuals_val, jacobian_val init_rot = np.hstack([np.eye(3)] * n_vertices) init_vertices = vertices_val init_vars = np.hstack(( init_rot.T.reshape(-1), init_vertices.reshape(-1) )) res = perform_gauss_newton(init_vars, compute_residuals_and_jac, 50, dumping_factor=0.5) new_vertices = res[9 * n_vertices:].reshape(-1, 3).T return new_vertices
def __set_costs(self, ocp): if ocp.nb_phases != 1: raise NotImplementedError("ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) if self.acados_ocp.cost.cost_type == "LINEAR_LS": raise RuntimeError("LINEAR_LS is not interfaced yet.") elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J]) else: self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J]) elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()) ) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0])) if J[0]["target"] is not None: self.y_ref_end.append( [J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"] ) else: self.y_ref_end.append([0] * (J[0]["val"].numel())) else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.J, for now. if self.params: for j, J in enumerate(ocp.J): mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel())) ) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0])) if J[0]["target"] is not None: self.y_ref_end.append( [J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"] ) else: self.y_ref_end.append([0] * (J[0]["val"].numel())) # Set costs self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1) self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0] # Set weight self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],)) self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],)) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def gen_lat_model(): model = AcadosModel() model.name = 'lat' # set up states & controls x_ego = SX.sym('x_ego') y_ego = SX.sym('y_ego') psi_ego = SX.sym('psi_ego') curv_ego = SX.sym('curv_ego') v_ego = SX.sym('v_ego') rotation_radius = SX.sym('rotation_radius') model.x = vertcat(x_ego, y_ego, psi_ego, curv_ego, v_ego, rotation_radius) # controls curv_rate = SX.sym('curv_rate') model.u = vertcat(curv_rate) # xdot x_ego_dot = SX.sym('x_ego_dot') y_ego_dot = SX.sym('y_ego_dot') psi_ego_dot = SX.sym('psi_ego_dot') curv_ego_dot = SX.sym('curv_ego_dot') v_ego_dot = SX.sym('v_ego_dot') rotation_radius_dot = SX.sym('rotation_radius_dot') model.xdot = vertcat(x_ego_dot, y_ego_dot, psi_ego_dot, curv_ego_dot, v_ego_dot, rotation_radius_dot) # dynamics model f_expl = vertcat( v_ego * cos(psi_ego) - rotation_radius * sin(psi_ego) * (v_ego * curv_ego), v_ego * sin(psi_ego) + rotation_radius * cos(psi_ego) * (v_ego * curv_ego), v_ego * curv_ego, curv_rate, 0.0, 0.0) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model
h_lb = 0 eta_ub = 200 eta_lb = 0 # Abtastzeit und Prediction horizon T = 0.1 N_pred = 3 # Zielruhelage von den Zustaenden # Referenz von h, h_p, eta state_r = [0.8, 0, 48] # activate_ips_on_exception() # IPS() # Zustaende als Symbol definieren h = SX.sym('h') h_p = SX.sym('h_p') eta = SX.sym('eta') # Zustandsvektor states = vertcat(h, h_p, eta) # print(states.shape) # kriegen die Anzahl der Zeiler("shape" ist ein Tuple) n_states = states.shape[0] # p 22. # Eingang symbolisch definieren u_pwm = SX.sym('u_pwm') # Eingangsverktor controls = vertcat(u_pwm) # Eingangsanzahl n_controls = controls.shape[0] # Zustandsdarstellung
def __set_costs(self, ocp): if ocp.nb_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) ctrl_objs = [ PenaltyType.MINIMIZE_TORQUE, PenaltyType.MINIMIZE_MUSCLES_CONTROL, PenaltyType.MINIMIZE_ALL_CONTROLS, ] state_objs = [ PenaltyType.MINIMIZE_STATE, ] if self.acados_ocp.cost.cost_type == "LINEAR_LS": self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu) self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx) self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx) for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: if J[0]["objective"].type.value[0] in ctrl_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nu))) vu = np.zeros(ocp.nlp[0].nu) vu[index] = 1.0 self.Vu = np.vstack((self.Vu, np.diag(vu))) self.Vx = np.vstack( (self.Vx, np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nu)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nu, 1)) y_ref_tp = [] for J_tp in J: y_tp[index] = J_tp["target"].T.reshape( (-1, 1)) y_ref_tp.append(y_tp) self.y_ref.append(y_ref_tp) else: self.y_ref.append([ np.zeros((ocp.nlp[0].nu, 1)) for J_tp in J ]) elif J[0]["objective"].type.value[0] in state_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vx = np.zeros(ocp.nlp[0].nx) vx[index] = 1.0 self.Vx = np.vstack((self.Vx, np.diag(vx))) self.Vu = np.vstack( (self.Vu, np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_ref_tp = [] for J_tp in J: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J_tp["target"].T.reshape( (-1, 1)) y_ref_tp.append(y_tp) self.y_ref.append(y_ref_tp) else: self.y_ref.append([ np.zeros((ocp.nlp[0].nx, 1)) for J_tp in J ]) else: raise RuntimeError( f"{J[0]['objective'].type.name} is an incompatible objective term with " f"LINEAR_LS cost type") # Deal with last node to match ipopt formulation if J[0]["objective"].node[0].value == "all" and len( J) > ocp.nlp[0].ns: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vxe = np.zeros(ocp.nlp[0].nx) vxe[index] = 1.0 self.Vxe = np.vstack((self.Vxe, np.diag(vxe))) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J[-1]["target"].T.reshape( (-1, 1)) self.y_ref_end.append(y_tp) else: self.y_ref_end.append( np.zeros((ocp.nlp[0].nx, 1))) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: if J[0]["objective"].type.value[0] in state_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vxe = np.zeros(ocp.nlp[0].nx) vxe[index] = 1.0 self.Vxe = np.vstack((self.Vxe, np.diag(vxe))) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J[-1]["target"].T.reshape( (-1, 1)) self.y_ref_end.append(y_tp) else: self.y_ref_end.append( np.zeros((ocp.nlp[0].nx, 1))) else: raise RuntimeError( f"{J[0]['objective'].type.name} is an incompatible objective term " f"with LINEAR_LS cost type") else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.params: raise RuntimeError( "Params not yet handled with LINEAR_LS cost type") # Set costs self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[ 0] else np.zeros((0, 0)) # Set dimensions self.acados_ocp.dims.ny = sum( [len(data[0]) for data in self.y_ref]) self.acados_ocp.dims.ny_e = sum( [len(data) for data in self.y_ref_end]) # Set weight self.acados_ocp.cost.W = self.W self.acados_ocp.cost.W_e = self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([ J_tp["target"].T.reshape((-1, 1)) for J_tp in J ]) else: self.y_ref.append([ np.zeros((J_tp["val"].numel(), 1)) for J_tp in J ]) # Deal with last node to match ipopt formulation if J[0]["objective"].node[0].value == "all" and len( J) > ocp.nlp[0].ns: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape( (-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append( J[-1]["target"].T.reshape((-1, 1))) else: self.y_ref_end.append( np.zeros((J[-1]["val"].numel(), 1))) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append(J[0]["target"].T.reshape( (-1, 1))) else: self.y_ref_end.append( np.zeros((J[0]["val"].numel(), 1))) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.J, for now. if self.params: for j, J in enumerate(ocp.J): mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append(J[0]["target"].T.reshape( (-1, 1))) else: self.y_ref_end.append( np.zeros((J[0]["val"].numel(), 1))) # Set costs self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel( ) else SX(1, 1) self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel( ) else SX(1, 1) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] # Set weight self.acados_ocp.cost.W = np.zeros( (1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros( (1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError( "EXTERNAL is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." )
def __set_constrs(self, ocp): # constraints handling in self.acados_ocp u_min = np.array(ocp.nlp[0].u_bounds.min) u_max = np.array(ocp.nlp[0].u_bounds.max) x_min = np.array(ocp.nlp[0].x_bounds.min) x_max = np.array(ocp.nlp[0].x_bounds.max) self.all_constr = SX() self.end_constr = SX() ##TODO:change for more node flexibility on bounds self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) for i in range(ocp.nb_phases): for g, G in enumerate(ocp.nlp[i].g): if not G: continue if G[0]["constraint"].node[0] is Node.ALL: self.all_constr = vertcat(self.all_constr, G[0]["val"].reshape((-1, 1))) self.all_g_bounds.concatenate(G[0]["bounds"]) if len(G) > ocp.nlp[0].ns: constr_end_func_tp = Function( f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]], [G[0]["val"]]) self.end_constr = vertcat( self.end_constr, constr_end_func_tp(ocp.nlp[i].X[0]).reshape( (-1, 1))) self.end_g_bounds.concatenate(G[0]["bounds"]) elif G[0]["constraint"].node[0] is Node.END: constr_end_func_tp = Function( f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]], [G[0]["val"]]) self.end_constr = vertcat( self.end_constr, constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) self.end_g_bounds.concatenate(G[0]["bounds"]) else: raise RuntimeError( "Except for states and controls, Acados solver only handles constraints on last or all nodes." ) self.acados_model.con_h_expr = self.all_constr self.acados_model.con_h_expr_e = self.end_constr if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)): raise NotImplementedError( "u_bounds min must be the same at each shooting point with ACADOS" ) if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)): raise NotImplementedError( "u_bounds max must be the same at each shooting point with ACADOS" ) if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all() or not np.isfinite(u_max).all() or not np.isfinite(x_max).all()): raise NotImplementedError( "u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it" "to a big value instead.") # setup state constraints self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) param_bounds_max = [] param_bounds_min = [] if self.params: param_bounds_max = np.concatenate( [self.params[key].bounds.max for key in self.params.keys()])[:, 0] param_bounds_min = np.concatenate( [self.params[key].bounds.min for key in self.params.keys()])[:, 0] for i in range(3): self.x_bound_max[:, i] = np.concatenate( (param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i]))) self.x_bound_min[:, i] = np.concatenate( (param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i]))) # setup control constraints self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0]) self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0]) self.acados_ocp.constraints.idxbu = np.array( range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # initial state constraints self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0] self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0] self.acados_ocp.constraints.idxbx_0 = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # setup path state constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1] self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1] self.acados_ocp.constraints.idxbx = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # setup terminal state constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1] self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1] self.acados_ocp.constraints.idxbx_e = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx # setup algebraic constraint self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0]) # setup terminal algebraic constraint self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0])
def __init__(self, ocp, **solver_options): """ Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram solver_options: dict The options to pass to the solver """ if not isinstance(ocp.cx(), SX): raise RuntimeError( "CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP" ) super().__init__(ocp) # If Acados is installed using the acados_install.sh file, you probably can leave this to unset acados_path = "" if "acados_dir" in solver_options: acados_path = solver_options["acados_dir"] self.acados_ocp = AcadosOcp(acados_path=acados_path) self.acados_model = AcadosModel() if "cost_type" in solver_options: self.__set_cost_type(solver_options["cost_type"]) else: self.__set_cost_type() if "constr_type" in solver_options: self.__set_constr_type(solver_options["constr_type"]) else: self.__set_constr_type() self.lagrange_costs = SX() self.mayer_costs = SX() self.y_ref = [] self.y_ref_end = [] self.nparams = 0 self.params_initial_guess = None self.params_bounds = None self.__acados_export_model(ocp) self.__prepare_acados(ocp) self.ocp_solver = None self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) self.status = None self.out = {} self.all_constr = None self.end_constr = SX() self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].controls.shape) self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape) self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape)
from casadi import SX, DM, vertcat, reshape, Function, nlpsol, inf, norm_2, sqrt, gradient, dot import matplotlib.pyplot as plt import random if __name__ == '__main__': # Parameter konfiguration A_B = 2.8274e-3 # [m**2] A_SP = 0.4299e-3 # [m**2] m = 2.8e-3 # [kg] g = 9.81 # [m/(s**2)] T_M = 0.57 # [s] k_M = 0.31 # [s**-1] k_V = 6e-5 # [m**3] k_L = 2.18e-4 # [kg/m] eta_0 = 1900 / 60 # / 60 * 2 * pi A_d = SX([[1, 0.1, 0], [0, -0.14957, 0.024395], [0, 0, 0.82456]]) B_d = SX([[0], [0], [0.054386]]) T = 0.1 h = SX.sym('h') h_p = SX.sym('h_p') eta = SX.sym('eta') states = vertcat(h, h_p, eta) u_pwm = SX.sym('u_pwm') controls = vertcat(u_pwm) rhs = vertcat(h_p, k_L / m * ((k_V * (eta + eta_0) - A_B * h_p) / A_SP)**2 - g, -1 / T_M * eta + k_M / T_M * u_pwm) f = Function('f', [states, controls], [rhs * T + states])
def solve(problem_spec): # problem_spec is dummy t = sp.Symbol('t') # time variable np = 2 nq = 2 ns = 2 n = np + nq + ns p1, p2 = pp = st.symb_vector("p1:{0}".format(np + 1)) q1, q2 = qq = st.symb_vector("q1:{0}".format(nq + 1)) s1, s2 = ss = st.symb_vector("s1:{0}".format(ns + 1)) ttheta = st.row_stack(qq[0], pp[0], ss[0], qq[1], pp[1], ss[1]) qdot1, pdot1, sdot1, qdot2, pdot2, sdot2 = tthetad = st.time_deriv(ttheta, ttheta) tthetadd = st.time_deriv(ttheta, ttheta, order=2) ttheta_all = st.concat_rows(ttheta, tthetad, tthetadd) c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g = params = sp.symbols( 'c1, c2, c3, c4, c5, c6, m1, m2, m3, m4, m5, m6, J1, J2, J3, J4, J5, J6, l1, l2, l3, l4, l5, l6, d, g') tau1, tau2, tau3, tau4, tau5, tau6 = ttau = st.symb_vector("tau1, tau2, tau3, tau4, tau5, tau6 ") # unit vectors ex = sp.Matrix([1, 0]) ey = sp.Matrix([0, 1]) # coordinates of centers of mass and joints # left G0 = 0 * ex ##: C1 = G0 + Rz(q1) * ex * c1 ##: G1 = G0 + Rz(q1) * ex * l1 ##: C2 = G1 + Rz(q1 + p1) * ex * c2 ##: G2 = G1 + Rz(q1 + p1) * ex * l2 ##: C3 = G2 + Rz(q1 + p1 + s1) * ex * c3 ##: G3 = G2 + Rz(q1 + p1 + s1) * ex * l3 ##: # right G6 = d * ex ##: C6 = G6 + Rz(q2) * ex * c6 ##: G5 = G6 + Rz(q2) * ex * l6 ##: C5 = G5 + Rz(q2 + p2) * ex * c5 ##: G4 = G5 + Rz(q2 + p2) * ex * l5 ##: C4 = G4 + Rz(q2 + p2 + s2) * ex * c4 ##: G3b = G4 + Rz(q2 + p2 + s2) * ex * l4 ##: # time derivatives of centers of mass Sd1, Sd2, Sd3, Sd4, Sd5, Sd6 = st.col_split(st.time_deriv(st.col_stack(C1, C2, C3, C4, C5, C6), ttheta)) # Kinetic Energy (note that angles are relative) T_rot = (J1 * qdot1 ** 2) / 2 + (J2 * (qdot1 + pdot1) ** 2) / 2 + (J3 * (qdot1 + pdot1 + sdot1) ** 2) / 2 + \ (J4 * (qdot2 + pdot2 + sdot2) ** 2) / 2 + (J5 * (qdot2 + pdot2) ** 2) / 2 + (J6 * qdot2 ** 2) / 2 T_trans = ( m1 * Sd1.T * Sd1 + m2 * Sd2.T * Sd2 + m3 * Sd3.T * Sd3 + m4 * Sd4.T * Sd4 + m5 * Sd5.T * Sd5 + m6 * Sd6.T * Sd6) / 2 T = T_rot + T_trans[0] # Potential Energy V = m1 * g * C1[1] + m2 * g * C2[1] + m3 * g * C3[1] + m4 * g * C4[1] + m5 * g * C5[1] + m6 * g * C6[1] parameter_values = list(dict(c1=0.4 / 2, c2=0.42 / 2, c3=0.55 / 2, c4=0.55 / 2, c5=0.42 / 2, c6=0.4 / 2, m1=6.0, m2=12.0, m3=39.6, m4=39.6, m5=12.0, m6=6.0, J1=(6 * 0.4 ** 2) / 12, J2=(12 * 0.42 ** 2) / 12, J3=(39.6 * 0.55 ** 2) / 12, J4=(39.6 * 0.55 ** 2) / 12, J5=(12 * 0.42 ** 2) / 12, J6=(6 * 0.4 ** 2) / 12, l1=0.4, l2=0.42, l3=0.55, l4=0.55, l5=0.42, l6=0.4, d=0.26, g=9.81).items()) external_forces = [tau1, tau2, tau3, tau4, tau5, tau6] dir_of_this_file = os.path.dirname(os.path.abspath(sys.modules.get(__name__).__file__)) fpath = os.path.join(dir_of_this_file, "7L-dae-2020-07-15.pcl") if not os.path.isfile(fpath): # if model is not present it could be regenerated # however this might take long (approx. 20min) mod = mt.generate_symbolic_model(T, V, ttheta, external_forces, constraints=[G3 - G3b], simplify=False) mod.calc_state_eq(simplify=False) mod.f_sympy = mod.f.subs(parameter_values) mod.G_sympy = mod.g.subs(parameter_values) with open(fpath, "wb") as pfile: pickle.dump(mod, pfile) else: with open(fpath, "rb") as pfile: mod = pickle.load(pfile) # calculate DAE equations from symbolic model dae = mod.calc_dae_eq(parameter_values) dae.generate_eqns_funcs() torso1_unit = Rz(q1 + p1 + s1) * ex torso2_unit = Rz(q2 + p2 + s2) * ex neck_length = 0.12 head_radius = 0.1 body_width = 15 neck_width = 15 H1 = G3 + neck_length * torso1_unit H1r = G3 + (neck_length - head_radius) * torso1_unit H2 = G3b + neck_length * torso2_unit H2r = G3b + (neck_length - head_radius) * torso2_unit vis = vt.Visualiser(ttheta, xlim=(-1.5, 1.5), ylim=(-.2, 2)) # get default colors and set them explicitly # this prevents color changes in onion skin plot default_colors = plt.get_cmap("tab10") guy1_color = default_colors(0) guy1_joint_color = "darkblue" guy2_color = default_colors(1) guy2_joint_color = "red" guy1_head_fc = guy1_color # facecolor guy1_head_ec = guy1_head_fc # edgecolor guy2_head_fc = guy2_color # facecolor guy2_head_ec = guy2_head_fc # edgecolor # guy 1 body vis.add_linkage(st.col_stack(G0, G1, G2, G3).subs(parameter_values), color=guy1_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy1_joint_color) # guy 1 neck # vis.add_linkage(st.col_stack(G3, H1r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 1 head vis.add_disk(st.col_stack(H1, H1r).subs(parameter_values), fc=guy1_head_fc, ec=guy1_head_ec, plot_radius=False, fill=True) # guy 2 body vis.add_linkage(st.col_stack(G6, G5, G4, G3b).subs(parameter_values), color=guy2_color, solid_capstyle='round', lw=body_width, ms=body_width, mfc=guy2_joint_color) # guy 2 neck # vis.add_linkage(st.col_stack(G3b, H2r).subs(parameter_values), color=head_color, solid_capstyle='round', lw=neck_width) # guy 2 head vis.add_disk(st.col_stack(H2, H2r).subs(parameter_values), fc=guy2_head_fc, ec=guy2_head_ec, plot_radius=False, fill=True) eq_stat = mod.eqns.subz0(tthetadd).subz0(tthetad).subs(parameter_values) # vector for tau and lambda together ttau_symbols = sp.Matrix(mod.uu) ##:T mmu = st.row_stack(ttau_symbols, mod.llmd) ##:T # linear system of equations (and convert to function w.r.t. ttheta) K0_expr = eq_stat.subz0(mmu) ##:i K1_expr = eq_stat.jacobian(mmu) ##:i K0_func = st.expr_to_func(ttheta, K0_expr) K1_func = st.expr_to_func(ttheta, K1_expr, keep_shape=True) def get_mu_stat_for_theta(ttheta_arg, rho=10): # weighting matrix for mu K0 = K0_func(*ttheta_arg) K1 = K1_func(*ttheta_arg) return solve_qlp(K0, K1, rho) def solve_qlp(K0, K1, rho): R_mu = npy.diag([1, 1, 1, rho, rho, rho, .1, .1]) n1, n2 = K1.shape # construct the equation system for least squares with linear constraints M1 = npy.column_stack((R_mu, K1.T)) M2 = npy.column_stack((K1, npy.zeros((n1, n1)))) M_coeff = npy.row_stack((M1, M2)) M_rhs = npy.concatenate((npy.zeros(n2), -K0)) mmu_stat = npy.linalg.solve(M_coeff, M_rhs)[:n2] return mmu_stat ttheta_start = npy.r_[0.9, 1.5, -1.9, 2.1, -2.175799453493845, 1.7471971159642905] mmu_start = get_mu_stat_for_theta(ttheta_start) connection_point_func = st.expr_to_func(ttheta, G3.subs(parameter_values)) cs_ttau = mpc.casidify(mod.uu, mod.uu)[0] cs_llmd = mpc.casidify(mod.llmd, mod.llmd)[0] controls_sp = mmu controls_cs = cs.vertcat(cs_ttau, cs_llmd) coords_cs, _ = mpc.casidify(ttheta, ttheta) # parameters: 0: value of y_connection, 1: x_connection_last, # 2: y_connection_last, 3: delta_r_max, 4: rho (penalty factor for 2nd persons torques), # 5:11: ttheta_old[6], 11:17: ttheta:old2 # P = SX.sym('P', 5 + 12) rho = P[10] # weightning of inputs R = mpc.SX_diag_matrix((1, 1, 1, rho, rho, rho, 0.1, 0.1)) # Construction of Constraints g1 = [] # constraints vector (system dynamics) g2 = [] # inequality-constraints closed_chain_constraint, _ = mpc.casidify(mod.dae.constraints, ttheta, cs_vars=coords_cs) connection_position, _ = mpc.casidify(list(G3.subs(parameter_values)), ttheta, cs_vars=coords_cs) ##:i connection_y_value, _ = mpc.casidify([G3[1].subs(parameter_values)], ttheta, cs_vars=coords_cs) ##:i stationary_eqns, _, _ = mpc.casidify(eq_stat, ttheta, controls_sp, cs_vars=(coords_cs, controls_cs)) ##:i g1.extend(mpc.unpack(stationary_eqns)) g1.extend(mpc.unpack(closed_chain_constraint)) # force the connecting joint to a given hight (which will be provided later) g1.append(connection_y_value - P[0]) ng1 = len(g1) # squared distance from the last reference should be smaller than P[3] (delta_r_max): # this will be a restriction between -inf and 0 r = connection_position - P[1:3] g2.append(r.T @ r - P[3]) # change of angles should be smaller than a given bound (P[5:11] are the old coords) coords_old = P[5:11] coords_old2 = P[11:17] pseudo_vel = (coords_cs - coords_old) / 1 pseudo_acc = (coords_cs - 2 * coords_old + coords_old2) / 1 g2.extend(mpc.unpack(pseudo_vel)) g2.extend(mpc.unpack(pseudo_acc)) g_all = mpc.seq_to_SX_matrix(g1 + g2) ### Construction of objective Function obj = controls_cs.T @ R @ controls_cs + 1e5 * pseudo_acc.T @ pseudo_acc + 0.3e6 * pseudo_vel.T @ pseudo_vel OPT_variables = cs.vertcat(coords_cs, controls_cs) # for debugging g_all_cs_func = cs.Function("g_all_cs_func", (OPT_variables, P), (g_all,)) nlp_prob = dict(f=obj, x=OPT_variables, g=g_all, p=P) ipopt_settings = dict(max_iter=5000, print_level=0, acceptable_tol=1e-8, acceptable_obj_change_tol=1e-6) opts = dict(print_time=False, ipopt=ipopt_settings) xx_guess = npy.r_[ttheta_start, mmu_start] # note: g1 contains the equality constraints (system dynamics) (lower bound = upper bound) delta_phi = .05 d_delta_phi = .02 eps = 1e-9 lbg = npy.r_[[-eps] * ng1 + [-inf] + [-delta_phi] * n, [-d_delta_phi] * n] ubg = npy.r_[[eps] * ng1 + [0] + [delta_phi] * n, [d_delta_phi] * n] # ubx = [inf]*OPT_variables.shape[0]##: # lower and upper bounds for decision variables: # lbx = [-inf, -inf, -inf, -inf, -inf, -inf, -inf, -inf]*N1 + [tau1_min, tau4_min, -inf, -inf]*N # ubx = [inf, inf, inf, inf, inf, inf, inf, inf]*N1 + [tau1_max, tau4_max, inf, inf]*N rho = 3 P_init = npy.r_[connection_point_func(*ttheta_start)[1], connection_point_func(*ttheta_start), 0.01, rho, ttheta_start, ttheta_start] args = dict(lbx=-inf, ubx=inf, lbg=lbg, ubg=ubg, # unconstrained optimization p=P_init, # initial and final state x0=xx_guess # initial guess ) solver = cs.nlpsol("solver", "ipopt", nlp_prob, opts) sol = solver(**args) global_vars = ipydex.Container(old_sol=xx_guess, old_sol2=xx_guess) def get_optimal_equilibrium(y_value, rho=3): ttheta_old = global_vars.old_sol[:n] ttheta_old2 = global_vars.old_sol2[:n] opt_prob_params = npy.r_[y_value, connection_point_func(*ttheta_old), 0.01, rho, ttheta_old, ttheta_old2] args.update(dict(p=opt_prob_params, x0=global_vars.old_sol)) sol = solver(**args) stats = solver.stats() if not stats['success']: raise ValueError(stats["return_status"]) XX = sol["x"].full().squeeze() # save the last two results global_vars.old_sol2 = global_vars.old_sol global_vars.old_sol = XX return XX y_start = connection_point_func(*ttheta_start)[1] N = 100 y_end = 1.36 y_func = st.expr_to_func(t, st.condition_poly(t, (0, y_start, 0, 0), (1, y_end, 0, 0))) def get_qs_trajectory(rho): pseudo_time = npy.linspace(0, 1, N) yy_connection = y_func(pseudo_time) # reset the initial guess global_vars.old_sol = xx_guess global_vars.old_sol2 = xx_guess XX_list = [] for i, y_value in enumerate(yy_connection): # print(i, y_value) XX_list.append(get_optimal_equilibrium(y_value, rho=rho)) XX = npy.array(XX_list) return XX rho = 30 XX = get_qs_trajectory(rho=rho) def smooth_time_scaling(Tend, N, phase_fraction=.5): """ :param Tend: :param N: :param phase_fraction: fraction of Tend for smooth initial and end phase """ T0 = 0 T1 = Tend * phase_fraction y0 = 0 y1 = 1 # for initial phase poly1 = st.condition_poly(t, (T0, y0, 0, 0), (T1, y1, 0, 0)) # for end phase poly2 = poly1.subs(t, Tend - t) # there should be a phase in the middle with constant slope deriv_transition = st.piece_wise((y0, t < T0), (poly1, t < T1), (y1, t < Tend - T1), (poly2, t < Tend), (y0, True)) scaling = sp.integrate(deriv_transition, (t, T0, Tend)) time_transition = sp.integrate(deriv_transition * N / scaling, t) # deriv_transition_func = st.expr_to_func(t, full_transition) time_transition_func = st.expr_to_func(t, time_transition) deriv_func = st.expr_to_func(t, deriv_transition * N / scaling) deriv_func2 = st.expr_to_func(t, deriv_transition.diff(t) * N / scaling) C = ipydex.Container(fetch_locals=True) return C N = XX.shape[0] Tend = 4 res = smooth_time_scaling(Tend, N) def get_derivatives(XX, time_scaling, res=100): """ :param XX: Nxm array :param time_scaling: container for time scaling :param res: time resolution of the returned arrays """ N = XX.shape[0] Tend = time_scaling.Tend assert npy.isclose(time_scaling.time_transition_func([0, Tend])[-1], N) tt = npy.linspace(time_scaling.T0, time_scaling.Tend, res) NN = npy.arange(N) # high_resolution version of index arry NN2 = npy.linspace(0, N, res, endpoint=False) # time-scaled verion of index-array NN3 = time_scaling.time_transition_func(tt) NN3d = time_scaling.deriv_func(tt) NN3dd = time_scaling.deriv_func2(tt) XX_num, XXd_num, XXdd_num = [], [], [] # iterate over every column for col in XX.T: spl = splrep(NN, col) # function value and derivatives XX_num.append(splev(NN3, spl)) XXd_num.append(splev(NN3, spl, der=1)) XXdd_num.append(splev(NN3, spl, der=2)) XX_num = npy.array(XX_num).T XXd_num = npy.array(XXd_num).T XXdd_num = npy.array(XXdd_num).T NN3d_bc = npy.broadcast_to(NN3d, XX_num.T.shape).T NN3dd_bc = npy.broadcast_to(NN3dd, XX_num.T.shape).T XXd_n = XXd_num * NN3d_bc # apply chain rule XXdd_n = XXdd_num * NN3d_bc ** 2 + XXd_num * NN3dd_bc C = ipydex.Container(fetch_locals=True) return C C = XX_derivs = get_derivatives(XX[:, :], time_scaling=res) expr = mod.eqns.subz0(mod.uu, mod.llmd).subs(parameter_values) dynterm_func = st.expr_to_func(ttheta_all, expr) def get_torques(dyn_term_func, XX_derivs, static1=False, static2=False): ttheta_num_all = npy.c_[XX_derivs.XX_num[:, :n], XX_derivs.XXd_n[:, :n], XX_derivs.XXdd_n[:, :n]] ##:S if static1: # set velocities to 0 ttheta_num_all[:, n:2 * n] = 0 if static2: # set accelerations to 0 ttheta_num_all[:, 2 * n:] = 0 res = dynterm_func(*ttheta_num_all.T) return res lhs_static = get_torques(dynterm_func, XX_derivs, static1=True, static2=True) ##:i lhs_dynamic = get_torques(dynterm_func, XX_derivs, static2=False) ##:i mmu_stat_list = [] for L_k_stat, L_k_dyn, ttheta_k in zip(lhs_static, lhs_dynamic, XX_derivs.XX_num[:, :n]): K1_k = K1_func(*ttheta_k) mmu_stat_k = solve_qlp(L_k_stat, K1_k, rho) mmu_stat_list.append(mmu_stat_k) mmu_stat_all = npy.array(mmu_stat_list) solution_data = SolutionData() solution_data.tt = XX_derivs.tt solution_data.xx = XX_derivs.XX_num solution_data.mmu = mmu_stat_all solution_data.vis = vis save_plot(problem_spec, solution_data) return solution_data
from ipydex import IPS, activate_ips_on_exception from sys import path from casadi import sin, cos, SX, DM, vertcat, reshape, Function, nlpsol, inf, norm_2, sqrt, gradient, dot import matplotlib.pyplot as plt import random if __name__ == '__main__': # Parameter konfiguration A_d = SX([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) B_d = SX([[0, 0], [1, 0], [0, 1]]) T = 0.1 temp_x1 = 0.2 temp_x2 = 5 temp_x3 = 0 N_fe_range = 10 # Zustaende als Symbol definieren x = SX.sym('x') y = SX.sym('y') theta = SX.sym('theta') # ZustandsvekFtor states = vertcat(x, y, theta) # print(states.shape) # Eingang symbolisch definieren u_1 = SX.sym('u_1') u_2 = SX.sym("u_2") # Eingangsverktor controls = vertcat(u_1, u_2)
def __set_costs(self, ocp): # set weight for states and controls (default: 1.00) # Q = 1.00 * np.eye(self.acados_ocp.dims.nx) # R = 1.00 * np.eye(self.acados_ocp.dims.nu) # self.acados_ocp.cost.W = linalg.block_diag(Q, R) # self.acados_ocp.cost.W_e = Q self.y_ref = [] self.y_ref_end = [] if self.acados_ocp.cost.cost_type == "LINEAR_LS": # set Lagrange terms self.acados_ocp.cost.Vx = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nx)) self.acados_ocp.cost.Vx[: self.acados_ocp.dims.nx, :] = np.eye(self.acados_ocp.dims.nx) Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu)) Vu[self.acados_ocp.dims.nx :, :] = np.eye(self.acados_ocp.dims.nu) self.acados_ocp.cost.Vu = Vu # set Mayer term self.acados_ocp.cost.Vx_e = np.zeros((self.acados_ocp.dims.nx, self.acados_ocp.dims.nx)) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": if ocp.nb_phases != 1: # TODO: Please confirm this raise NotImplementedError("ACADOS with more than one phase is not implemented yet") for i in range(ocp.nb_phases): # TODO: I think ocp.J is missing here (the parameters would be stored there) for j, J in enumerate(ocp.nlp[i]["J"]): if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J]) else: raise RuntimeError("Should we put y_ref = zeros?") elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction: raise RuntimeError("TODO: I may have broken this (is this the right J?)") mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]]) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) if J[0]["target"] is not None: self.y_ref_end.append([J[0]["target"]]) else: raise RuntimeError("TODO: Should we put y_ref_end = zeros?") else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") if self.lagrange_costs.numel(): self.acados_ocp.model.cost_y_expr = self.lagrange_costs else: self.acados_ocp.model.cost_y_expr = SX(1, 1) if self.mayer_costs.numel(): self.acados_ocp.model.cost_y_expr_e = self.mayer_costs else: self.acados_ocp.model.cost_y_expr_e = SX(1, 1) self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0] self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1),)) self.acados_ocp.cost.yref_e = np.zeros((max(self.acados_ocp.dims.ny_e, 1),)) # TODO changed hard coded values below (you can use J["weight"]) Q_ocp = np.zeros((15, 15)) np.fill_diagonal(Q_ocp, 1000) R_ocp = np.zeros((4, 4)) np.fill_diagonal(R_ocp, 1000) self.acados_ocp.cost.W = linalg.block_diag(Q_ocp, R_ocp) self.acados_ocp.cost.W_e = np.zeros((1, 1)) # TODO: Is the following useful? # if len(self.y_ref): # self.y_ref = np.vstack(self.y_ref) # else: # self.y_ref = [np.zeros((1, 1))] * self.ocp # if len(self.y_ref_end): # self.y_ref_end = np.vstack(self.y_ref_end) # else: # self.y_ref_end = np.zeros((1, 1)) elif self.acados_ocp.cost.cost_type == "EXTERNAL": for i in range(ocp.nb_phases): for j in range(len(ocp.nlp[i]["J"])): J = ocp.nlp[i]["J"][j][0] raise RuntimeError("TODO: The target is not right currently") if J["type"] == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J["val"][0] - J["target"][0]) elif J["type"] == ObjectiveFunction.MayerFunction: raise RuntimeError("TODO: I may have broken this (is this the right J?)") mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]]) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") self.acados_ocp.model.cost_expr_ext_cost = sum1(self.lagrange_costs) self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs) else: raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.")
def qp_solve(prob, obj, p_init, x_init, y_init, lam_opt, mu_opt, case): """ QP solver for path-following algorithm inputs: prob - problem description obj - problem equations p_init - initial parameter x_init - initial primal variable y_init - initial dual variable lam_opt - Lagrange multipliers of equality and active constraints mu_opt - Lagrange multipliers of inequality constraints outputs: y - solution primal variable qp_val - objective function value qp_exit - return status of QP solver deriv - derivatives of the problem k_zero_tilde - active set index k_plus_tilde - inactive set index grad - gradient of objective function """ print 'Current point x:', x_init #Importing problem to be solved nx, np, neq, niq, name = prob() x, p, f, f_fun, con, conf, ubx, lbx, ubg, lbg = obj( x_init, y_init, p_init, neq, niq, nx, np) #Deteriming constraint types eq_con_ind = array([]) #indices of equality constraints iq_con_ind = array([]) #indices of inequality constraints eq_con = array([]) #equality constraints iq_con = array([]) #inequality constraints for i in range(0, len(lbg[0])): if lbg[0, i] == 0: eq_con = vertcat(eq_con, con[i]) eq_con_ind = append(eq_con_ind, i) elif lbg[0, i] < 0: iq_con = vertcat(iq_con, con[i]) iq_con_ind = append(iq_con_ind, i) # print 'Equality Constraint:', eq_con # print 'Inequality Constraint:', iq_con # if case == 'pure-predictor': # return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt if case == 'predictor-corrector': #Evaluating constraints at current iteration point con_vals = conf(x_init, p_init) #Determining which inequality constraints are active k_plus_tilde = array([]) #active constraint k_zero_tilde = array([]) #inactive constraint tol = 10e-5 #tolerance for i in range(0, len(iq_con_ind)): if ubg[0, i] - tol <= con_vals[i] and con_vals[i] <= ubg[0, i] + tol: k_plus_tilde = append(k_plus_tilde, i) else: k_zero_tilde = append(k_zero_tilde, i) # print 'Active constraints:', k_plus_tilde # print 'Inactive constraints:', k_zero_tilde # print 'Constraint values:', con_vals nk_pt = len(k_plus_tilde) #number of active constraints nk_zt = len(k_zero_tilde) #number of inactive constraints #Calculating Lagrangian lam = SX.sym('lam', neq) #Lagrangian multiplier equality constraints mu = SX.sym('mu', niq) #Lagrangian multiplier inequality constraints lag_f = f + mtimes(lam.T, eq_con) + mtimes( mu.T, iq_con) #Lagrangian equation #Calculating derivatives g = gradient(f, x) #Derivative of objective function g_fun = Function('g_fun', [x, p], [gradient(f, x)]) H = 2 * jacobian(gradient(lag_f, x), x) #Second derivative of the Lagrangian H_fun = Function('H_fun', [x, p, lam, mu], [jacobian(jacobian(lag_f, x), x)]) if len(eq_con_ind) > 0: deq = jacobian(eq_con, x) #Derivative of equality constraints else: deq = array([]) if len(iq_con_ind) > 0: diq = jacobian(iq_con, x) #Derivative of inequality constraints else: diq = array([]) #Creating constraint matrices nc = niq + neq #Total number of constraints if (niq > 0) and (neq > 0): #Equality and inequality constraints #this part needs to be tested if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) print deq A[0, :] = deq #A matrix lba = -1e16 * SX.zeros((nc, 1)) lba[0, :] = -eq_con #lower bound of A uba = 1e16 * SX.zeros((nc, 1)) uba[0, :] = -eq_con #upper bound of A for j in range(0, nk_pt): #adding active constraints A[neq + j + 1, :] = diq[int(k_plus_tilde[j]), :] lba[neq + j + 1] = -iq_con[int(k_plus_tilde[j])] uba[neq + j + 1] = -iq_con[int(k_plus_tilde[i])] for i in range(0, nk_zt): #adding inactive constraints A[neq + nk_pt + i + 1, :] = diq[int(k_zero_tilde[i]), :] uba[neq + nk_pt + i + 1] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: #Active constraints only A = vertcat(deq, diq) lba = vertcat(-eq_con, -iq_con) uba = vertcat(-eq_con, -iq_con) elif (niq > 0) and (neq == 0): #Inquality constraints if (nk_zt > 0): #Inactive constraints exist A = SX.zeros((nc, nx)) lba = -1e16 * SX.ones((nc, 1)) uba = 1e16 * SX.ones((nc, 1)) for j in range(0, nk_pt): #adding active constraints A[j, :] = diq[int(k_plus_tilde[j]), :] lba[j] = -iq_con[int(k_plus_tilde[j])] uba[j] = -iq_con[int(k_plus_tilde[j])] for i in range(0, nk_zt): #adding inactive constraints A[nk_pt + i, :] = diq[int(k_zero_tilde[i]), :] uba[nk_pt + i] = -iq_con[int(k_zero_tilde[i])] #inactive constraints don't have lower bounds else: raw_input() A = vertcat(deq, diq) lba = -iq_con uba = -iq_con elif (niq == 0) and (neq > 0): #Equality constriants A = deq lba = -eq_con uba = -eq_con A_fun = Function('A_fun', [x, p], [A]) lba_fun = Function('lba_fun', [x, p], [lba]) uba_fun = Function('uba_fun', [x, p], [uba]) #Checking that matrices are correct sizes and types if (H.size1() != nx) or (H.size2() != nx) or (H.is_dense() == 'False'): #H matrix should be a sparse (nxn) and symmetrical print( 'WARNING: H matrix is not the correct dimensions or matrix type' ) if (g.size1() != nx) or (g.size2() != 1) or g.is_dense() == 'True': #g matrix should be a dense (nx1) print( 'WARNING: g matrix is not the correct dimensions or matrix type' ) if (A.size1() != (neq + niq)) or (A.size2() != nx) or (A.is_dense() == 'False'): #A should be a sparse (nc x n) print( 'WARNING: A matrix is not the correct dimensions or matrix type' ) if lba.size1() != (neq + niq) or (lba.size2() != 1) or lba.is_dense() == 'False': print( 'WARNING: lba matrix is not the correct dimensions or matrix type' ) if uba.size1() != (neq + niq) or (uba.size2() != 1) or uba.is_dense() == 'False': print( 'WARNING: uba matrix is not the correct dimensions or matrix type' ) #Evaluating QP matrices at optimal points H_opt = H_fun(x_init, p_init, lam_opt, mu_opt) g_opt = g_fun(x_init, p_init) A_opt = A_fun(x_init, p_init) lba_opt = lba_fun(x_init, p_init) uba_opt = uba_fun(x_init, p_init) # print 'Lower bounds', lba_opt # print 'Upper bounds', uba_opt # print 'Bound matrix', A_opt #Defining QP structure qp = {} qp['h'] = H_opt.sparsity() qp['a'] = A_opt.sparsity() optimize = conic('optimize', 'qpoases', qp) optimal = optimize(h=H_opt, g=g_opt, a=A_opt, lba=lba_opt, uba=uba_opt, x0=x_init) x_qpopt = optimal['x'] if x_qpopt.shape == x_init.shape: qp_exit = 'optimal' else: qp_exit = '' lag_qpopt = optimal['lam_a'] #Determing Lagrangian multipliers (lambda and mu) lam_qpopt = zeros( (nk_pt, 1)) #Lagrange multiplier of active constraints mu_qpopt = zeros( (nk_zt, 1)) #Lagrange multiplier of inactive constraints if nk_pt > 0: for j in range(0, len(k_plus_tilde)): lam_qpopt[j] = lag_qpopt[int(k_plus_tilde[j])] if nk_zt > 0: for k in range(0, len(k_zero_tilde)): print lag_qpopt[int(k_zero_tilde[k])] return qp_exit, optimal, x_qpopt, lam_qpopt, mu_qpopt
y_ub = 10 y_lb = -10 # Abtastzeit und Prediction horizon T = 0.1 N_pred = 9 # Zielruhelage von den Zustaenden # Referenz von h, h_p, eta state_r = [0, 0, 0] input_r = [0, 0] if __name__ == '__main__': NMPC_init() # Zustaende als Symbol definieren x = SX.sym('x') y = SX.sym('y') theta = SX.sym('theta') # ZustandsvekFtor states = vertcat(x, y, theta) # print(states.shape) # Eingang symbolisch definieren u_1 = SX.sym('u_1') u_2 = SX.sym("u_2") # Eingangsverktor controls = vertcat(u_1, u_2) # kriegen die Anzahl der Zeiler("shape" ist ein Tuple) n_states = states.shape[0] # p 22.
def main(use_cython=True): # (very) simple crane model beta = 0.001 k = 0.9 a_max = 10 dt_max = 2.0 # states p1 = SX.sym('p1') v1 = SX.sym('v1') p2 = SX.sym('p2') v2 = SX.sym('v2') x = vertcat(p1, v1, p2, v2) # controls a = SX.sym('a') dt = SX.sym('dt') u = vertcat(a, dt) f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1)) model = AcadosModel() model.f_expl_expr = f_expl model.x = x model.u = u model.name = 'crane_time_opt' # create ocp object to formulate the OCP x0 = np.array([2.0, 0.0, 2.0, 0.0]) xf = np.array([0.0, 0.0, 0.0, 0.0]) ocp = AcadosOcp() ocp.model = model # N - maximum number of bangs N = 7 Tf = N nx = model.x.size()[0] nu = model.u.size()[0] # set dimensions ocp.dims.N = N # set cost ocp.cost.cost_type = 'EXTERNAL' ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost = dt ocp.model.cost_expr_ext_cost_e = 0 ocp.constraints.lbu = np.array([-a_max, 0.0]) ocp.constraints.ubu = np.array([+a_max, dt_max]) ocp.constraints.idxbu = np.array([0, 1]) ocp.constraints.x0 = x0 ocp.constraints.lbx_e = xf ocp.constraints.ubx_e = xf ocp.constraints.idxbx_e = np.array([0, 1, 2, 3]) # set prediction horizon ocp.solver_options.tf = Tf # set options ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' #'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 3 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = 'MERIT_BACKTRACKING' ocp.solver_options.nlp_solver_max_iter = 5000 ocp.solver_options.nlp_solver_tol_stat = 1e-6 ocp.solver_options.levenberg_marquardt = 0.1 ocp.solver_options.sim_method_num_steps = 15 ocp.solver_options.qp_solver_iter_max = 100 ocp.code_export_directory = 'c_generated_code' ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.exact_hess_dyn = 0 if use_cython: AcadosOcpSolver.generate(ocp, json_file='acados_ocp.json') AcadosOcpSolver.build(ocp.code_export_directory, with_cython=True) ocp_solver = AcadosOcpSolver.create_cython_solver('acados_ocp.json') else: # ctypes ## Note: skip generate and build assuming this is done before (in cython run) ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', build=False, generate=False) ocp_solver.reset() for i, tau in enumerate(np.linspace(0, 1, N)): ocp_solver.set(i, 'x', (1 - tau) * x0 + tau * xf) ocp_solver.set(i, 'u', np.array([0.1, 0.5])) simX = np.zeros((N + 1, nx)) simU = np.zeros((N, nu)) status = ocp_solver.solve() if status != 0: ocp_solver.print_statistics() raise Exception(f'acados returned status {status}.') # get solution for i in range(N): simX[i, :] = ocp_solver.get(i, "x") simU[i, :] = ocp_solver.get(i, "u") simX[N, :] = ocp_solver.get(N, "x") dts = simU[:, 1] print( "acados solved OCP successfully, creating integrator to simulate the solution" ) # simulate on finer grid sim = AcadosSim() # set model sim.model = model # set options sim.solver_options.integrator_type = 'ERK' sim.solver_options.num_stages = 4 sim.solver_options.num_steps = 3 sim.solver_options.T = 1.0 # dummy value dt_approx = 0.0005 dts_fine = np.zeros((N, )) Ns_fine = np.zeros((N, ), dtype='int16') # compute number of simulation steps for bang interval + dt_fine for i in range(N): N_approx = max(int(dts[i] / dt_approx), 1) dts_fine[i] = dts[i] / N_approx Ns_fine[i] = int(round(dts[i] / dts_fine[i])) N_fine = int(np.sum(Ns_fine)) simU_fine = np.zeros((N_fine, nu)) ts_fine = np.zeros((N_fine + 1, )) simX_fine = np.zeros((N_fine + 1, nx)) simX_fine[0, :] = x0 acados_integrator = AcadosSimSolver(sim) k = 0 for i in range(N): u = simU[i, 0] acados_integrator.set("u", np.hstack((u, np.ones(1, )))) # set simulation time acados_integrator.set("T", dts_fine[i]) for j in range(Ns_fine[i]): acados_integrator.set("x", simX_fine[k, :]) status = acados_integrator.solve() if status != 0: raise Exception(f'acados returned status {status}.') simX_fine[k + 1, :] = acados_integrator.get("x") simU_fine[k, :] = u ts_fine[k + 1] = ts_fine[k] + dts_fine[i] k += 1 # visualize if os.environ.get('ACADOS_ON_TRAVIS'): plt.figure() state_labels = ['p1', 'v1', 'p2', 'v2'] for i, l in enumerate(state_labels): plt.subplot(5, 1, i + 1) plt.plot(ts_fine, simX_fine[:, i], label='time optimal solution') plt.grid(True) plt.ylabel(l) if i == 0: plt.legend(loc=1) plt.subplot(5, 1, 5) plt.step(ts_fine, np.hstack((simU_fine[:, 0], simU_fine[-1, 0])), '-', where='post') plt.grid(True) plt.ylabel('a') plt.xlabel('t') plt.show()
from casadi import SX, Function, vertcat from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver from models import chen_model N = 10 ode_fun, nx, nu = chen_model(implicit=True) nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N * [1] + [0]}) # ODE Model step = 0.1 nlp.set_model(ode_fun, step) # Cost function Q = diag([1.0, 1.0]) R = 1e-2 x = SX.sym('x', nx) u = SX.sym('u', nu) u_N = SX.sym('u', 0) f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)])) f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x])) ls_cost = ocp_nlp_ls_cost(N, N * [f] + [f_N]) ls_cost.ls_cost_matrix = N * [block_diag(Q, R)] + [Q] nlp.set_cost(ls_cost) # Constraints g = ocp_nlp_function(Function('path_constraint', [x, u], [u])) g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])])) nlp.set_path_constraints(N * [g] + [g_N]) for i in range(N): nlp.lg[i] = -0.5 nlp.ug[i] = +0.5
def __set_costs(self, ocp): """ Set the cost functions from ocp Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ def add_linear_ls_lagrange(acados, objectives): def add_objective(n_variables, is_state): v_var = np.zeros(n_variables) var_type = acados.ocp.nlp[ 0].states if is_state else acados.ocp.nlp[0].controls rows = objectives.rows + var_type[ objectives.params["key"]].index[0] v_var[rows] = 1.0 if is_state: acados.Vx = np.vstack((acados.Vx, np.diag(v_var))) acados.Vu = np.vstack( (acados.Vu, np.zeros((n_states, n_controls)))) else: acados.Vx = np.vstack( (acados.Vx, np.zeros((n_controls, n_states)))) acados.Vu = np.vstack((acados.Vu, np.diag(v_var))) acados.W = linalg.block_diag( acados.W, np.diag([objectives.weight] * n_variables)) node_idx = objectives.node_idx[:-1] if objectives.node[ 0] == Node.ALL else objectives.node_idx y_ref = [ np.zeros((n_states if is_state else n_controls, 1)) for _ in node_idx ] if objectives.target is not None: for idx in node_idx: y_ref[idx][rows] = objectives.target[..., idx].T.reshape( (-1, 1)) acados.y_ref.append(y_ref) if objectives.type in allowed_control_objectives: add_objective(n_controls, False) elif objectives.type in allowed_state_objectives: add_objective(n_states, True) else: raise RuntimeError( f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type" ) def add_linear_ls_mayer(acados, objectives): if objectives.type in allowed_state_objectives: vxe = np.zeros(n_states) rows = objectives.rows + acados.ocp.nlp[0].states[ objectives.params["key"]].index[0] vxe[rows] = 1.0 acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe))) acados.W_e = linalg.block_diag( acados.W_e, np.diag([objectives.weight] * n_states)) y_ref_end = np.zeros((n_states, 1)) if objectives.target is not None: y_ref_end[rows] = objectives.target[..., -1].T.reshape( (-1, 1)) acados.y_ref_end.append(y_ref_end) else: raise RuntimeError( f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type" ) def add_nonlinear_ls_lagrange(acados, objectives, x, u, p): acados.lagrange_costs = vertcat( acados.lagrange_costs, objectives.function(x, u, p).reshape((-1, 1))) acados.W = linalg.block_diag( acados.W, np.diag([objectives.weight] * objectives.function.numel_out())) node_idx = objectives.node_idx[:-1] if objectives.node[ 0] == Node.ALL else objectives.node_idx if objectives.target is not None: acados.y_ref.append([ objectives.target[..., idx].T.reshape((-1, 1)) for idx in node_idx ]) else: acados.y_ref.append([ np.zeros((objectives.function.numel_out(), 1)) for _ in node_idx ]) def add_nonlinear_ls_mayer(acados, objectives, x, u, p): acados.W_e = linalg.block_diag( acados.W_e, np.diag([objectives.weight] * objectives.function.numel_out())) x = x if objectives.function.sparsity_in("i0").shape != ( 0, 0) else [] u = u if objectives.function.sparsity_in("i1").shape != ( 0, 0) else [] acados.mayer_costs = vertcat( acados.mayer_costs, objectives.function(x, u, p).reshape((-1, 1))) if objectives.target is not None: acados.y_ref_end.append(objectives.target[..., -1].T.reshape( (-1, 1))) else: acados.y_ref_end.append( np.zeros((objectives.function.numel_out(), 1))) if ocp.n_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL] allowed_state_objectives = [ ObjectiveFcn.Lagrange.MINIMIZE_STATE, ObjectiveFcn.Mayer.TRACK_STATE ] if self.acados_ocp.cost.cost_type == "LINEAR_LS": n_states = ocp.nlp[0].states.shape n_controls = ocp.nlp[0].controls.shape self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls) self.Vx = np.array([], dtype=np.int64).reshape(0, n_states) self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states) for i in range(ocp.n_phases): for J in ocp.nlp[i].J: if not J: continue if J.multi_thread: raise RuntimeError( f"The objective function {J.name} was declared with multi_thread=True, " f"but this is not possible to multi_thread objective function with ACADOS" ) if J.type.get_type() == ObjectiveFunction.LagrangeFunction: add_linear_ls_lagrange(self, J) # Deal with last node to match ipopt formulation if J.node[0] == Node.ALL: add_linear_ls_mayer(self, J) elif J.type.get_type() == ObjectiveFunction.MayerFunction: add_linear_ls_mayer(self, J) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.nparams: raise RuntimeError( "Params not yet handled with LINEAR_LS cost type") # Set costs self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[ 0] else np.zeros((0, 0)) # Set dimensions self.acados_ocp.dims.ny = sum( [len(data[0]) for data in self.y_ref]) self.acados_ocp.dims.ny_e = sum( [len(data) for data in self.y_ref_end]) # Set weight self.acados_ocp.cost.W = self.W self.acados_ocp.cost.W_e = self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i, nlp in enumerate(ocp.nlp): for j, J in enumerate(nlp.J): if not J: continue if J.multi_thread: raise RuntimeError( f"The objective function {J.name} was declared with multi_thread=True, " f"but this is not possible to multi_thread objective function with ACADOS" ) if J.type.get_type() == ObjectiveFunction.LagrangeFunction: add_nonlinear_ls_lagrange(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) # Deal with last node to match ipopt formulation if J.node[0] == Node.ALL: add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) elif J.type.get_type() == ObjectiveFunction.MayerFunction: add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now. if self.nparams: nlp = ocp.nlp[0] # Assume 1 phase for j, J in enumerate(ocp.J): add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) # Set costs self.acados_ocp.model.cost_y_expr = (self.lagrange_costs.reshape( (-1, 1)) if self.lagrange_costs.numel() else SX(1, 1)) self.acados_ocp.model.cost_y_expr_e = (self.mayer_costs.reshape( (-1, 1)) if self.mayer_costs.numel() else SX(1, 1)) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] # Set weight self.acados_ocp.cost.W = np.zeros( (1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros( (1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError( "EXTERNAL is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." )
def test_multistep_trajectory(before_test_onestep_reachability): """ Compare multi-steps 'by hand' with the function """ mu_0, _, gp, k_fb, k_ff, L_mu, L_sigm, c_safety, a, b = before_test_onestep_reachability T = 3 n_u, n_s = np.shape(k_fb) k_fb_cas_single = SX.sym("k_fb_single", (n_u, n_s)) k_ff_cas_single = SX.sym("k_ff_single", (n_u, 1)) k_ff_cas_all = SX.sym("k_ff_single", (T, n_u)) k_fb_cas_all = SX.sym("k_fb_all", (T - 1, n_s * n_u)) k_fb_cas_all_inp = [ k_fb_cas_all[i, :].reshape((n_u, n_s)) for i in range(T - 1) ] mu_0_cas = SX.sym("mu_0", (n_s, 1)) sigma_0_cas = SX.sym("sigma_0", (n_s, n_s)) mu_onestep_no_var_in, sigm_onestep_no_var_in, _ = prop_casadi.one_step_taylor( mu_0_cas, gp, k_ff_cas_single, a=a, b=b) mu_one_step, sigm_onestep, _ = prop_casadi.one_step_taylor( mu_0_cas, gp, k_ff_cas_single, k_fb=k_fb_cas_single, sigma_x=sigma_0_cas, a=a, b=b) mu_multistep, sigma_multistep, _ = prop_casadi.multi_step_taylor_symbolic( mu_0_cas, gp, k_ff_cas_all, k_fb_cas_all_inp, a=a, b=b) on_step_no_var_in = Function( "on_step_no_var_in", [mu_0_cas, k_ff_cas_single], [mu_onestep_no_var_in, sigm_onestep_no_var_in]) one_step = Function( "one_step", [mu_0_cas, sigma_0_cas, k_ff_cas_single, k_fb_cas_single], [mu_one_step, sigm_onestep]) multi_step = Function("multi_step", [mu_0_cas, k_ff_cas_all, k_fb_cas_all], [mu_multistep, sigma_multistep]) # TODO: Need mu, sigma as input aswell mu_1, sigma_1 = on_step_no_var_in(mu_0, k_ff) mu_2, sigma_2 = one_step(mu_1, sigma_1, k_ff, k_fb) mu_3, sigma_3 = one_step(mu_2, sigma_2, k_ff, k_fb) # TODO: stack k_ff and k_fb k_fb_mult = np.array(cas_reshape(k_fb, (1, n_u * n_s))) k_fb_mult = np.array(vertcat(*[k_fb_mult] * (T - 1))) k_ff_mult = vertcat(*[k_ff] * T) mu_all, sigma_all = multi_step(mu_0, k_ff_mult, k_fb_mult) assert np.allclose( np.array(mu_all[0, :]), np.array(mu_1).T), "Are the centers of the first prediction the same?" assert np.allclose( np.array(mu_all[-1, :]), np.array(mu_3).T), "Are the centers of the final prediction the same?" assert np.allclose(cas_reshape(sigma_all[-1, :], (n_s, n_s)), sigma_3), "Are the last covariance matrices the same?"
import matplotlib.pyplot as plt from numpy import array, diag, eye, zeros from scipy.linalg import block_diag from acados import ocp_nlp from casadi import SX, Function, vertcat nx, nu = 2, 1 x = SX.sym('x', nx) u = SX.sym('u', nu) ode_fun = Function('ode_fun', [x, u], [vertcat(x[1], u)], ['x', 'u'], ['rhs']) N = 15 nlp = ocp_nlp(N, nx, nu) nlp.set_dynamics(ode_fun, {'integrator': 'rk4', 'step': 0.1}) q, r = 1, 1 P = eye(nx) nlp.set_stage_cost(eye(nx+nu), zeros(nx+nu), diag([q, q, r])) nlp.set_terminal_cost(eye(nx), zeros(nx), P) x0 = array([1, 1]) nlp.set_field("lbx", 0, x0) nlp.set_field("ubx", 0, x0) nlp.initialize_solver("sqp", {"qp_solver": "qpoases"})
class AcadosInterface(SolverInterface): def __init__(self, ocp, **solver_options): if not isinstance(ocp.CX(), SX): raise RuntimeError( "CasADi graph must be SX to be solved with ACADOS") super().__init__(ocp) # If Acados is installed using the acados_install.sh file, you probably can leave this to unset acados_path = "" if "acados_dir" in solver_options: acados_path = solver_options["acados_dir"] self.acados_ocp = AcadosOcp(acados_path=acados_path) self.acados_model = AcadosModel() if "cost_type" in solver_options: self.__set_cost_type(solver_options["cost_type"]) else: self.__set_cost_type() self.lagrange_costs = SX() self.mayer_costs = SX() self.y_ref = [] self.y_ref_end = [] self.__acados_export_model(ocp) self.__prepare_acados(ocp) self.ocp_solver = None self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) def __acados_export_model(self, ocp): # Declare model variables x = ocp.nlp[0]["X"][0] u = ocp.nlp[0]["U"][0] p = ocp.nlp[0]["p"] mod = ocp.nlp[0]["model"] x_dot = SX.sym("x_dot", mod.nbQdot() * 2, 1) f_expl = ocp.nlp[0]["dynamics_func"](x, u, p) f_impl = x_dot - f_expl self.acados_model.f_impl_expr = f_impl self.acados_model.f_expl_expr = f_expl self.acados_model.x = x self.acados_model.xdot = x_dot self.acados_model.u = u self.acados_model.p = [] self.acados_model.name = "model_name" def __prepare_acados(self, ocp): if ocp.nb_phases > 1: raise NotImplementedError( "more than 1 phase is not implemented yet with ACADOS backend") if ocp.param_to_optimize: raise NotImplementedError( "Parameters optimization is not implemented yet with ACADOS") # set model self.acados_ocp.model = self.acados_model # set dimensions for i in range(ocp.nb_phases): # set time self.acados_ocp.solver_options.tf = ocp.nlp[i]["tf"] # set dimensions self.acados_ocp.dims.nx = ocp.nlp[i]["nx"] self.acados_ocp.dims.nu = ocp.nlp[i]["nu"] self.acados_ocp.dims.ny = self.acados_ocp.dims.nx + self.acados_ocp.dims.nu self.acados_ocp.dims.ny_e = ocp.nlp[i]["nx"] self.acados_ocp.dims.N = ocp.nlp[i]["ns"] for i in range(ocp.nb_phases): # set constraints for j in range(ocp.nlp[i]["nx"]): if ocp.nlp[i]["X_bounds"].min[j, 0] == -np.inf and ocp.nlp[i][ "X_bounds"].max[j, 0] == np.inf: pass elif ocp.nlp[i]["X_bounds"].min[ j, 0] != ocp.nlp[i]["X_bounds"].max[j, 0]: raise RuntimeError( "Initial constraint on state must be hard. Hint: you can pass it as an objective" ) else: self.acados_ocp.constraints.x0 = np.array( ocp.nlp[i]["X_bounds"].min[:, 0]) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # control constraints self.acados_ocp.constraints.constr_type = "BGH" self.acados_ocp.constraints.lbu = np.array( ocp.nlp[i]["U_bounds"].min[:, 0]) self.acados_ocp.constraints.ubu = np.array( ocp.nlp[i]["U_bounds"].max[:, 0]) self.acados_ocp.constraints.idxbu = np.array( range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # path constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.ubx = np.array( ocp.nlp[i]["X_bounds"].max[:, 1]) self.acados_ocp.constraints.lbx = np.array( ocp.nlp[i]["X_bounds"].min[:, 1]) self.acados_ocp.constraints.idxbx = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # terminal constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.ubx_e = np.array( ocp.nlp[i]["X_bounds"].max[:, -1]) self.acados_ocp.constraints.lbx_e = np.array( ocp.nlp[i]["X_bounds"].min[:, -1]) self.acados_ocp.constraints.idxbx_e = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx return self.acados_ocp def __set_cost_type(self, cost_type="NONLINEAR_LS"): self.acados_ocp.cost.cost_type = cost_type self.acados_ocp.cost.cost_type_e = cost_type def __set_costs(self, ocp): self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) if self.acados_ocp.cost.cost_type == "LINEAR_LS": # set Lagrange terms self.acados_ocp.cost.Vx = np.zeros( (self.acados_ocp.dims.ny, self.acados_ocp.dims.nx)) self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye( self.acados_ocp.dims.nx) Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu)) Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu) self.acados_ocp.cost.Vu = Vu # set Mayer term self.acados_ocp.cost.Vx_e = np.zeros( (self.acados_ocp.dims.nx, self.acados_ocp.dims.nx)) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": if ocp.nb_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet") for i in range(ocp.nb_phases): # TODO: I think ocp.J is missing here (the parameters would be stored there) # TODO: Yes the objectives in ocp are not dealt with, # does that makes sense since we only work with 1 phase ? for j, J in enumerate(ocp.nlp[i]["J"]): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([ J_tp["target"].T.reshape((-1, 1)) for J_tp in J ]) else: self.y_ref.append([ np.zeros((J_tp["val"].numel(), 1)) for J_tp in J ]) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) if J[0]["target"] is not None: self.y_ref_end.append([J[0]["target"]]) else: self.y_ref_end.append( [np.zeros((J[0]["val"].numel(), 1))]) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.lagrange_costs.numel(): self.acados_ocp.model.cost_y_expr = self.lagrange_costs else: self.acados_ocp.model.cost_y_expr = SX(1, 1) if self.mayer_costs.numel(): self.acados_ocp.model.cost_y_expr_e = self.mayer_costs else: self.acados_ocp.model.cost_y_expr_e = SX(1, 1) self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1), )) self.acados_ocp.cost.yref_e = np.concatenate(self.y_ref_end, -1).T.squeeze() if self.W.shape == (0, 0): self.acados_ocp.cost.W = np.zeros((1, 1)) else: self.acados_ocp.cost.W = self.W if self.W_e.shape == (0, 0): self.acados_ocp.cost.W_e = np.zeros((1, 1)) else: self.acados_ocp.cost.W_e = self.W_e elif self.acados_ocp.cost.cost_type == "EXTERNAL": for i in range(ocp.nb_phases): for j in range(len(ocp.nlp[i]["J"])): J = ocp.nlp[i]["J"][j][0] raise RuntimeError( "TODO: The target is not right currently") if J["type"] == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J["val"][0] - J["target"][0]) elif J["type"] == ObjectiveFunction.MayerFunction: raise RuntimeError( "TODO: I may have broken this (is this the right J?)" ) mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]]) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) self.acados_ocp.model.cost_expr_ext_cost = sum1( self.lagrange_costs) self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs) else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." ) def configure(self, options): if "acados_dir" in options: del options["acados_dir"] if "cost_type" in options: del options["cost_type"] self.acados_ocp.solver_options.qp_solver = "FULL_CONDENSING_QPOASES" # FULL_CONDENSING_QPOASES self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON" self.acados_ocp.solver_options.integrator_type = "ERK" self.acados_ocp.solver_options.nlp_solver_type = "SQP" self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06 self.acados_ocp.solver_options.nlp_solver_max_iter = 200 self.acados_ocp.solver_options.sim_method_newton_iter = 5 self.acados_ocp.solver_options.sim_method_num_stages = 4 self.acados_ocp.solver_options.sim_method_num_steps = 1 self.acados_ocp.solver_options.print_level = 1 for key in options: setattr(self.acados_ocp.solver_options, key, options[key]) def get_iterations(self): raise NotImplementedError( "return_iterations is not implemented yet with ACADOS backend") def online_optim(self, ocp): raise NotImplementedError( "online_optim is not implemented yet with ACADOS backend") def get_optimized_value(self, ocp): acados_x = np.array([ self.ocp_solver.get(i, "x") for i in range(ocp.nlp[0]["ns"] + 1) ]).T acados_q = acados_x[:ocp.nlp[0]["nu"], :] acados_qdot = acados_x[ocp.nlp[0]["nu"]:, :] acados_u = np.array( [self.ocp_solver.get(i, "u") for i in range(ocp.nlp[0]["ns"])]).T out = { "qqdot": acados_x, "x": [], "u": acados_u, "time_tot": self.ocp_solver.get_stats("time_tot")[0], } for i in range(ocp.nlp[0]["ns"]): out["x"] = vertcat(out["x"], acados_q[:, i]) out["x"] = vertcat(out["x"], acados_qdot[:, i]) out["x"] = vertcat(out["x"], acados_u[:, i]) out["x"] = vertcat(out["x"], acados_q[:, ocp.nlp[0]["ns"]]) out["x"] = vertcat(out["x"], acados_qdot[:, ocp.nlp[0]["ns"]]) return out def solve(self): # populate costs vectors self.__set_costs(self.ocp) if self.ocp_solver is None: self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json") for n in range(self.acados_ocp.dims.N): self.ocp_solver.cost_set( n, "yref", np.concatenate([data[n] for data in self.y_ref])[:, 0]) self.ocp_solver.solve() return self
class AcadosInterface(SolverInterface): def __init__(self, ocp, **solver_options): if not isinstance(ocp.CX(), SX): raise RuntimeError( "CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP" ) super().__init__(ocp) # If Acados is installed using the acados_install.sh file, you probably can leave this to unset acados_path = "" if "acados_dir" in solver_options: acados_path = solver_options["acados_dir"] self.acados_ocp = AcadosOcp(acados_path=acados_path) self.acados_model = AcadosModel() if "cost_type" in solver_options: self.__set_cost_type(solver_options["cost_type"]) else: self.__set_cost_type() if "constr_type" in solver_options: self.__set_constr_type(solver_options["constr_type"]) else: self.__set_constr_type() self.lagrange_costs = SX() self.mayer_costs = SX() self.y_ref = [] self.y_ref_end = [] self.__acados_export_model(ocp) self.__prepare_acados(ocp) self.ocp_solver = None self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) self.status = None self.out = {} def __acados_export_model(self, ocp): if ocp.nb_phases > 1: raise NotImplementedError( "More than 1 phase is not implemented yet with ACADOS backend") # Declare model variables x = ocp.nlp[0].X[0] u = ocp.nlp[0].U[0] p = ocp.nlp[0].p if ocp.nlp[0].parameters_to_optimize: for n in range(ocp.nb_phases): for i in range(len(ocp.nlp[0].parameters_to_optimize)): if str(ocp.nlp[0].p[i]) == f"time_phase_{n}": raise RuntimeError( "Time constraint not implemented yet with Acados.") self.params = ocp.nlp[0].parameters_to_optimize x = vertcat(p, x) x_dot = SX.sym("x_dot", x.shape[0], x.shape[1]) f_expl = vertcat([0] * ocp.nlp[0].np, ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np:, :], u, p)) f_impl = x_dot - f_expl self.acados_model.f_impl_expr = f_impl self.acados_model.f_expl_expr = f_expl self.acados_model.x = x self.acados_model.xdot = x_dot self.acados_model.u = u self.acados_model.con_h_expr = np.zeros((0, 0)) self.acados_model.con_h_expr_e = np.zeros((0, 0)) self.acados_model.p = [] now = datetime.now() # current date and time self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}" def __prepare_acados(self, ocp): # set model self.acados_ocp.model = self.acados_model # set time self.acados_ocp.solver_options.tf = ocp.nlp[0].tf # set dimensions self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np self.acados_ocp.dims.nu = ocp.nlp[0].nu self.acados_ocp.dims.N = ocp.nlp[0].ns def __set_constr_type(self, constr_type="BGH"): self.acados_ocp.constraints.constr_type = constr_type self.acados_ocp.constraints.constr_type_e = constr_type def __set_constrs(self, ocp): # constraints handling in self.acados_ocp u_min = np.array(ocp.nlp[0].u_bounds.min) u_max = np.array(ocp.nlp[0].u_bounds.max) x_min = np.array(ocp.nlp[0].x_bounds.min) x_max = np.array(ocp.nlp[0].x_bounds.max) self.all_constr = SX() self.end_constr = SX() ##TODO:change for more node flexibility on bounds self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) for i in range(ocp.nb_phases): for g, G in enumerate(ocp.nlp[i].g): if not G: continue if G[0]["constraint"].node[0] is Node.ALL: self.all_constr = vertcat(self.all_constr, G[0]["val"].reshape((-1, 1))) self.all_g_bounds.concatenate(G[0]["bounds"]) if len(G) > ocp.nlp[0].ns: constr_end_func_tp = Function( f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]], [G[0]["val"]]) self.end_constr = vertcat( self.end_constr, constr_end_func_tp(ocp.nlp[i].X[0]).reshape( (-1, 1))) self.end_g_bounds.concatenate(G[0]["bounds"]) elif G[0]["constraint"].node[0] is Node.END: constr_end_func_tp = Function( f"cas_constr_end_func_{i}_{g}", [ocp.nlp[i].X[-1]], [G[0]["val"]]) self.end_constr = vertcat( self.end_constr, constr_end_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) self.end_g_bounds.concatenate(G[0]["bounds"]) else: raise RuntimeError( "Except for states and controls, Acados solver only handles constraints on last or all nodes." ) self.acados_model.con_h_expr = self.all_constr self.acados_model.con_h_expr_e = self.end_constr if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)): raise NotImplementedError( "u_bounds min must be the same at each shooting point with ACADOS" ) if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)): raise NotImplementedError( "u_bounds max must be the same at each shooting point with ACADOS" ) if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all() or not np.isfinite(u_max).all() or not np.isfinite(x_max).all()): raise NotImplementedError( "u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it" "to a big value instead.") # setup state constraints self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) param_bounds_max = [] param_bounds_min = [] if self.params: param_bounds_max = np.concatenate( [self.params[key].bounds.max for key in self.params.keys()])[:, 0] param_bounds_min = np.concatenate( [self.params[key].bounds.min for key in self.params.keys()])[:, 0] for i in range(3): self.x_bound_max[:, i] = np.concatenate( (param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i]))) self.x_bound_min[:, i] = np.concatenate( (param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i]))) # setup control constraints self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0]) self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0]) self.acados_ocp.constraints.idxbu = np.array( range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # initial state constraints self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0] self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0] self.acados_ocp.constraints.idxbx_0 = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # setup path state constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1] self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1] self.acados_ocp.constraints.idxbx = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # setup terminal state constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1] self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1] self.acados_ocp.constraints.idxbx_e = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx # setup algebraic constraint self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0]) # setup terminal algebraic constraint self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0]) def __set_cost_type(self, cost_type="NONLINEAR_LS"): self.acados_ocp.cost.cost_type = cost_type self.acados_ocp.cost.cost_type_e = cost_type def __set_costs(self, ocp): if ocp.nb_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) ctrl_objs = [ PenaltyType.MINIMIZE_TORQUE, PenaltyType.MINIMIZE_MUSCLES_CONTROL, PenaltyType.MINIMIZE_ALL_CONTROLS, ] state_objs = [ PenaltyType.MINIMIZE_STATE, ] if self.acados_ocp.cost.cost_type == "LINEAR_LS": self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nu) self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx) self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].nx) for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: if J[0]["objective"].type.value[0] in ctrl_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nu))) vu = np.zeros(ocp.nlp[0].nu) vu[index] = 1.0 self.Vu = np.vstack((self.Vu, np.diag(vu))) self.Vx = np.vstack( (self.Vx, np.zeros((ocp.nlp[0].nu, ocp.nlp[0].nx)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nu)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nu, 1)) y_ref_tp = [] for J_tp in J: y_tp[index] = J_tp["target"].T.reshape( (-1, 1)) y_ref_tp.append(y_tp) self.y_ref.append(y_ref_tp) else: self.y_ref.append([ np.zeros((ocp.nlp[0].nu, 1)) for J_tp in J ]) elif J[0]["objective"].type.value[0] in state_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vx = np.zeros(ocp.nlp[0].nx) vx[index] = 1.0 self.Vx = np.vstack((self.Vx, np.diag(vx))) self.Vu = np.vstack( (self.Vu, np.zeros((ocp.nlp[0].nx, ocp.nlp[0].nu)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_ref_tp = [] for J_tp in J: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J_tp["target"].T.reshape( (-1, 1)) y_ref_tp.append(y_tp) self.y_ref.append(y_ref_tp) else: self.y_ref.append([ np.zeros((ocp.nlp[0].nx, 1)) for J_tp in J ]) else: raise RuntimeError( f"{J[0]['objective'].type.name} is an incompatible objective term with " f"LINEAR_LS cost type") # Deal with last node to match ipopt formulation if J[0]["objective"].node[0].value == "all" and len( J) > ocp.nlp[0].ns: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vxe = np.zeros(ocp.nlp[0].nx) vxe[index] = 1.0 self.Vxe = np.vstack((self.Vxe, np.diag(vxe))) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J[-1]["target"].T.reshape( (-1, 1)) self.y_ref_end.append(y_tp) else: self.y_ref_end.append( np.zeros((ocp.nlp[0].nx, 1))) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: if J[0]["objective"].type.value[0] in state_objs: index = (J[0]["objective"].index if J[0]["objective"].index else list( np.arange(ocp.nlp[0].nx))) vxe = np.zeros(ocp.nlp[0].nx) vxe[index] = 1.0 self.Vxe = np.vstack((self.Vxe, np.diag(vxe))) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * ocp.nlp[0].nx)) if J[0]["target"] is not None: y_tp = np.zeros((ocp.nlp[0].nx, 1)) y_tp[index] = J[-1]["target"].T.reshape( (-1, 1)) self.y_ref_end.append(y_tp) else: self.y_ref_end.append( np.zeros((ocp.nlp[0].nx, 1))) else: raise RuntimeError( f"{J[0]['objective'].type.name} is an incompatible objective term " f"with LINEAR_LS cost type") else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.params: raise RuntimeError( "Params not yet handled with LINEAR_LS cost type") # Set costs self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[ 0] else np.zeros((0, 0)) # Set dimensions self.acados_ocp.dims.ny = sum( [len(data[0]) for data in self.y_ref]) self.acados_ocp.dims.ny_e = sum( [len(data) for data in self.y_ref_end]) # Set weight self.acados_ocp.cost.W = self.W self.acados_ocp.cost.W_e = self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([ J_tp["target"].T.reshape((-1, 1)) for J_tp in J ]) else: self.y_ref.append([ np.zeros((J_tp["val"].numel(), 1)) for J_tp in J ]) # Deal with last node to match ipopt formulation if J[0]["objective"].node[0].value == "all" and len( J) > ocp.nlp[0].ns: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape( (-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append( J[-1]["target"].T.reshape((-1, 1))) else: self.y_ref_end.append( np.zeros((J[-1]["val"].numel(), 1))) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append(J[0]["target"].T.reshape( (-1, 1))) else: self.y_ref_end.append( np.zeros((J[0]["val"].numel(), 1))) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.J, for now. if self.params: for j, J in enumerate(ocp.J): mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel()))) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0]).reshape((-1, 1))) if J[0]["target"] is not None: self.y_ref_end.append(J[0]["target"].T.reshape( (-1, 1))) else: self.y_ref_end.append( np.zeros((J[0]["val"].numel(), 1))) # Set costs self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel( ) else SX(1, 1) self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel( ) else SX(1, 1) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] # Set weight self.acados_ocp.cost.W = np.zeros( (1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros( (1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError( "EXTERNAL is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." ) def __update_solver(self): param_init = [] for n in range(self.acados_ocp.dims.N): if self.y_ref: self.ocp_solver.cost_set( n, "yref", np.vstack([data[n] for data in self.y_ref])[:, 0]) # check following line # self.ocp_solver.cost_set(n, "W", self.W) if self.params: param_init = np.concatenate([ self.params[key].initial_guess.init.evaluate_at(n) for key in self.params.keys() ]) self.ocp_solver.set( n, "x", np.concatenate( (param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n)))) self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n)) self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0]) self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0]) self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:, 0]) self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:, 0]) if n == 0: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0]) else: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1]) if self.y_ref_end: if len(self.y_ref_end) == 1: self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.array(self.y_ref_end[0])[:, 0]) else: self.ocp_solver.cost_set( self.acados_ocp.dims.N, "yref", np.concatenate([data for data in self.y_ref_end])[:, 0]) # check following line # self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1]) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1]) if len(self.end_g_bounds.max[:, 0]): self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh", self.end_g_bounds.max[:, 0]) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh", self.end_g_bounds.min[:, 0]) if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1: if self.params: self.ocp_solver.set( self.acados_ocp.dims.N, "x", np.concatenate(( np.concatenate([ self.params[key]["initial_guess"].init for key in self.params.keys() ])[:, 0], self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N], )), ) else: self.ocp_solver.set( self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N]) def configure(self, options): if "acados_dir" in options: del options["acados_dir"] if "cost_type" in options: del options["cost_type"] if self.ocp_solver is None: self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON" self.acados_ocp.solver_options.integrator_type = "IRK" self.acados_ocp.solver_options.nlp_solver_type = "SQP" self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06 self.acados_ocp.solver_options.nlp_solver_max_iter = 200 self.acados_ocp.solver_options.sim_method_newton_iter = 5 self.acados_ocp.solver_options.sim_method_num_stages = 4 self.acados_ocp.solver_options.sim_method_num_steps = 1 self.acados_ocp.solver_options.print_level = 1 for key in options: setattr(self.acados_ocp.solver_options, key, options[key]) else: available_options = [ "nlp_solver_tol_comp", "nlp_solver_tol_eq", "nlp_solver_tol_ineq", "nlp_solver_tol_stat", ] for key in options: if key in available_options: short_key = key[11:] self.ocp_solver.options_set(short_key, options[key]) else: raise RuntimeError( f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}" ) def get_iterations(self): raise NotImplementedError( "return_iterations is not implemented yet with ACADOS backend") def online_optim(self, ocp): raise NotImplementedError( "online_optim is not implemented yet with ACADOS backend") def get_optimized_value(self): ns = self.acados_ocp.dims.N nx = self.acados_ocp.dims.nx nq = self.ocp.nlp[0].q.shape[0] nparams = self.ocp.nlp[0].np acados_x = np.array( [self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T acados_p = acados_x[:nparams, :] acados_q = acados_x[nparams:nq + nparams, :] acados_qdot = acados_x[nq + nparams:nx, :] acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T out = { "qqdot": acados_x, "x": [], "u": acados_u, "time_tot": self.ocp_solver.get_stats("time_tot")[0], "iter": self.ocp_solver.get_stats("sqp_iter")[0], "status": self.status, } for i in range(ns): out["x"] = vertcat(out["x"], acados_q[:, i]) out["x"] = vertcat(out["x"], acados_qdot[:, i]) out["x"] = vertcat(out["x"], acados_u[:, i]) out["x"] = vertcat(out["x"], acados_q[:, ns]) out["x"] = vertcat(out["x"], acados_qdot[:, ns]) out["x"] = vertcat(acados_p[:, 0], out["x"]) self.out["sol"] = out out = [] for key in self.out.keys(): out.append(self.out[key]) return out[0] if len(out) == 1 else out def solve(self): # Populate costs and constrs vectors self.__set_costs(self.ocp) self.__set_constrs(self.ocp) if self.ocp_solver is None: self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json") self.__update_solver() self.status = self.ocp_solver.solve() self.get_optimized_value() return self
def __set_costs(self, ocp): self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) if self.acados_ocp.cost.cost_type == "LINEAR_LS": # set Lagrange terms self.acados_ocp.cost.Vx = np.zeros( (self.acados_ocp.dims.ny, self.acados_ocp.dims.nx)) self.acados_ocp.cost.Vx[:self.acados_ocp.dims.nx, :] = np.eye( self.acados_ocp.dims.nx) Vu = np.zeros((self.acados_ocp.dims.ny, self.acados_ocp.dims.nu)) Vu[self.acados_ocp.dims.nx:, :] = np.eye(self.acados_ocp.dims.nu) self.acados_ocp.cost.Vu = Vu # set Mayer term self.acados_ocp.cost.Vx_e = np.zeros( (self.acados_ocp.dims.nx, self.acados_ocp.dims.nx)) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": if ocp.nb_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet") for i in range(ocp.nb_phases): # TODO: I think ocp.J is missing here (the parameters would be stored there) # TODO: Yes the objectives in ocp are not dealt with, # does that makes sense since we only work with 1 phase ? for j, J in enumerate(ocp.nlp[i]["J"]): if J[0]["objective"].type.get_type( ) == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([ J_tp["target"].T.reshape((-1, 1)) for J_tp in J ]) else: self.y_ref.append([ np.zeros((J_tp["val"].numel(), 1)) for J_tp in J ]) elif J[0]["objective"].type.get_type( ) == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) if J[0]["target"] is not None: self.y_ref_end.append([J[0]["target"]]) else: self.y_ref_end.append( [np.zeros((J[0]["val"].numel(), 1))]) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.lagrange_costs.numel(): self.acados_ocp.model.cost_y_expr = self.lagrange_costs else: self.acados_ocp.model.cost_y_expr = SX(1, 1) if self.mayer_costs.numel(): self.acados_ocp.model.cost_y_expr_e = self.mayer_costs else: self.acados_ocp.model.cost_y_expr_e = SX(1, 1) self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] self.acados_ocp.cost.yref = np.zeros((max(self.acados_ocp.dims.ny, 1), )) self.acados_ocp.cost.yref_e = np.concatenate(self.y_ref_end, -1).T.squeeze() if self.W.shape == (0, 0): self.acados_ocp.cost.W = np.zeros((1, 1)) else: self.acados_ocp.cost.W = self.W if self.W_e.shape == (0, 0): self.acados_ocp.cost.W_e = np.zeros((1, 1)) else: self.acados_ocp.cost.W_e = self.W_e elif self.acados_ocp.cost.cost_type == "EXTERNAL": for i in range(ocp.nb_phases): for j in range(len(ocp.nlp[i]["J"])): J = ocp.nlp[i]["J"][j][0] raise RuntimeError( "TODO: The target is not right currently") if J["type"] == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat( self.lagrange_costs, J["val"][0] - J["target"][0]) elif J["type"] == ObjectiveFunction.MayerFunction: raise RuntimeError( "TODO: I may have broken this (is this the right J?)" ) mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i]["X"][-1]], [J["val"]]) self.mayer_costs = vertcat( self.mayer_costs, mayer_func_tp(ocp.nlp[i]["X"][0])) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) self.acados_ocp.model.cost_expr_ext_cost = sum1( self.lagrange_costs) self.acados_ocp.model.cost_expr_ext_cost_e = sum1(self.mayer_costs) else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." )
Vu[4,0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) # set constraints Fmax = 80 # use equivalent formulation with h constraint # ocp.constraints.lbu = np.array([-Fmax]) # ocp.constraints.ubu = np.array([+Fmax]) # ocp.constraints.idxbu = np.array([0]) p = SX.sym('p') ocp.model.p = p ocp.constraints.lh = np.array([-Fmax]) ocp.constraints.uh = np.array([+Fmax]) ocp.model.con_h_expr = model.u / p ocp.parameter_values = np.array([0]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'EXACT' # GAUSS_NEWTON, EXACT ocp.solver_options.regularize_method = 'CONVEXIFY' # GAUSS_NEWTON, EXACT ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0
def wrapperRWSC(IC, args, optimal): # Converting the Optimal Control Problem into a Non-Linear Programming Problem numStates = 6 numInputs = 2 nodes = 30 # Keep this Number Small to Reduce Runtime dt = SX.sym("dt") states = SX.sym("state", nodes, numStates) controls = SX.sym("controls", nodes, numInputs) variables_list = [dt, states, controls] variables_name = ["dt", "states", "controls"] variables_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in variables_list]) pack_variables_fn = casadi.Function( "pack_variables_fn", variables_list, [variables_flat], variables_name, ["flat"] ) unpack_variables_fn = casadi.Function( "unpack_variables_fn", [variables_flat], variables_list, ["flat"], variables_name, ) # Bounds bds = [ [np.sqrt(np.finfo(float).eps), np.inf], [-100, 300], [0, np.inf], [-np.inf, np.inf], [-np.inf, np.inf], [np.sqrt(np.finfo(float).eps), np.inf], [-1, 1], [np.sqrt(np.finfo(float).eps), 1], ] lower_bounds = unpack_variables_fn(flat=-float("inf")) lower_bounds["dt"][:, :] = bds[0][0] lower_bounds["states"][:, 0] = bds[1][0] lower_bounds["states"][:, 1] = bds[2][0] lower_bounds["states"][:, 4] = bds[5][0] lower_bounds["controls"][:, 0] = bds[6][0] lower_bounds["controls"][:, 1] = bds[7][0] upper_bounds = unpack_variables_fn(flat=float("inf")) upper_bounds["dt"][:, :] = bds[0][1] upper_bounds["states"][:, 0] = bds[1][1] upper_bounds["controls"][:, 0] = bds[6][1] upper_bounds["controls"][:, 1] = bds[7][1] # Set Initial Conditions # Casadi does not accept equality constraints, so boundary constraints are # set as box constraints with 0 area. lower_bounds["states"][0, 0] = IC[0] lower_bounds["states"][0, 1] = IC[1] lower_bounds["states"][0, 2] = IC[2] lower_bounds["states"][0, 3] = IC[3] lower_bounds["states"][0, 4] = IC[4] lower_bounds["states"][0, 5] = IC[5] upper_bounds["states"][0, 0] = IC[0] upper_bounds["states"][0, 1] = IC[1] upper_bounds["states"][0, 2] = IC[2] upper_bounds["states"][0, 3] = IC[3] upper_bounds["states"][0, 4] = IC[4] upper_bounds["states"][0, 5] = IC[5] # Set Final Conditions # Currently set for a soft touchdown at the origin lower_bounds["states"][-1, 0] = 0 lower_bounds["states"][-1, 1] = 0 lower_bounds["states"][-1, 2] = 0 lower_bounds["states"][-1, 3] = 0 lower_bounds["states"][-1, 5] = 0 upper_bounds["states"][-1, 0] = 0 upper_bounds["states"][-1, 1] = 0 upper_bounds["states"][-1, 2] = 0 upper_bounds["states"][-1, 3] = 0 upper_bounds["states"][-1, 5] = 0 # Initial Guess Generation # Generate the initial guess as a line between initial and final conditions xIG = np.array( [ np.linspace(IC[0], 0, nodes), np.linspace(IC[1], 0, nodes), np.linspace(IC[2], 0, nodes), np.linspace(IC[3], 0, nodes), np.linspace(IC[4], IC[4] * 0.5, nodes), np.linspace(IC[5], 0, nodes), ] ).T uIG = np.array([np.linspace(0, 1, nodes), np.linspace(1, 1, nodes)]).T ig_list = [60 / nodes, xIG, uIG] ig_flat = casadi.vertcat(*[casadi.reshape(e, -1, 1) for e in ig_list]) # Generating Defect Vector xLow = states[0: (nodes - 1), :] xHigh = states[1:nodes, :] contLow = controls[0: (nodes - 1), :] contHi = controls[1:nodes, :] contMid = (contLow + contHi) / 2 # Use a RK4 Method for Generating the Defects k1 = RWSC(xLow, contLow, args) k2 = RWSC(xLow + (0.5 * dt * k1), contMid, args) k3 = RWSC(xLow + (0.5 * dt * k2), contMid, args) k4 = RWSC(xLow + k3, contHi, args) xNew = xLow + ((dt / 6) * (k1 + (2 * k2) + (2 * k3) + k4)) defect = casadi.reshape(xNew - xHigh, -1, 1) # Choose the Cost Function if optimal == 1: J = -states[-1, 4] # Mass Optimal Cost Function elif optimal == 2: J = dt[0] # Time Optimal Cost Function # Put the OCP into a form that Casadi can solve nlp = {"x": variables_flat, "f": J, "g": defect} solver = casadi.nlpsol( "solver", "bonmin", nlp ) # Use bonmin instead of ipopt due to speed result = solver( x0=ig_flat, lbg=0.0, ubg=0.0, lbx=pack_variables_fn(**lower_bounds)["flat"], ubx=pack_variables_fn(**upper_bounds)["flat"], ) results = unpack_variables_fn(flat=result["x"]) return results
import matplotlib.pyplot as plt from acados_template import AcadosOcp, AcadosOcpSolver, AcadosModel, AcadosSimSolver, AcadosSim import numpy as np from casadi import SX, vertcat import pickle # (very) simple crane model beta = 0.001 k = 0.9 a_max = 10 dt_max = 2.0 # states p1 = SX.sym('p1') v1 = SX.sym('v1') p2 = SX.sym('p2') v2 = SX.sym('v2') x = vertcat(p1, v1, p2, v2) # controls a = SX.sym('a') dt = SX.sym('dt') u = vertcat(a, dt) f_expl = dt * vertcat(v1, a, v2, -beta * v2 - k * (p2 - p1)) model = AcadosModel()
eta_ub = 200 eta_lb = 0 # Abtastzeit und Prediction horizon T = 0.1 N_pred = 3 # Zielruhelage von den Zustaenden # Referenz von h, h_p, eta state_r = [0.8, 0, 48] input_r = [157.29] if __name__ == '__main__': plant_init() NMPC_init() # Zustaende als Symbol definieren h = SX.sym('h') h_p = SX.sym('h_p') eta = SX.sym('eta') # Zustandsvektor states = vertcat(h, h_p, eta) # print(states.shape) # Eingang symbolisch definieren u_pwm = SX.sym('u_pwm') # Eingangsverktor controls = vertcat(u_pwm) # kriegen die Anzahl der Zeiler("shape" ist ein Tuple) n_states = states.shape[0] # p 22. # Eingangsanzahl
def gen_long_model(): model = AcadosModel() model.name = 'long' # set up states & controls x_ego = SX.sym('x_ego') v_ego = SX.sym('v_ego') a_ego = SX.sym('a_ego') model.x = vertcat(x_ego, v_ego, a_ego) # controls j_ego = SX.sym('j_ego') model.u = vertcat(j_ego) # xdot x_ego_dot = SX.sym('x_ego_dot') v_ego_dot = SX.sym('v_ego_dot') a_ego_dot = SX.sym('a_ego_dot') model.xdot = vertcat(x_ego_dot, v_ego_dot, a_ego_dot) # live parameters x_obstacle = SX.sym('x_obstacle') a_min = SX.sym('a_min') a_max = SX.sym('a_max') prev_a = SX.sym('prev_a') model.p = vertcat(a_min, a_max, x_obstacle, prev_a) # dynamics model f_expl = vertcat(v_ego, a_ego, j_ego) model.f_impl_expr = model.xdot - f_expl model.f_expl_expr = f_expl return model
class AcadosInterface(SolverInterface): def __init__(self, ocp, **solver_options): if not isinstance(ocp.CX(), SX): raise RuntimeError("CasADi graph must be SX to be solved with ACADOS. Please set use_SX to True in OCP") super().__init__(ocp) # If Acados is installed using the acados_install.sh file, you probably can leave this to unset acados_path = "" if "acados_dir" in solver_options: acados_path = solver_options["acados_dir"] self.acados_ocp = AcadosOcp(acados_path=acados_path) self.acados_model = AcadosModel() if "cost_type" in solver_options: self.__set_cost_type(solver_options["cost_type"]) else: self.__set_cost_type() if "constr_type" in solver_options: self.__set_constr_type(solver_options["constr_type"]) else: self.__set_constr_type() self.lagrange_costs = SX() self.mayer_costs = SX() self.y_ref = [] self.y_ref_end = [] self.__acados_export_model(ocp) self.__prepare_acados(ocp) self.ocp_solver = None self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) self.status = None self.out = {} def __acados_export_model(self, ocp): # Declare model variables x = ocp.nlp[0].X[0] u = ocp.nlp[0].U[0] p = ocp.nlp[0].p self.params = ocp.nlp[0].parameters_to_optimize x = vertcat(p, x) x_dot = SX.sym("x_dot", x.shape[0], x.shape[1]) f_expl = vertcat([0] * ocp.nlp[0].np, ocp.nlp[0].dynamics_func(x[ocp.nlp[0].np :, :], u, p)) f_impl = x_dot - f_expl self.acados_model.f_impl_expr = f_impl self.acados_model.f_expl_expr = f_expl self.acados_model.x = x self.acados_model.xdot = x_dot self.acados_model.u = u self.acados_model.p = [] self.acados_model.name = "model_name" def __prepare_acados(self, ocp): if ocp.nb_phases > 1: raise NotImplementedError("More than 1 phase is not implemented yet with ACADOS backend") # set model self.acados_ocp.model = self.acados_model # set time self.acados_ocp.solver_options.tf = ocp.nlp[0].tf # set dimensions self.acados_ocp.dims.nx = ocp.nlp[0].nx + ocp.nlp[0].np self.acados_ocp.dims.nu = ocp.nlp[0].nu self.acados_ocp.dims.N = ocp.nlp[0].ns def __set_constr_type(self, constr_type="BGH"): self.acados_ocp.constraints.constr_type = constr_type self.acados_ocp.constraints.constr_type_e = constr_type def __set_constrs(self, ocp): # constraints handling in self.acados_ocp u_min = np.array(ocp.nlp[0].u_bounds.min) u_max = np.array(ocp.nlp[0].u_bounds.max) x_min = np.array(ocp.nlp[0].x_bounds.min) x_max = np.array(ocp.nlp[0].x_bounds.max) if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)): raise NotImplementedError("u_bounds min must be the same at each shooting point with ACADOS") if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)): raise NotImplementedError("u_bounds max must be the same at each shooting point with ACADOS") if ( not np.isfinite(u_min).all() or not np.isfinite(x_min).all() or not np.isfinite(u_max).all() or not np.isfinite(x_max).all() ): raise NotImplementedError( "u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it" "to a big value instead." ) ## TODO: implement constraints in g # setup state constraints self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) param_bounds_max = [] param_bounds_min = [] if self.params: param_bounds_max = np.concatenate([self.params[key]["bounds"].max for key in self.params.keys()])[:, 0] param_bounds_min = np.concatenate([self.params[key]["bounds"].min for key in self.params.keys()])[:, 0] for i in range(3): self.x_bound_max[:, i] = np.concatenate((param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i]))) self.x_bound_min[:, i] = np.concatenate((param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i]))) # setup control constraints self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0]) self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0]) self.acados_ocp.constraints.idxbu = np.array(range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # initial state constraints self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0] self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0] self.acados_ocp.constraints.idxbx_0 = np.array(range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # setup path state constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1] self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1] self.acados_ocp.constraints.idxbx = np.array(range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # setup terminal state constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1] self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1] self.acados_ocp.constraints.idxbx_e = np.array(range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx def __set_cost_type(self, cost_type="NONLINEAR_LS"): self.acados_ocp.cost.cost_type = cost_type self.acados_ocp.cost.cost_type_e = cost_type def __set_costs(self, ocp): if ocp.nb_phases != 1: raise NotImplementedError("ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) if self.acados_ocp.cost.cost_type == "LINEAR_LS": raise RuntimeError("LINEAR_LS is not interfaced yet.") elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i in range(ocp.nb_phases): for j, J in enumerate(ocp.nlp[i].J): if J[0]["objective"].type.get_type() == ObjectiveFunction.LagrangeFunction: self.lagrange_costs = vertcat(self.lagrange_costs, J[0]["val"].reshape((-1, 1))) self.W = linalg.block_diag(self.W, np.diag([J[0]["objective"].weight] * J[0]["val"].numel())) if J[0]["target"] is not None: self.y_ref.append([J_tp["target"].T.reshape((-1, 1)) for J_tp in J]) else: self.y_ref.append([np.zeros((J_tp["val"].numel(), 1)) for J_tp in J]) elif J[0]["objective"].type.get_type() == ObjectiveFunction.MayerFunction: mayer_func_tp = Function(f"cas_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag([J[0]["objective"].weight] * J[0]["val"].numel()) ) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0])) if J[0]["target"] is not None: self.y_ref_end.append( [J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"] ) else: self.y_ref_end.append([0] * (J[0]["val"].numel())) else: raise RuntimeError("The objective function is not Lagrange nor Mayer.") # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.J, for now. if self.params: for j, J in enumerate(ocp.J): mayer_func_tp = Function(f"cas_J_mayer_func_{i}_{j}", [ocp.nlp[i].X[-1]], [J[0]["val"]]) self.W_e = linalg.block_diag( self.W_e, np.diag(([J[0]["objective"].weight] * J[0]["val"].numel())) ) self.mayer_costs = vertcat(self.mayer_costs, mayer_func_tp(ocp.nlp[i].X[0])) if J[0]["target"] is not None: self.y_ref_end.append( [J[0]["target"]] if isinstance(J[0]["target"], (int, float)) else J[0]["target"] ) else: self.y_ref_end.append([0] * (J[0]["val"].numel())) # Set costs self.acados_ocp.model.cost_y_expr = self.lagrange_costs if self.lagrange_costs.numel() else SX(1, 1) self.acados_ocp.model.cost_y_expr_e = self.mayer_costs if self.mayer_costs.numel() else SX(1, 1) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[0] # Set weight self.acados_ocp.cost.W = np.zeros((1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros((1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros((self.acados_ocp.cost.W.shape[0],)) self.acados_ocp.cost.yref_e = np.zeros((self.acados_ocp.cost.W_e.shape[0],)) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError("External is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError("Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'.") def __update_solver(self): param_init = [] for n in range(self.acados_ocp.dims.N): self.ocp_solver.cost_set(n, "yref", np.concatenate([data[n] for data in self.y_ref])[:, 0]) self.ocp_solver.cost_set(n, "W", self.W) if self.params: param_init = np.concatenate( [self.params[key]["initial_guess"].init.evaluate_at(n) for key in self.params.keys()] ) self.ocp_solver.set(n, "x", np.concatenate((param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n)))) self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n)) self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0]) self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0]) if n == 0: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0]) else: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1]) if self.y_ref_end: self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.concatenate([data for data in self.y_ref_end])) self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1]) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1]) if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1: if self.params: self.ocp_solver.set( self.acados_ocp.dims.N, "x", np.concatenate( ( np.concatenate([self.params[key]["initial_guess"].init for key in self.params.keys()])[ :, 0 ], self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N], ) ), ) else: self.ocp_solver.set(self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N]) def configure(self, options): if "acados_dir" in options: del options["acados_dir"] if "cost_type" in options: del options["cost_type"] if self.ocp_solver is None: self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON" self.acados_ocp.solver_options.integrator_type = "IRK" self.acados_ocp.solver_options.nlp_solver_type = "SQP" self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06 self.acados_ocp.solver_options.nlp_solver_max_iter = 200 self.acados_ocp.solver_options.sim_method_newton_iter = 5 self.acados_ocp.solver_options.sim_method_num_stages = 4 self.acados_ocp.solver_options.sim_method_num_steps = 1 self.acados_ocp.solver_options.print_level = 1 for key in options: setattr(self.acados_ocp.solver_options, key, options[key]) else: for key in options: if key[:11] == "nlp_solver_": short_key = key[11:] self.ocp_solver.options_set(short_key, options[key]) else: raise RuntimeError( "[ACADOS] Only editable solver options after solver creation are :\n" "nlp_solver_tol_comp\n" "nlp_solver_tol_eq\n" "nlp_solver_tol_ineq\n" "nlp_solver_tol_stat\n" ) def get_iterations(self): raise NotImplementedError("return_iterations is not implemented yet with ACADOS backend") def online_optim(self, ocp): raise NotImplementedError("online_optim is not implemented yet with ACADOS backend") def get_optimized_value(self): ns = self.acados_ocp.dims.N nx = self.acados_ocp.dims.nx nq = self.ocp.nlp[0].q.shape[0] nparams = self.ocp.nlp[0].np acados_x = np.array([self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T acados_p = acados_x[:nparams, :] acados_q = acados_x[nparams : nq + nparams, :] acados_qdot = acados_x[nq + nparams : nx, :] acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T out = { "qqdot": acados_x, "x": [], "u": acados_u, "time_tot": self.ocp_solver.get_stats("time_tot")[0], "status": self.status, } for i in range(ns): out["x"] = vertcat(out["x"], acados_q[:, i]) out["x"] = vertcat(out["x"], acados_qdot[:, i]) out["x"] = vertcat(out["x"], acados_u[:, i]) out["x"] = vertcat(out["x"], acados_q[:, ns]) out["x"] = vertcat(out["x"], acados_qdot[:, ns]) out["x"] = vertcat(acados_p[:, 0], out["x"]) self.out["sol"] = out out = [] for key in self.out.keys(): out.append(self.out[key]) return out[0] if len(out) == 1 else out def solve(self): # Populate costs and constrs vectors self.__set_costs(self.ocp) self.__set_constrs(self.ocp) if self.ocp_solver is None: self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json") self.__update_solver() self.status = self.ocp_solver.solve() self.get_optimized_value() return self
class AcadosInterface(SolverInterface): """ The ACADOS solver interface Attributes ---------- acados_ocp: AcadosOcp The current AcadosOcp reference acados_model: AcadosModel The current AcadosModel reference lagrange_costs: SX The lagrange cost function mayer_costs: SX The mayer cost function y_ref = list[np.ndarray] The lagrange targets y_ref_end = list[np.ndarray] The mayer targets params = dict All the parameters to optimize W: np.ndarray The Lagrange weights W_e: np.ndarray The Mayer weights status: int The status of the optimization all_constr: SX All the Lagrange constraints end_constr: SX All the Mayer constraints all_g_bounds = Bounds All the Lagrange bounds on the variables end_g_bounds = Bounds All the Mayer bounds on the variables x_bound_max = np.ndarray All the bounds max x_bound_min = np.ndarray All the bounds min Vu: np.ndarray The control objective functions Vx: np.ndarray The Lagrange state objective functions Vxe: np.ndarray The Mayer state objective functions Methods ------- __acados_export_model(self, ocp: OptimalControlProgram) Creating a generic ACADOS model __prepare_acados(self, ocp: OptimalControlProgram) Set some important ACADOS variables __set_constr_type(self, constr_type: str = "BGH") Set the type of constraints __set_constraints(self, ocp: OptimalControlProgram) Set the constraints from the ocp __set_cost_type(self, cost_type: str = "NONLINEAR_LS") Set the type of cost functions __set_costs(self, ocp: OptimalControlProgram) Set the cost functions from ocp __update_solver(self) Update the ACADOS solver to new values configure(self, options: dict) Set some ACADOS options get_optimized_value(self) -> Union[list[dict], dict] Get the previously optimized solution solve(self) -> "AcadosInterface" Solve the prepared ocp """ def __init__(self, ocp, **solver_options): """ Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram solver_options: dict The options to pass to the solver """ if not isinstance(ocp.cx(), SX): raise RuntimeError( "CasADi graph must be SX to be solved with ACADOS. Please set use_sx to True in OCP" ) super().__init__(ocp) # If Acados is installed using the acados_install.sh file, you probably can leave this to unset acados_path = "" if "acados_dir" in solver_options: acados_path = solver_options["acados_dir"] self.acados_ocp = AcadosOcp(acados_path=acados_path) self.acados_model = AcadosModel() if "cost_type" in solver_options: self.__set_cost_type(solver_options["cost_type"]) else: self.__set_cost_type() if "constr_type" in solver_options: self.__set_constr_type(solver_options["constr_type"]) else: self.__set_constr_type() self.lagrange_costs = SX() self.mayer_costs = SX() self.y_ref = [] self.y_ref_end = [] self.nparams = 0 self.params_initial_guess = None self.params_bounds = None self.__acados_export_model(ocp) self.__prepare_acados(ocp) self.ocp_solver = None self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) self.status = None self.out = {} self.all_constr = None self.end_constr = SX() self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) self.Vu = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].controls.shape) self.Vx = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape) self.Vxe = np.array([], dtype=np.int64).reshape(0, ocp.nlp[0].states.shape) def __acados_export_model(self, ocp): """ Creating a generic ACADOS model Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ if ocp.n_phases > 1: raise NotImplementedError( "More than 1 phase is not implemented yet with ACADOS backend") # Declare model variables x = ocp.nlp[0].states.cx u = ocp.nlp[0].controls.cx p = ocp.nlp[0].parameters.cx if ocp.v.parameters_in_list: for param in ocp.v.parameters_in_list: if str(param.cx)[:11] == f"time_phase_": raise RuntimeError( "Time constraint not implemented yet with Acados.") self.nparams = ocp.nlp[0].parameters.shape self.params_initial_guess = ocp.v.parameters_in_list.initial_guess self.params_initial_guess.check_and_adjust_dimensions(self.nparams, 1) self.params_bounds = ocp.v.parameters_in_list.bounds self.params_bounds.check_and_adjust_dimensions(self.nparams, 1) x = vertcat(p, x) x_dot = SX.sym("x_dot", x.shape[0], x.shape[1]) f_expl = vertcat([0] * self.nparams, ocp.nlp[0].dynamics_func(x[self.nparams:, :], u, p)) f_impl = x_dot - f_expl self.acados_model.f_impl_expr = f_impl self.acados_model.f_expl_expr = f_expl self.acados_model.x = x self.acados_model.xdot = x_dot self.acados_model.u = u self.acados_model.con_h_expr = np.zeros((0, 0)) self.acados_model.con_h_expr_e = np.zeros((0, 0)) self.acados_model.p = [] now = datetime.now() # current date and time self.acados_model.name = f"model_{now.strftime('%Y_%m_%d_%H%M%S%f')[:-4]}" def __prepare_acados(self, ocp): """ Set some important ACADOS variables Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ # set model self.acados_ocp.model = self.acados_model # set time self.acados_ocp.solver_options.tf = ocp.nlp[0].tf # set dimensions self.acados_ocp.dims.nx = ocp.nlp[0].states.shape + ocp.nlp[ 0].parameters.shape self.acados_ocp.dims.nu = ocp.nlp[0].controls.shape self.acados_ocp.dims.N = ocp.nlp[0].ns def __set_constr_type(self, constr_type: str = "BGH"): """ Set the type of constraints Parameters ---------- constr_type: str The requested type of constraints """ self.acados_ocp.constraints.constr_type = constr_type self.acados_ocp.constraints.constr_type_e = constr_type def __set_constraints(self, ocp): """ Set the constraints from the ocp Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ # constraints handling in self.acados_ocp if ocp.nlp[ 0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: raise NotImplementedError( "ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the x_bounds") if ocp.nlp[ 0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: raise NotImplementedError( "ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the u_bounds") u_min = np.array(ocp.nlp[0].u_bounds.min) u_max = np.array(ocp.nlp[0].u_bounds.max) x_min = np.array(ocp.nlp[0].x_bounds.min) x_max = np.array(ocp.nlp[0].x_bounds.max) self.all_constr = SX() self.end_constr = SX() # TODO:change for more node flexibility on bounds self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) for i, nlp in enumerate(ocp.nlp): x = nlp.states.cx u = nlp.controls.cx p = nlp.parameters.cx for g, G in enumerate(nlp.g): if not G: continue if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING: self.all_constr = vertcat(self.all_constr, G.function(x, u, p)) self.all_g_bounds.concatenate(G.bounds) if G.node[0] == Node.ALL: self.end_constr = vertcat(self.end_constr, G.function(x, u, p)) self.end_g_bounds.concatenate(G.bounds) elif G.node[0] == Node.END: self.end_constr = vertcat(self.end_constr, G.function(x, u, p)) self.end_g_bounds.concatenate(G.bounds) else: raise RuntimeError( "Except for states and controls, Acados solver only handles constraints on last or all nodes." ) self.acados_model.con_h_expr = self.all_constr self.acados_model.con_h_expr_e = self.end_constr if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)): raise NotImplementedError( "u_bounds min must be the same at each shooting point with ACADOS" ) if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)): raise NotImplementedError( "u_bounds max must be the same at each shooting point with ACADOS" ) if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all() or not np.isfinite(u_max).all() or not np.isfinite(x_max).all()): raise NotImplementedError( "u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it " "to a big value instead.") # setup state constraints # TODO replace all these np.concatenate by proper bound and initial_guess classes self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) param_bounds_max = [] param_bounds_min = [] if self.nparams: param_bounds_max = self.params_bounds.max[:, 0] param_bounds_min = self.params_bounds.min[:, 0] for i in range(3): self.x_bound_max[:, i] = np.concatenate( (param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i]))) self.x_bound_min[:, i] = np.concatenate( (param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i]))) # setup control constraints self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0]) self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0]) self.acados_ocp.constraints.idxbu = np.array( range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # initial state constraints self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0] self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0] self.acados_ocp.constraints.idxbx_0 = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # setup path state constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1] self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1] self.acados_ocp.constraints.idxbx = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # setup terminal state constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1] self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1] self.acados_ocp.constraints.idxbx_e = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx # setup algebraic constraint self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0]) # setup terminal algebraic constraint self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0]) def __set_cost_type(self, cost_type: str = "NONLINEAR_LS"): """ Set the type of cost functions Parameters ---------- cost_type: str The type of cost function """ self.acados_ocp.cost.cost_type = cost_type self.acados_ocp.cost.cost_type_e = cost_type def __set_costs(self, ocp): """ Set the cost functions from ocp Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ def add_linear_ls_lagrange(acados, objectives): def add_objective(n_variables, is_state): v_var = np.zeros(n_variables) var_type = acados.ocp.nlp[ 0].states if is_state else acados.ocp.nlp[0].controls rows = objectives.rows + var_type[ objectives.params["key"]].index[0] v_var[rows] = 1.0 if is_state: acados.Vx = np.vstack((acados.Vx, np.diag(v_var))) acados.Vu = np.vstack( (acados.Vu, np.zeros((n_states, n_controls)))) else: acados.Vx = np.vstack( (acados.Vx, np.zeros((n_controls, n_states)))) acados.Vu = np.vstack((acados.Vu, np.diag(v_var))) acados.W = linalg.block_diag( acados.W, np.diag([objectives.weight] * n_variables)) node_idx = objectives.node_idx[:-1] if objectives.node[ 0] == Node.ALL else objectives.node_idx y_ref = [ np.zeros((n_states if is_state else n_controls, 1)) for _ in node_idx ] if objectives.target is not None: for idx in node_idx: y_ref[idx][rows] = objectives.target[..., idx].T.reshape( (-1, 1)) acados.y_ref.append(y_ref) if objectives.type in allowed_control_objectives: add_objective(n_controls, False) elif objectives.type in allowed_state_objectives: add_objective(n_states, True) else: raise RuntimeError( f"{objectives[0]['objective'].type.name} is an incompatible objective term with LINEAR_LS cost type" ) def add_linear_ls_mayer(acados, objectives): if objectives.type in allowed_state_objectives: vxe = np.zeros(n_states) rows = objectives.rows + acados.ocp.nlp[0].states[ objectives.params["key"]].index[0] vxe[rows] = 1.0 acados.Vxe = np.vstack((acados.Vxe, np.diag(vxe))) acados.W_e = linalg.block_diag( acados.W_e, np.diag([objectives.weight] * n_states)) y_ref_end = np.zeros((n_states, 1)) if objectives.target is not None: y_ref_end[rows] = objectives.target[..., -1].T.reshape( (-1, 1)) acados.y_ref_end.append(y_ref_end) else: raise RuntimeError( f"{objectives.type.name} is an incompatible objective term with LINEAR_LS cost type" ) def add_nonlinear_ls_lagrange(acados, objectives, x, u, p): acados.lagrange_costs = vertcat( acados.lagrange_costs, objectives.function(x, u, p).reshape((-1, 1))) acados.W = linalg.block_diag( acados.W, np.diag([objectives.weight] * objectives.function.numel_out())) node_idx = objectives.node_idx[:-1] if objectives.node[ 0] == Node.ALL else objectives.node_idx if objectives.target is not None: acados.y_ref.append([ objectives.target[..., idx].T.reshape((-1, 1)) for idx in node_idx ]) else: acados.y_ref.append([ np.zeros((objectives.function.numel_out(), 1)) for _ in node_idx ]) def add_nonlinear_ls_mayer(acados, objectives, x, u, p): acados.W_e = linalg.block_diag( acados.W_e, np.diag([objectives.weight] * objectives.function.numel_out())) x = x if objectives.function.sparsity_in("i0").shape != ( 0, 0) else [] u = u if objectives.function.sparsity_in("i1").shape != ( 0, 0) else [] acados.mayer_costs = vertcat( acados.mayer_costs, objectives.function(x, u, p).reshape((-1, 1))) if objectives.target is not None: acados.y_ref_end.append(objectives.target[..., -1].T.reshape( (-1, 1))) else: acados.y_ref_end.append( np.zeros((objectives.function.numel_out(), 1))) if ocp.n_phases != 1: raise NotImplementedError( "ACADOS with more than one phase is not implemented yet.") # costs handling in self.acados_ocp self.y_ref = [] self.y_ref_end = [] self.lagrange_costs = SX() self.mayer_costs = SX() self.W = np.zeros((0, 0)) self.W_e = np.zeros((0, 0)) allowed_control_objectives = [ObjectiveFcn.Lagrange.MINIMIZE_CONTROL] allowed_state_objectives = [ ObjectiveFcn.Lagrange.MINIMIZE_STATE, ObjectiveFcn.Mayer.TRACK_STATE ] if self.acados_ocp.cost.cost_type == "LINEAR_LS": n_states = ocp.nlp[0].states.shape n_controls = ocp.nlp[0].controls.shape self.Vu = np.array([], dtype=np.int64).reshape(0, n_controls) self.Vx = np.array([], dtype=np.int64).reshape(0, n_states) self.Vxe = np.array([], dtype=np.int64).reshape(0, n_states) for i in range(ocp.n_phases): for J in ocp.nlp[i].J: if not J: continue if J.multi_thread: raise RuntimeError( f"The objective function {J.name} was declared with multi_thread=True, " f"but this is not possible to multi_thread objective function with ACADOS" ) if J.type.get_type() == ObjectiveFunction.LagrangeFunction: add_linear_ls_lagrange(self, J) # Deal with last node to match ipopt formulation if J.node[0] == Node.ALL: add_linear_ls_mayer(self, J) elif J.type.get_type() == ObjectiveFunction.MayerFunction: add_linear_ls_mayer(self, J) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) if self.nparams: raise RuntimeError( "Params not yet handled with LINEAR_LS cost type") # Set costs self.acados_ocp.cost.Vx = self.Vx if self.Vx.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vu = self.Vu if self.Vu.shape[0] else np.zeros( (0, 0)) self.acados_ocp.cost.Vx_e = self.Vxe if self.Vxe.shape[ 0] else np.zeros((0, 0)) # Set dimensions self.acados_ocp.dims.ny = sum( [len(data[0]) for data in self.y_ref]) self.acados_ocp.dims.ny_e = sum( [len(data) for data in self.y_ref_end]) # Set weight self.acados_ocp.cost.W = self.W self.acados_ocp.cost.W_e = self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "NONLINEAR_LS": for i, nlp in enumerate(ocp.nlp): for j, J in enumerate(nlp.J): if not J: continue if J.multi_thread: raise RuntimeError( f"The objective function {J.name} was declared with multi_thread=True, " f"but this is not possible to multi_thread objective function with ACADOS" ) if J.type.get_type() == ObjectiveFunction.LagrangeFunction: add_nonlinear_ls_lagrange(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) # Deal with last node to match ipopt formulation if J.node[0] == Node.ALL: add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) elif J.type.get_type() == ObjectiveFunction.MayerFunction: add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) else: raise RuntimeError( "The objective function is not Lagrange nor Mayer." ) # parameter as mayer function # IMPORTANT: it is considered that only parameters are stored in ocp.objectives, for now. if self.nparams: nlp = ocp.nlp[0] # Assume 1 phase for j, J in enumerate(ocp.J): add_nonlinear_ls_mayer(self, J, nlp.states.cx, nlp.controls.cx, nlp.parameters.cx) # Set costs self.acados_ocp.model.cost_y_expr = (self.lagrange_costs.reshape( (-1, 1)) if self.lagrange_costs.numel() else SX(1, 1)) self.acados_ocp.model.cost_y_expr_e = (self.mayer_costs.reshape( (-1, 1)) if self.mayer_costs.numel() else SX(1, 1)) # Set dimensions self.acados_ocp.dims.ny = self.acados_ocp.model.cost_y_expr.shape[ 0] self.acados_ocp.dims.ny_e = self.acados_ocp.model.cost_y_expr_e.shape[ 0] # Set weight self.acados_ocp.cost.W = np.zeros( (1, 1)) if self.W.shape == (0, 0) else self.W self.acados_ocp.cost.W_e = np.zeros( (1, 1)) if self.W_e.shape == (0, 0) else self.W_e # Set target shape self.acados_ocp.cost.yref = np.zeros( (self.acados_ocp.cost.W.shape[0], )) self.acados_ocp.cost.yref_e = np.zeros( (self.acados_ocp.cost.W_e.shape[0], )) elif self.acados_ocp.cost.cost_type == "EXTERNAL": raise RuntimeError( "EXTERNAL is not interfaced yet, please use NONLINEAR_LS") else: raise RuntimeError( "Available acados cost type: 'LINEAR_LS', 'NONLINEAR_LS' and 'EXTERNAL'." ) def __update_solver(self): """ Update the ACADOS solver to new values """ param_init = [] for n in range(self.acados_ocp.dims.N): if self.y_ref: # Target self.ocp_solver.cost_set( n, "yref", np.vstack([data[n] for data in self.y_ref])[:, 0]) # check following line # self.ocp_solver.cost_set(n, "W", self.W) if self.nparams: param_init = self.params_initial_guess.init.evaluate_at(n) self.ocp_solver.set( n, "x", np.concatenate( (param_init, self.ocp.nlp[0].x_init.init.evaluate_at(n)))) self.ocp_solver.set(n, "u", self.ocp.nlp[0].u_init.init.evaluate_at(n)) self.ocp_solver.constraints_set(n, "lbu", self.ocp.nlp[0].u_bounds.min[:, 0]) self.ocp_solver.constraints_set(n, "ubu", self.ocp.nlp[0].u_bounds.max[:, 0]) self.ocp_solver.constraints_set(n, "uh", self.all_g_bounds.max[:, 0]) self.ocp_solver.constraints_set(n, "lh", self.all_g_bounds.min[:, 0]) if n == 0: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 0]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 0]) else: self.ocp_solver.constraints_set(n, "lbx", self.x_bound_min[:, 1]) self.ocp_solver.constraints_set(n, "ubx", self.x_bound_max[:, 1]) if self.y_ref_end: if len(self.y_ref_end) == 1: self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.array(self.y_ref_end[0])[:, 0]) else: self.ocp_solver.cost_set(self.acados_ocp.dims.N, "yref", np.concatenate(self.y_ref_end)[:, 0]) # check following line # self.ocp_solver.cost_set(self.acados_ocp.dims.N, "W", self.W_e) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lbx", self.x_bound_min[:, -1]) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "ubx", self.x_bound_max[:, -1]) if len(self.end_g_bounds.max[:, 0]): self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "uh", self.end_g_bounds.max[:, 0]) self.ocp_solver.constraints_set(self.acados_ocp.dims.N, "lh", self.end_g_bounds.min[:, 0]) if self.ocp.nlp[0].x_init.init.shape[1] == self.acados_ocp.dims.N + 1: if self.nparams: self.ocp_solver.set( self.acados_ocp.dims.N, "x", np.concatenate( (self.params_initial_guess.init[:, 0], self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N])), ) else: self.ocp_solver.set( self.acados_ocp.dims.N, "x", self.ocp.nlp[0].x_init.init[:, self.acados_ocp.dims.N]) def configure(self, options: dict): """ Set some ACADOS options Parameters ---------- options: dict The dictionary of options """ if options is None: options = {} if "acados_dir" in options: del options["acados_dir"] if "cost_type" in options: del options["cost_type"] if self.ocp_solver is None: self.acados_ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" # FULL_CONDENSING_QPOASES self.acados_ocp.solver_options.hessian_approx = "GAUSS_NEWTON" self.acados_ocp.solver_options.integrator_type = "IRK" self.acados_ocp.solver_options.nlp_solver_type = "SQP" self.acados_ocp.solver_options.nlp_solver_tol_comp = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_eq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_ineq = 1e-06 self.acados_ocp.solver_options.nlp_solver_tol_stat = 1e-06 self.acados_ocp.solver_options.nlp_solver_max_iter = 200 self.acados_ocp.solver_options.sim_method_newton_iter = 5 self.acados_ocp.solver_options.sim_method_num_stages = 4 self.acados_ocp.solver_options.sim_method_num_steps = 1 self.acados_ocp.solver_options.print_level = 1 for key in options: setattr(self.acados_ocp.solver_options, key, options[key]) else: available_options = [ "nlp_solver_tol_comp", "nlp_solver_tol_eq", "nlp_solver_tol_ineq", "nlp_solver_tol_stat", ] for key in options: if key in available_options: short_key = key[11:] self.ocp_solver.options_set(short_key, options[key]) else: raise RuntimeError( f"[ACADOS] Only editable solver options after solver creation are :\n {available_options}" ) def online_optim(self, ocp): raise NotImplementedError( "online_optim is not implemented yet with ACADOS backend") def get_optimized_value(self) -> Union[list, dict]: """ Get the previously optimized solution Returns ------- A solution or a list of solution depending on the number of phases """ ns = self.acados_ocp.dims.N n_params = self.ocp.nlp[0].parameters.shape acados_x = np.array( [self.ocp_solver.get(i, "x") for i in range(ns + 1)]).T acados_p = acados_x[:n_params, :] acados_x = acados_x[n_params:, :] acados_u = np.array([self.ocp_solver.get(i, "u") for i in range(ns)]).T out = { "x": [], "u": acados_u, "time_tot": self.ocp_solver.get_stats("time_tot")[0], "iter": self.ocp_solver.get_stats("sqp_iter")[0], "status": self.status, } out["x"] = vertcat(out["x"], acados_x.reshape(-1, 1, order="F")) out["x"] = vertcat(out["x"], acados_u.reshape(-1, 1, order="F")) out["x"] = vertcat(out["x"], acados_p[:, 0]) self.out["sol"] = out out = [] for key in self.out.keys(): out.append(self.out[key]) return out[0] if len(out) == 1 else out def solve(self) -> "AcadosInterface": """ Solve the prepared ocp Returns ------- A reference to the solution """ # Populate costs and constraints vectors self.__set_costs(self.ocp) self.__set_constraints(self.ocp) if self.ocp_solver is None: self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json") self.__update_solver() self.status = self.ocp_solver.solve() self.get_optimized_value() return self
def __set_constraints(self, ocp): """ Set the constraints from the ocp Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram """ # constraints handling in self.acados_ocp if ocp.nlp[ 0].x_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: raise NotImplementedError( "ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the x_bounds") if ocp.nlp[ 0].u_bounds.type != InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT: raise NotImplementedError( "ACADOS must declare an InterpolationType.CONSTANT_WITH_FIRST_AND_LAST_DIFFERENT " "for the u_bounds") u_min = np.array(ocp.nlp[0].u_bounds.min) u_max = np.array(ocp.nlp[0].u_bounds.max) x_min = np.array(ocp.nlp[0].x_bounds.min) x_max = np.array(ocp.nlp[0].x_bounds.max) self.all_constr = SX() self.end_constr = SX() # TODO:change for more node flexibility on bounds self.all_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) self.end_g_bounds = Bounds(interpolation=InterpolationType.CONSTANT) for i, nlp in enumerate(ocp.nlp): x = nlp.states.cx u = nlp.controls.cx p = nlp.parameters.cx for g, G in enumerate(nlp.g): if not G: continue if G.node[0] == Node.ALL or G.node[0] == Node.ALL_SHOOTING: self.all_constr = vertcat(self.all_constr, G.function(x, u, p)) self.all_g_bounds.concatenate(G.bounds) if G.node[0] == Node.ALL: self.end_constr = vertcat(self.end_constr, G.function(x, u, p)) self.end_g_bounds.concatenate(G.bounds) elif G.node[0] == Node.END: self.end_constr = vertcat(self.end_constr, G.function(x, u, p)) self.end_g_bounds.concatenate(G.bounds) else: raise RuntimeError( "Except for states and controls, Acados solver only handles constraints on last or all nodes." ) self.acados_model.con_h_expr = self.all_constr self.acados_model.con_h_expr_e = self.end_constr if not np.all(np.all(u_min.T == u_min.T[0, :], axis=0)): raise NotImplementedError( "u_bounds min must be the same at each shooting point with ACADOS" ) if not np.all(np.all(u_max.T == u_max.T[0, :], axis=0)): raise NotImplementedError( "u_bounds max must be the same at each shooting point with ACADOS" ) if (not np.isfinite(u_min).all() or not np.isfinite(x_min).all() or not np.isfinite(u_max).all() or not np.isfinite(x_max).all()): raise NotImplementedError( "u_bounds and x_bounds cannot be set to infinity in ACADOS. Consider changing it " "to a big value instead.") # setup state constraints # TODO replace all these np.concatenate by proper bound and initial_guess classes self.x_bound_max = np.ndarray((self.acados_ocp.dims.nx, 3)) self.x_bound_min = np.ndarray((self.acados_ocp.dims.nx, 3)) param_bounds_max = [] param_bounds_min = [] if self.nparams: param_bounds_max = self.params_bounds.max[:, 0] param_bounds_min = self.params_bounds.min[:, 0] for i in range(3): self.x_bound_max[:, i] = np.concatenate( (param_bounds_max, np.array(ocp.nlp[0].x_bounds.max[:, i]))) self.x_bound_min[:, i] = np.concatenate( (param_bounds_min, np.array(ocp.nlp[0].x_bounds.min[:, i]))) # setup control constraints self.acados_ocp.constraints.lbu = np.array(ocp.nlp[0].u_bounds.min[:, 0]) self.acados_ocp.constraints.ubu = np.array(ocp.nlp[0].u_bounds.max[:, 0]) self.acados_ocp.constraints.idxbu = np.array( range(self.acados_ocp.dims.nu)) self.acados_ocp.dims.nbu = self.acados_ocp.dims.nu # initial state constraints self.acados_ocp.constraints.lbx_0 = self.x_bound_min[:, 0] self.acados_ocp.constraints.ubx_0 = self.x_bound_max[:, 0] self.acados_ocp.constraints.idxbx_0 = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_0 = self.acados_ocp.dims.nx # setup path state constraints self.acados_ocp.constraints.Jbx = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx = self.x_bound_min[:, 1] self.acados_ocp.constraints.ubx = self.x_bound_max[:, 1] self.acados_ocp.constraints.idxbx = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx = self.acados_ocp.dims.nx # setup terminal state constraints self.acados_ocp.constraints.Jbx_e = np.eye(self.acados_ocp.dims.nx) self.acados_ocp.constraints.lbx_e = self.x_bound_min[:, -1] self.acados_ocp.constraints.ubx_e = self.x_bound_max[:, -1] self.acados_ocp.constraints.idxbx_e = np.array( range(self.acados_ocp.dims.nx)) self.acados_ocp.dims.nbx_e = self.acados_ocp.dims.nx # setup algebraic constraint self.acados_ocp.constraints.lh = np.array(self.all_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh = np.array(self.all_g_bounds.max[:, 0]) # setup terminal algebraic constraint self.acados_ocp.constraints.lh_e = np.array(self.end_g_bounds.min[:, 0]) self.acados_ocp.constraints.uh_e = np.array(self.end_g_bounds.max[:, 0])
def acados_nonlinear_quad_model(num_states, g=9.81): model_name = 'nonlinear_quad' position = SX.sym('o', 3) velocity = SX.sym('v', 3) phi = SX.sym('phi') theta = SX.sym('theta') psi = SX.sym('psi') gravity = SX.zeros(3) gravity[2] = g x = vertcat(position, phi, theta, psi, velocity) p = SX.sym('p') q = SX.sym('q') r = SX.sym('r') F = SX.sym('F') u = vertcat(p, q, r, F) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(phi) r1[1, 2] = -sin(phi) r1[2, 1] = sin(phi) r1[2, 2] = cos(phi) r2 = SX(3, 3) r2[0, 0] = cos(theta) r2[0, 2] = sin(theta) r2[1, 1] = 1 r2[2, 0] = -sin(theta) r2[2, 2] = cos(theta) r3 = SX(3, 3) r3[0, 0] = cos(psi) r3[0, 1] = -sin(psi) r3[1, 0] = sin(psi) r3[1, 1] = cos(psi) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] # tau[:,0] = (inv(r1))[:,0] # tau[:,1] = (inv(r1))[:,1] # tau[:,2] = (inv(r1 @ r2))[:,2] # tau[0,0] = cos(theta) # tau[0,2] = -sin(theta) # tau[1,1] = 1 # tau[1,2] = sin(phi) * cos(theta) # tau[2,0] = sin(theta) # tau[2,2] = cos(phi) * cos(theta) tc = 0.59375 # mapping control to throttle # f_const = 100 f_const = 20 # tc = 0.75 # # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) # f_expl = vertcat( # velocity, # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g)/tc)) + gravity # ) f_expl = vertcat( velocity, inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * (u[3] * ((g + f_const) / tc) - f_const) + gravity) # f_expl = vertcat( # velocity, # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * ((g - f_const)*(1 - tc) / (1 - u[3]) + f_const) + gravity # ) # xdot xdot = SX.sym('xdot', 9) #parameters p = [] f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def gp_pred_function(x, hyp, kern_types, x_train=None, beta=None, k_inv_training=None, pred_var=True, compute_grads=False): """ """ n_gps = len(kern_types) inp = SX.sym("input", x.shape) out_dict = dict() mu_all = [] pred_sigma_all = [] jac_mu_all = [] for i in range(n_gps): kern_i = _get_kernel_function(kern_types[i], hyp[i]) if beta is None: beta_i = None else: beta_i = beta[:, i] k_inv_i = None if not k_inv_training is None: k_inv_i = k_inv_training[i] if pred_var: mu_new, sigma_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i, pred_var) pred_func = Function("pred_func", [inp], [mu_new, sigma_new], ["inp"], ["mu_1", "sigma_1"]) F_1 = pred_func(inp=x) pred_sigma = F_1["sigma_1"] pred_sigma_all = horzcat(pred_sigma_all, pred_sigma) else: mu_new = gp_pred(inp, kern_i, beta_i, x_train, k_inv_i, pred_var) pred_func = Function("pred_func", [inp], [mu_new], ["inp"], ["mu_1"]) F_1 = pred_func(inp=x) mu_1 = F_1["mu_1"] mu_all = horzcat(mu_all, mu_1) if compute_grads: # jac_func = pred_func.jacobian("inp","mu_1") jac_func = pred_func.factory('dmudinp', ['inp'], ['jac:mu_1:inp']) F_1_jac = jac_func(inp=x) # print(F_1_jac) jac_mu = F_1_jac['jac_mu_1_inp'] jac_mu_all = vertcat(jac_mu_all, jac_mu) out_dict["pred_mu"] = mu_all if pred_var: out_dict["pred_sigma"] = pred_sigma_all if compute_grads: out_dict["jac_mu"] = jac_mu_all return out_dict
from casadi import SX, Function, vertcat from acados import ocp_nlp_function, ocp_nlp_ls_cost, ocp_nlp, ocp_nlp_solver from models import chen_model N = 10 ode_fun, nx, nu = chen_model() nlp = ocp_nlp({'N': N, 'nx': nx, 'nu': nu, 'ng': N*[1] + [0]}) # ODE Model step = 0.1 nlp.set_model(ode_fun, step) # Cost function Q = diag([1.0, 1.0]) R = 1e-2 x = SX.sym('x', nx) u = SX.sym('u', nu) u_N = SX.sym('u', 0) f = ocp_nlp_function(Function('ls_cost', [x, u], [vertcat(x, u)])) f_N = ocp_nlp_function(Function('ls_cost_N', [x, u_N], [x])) ls_cost = ocp_nlp_ls_cost(N, N*[f]+[f_N]) ls_cost.ls_cost_matrix = N*[block_diag(Q, R)] + [Q] nlp.set_cost(ls_cost) # Constraints g = ocp_nlp_function(Function('path_constraint', [x, u], [u])) g_N = ocp_nlp_function(Function('path_constraintN', [x, u], [SX([])])) nlp.set_path_constraints(N*[g] + [g_N]) for i in range(N): nlp.lg[i] = -0.5 nlp.ug[i] = +0.5
def acados_linear_quad_model_moving_eq(num_states, g=9.81): model_name = 'linear_quad_v2' tc = 0.59375 # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) gravity = SX.zeros(3) gravity[2] = g x = SX.sym('x', 9) u = SX.sym('u', 4) xdot = SX.sym('xdot', 9) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(x[3]) r1[1, 2] = -sin(x[3]) r1[2, 1] = sin(x[3]) r1[2, 2] = cos(x[3]) r2 = SX(3, 3) r2[0, 0] = cos(x[4]) r2[0, 2] = sin(x[4]) r2[1, 1] = 1 r2[2, 0] = -sin(x[4]) r2[2, 2] = cos(x[4]) r3 = SX(3, 3) r3[0, 0] = cos(x[5]) r3[0, 1] = -sin(x[5]) r3[1, 0] = sin(x[5]) r3[1, 1] = cos(x[5]) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] f_nl = vertcat(x[6:9], inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity) # f_const = 20 # f_nl = vertcat( # x[6:9], # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g + f_const)/tc) - f_const) + gravity # ) A = jacobian(f_nl, x) B = jacobian(f_nl, u) p = [] f_expl = mtimes(A, x) + mtimes(B, u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model