STOCS: Trajectory Optimization and Contact Selection

• STOCS is an optimization framework that integrates discrete contact selection with continuous trajectory generation to ensure physically feasible robotic motion.
• It employs advanced methods such as mixed-integer programming, contact-implicit nonlinear programming, and hierarchical decompositions to handle nonlinear dynamics and combinatorial challenges.
• Applications in legged locomotion and manipulation have demonstrated real-time performance and efficient solving of high-dimensional contact-rich tasks.

Simultaneous Trajectory Optimization and Contact Selection (STOCS) encompasses a class of optimization-based frameworks that unify the discrete selection of contact sequences or patches and the continuous generation of physically feasible trajectories for manipulation and locomotion. STOCS approaches balance hard combinatorial aspects (contact/gait/mode/patch selection) with nonlinear dynamics, hybrid constraints, and physical feasibility. Core applications span contact-rich manipulation, legged locomotion, and multi-modal robot behaviors, with leading instantiations based on mixed-integer programming, contact-implicit nonlinear programming, hierarchical decompositions, and hybrid tree/graph search frameworks.

1. Formal Problem Structure and Modeling Paradigms

STOCS problems jointly optimize over discrete and continuous decision variables. For legged locomotion, this includes contact schedules, footstep locations, phase durations, and full-body states/controls. For manipulation, the discrete choices encode which robot or environment contacts are active, their locations on the object or surface, and the mode (stick, slip, separation); continuous decisions include robot and object states, velocities, and applied wrenches (Seyde et al., 2019, Dhédin et al., 18 Aug 2025, Shirai et al., 11 Mar 2025).

Typical variables include:

• Discrete: contact/binary sequence $c_{k,i}\in\{0,1\}$, patch/region/primitive assignments $p_{k,i}\in\mathbb{Z}$, mode switches, gait selectors $T_{ij}\in\{0,1\}$, contact surface indicators $z_t^{i,p}\in\{0,1\}$.
• Continuous: robot/object configurations, velocities, controls, contact forces $\lambda_{l,k}\in\mathbb{R}^3$, Bézier coefficients, timing parameters, and auxiliary variables for hybrid-enabling relaxations.

Constraints encode:

• Hybrid rigid/centroidal/underactuated dynamics (Euler–Lagrange, centroidal, phase-based, or ODE-collocation),
• Unilateral contact, friction cone, complementarity (gap $\phi$ and normal force $\lambda_n$, i.e., $\phi \geq 0,\ \lambda_n\geq 0,\ \phi\,\lambda_n=0$),
• Mode or event transitions, kinematic reachability, workspace and obstacle avoidance, joint/actuator limitations,
• Contact activation/deactivation (e.g., Big-M, complementarity/relaxations, explicit patch/foot binary selection),
• Trajectory continuity and terminal task constraints.

2. Bi-level, Contact-Implicit, and Mixed-Integer Formulations

Bi-level optimization partitions STOCS into a discrete upper level (UL, e.g., contact/gait/patch planner) and a continuous lower level (LL, e.g., trajectory optimizer). For instance, in legged locomotion, the UL selects a contact schedule $s$ given context $z$ (terrain, task), and the LL solves the resulting nonlinear program (NLP) for trajectory and controls under fixed $p_{k,i}\in\mathbb{Z}$0 (Seyde et al., 2019). Upper level performance is modeled as a black-box, using Gaussian process regression and Bayesian optimization to search discrete sequences (Bayesian Trajectory Optimization).

Mixed-Integer Convex Programming (MICP/MIQCQP/MILP) is employed for simultaneous contact sequence, gait, motion, and region assignment planning. Binary variables encode contact on/off, gait sequencing, and region allocation. Convex constraints and strong relaxations (binary-encoded McCormick envelopes) yield tractable formulations and global optimality up to solver tolerance (Aceituno-Cabezas et al., 2019, Shirai et al., 11 Mar 2025).

Contact-implicit nonlinear programming directly folds continuous contact forces and complementarity into the constraints via Mathematical Programs with Complementarity Constraints (MPCC). Higher-order collocation, phase-based Bézier representations, and analytical enforcement of certain constraints (e.g., Bézier-convex hull for friction) mitigate the combinatorial explosion of naive mode enumeration (Patel et al., 2018, Kim et al., 28 Oct 2025).

3. Algorithmic Architectures and Solvers

Tree/Graph Search Cascades: Approaches such as MCTS-based STOCS interleave discrete MDP or tree search over contact sequence/patches with trajectory optimization that certifies whole-body feasibility for each contact plan. Expansion, exploration, rollout, and reward backpropagation ensure both diversity and physical consistency, enabling acyclic and multi-contact gaits (Dhédin et al., 18 Aug 2025).

Exchange and Oracle Methods: Frameworks for high-fidelity geometry manipulation (e.g., dense point-cloud models with tens of thousands of vertices) use exchange-based MPCCs with oracles to select a minimal set of salient active contact points per time step or phase, greatly reducing the search space and constraint load (Zhang et al., 2024, Zhang et al., 2023). Maximum-violation or spatial/temporal smoothing oracles dynamically add new active contacts only when constraint residuals indicate necessity.

Hierarchical Decomposition: Some STOCS algorithms employ a multi-stage hierarchical solve:

1. Kinematics-only NLP warm-start (collision-free object/robot trajectories),
2. MILP contact selection (convex relaxations of bilinear terms, combinatorial contact assignment),
3. Full NLP trajectory refinement (dynamics, exact complementarity and friction, softmode switching), with cut generation used to handle MILP infeasibility (Shirai et al., 11 Mar 2025).

