Source-linked AI summary
Trusted Polytopic Action Sets for Fast Planning in Underactuated Systems
Akshay Jaitly, Siavash Farzan
TL;DR
Underactuated feasible motions lie on a dynamics-consistent manifold, making direct convex planning difficult. The paper builds local affine action coordinates and refines linearly constrained actions into trusted PAS using an action-space IRIS-inspired procedure. PAS can be queried and composed for reachable-family tree planning, achieving 14–78× faster cluttered-scene planning and 26–86% lower terminal error on a nonlinear benchmark.
Problem
Underactuated feasible motions lie on a thin dynamics-consistent manifold, complicating direct extensions of convex free-space methods to kinodynamic planning.
Method
The method parameterizes nearby trajectories around nominal motions with local action coordinates, linearizes constraints, and uses a dynamics trust test with action-space IRIS-ZO refinement to generate trusted PAS.
Results
14–78× faster planning than a kinodynamic RRT baseline and 26–86% lower terminal error than direct sampling on the nonlinear benchmark.
Takeaways & Limitations
Reusable convex action families support LP queries, composition, and tree expansion over reachable sets rather than point states.
Abstract
from arXiv · showhide
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.
I. INTRODUCTION
The paper moves convexification into local action coordinates to generate trusted PAS for underactuated systems, then reuses them in tree expansion for longer-horizon planning.
- Underactuated feasible motions lie on a thin dynamics-consistent manifold, limiting direct extensions of convex free-space planning.
- Local action coordinates parameterize complete nearby trajectories around a nominal motion while making collision and control constraints linear.
- Trusted PAS retain actions within a prescribed nonlinear dynamics-consistency tolerance instead of convexifying the full manifold globally.
- PAS-guided tree expansion grows composed reachable families rather than single trajectories.
- IRIS-ZO is adapted to action space with batched oracle evaluations to compute convex inner approximations online.
II. POLYTOPIC ACTION SETS
PAS represent short-horizon trajectory families as convex polytopes in finite-dimensional action space, whose reachable sets, queries, and compositions remain tractable through linear constraints and programs.
- PAS is a convex polytope whose parameter vectors encode short-horizon trajectories through an affine trajectory map.
- Reachable Sets: A single PAS produces time-indexed reachable sets, while sampled collision constraints become halfspaces separating those sets from obstacles.
- Reachable Sets: Because reachable states are linear images of a PAS, reachable sets are polytopes and membership, goal, and closest-state queries reduce to LPs.
- Composability: Sequential PAS remain convex after imposing linear inter-segment continuity constraints.
- Composability: Composing trusted PAS inductively yields continuous multi-segment trajectories and supports post-hoc trajectory optimization as one convex program.
III. PAS GENERATION
PAS generation linearizes nearby motions around a nominal feasible trajectory, encodes constraints in action coordinates, and refines the result against nonlinear dynamics.
- The method constructs trusted PAS by building local action coordinates, imposing linear constraints, and applying a nonlinear trust-region test.
- Local coordinates parameterize nearby motions of nonlinear underactuated systems in a finite-dimensional space where constraints become linear.
- The nominal feasible trajectory is discretized over horizon T into N steps before local LTV linearization.
- Local Trajectory Parameterization: A fixed basis represents input deviations with static coefficients, including PWL knot values that define within-horizon control degrees of freedom.
- Local Trajectory Parameterization: The resulting perturbed trajectories are affine in γ, with γ = 0 recovering the nominal trajectory.
- Local Trajectory Parameterization: Endpoint constraints become linear feasibility conditions on γ.
B. Linear Collision and Control Constraints
Local action coordinates make sampled collision and actuator constraints linear, producing a convex family of locally admissible actions while exposing the nonlinear-dynamics limitation of the linearized model.
- Local action coordinates let sampled geometric and actuator constraints be enforced directly as linear inequalities in γ.
- Actuator bounds can be enforced on control points when the control parameterization is a convex combination of known points.
- Combining collision and control inequalities yields Alin, a convex family of locally admissible actions that retains the nominal action γ = 0.
- The linearized construction is sufficient for LTI or LTV systems but does not guarantee consistency with full nonlinear dynamics away from the nominal trajectory.
C. Dynamics-Consistency Trust Region
The dynamics-consistency trust region measures how parameterized trajectories depart from true nonlinear dynamics and bounds that departure by a user-chosen tolerance.
- The dynamics-violation metric sums squared one-step residuals between parameterized states and true-dynamics integrations on the discretization grid.
- The nominal parameter γ = 0 has zero violation, while increasing ∥γ∥ makes the linear approximation less accurate and increases ζ(γ).
- The trust region is the sublevel set Rtrust(ζmax) = {γ | ζ(γ) ≤ ζmax}, with ζmax chosen by the user.
- Smaller tolerances produce more conservative action sets, whereas larger tolerances broaden exploration at reduced local-model fidelity.
D. Zeroth-Order Action-Space Inner Approximation
The paper approximates the generally nonconvex trusted region by iteratively cutting an initial linearized polytope using batched, zeroth-order feasibility tests in action space.
- IRIS-ZO is adapted to action space to compute a convex inner approximation of the trusted-region intersection.
- The feasibility oracle requires only forward trajectory evaluation and residual computation, enabling large batched evaluations without gradients or nonlinear solves.
- Starting from Alin, the algorithm samples candidate parameters in parallel and bisects toward infeasible samples to refine the polytope.
- The resulting Atrust is an approximate, sample-resolved convex inner approximation whose conservatism increases for strongly nonlinear or poorly conditioned systems.
IV. PAS-GUIDED TREE EXPANSION
PAS-guided tree expansion represents nodes as composed convex families of multi-segment motions, allowing the planner to reason over reachable sets rather than individual states.
- Trusted PAS serve as tree edges, and each node stores a sequence of action sets representing motion segments.
- All constraints are linear in the composed parameter vector Γv, so the feasible set Cv is a convex polyhedron whenever nonempty.
- The terminal reachable set comprises states reachable through a continuous sequence of trusted local actions.
- The superscript T in XT_v denotes terminal rather than horizon length.
B. Expansion Procedure
Each tree expansion samples a target, selects the nearest cached node using a weighted state distance, and finds a reachable seed through a linear program.
- A random target xrand is drawn, optionally biased toward xgoal, to guide tree expansion.
- The planner selects node v⋆ by minimizing weighted distance between its characteristic state and xrand.The diagonal matrix W masks irrelevant state components, reducing selection to a nearest-neighbor lookup.
- One LP finds the closest point in the node’s reachable set under a weighted ℓ∞-norm, producing a reachable seed xseed.
3) Steer and build a new PAS:
From a reachable seed, the planner steers toward the sampled target, builds a trusted PAS, and appends it while enforcing continuity and checking goal reachability.
- Steer and build a new PAS:: A short-horizon steering routine produces a nominal trajectory toward xrand, from which a trusted PAS Anew is constructed.The new child appends Anew while enforcing continuity at xseed.
- Steer and build a new PAS:: The child node represents the composed reachable family formed by joining the selected node and Anew at xseed.Both the selected node and γnew = 0 reach xseed, establishing the joining point.
- Steer and build a new PAS:: The planner tests xgoal ∈ XT v+ with a linear feasibility program and terminates when feasible.
- Steer and build a new PAS:: If the goal is not reachable, the closest reachable point becomes the cached characteristic state for future node selection.
- Steer and build a new PAS:: The evaluation includes a 2D double integrator and a nonlinear cartpole, with local LTV PAS for the underactuated dynamics.The cartpole uses bounded control, a PWL basis, and a trust-region threshold ζmax = 0.05; the LTI model needs no refinement.
B. LTI Obstacle Avoidance
PAS-RRT performs fast obstacle planning and improves nonlinear cartpole reachability, while performance degrades in highly occupied or locally difficult settings.
- LTI Obstacle Avoidance: 3,600 randomized obstacle-planning trials span PWL knots M ∈ {5, 10, 15, 20}, horizons N ∈ {10, 20, 30}, and occupancy ≈4.5–28%.PAS-RRT succeeds in all trials under a 500-expansion budget and 0.5 m goal tolerance.
- LTI Obstacle Avoidance: 9.6 ms to 127 ms: runtime grows smoothly as occupancy increases from ≈4.5% to ≈28%, using 2.5–4.9 composed segments.
- LTI Obstacle Avoidance: 100% success holds through ≈32% occupancy, then falls to 92% near 41% and 82% at 63%.The reported failures are limited by workspace connectivity as free passages approach the collision margin.
- LTI Obstacle Avoidance: 14–78× faster: PAS-RRT matches the kinodynamic RRT baseline’s final path cost while replacing per-node simulation with convex action-set reuse.At 20 obstacles, runtime is ≈67 ms versus ≈2.0 s.
- LTI Obstacle Avoidance: All 50 narrow-passage and 50 bug-trap trials succeed, although the bug-trap requires 105 versus 10 expansions.The trajectories are minimum-control-effort QP solutions over the composed sets.
- Cartpole Swing-Up with Local LTV PAS: 26–86% lower terminal error: for M = 40 and T ∈ [1.5, 3.0] s, PAS improves over direct sampling while the NLP baseline does not.The NLP uses a soft dynamics penalty and returns a single point rather than a queryable family.
- Cartpole Swing-Up with Local LTV PAS: For T ≥ 3.0 s, cartpole planning solves in 1–2 expansions at depth 1–2, with per-step dynamics defect ≤0.006.Runtime is dominated by PAS construction, while additional cutting rounds mainly increase halfspaces and LP cost.
- Cartpole Swing-Up with Local LTV PAS: 100% success: sweeping ζmax from 0.005 to 1.0 keeps planning near 30 ms while residuals rise from ≈2×10^-3 to ≈1×10^-2.The tolerance trades dynamics fidelity against exploration smoothly, without a sharp threshold.
VI. CONCLUSIONS
The paper finds that trusted PAS support fast planning through reusable convex action families, while longer horizons reduce tree depth. It also identifies approximation quality and locality as important limitations.
- A single action-space cutting round is usually sufficient because additional rounds add LP overhead without reducing tree expansions.
- Longer per-PAS horizons reduce tree depth to as few as one or two expansions.
- PAS quality depends on cutting budget, system nonlinearity, and local-model conditioning, with conservatism excluding some feasible motions.
- The method remains local: performance degrades with poor nominal rollouts and passages narrower than the planner’s collision margin.