trailer_controller.horizon = number_of_steps trailer_controller.step_size = step_size trailer_controller.integrator_casadi = True # generate the code trailer_controller.generate_code() ## -- simulate controller -- # setup a simulator to test sim = tools.Simulator(trailer_controller.location) initial_state=np.array([0.,0.,0.]) state=initial_state state_history = np.zeros((number_of_states,number_of_steps)) result_simulation = sim.simulate_nmpc(state,reference_state,reference_input) for i in range(1,number_of_steps): print("The optimal input is: [" + str(result_simulation.optimal_input[0]) + "," + str(result_simulation.optimal_input[0]) + "]") state = model.get_next_state_numpy(state,result_simulation.optimal_input) state_history[:,i] = np.reshape(state[:],number_of_states) print(state_history[:,0:5]) plt.figure(1) example_models.trailer_print(state_history) plt.xlim([0, 2.5]) plt.ylim([0, 2.5]) plt.show()
def generate_controller_with_obs(trailer_controller_location, reference_state, Q, R, rectangular_obstacle_1, obstacle_weight, horizon, display_figure=True, index_figure=0): # get the continious system equations (system_equations, number_of_states, number_of_inputs, coordinates_indices) = example_models.get_trailer_model(L=0.5) step_size = 0.05 # simulation_time = 10 # number_of_steps = math.ceil(simulation_time / step_size) integrator = "RK44" constraint_input = cfunctions.IndicatorBoxFunction( [-1, -1], [1, 1]) # input needs stay within these borders model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states,\ number_of_inputs,coordinates_indices, integrator) # reference_state=np.array([2,2,0]) stage_cost = controller.Stage_cost_QR(model, Q, R) # define the controller trailer_controller = controller.Nmpc_panoc(trailer_controller_location, model, stage_cost) trailer_controller.horizon = horizon trailer_controller.step_size = step_size trailer_controller.integrator_casadi = True trailer_controller.panoc_max_steps = 1000 trailer_controller._lbgfs_buffer_size = 20 trailer_controller.min_residual = -5 # add an obstacle trailer_controller.add_obstacle(rectangular_obstacle_1) # generate the code trailer_controller.generate_code() # -- simulate controller -- # setup a simulator to test sim = tools.Simulator(trailer_controller.location) initial_state = np.array([0.01, 0., 0.]) state = initial_state state_history = np.zeros((number_of_states, horizon)) sim.set_weight_obstacle(0, obstacle_weight) reference_input = np.array([0, 0]) (sim_data, full_solution) = sim.simulate_nmpc_multistep_solution( initial_state, reference_state, reference_input, number_of_inputs * horizon) inputs = np.reshape(full_solution, (horizon, number_of_inputs)) print("solved NMPC problem time=" + sim_data.time_string + " number of panoc iterations=" + str(sim_data.panoc_interations)) for i in range(0, horizon): state = model.get_next_state_numpy(state, inputs[i, :]) state_history[:, i] = np.reshape(state[:], number_of_states) print("Reference state:") print(reference_state) print("Final state:") print(state) if (display_figure == True): plt.figure(index_figure) example_models.trailer_print(state_history) rectangular_obstacle_1.plot() plt.xlim([-2.2, 2.2]) plt.ylim([-0.1, 2.2]) # plt.clf() return state
inputs = np.reshape(full_solution[0:horizon * number_of_inputs], (horizon, number_of_inputs)) print("The optimal inputs are:") print(inputs) # get the intermediate states intermediate_states = np.reshape(full_solution[horizon * number_of_inputs:], (horizon - 1, number_of_states)) print("The intermediate states used by the multiple shot are:") print(intermediate_states) # use the inputs to simulate the system state = initial_state state_history = np.zeros((number_of_states, horizon)) for i in range(0, horizon): optimal_input = inputs[i, :].T state = model.get_next_state_numpy(state, optimal_input) state_history[:, i] = np.reshape(state[:], number_of_states) plt.figure(0) example_models.trailer_print(intermediate_states.T, color="r") example_models.trailer_print(state_history, color="g") # rectangle.plot() plt.xlim([0, 2.5]) plt.ylim([0, 0.7]) plt.xlabel('x') plt.xlabel('y') plt.title('Trailer parcour') plt.show()
# generate the dynamic code trailer_controller.generate_code() # simulate everything initial_state = np.array([0., 0.5,0]) reference_state = np.array([3, 0.5, 0]) reference_input = np.array([0, 0]) sim = tools.Simulator(trailer_controller.location) obstacle_weights = [10.] state_history_1 = calculate_horizon(trailer_controller,sim,initial_state,reference_state,reference_input,obstacle_weights) # obstacle_weights = [10.**3] # state_history_2 = calculate_horizon(trailer_controller,sim,initial_state,reference_state,reference_input,obstacle_weights) obstacle_weights = [10.**9] state_history_3 = calculate_horizon(trailer_controller,sim,initial_state,reference_state,reference_input,obstacle_weights) plt.figure(0) example_models.trailer_print(state_history_1,'k') example_models.trailer_print(state_history_2,'r') example_models.trailer_print(state_history_3,'g') # rectangular_obstacle.plot() circular_obstacle.plot() plt.xlim([-0.1, 4]) plt.ylim([-0.1, 1.1]) plt.xlabel('x') plt.xlabel('y') plt.title('Trailer') plt.show()
def generate_controller(controller_name, reference_state, horizon, panoc_max_steps, display_figure=True): ## -- GENERATE STATIC FILES -- # start by generating the static files and folder of the controller location_nmpc_repo = "../../.." location = location_nmpc_repo + "/test_controller_builds" # controller_name = "trailer_simple_controller" trailer_controller_location = location + "/" + controller_name + "/" tools.Bootstrapper.bootstrap(trailer_controller_location, simulation_tools=True) ## ----------------------------------------------------------------- # get the continious system equations (system_equations, number_of_states, number_of_inputs, coordinates_indices) = example_models.get_trailer_model(L=0.5) step_size = 0.05 simulation_time = 10 number_of_steps = math.ceil(simulation_time / step_size) integrator = "RK44" constraint_input = cfunctions.IndicatorBoxFunction( [-1, -1], [1, 1]) # input needs stay within these borders model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states,\ number_of_inputs,coordinates_indices, integrator) Q = np.diag([1., 100., 1.]) R = np.eye(model.number_of_inputs, model.number_of_inputs) * 0.1 # reference_state=np.array([2,2,0]) stage_cost = controller.Stage_cost_QR(model, Q, R) # define the controller trailer_controller = controller.Nmpc_panoc(trailer_controller_location, model, stage_cost) trailer_controller.horizon = horizon trailer_controller.step_size = step_size trailer_controller.integrator_casadi = True trailer_controller.panoc_max_steps = panoc_max_steps trailer_controller._lbgfs_buffer_size = 30 trailer_controller.min_residual = -3 # generate the code trailer_controller.generate_code() # -- simulate controller -- # setup a simulator to test sim = tools.Simulator(trailer_controller.location) initial_state = np.array([0.01, 0., 0.]) state = initial_state state_history = np.zeros((number_of_states, number_of_steps)) reference_input = np.array([0, 0]) for i in range(1, number_of_steps): result_simulation = sim.simulate_nmpc(state, reference_state, reference_input) print("Step [" + str(i) + "/" + str(number_of_steps) + "]: The optimal input is: [" \ + str(result_simulation.optimal_input[0]) + "," + str(result_simulation.optimal_input[0]) + "]" \ + " time=" + result_simulation.time_string + " number of panoc iterations=" + str( result_simulation.panoc_interations)) sys.stdout.flush() state = model.get_next_state_numpy(state, result_simulation.optimal_input) state_history[:, i] = np.reshape(state[:], number_of_states) print("Final state:") print(state) if (display_figure == True): plt.figure(0) example_models.trailer_print(state_history) plt.xlim([-2.2, 2.2]) plt.ylim([-0.1, 2.2]) plt.show() plt.savefig(controller_name + '.png') plt.clf() return state