f110_planning.tracking package

Submodules

f110_planning.tracking.lqr_planner module

LQR waypoint tracker Implementation inspired by https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/lqr_steer_control/lqr_steer_control.py

class f110_planning.tracking.lqr_planner.LQRPlanner(wheelbase: float = 0.33, waypoints: ndarray | None = None, max_speed: float = 5.0, timestep: float = 0.01, matrix_q_1: float = 0.999, matrix_q_2: float = 0.0, matrix_q_3: float = 0.0066, matrix_q_4: float = 0.0, matrix_r: float = 0.75, iterations: int = 50, eps: float = 0.001)

Bases: BasePlanner

Lateral Controller using Linear Quadratic Regulator (LQR).

This planner linearizes the vehicle dynamics around a reference path and solves for an optimal gain matrix K that minimizes a cost function representing both tracking error and control effort.

calc_control_points(vehicle_state: ndarray, waypoints: ndarray) → tuple[float, float, float, float, float]

Calculates the current state errors and reference curvature.

controller(vehicle_state: ndarray) → tuple[float, float]

Optimal lateral control calculation.

This discrete-time LQR implementation computes steering based on a 4-state error vector: [crosstrack, lateral_velocity, heading, heading_rate].

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Compute lateral control command.

f110_planning.tracking.pure_pursuit_planner module

Pure Pursuit waypoint tracker

class f110_planning.tracking.pure_pursuit_planner.PurePursuitPlanner(wheelbase: float = 0.33, lookahead_distance: float = 0.8, max_speed: float = 5.0, waypoints: ndarray | None = None)

Bases: BasePlanner

Geometrically-inspired waypoint tracking controller.

Pure Pursuit calculates the curvature required to move the vehicle from its current position to a point on the reference path that is one ‘lookahead’ distance away.

Reference: Coulter, R. Craig. “Implementation of the Pure Pursuit Path Tracking Algorithm.” Carnegie Mellon University, 1992.

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Determines the steering and speed required to follow the path.

f110_planning.tracking.stanley_planner module

Stanley waypoint tracker

class f110_planning.tracking.stanley_planner.StanleyPlanner(wheelbase: float = 0.33, waypoints: ndarray | None = None, max_speed: float = 5.0, k_path: float = 5.0)

Bases: BasePlanner

Front Wheel Feedback Controller (Stanley) for path tracking.

The Stanley controller combines heading alignment and cross-track error compensation (from the perspective of the front axle) into a single steering law.

Reference: Thrun et al, “Stanley: The Robot that Won the DARPA Grand Challenge”, 2005.

calc_theta_and_ef(vehicle_state: ndarray, waypoints: ndarray) → tuple[float, float, int, float]

Computes the current tracking errors relative to the front axle hub.

controller(vehicle_state: ndarray, waypoints: ndarray, k_path: float) → tuple[float, float]

Computes steering and speed using the Stanley control law.

The steering angle delta is defined as: delta(t) = theta_e(t) + arctan((k * e(t)) / v(t))

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Determines the next control action for the vehicle.

Module contents

Tracking planners for F1TENTH.

class f110_planning.tracking.LQRPlanner(wheelbase: float = 0.33, waypoints: ndarray | None = None, max_speed: float = 5.0, timestep: float = 0.01, matrix_q_1: float = 0.999, matrix_q_2: float = 0.0, matrix_q_3: float = 0.0066, matrix_q_4: float = 0.0, matrix_r: float = 0.75, iterations: int = 50, eps: float = 0.001)

Bases: BasePlanner

Lateral Controller using Linear Quadratic Regulator (LQR).

This planner linearizes the vehicle dynamics around a reference path and solves for an optimal gain matrix K that minimizes a cost function representing both tracking error and control effort.

calc_control_points(vehicle_state: ndarray, waypoints: ndarray) → tuple[float, float, float, float, float]

Calculates the current state errors and reference curvature.

controller(vehicle_state: ndarray) → tuple[float, float]

Optimal lateral control calculation.

This discrete-time LQR implementation computes steering based on a 4-state error vector: [crosstrack, lateral_velocity, heading, heading_rate].

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Compute lateral control command.

class f110_planning.tracking.PurePursuitPlanner(wheelbase: float = 0.33, lookahead_distance: float = 0.8, max_speed: float = 5.0, waypoints: ndarray | None = None)

Bases: BasePlanner

Geometrically-inspired waypoint tracking controller.

Pure Pursuit calculates the curvature required to move the vehicle from its current position to a point on the reference path that is one ‘lookahead’ distance away.

Reference: Coulter, R. Craig. “Implementation of the Pure Pursuit Path Tracking Algorithm.” Carnegie Mellon University, 1992.

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Determines the steering and speed required to follow the path.

class f110_planning.tracking.StanleyPlanner(wheelbase: float = 0.33, waypoints: ndarray | None = None, max_speed: float = 5.0, k_path: float = 5.0)

Bases: BasePlanner

Front Wheel Feedback Controller (Stanley) for path tracking.

The Stanley controller combines heading alignment and cross-track error compensation (from the perspective of the front axle) into a single steering law.

Reference: Thrun et al, “Stanley: The Robot that Won the DARPA Grand Challenge”, 2005.

calc_theta_and_ef(vehicle_state: ndarray, waypoints: ndarray) → tuple[float, float, int, float]

Computes the current tracking errors relative to the front axle hub.

controller(vehicle_state: ndarray, waypoints: ndarray, k_path: float) → tuple[float, float]

Computes steering and speed using the Stanley control law.

The steering angle delta is defined as: delta(t) = theta_e(t) + arctan((k * e(t)) / v(t))

plan(obs: dict[str, Any], ego_idx: int = 0) → Action

Determines the next control action for the vehicle.