---
title: 'STOCS: Trajectory Optimization and Contact Selection'
url: https://www.emergentmind.com/topics/simultaneous-trajectory-optimization-and-contact-selection-stocs
type: topic
---

# STOCS: Trajectory Optimization and Contact Selection

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 [1903.03823][2508.12928][2503.07963].

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 $s$ [1903.03823]. 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 [1904.04595][2503.07963].

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 [1809.06436][2510.24069].

## 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 [2508.12928].

**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 [2407.16976][2306.06465]. 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 [2503.07963].

**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 [1904.04595][2510.24069][2508.12928]. Bayesian-optimized bi-level planners capture natural transition behaviors as task parameters (e.g., goal distance) change [1903.03823].

**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 [2305.11835]. 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 [2306.06465][2407.16976]. Hierarchical approaches achieve tractable computation for bimanual, multi-contact, and hardware settings by decomposing contact selection and trajectory optimization phases [2503.07963].

**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 [2407.16976].

**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 [2605.27972].

## 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 [2407.16976].

Binary encoding relaxations and tight convex approximations of nonconvex and bilinear constraints lead to 2–10× improvements in MILP-based contact-planning [2503.07963]. 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 [2510.24069].

## 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 [1904.04595][2510.24069].

**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 [2305.11835][2503.07963].

**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 [2407.16976].

**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 [2605.27972][2508.12928].

## 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 [2605.27972].
- 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 [1903.03823][2508.12928].
- 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 [1903.03823][1904.04595][2503.07963][2407.16976][2508.12928][2510.24069][2605.27972][2306.06465][2305.11835][1809.06436][2210.08627].

Source: https://www.emergentmind.com/topics/simultaneous-trajectory-optimization-and-contact-selection-stocs