# Object buffer is at the moment: 10 objects
# Can be adjusted in "example_model.py"
# Files needed to be adjusted for extra shapes:
# - "model.py"
# - "model_continuous.py"
# - "nmpc.c" -> in kernel
# - "nmpc_python.c" -> in kernel
# - "nmpc_problem_single_shot.py"
# - "simulator.py" -> "def simulate_nmpc"
for i in range(robot_controller.model.number_of_rect):
center = casadi.SX.sym("obstacle_rect_center" + str(i),2)
width = casadi.SX.sym("obstacle_rect_width" + str(i))
height = casadi.SX.sym("obstacle_rect_height" + str(i))
# construct rectangular
robot_controller.add_constraint(obstacles.Rectangular(robot_controller.model,center,width,height))
for i in range(robot_controller.model.number_of_circle):
center = casadi.SX.sym("obstacle_circle_center" + str(i),2)
radius = casadi.SX.sym("obstacle_radius_width" + str(i))
# construct circle
robot_controller.add_constraint(obstacles.Circular(robot_controller.model,center,radius))
# generate the dynamic code
robot_controller.generate_code()
#######################################################################################################
#######################################################################################################
#### All the above is only needed for the initialisation of the code
#### Future file should contain a build file(the code above)
# Q and R matrixes determined by the control engineer.
Q = np.diag([1., 1., 0.0]) * 0.2
R = np.diag([1., 1.]) * 0.01
trailer_controller = prepare_demo_trailer(step_size, Q, R)
trailer_controller.horizon = 50 # NMPC parameter
trailer_controller.integrator_casadi = True # optional feature that can generate the integrating used in the cost function
trailer_controller.panoc_max_steps = 2000 # the maximum amount of iterations the PANOC algorithm is allowed to do.
trailer_controller.min_residual = -3
trailer_controller.lbgfs_buffer_size = 50
# trailer_controller.pure_prox_gradient=True
# construct upper rectangular
rectangular_up = obstacles.Rectangular(trailer_controller.model,
np.array([1, 0.5]), 0.4, 0.5)
# construct lower rectangular
rectangular_down = obstacles.Rectangular(trailer_controller.model,
np.array([1, -0.2]), 0.4, 0.5)
# construct circle
circle = obstacles.Circular(trailer_controller.model, np.array([0.2, 0.2]),
0.2)
# add obstacles to controller
trailer_controller.add_constraint(rectangular_up)
trailer_controller.add_constraint(rectangular_down)
trailer_controller.add_constraint(circle)
# generate the dynamic code
# Q and R matrixes determined by the control engineer.
Q = np.diag([1., 1., 0.01]) * 0.2
R = np.diag([1., 1.]) * 0.01
trailer_controller = prepare_demo_trailer(step_size, Q, R)
trailer_controller.horizon = 30 # NMPC parameter
trailer_controller.integrator_casadi = True # optional feature that can generate the integrating used in the cost function
trailer_controller.panoc_max_steps = 500 # the maximum amount of iterations the PANOC algorithm is allowed to do.
trailer_controller.min_residual = -3
# construct lower rectangular
rectangular_center_coordinates = np.array([0.45, -0.1])
rectangular_width = 0.4
rectangular_height = 0.1
rectangular = obstacles.Rectangular(trailer_controller.model,rectangular_center_coordinates,\
rectangular_width,rectangular_height)
# construct left circle
left_circle = obstacles.Circular(trailer_controller.model,
np.array([0.2, 0.2]), 0.2)
# construct right circle
right_circle = obstacles.Circular(trailer_controller.model,
np.array([0.7, 0.2]), 0.2)
# add obstacles to controller
trailer_controller.add_constraint(rectangular)
trailer_controller.add_constraint(left_circle)
trailer_controller.add_constraint(right_circle)
# generate the dynamic code
