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 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 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 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
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 acados_settings_dyn(Tf, N, modelparams = "modelparams.yaml"): #create render arguments ocp = AcadosOcp() #load model model, constraints = dynamic_model(modelparams) # 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 #inputvector model_ac.u = model.u #parameter vector model_ac.p = model.p #parameter vector model_ac.z = model.z #external cost function model_ac.cost_expr_ext_cost = model.stage_cost #model_ac.cost_expr_ext_cost_e = 0 model_ac.con_h_expr = model.con_h_expr model_ac.name = model.name ocp.model = model_ac # set dimensions nx = model.x.size()[0] nu = model.u.size()[0] nz = model.z.size()[0] np = model.p.size()[0] #ny = nu + nx #ny_e = nx ocp.dims.nx = nx ocp.dims.nz = nz #ocp.dims.ny = ny #ocp.dims.ny_e = ny_e ocp.dims.nu = nu ocp.dims.np = np ocp.dims.nh = 1 #ocp.dims.ny = ny #ocp.dims.ny_e = ny_e ocp.dims.nbx = 3 ocp.dims.nsbx = 0 ocp.dims.nbu = nu #number of soft on h constraints ocp.dims.nsh = 1 ocp.dims.ns = 1 ocp.dims.N = N # set cost to casadi expression defined above ocp.cost.cost_type = "EXTERNAL" #ocp.cost.cost_type_e = "EXTERNAL" ocp.cost.zu = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.Zu = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.zl = 1000 * npy.ones((ocp.dims.ns,)) ocp.cost.Zl = 1000 * npy.ones((ocp.dims.ns,)) #not sure if needed #unscale = N / Tf #constraints #stagewise constraints for tracks with slack ocp.constraints.uh = npy.array([0.00]) ocp.constraints.lh = npy.array([-10]) ocp.constraints.lsh = 0.1*npy.ones(ocp.dims.nsh) ocp.constraints.ush = -0.1*npy.ones(ocp.dims.nsh) ocp.constraints.idxsh = npy.array([0]) #ocp.constraints.Jsh = 1 # boxconstraints # xvars = ['posx', 'posy', 'phi', 'vx', 'vy', 'omega', 'd', 'delta', 'theta'] ocp.constraints.lbx = npy.array([10, 10, 100, model.vx_min, model.vy_min, model.omega_min, model.d_min, model.delta_min, model.theta_min]) ocp.constraints.ubx = npy.array([10, 10, 100, model.vx_max, model.vy_max, model.omega_max, model.d_max, model.delta_max, model.theta_max]) ocp.constraints.idxbx = npy.arange(nx) #ocp.constraints.lsbx= -0.1 * npy.ones(ocp.dims.nbx) #ocp.constraints.usbx= 0.1 * npy.ones(ocp.dims.nbx) #ocp.constraints.idxsbx= npy.array([6,7,8]) #print("ocp.constraints.idxbx: ",ocp.constraints.idxbx) ocp.constraints.lbu = npy.array([model.ddot_min, model.deltadot_min, model.thetadot_min]) ocp.constraints.ubu = npy.array([model.ddot_max, model.deltadot_max, model.thetadot_max]) ocp.constraints.idxbu = npy.array([0, 1, 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"#"SQP_RTI" # ocp.solver_options.hessian_approx = "GAUSS_NEWTON" ocp.solver_options.integrator_type = "ERK" ocp.parameter_values = npy.zeros(np) #ocp.solver_options.sim_method_num_stages = 4 #ocp.solver_options.sim_method_num_steps = 3 ocp.solver_options.nlp_solver_step_length = 0.01 ocp.solver_options.nlp_solver_max_iter = 50 ocp.solver_options.tol = 1e-4 #ocp.solver_options.print_level = 1 # ocp.solver_options.nlp_solver_tol_comp = 1e-1 # create solver acados_solver = AcadosOcpSolver(ocp, json_file="acados_ocp_dynamic.json") return constraints, model, acados_solver, ocp
def generate(self, dae, 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) # 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 n_in = dae.n_in() if n_in == 2: # xdot = f(x, u) if 'integrator_type' in opts: if opts['integrator_type'] == 'IRK': 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': 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 # 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' else: # set weighting matrices if self.__type == 'tracking': 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)) # 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) ocp.cost.zu = np.squeeze(self.__scost.full(), axis=1) # 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()): 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