FARO: Feasibility-Aware Robot Motion Optimization

Figure 1: Motion optimization and real-world execution with FARO. Top: snapshots of a trajectory produced by motion optimization. Bottom: snapshots from executing the optimized trajectory on the real robot.

1. Abstract

Fast planning of novel behaviors in unseen scenarios remains a fundamental challenge in robotics. The high-dimensional, hybrid, and underactuated nature of humanoid loco-manipulation continues to hinder the realization of this goal. In this paper, we address this challenge by proposing a nested kino-dynamic framework for rapid feasibility checking and dynamically consistent trajectory generation given a candidate contact sequence. By integrating this module with a feasibility-guided tree search and a Large Language Model (LLM)-based contact plan sampling strategy, we demonstrate that the proposed framework can substantially improve the search process. Furthermore, we show that the generated trajectories can be tracked using a reinforcement learning (RL)-based controller and show that the resulting trajectories are of sufficiently high quality for execution in real-world loco-manipulation scenarios.

2. Background & Motivation

Humanoid loco-manipulation involves searching over an extremely high-dimensional underactuated system over a long horizon, creating combinatorial explosion. Two paradigms each have limitations:

• Trajectory Optimization (TO): Combines discrete contact sequence search with trajectory optimization as a relaxation of mixed-integer formulations. But combinatorial growth makes it computationally expensive, requiring simplification through simplified models, task-specific heuristics, or hand-designed high-level actions, limiting discoverable complex dynamic behaviors.
• Reinforcement Learning (RL): Successful in locomotion due to cyclic task nature and extensive reward shaping, but ineffective for complex long-horizon loco-manipulation skills. Recent advances rely on tracking demonstrations or human-to-humanoid retargeting, bypassing the exploration problem but requiring suitable reference motions.

Exploration remains the central obstacle: restricted in TO to manage computational complexity and avoided in RL through imitation or retargeting. FARO directly addresses this through more efficient multi-contact motion exploration—rapidly pruning infeasible contact decisions to reduce combinatorial burden without relying on predefined behavioral abstractions or reference motions.

3. Core Method

3.1 Contact Mode Sequence

Let $\mathcal{I}$ be the set of all contact interfaces. Each interface $a\in\mathcal{I}$ has contact state pair $(a,b)$ where $b\in\mathcal{I}$ indicates unilateral contact with interface $b$, and $b=\varnothing$ indicates $a$ is free. A contact mode is the complete assignment over all interfaces:

$$c=\{(a,b):\forall a\in\mathcal{I}\ \exists!\;b\in\mathcal{I}\cup\{\varnothing\}\}$$

The mode must be consistent: $(a,b)\in c\Leftrightarrow(b,a)\in c$. A contact-mode time sequence $\mathcal{C}=(c_{0},\ldots,c_{K-1})$ where the mode at time $s$ is:

$$c_{s}=\{(a,b_{s}):a\in\mathcal{I}\},\qquad s=0,\ldots,K-1$$

3.2 State and Control Spaces

State and control variables combine robot ($r$) and object ($o_{\ell}$) components:

$$x:=(x^{r},x^{o_{1}},\ldots,x^{o_{n_{o}}})\in\mathcal{X},\quad u:=(u^{r},u^{o_{1}},\ldots,u^{o_{n_{o}}})\in\mathcal{U}$$

Robot state $x^{r}:=(q^{r},v^{r},h^{r})$ includes configuration $q^{r}\in\mathrm{SE}(3)\times\mathbb{R}^{n}$, generalized velocity $v^{r}\in\mathfrak{se}(3)\times\mathbb{R}^{n}$, and centroidal momentum $h^{r}\in\mathbb{R}^{6}$. Robot control $u^{r}:=(\dot{v}^{r},\lambda_{1}^{r},\ldots,\lambda_{n_{ee}}^{r})$ includes generalized acceleration and end-effector wrenches $\lambda_{e}^{r}\in\mathbb{R}^{6}$.

3.3 Constraint Definitions

All end-effectors and environment contact interfaces modeled as rectangular patches with patch-to-patch unilateral contact. Instantaneous constraints include: normal alignment and zero normal separation (7a), patches within admissible region (7b), unilateral contact and pyramidal Coulomb friction approximation (7c), torsional friction and center-of-pressure bounds (7d). For sticking contacts persisting across timesteps, a no-slip condition (8) applies.

Joint position, velocity, and torque-speed limits:

$$q^{r}_{\min}\leq q^{r}\leq q^{r}_{\max},\quad |v^{r}|\leq v^{r}_{\max},\quad |\tau_{i}|\leq\tau_{\max,i}-\frac{\tau_{\max,i}}{v_{\max,i}}|v_{i}|$$

The last inequality is a linear torque-speed envelope: available torque decreases affinely with joint speed, reaching zero at $v_{\max,i}$.

3.4 Hierarchical Feasibility Checks

Figure 3: Graphical overview of FARO optimization problems. Mode/edge optimization finds a single configuration satisfying individual contact mode or transition constraints. Kinematic sequence optimization finds one configuration per transition plus initial and terminal. Trajectory optimization searches for dynamically feasible motion over discretized trajectory.

FARO's core is a four-level hierarchical feasibility check, from cheap to expensive:

Mode/Edge feasibility: IK optimization problem (14) checking if a single configuration satisfies contact constraints. Infeasible if NLP doesn't converge within max iterations.

