CASSR: Continuous A-Star Search through Reachability for real time footstep planning

Footstep planning involves a challenging combinatorial search. Traditional A* approaches require discretising reachability constraints, while Mixed-Integer Programming (MIP) supports continuous formulations but quickly becomes intractable, especially when rotations are included. We present CASSR, a novel framework that recursively propagates convex, continuous formulations of a robot’s kinematic constraints within an A* search. Combined with a new cost-to-go heuristic based on the EPA algorithm, CASSR efficiently plans contact sequences of up to 30 footsteps in under 125 ms. Experiments on biped locomotion tasks demonstrate that CASSR outperforms traditional discretised A* by up to a factor of 100, while also surpassing a commercial MIP solver. These results show that CASSR enables fast, reliable, and real-time footstep planning for biped robots.

💡 Research Summary

The paper introduces CASSR (Continuous A‑Star Search through Reachability), a novel footstep planning framework that combines the deterministic, low‑complexity nature of A* search with a continuous representation of robot kinematic constraints. Traditional A* approaches discretise the one‑step reachability set, limiting coverage of the feasible space, while Mixed‑Integer Programming (MIP) can model continuous constraints but becomes intractable when rotation degrees of freedom are included. CASSR resolves this tension by treating contact surfaces—not individual foot placements—as graph nodes and associating each node with a convex polytope that describes all positions the active foot can occupy while still being reachable from its parent node in a single step.

The core technical contribution is the recursive propagation of n‑step reachability. Starting from a 1‑step reachable polytope (derived from linearised kinematic limits such as stride length, height change, and yaw bounds), the algorithm computes the Minkowski sum of this polytope with the polytope representing the current node’s surface. The convex hull of the extreme points yields the n‑step reachable set, which is then intersected with all environment contact surfaces. Each non‑empty intersection creates a new child node, storing the effector identifier, surface ID, extreme points of the intersected region, and the yaw angle used for that expansion. Rotation is handled by discretising yaw angles, a common practice in the literature, which introduces a manageable combinatorial factor.

CASSR’s A* implementation follows the classic priority‑queue scheme: node cost = g (number of steps from start) + h (heuristic estimate to goal). The heuristic is a weighted version based on the Expanding Polytope Algorithm (EPA), an extension of GJK that efficiently computes the minimum distance between two convex polytopes (or a polytope and a point). By scaling this distance with the maximum stride length, the heuristic provides a lower bound on the remaining number of steps. Although this makes the heuristic inadmissible, the authors report that the algorithm still finds the optimal solution in all tested scenarios, and the weighting dramatically reduces the number of explored nodes.

Additional algorithmic safeguards include: (1) a “nodeAlreadyExpanded” check that merges nodes with identical effector‑surface pairs and similar geometry (within 2 cm), preventing redundant expansions; (2) a “hasContactSurfaceNotBeenLeft” rule that forbids a foot from revisiting a surface after it has left, which curtails pathological long branches; and (3) a simple edge cost of 1 per step, with optional rotation penalties left out for clarity. The final footstep sequence is obtained by solving a small Quadratic Program (QP) that enforces each foot to lie inside its assigned surface polytope while minimizing a stride‑length cost. Because the QP is linear in the constraints and convex in the objective, feasibility is guaranteed and solution times are negligible.

Experimental validation focuses on biped locomotion tasks with up to 30 steps. CASSR consistently generates plans in under 125 ms, outperforming a discretised A* baseline by up to a factor of 100 and a commercial MIP solver (Gurobi) by 5–10×. The method remains effective when yaw rotations are included, demonstrating that the continuous reachability model does not sacrifice real‑time performance. The authors also provide a video illustrating the planner in action.

In summary, CASSR delivers a practical, real‑time footstep planner that retains the richness of continuous kinematic constraints while leveraging the deterministic search properties of A*. Its recursive reachability propagation, EPA‑based heuristic, and lightweight QP post‑processing constitute a compelling solution for fast, reliable biped navigation, and the framework opens avenues for extensions to multi‑legged robots, more complex terrains, and learning‑enhanced heuristics.