Trusted Polytopic Action Sets for Fast Planning in Underactuated Systems

Akshay Jaitly; Siavash Farzan

eess.SYAug 25, 2026

Trusted Polytopic Action Sets for Fast Planning in Underactuated Systems

Authors: Akshay Jaitly, Siavash Farzan

Organizations: Onyx Robotics, Boston, MA 02210, USA · Department of Electrical Engineering, California Polytechnic State University, San Luis Obispo, CA 93407, USA

Abstract

Underactuated systems pose a challenge for convex motion planning because their dynamically feasible motions lie on a manifold of trajectories in function space. Building on our earlier formulation of polytopic action sets (PAS), this paper presents a method for rapidly generating, online, trusted convex sets of short-horizon actions for underactuated and potentially nonlinear systems. Around a nominal trajectory, we construct local finite-dimensional action coordinates in which each parameter vector encodes a complete nearby motion through an affine trajectory map, rendering collision-avoidance and control bounds linear. To remain consistent with the nonlinear dynamics, we introduce a dynamics-violation metric and extract a trusted convex inner approximation using an IRIS-inspired inflation procedure directly in action space. The resulting PAS are reusable convex families of actions that can be queried and composed with linear programs, and a PAS-guided tree expansion treats nodes as composed reachable families rather than single trajectories, coupling local nonlinear fidelity with convex reuse for longer-horizon planning. The planner solves cluttered planar scenes in tens of milliseconds (14-78x faster than a kinodynamic RRT baseline) and reduces terminal error on a nonlinear underactuated benchmark by 26-86% over sampling and NLP baselines.

Figures & tables

Panel of Figure 1 — Fig. 1: Geometric view of PAS construction. Top: a local LTV approximation around the nominal parameterizes nearby trajectories as affine functions of the action parameter $\gamma$ . Bottom: in these action coordinates, a trusted PAS (red polytope) is extracted as a convex subset of the feasible-trajectory manifold. Here $\mathcal{M}$ is the dynamics-consistent trajectory manifold; $x^{*}(\cdot)$ the nominal trajectory and $x(\cdot)$ a neighbor on $\mathcal{M}$ ; $\delta x,\delta u(\cdot)$ the state/input deviations; and $T_{x^{*}}\mathcal{M}$ the tangent space, locally identified with the action-coordinate space $\gamma\in\mathbb{R}^{n_{\gamma}}$ .

Panel of Figure 2 — Fig. 2: (a) Reachable sets of a single PAS for the 2D double integrator, drawn at sampled time indices; the dashed curve is the nominal trajectory $x^{*}(t)$ about which the PAS is constructed, and sampled collision constraints enter as halfspaces separating the reachable sets from the obstacle. (b) Composing PAS extends the reachable set toward the goal, the mechanism underlying our PAS-guided tree expansion (Sec. IV ).

Fig. 3: Dynamics-defect level sets $\zeta(\gamma)$ in action space. The trusted PAS (orange) lies inside the sublevel set $\zeta(\gamma)\leq\zeta_{\max}$ .

$n_{\mathrm{obs}}$	Occ. [%]	Success [%]	Expansion Iters	Plan [ms]	Seg.
4	4.5	100	5.8	9.6	2.5
8	9.0	100	10.5	18.3	3.0
12	13.6	100	13.2	32.5	3.3
16	18.1	100	20.0	40.8	3.8
20	22.6	100	38.6	66.5	4.4
25	28.3	100	59.8	127.0	4.9

TABLE I: LTI obstacle avoidance: planning cost vs. workspace occupancy (

M\,{=}\,10

N\,{=}\,20

, 50 trials per row). Lower block: high-occupancy regime.

Fig. 4: Representative PAS-RRT solutions at 12 obstacles (left) and 16 obstacles (right). Magenta: final trajectory; colored tubes: forward reachable sets; dashed: nominal steering rollouts.

Fig. 5: Scenes with non-trivial topology: a narrow passage (left) and a bug-trap enclosing the goal (right). PAS-RRT solves both; trajectories are minimum-effort QP solutions.

