💻 computer science

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

The paper introduces CASSR, a novel real-time footstep planning framework that integrates continuous convex reachability propagation with an EPA-based heuristic into an A* search, enabling biped robots to efficiently compute complex contact sequences up to 30 steps in under 125 ms while significantly outperforming traditional discretized A* and commercial MIP solvers.

Original authors: Jiayi Wang, Steve Tonneau

Published 2026-03-05

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Jiayi Wang, Steve Tonneau

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine you are trying to guide a clumsy, two-legged robot (like a futuristic robot dog or a humanoid) through a complex obstacle course. Your goal is to tell it exactly where to place its feet to get from point A to point B without falling over or hitting anything.

This is the problem of Footstep Planning.

The Problem: A Maze of Choices

Traditionally, solving this is like trying to navigate a maze where every single possible step you could take is a separate path.

The Old Way (Discretized A):* Imagine the robot can only step on a grid of tiny squares. It can step forward, backward, or sideways, but only to the center of a square. If the perfect spot to step is between two squares, the robot misses it. To make the robot precise, you have to make the grid incredibly fine, which creates millions of paths to check. It's like trying to find a needle in a haystack by checking every single straw one by one.
The Other Way (MIP): This is like trying to solve a giant, complex math equation that considers every possible step, rotation, and force all at once. While it finds the perfect answer, it takes so long to calculate that the robot is already frozen in place waiting for the answer.

The Solution: CASSR (The "Smart Navigator")

The paper introduces a new method called CASSR. Think of CASSR not as a robot checking a grid, but as a smart navigator with a "reachability bubble."

Here is how it works, using simple analogies:

1. The "Reachability Bubble" (Continuous vs. Discrete)

Instead of asking, "Can I step on square A, B, or C?", CASSR asks, "What is the entire shape of the area I can reach with my next step?"

Imagine your foot is a stamp. Instead of looking at specific dots, CASSR draws a smooth, continuous shape (a polytope) representing every single spot that foot can land on.
It then looks at the next stepping stone (a rock or a platform) and sees where that "reachability bubble" overlaps with the stone.
The Magic: It doesn't check every single point on the stone. It treats the whole overlapping area as one single option. This drastically reduces the number of choices the robot has to think about.

2. The "Cost-to-Go" Heuristic (The GPS Estimate)

In a maze, a good navigator needs a guess about how far they are from the exit.

