trajectory_supervisor.helper_funcs.src.reachable_set_simple module

class trajectory_supervisor.helper_funcs.src.reachable_set_simple.ReachSetSimple(closed: bool = False, bound_l: Optional[numpy.ndarray] = None, bound_r: Optional[numpy.ndarray] = None)[source]

Bases: object

Class for calculation of simple reach set over-approximation. Provides option to trim the reachable set to provided track boundaries (provide via init call).

If this functionality is not required, calling the “simple_reachable_set()”-method is sufficient (no class needed).

Parameters

  • closed – flag indicating whether track bounds are a closed circuit or not

  • bound_l – coordinates of the left bound (numpy array with columns x, y)

  • bound_r – coordinates of the right bound (numpy array with columns x, y)

calc_reach_set(obj_pos: numpy.ndarray, obj_heading: float, obj_vel: float, obj_length: float, obj_width: float, dt: float, t_max: float, a_max: float) → dict[source]

Calculates a simple reachable set approximation for an object, based on its position and speed. ‘dt’ and ‘t_max’ define the resolution and the number of time-steps.

Parameters

  • obj_pos – position of the vehicle (x and y coordinate)

  • obj_heading – heading of the vehicle

  • obj_vel – velocity of the vehicle

  • obj_length – length of the vehicle [in m]

  • obj_width – width of the vehicle [in m]

  • dt – desired temporal resolution of the reachable set

  • t_max – maximum temporal horizon for the reachable set

  • a_max – maximum assumed acceleration of the object vehicle

Returns

  • poly - dict of reachable areas with:

    • keys holding the evaluated time-stamps

    • values holding the outlining coordinates as a np.ndarray with columns [x, y]

trajectory_supervisor.helper_funcs.src.reachable_set_simple.bx_boundary(vel: float, a_max: float, dt: float, t_max: float) → numpy.ndarray[source]

Calculate boundary (b_x) according to: “Set-Based Prediction of Traffic Participants onArbitrary Road Networks” by M. Althoff and S. Magdici -> Eq. (4)

Parameters

  • vel – velocity of the vehicle [in m/s]

  • a_max – maximum acceleration [in m/s²]

  • dt – temporal increment between pose predictions [in s]

  • t_max – prediction horizon [in s]

Returns

  • bx_bound - bx_boundary

trajectory_supervisor.helper_funcs.src.reachable_set_simple.calc_acc_rad(amax: float, dt: float, t_max: float) → list[source]

Calculates the radius of the largest reachable set.

Parameters

  • amax – maximum acceleration [in m/s^2]

  • dt – prediction resolution [in s]

  • t_max – precition horizon [in s]

Returns

  • racc - sequence of radii

trajectory_supervisor.helper_funcs.src.reachable_set_simple.calc_vertices(pos: numpy.ndarray, cvehicle: numpy.ndarray, bx_bound: numpy.ndarray, globalangle: float, racc: list, dt: float, veh_length: float = 0.0, veh_width: float = 0.0) → dict[source]

Calculates the vertices of the enveloping polygon.

      q2-----q3
    -        |
q1           |
 |           |
q6           |
    -        |
      q5-----q4

Calculation of over-approximation polygon for each step. Method based on “SPOT: A Tool for Set-Based Prediction of Traffic Participants” by M. Koschi and M. Althoff

Parameters

  • pos – position of the vehicle [in m]

  • cvehicle – center positions of vehicle at future time-stamps (along longitudinal axis)

  • bx_bound – front extension of overapprox. reach set (based on Althoff)

  • globalangle – angle [in rad]

  • racc – radius [in m]

  • dt – temporal increment between pose predictions [in s]

  • veh_length – (optional) vehicle length [in m], if not provided, reach-set for point-mass is calculated

  • veh_width – (optional) vehicle width [in m], if not provided, reach-set for point-mass is calculated

Returns

  • polypred - dict of reachable areas with:

    • keys holding the evaluated time-stamps

    • values holding the outlining coordinates as a np.ndarray with columns [x, y]

Authors
Created on

27.01.2020

trajectory_supervisor.helper_funcs.src.reachable_set_simple.cv_model(vel: float, dt: float, t_max: float) → numpy.ndarray[source]

CV-Model for prediction of the center of the vehicle (along vehicle’s longitudinal axis).

Parameters

  • vel – velocity of the vehicle [in m/s]

  • dt – temporal increment between pose predictions [in s]

  • t_max – precition horizon [in s]

Returns

  • cvehicle - predicted centers of the vehicle

trajectory_supervisor.helper_funcs.src.reachable_set_simple.polar2cart(radius: float, angle: float) → numpy.ndarray[source]

Transforms polar coordinates to cartesian coordinates and returns the values in the vector format.

Parameters

  • radius – radius of polar coordinate

  • angle – angle of polar coordinate

Returns

  • coord - x, y values of point converted to cartesian coordinates

trajectory_supervisor.helper_funcs.src.reachable_set_simple.rotate_vector(vector: numpy.array, angle: float) → numpy.ndarray[source]

Rotates a vector by the angle alpha around the origin.

Parameters

  • vector – vector

  • angle – angle in rad

Returns

  • vector - rotated vector

trajectory_supervisor.helper_funcs.src.reachable_set_simple.simple_reachable_set(obj_pos: numpy.ndarray, obj_heading: float, obj_vel: float, obj_length: float, obj_width: float, dt: float, t_max: float, a_max: float) → dict[source]

Calculates a simple reachable set approximation for an object, based on its position and speed. ‘dt’ and ‘t_max’ define the resolution and the number of time-steps.

Parameters

  • obj_pos – position of the vehicle (x and y coordinate)

  • obj_heading – heading of the vehicle

  • obj_vel – velocity of the vehicle

  • obj_length – length of the vehicle [in m]

  • obj_width – width of the vehicle [in m]

  • dt – desired temporal resolution of the reachable set

  • t_max – maximum temporal horizon for the reachable set

  • a_max – maximum assumed acceleration of the object vehicle

Returns

  • poly - dict of reachable areas with:

    • keys holding the evaluated time-stamps

    • values holding the outlining coordinates as a np.ndarray with columns [x, y]

Authors
Created on

27.01.2020