Fig. 6: Single-PAS cartpole goal reachability ( $M\,{=}\,40$ , $\zeta_{\max}\,{=}\,0.05$ ). Left: $W$ -masked terminal error vs. horizon; right: relative improvement over direct sampling. PAS outperforms sampling and the NLP baseline.

$M$	$T$	$M_{\mathrm{iter}}$	Expansion Iters	Depth	Total	Build	LP	Resid.
	[s]				[ms]	[ms]	[ms]
Shorter horizon
20	0.75	1	41.5	5.0	1297	8.1	21.2	0.016
30	1.50	1	5.5	2.0	184	16.3	18.8	0.024
Longer horizon
10	3.00	1	1.0	1.0	26	19.0	3.7	0.001

TABLE II: PAS-RRT cartpole swing-up results (

\Delta t{=}0.015

s, 20 trials, median). Resid.: avg. per-step dynamics defect along solution.

Motion Planning

Motion planning finds safe, feasible paths and trajectories for robots and vehicles amid geometric, dynamic, and task constraints.

See all 405 papers

Hierarchical Motion Planning and Control under Unknown Nonlinear Dynamics via Predicted Reachability
Mar 31, 2026Zhiquan Zhang, Melkior OrnikMotion Planning Dynamical Systems
University of Illinois Urbana-Champaign, IL, 61801 USA.
Continuous-Time Safe Planning and Control for Agile Quadrupeds
Oct 26, 2026Gage MacLin, Mason Peterson, Camilla Tabasso +4Safe Motion Planning Quadruped Robot Navigation
Amazon Fulfillment Technologies & Robotics, Westborough MA, USA. · University of Iowa, Iowa City, IA 52240. · Massachusetts Institute of Technology, Cambridge, MA 02139, USA.
Conformal Constraint Tightening for Chance-Constrained Motion Planning with Unknown Dynamics
Jul 24, 2026Shubham Natraj, Bruno Sinopoli, Yiannis KantarosConstrained Motion Planning Chance-Constrained Motion Planning
Department of Electrical and Systems Engineering, Washington University in St. Louis, 1 Brookings Dr, St. Louis, MO 63130, USA · School of Electrical, Computer and Energy Engineering, Arizona State University, 650 E. Tyler Mall, Tempe, AZ 85281, USA

Mar 31, 2026J/K move · Enter open · S save

Hierarchical Motion Planning and Control under Unknown Nonlinear Dynamics via Predicted Reachability

Zhiquan Zhang, Melkior Ornik

University of Illinois Urbana-Champaign, IL, 61801 USA.

Motion Planning Dynamical Systems

Open paper AskPDF arXiv

Autonomous motion planning under unknown nonlinear dynamics requires learning system properties while navigating toward a target. In this work, we develop a hierarchical planning-control framework that enables online motion synthesis with limited prior system knowledge. The state space is partitioned into polytopes and approximates the unknown nonlinear system using a piecewise-affine (PWA) model. The local affine models are identified once the agent enters the corresponding polytopes. To reduce computational complexity, we introduce a non-uniform adaptive state space partition strategy that refines the partition only in task-relevant regions. The resulting PWA system is abstracted into a directed weighted graph, whose edge existence is incrementally verified using reach control theory and predictive reachability conditions. Certified edges are weighted using provable time-to-reach bounds, while uncertain edges are assigned information-theoretic weights to guide exploration. The graph is updated online as new data becomes available, and high-level planning is performed by graph search, while low-level affine feedback controllers are synthesized to execute the plan. Furthermore, the conditions of classical reach control theory are often difficult to satisfy in underactuated settings. We therefore introduce relaxed reachability conditions to extend the framework to such systems. Simulations demonstrate effective exploration-exploitation trade-offs with formal reachability guarantees.

Abstract

Motion Planning

Explore similar work

Hierarchical Motion Planning and Control under Unknown Nonlinear Dynamics via Predicted Reachability

Continuous-Time Safe Planning and Control for Agile Quadrupeds

Conformal Constraint Tightening for Chance-Constrained Motion Planning with Unknown Dynamics