Old methods just measure the straight-line distance (Euclidean distance).
CASSR uses a clever trick called the EPA algorithm. Imagine you are trying to fit a puzzle piece (the robot's foot) into a hole (the target). The EPA algorithm calculates the exact minimum distance needed to push the piece into the hole, even if the piece needs to be rotated. This gives the robot a much smarter "GPS estimate" of how many steps are left, helping it skip dead ends faster.

3. The Two-Stage Process (Planning the Route vs. Walking the Route)

CASSR splits the job into two easy steps:

Stage 1 (The Map): It quickly figures out the sequence of surfaces to step on (e.g., "Step on the big rock, then the small ledge, then the floor"). It does this using the "reachability bubbles" and the smart GPS estimate. This is incredibly fast.
Stage 2 (The Steps): Once it knows the sequence of surfaces, it solves a simple math problem (a Quadratic Program) to figure out the exact coordinates for each footstep to ensure the robot doesn't trip.

Why is this a Big Deal?

The paper tested this on a robot named Talos in three tricky scenarios:

Stairs: A simple climb.
Local Minima: A trap where the robot has to step away from the goal to eventually reach it (like backing up to go around a wall).
Narrow Passage: A tight squeeze where the robot has to turn sideways to fit.

The Results:

Speed: CASSR was up to 100 times faster than the old "grid" method and significantly faster than the complex math solvers.
Real-Time: It can plan a path of 30 steps in less than 125 milliseconds. That's faster than a human blink. This means the robot can plan while it's walking, reacting to changes instantly.
Smarter: Because it doesn't force the robot to stick to a grid, it finds solutions that the old methods miss (like stepping on the edge of a rock or turning precisely to fit through a gap).

The Bottom Line

CASSR is like upgrading a robot's brain from a checklist (checking every single square on a grid) to a visionary (seeing the whole landscape of possibilities at once). It allows robots to walk through complex, real-world environments quickly, safely, and without getting stuck in "thinking mode."

1. Problem Statement

Footstep planning for legged robots is a combinatorial problem of exponential complexity. The goal is to determine a sequence of contact locations (and orientations) that allow a robot to reach a target while satisfying kinematic constraints (reachability) and avoiding obstacles.

Traditional A Approaches:* Rely on discretizing the reachable space (e.g., a grid of possible foot placements). While computationally efficient, this discretization reduces the solution space, potentially missing feasible paths and increasing the number of nodes required to find a solution, especially when rotations are involved.
Mixed-Integer Programming (MIP): Supports continuous formulations and guarantees global optimality but suffers from high computational costs and intractability as the planning horizon (number of steps) or the inclusion of rotations increases.
The Gap: There is a lack of methods that combine the determinism and efficiency of A* with continuous reachability constraints, particularly for long-horizon planning involving rotations.

2. Methodology: CASSR Framework

The authors propose CASSR (Continuous A-Star Search through Reachability), a two-stage framework that reformulates the search from finding specific contact locations to finding sequences of contact surfaces.

A. Core Concept: Recursive Reachability

Instead of discretizing the space, CASSR propagates convex, continuous formulations of kinematic constraints within the A* search graph.

Node Definition: A node represents a convex contact surface (a polytope) rather than a single point. It defines the set of all potential positions for a foot that can provably be reached from the parent node in one step.
Expansion Mechanism:
1. Minkowski Sum: To compute the reachable set for the next step, the algorithm computes the Minkowski sum of the current reachable polytope and the robot's 1-step kinematic constraints.
2. Intersection: This resulting polytope is intersected with all available environmental contact surfaces.
3. Rotation Handling: Foot yaw orientations are discretized (e.g., $10^\circ$ increments). For each discrete orientation, the reachable polytope is rotated and intersected with surfaces. This introduces a branching factor but maintains the continuous nature of the position search within each branch.
Feasibility Guarantee: If a sequence of surfaces is found, a simple Quadratic Program (QP) is guaranteed to be feasible to compute the exact contact locations and orientations.

B. Heuristic Function (Cost-to-Go)

CASSR introduces a novel heuristic to estimate the distance to the target:

EPA Algorithm: Instead of simple Euclidean distance between points, the heuristic uses the Expanding Polytope Algorithm (EPA) to compute the minimum distance between the current reachable polytope and the target polytope (or point).
Weighted A:* This heuristic is not strictly admissible (it may overestimate the minimum steps) because it scales based on maximum stride length. Consequently, CASSR operates as a Weighted A*, sacrificing the theoretical guarantee of global optimality for significantly faster search speeds. However, experiments show it empirically finds the optimal step count.

C. Robustness Cost

To prevent the robot from stepping on the very edges of surfaces (where the foot might not fit), the final QP includes a slack variable that maximizes the minimum distance to the surface borders, ensuring robust foot placement.

3. Key Contributions

First Continuous A Solver:* The first A* implementation that utilizes a continuous representation of reachability constraints, enabling real-time planning over long horizons (up to 30 steps).
Novel EPA-Based Heuristic: A cost-to-go estimator using the EPA algorithm to efficiently approximate distances between polytopes and targets, effectively handling rotation constraints without complex tuning.
Robust Safety Cost: A mechanism to maximize the distance to contact surface borders, ensuring the entire foot fits within the planned contact area.
Two-Stage Architecture: Decoupling the combinatorial search (surface sequence via A*) from the continuous optimization (exact positions via QP), guaranteeing feasibility once a path is found.

4. Experimental Results

The authors evaluated CASSR on the Talos biped robot across three scenarios: "Stairs" (easy), "Local Minima" (requires escaping a suboptimal path), and "Narrow Passage" (requires turning sideways).

Performance vs. Discretized A:*
- CASSR outperformed traditional discretized A* by a factor of up to 100x in complex scenarios (especially those involving rotation).
- In the "Local Minima" scenario, CASSR was 6,600ms faster than discretized A* (48ms vs. 6626ms).
- CASSR explored significantly fewer nodes (up to 2000x fewer in some cases) because the continuous formulation reduces the need for branching to cover the space.
Performance vs. MIP:
- CASSR was 6x to 20x faster than a commercial MIP solver (Gurobi), even when the MIP was given the optimal horizon length in advance.
- MIP failed to solve the "Narrow Passage" scenario within reasonable time, whereas CASSR solved it in ~125ms.
Real-Time Capability:
- CASSR successfully planned sequences of up to 30 footsteps in under 125ms, enabling real-time integration into control pipelines.
Optimality: Despite using a weighted heuristic, CASSR consistently found the solution with the minimum number of steps (matching the ground truth generated by global solvers).

5. Significance and Conclusion

CASSR represents a significant advancement in legged robot locomotion planning. By bridging the gap between the efficiency of graph search (A*) and the flexibility of continuous optimization, it overcomes the limitations of both discretized grids (which miss solutions) and MIP (which are too slow for real-time).

Impact: It enables biped robots to perform complex, long-horizon tasks in dynamic environments with real-time responsiveness.
Future Work: The authors suggest extending the framework to non-cyclic loco-manipulation and integrating machine learning to handle more complex, non-linear constraint representations (e.g., dynamic collisions) that are currently difficult to formulate convexly.

In summary, CASSR demonstrates that continuous reachability propagation within an A* framework is a superior strategy for real-time footstep planning, offering a 100x speedup over traditional methods while maintaining high solution quality and robustness.