Python Path.as_array_with_curvature Examples

Example #1

File: test_controllers.py Project: Javiercerna/navigation-python

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']

Example #2

File: run_controller_mpc.py Project: Javiercerna/navigation-python

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
    )