Module Motion Planner

The motion planner module in commonroad_search is based on search. The motion planner class is used to construct your planner, where you can define your own cost function and search algorithm in “calc_heuristic_cost” and “search_alg”. You can use all available measurements offered in this module to construct your own cost function. The priority queue class is a helper class for defining the search algorithm.

The starting node (start state of the planning problem) of the search tree is defined by the loaded scenario (a scenario can have more than one planning problems with different initial state and goal state). Other nodes of the search tree consist of different motion primitives. Each motion primitive is a short trajectory which consists of several states. The aim is to search the tree and find a path (a combination of primitives) that satisfies the conditions of the goal state without causing any collisions.

Motion Planner

MotionPlanner class

class Automata.MotionPlanner.MotionPlanner(scenario, planningProblem, automata, state_tuple, shapeEgoVehicle)[source]
map_obstacles_to_lanelets(time_step)[source]

Finds all obstacles that are located in every lanelet at time step t and returns a dictionary where obstacles are stored according to their lanelet ids.

Parameters

time_step (int) – maps obstacles in this time step to the lanelet

Return type

dict

get_obstacles(laneletObj, time_step)[source]

Returns the subset of obstacles, which are located in the given lanelet.

Parameters
  • laneletObj (Lanelet) – specify the lanelet object to get its obstacles

  • time_step (int) – the time step in which the occupancy of obstacles is checked

Return type

List[Obstacle]

calc_lanelet_cost(curLanelet, dist, visited_lanelets)[source]

Calculates distances of all lanelets which can be reached through recursive adjacency/predecessor relationship by the current lanelet. This is a recursive implementation.

Parameters
  • curLanelet (Lanelet) – the current lanelet object (often set to the goal lanelet)

  • dist (int) – the initial distance between 2 adjacent lanelets (often set to 1). This value will increase recursively during the execution of this function.

  • visited_lanelets (List[int]) – list of visited lanelet ids. In the iterations, visited lanelets will not be considered. This value changes during the recursive implementation.

The calculated costs will be stored in dictionary self.lanelet_cost[Lanelet].

calc_lanelet_orientation(lanelet_id, pos)[source]

Returns lanelet orientation (angle in radian, counter-clockwise defined) at the given position and lanelet id.

Parameters
  • lanelet_id (int) – id of the lanelet, based on which the orientation is calculated

  • pos (ndarray) – position, where orientation is calculated. (Often the position of the obstacle)

Return type

float

calc_angle_to_goal(state)[source]

Returns the orientation of the goal (angle in radian, counter-clockwise defined) with respect to the position of the state.

Parameters

state (State) – the angle between this state and the goal will be calculated

Return type

float

lanelets_of_position(lanelets, state, diff=0.6283185307179586)[source]

Returns all lanelets, whose angle to the orientation of the input state are smaller than pi/5.

Parameters
  • lanelets (List[int]) – potential lanelets

  • state (State) – the input state

  • diff (float) – acceptable angle difference between the state and the lanelet

Return type

List[Lanelet]

dist_to_closest_obstacle(lanelet_id, pos, time_step)[source]

Returns distance between the given position and the center of the closest obstacle in the given lanelet (specified by lanelet id).

Parameters
  • lanelet_id (int) – the id of the lanelet where the distance to obstacle is calculated

  • pos (ndarray) – current input position

  • time_step (int) – current time step

Return type

float

num_obstacles_in_lanelet_at_time_step(time_step, lanelet_id)[source]

Returns the number of obstacles in the given lanelet (specified by lanelet id) at time step t.

Parameters
  • time_step (int) – time step

  • lanelet_id (int) – specifies the lanelet

Return type

int

is_adjacent(start_lanelet_id, final_lanelet_id)[source]

Returns true if the the final lanelet is adjacent to the start lanelet.

Parameters
  • start_lanelet_id (int) – id of the first lanelet (start lanelet)

  • final_lanelet_id (int) – id of the second lanelet (final lanelet)

Return type

bool

is_successor(start_lanelet_id, final_lanelet_id)[source]

