def export_linear_mass_model(): model_name = 'linear_mass' # set up states & controls qx = SX.sym('qx') qy = SX.sym('qy') vx = SX.sym('vx') vy = SX.sym('vy') x = vertcat(qx, qy, vx, vy) # u = SX.sym('u', 2) ux = SX.sym('ux') uy = SX.sym('uy') u = vertcat(ux, uy) f_expl = vertcat(vx, vy, u) model = AcadosModel() # model.f_impl_expr = f_impl model.f_expl_expr = f_expl # model.disc_dyn_expr model.x = x model.u = u # model.xdot = xdot # model.z = z model.p = [] model.name = model_name return model
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 export_external_ode_model(): model_name = 'external_ode' # Declare model variables x = MX.sym('x', 2) u = MX.sym('u', 1) xDot = MX.sym('xDot', 2) cdll.LoadLibrary('./test_external_lib/build/libexternal_ode_casadi.so') f_ext = external('libexternal_ode_casadi', 'libexternal_ode_casadi.so', {'enable_fd': True}) f_expl = f_ext(x, u) f_impl = xDot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xDot model.u = u model.p = [] model.name = model_name return model
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 export_pendulum_ode_model(): model_name = 'pendulum_ode' # constants M = 1. # mass of the cart [kg] -> now estimated m = 0.1 # mass of the ball [kg] g = 9.81 # length of the rod [m] l = 0.8 # gravity constant [m/s^2] # set up states & controls x1 = SX.sym('x1') theta = SX.sym('theta') v1 = SX.sym('v1') dtheta = SX.sym('dtheta') x = vertcat(x1, theta, v1, dtheta) # controls F = SX.sym('F') u = vertcat(F) # xdot x1_dot = SX.sym('x1_dot') theta_dot = SX.sym('theta_dot') v1_dot = SX.sym('v1_dot') dtheta_dot = SX.sym('dtheta_dot') xdot = vertcat(x1_dot, theta_dot, v1_dot, dtheta_dot) # algebraic variables # z = None # parameters p = [] # dynamics denominator = M + m - m * cos(theta) * cos(theta) f_expl = vertcat( v1, dtheta, (-m * l * sin(theta) * dtheta * dtheta + m * g * cos(theta) * sin(theta) + F) / denominator, (-m * l * cos(theta) * sin(theta) * dtheta * dtheta + F * cos(theta) + (M + m) * g * sin(theta)) / (l * denominator)) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def export_chain_mass_model(n_mass, m, D, L): model_name = 'chain_mass_' + str(n_mass) x0 = np.array([0, 0, 0]) # fix mass (at wall) M = n_mass - 2 # number of intermediate masses nx = (2 * M + 1) * 3 # differential states nu = 3 # control inputs xpos = SX.sym('xpos', (M + 1) * 3, 1) # position of fix mass eliminated xvel = SX.sym('xvel', M * 3, 1) u = SX.sym('u', nu, 1) xdot = SX.sym('xdot', nx, 1) f = SX.zeros(3 * M, 1) # force on intermediate masses for i in range(M): f[3 * i + 2] = -9.81 for i in range(M + 1): if i == 0: dist = xpos[i * 3:(i + 1) * 3] - x0 else: dist = xpos[i * 3:(i + 1) * 3] - xpos[(i - 1) * 3:i * 3] scale = D / m * (1 - L / norm_2(dist)) F = scale * dist # mass on the right if i < M: f[i * 3:(i + 1) * 3] -= F # mass on the left if i > 0: f[(i - 1) * 3:i * 3] += F # parameters p = [] x = vertcat(xpos, xvel) # dynamics f_expl = vertcat(xvel, u, f) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u model.p = p model.name = model_name return model
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.params = 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].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)
def export_sine_wave_mhe_ode_model(): model_name = 'sine_wave_mhe_ode' # set up states & controls s = SX.sym('s') amp = SX.sym('amp') freq = SX.sym('freq') phase = SX.sym('phase') trend = SX.sym('trend') x = vertcat(s, amp, freq, phase, trend) # (controls) state noise w_s = SX.sym('w_s') w = vertcat(w_s) # xdot s_dot = SX.sym('s_dot') amp_dot = SX.sym('amp_dot') freq_dot = SX.sym('freq_dot') phase_dot = SX.sym('phase_dot') trend_dot = SX.sym('trend_dot') xdot = vertcat(s_dot, amp_dot, freq_dot, phase_dot, trend_dot) # algebraic variables # z = None # parameters # set up parameters t = SX.sym('t') # time p = vertcat(t) # dynamics f_expl = vertcat( trend + amp * cos(2 * pi * freq * t + phase) * 2 * pi * freq, 0, 0, 0, 0) f_expl[0] = f_expl[0] + w f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = w # model.z = [] model.p = p model.name = model_name return model
def fill_in_acados_model(x, u, p, dynamics, name): x_dot = cs.MX.sym('x_dot', dynamics.shape) f_impl = x_dot - dynamics # Dynamics model model = AcadosModel() model.f_expl_expr = dynamics model.f_impl_expr = f_impl model.x = x model.xdot = x_dot model.u = u model.p = p model.name = name return model
def export_so_ode_ct(): model_name = 'second_order_ode_ct' # set up states & controls z1 = SX.sym('z1') z2 = SX.sym('z2') x = vertcat(z1, z2) # control u = SX.sym('u') # xdot z1_dot = SX.sym('z1_dot') z2_dot = SX.sym('z2_dot') xdot = vertcat(z1_dot, z2_dot) # algebraic variables # z = None # parameters # set up parameters zeta = SX.sym('zeta') # zeta ts = SX.sym('ts') # ts Kp = SX.sym('Kp') # Kp p = vertcat(zeta, ts, Kp) # dynamics f_expl = vertcat( z2, -1 / (ts * ts) * z1 - 2 * zeta / ts * z2 + Kp / (ts * ts) * u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name return model
def acados_linear_quad_model(num_states, g=9.81): model_name = 'linear_quad' A, B = linear_quad_model(num_states) tc = 0.59375 # mapping control to throttle # tc = 0.75 scaling_control_mat = np.array([1, 1, 1, (g / tc)]) f_const = 20 # scaling_control_mat = np.array([1, 1, 1, ((g + f_const)/tc)]) # add_mat = np.array([0,0,0,-f_const]) # scaling_control_mat = np.array([1,1,1,1]) # set up states & controls x = SX.sym('x', 9) u = SX.sym('u', 4) # xdot xdot = SX.sym('xdot', 9) #parameters p = [] f_expl = mtimes(A, x) + mtimes(B, u * scaling_control_mat) # f_expl = mtimes(A,x) + mtimes(B,u * scaling_control_mat + add_mat) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def acados_linear_quad_model(num_states, g=9.81): ''' A simple linear model linearized about x = 0, u = [0,0,0,g] but with a format compatible with acados. ''' model_name = 'linear_quad' A, B = linear_quad_model(num_states) tc = 0.59375 # mapping control to throttle # tc = 0.75 scaling_control_mat = np.array([1, 1, 1, (g / tc)]) # scaling_control_mat = np.array([1,1,1,1]) # set up states & controls x = SX.sym('x', 9) u = SX.sym('u', 4) # xdot xdot = SX.sym('xdot', 9) #parameters p = [] f_expl = mtimes(A, x) + mtimes(B, u * scaling_control_mat) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def acados_nonlinear_quad_model(num_states, g=9.81): ''' A non-linear model of the quadrotor. We assume the input u = [phi_dot,theta_dot,psi_dot,throttle]. Therefore, the system has only 9 states. ''' model_name = 'nonlinear_quad' position = SX.sym('o', 3) velocity = SX.sym('v', 3) phi = SX.sym('phi') theta = SX.sym('theta') psi = SX.sym('psi') gravity = SX.zeros(3) gravity[2] = g x = vertcat(position, phi, theta, psi, velocity) p = SX.sym('p') q = SX.sym('q') r = SX.sym('r') F = SX.sym('F') u = vertcat(p, q, r, F) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(phi) r1[1, 2] = -sin(phi) r1[2, 1] = sin(phi) r1[2, 2] = cos(phi) r2 = SX(3, 3) r2[0, 0] = cos(theta) r2[0, 2] = sin(theta) r2[1, 1] = 1 r2[2, 0] = -sin(theta) r2[2, 2] = cos(theta) r3 = SX(3, 3) r3[0, 0] = cos(psi) r3[0, 1] = -sin(psi) r3[1, 0] = sin(psi) r3[1, 1] = cos(psi) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] # tau[:,0] = (inv(r1))[:,0] # tau[:,1] = (inv(r1))[:,1] # tau[:,2] = (inv(r1 @ r2))[:,2] # tau[0,0] = cos(theta) # tau[0,2] = -sin(theta) # tau[1,1] = 1 # tau[1,2] = sin(phi) * cos(theta) # tau[2,0] = sin(theta) # tau[2,2] = cos(phi) * cos(theta) tc = 0.59375 # mapping control to throttle # tc = 0.75 # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) f_expl = vertcat(velocity, inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity) # xdot xdot = SX.sym('xdot', 9) #parameters p = [] f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def export_quad_ode_model(): model_name = 'quad_ode' # set up states & controls q0 = SX.sym('q0') q1 = SX.sym('q1') q2 = SX.sym('q2') q3 = SX.sym('q3') omegax = SX.sym('omegax') omegay = SX.sym('omegay') omegaz = SX.sym('omegaz') x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz) # controls w1 = SX.sym('w1') w2 = SX.sym('w2') w3 = SX.sym('w3') w4 = SX.sym('w4') u = vertcat(w1, w2, w3, w4) # xdot q0_dot = SX.sym('q0_dot') q1_dot = SX.sym('q1_dot') q2_dot = SX.sym('q2_dot') q3_dot = SX.sym('q3_dot') omegax_dot = SX.sym('omegax_dot') omegay_dot = SX.sym('omegay_dot') omegaz_dot = SX.sym('omegaz_dot') xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot, omegaz_dot) # algebraic variables # z = None # parameters # set up parameters rho = SX.sym('rho') # air density A = SX.sym('A') # propeller area Cl = SX.sym('Cl') # lift coefficient Cd = SX.sym('Cd') # drag coefficient m = SX.sym('m') # mass of quad g = SX.sym('g') # gravity J1 = SX.sym('J1') # mom inertia J2 = SX.sym('J2') # mom inertia J3 = SX.sym('J3') # mom inertia J = diag(vertcat(J1, J2, J3)) # This comes from ref[6] pg 449. # dq = 1/2 * G(q)' * Ω S = SX.zeros(3, 4) S[:, 0] = vertcat(-q1, -q2, -q3) S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3)) print("S:", S, S.shape) #Define angular velocity vector OMG = vertcat(omegax, omegay, omegaz) p = vertcat(rho, A, Cl, Cd, m, g, J1, J2, J3) #Define torque applied to the system T = SX(3, 1) T[0, 0] = 0.5 * A * Cl * rho * (w2 * w2 - w4 * w4) T[1, 0] = 0.5 * A * Cl * rho * (w1 * w1 - w3 * w3) T[2, 0] = 0.5 * A * Cd * rho * (w1 * w1 - w2 * w2 + w3 * w3 - w4 * w4) # Reference generation (quaternion to eul) used in cost function. Eul = SX(3, 1) Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1)) Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) # dynamics f_expl = vertcat(0.5 * mtimes(transpose(S), OMG), mtimes(inv(J), (T - cross(OMG, mtimes(J, OMG))))) f_impl = xdot - f_expl print("dynamics shape: ", f_expl.shape) model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name # add the nonlinear cost. # Cost defined in paper # --- --- # | roll(x) - roll_ref | # | pitch(x) - pitch_ref | # l(x,u,z) = | yaw(x) - yaw_ref | # | x - x_ref | # | u - u_ref | # --- --- 14x1 # --- --- # | roll(x) - roll_ref | # | pitch(x) - pitch_ref | # m(x) = | yaw(x) - yaw_ref | # | x - x_ref | # --- --- 10x1 model.cost_y_expr = vertcat( Eul, x, u) #: CasADi expression for nonlinear least squares model.cost_y_expr_e = vertcat( Eul, x) #: CasADi expression for nonlinear least squares, terminal model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 # # yReference checking. # refCheck = Function('y_ref', [vertcat(x, u)], [model.cost_y_expr]) # print(refCheck) # y_ref = refCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4])) # print(y_ref) # # dynCheck = Function('sys_dyn', [vertcat(x, u),p], [f_expl]) # print(dynCheck) # dx_ref = dynCheck(np.array([0.566, 0.571, 0.168, 0.571, 0.1, 0.2, 0.3, 1, 2, 3, 4]),np.array([1, 1, 1, 1, 1, 1, 1, 1, 1])) # print(dx_ref) return model
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")
def export_car_ode_model(mb, mw, Iw, Rw, l): model_name = 'car_phase0_ode' #system parameters mt = mb + mw g = 9.81 # [m/s^2] # set up states & controls phi = MX.sym('phi') theta = MX.sym('theta') phi_dot = MX.sym('phi_dot') theta_dot = MX.sym('theta_dot') x = vertcat(phi, theta, phi_dot, theta_dot) # controls tau = MX.sym('tau') u = vertcat(tau) # xdot phi_ddot = MX.sym('phi_ddot') theta_ddot = MX.sym('theta_ddot') xdot = vertcat(phi_dot, theta_dot, phi_ddot, theta_ddot) phidd = tau/(Iw+mt*Rw*Rw) # dynamics f_expl = vertcat(phi_dot, theta_dot, phidd, (1/mb*l*l)*(-mb*Rw*phidd*l*sin(theta) + mb*g*l*cos(theta) - tau) ) print(f_expl.shape) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # algebraic variables model.z = vertcat([]) # parameters p = vertcat([]) model.p = p model.name = model_name # activate1 = MX.sym('activate1') # activate2 = MX.sym('activate2') # activate3 = MX.sym('activate3') # activate4 = MX.sym('activate4') # activate5 = MX.sym('activate5') # activate6 = MX.sym('activate6') # activate7 = MX.sym('activate7') # model.p = vertcat(activate1, activate2, activate3, activate4, activate5, activate6, activate7) #model.con_h_expr = vertcat( (lambda_z - fabs(lambda_x)), # (x1_dot-Rw*phi_dot)) print(f"SHAPE: {model.con_h_expr}") return model
def __init__(self, ): #constants g = 9.8066 l = 0.1 # control inputs roll_ref = ca.SX.sym('roll_ref_') pitch_ref = ca.SX.sym('pitch_ref_') yaw_ref = ca.SX.sym('yaw_ref_') thrust_ref = ca.SX.sym('thrust_ref_') controls = ca.vcat([roll_ref, pitch_ref, yaw_ref, thrust_ref]) # model constants after SI roll_gain = 2.477 roll_tau = 0.477 pitch_gain = 2.477 pitch_tau = 0.477 # roll_gain = 1.477 # roll_tau = 0.477 # pitch_gain = 1.477 # pitch_tau = 0.477 # model states x = ca.SX.sym('x') y = ca.SX.sym('y') z = ca.SX.sym('z') vx = ca.SX.sym('vx') vy = ca.SX.sym('vy') vz = ca.SX.sym('vz') s = ca.SX.sym('s') # pendulum x coordinate r = ca.SX.sym('r') # pendulum y coordinate ds = ca.SX.sym('ds') dr = ca.SX.sym('dr') roll = ca.SX.sym('roll') pitch = ca.SX.sym('pitch') yaw = ca.SX.sym('yaw') states = ca.vcat([x, y, z, vx, vy, vz, s, r, ds, dr, roll, pitch, yaw]) ### DYNAMICS dragacc1, dragacc2 = 0.0, 0.0 h = ca.sqrt(l * l - s * s - r * r) ddx = thrust_ref * (ca.sin(roll) * ca.sin(yaw) + ca.cos(roll) * ca.cos(yaw) * ca.sin(pitch)) - dragacc1 ddy = thrust_ref * (ca.cos(roll) * ca.sin(pitch) * ca.sin(yaw) - ca.sin(roll) * ca.cos(yaw)) - dragacc2 ddz = thrust_ref * ca.cos(roll) * ca.cos(pitch) - g rhs = [states[3], states[4], states[5]] rhs.append(ddx) rhs.append(ddy) rhs.append(ddz) rhs.append(states[8]) rhs.append(states[9]) rhs.append( #dds -1 / (l * l * h * h) * (l * l * l * l * ddx + s * s * s * (s * ddx - ddy) + l * l * s * (ds * ds + dr * dr) - l * l * ddx * (2 * s * s + r * r) - s * r * r * ds * ds + s * s * s * s * s * (ddz + g) / h + s * r * r * r * ddy + s * s * s * r * (ddy + r * 2 * ddz / h) + 2 * s * s * r * ds * dr + l * l * l * l * s * ddz / h - l * l * s * r * ddy + 1 / h * (2 * g * s * s * s * r * r + g * l * l * l * l * s - 2 * l * l * s * s * s * ddz + s * r * r * r * r * ddz - 2 * g * l * l * s * s * s + g * s * r * r * r * r - 2 * l * l * s * r * r * ddz - 2 * g * l * l * s * r * r))) rhs.append( #ddr -1 / (l * l * h * h) * (l * l * l * l * ddy - r * r * r * (r * ddy - ds * ds) + s * s * r * r * ddy + l * l * r * (ds * ds + dr * dr) - l * l * s * s * ddy - 2 * l * l * r * r * ddy - s * s * r * dr * dr + s * r * (r * r * ddx + s * s * ddx) + 2 * s * r * r * ds * dr - l * l * s * r * ddx + 1 / h * (r * r * r * r * r * (ddz + g) + 2 * s * s * r * r * r * ddz + l * l * l * l * r * ddz + 2 * r * r * r * (g * s * s - l * l * ddz - 2 * g * l * l) + g * l * l * l * l * r + s * s * s * s * r * (g + ddz) - 2 * l * l * s * s * r * (ddz + g)))) rhs.append((roll_gain * roll_ref - roll) / roll_tau) #droll rhs.append((pitch_gain * pitch_ref - pitch) / pitch_tau) #dpitch rhs.append(yaw_ref) #dyaw self.f = ca.Function('f', [states, controls], [ca.vcat(rhs)]) # acados model x_dot = ca.SX.sym('x_dot', len(rhs)) f_impl = x_dot - self.f(states, controls) model = AcadosModel() model.f_expl_expr = self.f(states, controls) model.f_impl_expr = f_impl model.x = states model.xdot = x_dot model.u = controls model.p = [] # ca.vcat([]) model.name = 'quadrotor' ### CONSTRAINTS constraints = ca.types.SimpleNamespace() constraints.roll_min = np.deg2rad(-45) constraints.pitch_min = np.deg2rad(-45) constraints.roll_max = np.deg2rad(45) constraints.pitch_max = np.deg2rad(45) constraints.yaw_min = np.deg2rad(-45) constraints.yaw_max = np.deg2rad(45) constraints.thrust_min = 0.0 * g # 0.5*g constraints.thrust_max = 1.9 * g # 1.9*g # constraints.s_min = -l # constraints.s_max = l # constraints.r_min = -l # constraints.r_max = l self.model = model self.constraints = constraints
def generate(self, dae=None, quad=None, name='tunempc', opts={}): """ Create embeddable NLP solver """ from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver, AcadosSimSolver # extract dimensions nx = self.__nx nu = self.__nu + self.__ns # treat slacks as pseudo-controls # extract reference ref = self.__ref xref = np.squeeze(self.__ref[0][:nx], axis=1) uref = np.squeeze(self.__ref[0][nx:nx + nu], axis=1) # sampling time self.__ts = opts['tf'] / self.__N # create acados model model = AcadosModel() model.x = ca.MX.sym('x', nx) model.u = ca.MX.sym('u', nu) model.p = [] model.name = name # detect input type if dae is None: model.f_expl_expr = self.__F(x0=model.x, p=model.u)['xf'] / self.__ts opts['integrator_type'] = 'ERK' opts['sim_method_num_stages'] = 1 opts['sim_method_num_steps'] = 1 else: n_in = dae.n_in() if n_in == 2: # xdot = f(x, u) if 'integrator_type' in opts: if opts['integrator_type'] in ['IRK', 'GNSF']: xdot = ca.MX.sym('xdot', nx) model.xdot = xdot model.f_impl_expr = xdot - dae(model.x, model.u[:self.__nu]) model.f_expl_expr = xdot elif opts['integrator_type'] == 'ERK': model.f_expl_expr = dae(model.x, model.u[:self.__nu]) else: raise ValueError('Provide numerical integrator type!') else: xdot = ca.MX.sym('xdot', nx) model.xdot = xdot model.f_expl_expr = xdot if n_in == 3: # f(xdot, x, u) = 0 model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu]) elif n_in == 4: # f(xdot, x, u, z) = 0 nz = dae.size1_in(3) z = ca.MX.sym('z', nz) model.z = z model.f_impl_expr = dae(xdot, model.x, model.u[:self.__nu], z) else: raise ValueError( 'Invalid number of inputs for system dynamics function.' ) if self.__gnl is not None: model.con_h_expr = self.__gnl(model.x, model.u[:self.__nu], model.u[self.__nu:]) if self.__type == 'economic': if quad is None: model.cost_expr_ext_cost = self.__cost( model.x, model.u[:self.__nu]) / self.__ts else: model.cost_expr_ext_cost = self.__cost(model.x, model.u[:self.__nu]) # create acados ocp ocp = AcadosOcp() ocp.model = model ny = nx + nu ny_e = nx if 'integrator_type' in opts and opts['integrator_type'] == 'GNSF': from acados_template import acados_dae_model_json_dump import os acados_dae_model_json_dump(model) # Set up Octave to be able to run the following: ## if using a virtual python env, the following lines can be added to the env/bin/activate script: # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/external/casadi-octave # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/ # export OCTAVE_PATH=$OCTAVE_PATH:$ACADOS_INSTALL_DIR/interfaces/acados_matlab_octave/acados_template_mex/ # echo # echo "OCTAVE_PATH=$OCTAVE_PATH" status = os.system( "octave --eval \"convert({})\"".format("\'" + model.name + "_acados_dae.json\'")) # load gnsf from json with open(model.name + '_gnsf_functions.json', 'r') as f: import json gnsf_dict = json.load(f) ocp.gnsf_model = gnsf_dict # set horizon length ocp.dims.N = self.__N # set cost module if self.__type == 'economic': # set cost function type to external (provided in model) ocp.cost.cost_type = 'EXTERNAL' if quad is not None: ocp.solver_options.cost_discretization = 'INTEGRATOR' elif self.__type == 'tracking': # set weighting matrices ocp.cost.W = self.__Href[0][0] # set-up linear least squares cost ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.W_e = np.zeros((nx, nx)) ocp.cost.Vx = np.zeros((ny, nx)) ocp.cost.Vx[:nx, :nx] = np.eye(nx) Vu = np.zeros((ny, nu)) Vu[nx:, :] = np.eye(nu) ocp.cost.Vu = Vu ocp.cost.Vx_e = np.eye(nx) ocp.cost.yref = np.squeeze( ca.vertcat(xref,uref).full() - \ ct.mtimes(np.linalg.inv(ocp.cost.W),self.__qref[0][0].T).full(), # gradient term axis = 1 ) ocp.cost.yref_e = np.zeros((ny_e, )) if n_in == 4: # DAE flag ocp.cost.Vz = np.zeros((ny, nz)) # if 'custom_hessian' in opts: # self.__custom_hessian = opts['custom_hessian'] # initial condition ocp.constraints.x0 = xref # set inequality constraints ocp.constraints.constr_type = 'BGH' if self.__S['C'] is not None: C = self.__S['C'][0][:, :nx] D = self.__S['C'][0][:, nx:] lg = -self.__S['e'][0] + ct.mtimes(C, xref).full() + ct.mtimes( D, uref).full() ug = 1e8 - self.__S['e'][0] + ct.mtimes( C, xref).full() + ct.mtimes(D, uref).full() ocp.constraints.lg = np.squeeze(lg, axis=1) ocp.constraints.ug = np.squeeze(ug, axis=1) ocp.constraints.C = C ocp.constraints.D = D if 'usc' in self.__vars: if 'us' in self.__vars: arg = [ self.__vars['x'], self.__vars['u'], self.__vars['us'], self.__vars['usc'] ] else: arg = [ self.__vars['x'], self.__vars['u'], self.__vars['usc'] ] Jsg = ca.Function( 'Jsg', [self.__vars['usc']], [ca.jacobian(self.__h(*arg), self.__vars['usc'])])(0.0) self.__Jsg = Jsg.full()[:-self.__nsc, :] ocp.constraints.Jsg = self.__Jsg ocp.cost.Zl = np.zeros((self.__nsc, )) ocp.cost.Zu = np.zeros((self.__nsc, )) ocp.cost.zl = np.squeeze(self.__scost.full(), axis=1) / self.__ts ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1) / self.__ts # set nonlinear equality constraints if self.__gnl is not None: ocp.constraints.lh = np.zeros(self.__ns, ) ocp.constraints.uh = np.zeros(self.__ns, ) # terminal constraint: x_term = self.__p_operator(model.x) Jbx = ca.Function('Jbx', [model.x], [ca.jacobian(x_term, model.x)])(0.0) ocp.constraints.Jbx_e = Jbx.full() ocp.constraints.lbx_e = np.squeeze(self.__p_operator(xref).full(), axis=1) ocp.constraints.ubx_e = np.squeeze(self.__p_operator(xref).full(), axis=1) for option in list(opts.keys()): if hasattr(ocp.solver_options, option): setattr(ocp.solver_options, option, opts[option]) self.__acados_ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp_' + model.name + '.json') self.__acados_integrator = AcadosSimSolver(ocp, json_file='acados_ocp_' + model.name + '.json') # set initial guess self.__set_acados_initial_guess() return self.__acados_ocp_solver, self.__acados_integrator
def export_car_ode_model(mb, mw, Iw, Rw): model_name = 'car_ode' #system parameters mt = mb + mw g = 9.81 # [m/s^2] # set up states & controls x1 = MX.sym('x1') z = MX.sym('z') phi = MX.sym('phi') l = MX.sym('l') theta = MX.sym('theta') x1_dot = MX.sym('x1_dot') z_dot = MX.sym('z_dot') phi_dot = MX.sym('phi_dot') l_dot = MX.sym('l_dot') theta_dot = MX.sym('theta_dot') x = vertcat(x1, z, phi, l, theta, x1_dot, z_dot, phi_dot, l_dot, theta_dot) # controls tau = MX.sym('tau') f = MX.sym('f') lambda_x = MX.sym('lambda_x') lambda_z = MX.sym('lambda_z') u = vertcat(tau, f, lambda_x, lambda_z) # xdot x1_ddot = MX.sym('x1_ddot') z_ddot = MX.sym('z_ddot') phi_ddot = MX.sym('phi_ddot') l_ddot = MX.sym('l_ddot') theta_ddot = MX.sym('theta_ddot') xdot = vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot, x1_ddot, z_ddot, phi_ddot, l_ddot, theta_ddot) # inertia matrix M = MX(5, 5) M[0, 0] = mt M[0, 1] = 0 M[0, 2] = 0 M[0, 3] = mb * sin(theta) M[0, 4] = mb * l * cos(theta) M[1, 0] = 0 M[1, 1] = mt M[1, 2] = 0 M[1, 3] = mb * cos(theta) M[1, 4] = -mb * l * sin(theta) M[2, 0] = 0 M[2, 1] = 0 M[2, 2] = Iw M[2, 3] = 0 M[2, 4] = 0 M[3, 0] = mb * sin(theta) M[3, 1] = mb * cos(theta) M[3, 2] = 0 M[3, 3] = mb M[3, 4] = 0 M[4, 0] = mb * l * cos(theta) M[4, 1] = -mb * l * sin(theta) M[4, 2] = 0 M[4, 3] = 0 M[4, 4] = mb * (l**2) # coriolis and centrifugal terms C = MX(5, 5) C[0, 0] = 0 C[0, 1] = 0 C[0, 2] = 0 C[0, 3] = 2 * mb * cos(theta) * theta_dot C[0, 4] = -mb * l * sin(theta) * theta_dot C[1, 0] = 0 C[1, 1] = 0 C[1, 2] = 0 C[1, 3] = -2 * mb * sin(theta) * theta_dot C[1, 4] = -mb * l * cos(theta) * theta_dot C[2, 0] = 0 C[2, 1] = 0 C[2, 2] = 0 C[2, 3] = 0 C[2, 4] = 0 C[3, 0] = 0 C[3, 1] = 0 C[3, 2] = 0 C[3, 3] = 0 C[3, 4] = -mb * l * theta_dot C[4, 0] = 0 C[4, 1] = 0 C[4, 2] = 0 C[4, 3] = 2 * mb * l * theta_dot C[4, 4] = 0 # gravity term G = MX(5, 1) G[0, 0] = 0 G[1, 0] = g * mt G[2, 0] = 0 G[3, 0] = g * mb * cos(theta) G[4, 0] = -g * mb * l * sin(theta) # define S S = MX(2, 5) S[0, 0] = 0 S[0, 1] = 0 S[0, 2] = 1 S[0, 3] = 0 S[0, 4] = -1 S[1, 0] = 0 S[1, 1] = 0 S[1, 2] = 0 S[1, 3] = 1 S[1, 4] = 0 # define Jc Jc = MX(2, 5) Jc[0, 0] = 1 Jc[0, 1] = 0 Jc[0, 2] = -Rw Jc[0, 3] = 0 Jc[0, 4] = 0 Jc[1, 0] = 0 Jc[1, 1] = 1 Jc[1, 2] = 0 Jc[1, 3] = 0 Jc[1, 4] = 0 # compute acceleration ddq = mtimes( inv(M), (mtimes(transpose(S), vertcat(tau, f)) + mtimes(transpose(Jc), vertcat(lambda_x, lambda_z)) - mtimes(C, vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot)) - G)) print(ddq.shape) # dynamics f_expl = vertcat(x1_dot, z_dot, phi_dot, l_dot, theta_dot, ddq) print(f_expl.shape) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # algebraic variables model.z = vertcat([]) # parameters p = vertcat([]) model.p = p model.name = model_name # activate1 = MX.sym('activate1') # activate2 = MX.sym('activate2') # activate3 = MX.sym('activate3') # activate4 = MX.sym('activate4') # activate5 = MX.sym('activate5') # activate6 = MX.sym('activate6') # activate7 = MX.sym('activate7') # model.p = vertcat(activate1, activate2, activate3, activate4, activate5, activate6, activate7) model.con_h_expr = vertcat((lambda_z - fabs(lambda_x)), (x1_dot - Rw * phi_dot)) print(f"SHAPE: {model.con_h_expr}") return model
def export_car_ode_model(mb, mw, Iw, Rw): model_name = 'car_ode' #system parameters mt = mb + mw g = 9.81 # [m/s^2] # set up states & controls phi = MX.sym('phi') l = MX.sym('l') theta = MX.sym('theta') phi_dot = MX.sym('phi_dot') l_dot = MX.sym('l_dot') theta_dot = MX.sym('theta_dot') x = vertcat(phi, l, theta, phi_dot, l_dot, theta_dot) # controls tau = MX.sym('tau') f = MX.sym('f') u = vertcat(tau, f) # xdot phi_ddot = MX.sym('phi_ddot') l_ddot = MX.sym('l_ddot') theta_ddot = MX.sym('theta_ddot') xdot = vertcat(phi_dot, l_dot, theta_dot, phi_ddot, l_ddot, theta_ddot) # inertia matrix M = MX(3, 3) M[0,0]=Iw + (Rw**2)*mb + (Rw**2)*mw M[0,1]=Rw*mb*sin(theta) M[0,2]=Rw*l*mb*cos(theta) M[1,0]=Rw*mb*sin(theta) M[1,1]=mb M[1,2]=0 M[2,0]=Rw*l*mb*cos(theta) M[2,1]=0 M[2,2]=(l**2)*mb # coriolis and centrifugal terms C = MX(3,1) C[0, 0]=Rw*mb*theta_dot*(2*l_dot*cos(theta) - l*theta_dot*sin(theta)) C[1, 0]=-l*mb*(theta_dot**2) C[2, 0]=2*l*l_dot*mb*theta_dot # gravity term G = MX(3,1) G[0, 0] = 0 G[1, 0] = g*mb*cos(theta) G[2, 0] = -g*l*mb*sin(theta) # define S S = MX(2,3) S[0, 0] = 1 S[0, 1] = 0 S[0, 2] = -1 S[1, 0] = 0 S[1, 1] = 1 S[1, 2] = 0 p = vertcat([]) # compute acceleration ddq = mtimes(inv(M), (mtimes(transpose(S),vertcat(tau, f)) - C - G)) print(ddq.shape) # dynamics f_expl = vertcat(phi_dot, l_dot, theta_dot, ddq ) print(f_expl.shape) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # algebraic variables model.z = vertcat([]) # parameters model.p = p model.name = model_name #model.con_h_expr = vertcat( (lambda_z - fabs(lambda_x)), # (x1_dot-Rw*phi_dot)) #print(f"SHAPE: {model.con_h_expr}") return model
def export_quaternion_ode_model(): model_name = 'quaternion_ode' # set up states & controls q0 = SX.sym('q0') q1 = SX.sym('q1') q2 = SX.sym('q2') q3 = SX.sym('q3') omegax = SX.sym('omegax') omegay = SX.sym('omegay') omegaz = SX.sym('omegaz') x = vertcat(q0, q1, q2, q3, omegax, omegay, omegaz) # controls omegax_cont = SX.sym('omegax_cont') omegay_cont = SX.sym('omegay_cont') omegaz_cont = SX.sym('omegaz_cont') u = vertcat(omegax_cont, omegay_cont, omegaz_cont) # xdot q0_dot = SX.sym('q0_dot') q1_dot = SX.sym('q1_dot') q2_dot = SX.sym('q2_dot') q3_dot = SX.sym('q3_dot') omegax_dot = SX.sym('omegax_dot') omegay_dot = SX.sym('omegay_dot') omegaz_dot = SX.sym('omegaz_dot') xdot = vertcat(q0_dot, q1_dot, q2_dot, q3_dot, omegax_dot, omegay_dot, omegaz_dot) # algebraic variables # z = None # parameters p = [] # This comes from ref[6] pg 449. # dq = 1/2 * G(q)' * Ω S = SX.zeros(3, 4) S[:, 0] = vertcat(-q1, -q2, -q3) S[:, 1:4] = q0 * np.eye(3) - skew(vertcat(q1, q2, q3)) print("S:", S, S.shape) OMG = vertcat(omegax, omegay, omegaz) # dynamics f_expl = vertcat(0.5 * mtimes(S.T, OMG), u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name # Reference generation (quaternion to eul) used in cost function. Eul = SX(3, 1) Eul[0, 0] = arctan2(2 * (q0 * q1 + q2 * q3), 1 - 2 * (q1**2 + q2**2)) Eul[1, 0] = arcsin(2 * (q0 * q2 - q3 * q1)) Eul[2, 0] = arctan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2**2 + q3**2)) quat_to_eul = Function("quat_to_eul", [x, u], [Eul]) model.cost_y_expr = vertcat( Eul, x[4:, :], u) #: CasADi expression for nonlinear least squares model.cost_y_expr_e = vertcat( Eul, x[4:, :]) #: CasADi expression for nonlinear least squares, terminal model.con_h_expr = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3 return model
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")
def export_pend_ode_model(): model_name = 'pend_ode' # set up states & controls q1 = SX.sym('q1') q2 = SX.sym('q2') dq1 = SX.sym('dq1') dq2 = SX.sym('dq2') b1 = SX.sym('b1') b2 = SX.sym('b2') x = vertcat(q1, q2, dq1, dq2, b1, b2) # controls tau1 = SX.sym('tau1') tau = vertcat(tau1, 0) u = vertcat(tau1) # xdot q1_dot = SX.sym('q1_dot') q2_dot = SX.sym('q2_dot') dq1_dot = SX.sym('dq1_dot') dq2_dot = SX.sym('dq2_dot') b1_dot = SX.sym('b1_dot') b2_dot = SX.sym('b2_dot') xdot = vertcat(q1_dot, q2_dot, dq1_dot, dq2_dot, b1_dot, b2_dot) # algebraic variables # z = None # parameters # set up parameters m1 = SX.sym('m1') # mass link 1 m2 = SX.sym('m2') # mass link 2 l1 = SX.sym('l1') # length link 1 l2 = SX.sym('l2') # length link 2 lc1 = SX.sym('lc1') # cm link 1 lc2 = SX.sym('lc2') # cm link 2 g = SX.sym('g') I1 = (1 / 3) * m1 * l1 * l1 I2 = (1 / 3) * m2 * l2 * l2 p = vertcat(m1, m2, l1, l2, lc1, lc2, g) # system dynamics. theta1 = m1 * lc1 * lc1 + m2 * l1 * l1 + I1 theta2 = m2 * lc2 * lc2 + I2 theta3 = m2 * l1 * lc2 theta4 = m1 * lc1 + m2 * l1 theta5 = m2 * lc2 # In matrix form, D(q) -> Inertia Matrix, C(dq,q) ->Coriolis Matrix, F(dq) -> Dissipation matrix # D(q) ddq + C(q,dq) dq + F(dq) + g(q) = tau D = SX.zeros(2, 2) C = SX.zeros(2, 2) F = SX.zeros(2, 1) gVec = SX.zeros(2, 1) D[0, 0] = theta1 + theta2 + 2 * theta3 * cos(q2) D[1, 0] = theta2 + theta3 * cos(q2) D[0, 1] = D[1, 0] D[1, 1] = theta2 C[0, 0] = -theta3 * sin(q2) * dq2 C[1, 0] = -theta3 * sin(q2) * dq2 - theta3 * sin(q2) * dq1 C[0, 1] = theta3 * sin(q2) * dq1 C[1, 1] = 0 F[0, 0] = b1 * dq1 F[1, 0] = b2 * dq2 gVec[0, 0] = theta4 * g * cos(q1) + theta5 * g * cos(q1 + q2) gVec[1, 0] = theta5 * g * cos(q1 + q2) # dynamics f_expl = vertcat( dq1, dq2, mtimes(inv(D), tau - mtimes(C, vertcat(dq1, dq2)) - F - gVec), 0, 0) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = [] model.p = p model.name = model_name return model
def acados_linear_quad_model_moving_eq(num_states, g=9.81): model_name = 'linear_quad_v2' tc = 0.59375 # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) gravity = SX.zeros(3) gravity[2] = g x = SX.sym('x', 9) u = SX.sym('u', 4) xdot = SX.sym('xdot', 9) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(x[3]) r1[1, 2] = -sin(x[3]) r1[2, 1] = sin(x[3]) r1[2, 2] = cos(x[3]) r2 = SX(3, 3) r2[0, 0] = cos(x[4]) r2[0, 2] = sin(x[4]) r2[1, 1] = 1 r2[2, 0] = -sin(x[4]) r2[2, 2] = cos(x[4]) r3 = SX(3, 3) r3[0, 0] = cos(x[5]) r3[0, 1] = -sin(x[5]) r3[1, 0] = sin(x[5]) r3[1, 1] = cos(x[5]) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] f_nl = vertcat(x[6:9], inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity) # f_const = 20 # f_nl = vertcat( # x[6:9], # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g + f_const)/tc) - f_const) + gravity # ) A = jacobian(f_nl, x) B = jacobian(f_nl, u) p = [] f_expl = mtimes(A, x) + mtimes(B, u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def acados_settings(Tf, N): # create render arguments ocp = AcadosOcp() # export model model, constraint = usv_model() # define acados ODE model_ac = AcadosModel() model_ac.f_impl_expr = model.f_impl_expr model_ac.f_expl_expr = model.f_expl_expr model_ac.x = model.x model_ac.xdot = model.xdot model_ac.u = model.U model_ac.z = model.z model_ac.p = model.p model_ac.name = model.name ocp.model = model_ac # define constraint model_ac.con_h_expr = constraint.expr # set dimensions nx = model.x.size()[0] nu = model.U.size()[0] ny = nx + nu ny_e = nx ocp.dims.N = N ns = 8 nsh = 8 # set cost Q = np.diag([0, 0, 0.05, 0.01, 0, 0, 0, 0]) R = np.eye(nu) R[0, 0] = 0.2 Qe = np.diag([0, 0, 0.1, 0.05, 0, 0, 0, 0]) ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_e = "LINEAR_LS" #unscale = N / Tf #ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R) #ocp.cost.W_e = Qe / unscale ocp.cost.W = scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Qe Vx = np.zeros((ny, nx)) Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vx = Vx Vu = np.zeros((ny, nu)) Vu[8, 0] = 1.0 ocp.cost.Vu = Vu Vx_e = np.zeros((ny_e, nx)) Vx_e[:nx, :nx] = np.eye(nx) ocp.cost.Vx_e = Vx_e ocp.cost.zl = 10 * np.ones((ns, )) #previously 100 ocp.cost.Zl = 0 * np.ones((ns, )) ocp.cost.zu = 10 * np.ones((ns, )) #previously 100 ocp.cost.Zu = 0 * np.ones((ns, )) # set intial references ocp.cost.yref = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0, 0, 0]) # setting constraints #ocp.constraints.lbx = np.array([model.psieddot_min]) #ocp.constraints.ubx = np.array([model.psieddot_max]) #ocp.constraints.idxbx = np.array([8]) ocp.constraints.lbu = np.array([model.Upsieddot_min]) ocp.constraints.ubu = np.array([model.Upsieddot_max]) ocp.constraints.idxbu = np.array([0]) # ocp.constraints.lsbx=np.zero s([1]) # ocp.constraints.usbx=np.zeros([1]) # ocp.constraints.idxsbx=np.array([1]) ocp.constraints.lh = np.array([ constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min, constraint.distance_min ]) ocp.constraints.uh = np.array([ 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000, 1000000 ]) '''ocp.constraints.lsh = np.zeros(nsh) ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2])''' ocp.constraints.lsh = np.array( [-0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2, -0.2]) ocp.constraints.ush = np.array([0, 0, 0, 0, 0, 0, 0, 0]) ocp.constraints.idxsh = np.array([0, 1, 2, 3, 4, 5, 6, 7]) # set intial condition ocp.constraints.x0 = model.x0 #ocp.parameter_values = np.array([4,4,4,8,4,12,4,20,100,100,100,100,100,100,100,100]) ocp.parameter_values = np.array([ 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100, 100 ]) # set QP solver and integration ocp.solver_options.tf = Tf #ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" ocp.solver_options.nlp_solver_type = "SQP_RTI" #ocp.solver_options.nlp_solver_type = "SQP" ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" #ocp.solver_options.sim_method_num_stages = 4 #ocp.solver_options.sim_method_num_steps = 3 #ocp.solver_options.nlp_solver_max_iter = 200 #ocp.solver_options.tol = 1e-4 #ocp.solver_options.qp_solver_tol_stat = 1e-2 #ocp.solver_options.qp_solver_tol_eq = 1e-2 #ocp.solver_options.qp_solver_tol_ineq = 1e-2 #ocp.solver_options.qp_solver_tol_comp = 1e-2 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") return constraint, model, acados_solver
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
def acados_settings(Tf, N, track_file): # create render arguments ocp = AcadosOcp() # export model model, constraint = bicycle_model(track_file) # define acados ODE model_ac = AcadosModel() model_ac.f_impl_expr = model.f_impl_expr model_ac.f_expl_expr = model.f_expl_expr model_ac.x = model.x model_ac.xdot = model.xdot model_ac.u = model.u model_ac.z = model.z model_ac.p = model.p model_ac.name = model.name ocp.model = model_ac # define constraint model_ac.con_h_expr = constraint.expr # set dimensions nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx ocp.dims.N = N ns = 2 nsh = 2 # set cost Q = np.diag([1e-1, 1e-8, 1e-8, 1e-8, 1e-3, 5e-3]) R = np.eye(nu) R[0, 0] = 1e-3 R[1, 1] = 5e-3 Qe = np.diag([5e0, 1e1, 1e-8, 1e-8, 5e-3, 2e-3]) ocp.cost.cost_type = "LINEAR_LS" ocp.cost.cost_type_e = "LINEAR_LS" unscale = N / Tf ocp.cost.W = unscale * scipy.linalg.block_diag(Q, R) ocp.cost.W_e = Qe / unscale Vx = np.zeros((ny, nx)) Vx[:nx, :nx] = np.eye(nx) ocp.cost.Vx = Vx Vu = np.zeros((ny, nu)) Vu[6, 0] = 1.0 Vu[7, 1] = 1.0 ocp.cost.Vu = Vu Vx_e = np.zeros((ny_e, nx)) Vx_e[:nx, :nx] = np.eye(nx) ocp.cost.Vx_e = Vx_e ocp.cost.zl = 100 * np.ones((ns, )) ocp.cost.Zl = 0 * np.ones((ns, )) ocp.cost.zu = 100 * np.ones((ns, )) ocp.cost.Zu = 0 * np.ones((ns, )) # set intial references ocp.cost.yref = np.array([1, 0, 0, 0, 0, 0, 0, 0]) ocp.cost.yref_e = np.array([0, 0, 0, 0, 0, 0]) # setting constraints ocp.constraints.lbx = np.array([-12]) ocp.constraints.ubx = np.array([12]) ocp.constraints.idxbx = np.array([1]) ocp.constraints.lbu = np.array([model.dthrottle_min, model.ddelta_min]) ocp.constraints.ubu = np.array([model.dthrottle_max, model.ddelta_max]) ocp.constraints.idxbu = np.array([0, 1]) # ocp.constraints.lsbx=np.zero s([1]) # ocp.constraints.usbx=np.zeros([1]) # ocp.constraints.idxsbx=np.array([1]) ocp.constraints.lh = np.array([ constraint.along_min, constraint.alat_min, model.n_min, model.throttle_min, model.delta_min, ]) ocp.constraints.uh = np.array([ constraint.along_max, constraint.alat_max, model.n_max, model.throttle_max, model.delta_max, ]) ocp.constraints.lsh = np.zeros(nsh) ocp.constraints.ush = np.zeros(nsh) ocp.constraints.idxsh = np.array([0, 2]) # set intial condition ocp.constraints.x0 = model.x0 # set QP solver and integration ocp.solver_options.tf = Tf # ocp.solver_options.qp_solver = 'FULL_CONDENSING_QPOASES' ocp.solver_options.qp_solver = "PARTIAL_CONDENSING_HPIPM" ocp.solver_options.nlp_solver_type = "SQP_RTI" ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" ocp.solver_options.sim_method_num_stages = 4 ocp.solver_options.sim_method_num_steps = 3 # ocp.solver_options.qp_solver_tol_stat = 1e-2 # ocp.solver_options.qp_solver_tol_eq = 1e-2 # ocp.solver_options.qp_solver_tol_ineq = 1e-2 # ocp.solver_options.qp_solver_tol_comp = 1e-2 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp.json") return constraint, model, acados_solver
def acados_linear_quad_model_moving_eq(num_states, g=9.81): ''' A linear model linearized about the moving point. (I am not certain what I am linearizing about here actually). I compute the Jacobian at every execution basically. ''' model_name = 'linear_quad_v2' tc = 0.59375 # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) gravity = SX.zeros(3) gravity[2] = g x = SX.sym('x', 9) u = SX.sym('u', 4) xdot = SX.sym('xdot', 9) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(x[3]) r1[1, 2] = -sin(x[3]) r1[2, 1] = sin(x[3]) r1[2, 2] = cos(x[3]) r2 = SX(3, 3) r2[0, 0] = cos(x[4]) r2[0, 2] = sin(x[4]) r2[1, 1] = 1 r2[2, 0] = -sin(x[4]) r2[2, 2] = cos(x[4]) r3 = SX(3, 3) r3[0, 0] = cos(x[5]) r3[0, 1] = -sin(x[5]) r3[1, 0] = sin(x[5]) r3[1, 1] = cos(x[5]) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] f_nl = vertcat(x[6:9], inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * u[3] * (g / tc) + gravity) A = jacobian(f_nl, x) B = jacobian(f_nl, u) p = [] f_expl = mtimes(A, x) + mtimes(B, u) f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def acados_nonlinear_quad_model(num_states, g=9.81): model_name = 'nonlinear_quad' position = SX.sym('o', 3) velocity = SX.sym('v', 3) phi = SX.sym('phi') theta = SX.sym('theta') psi = SX.sym('psi') gravity = SX.zeros(3) gravity[2] = g x = vertcat(position, phi, theta, psi, velocity) p = SX.sym('p') q = SX.sym('q') r = SX.sym('r') F = SX.sym('F') u = vertcat(p, q, r, F) r1 = SX(3, 3) r1[0, 0] = 1 r1[1, 1] = cos(phi) r1[1, 2] = -sin(phi) r1[2, 1] = sin(phi) r1[2, 2] = cos(phi) r2 = SX(3, 3) r2[0, 0] = cos(theta) r2[0, 2] = sin(theta) r2[1, 1] = 1 r2[2, 0] = -sin(theta) r2[2, 2] = cos(theta) r3 = SX(3, 3) r3[0, 0] = cos(psi) r3[0, 1] = -sin(psi) r3[1, 0] = sin(psi) r3[1, 1] = cos(psi) r3[2, 2] = 1 tau = SX(3, 3) tau[:, 0] = (inv(r2))[:, 0] tau[:, 1] = (inv(r2))[:, 1] tau[:, 2] = (inv(r2 @ r1))[:, 2] # tau[:,0] = (inv(r1))[:,0] # tau[:,1] = (inv(r1))[:,1] # tau[:,2] = (inv(r1 @ r2))[:,2] # tau[0,0] = cos(theta) # tau[0,2] = -sin(theta) # tau[1,1] = 1 # tau[1,2] = sin(phi) * cos(theta) # tau[2,0] = sin(theta) # tau[2,2] = cos(phi) * cos(theta) tc = 0.59375 # mapping control to throttle # f_const = 100 f_const = 20 # tc = 0.75 # # scaling_control_mat = np.array([1, 1, 1, (g/tc)]) # f_expl = vertcat( # velocity, # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * (u[3] * ((g)/tc)) + gravity # ) f_expl = vertcat( velocity, inv(tau) @ u[0:3], -((r3 @ r1 @ r2)[:, 2]) * (u[3] * ((g + f_const) / tc) - f_const) + gravity) # f_expl = vertcat( # velocity, # inv(tau) @ u[0:3], # - ((r3 @ r1 @ r2)[:,2]) * ((g - f_const)*(1 - tc) / (1 - u[3]) + f_const) + gravity # ) # xdot xdot = SX.sym('xdot', 9) #parameters p = [] f_impl = xdot - f_expl model = AcadosModel() model.f_impl_expr = f_impl model.f_expl_expr = f_expl model.x = x model.xdot = xdot model.u = u # model.z = z model.p = p model.name = model_name return model
def 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()