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)
