trajectory_supervisor.helper_funcs.src.bound_trajectory module

trajectory_supervisor.helper_funcs.src.bound_trajectory.check_intersect(u1: numpy.ndarray, u2: numpy.ndarray, v1: numpy.ndarray, v2: numpy.ndarray) → tuple[source]

Checks if two line segments given by coordinates u1(start-point),u2 (end-point) and v1,v2 intersect. Code is based on ideas in: http://geomalgorithms.com/a05-_intersect-1.html

Parameters

  • u1 – start point of segment u as a np.array with column [x, y]

  • u2 – end point of segment u as a np.array with column [x, y]

  • v1 – start point of segment v as a np.array with column [x, y]

  • v2 – end point of segment v as a np.array with column [x, y]

Returns

  • intersect - boolean => true when segments intersect

  • intersection - coordinates of intersection as a np.array with column [x, y]

trajectory_supervisor.helper_funcs.src.bound_trajectory.check_slope(s1: numpy.ndarray, s2: numpy.ndarray, tolerance: float) → bool[source]

Checks if two segment have the same slope within the defined tolerance

Parameters

  • s1 – start and end point of segment 1 as a np.array with column [x, y]

  • s2 – start and end point of segment 2 as a np.array with column [x, y]

  • tolerance – tolerance as the minimum value of cosine(angle between c and d)

Returns

  • equal - boolean => true when segments have same slope

trajectory_supervisor.helper_funcs.src.bound_trajectory.cut_out_bound(bound: numpy.ndarray, ref_x: float, ref_y: float, ref_theta: float, delta_x: numpy.ndarray, delta_y: numpy.ndarray, closed: bool) → tuple[source]
Extract a segment of a path (e.g track bound), which is within a window of

[object_x - delta_x[0], object_x + delta_x[1]], [ref_y + delta_y[0], ref_y + delta_y[1]]

Parameters

  • bound – bound, where segment should be extracted

  • ref_x – x-position of the reference pose

  • ref_y – y-position of the reference pose

  • ref_theta – heading of the reference pose

  • delta_x – width of window => delta_x[0] distance to left of ref pos (in direction of heading) delta_x[1] distance to right of ref pos (in direction of heading)

  • delta_y – length of window => delta_y[0] lower bound starting at cg delta_y[1] upper bound starting at cg

  • closed – flag indicating whether provided bound is closed or not

Returns

  • extr_path - extracted path segment

  • first_index - first index of path which is cut extracted

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_bound_reach_intersect(object_x: float, object_y: float, object_theta: float, object_vel: float, direction: float, localgg: numpy.ndarray, long_step: float, bound: numpy.ndarray, index_track: list, object_length: float, object_width: float, closed: bool) → tuple[source]

Calculates intersection between track bound and bound of reachable set.

Parameters

  • object_x – x-position of the vehicle

  • object_y – y-position of the vehicle

  • object_theta – heading of the vehicle

  • object_vel – absolute velocity of the vehicle

  • direction – 1:left -1: right

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • long_step – longitudinal step width boundary of occupancy

  • bound – boundary of race track according to choice of direction

  • index_track – first & last index of track boundary relevant for calculation

  • object_length – length of the vehicle

:param object_width : width of the vehicle :param closed: boolean flag indicating whether the considered track is closed or not

Returns

  • bound_reachset - boundary of reachable set as np.array with columns [x y t heading]

  • index[0] - index of boundary segment

  • inter - boolean intersection found

  • inter_point - point of intersection as a np.array with column [x y]

  • index_track - first & last index of cut out track boundary

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_bound_reachable_set(object_x: float, object_y: float, object_theta: float, object_vel: float, direction: float, localgg: numpy.ndarray, long_step: float, object_length: float, object_width: float) → numpy.ndarray[source]
Calculates front most boundary of reachable set, left side => direction = 1

right side => direction = -1

The boundary calculation uses a friction ellipse as its car model. As the max lateral and longitudinal acceleration the max g-values of the tires are used.

Parameters

  • object_x – x-position of the vehicle

  • object_y – y-position of the vehicle

  • object_theta – heading of the vehicle

  • object_vel – absolute velocity of the vehicle

  • direction – 1:left -1: right

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • long_step – longitudinal step width

  • object_length – length of the vehicle

