💻 computer science

FARO: Feasibility-Aware Robot Motion Optimization

This paper introduces FARO, a framework that combines a nested kino-dynamic feasibility checker, an LLM-guided contact planner, and a reinforcement learning controller to enable fast, high-quality trajectory generation and execution for complex humanoid loco-manipulation tasks in unseen scenarios.

Original authors: Michal Ciebielski, Shafeef Omar, Aaron Johnson, Majid Khadiv

Published 2026-07-22

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Michal Ciebielski, Shafeef Omar, Aaron Johnson, Majid Khadiv

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 trying to teach a robot to do a complex dance, like juggling a box while climbing a ladder. This isn't just about moving arms and legs; it's about figuring out exactly where to put its feet, hands, and the object it's holding at every single moment. In the world of robotics, this is called "loco-manipulation," and it's a massive headache for engineers. The problem is that the robot has to choose from millions of possible moves, but most of them are impossible—like trying to stand on your head while holding a heavy suitcase. If the robot tries to plan every single step in perfect detail before moving, it gets stuck in a math maze and never finishes. If it just guesses randomly, it wastes time trying impossible moves. The goal is to find a way to quickly spot the "bad" ideas so the robot can focus its brainpower on the "good" ones, all while making sure the final plan is physically possible to execute in the real world.

Enter FARO (Feasibility-Aware Robot Motion Optimization), a new method designed to be the robot's "reality check" before it even starts moving. Think of planning a robot's movement like a detective solving a mystery. Usually, detectives (or robots) might try to solve the whole case at once, which takes forever. FARO changes the game by using a series of quick, increasingly strict filters. First, it asks, "Is this pose even physically possible?" (like checking if a human could stand on one hand). If yes, it moves to the next question: "Can the robot get from this pose to the next one without falling?" (checking the transition). Only if those quick checks pass does it run the super-expensive, full-blown simulation that calculates every tiny force and movement.

The paper introduces a "hierarchy" of these checks, acting like a sieve that gets finer and finer. The first layer is a quick "Mode/Edge" check, which is like a sketch artist quickly drawing a pose to see if the limbs fit. The second layer is a "Kinematic Sequence" check, which connects those sketches to see if the robot can flow from one pose to another without breaking its own joints. The final layer is the full "Trajectory Optimization," which is the heavy-duty physics engine that simulates the entire dance in slow motion. By using the quick sketches to throw out the impossible ideas early, FARO saves a massive amount of time. The authors found that this middle layer (the Kinematic Sequence Optimization) is about 74.8 times faster than the full simulation while still catching about 70% of the rules that the full simulation checks.

The researchers tested this idea in three different ways. First, they gave the robot human-designed plans for tasks like "Pick and Place" and "Double Catch." They found that the quick checks could verify these plans almost instantly, and when they actually ran the plans on a real robot, the robot successfully performed the tasks using a reinforcement learning controller. Second, they used FARO to guide a "tree search" algorithm, which is like a robot exploring a maze of possibilities. In a difficult task where the robot had to place a box on a platform, a standard method that only used the slow, full simulation found zero solutions in two hours. However, the FARO-guided search found 26.4 solutions on average in the same time, exploring hundreds of more paths because it didn't get bogged down by impossible ideas. Finally, they tested FARO with an AI (a Large Language Model) that generated random ideas for how the robot should move. The FARO filter acted as a gatekeeper, quickly rejecting the AI's bad ideas and letting the good ones through, speeding up the process by up to 15.5 times in harder scenarios without ever accidentally rejecting a plan that actually worked (a "false negative" rate near zero).

In short, FARO doesn't invent new ways for robots to move; instead, it invents a smarter way to look for them. It suggests that by checking the "geometry" of a plan first (does it fit?) before checking the "physics" (does it work?), robots can discover complex, dynamic behaviors much faster. The authors show that this approach works in simulations and on real hardware, proving that you don't need to run the full, expensive physics engine for every single guess to find a solution. It's a step toward robots that can figure out how to handle new, messy situations on the fly, rather than just repeating pre-programmed moves.

Technical Summary: FARO: Feasibility-Aware Robot Motion Optimization

Problem Statement

The generation of novel, complex behaviors for humanoid loco-manipulation in unseen scenarios remains a fundamental challenge in robotics. This difficulty arises from the high-dimensional, hybrid, and underactuated nature of the systems, which necessitates simultaneous exploration of motion and feasible force spaces over long horizons.

Existing approaches face significant limitations:

Trajectory Optimization (TO): While effective for specific tasks, combining discrete contact search with TO leads to combinatorial explosion. To maintain tractability, these methods often rely on simplified models, task-specific heuristics, or hand-designed high-level actions, limiting the discovery of complex dynamic behaviors.
Reinforcement Learning (RL): RL has succeeded in cyclic locomotion tasks but struggles with complex, long-horizon loco-manipulation skills. Recent successes often rely on tracking demonstrations or human-to-humanoid retargeting, which bypass the core problem of discovering behaviors in novel situations rather than solving the exploration problem itself.

