Tutorial 04: Feasibility Checking Using CommonRoad Vehicle Models

The feasibility checker library provides a convenient way to check whether a given trajectory respects the dynamics of a given vehicle model. Therefore, we try to compute an input trajectory which allows us to closely match the given state trajectory. We accept the state trajectory as feasible if the error (position and orientation) between the reconstructed and given trajectory is smaller than a predefined error value. This tutorial demonstrate how the feasibility checker can be used.

1 Necessary imports

We first import all necessary files for the feasibility check. In our example, we generate random trajectories for the kinematic single-track model and check those for feasiblity afterwards.

%config InlineBackend.figure_formats = ['svg']
# visualization and numpy
import numpy as np
import matplotlib.pyplot as plt
import copy
import random
import math

from commonroad_dc.feasibility.vehicle_dynamics import VehicleDynamics

# import vehicle parameters from CommonRoad benchmark suite
from commonroad.scenario.trajectory import Trajectory
from commonroad.scenario.state import InitialState, InputState, KSState
from commonroad.common.solution import VehicleType

2 Functionalities of the Feasibility Checker

2.1 Forward simulation of inputs

Our feasibility checker offers a method for forward simulation of a given input with respect to a vehicle model.

# specification of time step and vehicle model
# (Kinematics Single Track Model with FORD ESCORT parameters)
dt = 0.1
vehicle = VehicleDynamics.KS(VehicleType.FORD_ESCORT)

# create initial zero initial state (all entries are zero)
initial_state = InitialState(
    position=np.array([0.0, 0.0]),
    velocity=0.0,
    orientation=0.0,
    acceleration=0.0,
    yaw_rate=0.0,
    slip_angle=0.0,
    time_step=0
)

# create input with steering_angle=0 and acceleration=10
input = InputState(
    steering_angle_speed=0,
    acceleration=10,
    time_step=0
)

# forward simulation
next_state = vehicle.simulate_next_state(initial_state, input, dt)

# compare forward simulation with expected values
expected_pos_x = 0.05
expected_pos_y = 0.0
expected_steering_angle = 0.0
expected_velocity = 1
expected_orientation = 0.0

print('Error in position x: {}'.format(abs(expected_pos_x-next_state.position[0])))
print('Error in position y: {}'.format(abs(expected_pos_y-next_state.position[1])))
print('Error in steering angle: {}'.format(abs(expected_steering_angle-next_state.steering_angle)))
print('Error in velocity: {}'.format(abs(expected_velocity-next_state.velocity)))
print('Error in orientation: {}'.format(abs(expected_orientation-next_state.orientation)))

Error in position x: 7.450602773628745e-10
Error in position y: 0.0
Error in steering angle: 0.0
Error in velocity: 0.0
Error in orientation: 0.0

2.2 Checking the feasibility of state transitions

Our module can check the feasibility of a given state transition between two given states. To demonstrate the method, we create a random input for the kinematic single track model.

# import the feasibility checker
import commonroad_dc.feasibility.feasibility_checker as feasibility_checker

# specification of time step and vehicle model
# (parameters correspond to FORD ESCORT)
dt = 0.1
vehicle = VehicleDynamics.KS(VehicleType.FORD_ESCORT)

# create initial zero initial state (all entries are zero)
initial_state = vehicle.convert_initial_state(
    InitialState(position=np.array([0.0, 0.0]),
                 velocity=0.0,
                 orientation=0.0,
                 acceleration=0.0,
                 yaw_rate=0.0,
                 slip_angle=0.0,
                 time_step=0)
)

# create random input for the KS dynamics
input = InputState(
    acceleration=random.uniform(-11.5, 11.5),
    steering_angle_speed=random.uniform(-0.4, 0.4),
    time_step=0
)

# forward simulation
next_state = vehicle.simulate_next_state(initial_state, input, dt)

feasible, reconstructed_input = feasibility_checker.state_transition_feasibility(initial_state, next_state,
                                                                                 vehicle, dt)
print('State transition feasible? {}'.format(feasible)) # feasible if error is <= 2e-2

# Reconstruct the next state using the found input
reconstructed = vehicle.simulate_next_state(initial_state, reconstructed_input, dt)

