def prepare_demo_trailer(step_size, Q, R, Q_terminal=None, R_terminal=None): "construct a simple demo controller" # generate static files trailer_controller_output_location = "../test_controller_builds/demo_controller" tools.Bootstrapper.bootstrap(trailer_controller_output_location, simulation_tools=True) # get example model from lib (system_equations, number_of_states, number_of_inputs, coordinates_indices) = nmpc.example_models.get_trailer_model(L=0.5) integrator = "RK44" # select a Runga-Kutta integrator (FE is forward euler) constraint_input = cfunctions.IndicatorBoxFunction( [-4, -4], [4, 4]) # 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) # define the contro stage_cost = controller.Stage_cost_QR(model, Q, R) if (Q_terminal is None): trailer_controller = controller.Nmpc_panoc( trailer_controller_output_location, model, stage_cost) else: terminal_cost = controller.Stage_cost_QR(model, Q_terminal, R_terminal) trailer_controller = controller.Nmpc_panoc( trailer_controller_output_location, model, stage_cost, terminal_cost) return trailer_controller
def prepare_robot(step_size,Q,R,Q_terminal=None,R_terminal=None): "construct a simple demo controller" # generate static files robot_controller_output_location = "../robot_controller_builds/robot_controller" tools.Bootstrapper.bootstrap(robot_controller_output_location, simulation_tools=True) # get example model from lib # Take get_robot_model for Robosavvy # get_trailer_model is for original model (system_equations, number_of_states, number_of_inputs, coordinates_indices,number_of_rect,number_of_circle) = nmpc.example_models.get_robot_model() integrator = "RK44" # select a Runga-Kutta integrator (FE is forward euler) # Sets the upper and lower bounds of the inputs and stores them constraint_input = cfunctions.IndicatorBoxFunction([-1, -2], [1, 2]) # 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,number_of_rect,number_of_circle) # define the contro stage_cost = controller.Stage_cost_QR(model, Q, R) if(Q_terminal is None): robot_controller = controller.Nmpc_panoc(robot_controller_output_location, model, stage_cost) else: terminal_cost = controller.Stage_cost_QR(model, Q_terminal, R_terminal) robot_controller = controller.Nmpc_panoc(robot_controller_output_location, model, stage_cost,terminal_cost) return robot_controller
def main(): (system_equations, number_of_states, number_of_inputs, indices_coordinates) = nmpc.example_models.get_chain_model() dimension = 2 number_of_balls = 4 step_size = 0.01 simulation_time = 5 number_of_steps = math.ceil(simulation_time / step_size) integrator = "RK44" constraint_input = cfunctions.IndicatorBoxFunction( [-2, -2], [2, 2]) # input needs stay within these borders model = models.Model_continious(system_equations, constraint_input, step_size, number_of_states, \ number_of_inputs,indices_coordinates, integrator) # Q = np.eye(model.number_of_states, model.number_of_states) Q = np.diag([0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1]) * 10 R = np.eye(model.number_of_inputs, model.number_of_inputs) # reference_state=np.array([2,2,0]) stage_cost = controller.Stage_cost_QR(model, Q, R) # define the controller controller_location = "./" nmpc_controller = controller.Nmpc_panoc(controller_location, model, stage_cost) nmpc_controller.horizon = 50 nmpc_controller.step_size = 0.1 # add the optional integrator, very useful to debug the code nmpc_controller.integrator_casadi = True nmpc_controller.generate_code() # replace the panoc dimension by 2 dyn_globals_location = './globals/globals_dyn.h' replaceAll(dyn_globals_location, 'DIMENSION_PANOC 100', 'DIMENSION_PANOC 2')
step_size = 0.1 simulation_time = 5 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,1,1]) R = np.eye(model.number_of_inputs, model.number_of_inputs) reference_state=np.array([2,2,0]) reference_input=np.array([0,0]) stage_cost = controller.Stage_cost_QR(model, Q, R) # define the controller trailer_controller = controller.Nmpc_panoc(location,model,stage_cost ) 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
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
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