From 63dbb66752893ef68907c22b675fce6111f26268 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Mon, 5 Oct 2026 21:15:35 -0700 Subject: [PATCH] Add Hybrid A* planning for a car with a passive trailer --- .../trailer_hybrid_a_star.py | 373 ++++++++++++++++++ .../hybrid_astar_trailer_main.rst | 97 +++++ .../5_path_planning/path_planning_main.rst | 1 + tests/test_hybrid_a_star_trailer.py | 154 ++++++++ 4 files changed, 625 insertions(+) create mode 100644 PathPlanning/HybridAStarTrailer/trailer_hybrid_a_star.py create mode 100644 docs/modules/5_path_planning/hybrid_astar_trailer/hybrid_astar_trailer_main.rst create mode 100644 tests/test_hybrid_a_star_trailer.py diff --git a/PathPlanning/HybridAStarTrailer/trailer_hybrid_a_star.py b/PathPlanning/HybridAStarTrailer/trailer_hybrid_a_star.py new file mode 100644 index 0000000000..f873e9cf08 --- /dev/null +++ b/PathPlanning/HybridAStarTrailer/trailer_hybrid_a_star.py @@ -0,0 +1,373 @@ +"""Hybrid A* for a car with one passive, on-axle trailer. + +Author: Atsushi Sakai (with Codex) +References: +- Dolgov et al., Practical Search Techniques in Path Planning for Autonomous Driving +- LaValle, Planning Algorithms, Section 13.1.2.4 +- Atsushi Sakai's HybridAStarTrailer (Julia), https://github.com/yinflight/HybridAStarTrailer + +Positions and lengths are in metres; headings and steering angles are in radians. +The hitch is at the tractor rear axle. Obstacles are sampled points. +""" + +from __future__ import annotations + +from dataclasses import dataclass +import heapq +import itertools +import math +import pathlib +import sys + +import matplotlib.pyplot as plt +import numpy as np +from scipy.spatial import cKDTree + +sys.path.append(str(pathlib.Path(__file__).resolve().parents[2])) +from PathPlanning.ReedsSheppPath import reeds_shepp_path_planning as rs + +# Geometry from the reference trailer example, relative to the hitch. +WHEEL_BASE = 3.7 +TRAILER_LENGTH = 8.0 # hitch to trailer axle +WIDTH = 2.6 +TRACTOR_FRONT, TRACTOR_REAR = 4.5, 1.0 +TRAILER_FRONT, TRAILER_REAR = 1.0, 9.0 +MAX_STEER = 0.6 +MAX_ARTICULATION = math.radians(75.0) + +XY_RESOLUTION = 2.0 +YAW_RESOLUTION = math.radians(15.0) +MOTION_RESOLUTION = 0.2 +PRIMITIVE_LENGTH = 3.0 +N_STEER = 7 +TRAILER_GOAL_TOLERANCE = math.radians(5.0) +SEARCH_MARGIN = 15.0 +BACK_COST = 5.0 +SWITCH_COST = 10.0 +STEER_COST = 1.0 +STEER_CHANGE_COST = 1.0 +ARTICULATION_COST = 1.0 +HEURISTIC_WEIGHT = 3.0 +show_animation = True + + +@dataclass +class Node: + """A continuous trajectory segment and its predecessor in the search.""" + + poses: list + distances: list + steer: float = 0.0 + direction: int = 0 + cost: float = 0.0 + parent: Node | None = None + + +@dataclass +class Path: + """Sampled (x, y, tractor yaw, trailer yaw), signed steps, and search cost. + + ``distances[i]`` and ``steers[i]`` produce ``poses[i]`` from the previous + pose. Their first entries are zero. ``expanded_nodes`` counts heap pops. + """ + + poses: np.ndarray + distances: np.ndarray + steers: np.ndarray + cost: float + expanded_nodes: int + + +def wrap_angle(angle): + """Map an angle or an array of angles to [-pi, pi).""" + return (angle + math.pi) % (2.0 * math.pi) - math.pi + + +def move(pose, distance, steer): + """Integrate a constant-steering signed step, including reverse motion. + + The tractor follows an exact circular arc. The passive trailer heading + follows d(phi)/ds = sin(theta - phi) / TRAILER_LENGTH, integrated with RK4. + """ + x, y, yaw, trailer_yaw = pose + curvature = math.tan(steer) / WHEEL_BASE + yaw_change = distance * curvature + # sinc avoids division by zero for a straight segment. + travel = distance * np.sinc(yaw_change / (2.0 * math.pi)) + x += travel * math.cos(yaw + yaw_change / 2.0) + y += travel * math.sin(yaw + yaw_change / 2.0) + + def derivative(offset, heading): + return math.sin(yaw + curvature * offset - heading) / TRAILER_LENGTH + + k1 = derivative(0.0, trailer_yaw) + k2 = derivative(distance / 2.0, trailer_yaw + distance * k1 / 2.0) + k3 = derivative(distance / 2.0, trailer_yaw + distance * k2 / 2.0) + k4 = derivative(distance, trailer_yaw + distance * k3) + trailer_yaw += distance * (k1 + 2.0 * k2 + 2.0 * k3 + k4) / 6.0 + return x, y, wrap_angle(yaw + yaw_change), wrap_angle(trailer_yaw) + + +def collision_free(poses, obstacle_tree): + """Check both rectangular bodies at every pose against obstacle points.""" + poses = np.asarray(poses) + for heading_column, front, rear in ( + (2, TRACTOR_FRONT, TRACTOR_REAR), + (3, TRAILER_FRONT, TRAILER_REAR), + ): + headings = poses[:, heading_column] + offsets = (front - rear) / 2.0 + centers = poses[:, :2] + offsets * np.column_stack( + (np.cos(headings), np.sin(headings))) + radius = math.hypot((front + rear) / 2.0, WIDTH / 2.0) + neighbors = obstacle_tree.query_ball_point(centers, radius) + for pose, heading, indices in zip(poses, headings, neighbors): + if not indices: + continue + delta = obstacle_tree.data[indices] - pose[:2] + local_x = delta[:, 0] * math.cos(heading) + delta[:, 1] * math.sin(heading) + local_y = -delta[:, 0] * math.sin(heading) + delta[:, 1] * math.cos(heading) + if np.any((local_x >= -rear) & (local_x <= front) + & (np.abs(local_y) <= WIDTH / 2.0)): + return False + return True + + +def valid_poses(poses, obstacle_tree, bounds): + """Reject out-of-bounds hitch positions, excessive articulation, or collision.""" + poses = np.asarray(poses) + xmin, xmax, ymin, ymax = bounds + return (np.all((poses[:, 0] >= xmin) & (poses[:, 0] <= xmax) + & (poses[:, 1] >= ymin) & (poses[:, 1] <= ymax)) + and np.all(np.abs(wrap_angle(poses[:, 2] - poses[:, 3])) + <= MAX_ARTICULATION) + and collision_free(poses, obstacle_tree)) + + +def extend(parent, length, steer, obstacle_tree, bounds): + """Roll out and validate one steering segment without snapping its end pose.""" + count = max(1, math.ceil(abs(length) / MOTION_RESOLUTION)) + distance = length / count + pose = parent.poses[-1] + poses = [] + for _ in range(count): + pose = move(pose, distance, steer) + poses.append(pose) + if not valid_poses(poses, obstacle_tree, bounds): + return None + direction = 1 if length > 0.0 else -1 + trajectory = np.array(poses) + articulation = np.abs(wrap_angle(trajectory[:, 2] - trajectory[:, 3])) + cost = abs(length) * (1.0 if direction > 0 else BACK_COST) + cost += STEER_COST * abs(steer) * abs(length) + cost += STEER_CHANGE_COST * abs(steer - parent.steer) + cost += ARTICULATION_COST * float(np.sum(articulation)) * abs(distance) + if parent.direction and direction != parent.direction: + cost += SWITCH_COST + return Node(poses, [distance] * count, steer, direction, parent.cost + cost, parent) + + +def state_key(node): + """Discretize all four coordinates; retain direction and steering for costs.""" + x, y, yaw, trailer_yaw = node.poses[-1] + bins = round(2.0 * math.pi / YAW_RESOLUTION) + return (round(x / XY_RESOLUTION), round(y / XY_RESOLUTION), + round(wrap_angle(yaw) / YAW_RESOLUTION) % bins, + round(wrap_angle(trailer_yaw) / YAW_RESOLUTION) % bins, + node.direction, node.steer) + + +def at_goal(pose, goal): + """Require the tractor pose and the independently integrated trailer heading.""" + return (math.hypot(pose[0] - goal[0], pose[1] - goal[1]) < 1e-6 + and abs(wrap_angle(pose[2] - goal[2])) < 1e-6 + and abs(wrap_angle(pose[3] - goal[3])) <= TRAILER_GOAL_TOLERANCE) + + +def analytic_expansion(current, goal, obstacle_tree, bounds): + """Try Reeds-Shepp tractor connections, propagating and checking the trailer. + + A tractor connection alone is insufficient: reject it when the trailer + misses its goal heading, collides, or exceeds the articulation limit. + """ + pose = current.poses[-1] + if math.hypot(pose[0] - goal[0], pose[1] - goal[1]) < 1e-8: + # The existing Reeds-Shepp generator has singular coincident poses. + # Ordinary motion primitives can still leave and return to this position. + return None + curvature = math.tan(MAX_STEER) / WHEEL_BASE + try: + candidates = rs.generate_path(pose[:3], goal[:3], curvature, 1e-6) + except ZeroDivisionError: + # Some exact circular configurations are singular in the shared + # generator. Keep searching with ordinary motion primitives. + return None + best = None + for candidate in candidates: + node = current + for length, mode in zip(candidate.lengths, candidate.ctypes): + if abs(length) < 1e-10: + continue + steer = {"L": MAX_STEER, "R": -MAX_STEER, "S": 0.0}[mode] + node = extend(node, length / curvature, steer, obstacle_tree, bounds) + if node is None: + break + if node is not None and at_goal(node.poses[-1], goal): + if best is None or node.cost < best.cost: + best = node + return best + + +def reconstruct_path(node, expanded_nodes): + """Follow predecessor objects so reopening a grid cell cannot alter a path.""" + segments = [] + cost = node.cost + while node is not None: + segments.append(node) + node = node.parent + segments.reverse() + return Path(np.array([pose for segment in segments for pose in segment.poses]), + np.array([step for segment in segments for step in segment.distances]), + np.array([segment.steer for segment in segments for _ in segment.poses]), + cost, expanded_nodes) + + +def hybrid_a_star_planning(start, goal, obstacles, *, bounds=None, max_expansions=5000): + """Plan for (x, y, tractor yaw, trailer yaw) poses and N x 2 obstacle points. + + ``bounds`` is (xmin, xmax, ymin, ymax) for the hitch [m]. By default it + encloses the inputs plus SEARCH_MARGIN. Returns a Path, or None if an input + pose is infeasible, the frontier is exhausted, or max_expansions is reached. + No claim of completeness or optimality is made for this discretized search. + Inputs are not modified. Returned tractor pose is exact to 1e-6; trailer yaw + is within TRAILER_GOAL_TOLERANCE of the requested goal, not snapped to it. + """ + start, goal = np.array(start, dtype=float), np.array(goal, dtype=float) + obstacles = np.asarray(obstacles, dtype=float).reshape(-1, 2) + start[2:], goal[2:] = wrap_angle(start[2:]), wrap_angle(goal[2:]) + obstacle_tree = cKDTree(obstacles) + if bounds is None: + points = np.vstack((start[:2], goal[:2], obstacles)) + low, high = points.min(axis=0) - SEARCH_MARGIN, points.max(axis=0) + SEARCH_MARGIN + bounds = low[0], high[0], low[1], high[1] + if not valid_poses([start, goal], obstacle_tree, bounds): + return None + current = Node([tuple(start)], [0.0]) + if at_goal(start, goal): + return reconstruct_path(current, 0) + serial = itertools.count() + queue = [(0.0, next(serial), current)] + best_cost = {state_key(current): 0.0} + expanded = 0 + while queue and expanded < max_expansions: + _, _, current = heapq.heappop(queue) + if current.cost > best_cost[state_key(current)]: + continue + expanded += 1 + if at_goal(current.poses[-1], goal): + return reconstruct_path(current, expanded) + connection = analytic_expansion(current, goal, obstacle_tree, bounds) + if connection is not None: + return reconstruct_path(connection, expanded) + for steer in np.linspace(-MAX_STEER, MAX_STEER, N_STEER): + for direction in (1, -1): + neighbor = extend(current, direction * PRIMITIVE_LENGTH, + float(steer), obstacle_tree, bounds) + if neighbor is None: + continue + key = state_key(neighbor) + if neighbor.cost >= best_cost.get(key, math.inf): + continue + best_cost[key] = neighbor.cost + pose = neighbor.poses[-1] + heuristic = math.hypot(pose[0] - goal[0], pose[1] - goal[1]) + heapq.heappush(queue, (neighbor.cost + HEURISTIC_WEIGHT * heuristic, + next(serial), neighbor)) + return None + + +def body_outline(pose, heading_column, front, rear): + """Return a closed body polygon in world coordinates for plotting.""" + heading = pose[heading_column] + local = np.array([[front, WIDTH / 2], [front, -WIDTH / 2], + [-rear, -WIDTH / 2], [-rear, WIDTH / 2], [front, WIDTH / 2]]) + rotation = np.array([[math.cos(heading), -math.sin(heading)], + [math.sin(heading), math.cos(heading)]]) + return local @ rotation.T + np.array(pose[:2]) + + +def draw_frame(path, obstacles, start, goal, index): # pragma: no cover + """Draw the hitch path, trailer axle path, start/goal, and the two bodies.""" + plt.cla() + plt.plot(obstacles[:, 0], obstacles[:, 1], ".k", label="Obstacles") + plt.plot(path.poses[:, 0], path.poses[:, 1], "c--", label="Hitch path") + trailer_axles = path.poses[:, :2] - TRAILER_LENGTH * np.column_stack( + (np.cos(path.poses[:, 3]), np.sin(path.poses[:, 3]))) + plt.plot(trailer_axles[:index + 1, 0], trailer_axles[:index + 1, 1], + color="tab:orange", label="Trailer axle path") + for pose, label, color in ((start, "Start", "tab:green"), (goal, "Goal", "tab:red")): + plt.plot(pose[0], pose[1], "o", color=color, label=label) + plt.arrow(pose[0], pose[1], 2 * math.cos(pose[2]), 2 * math.sin(pose[2]), + color=color, width=0.1) + for column, front, rear in ((2, TRACTOR_FRONT, TRACTOR_REAR), + (3, TRAILER_FRONT, TRAILER_REAR)): + outline = body_outline(goal, column, front, rear) + plt.plot(outline[:, 0], outline[:, 1], "--", color="tab:red", alpha=0.4) + pose = path.poses[index] + for column, front, rear, color, label in ( + (2, TRACTOR_FRONT, TRACTOR_REAR, "tab:blue", "Tractor"), + (3, TRAILER_FRONT, TRAILER_REAR, "tab:orange", "Trailer"), + ): + outline = body_outline(pose, column, front, rear) + plt.plot(outline[:, 0], outline[:, 1], color=color, label=label) + motion = "Reverse" if path.distances[index] < 0 else "Forward" + angle = math.degrees(abs(wrap_angle(pose[2] - pose[3]))) + plt.title(f"Hybrid A* with trailer | {motion} | Articulation {angle:.1f}°") + plt.xlabel("x [m]") + plt.ylabel("y [m]") + plt.gca().set_aspect("equal", adjustable="box") + plt.xlim(obstacles[:, 0].min() - 2, obstacles[:, 0].max() + 2) + plt.ylim(obstacles[:, 1].min() - 2, obstacles[:, 1].max() + 2) + plt.grid(True) + plt.legend(loc="upper left", fontsize=8) + + +def example(): + """Return a parking maneuver with point-sampled walls, in metres/radians.""" + start = [10.0, 10.0, 0.0, 0.0] + goal = [30.0, 20.0, 0.0, 0.0] + horizontal = np.arange(-2.0, 45.1, 0.5) + vertical = np.arange(0.0, 35.1, 0.5) + box_x, box_y = np.arange(17.0, 23.1, 0.5), np.arange(13.0, 17.1, 0.5) + obstacles = np.vstack(( + np.column_stack((horizontal, np.zeros_like(horizontal))), + np.column_stack((horizontal, np.full_like(horizontal, 35.0))), + np.column_stack((np.full_like(vertical, -2.0), vertical)), + np.column_stack((np.full_like(vertical, 45.0), vertical)), + np.column_stack((box_x, np.full_like(box_x, 13.0))), + np.column_stack((box_x, np.full_like(box_x, 17.0))), + np.column_stack((np.full_like(box_y, 17.0), box_y)), + np.column_stack((np.full_like(box_y, 23.0), box_y)), + )) + return start, goal, obstacles + + +def main(): + """Run the deterministic trailer example; return its path for inspection.""" + start, goal, obstacles = example() + path = hybrid_a_star_planning(start, goal, obstacles) + if path is None: + raise RuntimeError("No trailer path found within the search limit") + print(f"Path found after {path.expanded_nodes} expansions; cost {path.cost:.2f}") + if show_animation: # pragma: no cover + plt.figure(figsize=(10, 6)) + for index in range(0, len(path.poses), 3): + draw_frame(path, obstacles, start, goal, index) + plt.pause(0.01) + draw_frame(path, obstacles, start, goal, len(path.poses) - 1) + plt.show() + return path + + +if __name__ == "__main__": + main() diff --git a/docs/modules/5_path_planning/hybrid_astar_trailer/hybrid_astar_trailer_main.rst b/docs/modules/5_path_planning/hybrid_astar_trailer/hybrid_astar_trailer_main.rst new file mode 100644 index 0000000000..1750870a69 --- /dev/null +++ b/docs/modules/5_path_planning/hybrid_astar_trailer/hybrid_astar_trailer_main.rst @@ -0,0 +1,97 @@ +Hybrid A* with a Trailer +============================= + +A car towing a passive trailer needs more space to turn, and reversing can +increase the angle between the two bodies. This example extends Hybrid A* to +search over both headings, check both vehicle footprints, and arrive with the +trailer aligned to the requested goal. + +.. image:: https://raw.githubusercontent.com/AtsushiSakai/PythonRoboticsGifs/e73e6c50d46b8a24d64594a672509bf63fc235d9/PathPlanning/HybridAStarTrailer/animation.gif + :alt: A tractor and trailer drive around an obstacle and reverse into the goal. + +The blue outline is the tractor and the orange outline is the trailer. The cyan +dashed line shows the hitch path; the orange line follows the trailer axle. +Green marks the start, and red marks the goal and its desired body outlines. +Black points are obstacles. The title shows forward/reverse motion and the +current articulation angle. + +Kinematic model +--------------- + +The hitch is at the tractor's rear axle. Let :math:`(x,y)` be its position, +:math:`\theta` the tractor heading, :math:`\phi` the trailer heading, +:math:`L` the tractor wheelbase, and :math:`d` the hitch-to-trailer-axle distance. +With signed speed :math:`v` and tractor steering angle :math:`\delta`, + +.. math:: + + \dot{x} = v\cos\theta, \qquad + \dot{y} = v\sin\theta, \qquad + \dot{\theta} = \frac{v}{L}\tan\delta, \qquad + \dot{\phi} = \frac{v}{d}\sin(\theta-\phi). + +The trailer axle is at +:math:`(x-d\cos\phi,\ y-d\sin\phi)`. +For straight forward travel the trailer tends to align with the tractor; +reversing can amplify the articulation +:math:`\gamma=\operatorname{wrap}(\theta-\phi)`. +The model has one on-axle trailer and no steerable trailer wheels. + +Search and goal connection +-------------------------- + +#. Discretize :math:`(x,y,\theta,\phi)` for the search, while keeping the + continuous pose at each node. Direction and the previous steering input are + also retained because they affect transition costs. +#. Expand forward and reverse constant-steering motion primitives. Integrate + the tractor along a circular arc and the trailer heading with fourth-order + Runge--Kutta, using signed distance steps. +#. Reject trajectories whose sampled poses collide, exceed the articulation + limit, or leave the hitch-position search bounds. +#. Try Reeds--Shepp connections for the tractor. Propagate the trailer along + every candidate and reject connections that leave its final heading outside + the goal tolerance. The trailer heading is never snapped to the goal. +#. Recover the continuous path through the accepted predecessor segments. + +The accumulated cost penalizes distance, reversing, direction changes, +steering, steering changes, and articulation. For one constant-steering segment, + +.. math:: + + \Delta g = w_{\mathrm{dir}}\ell + + w_\delta |\delta|\ell + + w_{\Delta\delta}|\delta-\delta_{\mathrm{prev}}| + + w_{\mathrm{switch}}\mathbf{1}_{\mathrm{direction\ change}} + + w_\gamma\int_0^\ell |\gamma(s)|\,ds, + +where :math:`\ell` is the positive segment length and +:math:`w_{\mathrm{dir}}` is larger for reverse motion. A weighted Euclidean +hitch-to-goal distance guides the search. This educational implementation uses +finite grid and control resolutions and a bounded number of expansions; it does +not guarantee completeness or the minimum-cost path. It does not perform the +trajectory-smoothing stage of Dolgov et al. + +The example uses 2 m position cells, 15 degree heading cells, and motion samples +no farther than 0.2 m apart. Both bodies are rectangles checked against obstacle +points at every motion sample. These are discrete collision checks, not a +continuous swept-volume calculation; obstacle boundaries must be sampled densely. +The articulation limit is 75 degrees. A goal connection must match the tractor +pose within numerical tolerance and the trailer heading within 5 degrees. +The returned heading retains this residual error. + +Code +---- + +.. autofunction:: PathPlanning.HybridAStarTrailer.trailer_hybrid_a_star.hybrid_a_star_planning + +.. autofunction:: PathPlanning.HybridAStarTrailer.trailer_hybrid_a_star.move + +References +---------- + +- `Dolgov et al., Practical Search Techniques in Path Planning for Autonomous Driving + `_. +- `LaValle, Planning Algorithms, Section 13.1.2.4: A car pulling trailers + `_. +- `Atsushi Sakai's HybridAStarTrailer reference implementation (Julia) + `_. diff --git a/docs/modules/5_path_planning/path_planning_main.rst b/docs/modules/5_path_planning/path_planning_main.rst index 5132760dc5..da879a1946 100644 --- a/docs/modules/5_path_planning/path_planning_main.rst +++ b/docs/modules/5_path_planning/path_planning_main.rst @@ -31,6 +31,7 @@ Path planning is the ability of a robot to search feasible and efficient path to reeds_shepp_path/reeds_shepp_path lqr_path/lqr_path hybridastar/hybridastar + hybrid_astar_trailer/hybrid_astar_trailer frenet_frame_path/frenet_frame_path coverage_path/coverage_path elastic_bands/elastic_bands \ No newline at end of file diff --git a/tests/test_hybrid_a_star_trailer.py b/tests/test_hybrid_a_star_trailer.py new file mode 100644 index 0000000000..d48d17d124 --- /dev/null +++ b/tests/test_hybrid_a_star_trailer.py @@ -0,0 +1,154 @@ +import math + +import conftest +import numpy as np +import pytest +from scipy.integrate import solve_ivp +from scipy.spatial import cKDTree + +from PathPlanning.HybridAStarTrailer import trailer_hybrid_a_star as m + + +EMPTY_TREE = cKDTree(np.empty((0, 2))) +BOUNDS = (-100.0, 100.0, -100.0, 100.0) + + +@pytest.mark.parametrize("distance", [0.2, -0.2]) +def test_motion_matches_independent_kinematic_integration(distance): + pose = [2.0, -3.0, 0.4, -0.2] + steer = 0.35 + + def derivative(_, state): + return [math.cos(state[2]), math.sin(state[2]), + math.tan(steer) / m.WHEEL_BASE, + math.sin(state[2] - state[3]) / m.TRAILER_LENGTH] + + reference = solve_ivp(derivative, (0, distance), pose, rtol=1e-12, atol=1e-13) + np.testing.assert_allclose(m.move(pose, distance, steer), + reference.y[:, -1], atol=1e-9, rtol=0) + + +@pytest.mark.parametrize("distance", [10.0, -10.0]) +def test_straight_aligned_motion(distance): + pose = [1.0, 2.0, math.pi / 2.0, math.pi / 2.0] + np.testing.assert_allclose(m.move(pose, distance, 0.0), + [1.0, 2.0 + distance, *pose[2:]], atol=1e-12) + + +def test_forward_stabilizes_and_reverse_amplifies_articulation(): + pose = [0.0, 0.0, 0.0, 0.2] + assert 0 < m.move(pose, 0.2, 0.0)[3] < pose[3] + assert m.move(pose, -0.2, 0.0)[3] > pose[3] + + +@pytest.mark.parametrize("obstacle", [[3.0, 0.0], [-8.0, 0.0], [0.0, m.WIDTH / 2]]) +def test_collision_detects_either_body_and_boundary(obstacle): + assert not m.collision_free([[0, 0, 0, 0]], cKDTree([obstacle])) + + +def test_collision_uses_trailer_heading(): + pose = [0, 0, 0, math.pi / 2] + assert not m.collision_free([pose], cKDTree([[0, -8]])) + assert m.collision_free([pose], cKDTree([[-8, 0]])) + assert m.collision_free([pose], EMPTY_TREE) + + +def test_entire_motion_is_checked_for_collision(): + root = m.Node([(0, 0, 0, 0)], [0.0]) + tree = cKDTree([[10.0, 0.0]]) + assert m.collision_free([root.poses[-1], (20, 0, 0, 0)], tree) + assert m.extend(root, 20.0, 0.0, tree, BOUNDS) is None + + +def test_reverse_motion_rejects_jackknife(): + root = m.Node([(0, 0, 0, math.radians(70))], [0.0]) + assert m.extend(root, -3.0, 0.0, EMPTY_TREE, BOUNDS) is None + + +def test_state_key_includes_trailer_and_wraps_angles(): + root = m.Node([(0, 0, math.pi, math.pi)], [0.0]) + wrapped = m.Node([(0, 0, -math.pi, -math.pi)], [0.0]) + different_trailer = m.Node([(0, 0, math.pi, math.pi - 0.5)], [0.0]) + assert m.state_key(root) == m.state_key(wrapped) + assert m.state_key(root) != m.state_key(different_trailer) + + +def test_analytic_connection_rejects_wrong_trailer_goal_heading(): + root = m.Node([(0, 0, 0, 0)], [0.0]) + goal = [20, 0, 0, 0.5] + assert m.analytic_expansion(root, goal, EMPTY_TREE, BOUNDS) is None + + +def test_singular_tractor_connection_falls_back_to_grid_search(): + radius = m.WHEEL_BASE / math.tan(m.MAX_STEER) + start, goal = [0, 0, 0, 0], [radius, radius, math.pi / 2, 0.9] + root = m.Node([start], [0.0]) + assert m.analytic_expansion(root, goal, EMPTY_TREE, BOUNDS) is None + path = m.hybrid_a_star_planning(start, goal, [], max_expansions=100) + assert path is not None + assert path.expanded_nodes > 1 + assert m.at_goal(path.poses[-1], goal) + + +def test_coincident_tractor_pose_does_not_ignore_trailer_heading(): + root = m.Node([(0, 0, 0, 0)], [0.0]) + goal = [0, 0, 0, 0.5] + assert not m.at_goal(root.poses[-1], goal) + assert m.analytic_expansion(root, goal, EMPTY_TREE, BOUNDS) is None + + +def test_tractor_goal_alone_is_not_success(): + assert not m.at_goal((10, 0, 0, 0.3), (10, 0, 0, 0)) + assert m.at_goal((10, 0, math.pi, -math.pi), (10, 0, -math.pi, math.pi)) + + +@pytest.mark.parametrize("goal_x", [20.0, -20.0]) +def test_straight_plan_preserves_inputs_and_reaches_all_four_coordinates(goal_x): + start, goal, obstacles = [0.0] * 4, [goal_x, 0.0, 0.0, 0.0], [] + original = start[:], goal[:], obstacles[:] + path = m.hybrid_a_star_planning(start, goal, obstacles) + assert path is not None + np.testing.assert_allclose(path.poses[0], start) + np.testing.assert_allclose(path.poses[-1], goal, atol=1e-10) + assert np.all(np.sign(path.distances[1:]) == np.sign(goal_x)) + assert (start, goal, obstacles) == original + + +def test_already_at_goal_and_colliding_start(): + pose = [0, 0, math.pi, -math.pi] + path = m.hybrid_a_star_planning(pose, pose, []) + assert path is not None + assert len(path.poses) == 1 + assert path.cost == 0 + assert m.hybrid_a_star_planning(pose, pose, [[0, 0]]) is None + + +def test_goal_collision_and_search_limit_return_none(): + start, goal = [0, 0, 0, 0], [25, 0, 0, 0] + assert m.hybrid_a_star_planning(start, goal, [[25, 0]]) is None + assert m.hybrid_a_star_planning(start, goal, [], max_expansions=0) is None + wall = [[10, y] for y in np.arange(-15.0, 15.1, 0.5)] + assert m.hybrid_a_star_planning(start, goal, wall, bounds=(-10, 40, -5, 5), + max_expansions=100) is None + + +def test_example_search_is_continuous_feasible_and_reaches_trailer_goal(monkeypatch): + monkeypatch.setattr(m, "show_animation", False) + start, goal, obstacles = m.example() + path = m.main() + assert path.expanded_nodes > 1 # Exercise the grid search, not just a shortcut. + np.testing.assert_allclose(path.poses[0], start) + assert m.at_goal(path.poses[-1], goal) + assert np.any(path.distances > 0) and np.any(path.distances < 0) + assert np.max(np.abs(path.distances)) <= m.MOTION_RESOLUTION + 1e-12 + assert np.max(np.abs(path.steers)) <= m.MAX_STEER + assert m.valid_poses(path.poses, cKDTree(obstacles), BOUNDS) + for previous, pose, distance, steer in zip( + path.poses[:-1], path.poses[1:], path.distances[1:], path.steers[1:] + ): + np.testing.assert_allclose(m.move(previous, distance, steer), pose, + atol=1e-10, rtol=0) + + +if __name__ == "__main__": + conftest.run_this_test(__file__)