:param object_width : width of the vehicle

Returns

  • bound - boundary of reachable set defined via connected segments in a np.ndarray

    with columns [x, y, tau, heading]

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_bound_trajectory(object_x: float, object_y: float, object_theta: float, object_vel: float, direction: int, localgg: numpy.ndarray, long_step: float, bound: numpy.ndarray, index_track: list, object_length: float, object_width: float, tangential_t: float, t_div: float, delta_t: float, object_turn_rad: float, closed: bool) → tuple[source]

Calculate an outer trajectory for a reachable set, that tries to incorporate kinematics for a proper response.

For example, the simple reachable set might end non-tangential to the track bounds which would not be executed on purpose by any vehicle –> more space is claimed by the reachable set than necessary. The bound trajectory tries to identify the outermost trajectory of a reachable set, that is still drivable, i.e. ends tangential to the bounds.

NOTE: BETA and does not cope closed tracks yet! (e.g. cut_out_bound (selection of relevant bound segment; returned

“first_index” of this function -> further usage of it; …))

Parameters

  • object_x – x-position of the vehicle [in m]

  • object_y – y-position of the vehicle [in m]

  • object_theta – heading of the vehicle [in rad]

  • object_vel – x-velocity of the vehicle [in m/s]

  • direction – 1:left -1: right

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • long_step – longitudinal step width boundary of occupancy [in m]

  • bound – bound of the race track which SHOULD match the choice of direction

  • index_track – index of first & last index of track boundary relevant for calculation

  • object_length – length of the vehicle [in m]

:param object_width : width of the vehicle [in m] :param tangential_t: approximation of the switching time [in s] :param t_div: max time step needed for trajectory calculation [in s] :param delta_t: time step of double arc [in s] :param object_turn_rad: turning radius of vehicle [in m] :param closed: boolean flag indicating a closed track :returns:

  • trajectory - trajectory indicating new bound of reach set (either transition to bound or pure

    steering) - np.array with columns [x, y, t, heading]

  • bound_reachset - boundary of reachable set as np.array with columns [x y t heading]

  • index_track - first & last index of cut out (relevant) track boundary

  • tangential_t - new approximation of the switching time, remains same if pure steering [in s]

Authors
Created on

09.07.2020

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_double_arc(object_x: float, object_y: float, object_theta: float, object_vel: float, direction: float, t_switch: float, delta_t: float, localgg: numpy.ndarray, object_length: float, object_width: float) → numpy.ndarray[source]

Calculates trajectory of pure steering maneuver with t_switch at a_lat from +a_max to - a_max

Parameters

  • object_x – x-position of the vehicle

  • object_y – y-position of the vehicle

  • object_theta – heading of the vehicle

  • object_vel – absolute velocity of the vehicle

  • direction – 1:left -1: right

  • t_switch – time of switching from full lat. acceleration to full lat. deceleration

  • delta_t – time step

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • object_length – length of the vehicle

:param object_width : width of the vehicle

Returns

  • trajectory - double arc trajectory as a np.ndarray with columns [x, y, t, heading]

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_edge_trajectory(object_x: float, object_y: float, object_theta: float, object_vel: float, localgg: numpy.ndarray, direction: float, t_approximation: float, delta_t: float, max_t: float, bound: numpy.ndarray, lower_limit_long: float, object_length: float, object_width: float, closed: bool) → tuple[source]
Calculates the edge trajectory of a pure steering maneuver + connected extremal trajectory

=> a_lat(t<t_s)=a_max => a_lat(t>t_s)=-a_max

Parameters

  • object_x – x-position of the vehicle

  • object_y – y-position of the vehicle

  • object_theta – heading of the vehicle

  • object_vel – x-velocity of the vehicle

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • direction – 1:left -1: right

  • delta_t – time step of double arc

  • t_approximation – approximation of the switching time

  • max_t – max time step needed for trajectory calculation

  • bound – bound of the race track which SHOULD match the choice of direction

  • lower_limit_long – lower longitudinal limit of cutting window

  • object_length – length of the vehicle

