from demo import prepare_demo_trailer,simulate_demo,calculate_horizon if __name__ == '__main__': step_size=0.05 # 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 # Q_terminal = np.diag([1., 1., 0.01])*100 # R_terminal = np.diag([1., 1.]) * 0.01 trailer_controller = prepare_demo_trailer(step_size,Q,R) # trailer_controller = prepare_demo_trailer(step_size, Q, R,Q_terminal,Q_terminal) 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([2,0.5]) rectangular_width = 0.4 rectangular_height = 0.4 rectangular_obstacle = obstacles.Obstacle_rectangular(rectangular_center_coordinates,\ rectangular_width,rectangular_height) circular_obstacle = obstacles.Obstacle_circle(rectangular_center_coordinates,0.5)
import matplotlib.pyplot as plt import math from demo import prepare_demo_trailer, simulate_demo, draw_obstacle_border if __name__ == '__main__': step_size = 0.025 # Q and R matrixes determined by the control engineer. Q = np.diag([1., 1., 0.1]) * 2 R = np.diag([1., 1.]) * 0.01 Q_terminal = np.diag([1., 1., 0.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 = 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 = 10 # trailer_controller.pure_prox_gradient = True # this will ignore the lbgfs, only usefull when testing !!! # construct upper rectangular costum_obstacle = obstacles.Nonconvex_constraints(trailer_controller.model) h_0 = lambda x: x[1] - 2. * math.sin(-x[0] / 2.) h_1 = lambda x: 3. * math.sin(x[0] / 2 - 1) - x[1] h_2 = lambda x: x[0] - 1 h_3 = lambda x: 8 - x[0] costum_obstacle.add_constraint(h_0)