Sequential Quadratic Programming (SQP), SQP-RTI, CasADi, IPOPT, qpSWIFT, Gurobi, SNOPT: These solvers are commonly utilized at various stages, depending on the structure (continuous, MIQP, SQP, MPCC, NLP).

4. Applications: Manipulation, Locomotion, Multi-Modal Planning

Legged Locomotion: STOCS formulations enable automatic discovery of contact schedules and dynamically feasible body motions across flat, non-flat, and uncertain terrains. Mixed-integer and phase-based decompositions allow flexible gaits, robust to significant surface variation, with sub-second solve times and real-robot transfer demonstrated on platforms including HyQ and Unitree Go2 (Aceituno-Cabezas et al., 2019, Kim et al., 28 Oct 2025, Dhédin et al., 18 Aug 2025). Bayesian-optimized bi-level planners capture natural transition behaviors as task parameters (e.g., goal distance) change (Seyde et al., 2019).

Manipulation: In non-prehensile long-horizon tasks, such as planar pushing and sliding, STOCS frameworks leverage demonstration-derived mode sequences to efficiently overcome local minima and generalization failures in high-dimensional hybrid spaces (Xue et al., 2023). For multi-modal dexterous manipulation, infinite-programming-based STOCS can reason about arbitrarily complex object contacts, transitions between prehensile/non-prehensile behaviors, and object–environment interaction (Zhang et al., 2023, Zhang et al., 2024). Hierarchical approaches achieve tractable computation for bimanual, multi-contact, and hardware settings by decomposing contact selection and trajectory optimization phases (Shirai et al., 11 Mar 2025).

Contact-rich, High-Fidelity Geometries: Exchange-based STOCS methods identify active contacts from surfaces with tens of thousands of candidate vertices, making large-scale, physically valid manipulation problems tractable without a combinatorial explosion in constraints (Zhang et al., 2024).

Dynamic and Hybrid Tasks: STOCS frameworks achieve real-time multi-contact planning on robots with redundant DoF under model/estimation uncertainty, exploiting global search over contact sets and local MPC replanning for robustness to perception and actuation errors (Zhang et al., 27 May 2026).

5. Computational Strategies: Efficiency, Scalability, and Oracle Design

Stochastic and sampling-based components (MCTS trees, GP-Bayesian BO) mitigate the curse of dimensionality in discrete action spaces. Oracle-based contact selection reduces the variable count per trajectory optimization call by multiple orders of magnitude, evidenced empirically by planning times scaling from hours (full MPCC) to seconds (STOCS with oracle) in high-fidelity settings (Zhang et al., 2024).

Binary encoding relaxations and tight convex approximations of nonconvex and bilinear constraints lead to 2–10× improvements in MILP-based contact-planning (Shirai et al., 11 Mar 2025). Analytical structure—Bézier-closing for phase-based trajectories, differentiation-matrix mapping—enables continuous NLPs to satisfy exact translational dynamics across arbitrarily many contact points and legs (Kim et al., 28 Oct 2025).

6. Experimental Benchmarking and Performance Trends

Locomotion: Sub-second solve times per gait cycle, real robot traversals of multi-terrain scenarios, and robust gait selection in non-coplanar, uneven, or steep slopes on HyQ and other robots. Automatic transition between walk, trot, or cautious gaits emerges from a unified MIQCQP (Aceituno-Cabezas et al., 2019, Kim et al., 28 Oct 2025).

Manipulation: For planar multi-contact pushing and pivoting with demonstration-based warm starting, success rates and trajectory precision dramatically exceed zero-shot policies. Hierarchical MILP–NLP pipelines achieve high solution quality and feasibility with sharp gains in computation for challenging bimanual and multi-surface scenarios (Xue et al., 2023, Shirai et al., 11 Mar 2025).

Contact-rich/High-dimensional geometry: STOCS leveraging oracular contact selection yields 10³–10⁴× speedups over monolithic MPCCs, converging on challenging 3D tasks (pivot/slide/roll) with only a handful of active contacts per time step, enabling practical solutions for objects with 10⁴–10⁵ surface points (Zhang et al., 2024).

Real-World Hardware: Robust transfer of contact-optimized plans to hardware seen across bimanual manipulation, long-horizon pushing, legged traversals, and nonprehensile rearrangements; closed-loop schemes adaptively replan under perceptual drift and actuation uncertainty (Zhang et al., 27 May 2026, Dhédin et al., 18 Aug 2025).

7. Outlook and Current Frontiers

State-of-the-art STOCS frameworks are extending to:

• Real-time and perception-uncertain settings, leveraging surrogate models, cascaded MIQP–MPC pipelines, and active contact re-selection (Zhang et al., 27 May 2026).
• Combinatorially deep models (multi-finger, multi-patch, generalizable to full polyhedral friction and 3D manipulation).
• Datasets for learning contact-rich policies and online adaptivity via data-driven function approximation, replacing expensive GPs or oracles (Seyde et al., 2019, Dhédin et al., 18 Aug 2025).
• Limitations include scalability bottlenecks for thousands of candidate contacts in vanilla MPCCs (hence reliance on oracle pruning), mixed-integer scaling for fine discretizations, and offline enumeration for some approaches.

STOCS represents the architectural core unifying modern contact-rich motion and manipulation planning: integrating the combinatorial complexity of contact sequencing with nonlinear, physically accurate continuous optimization, enabled by hierarchical, oracle-driven, or hybridized algorithmic solutions (Seyde et al., 2019, Aceituno-Cabezas et al., 2019, Shirai et al., 11 Mar 2025, Zhang et al., 2024, Dhédin et al., 18 Aug 2025, Kim et al., 28 Oct 2025, Zhang et al., 27 May 2026, Zhang et al., 2023, Xue et al., 2023, Patel et al., 2018, Natarajan et al., 2022).