The central obstacle identified is exploration: TO restricts it to manage complexity, while RL largely avoids it through imitation.

Methodology

The paper proposes FARO (Feasibility-Aware Robot Motion Optimization), a nested kino-dynamic framework designed to rapidly check feasibility and generate dynamically consistent trajectories given a candidate contact sequence. The framework integrates three core components:

1. Hierarchical Feasibility Checks

FARO employs a hierarchy of increasingly restrictive optimization-based tests to prune infeasible contact decisions early, reducing the computational burden of full trajectory optimization. These checks share common constraints (contact, collision, dynamics, limits) but differ in variables and scope:

Mode/Edge Optimization: An inverse kinematics problem that verifies if a single contact mode or a transition between two modes is kinematically feasible. It solves for a single configuration satisfying contact, collision, and joint limits.
Kinematic Sequence Optimization (KSO): Given a sequence of contact modes, this optimization solves for $K+1$ configurations (one per transition plus initial/terminal). It enforces geometric consistency, contact constraints, and collision avoidance across the sequence but omits time-dependent dynamics. It acts as a fast proxy for dynamic feasibility.
Full Trajectory Optimization (TO): The most restrictive check, solving for state trajectories, controls, and time-scaling factors over a discretized trajectory. It enforces full robot-object dynamics, contact constraints, and limits.

2. Feasibility-Guided Tree Search

The framework integrates these checks into a tree search algorithm (Algorithm 1) for exploring discrete contact-mode sequences.

Search Strategy: Uses a cost-based variant of Upper Confidence bounds applied to Trees (UCT) with progressive widening to balance exploitation and exploration.
Filtering: Before a candidate node is added to the tree, it passes through a sequence of filters (Mode, Edge, KSO, TO). If a cheaper filter (e.g., KSO) fails, expensive checks (TO) are skipped.
Caching: Results of feasibility checks are stored in caches ( $K_{feas}$ , $K_{infeas}$ ) to avoid re-solving optimization problems for previously evaluated modes and transitions.

3. Contact Plan Generation

The framework is agnostic to how candidate contact sequences are generated. The paper demonstrates its integration with:

Model-based tree search.
Large Language Model (LLM)-based sampling (using GPT-5.5) to generate diverse contact plans.
Human-specified contact plans.

4. Execution

Generated trajectories are transferred to a real humanoid robot using a pre-trained Reinforcement Learning (RL)-based trajectory-tracking controller.

Key Contributions

FARO Pruning Strategy: A novel optimization-based pruning strategy for multi-contact motion planning that accelerates search by filtering infeasible candidates early without introducing significant false-negative classifications.
Feasibility-Guided Tree Search: The application of FARO to a tree search algorithm, demonstrating substantial performance improvements over direct trajectory-optimization-based exploration.
Systematic Evaluation: A comprehensive evaluation of the approach across three distinct generation methods: model-based search, LLM-guided generation, and human input.

Results

The paper evaluates FARO in three settings:

A. Human-Defined Contact Sequences

Performance: The Kinematic Sequence Optimization (KSO) was approximately two orders of magnitude faster than full Trajectory Optimization (TO) (average 0.60s vs. 64.70s).
Efficiency: KSO reduced decision variables by an average of 74.8× while retaining 70.0% of the constraint types present in TO.
Real-World Execution: Trajectories generated via this hierarchy were successfully executed on a real humanoid robot for tasks like "Double Catch" and "Pick Place."

B. Feasibility-Guided Tree Search

Solution Discovery: In a "Hard" box-placement task, a baseline TO-only search found 0 solutions and expanded only 12.8 nodes on average. In contrast, the FARO variant (using Mode, Edge, and KSO filters) expanded 814.6 nodes and found 26.4 solutions on average.
Efficiency: Filters excluding TO during node expansion allowed for much faster exploration, invoking TO only after reaching the discrete goal.

C. LLM-Based Contact Plan Sampling

Filtering Efficacy: When used as a pre-filter for LLM-generated plans, KSO provided speedups ranging from 2.4× to 15.5× depending on task difficulty.
Accuracy: The False Negative Rate (FNR) remained near 0.0%, indicating that KSO rarely rejected plans that were feasible under full TO. False positives (plans deemed feasible by KSO but infeasible by TO) were observed but did not prevent the discovery of valid solutions.

Significance and Claims

The paper claims that FARO represents a step toward the efficient, on-the-fly generation of complex loco-manipulation behaviors in real-world environments. Its significance lies in:

Bridging the Exploration Gap: It addresses the central obstacle of exploration by enabling efficient search through multi-contact motion spaces without relying on predefined behavioral abstractions or reference motions.
Scalability: By decoupling geometric feasibility (KSO) from dynamic feasibility (TO), the framework allows for broader exploration of the contact space, making it feasible to discover complex skills that would be computationally prohibitive with direct TO.
Practicality: The integration with RL-based controllers demonstrates that the generated trajectories are of sufficient quality for real-world execution.

The authors note that the current formulation assumes planar contact interfaces and that future work may involve integrating FARO with higher-level task and motion planning to manage the combinatorial complexity of even longer-horizon tasks.