Legged robots face significant challenges in navigating complex environments, as they require precise real-time decisions for foothold selection and contact planning. While existing research has explored methods to select footholds based on terrain geometry or kinematics, a critical gap remains: few existing methods efficiently validate the existence of a non-collision swing trajectory. This paper addresses this gap by introducing KCFRC, a novel approach for efficient foothold reachability analysis. We first formally define the foothold reachability problem and establish a sufficient condition for foothold reachability. Based on this condition, we develop the KCFRC algorithm, which enables robots to validate foothold reachability in real time. Our experimental results demonstrate that KCFRC achieves remarkable time efficiency, completing foothold reachability checks for a single leg across 900 potential footholds in an average of 2 ms. Furthermore, we show that KCFRC can accelerate trajectory optimization and is particularly beneficial for contact planning in confined spaces, enhancing the adaptability and robustness of legged robots in challenging environments.
Figures & tables
Figure 1: Kinematic Collision-Aware Foothold Reachability Criteria demonstration in barrier traversal. Blue cubes show validated reachable footholds, orange/red curves show initialized/optimized swing trajectories. The hexapod robot must carefully select footholds to navigate through the confined space. More supplemental materials are available at KCFRC Webpage .
Figure 2: Foothold Evaluation Criteria. (a) Foot Collision checking for foot-terrain interference, (b) Kinematic Feasibility ensuring joint limits compliance, and (c) Leg Collision detecting segment-obstacle interference. These criteria require explicit trajectory generation for validation.
Symbols
Description
p
Current foothold
q
Candidate foothold
X
Space used in the FRP
Df
Feasible domain in the FRP
Do
Occupied domain in the FRP
fP:E3→SE(3)
Torso pose query function
Table I: Symbol & Abbreviation
Figure 3: Reachability of the expected foothold. The foothold is reachable if feasible domain is not split by obstacles, and vice versa.
Figure 5: Diagram of the Sufficient Condition for Reachability (Theorem 1 ).
Figure 6: Collision detection method. (a) Collision detection under SDF. (b) Foot-collision-only, joint-collision-only and link-subdivision collision models.
Figure 7: Approximate feasible domain PITD.
Figure 8: Grid map and generated guiding surfaces. (a) Keypoint-weighted guiding surface creates smooth interpolation between key points with k=1,wi=1 . (b) Convolutional guiding surface smooths terrain features using n=5,l=0.03 m kernel . The grid map resolution is 0.05 m.
Figure 9: 2D intersection border search. (a) Intersection of Df and Sa . (b) schematic diagram of Algorithm 1 .
Figure 10: Shortest path in projected 2D feasible domain. The shortest path in this domain goes through concave points and connects start and goal points.
Figure 11: Reachability check experiment. Random convex polyhedra corridor is generated as the feasible domain, and stochastic fractal grid map as obstacles. Meanwhile, the shortest polygon path, visibility graph, and initialized trajectory are visualized.
Figure 12: Reachability check time w.r.t intersection border length. Linear fitting of the data is y=ax+b,a=0.00077,b=0.02722 .
Figure 13: Representative planning scenes for foothold reachability check comparison. Top Left&Mid: Hexapod dense planning scenes. Top Right: Hexapod sparse planning scenes. Mid-Row: Quadruped dense planning scenes. Bottom Row: Hexapod confined planning scenes. Planned contact states are recorded for foothold reachability check benchmarking. The grid map resolution is set to 2.5 cm/grid.
Figure 14: Foothold reachability check methods participate in the reachability check experiment for comparison. (a) FEC with predefined swing trajectory. (b) KCFRC. (c) RRT-Connect. (d) STOMP.
Parameters
Value
Description
Robot
ElAir Knee
0.05 m
Knee collision ball radius.
ElAir Foot
0.03 m
Foot collision ball radius.
A1 Knee
0.03 m
Knee collision ball radius.
A1 Foot
0.02 m
Foot collision ball radius.
Ground Truth
Table II: Key Parameters of the Methods
Method
Ave. Time(ms)
Max. Time(ms)
Ave. Accuracy
Ave. Precision
Ave. Recall
Hexapod Confined Planning Scenes
Ground Truth
827.852
18941.200
1.000
1.000
1.000
KCFRC(key)
50.574
78.740
0.996
0.998
0.986
KCFRC(conv)
2.580
4.070
0.993
0.999
0.974
RRT-C(1ms)
303.257
1085.290
0.998
1.000
0.992
RRT-C(50us)
209.600
948.055
0.972
1.000
0.889
Table III: Foothold Reachability Check Comparison
Figure 15: Comparing our proposed method with other methods in a U-shape clamped barrier planning scene considering both knee and foot collision, the final result trajectory is drawn in red. (b) The proposed planner returns a smooth swing trajectory in one shot. (c) MINCO-LBFGS optimizer returns success after many retries. (d) RRT-Connect finds a feasible swing trajectory with low quality even after interpolation. (e) STOMP returns success after many retries.
Figure 16: ElSpider 4 Air climbs across a barrier. (a) The robot is getting across a barrier 0.12m in width and 0.19m in height. (b) Snapshots of the robot and Rviz visualization. (c) Foot SDF curves with time of the snapshots annotated. The SDF value of each foot during the swing phase is no less than 0.02m, claiming no unexpected collision.
Planners in Config Space
Time ( ms )
Len. ( rad )
Cost ( s−3 )
Succ. Num.
Succ. Rate
Proposed
Mean
0.818
2.440
140.117
493
0.992
Max.
12.509
8.031
6079.370
Std.
0.871
1.010
365.728
MINCO LBFGS
Mean
1.447
3.296
147.635
490
0.986
Max.
9.025
5.352
697.719
Std.
0.909
0.647
71.668
Table IV: Comparison of swing trajectory planning performance
Figure 17: Real-world experimental hardware setups demonstrating KCFRC-equipped hexapod locomotion across diverse terrains. (a1, a2) Timber piles: ElSpider 4 Air navigating across irregular timber obstacles with varying heights from 0.1m to 0.4m. (b1, b2) Hole navigation: Hexapod climbing through confined spaces requiring precise foothold planning and collision avoidance. (c1, c2) Stairs: Robot ascending and descending stair structures with 0.12m step height and 0.35m depth, showcasing multi-level terrain traversal capabilities.
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.
Jiayi Wang, Steve Tonneau
State Key Laboratory of General Artificial Intelligence, BIGAI, Beijing 100080, China · University of Edinburgh, IPAB, Edinburgh EH8 9YL, U.K.
Legged robots offer a variety of automation applications in real-world scenarios. But areas that are difficult to traverse, like slopes, caves, or scaffolding, still pose a great challenge for traversal. To tackle this problem, we propose an optimized algorithm for evaluating the full actuatable wrench polytope for arbitrary contact scenarios. With our improved analysis algorithm, the torques for each joint of the robot can be calculated within a control frequency of 49 Hz. The achieved speedup allows for deployment within a regular control loop for actuating robot poses for different contact scenarios. We evaluated our stability controller extensively in simulation scenarios and validated its applicability by deploying it on actual walking robot hardware. The proposed controller achieved stability in very complex scenarios that are currently not achievable by any other controller.
Friedrich Graaf, Elias Birkefeld, Christian Eichmann +5
FZI Research Center for Information Technology, Karlsruhe, Germany · Machine Intelligence and Robotics Lab (MaiRo), Karlsruhe Institute of Technology (KIT), Karlsruhe, Germany
Walking over disconnected footholds requires selecting landing regions while adapting step timing to the robot's motion. We present a perceptive model predictive planner that combines discrete region selection, foot placement, and current-step duration in a mixed-integer quadratic program using step-to-step Divergent Component of Motion (DCM) dynamics. Onboard depth data are fused into a probabilistic heightmap in the stance foot frame. Convex steppable regions are extracted and shrunk by margins that increase with estimated height uncertainty. Linear constraints impose capturability-based bounds on DCM evolution and limit swing-foot retargeting using the remaining commanded swing time. Within-step replanning updates the initial DCM from measurements to respond to model mismatch and disturbances. We evaluate the framework on Digit in simulation over randomized footholds and on hardware over separated plates, with mean planner solve times of 11.4ms and 4.0ms, respectively.
Zhaoyang Xiang, Upama Pant, Ayonga Hereid
Mechanical and Aerospace Engineering, The Ohio State University, Columbus, OH 43210, USA.