Kinematic Sequence Optimization (KSO): Given a contact mode sequence, solves for robot-object configurations verifying geometric feasibility without imposing time-dependent dynamics. Decision variables reduced by 74.8% on average vs TO, with 70.0% constraint coverage.

Trajectory Optimization (TO): Beyond KSO's geometric consistency, additionally enforces full robot-object dynamics. Solves for state trajectory, control trajectory, and one time-scaling factor per contact-mode stage.

Figure 2: High-level overview of FARO. The scene defines robot, movable objects, environment geometry, and available contact interfaces. Candidate contact plans from search, user-specified, or LLM methods are evaluated by hierarchical feasibility checks with cached results.

3.5 Feasibility-Guided Tree Search

Tree search rooted at initial mode, node selection using cost-based UCT variant:

$$\mathrm{UCT}(n) = \mathrm{cost}(n) + c\frac{\ln N(n)}{N(n)}$$

Expansion controlled by progressive widening. Candidate sequences pass through cheaper filters first, terminating at first failure. Reusable results stored in feasible/infeasible caches. Four search variants use different filter sets: {KSO}, {M,E,KSO}, {M,E,KSO,TO}, {TO}.

graph TD
    SCENE[Scene Definition] --> GEN[Contact Sequence Generator]
    GEN -->|candidate plans| CHECK{Hierarchical Feasibility Checks}
    CHECK -->|1.Mode M| M[Mode Feasibility]
    M -->|pass| E[Edge Feasibility]
    E -->|pass| KSO[Kinematic Sequence Opt.]
    KSO -->|pass| TO[Trajectory Optimization]
    TO -->|feasible| FEAS[Feasible Plan → Execute]
    M -->|fail| CACHE_F[Infeasible Cache]
    E -->|fail| CACHE_F
    KSO -->|fail| CACHE_F
    CACHE_F -->|feedback| GEN
    FEAS --> RL[RL Trajectory Tracking Controller]
    RL --> ROBOT[Real Humanoid Robot Execution]

4. Key Experiments

4.1 Qualitative Analysis on Human-Defined Sequences

Eight contact-mode sequences with varying lengths and complexity designed. Each mode discretized with 20 optimization knots in TO. Asterisked sequences executed on real robot with RL trajectory-tracking controller.

Table I: Computational performance and constraint coverage on human-specified contact sequences. KSO is ~2 orders of magnitude faster than TO with 70.0% average constraint coverage. * = executed on real robot.

KSO is approximately two orders of magnitude faster than TO (avg 0.60s vs 64.70s) while retaining 70.0% of TO constraint types on average. The 74.8% average decision variable reduction makes KSO a fast yet expressive feasibility check. KSO omits dynamics-related constraints since it optimizes one configuration per mode transition rather than a fully discretized trajectory.

4.2 Feasibility-Guided Tree Search

Figure 4: Tree Search Evaluation Scenes. Two box-placement tasks with same contact-mode search space but different kinematic and dynamic difficulty. Objective: place white box on platform.

Evaluated on two box-placement task variants, 5 runs each (different seeds), 2-hour budget per run, max depth 5 switches. Baseline: TO-only. Maximum branching factor 108.

Key finding: On the hard task, TO-only baseline finds 0 solutions, expands only 12.8 nodes; while {KSO} variant expands 814.6 nodes and finds 26.4 solutions on average. Filters excluding TO explore substantially faster during node expansion, invoking TO only after reaching the discrete goal, indicating KSO serves as an effective proxy for guiding search. The hard task highlights the benefit of mode and edge filtering in addition to KSO—cached infeasibility results allow previously checked queries to be rejected without re-solving, substantially reducing redundant exploration.

4.3 LLM Contact-Plan Exploration

Figure 5: LLM Contact-Plan Sampling Scenes. KSO feasibility filter evaluated on LLM-generated contact plans across four task scenarios. Each task's -b variant is more challenging than -a.

KSO evaluated as feasibility filter for LLM-generated contact plans across four task scenarios. KSO rapidly identifies infeasible sequences in LLM-generated plans, providing feedback for LLM to improve subsequent plans.

5. Limitations & Future Work

• Planar contact interfaces (author-stated): Current formulation assumes planar contact interfaces; could be extended to richer geometries using differentiable signed-distance representations.
• Nested feasible sets (author-stated): The optimization hierarchy is designed around increasingly restrictive feasibility conditions with expected nested feasible sets, but a formal proof is left to future work.
• Long-horizon scaling (author-stated): Scaling to substantially longer-horizon tasks will likely require integrating FARO with higher-level task and motion planning or program search.
• False negatives: The few observed false negatives are primarily associated with poor initialization or insufficient solver iterations in KSO.

6. Conclusion

FARO is a feasibility-aware framework for rapidly discovering dynamic humanoid loco-manipulation behaviors from candidate contact-mode sequences. It hierarchically filters candidates using contact mode, transition, kinematic sequence, and full trajectory optimization checks. Early pruning and cached feasibility results reduce expensive trajectory optimizations while preserving promising contact sequences. Experiments across feasibility-guided tree search, LLM-generated, and human-designed plans show that mode/edge and kinematic sequence optimizations capture most relevant constraints at substantially lower cost than full trajectory optimization, enabling faster and broader exploration of candidate contact plans and improving solution discovery in challenging scenarios while producing very few false-negative classifications. This work demonstrates that guiding search through hierarchical feasibility checks rather than full trajectory optimization is an effective approach to balancing efficiency and quality in humanoid multi-contact motion planning.