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')
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()
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
