# - "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)
#### And a run file (code below)
#######################################################################################################
#######################################################################################################
# simulate everything
initial_state = np.array([0.6, -0.1,math.pi/4])
reference_input = np.array([0, 0]) # lambda i : np.array([1.5+0.02*i, 0.7, 0.1])
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
trailer_controller.generate_code()
# simulate everything
initial_state = np.array([-0.1, -0.1, math.pi / 4])
reference_state = np.array([1.5, 0.4, 0])
reference_input = np.array([0, 0])
obstacle_weights = [1e3, 1e3, 1e3]
Q_terminal = np.diag([1., 1., 1]) * 10
R_terminal = np.diag([1., 1.]) * 0.01
trailer_controller = prepare_demo_trailer(step_size, Q, R, Q_terminal,
R_terminal)
trailer_controller.horizon = 40 # 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 left circle
circle1 = obstacles.Circular(trailer_controller.model, np.array([1.5, 0.]),
1.)
circle2 = obstacles.Circular(trailer_controller.model, np.array([3.5, 2.]),
0.6)
circle3 = obstacles.Circular(trailer_controller.model, np.array([2., 2.5]),
0.8)
circle4 = obstacles.Circular(trailer_controller.model, np.array([5., 4.]),
1.05)
# add obstacles to controller
trailer_controller.add_constraint(circle1)
trailer_controller.add_constraint(circle2)
trailer_controller.add_constraint(circle3)
trailer_controller.add_constraint(circle4)
# generate the dynamic code
trailer_controller.generate_code()
