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].X[0] u = ocp.nlp[0].U[0] 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 in range(ocp.n_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 # 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 """ 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)) 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": 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, 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(n_controls)) vu = np.zeros(n_controls) vu[index] = 1.0 self.Vu = np.vstack((self.Vu, np.diag(vu))) self.Vx = np.vstack( (self.Vx, np.zeros((n_controls, n_states)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * n_controls)) if J[0]["target"] is not None: y_tp = np.zeros((n_controls, 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((n_controls, 1)) for _ 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(n_states)) vx = np.zeros(n_states) vx[index] = 1.0 self.Vx = np.vstack((self.Vx, np.diag(vx))) self.Vu = np.vstack( (self.Vu, np.zeros((n_states, n_controls)))) self.W = linalg.block_diag( self.W, np.diag([J[0]["objective"].weight] * n_states)) if J[0]["target"] is not None: y_ref_tp = [] for J_tp in J: y_tp = np.zeros((n_states, 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((n_states, 1)) for _ 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(n_states)) vxe = np.zeros(n_states) 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] * n_states)) if J[0]["target"] is not None: y_tp = np.zeros((n_states, 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((n_states, 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(n_states)) vxe = np.zeros(n_states) 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] * n_states)) if J[0]["target"] is not None: y_tp = np.zeros((n_states, 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((n_states, 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.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 in range(ocp.n_phases): for j, J in enumerate(ocp.nlp[i].J): if not J: continue 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.nparams: 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): """ 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([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.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
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. Use use_SX " ) 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), )) if len(self.y_ref_end): self.acados_ocp.cost.yref_e = np.concatenate( self.y_ref_end, -1).T.squeeze() else: self.acados_ocp.cost.yref_e = np.zeros((1, )) 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 = "PARTIAL_CONDENSING_HPIPM" # 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
tau = -Kp_theta * (target_position[2] - x_noise[2]) - Kd_theta * ( target_position[5] - x_noise[5]) - Kp_phi * ( target_position[0] - x_noise[0]) - Kd_phi * (target_position[3] - x_noise[3]) f = mb * g * np.cos( x_noise[2]) + Kp_l * (target_position[1] - x_noise[1]) + Kd_l * ( target_position[4] - x_noise[4]) # update reference for j in range(N): acados_solver.set(j, "p", np.array([tau, f])) # solve ocp t = time.time() status = acados_solver.solve() if status != 0: raise Exception( "acados returned status {} in closed loop iteration {}.".format( status, i)) elapsed = time.time() - t time_iterations[i] = elapsed # manage timings tcomp_sum += elapsed if elapsed > tcomp_max: tcomp_max = elapsed # get solution x0 = acados_solver.get(0, "x")
def main(interface_type='ctypes'): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx # define the different options for the use-case demonstration N0 = 20 # original number of shooting nodes N12 = 15 # change the number of shooting nodes for use-cases 1 and 2 condN12 = max(1, round(N12/1)) # change the number of cond_N for use-cases 1 and 2 (for PARTIAL_* solvers only) Tf_01 = 1.0 # original final time and for use-case 1 Tf_2 = Tf_01 * 0.7 # change final time for use-case 2 (but keep N identical) # set dimensions ocp.dims.N = N0 # set cost Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) 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 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' # ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # set prediction horizon ocp.solver_options.tf = Tf_01 print(80*'-') print('generate code and compile...') if interface_type == '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') elif interface_type == 'ctypes': ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json') elif interface_type == 'cython_prebuilt': from c_generated_code.acados_ocp_solver_pyx import AcadosOcpSolverCython ocp_solver = AcadosOcpSolverCython(ocp.model.name, ocp.solver_options.nlp_solver_type, ocp.dims.N) # test setting HPIPM options ocp_solver.options_set('qp_tol_ineq', 1e-8) ocp_solver.options_set('qp_tau_min', 1e-10) ocp_solver.options_set('qp_mu0', 1e0) # -------------------------------------------------------------------------------- # 0) solve the problem defined here (original from code export), analog to 'minimal_example_ocp.py' nvariant = 0 simX0 = np.ndarray((N0 + 1, nx)) simU0 = np.ndarray((N0, nu)) print(80*'-') print(f'solve original code with N = {N0} and Tf = {Tf_01} s:') status = ocp_solver.solve() if status != 0: ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") raise Exception(f'acados returned status {status}.') # get solution for i in range(N0): simX0[i, :] = ocp_solver.get(i, "x") simU0[i, :] = ocp_solver.get(i, "u") simX0[N0, :] = ocp_solver.get(N0, "x") ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") ocp_solver.store_iterate(filename=f'final_iterate_{interface_type}_variant{nvariant}.json', overwrite=True) if PLOT:# plot but don't halt plot_pendulum(np.linspace(0, Tf_01, N0 + 1), Fmax, simU0, simX0, latexify=False, plt_show=False, X_true_label=f'original: N={N0}, Tf={Tf_01}')
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 opts: ACADOS Options of Acados from ACADOS 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 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: Solver.ACADOS = None): """ Parameters ---------- ocp: OptimalControlProgram A reference to the current OptimalControlProgram solver_options: ACADOS 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) # solver_options = solver_options.__dict__ if solver_options is None: solver_options = Solver.ACADOS() self.acados_ocp = AcadosOcp(acados_path=solver_options.acados_dir) self.acados_model = AcadosModel() self.__set_cost_type(solver_options.cost_type) self.__set_constr_type(solver_options.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.real_time_to_optimize = -1 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) self.opts = Solver.ACADOS( ) if solver_options is None else solver_options 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 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, "solver_time_to_optimize": self.ocp_solver.get_stats("time_tot")[0], "real_time_to_optimize": self.real_time_to_optimize, "iter": self.ocp_solver.get_stats("sqp_iter")[0], "status": self.status, "solver": SolverType.ACADOS, } 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) -> Union[list, dict]: """ Solve the prepared ocp Returns ------- A reference to the solution """ tic = perf_counter() # Populate costs and constraints vectors self.__set_costs(self.ocp) self.__set_constraints(self.ocp) options = self.opts.as_dict(self) if self.ocp_solver is None: for key in options: setattr(self.acados_ocp.solver_options, key, options[key]) self.ocp_solver = AcadosOcpSolver(self.acados_ocp, json_file="acados_ocp.json") self.opts.set_only_first_options_has_changed(False) self.opts.set_has_tolerance_changed(False) else: if self.opts.only_first_options_has_changed: raise RuntimeError( "Some options has been changed the second time acados was run.", "Only " + str(Solver.ACADOS.get_tolerance_keys()) + " can be modified.", ) if self.opts.has_tolerance_changed: for key in self.opts.get_tolerance_keys(): short_key = key[12:] self.ocp_solver.options_set(short_key, options[key[1:]]) self.opts.set_has_tolerance_changed(False) self.__update_solver() self.status = self.ocp_solver.solve() self.real_time_to_optimize = perf_counter() - tic return self.get_optimized_value()
j, "y_ref", nmp.array([ -1, -1, 0, 0.770, -0.421, -0.421, -0.230, 0, 0, 0, uSS, uSS, uSS, uSS ])) elif tCurr <= 6: # roll = -1 pitch = 1 yaw = 0 # q = 0.770 -0.421 0.421 0.230 acados_ocp_solver.set( j, "y_ref", nmp.array([ -1, 1, 0, 0.770, -0.421, 0.421, 0.230, 0, 0, 0, uSS, uSS, uSS, uSS ])) status = acados_ocp_solver.solve() if status != 0: raise Exception( 'acados acados_ocp_solver returned status {}. Exiting.'.format( status)) simU[i, :] = acados_ocp_solver.get(0, "u") acados_integrator.set("x", x0) acados_integrator.set("u", simU[i, :]) status = acados_integrator.solve() if status != 0: raise Exception( 'acados integrator returned status {}. Exiting.'.format(status)) # update state x0 = acados_integrator.get("x")
class QuadOptimizer: def __init__(self, quad_model, quad_constraints, t_horizon, n_nodes, sim_required=False, max_hight=1.0): self.model = quad_model self.constraints = quad_constraints self.g = 9.8066 self.T = t_horizon self.N = n_nodes self.simulation_required = sim_required robot_name_ = rospy.get_param("~robot_name", "bebop1_r") self.current_pose = None self.current_state = np.zeros((13, 1)) self.dt = 0.02 self.rate = rospy.Rate(1 / self.dt) self.time_stamp = None self.trajectory_path = None self.current_twist = np.zeros(3) self.att_command = AttitudeTarget() self.att_command.type_mask = 3 self.pendulum_state = np.zeros(4) # subscribers # the robot state robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry, self.robot_state_callback) # pendulum state pendulum_state_sub_ = rospy.Subscriber('/pendulum_pose', Odometry, self.pendulum_state_callback) # trajectory robot_trajectory_sub_ = rospy.Subscriber( '/robot_trajectory', itm_trajectory_msg, self.trajectory_command_callback) # publisher self.att_setpoint_pub = rospy.Publisher( '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) # create a server server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server) # setup optimizer self.quadrotor_optimizer_setup() # # It seems that thread cannot ensure the performance of the time self.att_thread = Thread(target=self.send_command, args=()) self.att_thread.daemon = True self.att_thread.start() def robot_state_callback(self, data): # robot state as [x, y, z, vx, vy, vz, roll, pitch, yaw] roll, pitch, yaw = self.quaternion_to_rpy(data.pose.pose.orientation) # self.current_state[:6] = np.array([data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, # data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z]).reshape(6,1) # self.current_state[-3:] = np.array([roll, pitch, yaw]).reshape(3,1) self.current_state = np.array([ data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z, data.pose.pose.position.x, data.pose.pose.position.y, 0., 0., roll, pitch, yaw ], dtype=np.float64) def pendulum_state_callback(self, data): # pendulum state as [x, y, z, vx, vy, vz, s, r, ds, dr, roll, pitch, yaw] s, r = data.pose.pose.position.x, data.pose.pose.position.y ds, dr = data.twist.twist.linear.x, data.twist.twist.linear.y # self.current_state[7:10] = np.array([s, r, ds, dr]).reshape(4, 1) self.pendulum_state = np.array([s, r, ds, dr]) # .reshape(4, 1) # self.current_state_pendulum = np.array([self.current_state[0], self.current_state[1], self.current_state[2], # self.current_state[3], self.current_state[4], self.current_state[5], # s, r, ds, dr, # self.current_state[10], self.current_state[11], self.current_state[12]], dtype=np.float64) def trajectory_command_callback(self, data): temp_traj = data.traj if data.size != len(temp_traj): rospy.logerr('Some data are lost') else: self.trajectory_path = np.zeros((data.size, 13)) for i in range(data.size): self.trajectory_path[i] = np.array([ temp_traj[i].x, temp_traj[i].y, temp_traj[i].z, temp_traj[i].vx, temp_traj[i].vy, temp_traj[i].vz, temp_traj[i].load_x, temp_traj[i].load_y, temp_traj[i].load_vx, temp_traj[i].load_vy, temp_traj[i].roll, temp_traj[i].pitch, temp_traj[i].yaw, ]) def quadrotor_optimizer_setup(self, ): # Q_m_ = np.diag([80.0, 80.0, 120.0, 20.0, 20.0, # 30.0, 10.0, 10.0, 0.0]) # position, velocity, roll, pitch, (not yaw) # P_m_ = np.diag([86.21, 86.21, 120.95, # 6.94, 6.94, 11.04]) # only p and v # P_m_[0, 3] = 6.45 # P_m_[3, 0] = 6.45 # P_m_[1, 4] = 6.45 # P_m_[4, 1] = 6.45 # P_m_[2, 5] = 10.95 # P_m_[5, 2] = 10.95 # R_m_ = np.diag([50.0, 60.0, 1.0]) Q_m_ = np.diag( [ 10.0, 10.0, 10.0, 3e-1, 3e-1, 3e-1, #3e-1, 3e-1, 3e-2, 3e-2, 100.0, 100.0, 1e-3, 1e-3, 10.5, 10.5, 10.5 ] ) # position, velocity, load_position, load_velocity, [roll, pitch, yaw] P_m_ = np.diag([ 10.0, 10.0, 10.0, 0.05, 0.05, 0.05 # 10.0, 10.0, 10.0, # 0.05, 0.05, 0.05 ]) # only p and v # P_m_[0, 8] = 6.45 # P_m_[8, 0] = 6.45 # P_m_[1, 9] = 6.45 # P_m_[9, 1] = 6.45 # P_m_[2, 10] = 10.95 # P_m_[10, 2] = 10.95 R_m_ = np.diag([3.0, 3.0, 3.0, 1.0]) nx = self.model.x.size()[0] self.nx = nx nu = self.model.u.size()[0] self.nu = nu ny = nx + nu n_params = self.model.p.size()[0] if isinstance(self.model.p, ca.SX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, acados_source_path) # create OCP ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = self.model ocp.dims.N = self.N ocp.solver_options.tf = self.T # initialize parameters ocp.dims.np = n_params ocp.parameter_values = np.zeros(n_params) # cost type ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_) ocp.cost.W_e = P_m_ # np.zeros((nx-3, nx-3)) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[-nu:, -nu:] = np.eye(nu) ocp.cost.Vx_e = np.zeros((nx - 7, nx)) # only consider p and v ocp.cost.Vx_e[:nx - 7, :nx - 7] = np.eye(nx - 7) # initial reference trajectory_ref x_ref = np.zeros(nx) x_ref_e = np.zeros(nx - 7) u_ref = np.zeros(nu) u_ref[-1] = self.g ocp.cost.yref = np.concatenate((x_ref, u_ref)) ocp.cost.yref_e = x_ref_e # Set constraints ocp.constraints.lbu = np.array([ self.constraints.roll_min, self.constraints.pitch_min, self.constraints.yaw_min, self.constraints.thrust_min ]) ocp.constraints.ubu = np.array([ self.constraints.roll_max, self.constraints.pitch_max, self.constraints.yaw_max, self.constraints.thrust_max ]) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # initial state ocp.constraints.x0 = x_ref # solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # explicit Runge-Kutta integrator ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI' ocp.solver_options.levenberg_marquardt = 0.12 # 0.0 # compile acados ocp json_file = os.path.join('./' + self.model.name + '_acados_ocp.json') self.solver = AcadosOcpSolver(ocp, json_file=json_file) if self.simulation_required: self.integrator = AcadosSimSolver(ocp, json_file=json_file) def mpc_estimation_loop(self, mpc_iter): if self.trajectory_path is not None and self.current_state is not None: t1 = time.time() # dt = 0.1 current_trajectory = self.trajectory_path u_des = np.array([0.0, 0.0, 0.0, self.g]) new_state = self.current_state # new_state[6:10] = self.pendulum_state new_state[6] = self.pendulum_state[0] - self.current_state[0] new_state[7] = self.pendulum_state[1] - self.current_state[1] new_state[8] = self.pendulum_state[2] - self.current_state[3] new_state[9] = self.pendulum_state[3] - self.current_state[4] simX[mpc_iter] = new_state # mpc_iter+1 ? simD[mpc_iter] = current_trajectory[0] l = 0.1 #print(np.rad2deg(np.arcsin(new_state[6]/l))) # print() # print("current state: ") # np.set_printoptions(suppress=True) # print(new_state) # print() # print("current trajectory: ") # print(current_trajectory[-1]) tra = current_trajectory[0] # error_xyz = np.linalg.norm(np.array([new_state[0] - tra[0], new_state[1] - tra[1], new_state[2] - tra[2]])) # print(error_xyz) self.solver.set(self.N, 'yref', current_trajectory[-1, :6]) for i in range(self.N): self.solver.set( i, 'yref', np.concatenate([current_trajectory[i].flatten(), u_des])) # self.solver.set(0, 'lbx', self.current_state) # self.solver.set(0, 'ubx', self.current_state) self.solver.set(0, 'lbx', new_state) self.solver.set(0, 'ubx', new_state) status = self.solver.solve() tarr[mpc_iter] = time.time() - t1 if status != 0: rospy.logerr("MPC cannot find a proper solution.") # self.solver.print_statistics() print() print("current state: ") np.set_printoptions(suppress=True) print(new_state) print() print("current trajectory: ") print(current_trajectory[0]) self.att_command.orientation = Quaternion( *self.rpy_to_quaternion(0.0, 0.0, 0.0, w_first=False)) #self.att_command.thrust = 0.5 self.att_command.thrust = 0.62 self.att_command.body_rate.z = 0.0 else: # print() # print("predicted trajectory:") # for i in range(self.N): # print(self.solver.get(i, 'x')) mpc_u_ = self.solver.get(0, 'u') simU[mpc_iter] = mpc_u_ quat_local_ = self.rpy_to_quaternion(mpc_u_[0], mpc_u_[1], mpc_u_[2], w_first=False) self.att_command.orientation.x = quat_local_[0] self.att_command.orientation.y = quat_local_[1] self.att_command.orientation.z = quat_local_[2] self.att_command.orientation.w = quat_local_[3] # print(self.att_command.orientation) #self.att_command.thrust = mpc_u_[3]/9.8066 - 0.5 # self.att_command.thrust = mpc_u_[3]/9.8066 - 0.38 self.att_command.thrust = mpc_u_[3] / 9.8066 * 0.6175 # print() # print("mpc_u: ") # print(mpc_u_) # print() # print("---------------------------------------") # print(self.att_command.thrust) # yaw_command_ = self.yaw_command(current_yaw_, trajectory_path_[1, -1], 0.0) # yaw_command_ = self.yaw_controller(trajectory_path_[1, -1]-current_yaw_) # self.att_command.angular.z = yaw_command_ #self.att_command.body_rate.z = 0.0 # self.att_setpoint_pub.publish(self.att_command) # print("time: ") # print(time.time()-time_1) else: if self.trajectory_path is None: rospy.loginfo("waiting trajectory") elif self.current_state is None: rospy.loginfo("waiting current state") else: rospy.loginfo("Unknown error") self.rate.sleep() return True @staticmethod def quaternion_to_rpy(quaternion): q0, q1, q2, q3 = quaternion.w, quaternion.x, quaternion.y, quaternion.z roll_ = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) pitch_ = np.arcsin(2 * (q0 * q2 - q3 * q1)) yaw_ = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) return roll_, pitch_, yaw_ @staticmethod def state_server(req): return SetBoolResponse(True, 'MPC is ready') @staticmethod def rpy_to_quaternion(r, p, y, w_first=True): cy = np.cos(y * 0.5) sy = np.sin(y * 0.5) cp = np.cos(p * 0.5) sp = np.sin(p * 0.5) cr = np.cos(r * 0.5) sr = np.sin(r * 0.5) qw = cr * cp * cy + sr * sp * sy qx = sr * cp * cy - cr * sp * sy qy = cr * sp * cy + sr * cp * sy qz = cr * cp * sy - sr * sp * cy if w_first: return np.array([qw, qx, qy, qz]) else: return np.array([qx, qy, qz, qw]) def send_command(self, ): rate = rospy.Rate(100) # Hz self.att_command.header = Header() while not rospy.is_shutdown(): # t2 = time.time() command_ = self.att_command # self.att_command.header.stamp = rospy.Time.now() self.att_setpoint_pub.publish(command_) try: # prevent garbage in console output when thread is killed rate.sleep() except rospy.ROSInterruptException: pass
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 main(discretization='shooting_nodes'): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model integrator_type = 'LIFTED_IRK' # ERK, IRK, GNSF, LIFTED_IRK if integrator_type == 'GNSF': acados_dae_model_json_dump(model) # structure detection in Matlab/Octave -> produces 'pendulum_ode_gnsf_functions.json' status = os.system('octave detect_gnsf_from_json.m') # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict Tf = 1.0 nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = 15 # discretization ocp.dims.N = N # shooting_nodes = np.linspace(0, Tf, N+1) time_steps = np.linspace(0, 1, N) time_steps = Tf * time_steps / sum(time_steps) shooting_nodes = np.zeros((N + 1, )) for i in range(len(time_steps)): shooting_nodes[i + 1] = shooting_nodes[i] + time_steps[i] # nonuniform discretizations can be defined either by shooting_nodes or time_steps: if discretization == 'shooting_nodes': ocp.solver_options.shooting_nodes = shooting_nodes elif discretization == 'time_steps': ocp.solver_options.time_steps = time_steps else: raise NotImplementedError( f"discretization type {discretization} not supported.") # set num_steps ocp.solver_options.sim_method_num_steps = 2 * np.ones((N, )) ocp.solver_options.sim_method_num_steps[0] = 3 # set num_stages ocp.solver_options.sim_method_num_stages = 2 * np.ones((N, )) ocp.solver_options.sim_method_num_stages[0] = 4 # set cost Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) 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 ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.x0 = x0 ocp.constraints.idxbu = np.array([0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = integrator_type ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.initialize_t_slacks = 1 # Set additional options for Simulink interface: acados_path = get_acados_path() json_path = os.path.join(acados_path, 'interfaces/acados_template/acados_template') with open(json_path + '/simulink_default_opts.json', 'r') as f: simulink_opts = json.load(f) ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json', simulink_opts=simulink_opts) # ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') simX = np.ndarray((N + 1, nx)) simU = np.ndarray((N, nu)) # change options after creating ocp_solver ocp_solver.options_set("step_length", 0.99999) ocp_solver.options_set("globalization", "fixed_step") # fixed_step, merit_backtracking ocp_solver.options_set("tol_eq", TOL) ocp_solver.options_set("tol_stat", TOL) ocp_solver.options_set("tol_ineq", TOL) ocp_solver.options_set("tol_comp", TOL) # initialize solver for i in range(N): ocp_solver.set(i, "x", x0) status = ocp_solver.solve() if status not in [0, 2]: raise Exception('acados returned status {}. Exiting.'.format(status)) # get primal 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") print("inequality multipliers at stage 1") print(ocp_solver.get(1, "lam")) # inequality multipliers at stage 1 print("slack values at stage 1") print(ocp_solver.get(1, "t")) # slack values at stage 1 print("multipliers of dynamic conditions between stage 1 and 2") print(ocp_solver.get( 1, "pi")) # multipliers of dynamic conditions between stage 1 and 2 # initialize ineq multipliers and slacks at stage 1 ocp_solver.set(1, "lam", np.zeros(2, )) ocp_solver.set(1, "t", np.zeros(2, )) ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # timings time_tot = ocp_solver.get_stats("time_tot") time_lin = ocp_solver.get_stats("time_lin") time_sim = ocp_solver.get_stats("time_sim") time_qp = ocp_solver.get_stats("time_qp") print( f"timings OCP solver: total: {1e3*time_tot}ms, lin: {1e3*time_lin}ms, sim: {1e3*time_sim}ms, qp: {1e3*time_qp}ms" ) # print("simU", simU) # print("simX", simX) iterate_filename = f'final_iterate_{discretization}.json' ocp_solver.store_iterate(filename=iterate_filename, overwrite=True) plot_pendulum(shooting_nodes, Fmax, simU, simX, latexify=False) del ocp_solver
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()
def solve_armijo_problem_with_setting(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = AcadosModel() x = SX.sym('x') # dynamics: identity model.disc_dyn_expr = x model.x = x model.u = SX.sym('u', 0, 0) # [] / None doesnt work model.p = [] model.name = f'armijo_problem' ocp.model = model # discretization Tf = 1 N = 1 ocp.dims.N = N ocp.solver_options.tf = Tf # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = x @ x ocp.model.cost_expr_ext_cost_custom_hess_e = 1.0 # 2.0 is the actual hessian # constarints ocp.constraints.idxbx = np.array([0]) ocp.constraints.lbx = np.array([-10.0]) ocp.constraints.ubx = np.array([10.0]) # options ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' # 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.integrator_type = 'DISCRETE' ocp.solver_options.print_level = 0 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = globalization ocp.solver_options.alpha_reduction = 0.9 ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.eps_sufficient_descent = 5e-1 SQP_max_iter = 200 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.nlp_solver_max_iter = SQP_max_iter # create solver ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver xinit = np.array([1.0]) [ocp_solver.set(i, "x", xinit) for i in range(N + 1)] # get stats status = ocp_solver.solve() ocp_solver.print_statistics() iter = ocp_solver.get_stats('sqp_iter')[0] alphas = ocp_solver.get_stats('alpha')[1:] qp_iters = ocp_solver.get_stats('qp_iter') print(f"acados ocp solver returned status {status}") # get solution solution = ocp_solver.get(0, "x") print(f"found solution {solution}") # print summary print(f"solved Armijo test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations" ) print(f"alphas: {alphas[:iter]}") print(f"total number of QP iterations: {sum(qp_iters[:iter])}") # compare to analytical solution exact_solution = np.array([0.0]) sol_err = max(np.abs(solution - exact_solution)) print(f"error wrt analytical solution {sol_err}") # checks if ocp.model.cost_expr_ext_cost_custom_hess_e == 1.0: if globalization == 'MERIT_BACKTRACKING': if sol_err > TOL * 1e1: raise Exception( f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}" ) else: print(f"matched analytical solution with tolerance {TOL}") if status != 0: raise Exception( f"acados solver returned status {status} != 0.") if line_search_use_sufficient_descent == 1: if iter > 22: raise Exception(f"acados ocp solver took {iter} iterations." + \ "Expected <= 22 iterations for globalized SQP method with aggressive eps_sufficient_descent condition on Armijo test problem.") else: if iter < 64: raise Exception(f"acados ocp solver took {iter} iterations." + \ "Expected > 64 iterations for globalized SQP method without sufficient descent condition on Armijo test problem.") elif globalization == 'FIXED_STEP': if status != 2: raise Exception( f"acados solver returned status {status} != 2. Expected maximum iterations for full-step SQP on Armijo test problem." ) else: print( f"Sucess: Expected maximum iterations for full-step SQP on Armijo test problem." ) print(f"\n\n----------------------\n")
class MPC_controller(object): def __init__(self, quad_model, quad_constraints, t_horizon, n_nodes, sim_required=False): self.model = quad_model self.constraints = quad_constraints self.g_ = 9.8066 self.T = t_horizon self.N = n_nodes self.simulation_required = sim_required self.current_pose = None self.current_state = None self.dt = 0.05 self.rate = rospy.Rate(1 / self.dt) self.time_stamp = None self.trajectory_path = None self.current_twist = np.zeros(3) self.att_command = AttitudeTarget() self.att_command.type_mask = 128 # subscribers ## the robot state robot_state_sub_ = rospy.Subscriber('/robot_pose', Odometry, self.robot_state_callback) ## trajectory robot_trajectory_sub_ = rospy.Subscriber( '/robot_trajectory', itm_trajectory_msg, self.trajectory_command_callback) # publisher self.att_setpoint_pub = rospy.Publisher( '/mavros/setpoint_raw/attitude', AttitudeTarget, queue_size=1) # create a server server_ = rospy.Service('uav_mpc_server', SetBool, self.state_server) # setup optimizer self.quadrotor_optimizer_setup() # # It seems that thread cannot ensure the performance of the time self.att_thread = Thread(target=self.send_command, args=()) self.att_thread.daemon = True self.att_thread.start() def robot_state_callback(self, data): # robot state as [x, y, z, vx, vy, vz, [w, x, y, z]] self.current_state = np.array([ data.pose.pose.position.x, data.pose.pose.position.y, data.pose.pose.position.z, data.pose.pose.orientation.w, data.pose.pose.orientation.x, data.pose.pose.orientation.y, data.pose.pose.orientation.z, data.twist.twist.linear.x, data.twist.twist.linear.y, data.twist.twist.linear.z, ]).reshape(1, -1) def trajectory_command_callback(self, data): temp_traj = data.traj if data.size != len(temp_traj): rospy.logerr('Some data are lost') else: self.trajectory_path = np.zeros((data.size, 10)) for i in range(data.size): quaternion_ = self.rpy_to_quaternion( [temp_traj[i].roll, temp_traj[i].pitch, temp_traj[i].yaw]) self.trajectory_path[i] = np.array([ temp_traj[i].x, temp_traj[i].y, temp_traj[i].z, quaternion_[0], quaternion_[1], quaternion_[2], quaternion_[3], temp_traj[i].vx, temp_traj[i].vy, temp_traj[i].vz, ]) def quadrotor_optimizer_setup(self, ): Q_m_ = np.diag([ 10, 10, 10, 0.3, 0.3, 0.3, 0.3, 0.05, 0.05, 0.05, ]) # position, q, v P_m_ = np.diag([10, 10, 10, 0.05, 0.05, 0.05]) # only p and v R_m_ = np.diag([5.0, 5.0, 5.0, 0.6]) nx = self.model.x.size()[0] self.nx = nx nu = self.model.u.size()[0] self.nu = nu ny = nx + nu n_params = self.model.p.size()[0] if isinstance(self.model.p, ca.SX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, acados_source_path) # create OCP ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = self.model ocp.dims.N = self.N ocp.solver_options.tf = self.T # initialize parameters ocp.dims.np = n_params ocp.parameter_values = np.zeros(n_params) # cost type ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = scipy.linalg.block_diag(Q_m_, R_m_) ocp.cost.W_e = P_m_ ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vu = np.zeros((ny, nu)) ocp.cost.Vu[-nu:, -nu:] = np.eye(nu) ocp.cost.Vx_e = np.zeros((nx - 4, nx)) # ocp.cost.Vx_e[:6, :6] = np.eye(6) ocp.cost.Vx_e[:3, :3] = np.eye(3) ocp.cost.Vx_e[-3:, -3:] = np.eye(3) # initial reference trajectory_ref x_ref = np.zeros(nx) x_ref[3] = 1.0 x_ref_e = np.zeros(nx - 4) u_ref = np.zeros(nu) u_ref[-1] = self.g_ ocp.cost.yref = np.concatenate((x_ref, u_ref)) ocp.cost.yref_e = x_ref_e # Set constraints ocp.constraints.lbu = np.array([ self.constraints.roll_rate_min, self.constraints.pitch_rate_min, self.constraints.yaw_rate_min, self.constraints.thrust_min ]) ocp.constraints.ubu = np.array([ self.constraints.roll_rate_max, self.constraints.pitch_rate_max, self.constraints.yaw_rate_max, self.constraints.thrust_max ]) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # initial state ocp.constraints.x0 = x_ref # solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # explicit Runge-Kutta integrator ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # 'SQP_RTI' ocp.solver_options.nlp_solver_max_iter = 400 # compile acados ocp ## files are stored in .ros/ json_file = os.path.join('./' + self.model.name + '_acados_ocp.json') self.solver = AcadosOcpSolver(ocp, json_file=json_file) if self.simulation_required: self.integrator = AcadosSimSolver(ocp, json_file=json_file) def mpc_estimation_loop(self, ): t1 = time.time() if self.trajectory_path is not None and self.current_state is not None: current_trajectory = self.trajectory_path u_des = np.array([0.0, 0.0, 0.0, self.g_]) self.solver.set( self.N, 'yref', np.concatenate( (current_trajectory[-1, :3], current_trajectory[-1, -3:]))) # self.solver.set(self.N, 'yref', current_trajectory[-1, :6]) for i in range(self.N): self.solver.set(i, 'yref', np.concatenate((current_trajectory[i], u_des))) self.solver.set(0, 'lbx', self.current_state.flatten()) self.solver.set(0, 'ubx', self.current_state.flatten()) # print(self.current_state) status = self.solver.solve() if status != 0: rospy.logerr("MPC cannot find a proper solution.") self.att_command.thrust = 0.5 self.att_command.body_rate.z = 0.0 self.att_command.body_rate.x = 0.0 self.att_command.body_rate.y = 0.0 # only for debug # print(self.trajectory_path) # print("----") # print(self.current_state) else: mpc_u_ = self.solver.get(0, 'u') # print(mpc_u_) # for i in range(self.N): # print(self.solver.get(i, 'x')) self.att_command.body_rate.x = mpc_u_[0] self.att_command.body_rate.y = mpc_u_[1] self.att_command.body_rate.z = mpc_u_[2] self.att_command.thrust = mpc_u_[3] / self.g_ - 0.5 # self.att_setpoint_pub.publish(self.att_command) else: if self.trajectory_path is None: rospy.loginfo("waiting trajectory") elif self.current_state is None: rospy.loginfo("waiting current state") else: rospy.loginfo("Unknown error") self.rate.sleep() # print(time.time()-t1) return True @staticmethod def quaternion_to_rpy(quaternion): q0, q1, q2, q3 = quaternion.w, quaternion.x, quaternion.y, quaternion.z roll_ = np.arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) pitch_ = np.arcsin(2 * (q0 * q2 - q3 * q1)) yaw_ = np.arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) return roll_, pitch_, yaw_ @staticmethod def rpy_to_quaternion(rqy): roll_, pitch_, yaw_ = rqy cy = np.cos(yaw_ * 0.5) sy = np.sin(yaw_ * 0.5) cp = np.cos(pitch_ * 0.5) sp = np.sin(pitch_ * 0.5) cr = np.cos(roll_ * 0.5) sr = np.sin(roll_ * 0.5) w_ = cr * cp * cy + sr * sp * sy x_ = sr * cp * cy - cr * sp * sy y_ = cr * sp * cy + sr * cp * sy z_ = cr * cp * sy - sr * sp * cy return np.array([w_, x_, y_, z_]) def state_server(self, req): return SetBoolResponse(True, 'MPC is ready') def send_command(self, ): rate = rospy.Rate(100) # Hz self.att_command.header = Header() while not rospy.is_shutdown(): # t2 = time.time() command_ = self.att_command # self.att_command.header.stamp = rospy.Time.now() self.att_setpoint_pub.publish(command_) try: # prevent garbage in console output when thread is killed rate.sleep() except rospy.ROSInterruptException: pass
def main(cost_type='NONLINEAR_LS', hessian_approximation='EXACT', ext_cost_use_num_hess=0, integrator_type='ERK'): print(f"using: cost_type {cost_type}, integrator_type {integrator_type}") # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model Tf = 1.0 nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = 20 ocp.dims.N = N # set cost Q = 2*np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2*np.diag([1e-2]) x = ocp.model.x u = ocp.model.u cost_W = scipy.linalg.block_diag(Q, R) if cost_type == 'LS': ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx,:nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[4,0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) elif cost_type == 'NONLINEAR_LS': ocp.cost.cost_type = 'NONLINEAR_LS' ocp.cost.cost_type_e = 'NONLINEAR_LS' ocp.model.cost_y_expr = vertcat(x, u) ocp.model.cost_y_expr_e = x elif cost_type == 'EXTERNAL': ocp.cost.cost_type = 'EXTERNAL' ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost = vertcat(x, u).T @ cost_W @ vertcat(x, u) ocp.model.cost_expr_ext_cost_e = x.T @ Q @ x else: raise Exception('Unknown cost_type. Possible values are \'LS\' and \'NONLINEAR_LS\'.') if cost_type in ['LS', 'NONLINEAR_LS']: ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) ocp.cost.W_e = Q ocp.cost.W = cost_W # set constraints Fmax = 80 ocp.constraints.constr_type = 'BGH' ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.x0 = x0 ocp.constraints.idxbu = np.array([0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = hessian_approximation ocp.solver_options.regularize_method = 'CONVEXIFY' ocp.solver_options.integrator_type = integrator_type if ocp.solver_options.integrator_type == 'GNSF': import json with open('../getting_started/common/' + model.name + '_gnsf_functions.json', 'r') as f: gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict ocp.solver_options.qp_solver_cond_N = 5 # set prediction horizon ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp.solver_options.ext_cost_num_hess = ext_cost_use_num_hess ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') # set NaNs as input to test reset() -> NOT RECOMMENDED!!! # ocp_solver.options_set('print_level', 2) for i in range(N): ocp_solver.set(i, 'x', np.NaN * np.ones((nx,))) ocp_solver.set(i, 'u', np.NaN * np.ones((nu,))) status = ocp_solver.solve() ocp_solver.print_statistics() # encapsulates: stat = ocp_solver.get_stats("statistics") if status == 0: raise Exception(f'acados returned status {status}, although NaNs were given.') else: print(f'acados returned status {status}, which is expected, since NaNs were given.') # RESET ocp_solver.reset() for i in range(N): ocp_solver.set(i, 'x', x0) if cost_type == 'EXTERNAL': # NOTE: hessian is wrt [u,x] if ext_cost_use_num_hess: for i in range(N): ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ])) simX = np.ndarray((N+1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() ocp_solver.print_statistics() if status != 0: raise Exception(f'acados returned status {status} for cost_type {cost_type}\n' f'integrator_type = {integrator_type}.') # 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")
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
def run_closed_loop_experiment(FORMULATION): # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_pendulum_ode_model() ocp.model = model Tf = 1.0 nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx N = 20 # set dimensions # NOTE: all dimensions but N ar detected ocp.dims.N = N # set cost module ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' Q = 2 * np.diag([1e3, 1e3, 1e-2, 1e-2]) R = 2 * np.diag([1e-2]) ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[4, 0] = 1.0 ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.W_e = Q ocp.cost.yref = np.zeros((ny, )) ocp.cost.yref_e = np.zeros((ny_e, )) ocp.cost.zl = 2000 * np.ones((1, )) ocp.cost.Zl = 1 * np.ones((1, )) ocp.cost.zu = 2000 * np.ones((1, )) ocp.cost.Zu = 1 * np.ones((1, )) # set constraints Fmax = 80 vmax = 5 x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.constraints.x0 = x0 # bound on u ocp.constraints.lbu = np.array([-Fmax]) ocp.constraints.ubu = np.array([+Fmax]) ocp.constraints.idxbu = np.array([0]) if FORMULATION == 0: # soft bound on x ocp.constraints.lbx = np.array([-vmax]) ocp.constraints.ubx = np.array([+vmax]) ocp.constraints.idxbx = np.array([2]) # v is x[2] # indices of slacked constraints within bx ocp.constraints.idxsbx = np.array([0]) elif FORMULATION == 1: # soft bound on x, using constraint h v1 = ocp.model.x[2] ocp.model.con_h_expr = v1 ocp.constraints.lh = np.array([-vmax]) ocp.constraints.uh = np.array([+vmax]) # indices of slacked constraints within h ocp.constraints.idxsh = np.array([0]) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.tf = Tf ocp.solver_options.nlp_solver_type = 'SQP' ocp.solver_options.tol = 1e-1 * tol json_filename = 'pendulum_soft_constraints.json' acados_ocp_solver = AcadosOcpSolver(ocp, json_file=json_filename) acados_integrator = AcadosSimSolver(ocp, json_file=json_filename) # closed loop Nsim = 20 simX = np.ndarray((Nsim + 1, nx)) simU = np.ndarray((Nsim, nu)) xcurrent = x0 for i in range(Nsim): simX[i, :] = xcurrent # solve ocp acados_ocp_solver.set(0, "lbx", xcurrent) acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() if status != 0: raise Exception( 'acados acados_ocp_solver returned status {}. Exiting.'.format( status)) simU[i, :] = acados_ocp_solver.get(0, "u") # simulate system acados_integrator.set("x", xcurrent) acados_integrator.set("u", simU[i, :]) status = acados_integrator.solve() if status != 0: raise Exception( 'acados integrator returned status {}. Exiting.'.format( status)) # update state xcurrent = acados_integrator.get("x") simX[Nsim, :] = xcurrent # get slack values at stage 1 sl = acados_ocp_solver.get(1, "sl") su = acados_ocp_solver.get(1, "su") print("sl", sl, "su", su) # plot results # plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False) # store results np.savetxt('test_results/simX_soft_formulation_' + str(FORMULATION), simX) np.savetxt('test_results/simU_soft_formulation_' + str(FORMULATION), simU) print("soft constraint example: ran formulation", FORMULATION, "successfully.")
def solve_marathos_ocp(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] qp_solver = setting['qp_solver'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = export_linear_mass_model() ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nu # discretization Tf = 2 N = 20 shooting_nodes = np.linspace(0, Tf, N + 1) ocp.dims.N = N # set cost Q = 2 * np.diag([]) R = 2 * np.diag([1e1, 1e1]) ocp.cost.W_e = Q ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.Vx = np.zeros((ny, nx)) Vu = np.eye((nu)) ocp.cost.Vu = Vu ocp.cost.yref = np.zeros((ny, )) # set constraints Fmax = 5 ocp.constraints.lbu = -Fmax * np.ones((nu, )) ocp.constraints.ubu = +Fmax * np.ones((nu, )) ocp.constraints.idxbu = np.array(range(nu)) x0 = np.array([1e-1, 1.1, 0, 0]) ocp.constraints.x0 = x0 # terminal constraint x_goal = np.array([0, -1.1, 0, 0]) ocp.constraints.idxbx_e = np.array(range(nx)) ocp.constraints.lbx_e = x_goal ocp.constraints.ubx_e = x_goal if SOFTEN_TERMINAL: ocp.constraints.idxsbx_e = np.array(range(nx)) ocp.cost.zl_e = 1e4 * np.ones(nx) ocp.cost.zu_e = 1e4 * np.ones(nx) ocp.cost.Zl_e = 1e6 * np.ones(nx) ocp.cost.Zu_e = 1e6 * np.ones(nx) # add obstacle if OBSTACLE: obs_rad = 1.0 obs_x = 0.0 obs_y = 0.0 circle = (obs_x, obs_y, obs_rad) ocp.constraints.uh = np.array([100.0]) # doenst matter ocp.constraints.lh = np.array([obs_rad**2]) x_square = model.x[0]**OBSTACLE_POWER + model.x[1]**OBSTACLE_POWER ocp.model.con_h_expr = x_square # copy for terminal ocp.constraints.uh_e = ocp.constraints.uh ocp.constraints.lh_e = ocp.constraints.lh ocp.model.con_h_expr_e = ocp.model.con_h_expr else: circle = None # soften if OBSTACLE and SOFTEN_OBSTACLE: ocp.constraints.idxsh = np.array([0]) ocp.constraints.idxsh_e = np.array([0]) Zh = 1e6 * np.ones(1) zh = 1e4 * np.ones(1) ocp.cost.zl = zh ocp.cost.zu = zh ocp.cost.Zl = Zh ocp.cost.Zu = Zh ocp.cost.zl_e = np.concatenate((ocp.cost.zl_e, zh)) ocp.cost.zu_e = np.concatenate((ocp.cost.zu_e, zh)) ocp.cost.Zl_e = np.concatenate((ocp.cost.Zl_e, Zh)) ocp.cost.Zu_e = np.concatenate((ocp.cost.Zu_e, Zh)) # set options ocp.solver_options.qp_solver = qp_solver # FULL_CONDENSING_QPOASES # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' # ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = globalization ocp.solver_options.alpha_min = 0.01 # ocp.solver_options.__initialize_t_slacks = 0 # ocp.solver_options.levenberg_marquardt = 1e-2 ocp.solver_options.qp_solver_cond_N = 0 ocp.solver_options.print_level = 1 ocp.solver_options.nlp_solver_max_iter = 200 ocp.solver_options.qp_solver_iter_max = 400 # NOTE: this is needed for PARTIAL_CONDENSING_HPIPM to get expected behavior qp_tol = 5e-7 ocp.solver_options.qp_solver_tol_stat = qp_tol ocp.solver_options.qp_solver_tol_eq = qp_tol ocp.solver_options.qp_solver_tol_ineq = qp_tol ocp.solver_options.qp_solver_tol_comp = qp_tol ocp.solver_options.qp_solver_ric_alg = 1 # ocp.solver_options.qp_solver_cond_ric_alg = 1 # set prediction horizon ocp.solver_options.tf = Tf ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}_ocp.json') ocp_solver.options_set('line_search_use_sufficient_descent', line_search_use_sufficient_descent) ocp_solver.options_set('globalization_use_SOC', globalization_use_SOC) ocp_solver.options_set('full_step_dual', 1) if INITIALIZE: # initialize solver # [ocp_solver.set(i, "x", x0 + (i/N) * (x_goal-x0)) for i in range(N+1)] [ocp_solver.set(i, "x", x0) for i in range(N + 1)] # [ocp_solver.set(i, "u", 2*(np.random.rand(2) - 0.5)) for i in range(N)] # solve status = ocp_solver.solve() ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") sqp_iter = ocp_solver.get_stats('sqp_iter')[0] print(f'acados returned status {status}.') # ocp_solver.store_iterate(f'it{ocp.solver_options.nlp_solver_max_iter}_{model.name}.json') # get solution simX = np.array([ocp_solver.get(i, "x") for i in range(N + 1)]) simU = np.array([ocp_solver.get(i, "u") for i in range(N)]) pi_multiplier = [ocp_solver.get(i, "pi") for i in range(N)] print(f"cost function value = {ocp_solver.get_cost()}") # print summary print(f"solved Marathos test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {sqp_iter} SQP iterations" ) # print(f"alphas: {alphas[:iter]}") # print(f"total number of QP iterations: {sum(qp_iters[:iter])}") # max_infeasibility = np.max(residuals[1:3]) # print(f"max infeasibility: {max_infeasibility}") # checks if status != 0: raise Exception(f"acados solver returned status {status} != 0.") if globalization == "FIXED_STEP": if sqp_iter != 18: raise Exception( f"acados solver took {sqp_iter} iterations, expected 18.") elif globalization == "MERIT_BACKTRACKING": if globalization_use_SOC == 1 and line_search_use_sufficient_descent == 0 and sqp_iter not in range( 21, 23): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(21, 23)." ) elif globalization_use_SOC == 1 and line_search_use_sufficient_descent == 1 and sqp_iter not in range( 21, 24): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(21, 24)." ) elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 0 and sqp_iter not in range( 155, 165): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(155, 165)." ) elif globalization_use_SOC == 0 and line_search_use_sufficient_descent == 1 and sqp_iter not in range( 160, 175): raise Exception( f"acados solver took {sqp_iter} iterations, expected range(160, 175)." ) if PLOT: plot_linear_mass_system_X_state_space(simX, circle=circle, x_goal=x_goal) plot_linear_mass_system_U(shooting_nodes, simU) # plot_linear_mass_system_X(shooting_nodes, simX) # import pdb; pdb.set_trace() print(f"\n\n----------------------\n")
# simulate for i in tqdm(range(Nsim)): #print(x0) x_noise = x0 + np.diag([1, 0.1, 0.2, 1, 0.1, 0.2 ]) @ np.random.normal( 0, noise_std, x0.shape) #print(x_noise) acados_solver.set(0, "lbx", x_noise) acados_solver.set(0, "ubx", x_noise) #acados_solver.set(0, "x", x_noise) for j in range(N): acados_solver.set(j, "yref", yref) acados_solver.set(N, 'yref', yref_e) status = acados_solver.solve() if status != 0: raise Exception( "acados returned status {} in closed loop iteration {}." .format(status, i)) # get solution u0 = acados_solver.get(0, "u") #print(u0) for j in range(2): simU[i, j] = u0[j] for j in range(nx): simX[i, j] = x0[j] # update initial condition
def solve_marathos_problem_with_setting(setting): globalization = setting['globalization'] line_search_use_sufficient_descent = setting[ 'line_search_use_sufficient_descent'] globalization_use_SOC = setting['globalization_use_SOC'] # create ocp object to formulate the OCP ocp = AcadosOcp() # set model model = AcadosModel() x1 = SX.sym('x1') x2 = SX.sym('x2') x = vertcat(x1, x2) # dynamics: identity model.disc_dyn_expr = x model.x = x model.u = SX.sym('u', 0, 0) # [] / None doesnt work model.p = [] model.name = f'marathos_problem' ocp.model = model # discretization Tf = 1 N = 1 ocp.dims.N = N ocp.solver_options.tf = Tf # cost ocp.cost.cost_type_e = 'EXTERNAL' ocp.model.cost_expr_ext_cost_e = x1 # constarints ocp.model.con_h_expr = x1**2 + x2**2 ocp.constraints.lh = np.array([1.0]) ocp.constraints.uh = np.array([1.0]) # # soften # ocp.constraints.idxsh = np.array([0]) # ocp.cost.zl = 1e5 * np.array([1]) # ocp.cost.zu = 1e5 * np.array([1]) # ocp.cost.Zl = 1e5 * np.array([1]) # ocp.cost.Zu = 1e5 * np.array([1]) # add bounds on x # nx = 2 # ocp.constraints.idxbx_0 = np.array(range(nx)) # ocp.constraints.lbx_0 = -2 * np.ones((nx)) # ocp.constraints.ubx_0 = 2 * np.ones((nx)) # set options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES # PARTIAL_CONDENSING_HPIPM, FULL_CONDENSING_QPOASES, FULL_CONDENSING_HPIPM, # PARTIAL_CONDENSING_QPDUNES, PARTIAL_CONDENSING_OSQP ocp.solver_options.hessian_approx = 'EXACT' ocp.solver_options.integrator_type = 'DISCRETE' # ocp.solver_options.print_level = 1 ocp.solver_options.tol = TOL ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP ocp.solver_options.globalization = globalization ocp.solver_options.alpha_min = 1e-2 # ocp.solver_options.__initialize_t_slacks = 0 # ocp.solver_options.regularize_method = 'CONVEXIFY' ocp.solver_options.levenberg_marquardt = 1e-1 # ocp.solver_options.print_level = 2 SQP_max_iter = 300 ocp.solver_options.qp_solver_iter_max = 400 ocp.solver_options.regularize_method = 'MIRROR' # ocp.solver_options.exact_hess_constr = 0 ocp.solver_options.line_search_use_sufficient_descent = line_search_use_sufficient_descent ocp.solver_options.globalization_use_SOC = globalization_use_SOC ocp.solver_options.eps_sufficient_descent = 1e-1 ocp.solver_options.qp_tol = 5e-7 if FOR_LOOPING: # call solver in for loop to get all iterates ocp.solver_options.nlp_solver_max_iter = 1 ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') else: ocp.solver_options.nlp_solver_max_iter = SQP_max_iter ocp_solver = AcadosOcpSolver(ocp, json_file=f'{model.name}.json') # initialize solver rad_init = 0.1 #0.1 #np.pi / 4 xinit = np.array([np.cos(rad_init), np.sin(rad_init)]) # xinit = np.array([0.82120912, 0.58406911]) [ocp_solver.set(i, "x", xinit) for i in range(N + 1)] # solve if FOR_LOOPING: # call solver in for loop to get all iterates iterates = np.zeros((SQP_max_iter + 1, 2)) iterates[0, :] = xinit alphas = np.zeros((SQP_max_iter, )) qp_iters = np.zeros((SQP_max_iter, )) iter = SQP_max_iter residuals = np.zeros((4, SQP_max_iter)) # solve for i in range(SQP_max_iter): status = ocp_solver.solve() ocp_solver.print_statistics( ) # encapsulates: stat = ocp_solver.get_stats("statistics") # print(f'acados returned status {status}.') iterates[i + 1, :] = ocp_solver.get(0, "x") if status in [0, 4]: iter = i break alphas[i] = ocp_solver.get_stats('alpha')[1] qp_iters[i] = ocp_solver.get_stats('qp_iter')[1] residuals[:, i] = ocp_solver.get_stats('residuals') else: ocp_solver.solve() ocp_solver.print_statistics() iter = ocp_solver.get_stats('sqp_iter')[0] alphas = ocp_solver.get_stats('alpha')[1:] qp_iters = ocp_solver.get_stats('qp_iter') residuals = ocp_solver.get_stats('statistics')[1:5, 1:iter] # get solution solution = ocp_solver.get(0, "x") # print summary print(f"solved Marathos test problem with settings {setting}") print( f"cost function value = {ocp_solver.get_cost()} after {iter} SQP iterations" ) print(f"alphas: {alphas[:iter]}") print(f"total number of QP iterations: {sum(qp_iters[:iter])}") max_infeasibility = np.max(residuals[1:3]) print(f"max infeasibility: {max_infeasibility}") # compare to analytical solution exact_solution = np.array([-1, 0]) sol_err = max(np.abs(solution - exact_solution)) # checks if sol_err > TOL * 1e1: raise Exception( f"error of numerical solution wrt exact solution = {sol_err} > tol = {TOL*1e1}" ) else: print(f"matched analytical solution with tolerance {TOL}") try: if globalization == 'FIXED_STEP': # import pdb; pdb.set_trace() if max_infeasibility < 5.0: raise Exception( f"Expected max_infeasibility > 5.0 when using full step SQP on Marathos problem" ) if iter != 10: raise Exception( f"Expected 10 SQP iterations when using full step SQP on Marathos problem, got {iter}" ) if any(alphas[:iter] != 1.0): raise Exception( f"Expected all alphas = 1.0 when using full step SQP on Marathos problem" ) elif globalization == 'MERIT_BACKTRACKING': if max_infeasibility > 0.5: raise Exception( f"Expected max_infeasibility < 0.5 when using globalized SQP on Marathos problem" ) if globalization_use_SOC == 0: if FOR_LOOPING and iter != 57: raise Exception( f"Expected 57 SQP iterations when using globalized SQP without SOC on Marathos problem, got {iter}" ) elif line_search_use_sufficient_descent == 1: if iter not in range(29, 37): # NOTE: got 29 locally and 36 on Github actions. # On Github actions the inequality constraint was numerically violated in the beginning. # This leads to very different behavior, since the merit gradient is so different. # Github actions: merit_grad = -1.669330e+00, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = -1.495535e+00 # Jonathan Laptop: merit_grad = -1.737950e-01, merit_grad_cost = -1.737950e-01, merit_grad_dyn = 0.000000e+00, merit_grad_ineq = 0.000000e+00 raise Exception( f"Expected SQP iterations in range(29, 37) when using globalized SQP with SOC on Marathos problem, got {iter}" ) else: if iter != 12: raise Exception( f"Expected 12 SQP iterations when using globalized SQP with SOC on Marathos problem, got {iter}" ) except Exception as inst: if FOR_LOOPING and globalization == "MERIT_BACKTRACKING": print( "\nAcados globalized OCP solver behaves different when for looping due to different merit function weights.", "Following exception is not raised\n") print(inst, "\n") else: raise (inst) if PLOT: plt.figure() axs = plt.plot(solution[0], solution[1], 'x', label='solution') if FOR_LOOPING: # call solver in for loop to get all iterates cm = plt.cm.get_cmap('RdYlBu') axs = plt.scatter(iterates[:iter + 1, 0], iterates[:iter + 1, 1], c=range(iter + 1), s=35, cmap=cm, label='iterates') plt.colorbar(axs) ts = np.linspace(0, 2 * np.pi, 100) plt.plot(1 * np.cos(ts) + 0, 1 * np.sin(ts) - 0, 'r') plt.axis('square') plt.legend() plt.title( f"Marathos problem with N = {N}, x formulation, SOC {globalization_use_SOC}" ) plt.show() print(f"\n\n----------------------\n")
# [00, @2, 00, 00, 00], # [00, 00, @2, 00, 00], # [00, 00, 00, @1, 00], # [00, 00, 00, 00, @1]]) # NOTE: hessian is wrt [u,x] if EXTERNAL_COST_USE_NUM_HESS: for i in range(N): ocp_solver.cost_set(i, "ext_cost_num_hess", np.diag([0.04, 4000, 4000, 0.04, 0.04, ])) ocp_solver.cost_set(N, "ext_cost_num_hess", np.diag([4000, 4000, 0.04, 0.04, ])) simX = np.ndarray((N+1, nx)) simU = np.ndarray((N, nu)) status = ocp_solver.solve() ocp_solver.print_statistics() if status != 0: raise Exception('acados returned status {}. Exiting.'.format(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") plot_pendulum(np.linspace(0, Tf, N+1), Fmax, simU, simX, latexify=False)
def run_nominal_control(chain_params): # create ocp object to formulate the OCP ocp = AcadosOcp() # chain parameters n_mass = chain_params["n_mass"] M = chain_params["n_mass"] - 2 # number of intermediate masses Ts = chain_params["Ts"] Tsim = chain_params["Tsim"] N = chain_params["N"] u_init = chain_params["u_init"] with_wall = chain_params["with_wall"] yPosWall = chain_params["yPosWall"] m = chain_params["m"] D = chain_params["D"] L = chain_params["L"] perturb_scale = chain_params["perturb_scale"] nlp_iter = chain_params["nlp_iter"] nlp_tol = chain_params["nlp_tol"] save_results = chain_params["save_results"] show_plots = chain_params["show_plots"] seed = chain_params["seed"] np.random.seed(seed) nparam = 3*M W = perturb_scale * np.eye(nparam) # export model model = export_disturbed_chain_mass_model(n_mass, m, D, L) # set model ocp.model = model nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx Tf = N * Ts # initial state xPosFirstMass = np.zeros((3,1)) xEndRef = np.zeros((3,1)) xEndRef[0] = L * (M+1) * 6 pos0_x = np.linspace(xPosFirstMass[0], xEndRef[0], n_mass) xrest = compute_steady_state(n_mass, m, D, L, xPosFirstMass, xEndRef) x0 = xrest # set dimensions ocp.dims.N = N # set cost module ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' Q = 2*np.diagflat( np.ones((nx, 1)) ) q_diag = np.ones((nx,1)) strong_penalty = M+1 q_diag[3*M] = strong_penalty q_diag[3*M+1] = strong_penalty q_diag[3*M+2] = strong_penalty Q = 2*np.diagflat( q_diag ) R = 2*np.diagflat( 1e-2 * np.ones((nu, 1)) ) ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Q ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx,:nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[nx:nx+nu, :] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) # import pdb; pdb.set_trace() yref = np.vstack((xrest, np.zeros((nu,1)))).flatten() ocp.cost.yref = yref ocp.cost.yref_e = xrest.flatten() # set constraints umax = 1*np.ones((nu,)) ocp.constraints.constr_type = 'BGH' ocp.constraints.lbu = -umax ocp.constraints.ubu = umax ocp.constraints.x0 = x0.reshape((nx,)) ocp.constraints.idxbu = np.array(range(nu)) # disturbances nparam = 3*M ocp.parameter_values = np.zeros((nparam,)) # wall constraint if with_wall: nbx = M + 1 Jbx = np.zeros((nbx,nx)) for i in range(nbx): Jbx[i, 3*i+1] = 1.0 ocp.constraints.Jbx = Jbx ocp.constraints.lbx = yPosWall * np.ones((nbx,)) ocp.constraints.ubx = 1e9 * np.ones((nbx,)) # slacks ocp.constraints.Jsbx = np.eye(nbx) L2_pen = 1e3 L1_pen = 1 ocp.cost.Zl = L2_pen * np.ones((nbx,)) ocp.cost.Zu = L2_pen * np.ones((nbx,)) ocp.cost.zl = L1_pen * np.ones((nbx,)) ocp.cost.zu = L1_pen * np.ones((nbx,)) # solver options ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'IRK' ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI ocp.solver_options.nlp_solver_max_iter = nlp_iter ocp.solver_options.sim_method_num_stages = 2 ocp.solver_options.sim_method_num_steps = 2 ocp.solver_options.qp_solver_cond_N = N ocp.solver_options.qp_tol = nlp_tol ocp.solver_options.tol = nlp_tol # ocp.solver_options.nlp_solver_tol_eq = 1e-9 # set prediction horizon ocp.solver_options.tf = Tf acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') # acados_integrator = AcadosSimSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json') acados_integrator = export_chain_mass_integrator(n_mass, m, D, L) #%% get initial state from xrest xcurrent = x0.reshape((nx,)) for i in range(5): acados_integrator.set("x", xcurrent) acados_integrator.set("u", u_init) status = acados_integrator.solve() if status != 0: raise Exception('acados integrator returned status {}. Exiting.'.format(status)) # update state xcurrent = acados_integrator.get("x") #%% actual simulation N_sim = int(np.floor(Tsim/Ts)) simX = np.ndarray((N_sim+1, nx)) simU = np.ndarray((N_sim, nu)) wall_dist = np.zeros((N_sim,)) timings = np.zeros((N_sim,)) simX[0,:] = xcurrent # closed loop for i in range(N_sim): # solve ocp acados_ocp_solver.set(0, "lbx", xcurrent) acados_ocp_solver.set(0, "ubx", xcurrent) status = acados_ocp_solver.solve() timings[i] = acados_ocp_solver.get_stats("time_tot")[0] if status != 0: raise Exception('acados acados_ocp_solver returned status {} in time step {}. Exiting.'.format(status, i)) simU[i,:] = acados_ocp_solver.get(0, "u") print("control at time", i, ":", simU[i,:]) # simulate system acados_integrator.set("x", xcurrent) acados_integrator.set("u", simU[i,:]) pertubation = sampleFromEllipsoid(np.zeros((nparam,)), W) acados_integrator.set("p", pertubation) status = acados_integrator.solve() if status != 0: raise Exception('acados integrator returned status {}. Exiting.'.format(status)) # update state xcurrent = acados_integrator.get("x") simX[i+1,:] = xcurrent # xOcpPredict = acados_ocp_solver.get(1, "x") # print("model mismatch = ", str(np.max(xOcpPredict - xcurrent))) yPos = xcurrent[range(1,3*M+1,3)] wall_dist[i] = np.min(yPos - yPosWall) print("time i = ", str(i), " dist2wall ", str(wall_dist[i])) print("dist2wall (minimum over simulation) ", str(np.min(wall_dist))) #%% plot results if os.environ.get('ACADOS_ON_TRAVIS') is None and show_plots: plot_chain_control_traj(simU) plot_chain_position_traj(simX, yPosWall=yPosWall) plot_chain_velocity_traj(simX) animate_chain_position(simX, xPosFirstMass, yPosWall=yPosWall) # animate_chain_position_3D(simX, xPosFirstMass) plt.show()