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 obstaclestime_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, counterclockwise defined) at the given position and lanelet id.
 Parameters
lanelet_id (
int
) – id of the lanelet, based on which the orientation is calculatedpos (
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, counterclockwise 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 laneletsstate (
State
) – the input statediff (
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 calculatedpos (
ndarray
) – current input positiontime_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 steplanelet_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 lanelettraversed_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 calculateddistance_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]¶ Goaltest 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 goaltested 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 translatedcurrentPath (
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 extendednewPath (
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 pathcurPos (
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 statemaxTreeDepth (
int
) – maximum number of concatenated primitivesstatus (
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]¶ 

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

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 consideredpos (
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 segmentvertexB (
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 calculatedpos (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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 pointpos2 (
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