Returns true if the the final lanelet is a successor of the start lanelet.

Parameters
  • start_lanelet_id (int) – id of the first lanelet (start lanelet)

  • final_lanelet_id (int) – id of the second lanelet (final lanelet)

Return type: bool

Return type

bool

is_goal_in_lane(lanelet_id, traversed_lanelets=None)[source]

Returns true if the goal is in the given lanelet or any successor (including all successors of successors) of the given lanelet.

Parameters
  • lanelet_id (int) – the id of the given lanelet

  • traversed_lanelets – helper variable which stores potential path (a list of lanelet id) to goal lanelet. Default value is None.

Return type

bool

calc_time_cost(path)[source]

Returns time cost (number of time steps) to perform the given path.

Parameters

path (List[State]) – the path whose time cost is calculated

Return type

int

calc_path_efficiency(path)[source]

Returns the path efficiency = travelled_distance / time_cost

Parameters

path (List[State]) – the path whose efficiency is calculated

Return type

float

calc_heuristic_distance(state, distance_type=0)[source]

Returns the heuristic distance between the current state and the goal state.

Parameters
  • state (State) – the state, whose heuristic distance to the goal is calculated

  • distance_type – default is euclidean distance. For other distance type please refer to the function “distance(pos1: np.ndarray, pos2: np.ndarray, type=0)”

Return type

float

calc_heuristic_lanelet(path)[source]

Calculates the distance between every individual state of the path and the centers of the path’s corresponding lanelets and sum them up.

Parameters

path (List[State]) – the path, whose heuristics is calculated

Returns the heuristic distance of the path (float), id of the end lanelet of the given path (list) and the start lanelet id (list). End lanelet means the lanelet where the last state of the path is in, start lanelet means the lanelet corresponding to the first state of the path.

Return type

Tuple[float, list, list]

reached_goal(path)[source]

Goal-test every state of the path and returns true if one of the state satisfies all conditions for the goal region: position, orientation, velocity, time.

Parameters

path (List[State]) – the path to be goal-tested

Return type

bool

remove_states_behind_goal(path)[source]

Removes all states that are behind the state which statisfies goal state conditions and returns the pruned path.

Parameters

path (List[State]) – the path to be pruned

Return type

List[State]

check_collision_free(path)[source]

Checks if path collides with an obstacle. Returns true for no collision and false otherwise.

Parameters

path (List[State]) – The path you want to check

Return type

bool

translate_primitive_to_current_state(primitive, currentPath)[source]

Uses the trajectory defined in the given primitive, translates it towards the last state of current path and returns the list of new path. In the newly appended part (created through translation of the primitive) of the path, the position, orientation and time step are changed, but the velocity is not changed. Attention: The input primitive itself will not be changed after this operation.

Parameters
  • primitive (MotionPrimitive) – the primitive to be translated

  • currentPath (List[State]) – the path whose last state is the goal state for the translation

Return type

List[State]

append_path(currentPath, newPath)[source]

Appends a new path to the current path and returns the whole path.

Parameters
  • currentPath (List[State]) – current path which is to be extended

  • newPath (List[State]) – new path which is going to be added to the current path

Return type

List[State]

get_successor_primitives(cur_primitive)[source]

Returns all possible successor primitives of the current primitive.

Parameters

cur_primitive (MotionPrimitive) – current primitive

Return type

List[MotionPrimitive]

calc_heuristic_cost(path, curPos)[source]

Returns heuristic cost for the path. You should define your own cost function here.

Parameters
  • path (List[State]) – the heuristic cost is calculated based on this path

  • curPos (State) – current position/state of your path. Every state has the following variables: position, velocity, orientation, time_step

Return type

float

search_alg(startSuccessor, maxTreeDepth, status)[source]

Returns the path and the used primitives as a tuple.

Parameters
  • startSuccessor (List[MotionPrimitive]) – all possible primitives for the start state

  • maxTreeDepth (int) – maximum number of concatenated primitives

  • status (dict) – helper values including ‘cost_so_far’, ‘time_step’, ‘orientation’, ‘velocity’, which are used for the visualization in the jupyter notebook

