Beispiel #1
0
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
Beispiel #2
0
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
Beispiel #3
0
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')
Beispiel #4
0
def main():
    (system_equations, number_of_states, number_of_inputs, coordinates_indices) = \
        nmpc.example_models.get_trailer_model(L=0.5)

    test_generator = controller.Globals_generator("./test_globals.c")

    step_size = 0.1
    integrator = "RK"
    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) * 1.

    stage_cost = controller.stage_costs.Stage_cost_QR(model, Q, R)

    # define the controller, set the controller on whatever location, as we won't generate code it doesnt matter
    trailer_controller = controller.Nmpc_panoc("./", model, stage_cost)

    test_generator.generate_globals(trailer_controller)
## -- GENERATE STATIC FILES --
# start by generating the static files and folder of the controller
location="../../../test_controller_builds/trailer_simple_controller"
tools.Bootstrapper.bootstrap(location,python_interface_enabled=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.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()
Beispiel #6
0
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