# show reconstructed state transition
plt.figure()
plt.plot([initial_state.position[0],next_state.position[0]],
         [initial_state.position[1],next_state.position[1]],'-or')
plt.plot([initial_state.position[0],reconstructed.position[0]],
         [initial_state.position[1],reconstructed.position[1]],'-xk')
plt.axis('equal')
plt.xlabel('position x')
plt.ylabel('position y')
plt.legend(['Original','Traced'])
plt.show()

State transition feasible? True

2.3 Checking the feasibility of trajectories

Our module can check the feasibility of a given trajectory by iteratively calling the state transition check for each pair of states of the trajectory. To demonstrate the method, we create a random trajectory for the kinematic single track model.

# specification of time step and vehicle model
# (parameters correspond to FORD ESCORT)
dt = 0.1
vehicle = VehicleDynamics.KS(VehicleType.FORD_ESCORT)

# create initial zero initial state (all entries are zero)
initial_state = InitialState(
    position=np.array([0.0, 0.0]),
    velocity=0.0,
    orientation=0.0,
    acceleration=0.0,
    yaw_rate=0.0,
    slip_angle=0.0,
    time_step=0
)

# create random input vector and the trajectory (20 timesteps)
states = [vehicle.convert_initial_state(initial_state)]
inputs = []
time_step = 0
while len(inputs) < 20:
    # create a random input
    inp = InputState(acceleration=random.uniform(-11.5, 11.5),
                     steering_angle_speed=random.uniform(-0.4, 0.4),
                     time_step=time_step)

    # check if input violates constraints
    if vehicle.violates_friction_circle(states[-1], inp) or not vehicle.input_within_bounds(inp):
        continue  # if violates constraints, pass this input

    # if valid input, simulate next state
    next_state = vehicle.simulate_next_state(states[-1], inp, dt, throw=False)
    states.append(next_state)
    inputs.append(inp)
    time_step += 1


# create trajectory and input vector
input_vector = Trajectory(initial_time_step=0, state_list=inputs)
trajectory = Trajectory(initial_time_step=0, state_list=states)

# check feasibility of trajectory
feasible, reconstructed_inputs = feasibility_checker.trajectory_feasibility(trajectory, vehicle, dt)

# reconstruct the trajectory using the reconstructed input vector inputs step wise
reconstructed_states = [vehicle.convert_initial_state(initial_state)] + [
    vehicle.simulate_next_state(trajectory.state_list[idx], inp, dt)
    for idx, inp in enumerate(reconstructed_inputs.state_list)
]
trajectory_reconstructed = Trajectory(initial_time_step=0, state_list=reconstructed_states)

# reconstucting the trajectory using the reconstructed input vector directly is also possible, but
# it will create a trajectory that is similar but different to the real trajectory due to the small
# errors between the real inputs and the approximated inputs.

#trajectory_reconstructed = vehicle.simulate_trajectory(initial_state, reconstructed_inputs, dt)

# print and analyze result
print('Feasible? {}'.format(feasible))

error_state = len(trajectory_reconstructed.state_list)
if not feasible:
    print('Error found at state transition {} to {}'.format(error_state-2,error_state-1))

Feasible? True

## Plot Figures
fsize = (4,1.5)

# position figure
orig_pos = np.array([x.position for x in trajectory.state_list])
recon_pos = np.array([x.position for x in trajectory_reconstructed.state_list])