Return type

Tuple[List[State], List[MotionPrimitive]]

Priority Queue

PriorityQueue class

class Automata.MotionPlanner.PriorityQueue[source]
empty()[source]

Test whether the queue is empty. Returns true if the queue is empty.

put(item, priority)[source]

Put an item into the queue and count the number of elements in the queue. The number is saved in self.count.

Parameters
  • item – the element to be put in the queue

  • priority – the value used to sort/prioritize elements in the queue. It’s often the value of some cost function

get()[source]

Pop the smallest item off the heap (Priority queue) if the queue is not empty.

Other Functions

Automata.MotionPlanner.findClosestVertex(centerVertices, pos)[source]

Return the index of the closest center vertice to the given position.

Parameters
  • center_vertices – the vertices of the center line of the Lanelet described as a polyline [[x0,x1,…,xn],[y0,y1,…,yn]]

  • pos (ndarray) – the closest vertex to this position will be found

Return type

int

Automata.MotionPlanner.calcAngleOfPosition(centerVertices, pos)[source]

Returns the angle (in world coordinate, radian) of the line defined by 2 nearest lanelet center vertices to the given position.

Parameters
  • centerVertices (ndarray) – lanelet center vertices, whose distance to the given position is considered

  • pos (ndarray) – the input position

Return type

float

Automata.MotionPlanner.distToClosestPointOnLine(vertexA, vertexB, pos)[source]

Returns the distance of the given position to a line segment (e.g. the nearest lanelet center line segment to the given position).

Parameters
  • vertexA (ndarray) – the start point of the line segment

  • vertexB (ndarray) – the end point of the line segment

Automata.MotionPlanner.findDistanceToNearestPoint(centerVertices, pos)[source]

Returns the closest euclidean distance to a polyline (e.g. defined by lanelet center vertices) according to the given position.

Parameters
  • centerVertices (ndarray) – the poyline, between which and the given position the distance is calculated

  • pos (ndarray) – the input position

Automata.MotionPlanner.calc_travelled_distance(path)[source]

Returns the travelled distance of the given path.

Param

the path, whose euclidean distance is calculated

Return type

float

Automata.MotionPlanner.euclideanDistance(pos1, pos2)[source]

Returns the euclidean distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.manhattanDistance(pos1, pos2)[source]

Returns the manhattan distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.chebyshevDistance(pos1, pos2)[source]

Returns the chebyshev distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.sumOfSquaredDifference(pos1, pos2)[source]

Returns the squared euclidean distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.meanAbsoluteError(pos1, pos2)[source]

Returns a half of the manhattan distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.meanSquaredError(pos1, pos2)[source]

Returns the mean of squared difference between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.canberraDistance(pos1, pos2)[source]

Returns the canberra distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.cosineDistance(pos1, pos2)[source]

Returns the cosine distance between 2 points.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

Return type

float

Automata.MotionPlanner.distance(pos1, pos2, type=0)[source]

Returns the distance between 2 points, the type is specified by ‘type’.

Parameters
  • pos1 (ndarray) – the first point

  • pos2 (ndarray) – the second point

type: 0 means euclideanDistance,

1 means manhattanDistance,

2 means chebyshevDistance,

3 means sumOfSquaredDifference,

4 means meanAbsoluteError,

5 means meanSquaredError,

6 means canberraDistance,

7 means cosineDistance.

Return type

float

Automata.MotionPlanner.curvature_of_polyline(polyline)[source]

Returns the curvature of the given polyline.

Parameters

polyline (ndarray) – the polyline to be calculated

Return type

float

Automata.MotionPlanner.orientation_diff(orientation_1, orientation_2)[source]

Returns the orientation difference between 2 orientations in radians.

Parameters
  • orientation_1 (float) – the first orientation.

  • orientation_2 (float) – the second orientation.

Return type

float

Automata.MotionPlanner.length_of_polyline(polyline)[source]

Returns the length of the polyline.

Parameters

polyline (ndarray) – the polyline, whose length is calculated