Module feasibility¶
feasibility_checker¶
- position_orientation_objective(u: numpy.array, x0: numpy.array, x1: numpy.array, dt: float, vehicle_dynamics: commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics, ftol: float = 1e-08, e: numpy.array = array([0.02, 0.02, 0.03])) float[source]¶
Position-Orientation objective function to be minimized for the state transition feasibility.
Simulates the next state using the inputs and calculates the norm of the difference between the simulated next state and actual next state. Position, velocity and orientation state fields will be used for calculation of the norm.
- Parameters
u – input values
x0 – initial state values
x1 – next state values
dt – delta time
vehicle_dynamics – the vehicle dynamics model to be used for forward simulation
ftol – ftol parameter used by the optimizer
e – error margin, function will return norm of the error vector multiplied with 100 as cost if the input violates the friction circle constraint or input bounds.
- Returns
cost
- position_orientation_feasibility_criteria(x: numpy.array, x_sim: numpy.array, vehicle_dynamics: commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics, e: numpy.array = array([0.02, 0.02, 0.03]), d: int = 4) bool[source]¶
Position-Orientation feasibility criteria to be checked between the real next state and the simulated next state in the state transition feasibility testing after a valid input has been found.
Checks whether the position and orientation difference is within acceptable between actual state and simulated state.
- Parameters
x – real next state
x_sim – simulated next state
vehicle_dynamics – the vehicle dynamics model to be used for forward simulation
e – error margin, function will return False if the positional difference between the simulated next state and the actual next state is bigger then error margin.
d – decimal points where the difference values are rounded up to in order to avoid floating point errors set it based on the error margin, i.e e=0.02, d=3
- Returns
True if the positional difference is below error margin.
- state_transition_feasibility(x0: commonroad.scenario.trajectory.State, x1: commonroad.scenario.trajectory.State, vehicle_dynamics: commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics, dt: float, objective=<function position_orientation_objective>, criteria=<function position_orientation_feasibility_criteria>, ftol: float = 1e-08, e: numpy.array = array([0.02, 0.02, 0.03]), d: int = 4, maxiter: int = 100, disp: bool = False) Tuple[bool, commonroad.scenario.trajectory.State][source]¶
Checks if the state transition is feasible between given two state according to the vehicle dynamics.
Tries to find a valid input for the state transition by minimizing the objective function, and then checks if the state simulated by using the reconstructed input is feasible.
By default, the trajectory feasibility checker will use position-orientation objective function as the objective and position-orientation feasibility criteria function will be used for feasibility criteria.
Objectives can be changed by passing a function with the signature fun(u: np.array, x0: np.array, x1: np.array, dt: float, vehicle_dynamics: VehicleDynamics, ftol: float = 1e-8, e: np.array -> float
Feasibility criteria can be changed by passing a function with the signature fun(x: np.array, x_sim: np.array, vehicle_dynamics: VehicleDynamics, e: np.array = np.array([2e-2, 2e-2, 3e-2]), d: int = 4) -> bool
- Parameters
x0 – initial state
x1 – next state
vehicle_dynamics – the vehicle dynamics model to be used for forward simulation
dt – delta time
objective – callable fun(u, x0, x1, dt, vehicle_dynamics) -> float, objective function to be minimized in order to find a valid input for state transition
criteria – callable fun(x1, x_sim, vehicle_dynamics) -> bool, feasibility criteria to be checked between the real next state and the simulated next state
ftol – ftol passed to the minimizer function
e – error margin passed to the feasibility criteria function
d – decimal points where the difference values are rounded up to in order to avoid floating point errors set it based on the error margin, i.e e=0.02, d=4
maxiter – maxiter passed to the minimizer function
disp – disp passed to the minimizer function
- Returns
True if feasible, and the constructed input as State
- trajectory_feasibility(trajectory: commonroad.scenario.trajectory.Trajectory, vehicle_dynamics: commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics, dt: float, objective=<function position_orientation_objective>, criteria=<function position_orientation_feasibility_criteria>, ftol: float = 1e-08, e: numpy.array = array([0.02, 0.02, 0.03]), d: int = 4, maxiter: int = 100, disp: bool = False) Tuple[bool, commonroad.scenario.trajectory.Trajectory][source]¶
Checks if the given trajectory is feasible for the vehicle model by checking if the state transition is feasible between each consecutive state of the trajectory.
The state_transition_feasibility function will be applied to consecutive states of a given trajectory, and the reconstructed inputs will be returned as Trajectory object. If the trajectory was not feasible, reconstructed inputs up to infeasible state will be returned.
ATTENTION: Reconstructed inputs are just approximated inputs for the forward simulation between consecutive states n and n+1. Simulating full trajectory from the initial state by using the reconstructed inputs can result in a different (but similar) trajectory compared to the real one. The reason for this is the small differences between the approximate inputs and the real inputs adding up as we simulate further from the initial state.
By default, the trajectory feasibility checker will use position-orientation objective function as the objective and position-orientation feasibility criteria function will be used for feasibility criteria.
Objectives can be changed by passing a function with the signature fun(u: np.array, x0: np.array, x1: np.array, dt: float, vehicle_dynamics: VehicleDynamics, ftol: float = 1e-8, e: np.array) -> float
Feasibility criteria can be changed by passing a function with the signature fun(x: np.array, x_sim: np.array, vehicle_dynamics: VehicleDynamics, e: np.array = np.array([2e-2, 2e-2, 3e-2]), d: int = 4) -> bool
- Parameters
trajectory – trajectory
vehicle_dynamics – the vehicle dynamics model to be used for forward simulation
dt – delta time
objective – callable fun(u, x0, x1, dt, vehicle_dynamics) -> float, objective function to be minimized in order to find a valid input for state transition
criteria – callable fun(x1, x_sim, vehicle_dynamics) -> bool, feasibility criteria to be checked between the real next state and the simulated next state
ftol – ftol passed to the minimizer function
e – error margin passed to the feasibility criteria function
d – decimal points where the difference values are rounded up to in order to avoid floating point errors set it based on the error margin, i.e e=0.02, d=4
maxiter – maxiter passed to the minimizer function
disp – disp passed to the minimizer function
- Returns
True if feasible, and list of constructed inputs as Trajectory object
- input_vector_feasibility(initial_state: commonroad.scenario.trajectory.State, input_vector: commonroad.scenario.trajectory.Trajectory, vehicle_dynamics: commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics, dt: float) Tuple[bool, commonroad.scenario.trajectory.Trajectory][source]¶
Checks whether the given input vector (as Trajectory object) is feasible according to the input and state constraints.
The input bounds and friction circle constraint of corresponding vehicle model is being used as criteria of validity. During the process of feasibility checking, the trajectory will be simulated for the given initial state and input vector. If there is an infeasible input, all the trajectory states simulated up to that input will be returned instead.
For example, if we have initial state wth time step 0, and valid input vector that contains 20 inputs, the trajectory will be completely simulated and returned together with the feasibility result. If we have an input vector that is valid up to 5th input, then the trajectory will be simulated up to 5th time step, but 6th time step will not be simulated since the input is not feasible.
- Parameters
initial_state – initial state which the input vector will be applied
input_vector – input vector s Trajectory object
vehicle_dynamics – the vehicle dynamics model to be used for input constraint checks
dt – delta time
- Returns
True if feasible, and simulated trajectory.
solution_checker¶
- exception SolutionCheckerException[source]¶
Main exception class for exceptions related to the solution checker.
- exception GoalNotReachedException[source]¶
Exception class for exceptions related to the goal checks.
- exception MissingSolutionException[source]¶
Exception class for exceptions related to the planning problem solutions.
- solution_feasible(solution: commonroad.common.solution.Solution, dt: float, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet) Dict[int, Tuple[bool, commonroad.scenario.trajectory.Trajectory, commonroad.scenario.trajectory.Trajectory]][source]¶
Checks whether the given solution is feasible.
- Parameters
solution – Solution
planning_problem_set – PlanningProblemSet
dt – Scenario dt
- Returns
planning problem id -> feasible, input or reconstructed input, trajectory or simulated trajectory. Raises FeasibilityException if there is any trajectory in the solution that is not feasible.
- obstacle_collision(scenario: commonroad.scenario.scenario.Scenario, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) bool[source]¶
Checks whether there is a collision between the ego vehicles of the solution and the scenario obstacles.
- Parameters
scenario – Scenario
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
False if there is no collision. Raises CollisionException if there are any obstacle collisions.
- boundary_collision(scenario: commonroad.scenario.scenario.Scenario, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) bool[source]¶
Checks whether the ego vehicles go out of lanelet boundaries.
- Parameters
scenario – Scenario
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
False if the ego vehicles stay in lanes. Raises CollisionException if there are any boundary collisions.
- ego_collision(scenario: commonroad.scenario.scenario.Scenario, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) bool[source]¶
Checks whether the ego vehicles collide with each other in the solution.
- Parameters
scenario – Scenario
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
False if there is no collision. Raises CollisionException if there are any collisions between ego vehicles.
- goal_reached(scenario: commonroad.scenario.scenario.Scenario, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) bool[source]¶
Checks whether the goal has been reached for each of the planning problem solutions.
- Parameters
scenario – Scenario
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
True if all reached goal. Raises GoalNotReachedException if there are any planning problem solutions that have not reached the goal position.
- solved_all_problems(planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) bool[source]¶
Checks whether the solution has solved all planning problems of the scenario.
- Parameters
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
True if there is a solution for each planning problem. Raises MissingSolutionException if there are any missing planning problem solutions.
- starts_at_correct_state(solution: commonroad.common.solution.Solution, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet) bool[source]¶
Checks whether the given solution trajectories or input vectors start at the correct state/time step.
Expected time steps are: - Equal to planning problem’s initial time step, if input vector solution - Planning problem’s initial state, if trajectory solution
- Parameters
solution – Solution
planning_problem_set – PlanningProblemSet
- Returns
True if all solution trajectories start at correct time step
- valid_solution(scenario: commonroad.scenario.scenario.Scenario, planning_problem_set: commonroad.planning.planning_problem.PlanningProblemSet, solution: commonroad.common.solution.Solution) Tuple[bool, Dict[int, Tuple[bool, commonroad.scenario.trajectory.Trajectory, commonroad.scenario.trajectory.Trajectory]]][source]¶
- Checks whether a solution is valid or not by checking
Solution has solved all planning problems of the scenario (solved_all_problems)
All planning problem solutions reached goal (goal_reached)
All planning problem solutions trajectories/input vectors start at correct time step (starts_at_correct_ts)
All planning problem solutions are feasible (solution_feasible)
There isn’t a collision between the ego vehicles and the scenario obstacles (obstacle_collision)
The ego vehicles don’t go out of lane boundaries (boundary_collision)
The ego vehicles don’t collide with each other (ego_collision)
It returns a dictionary that contains the inputs (or reconstructed inputs if trajectory solution), and trajectory (simulated trajectory if input vector solution).
The valid_solution functions is being used as a basis for validity when a submission was made to commonroad.in.tum.de website for evaluation. If the solution is not valid according to this function, then the solution will not be accepted.
- Parameters
scenario – Scenario
planning_problem_set – PlanningProblemSet
solution – Solution
- Returns
True if all checks pass successfully, and dictionary for simulated trajectories or reconstructed inputs. Raises SolutionCheckerException if any of the checks fail.
vehicle_dynamics¶
- class VehicleParameterMapping(value)[source]¶
Mapping for VehicleType name to VehicleParameters
- FORD_ESCORT = <vehiclemodels.vehicle_parameters.VehicleParameters object>¶
- BMW_320i = <vehiclemodels.vehicle_parameters.VehicleParameters object>¶
- VW_VANAGON = <vehiclemodels.vehicle_parameters.VehicleParameters object>¶
- class VehicleDynamics(vehicle_model: commonroad.common.solution.VehicleModel, vehicle_type: commonroad.common.solution.VehicleType)[source]¶
VehicleDynamics abstract class that encapsulates the common methods of all VehicleDynamics classes.
- List of currently implemented vehicle models
Point-Mass Model (PM)
Kinematic Single-Track Model (KS)
Single-Track Model (ST)
Multi-Body Model (MB)
New types of VehicleDynamics can be defined by extending this class. If there isn’t any mismatch with the state values, the new VehicleDynamics class can be used directly with the feasibility checkers as well.
For detailed documentation of the Vehicle Models, please check the Vehicle Model Documentation
- classmethod PM(vehicle_type: commonroad.common.solution.VehicleType) commonroad_dc.feasibility.vehicle_dynamics.PointMassDynamics[source]¶
Creates a PointMassDynamics model.
- Parameters
vehicle_type – VehicleType, i.e. VehileType.FORD_ESCORT
- Returns
PointMassDynamics instance with the given vehicle type.
- classmethod KS(vehicle_type: commonroad.common.solution.VehicleType) commonroad_dc.feasibility.vehicle_dynamics.KinematicSingleTrackDynamics[source]¶
Creates a KinematicSingleTrackDynamics model.
- Parameters
vehicle_type – VehicleType, i.e. VehileType.FORD_ESCORT
- Returns
KinematicSingleTrackDynamics instance with the given vehicle type.
- classmethod ST(vehicle_type: commonroad.common.solution.VehicleType) commonroad_dc.feasibility.vehicle_dynamics.SingleTrackDynamics[source]¶
Creates a SingleTrackDynamics VehicleDynamics model.
- Parameters
vehicle_type – VehicleType, i.e. VehileType.FORD_ESCORT
- Returns
SingleTrackDynamics instance with the given vehicle type.
- classmethod MB(vehicle_type: commonroad.common.solution.VehicleType) commonroad_dc.feasibility.vehicle_dynamics.MultiBodyDynamics[source]¶
Creates a MultiBodyDynamics VehicleDynamics model.
- Parameters
vehicle_type – VehicleType, i.e. VehileType.FORD_ESCORT
- Returns
MultiBodyDynamics instance with the given vehicle type.
- classmethod from_model(vehicle_model: commonroad.common.solution.VehicleModel, vehicle_type: commonroad.common.solution.VehicleType) commonroad_dc.feasibility.vehicle_dynamics.VehicleDynamics[source]¶
Creates a VehicleDynamics model for the given vehicle model and type.
- Parameters
vehicle_model – VehicleModel, i.e. VehicleModel.KS
vehicle_type – VehicleType, i.e. VehileType.FORD_ESCORT
- Returns
VehicleDynamics instance with the given vehicle type.
- abstract dynamics(t, x, u) List[float][source]¶
Vehicle dynamics function that models the motion of the vehicle during forward simulation.
- Parameters
t – time point which the differentiation is being calculated at.
x – state values
u – input values
- Returns
next state values
- property input_bounds: scipy.optimize._constraints.Bounds¶
Returns the bounds on inputs (constraints).
- Bounds are
min steering velocity <= steering_angle_speed <= max steering velocity
-max longitudinal acc <= acceleration <= max longitudinal acc
- Returns
Bounds
- input_within_bounds(u: Union[commonroad.scenario.trajectory.State, numpy.array], throw: bool = False) bool[source]¶
Checks whether the given input is within input constraints of the vehicle dynamics model.
- Parameters
u – input values as np.array or State - Contains 2 values
throw – if set to false, will return bool instead of throwing exception (default=False)
- Returns
True if within constraints
- violates_friction_circle(x: Union[commonroad.scenario.trajectory.State, numpy.array], u: Union[commonroad.scenario.trajectory.State, numpy.array], throw: bool = False) bool[source]¶
Checks whether given input violates the friction circle constraint for the given state.
- Parameters
x – current state
u – the input which was used to simulate the next state
throw – if set to false, will return bool instead of throwing exception (default=False)
- Returns
True if the constraint was violated
- forward_simulation(x: numpy.array, u: numpy.array, dt: float, throw: bool = True) Union[None, numpy.array][source]¶
Simulates the next state using the given state and input values as numpy arrays.
- Parameters
x – state values.
u – input values
dt – scenario delta time.
throw – if set to false, will return None as next state instead of throwing exception (default=True)
- Returns
simulated next state values, raises VehicleDynamicsException if invalid input.
- simulate_next_state(x: commonroad.scenario.trajectory.State, u: commonroad.scenario.trajectory.State, dt: float, throw: bool = True) Union[None, commonroad.scenario.trajectory.State][source]¶
Simulates the next state using the given state and input values as State objects.
- Parameters
x – current state
u – inputs for simulating the next state
dt – scenario delta time.
throw – if set to false, will return None as next state instead of throwing exception (default=True)
- Returns
simulated next state, raises VehicleDynamicsException if invalid input.
- simulate_trajectory(initial_state: commonroad.scenario.trajectory.State, input_vector: commonroad.scenario.trajectory.Trajectory, dt: float, throw: bool = True) Union[None, commonroad.scenario.trajectory.Trajectory][source]¶
Creates the trajectory for the given input vector.
- Parameters
initial_state – initial state of the planning problem
input_vector – input vector as Trajectory object
dt – scenario delta time
throw – if set to false, will return None as trajectory instead of throwing exception (default=True)
- Returns
simulated trajectory, raises VehicleDynamicsException if there is an invalid input.
- state_to_array(state: commonroad.scenario.trajectory.State, steering_angle_default=0.0) Tuple[numpy.array, int][source]¶
Converts the given State to numpy array.
- Parameters
state – State
- Returns
state values as numpy array and time step of the state
- array_to_state(x: numpy.array, time_step: int) commonroad.scenario.trajectory.State[source]¶
Converts the given numpy array of values to State.
- Parameters
x – list of state values
time_step – time step of the converted state
- Returns
State
- convert_initial_state(initial_state, steering_angle_default=0.0) commonroad.scenario.trajectory.State[source]¶
Converts the given default initial state to VehicleModel’s state by setting the state values accordingly.
- Parameters
initial_state – default initial state
steering_angle_default – default steering_angle value as it is not given in intiial state
- Returns
converted initial state
- input_to_array(input: commonroad.scenario.trajectory.State) Tuple[numpy.array, int][source]¶
Converts the given input (as State object) to numpy array.
- Parameters
input – input as State object
- Returns
state values as numpy array and time step of the state, raises VehicleDynamicsException if invalid input
- array_to_input(u: numpy.array, time_step: int) commonroad.scenario.trajectory.State[source]¶
Converts the given numpy array of values to input (as State object).
- Parameters
u – input values
time_step – time step of the converted input
- Returns
input as state object, raises VehicleDynamicsException if invalid input
- class PointMassDynamics(vehicle_type: commonroad.common.solution.VehicleType)[source]¶
- dynamics(t, x, u) List[float][source]¶
Point Mass model dynamics function. Overrides the dynamics function of VehicleDynamics for PointMass model.
- Parameters
t –
x – state values, [position x, position y, velocity x, velocity y]
u – input values, [acceleration x, acceleration y]
- Returns
- property input_bounds: scipy.optimize._constraints.Bounds¶
Overrides the bounds method of Vehicle Model in order to return bounds for the Point Mass inputs.
- Bounds are
-max longitudinal acc <= acceleration <= max longitudinal acc
-max longitudinal acc <= acceleration_y <= max longitudinal acc
- Returns
Bounds
- violates_friction_circle(x: Union[commonroad.scenario.trajectory.State, numpy.array], u: Union[commonroad.scenario.trajectory.State, numpy.array], throw: bool = False) bool[source]¶
Overrides the friction circle constraint method of Vehicle Model in order calculate friction circle constraint for the Point Mass model.
- Parameters
x – current state
u – the input which was used to simulate the next state
throw – if set to false, will return bool instead of throwing exception (default=False)
- Returns
True if the constraint was violated