Vx_e = np.zeros((ny_e, nx)) Vx_e[:nx, :nx] = np.eye(nx) ocp.cost.Vx_e = Vx_e # set intial references #target_position = np.array([0., 0.8, 0, 0, 0, 0]) # target standing position target_position = np.array([-2. / Rw, 0.8, 0, 0, 0, 0, 0, mb * g]) # target standing position yref = target_position yref_e = target_position[:6] ocp.cost.yref = yref ocp.cost.yref_e = yref_e # parameters ocp.parameter_values = np.array([]) # setting constraints # setting constraints lower_X = np.array([2 * Rw, -np.pi / 2]) upper_X = np.array([1 + 2 * Rw, np.pi / 2]) ocp.constraints.lbx = lower_X ocp.constraints.ubx = upper_X ocp.constraints.idxbx = np.array([1, 2]) lower_U = np.array([-10, -200]) upper_U = np.array([10, 200]) ocp.constraints.lbu = lower_U ocp.constraints.ubu = upper_U
ocp.cost.yref_e = nmp.zeros((10, )) ocp.cost.yref_e[3] = 1 # init conditions, start at yaw of +90deg x0 = nmp.array([0.285, -0.959, 0.0, 0.0, 0.0, 0.0, 0.0]) # set constraints uDel = 8 uSS = 39.99 ocp.constraints.constr_type = 'BGH' ocp.constraints.lbu = nmp.array( [uSS - uDel, uSS - uDel, uSS - uDel, uSS - uDel]) ocp.constraints.ubu = nmp.array( [uSS + uDel, uSS + uDel, uSS + uDel, uSS + uDel]) ocp.constraints.x0 = x0 ocp.parameter_values = p ocp.constraints.idxbu = nmp.array([0, 1, 2, 3]) if USE_QUAT_SLACK == 1: # nonlinear quaternion constraint ocp.constraints.lh = nmp.array([1.0]) ocp.constraints.uh = nmp.array([1.0]) # #slack for nonlinear quat ocp.constraints.lsh = nmp.array([-0.2]) ocp.constraints.ush = nmp.array([0.2]) ocp.constraints.idxsh = nmp.array([0]) ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' #'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_RTI' # SQP_RTI
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)
# set constraints Fmax = 80 # use equivalent formulation with h constraint # ocp.constraints.lbu = np.array([-Fmax]) # ocp.constraints.ubu = np.array([+Fmax]) # ocp.constraints.idxbu = np.array([0]) p = SX.sym('p') ocp.model.p = p ocp.constraints.lh = np.array([-Fmax]) ocp.constraints.uh = np.array([+Fmax]) ocp.model.con_h_expr = model.u / p ocp.parameter_values = np.array([0]) ocp.constraints.x0 = np.array([0.0, np.pi, 0.0, 0.0]) ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # FULL_CONDENSING_QPOASES ocp.solver_options.hessian_approx = 'EXACT' # GAUSS_NEWTON, EXACT ocp.solver_options.regularize_method = 'CONVEXIFY' # GAUSS_NEWTON, EXACT ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP # set prediction horizon ocp.solver_options.tf = Tf ocp_solver = AcadosOcpSolver(ocp, json_file='acados_ocp.json')
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 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 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 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()
def __init__(self, quad, t_horizon=1, n_nodes=20, q_cost=None, r_cost=None, q_mask=None, B_x=None, gp_regressors=None, rdrv_d_mat=None, model_name="quad_3d_acados_mpc", solver_options=None): """ :param quad: quadrotor object :type quad: Quadrotor3D :param t_horizon: time horizon for MPC optimization :param n_nodes: number of optimization nodes until time horizon :param q_cost: diagonal of Q matrix for LQR cost of MPC cost function. Must be a numpy array of length 12. :param r_cost: diagonal of R matrix for LQR cost of MPC cost function. Must be a numpy array of length 4. :param B_x: dictionary of matrices that maps the outputs of the gp regressors to the state space. :param gp_regressors: Gaussian Process ensemble for correcting the nominal model :type gp_regressors: GPEnsemble :param q_mask: Optional boolean mask that determines which variables from the state compute towards the cost function. In case no argument is passed, all variables are weighted. :param solver_options: Optional set of extra options dictionary for solvers. :param rdrv_d_mat: 3x3 matrix that corrects the drag with a linear model according to Faessler et al. 2018. None if not used """ # Weighted squared error loss function q = (p_xyz, a_xyz, v_xyz, r_xyz), r = (u1, u2, u3, u4) if q_cost is None: q_cost = np.array([10, 10, 10, 0.1, 0.1, 0.1, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05]) if r_cost is None: r_cost = np.array([0.1, 0.1, 0.1, 0.1]) self.T = t_horizon # Time horizon self.N = n_nodes # number of control nodes within horizon self.quad = quad self.max_u = quad.max_input_value self.min_u = quad.min_input_value # Declare model variables self.p = cs.MX.sym('p', 3) # position self.q = cs.MX.sym('a', 4) # angle quaternion (wxyz) self.v = cs.MX.sym('v', 3) # velocity self.r = cs.MX.sym('r', 3) # angle rate # Full state vector (13-dimensional) self.x = cs.vertcat(self.p, self.q, self.v, self.r) self.state_dim = 13 # Control input vector u1 = cs.MX.sym('u1') u2 = cs.MX.sym('u2') u3 = cs.MX.sym('u3') u4 = cs.MX.sym('u4') self.u = cs.vertcat(u1, u2, u3, u4) # Nominal model equations symbolic function (no GP) self.quad_xdot_nominal = self.quad_dynamics(rdrv_d_mat) # Linearized model dynamics symbolic function self.quad_xdot_jac = self.linearized_quad_dynamics() # Initialize objective function, 0 target state and integration equations self.L = None self.target = None # Check if GP ensemble has an homogeneous feature space (if actual Ensemble) if gp_regressors is not None and gp_regressors.homogeneous: self.gp_reg_ensemble = gp_regressors self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx] self.B_x = np.squeeze(np.stack(self.B_x, axis=1)) self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x self.B_z = gp_regressors.B_z elif gp_regressors and gp_regressors.no_ensemble: # If not homogeneous, we have to treat each z feature vector independently self.gp_reg_ensemble = gp_regressors self.B_x = [B_x[dim] for dim in gp_regressors.dim_idx] self.B_x = np.squeeze(np.stack(self.B_x, axis=1)) self.B_x = self.B_x[:, np.newaxis] if len(self.B_x.shape) == 1 else self.B_x self.B_z = gp_regressors.B_z else: self.gp_reg_ensemble = None # Declare model variables for GP prediction (only used in real quadrotor flight with EKF estimator). # Will be used as initial state for GP prediction as Acados parameters. self.gp_p = cs.MX.sym('gp_p', 3) self.gp_q = cs.MX.sym('gp_a', 4) self.gp_v = cs.MX.sym('gp_v', 3) self.gp_r = cs.MX.sym('gp_r', 3) self.gp_x = cs.vertcat(self.gp_p, self.gp_q, self.gp_v, self.gp_r) # The trigger variable is used to tell ACADOS to use the additional GP state estimate in the first optimization # node and the regular integrated state in the rest self.trigger_var = cs.MX.sym('trigger', 1) # Build full model. Will have 13 variables. self.dyn_x contains the symbolic variable that # should be used to evaluate the dynamics function. It corresponds to self.x if there are no GP's, or # self.x_with_gp otherwise acados_models, nominal_with_gp = self.acados_setup_model( self.quad_xdot_nominal(x=self.x, u=self.u)['x_dot'], model_name) # Convert dynamics variables to functions of the state and input vectors self.quad_xdot = {} for dyn_model_idx in nominal_with_gp.keys(): dyn = nominal_with_gp[dyn_model_idx] self.quad_xdot[dyn_model_idx] = cs.Function('x_dot', [self.x, self.u], [dyn], ['x', 'u'], ['x_dot']) # ### Setup and compile Acados OCP solvers ### # self.acados_ocp_solver = {} # Check if GP's have been loaded self.with_gp = self.gp_reg_ensemble is not None # Add one more weight to the rotation (use quaternion norm weighting in acados) q_diagonal = np.concatenate((q_cost[:3], np.mean(q_cost[3:6])[np.newaxis], q_cost[3:])) if q_mask is not None: q_mask = np.concatenate((q_mask[:3], np.zeros(1), q_mask[3:])) q_diagonal *= q_mask # Ensure current working directory is current folder os.chdir(os.path.dirname(os.path.realpath(__file__))) self.acados_models_dir = '../../acados_models' safe_mkdir_recursive(os.path.join(os.getcwd(), self.acados_models_dir)) for key, key_model in zip(acados_models.keys(), acados_models.values()): nx = key_model.x.size()[0] nu = key_model.u.size()[0] ny = nx + nu n_param = key_model.p.size()[0] if isinstance(key_model.p, cs.MX) else 0 acados_source_path = os.environ['ACADOS_SOURCE_DIR'] sys.path.insert(0, '../common') # Create OCP object to formulate the optimization ocp = AcadosOcp() ocp.acados_include_path = acados_source_path + '/include' ocp.acados_lib_path = acados_source_path + '/lib' ocp.model = key_model ocp.dims.N = self.N ocp.solver_options.tf = t_horizon # Initialize parameters ocp.dims.np = n_param ocp.parameter_values = np.zeros(n_param) ocp.cost.cost_type = 'LINEAR_LS' ocp.cost.cost_type_e = 'LINEAR_LS' ocp.cost.W = np.diag(np.concatenate((q_diagonal, r_cost))) ocp.cost.W_e = np.diag(q_diagonal) terminal_cost = 0 if solver_options is None or not solver_options["terminal_cost"] else 1 ocp.cost.W_e *= terminal_cost 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[-4:, -4:] = np.eye(nu) ocp.cost.Vx_e = np.eye(nx) # Initial reference trajectory (will be overwritten) x_ref = np.zeros(nx) ocp.cost.yref = np.concatenate((x_ref, np.array([0.0, 0.0, 0.0, 0.0]))) ocp.cost.yref_e = x_ref # Initial state (will be overwritten) ocp.constraints.x0 = x_ref # Set constraints ocp.constraints.lbu = np.array([self.min_u] * 4) ocp.constraints.ubu = np.array([self.max_u] * 4) ocp.constraints.idxbu = np.array([0, 1, 2, 3]) # Solver options ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM' ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' ocp.solver_options.integrator_type = 'ERK' ocp.solver_options.print_level = 0 ocp.solver_options.nlp_solver_type = 'SQP_RTI' if solver_options is None else solver_options["solver_type"] # Compile acados OCP solver if necessary json_file = os.path.join(self.acados_models_dir, key_model.name + '_acados_ocp.json') self.acados_ocp_solver[key] = AcadosOcpSolver(ocp, json_file=json_file)