plt.figure(figsize=fsize)
plt.plot(orig_pos[:,0],orig_pos[:,1],'-or')
plt.plot(recon_pos[:,0],recon_pos[:,1],'-xk')
plt.legend(['Original','Reconstructed'],loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('position x')
plt.ylabel('position y')
plt.axis('equal')
plt.autoscale()
plt.show()

# velocity figure
orig_velocity = np.array([[x.time_step, x.velocity] for x in trajectory.state_list])
recon_velocity = np.array([[x.time_step, x.velocity] for x in trajectory_reconstructed.state_list])

plt.figure(figsize=fsize)
plt.plot(orig_velocity[:,0],orig_velocity[:,1],'-or')
plt.plot(recon_velocity[:,0],recon_velocity[:,1],'-xk')
plt.xlabel('time step k')
plt.ylabel('velocity v')
plt.legend(['Original','Reconstructed'],loc='center left', bbox_to_anchor=(1, 0.5))
plt.autoscale()
plt.show()

# orientation figure
orig_orientation = np.array([[x.time_step, x.orientation] for x in trajectory.state_list])
recon_orientation = np.array([[x.time_step, x.orientation] for x in trajectory_reconstructed.state_list])

plt.figure(figsize=fsize)
plt.plot(orig_orientation[:,0],orig_orientation[:,1],'-or')
plt.plot(recon_orientation[:,0],recon_orientation[:,1],'-xk')
plt.legend(['Original','Reconstructed'],loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('time step k')
plt.ylabel('orientation theta')
plt.autoscale()
plt.show()

# acceleration figure
orig_acc = np.array([[u.time_step, u.acceleration] for u in input_vector.state_list])
recon_acc = np.array([[u.time_step, u.acceleration] for u in reconstructed_inputs.state_list])

plt.figure(figsize=fsize)
plt.plot(orig_acc[:,0],orig_acc[:,1],'-or')
plt.plot(recon_acc[:,0],recon_acc[:,1],'-xk')
plt.legend(['Original','Reconstructed'],loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('time step k')
plt.ylabel('acceleration a')
plt.autoscale()
plt.show()

# steering angle speed figure
orig_steering_speed = np.array([[u.time_step, u.steering_angle_speed] for u in input_vector.state_list])
recon_steering_speed = np.array([[u.time_step, u.steering_angle_speed] for u in reconstructed_inputs.state_list])

plt.figure(figsize=fsize)
plt.plot(orig_steering_speed[:,0],orig_steering_speed[:,1],'-or')
plt.plot(recon_steering_speed[:,0],recon_steering_speed[:,1],'-xk')
plt.legend(['Original','Reconstructed'],loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('time step k')
plt.ylabel('steeering angle speed phi')
plt.autoscale()
plt.show()

# trajectory error figure
pos_error = np.array([
    [recon.time_step,
     np.sqrt(np.sum(np.square(np.subtract(trajectory.state_list[idx].position, recon.position))))]
    for idx, recon in enumerate(trajectory_reconstructed.state_list)
])

vel_error = np.array([
    [recon.time_step, abs(recon.velocity - trajectory.state_list[idx].velocity)]
    for idx, recon in enumerate(trajectory_reconstructed.state_list)
])

orient_error = np.array([
    [recon.time_step, abs(recon.orientation - trajectory.state_list[idx].orientation)]
    for idx, recon in enumerate(trajectory_reconstructed.state_list)
])

plt.figure(figsize=fsize)
plt.plot(pos_error[:, 0], pos_error[:, 1],'r')
plt.plot(orient_error[:, 0], orient_error[:, 1],'b')
plt.plot(pos_error[:, 0], np.repeat(2e-2, error_state),'-k')
plt.legend(['Position Error (m)', 'Orientation Error (r)', 'Error 2e-2'],
           loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('time step k')
plt.ylabel('computed error')
plt.autoscale()
plt.show()

# input vector error figure
acc_error = np.array([
    [recon.time_step, abs(recon.acceleration - input_vector.state_list[idx].acceleration)]
    for idx, recon in enumerate(reconstructed_inputs.state_list)
])

steering_speed_error = np.array([
    [recon.time_step, abs(recon.steering_angle_speed - input_vector.state_list[idx].steering_angle_speed)]
    for idx, recon in enumerate(reconstructed_inputs.state_list)
])

plt.figure(figsize=fsize)
plt.plot(acc_error[:, 0], acc_error[:,1], 'g')
plt.plot(steering_speed_error[:, 0], steering_speed_error[:,1], 'y')
plt.legend(['Acceleration Error (m/s2)', 'Steering Angle Speed Error (r/s)'],
           loc='center left', bbox_to_anchor=(1, 0.5))
plt.xlabel('time step k')
plt.ylabel('computed error')
plt.autoscale()
plt.show()