:param object_width : width of the vehicle :param closed: boolean flag indicating a closed track

Returns

  • edge_trajectory - points on the arc trajectory with equally divided distance between each other.

    np. array with [x, y, t, heading] columns

  • t - new approximation of the switching time [in s]

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_ext_trajectory(x: float, y: float, theta: float, vel: float, direction: float, h: float, localgg: numpy.ndarray, delta_t: float) → numpy.ndarray[source]

Calculates extremal trajectory (trajectory reaching height h with the shortest long. path)

Parameters

  • x – x-position of the vehicle

  • y – y-position of the vehicle

  • theta – heading of the vehicle

  • vel – absolute velocity of the vehicle

  • direction – 1:left -1: right

  • h – height at intersection

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • delta_t – time step

Returns

  • trajectory - trajectory as a np.ndarray with columns [x, y, t, heading]

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_pure_steering(object_x: float, object_y: float, object_theta: float, object_vel: float, direction: float, t_end: float, delta_t: float, localgg: numpy.ndarray, turn_rad: float, object_length: float, object_width: float) → numpy.ndarray[source]

Calculates trajectory of pure steering maneuver

Parameters

  • object_x – x-position of the vehicle

  • object_y – y-position of the vehicle

  • object_theta – heading of the vehicle

  • object_vel – absolute velocity of the vehicle

  • direction – 1:left -1: right

  • t_end – time interval of calculated trajectory

  • delta_t – time step

  • localgg – maximum g-g-values at a certain position on the map (columns: x, y, s, ax, ay)

  • turn_rad – turning radius of vehicle

  • object_length – length of the vehicle

:param object_width : width of the vehicle

Returns

  • trajectory - pure steering trajectory as a np.ndarray with columns [x, y, t, heading]

trajectory_supervisor.helper_funcs.src.bound_trajectory.get_track_bound_traj(bound: numpy.ndarray, end_point_traj: numpy.ndarray, t_end: float, delta_t: float, object_vel: float) → numpy.ndarray[source]

Calculates extension of trajectory which follows track boundary with object velocity

Parameters

  • bound – bound of race track starting with segment which intersects

  • end_point_traj – last point of trajectory as a np.ndarray with the column [x, y, t, heading]

  • t_end – end time of trajectory extension

  • delta_t – time step

  • object_vel – absolute velocity of the vehicle

Returns

  • extension - trajectory extension as a np.ndarray with columns [x, y, t, heading]

trajectory_supervisor.helper_funcs.src.bound_trajectory.perp_dot(a: numpy.ndarray, b: numpy.ndarray) → numpy.ndarray[source]

This is a helper function to calculate the Perp Dot Product, which equals a two dimensional dot product, in which the first vector ist replaced by the perpendicular of itself. More information: https://mathworld.wolfram.com/PerpDotProduct.html

Parameters

  • a – multiplier vector as a np.ndarray with columns [x, y]

  • b – multiplicand vector as a np.ndarray with columns [x, y]

Returns

  • dot - product as a np.ndarray with columns [x, y]

trajectory_supervisor.helper_funcs.src.bound_trajectory.sweep_line_intersection(set1: numpy.ndarray, set2: numpy.ndarray, theta: float, tolerance: float) → tuple[source]

Calculates intersection between two sets of connected segments using a sweep line algorithm. The algorithm is a adapted version of the set-based intersection algorithm in: https://www.sciencedirect.com/science/article/pii/S0098300499000710 The sweep line moves in the direction of the object CAUTION: The algorithm can only be used for sets where der y-coordinate in the rotated coordinate system is strictly increasing with rising indices

Parameters

  • set1 – first set of coodinates (columns x, y)

  • set2 – second set of coodinates (columns x, y)

  • theta – heading of vehicle

:param tolerance tolerance of segments in set 1 in orthogonal direction of the object

>0 tolerance to the right <0 tolerance to the left

Returns

  • inter_found - true when intersection found

  • intersection_point - intersection coordinates as a np.array with column [x, y], Default [0,0]

  • index - first indexes of intersecting segments np.array [n_1, n_2]