# - "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()