def test_mpc(waypoints, vehicle, result, plot):
vehicle_pose = np.array([vehicle['x'], vehicle['y'], vehicle['theta']])
waypoints_x, waypoints_y = waypoints
path = Path(waypoints_x, waypoints_y)
spatial_bicycle_model = SpatialBicycleModel()
Q = sparse.diags([1., 1.])
Qn = Q
R = 0.1*sparse.eye(1)
prediction_horizon = 2
kappa_tilde_min = -100
kappa_tilde_max = 100
wheelbase = 1
mpc = MPC(Q, R, Qn, prediction_horizon,
kappa_tilde_min, kappa_tilde_max, wheelbase)
mpc.setup_optimization_problem(
spatial_bicycle_model, vehicle_pose, path.as_array_with_curvature())
steering_angle = mpc.compute_steering_angle(
spatial_bicycle_model, vehicle_pose, path.as_array_with_curvature())
assert steering_angle == result['steering_angle']
wheelbase = 1
max_velocity = 10
max_steering_angle = math.pi
initial_state = np.array([0, 0, math.pi / 2])
vehicle = Vehicle(
wheelbase=wheelbase, initial_state=initial_state, dt=dt,
max_velocity=max_velocity, max_steering_angle=max_steering_angle)
vehicle_x = np.append(initial_state[0], np.zeros(simulation_steps - 1))
vehicle_y = np.append(initial_state[1], np.zeros(simulation_steps - 1))
steering_angles = np.zeros(simulation_steps)
spatial_bicycle_model = SpatialBicycleModel(
path.as_array_with_curvature(), vehicle.state)
mpc = MPC(
Q=Q, R=R, Qn=Qn, prediction_horizon=prediction_horizon,
steering_angle_min=np.radians(-16),
steering_angle_max=np.radians(16),
wheelbase=wheelbase)
mpc.setup_optimization_problem(spatial_bicycle_model)
for k in range(1, simulation_steps):
spatial_bicycle_model.update(path.as_array_with_curvature(), vehicle.state)
steering_angle, predicted_poses = mpc.compute_steering_angle(
spatial_bicycle_model
)