Pith. sign in

REVIEW 4 major objections 4 minor 1 cited by

MD-COAS unifies soft and hard constraint enforcement inside a training-free model-based diffusion planner, and its adaptive schedule aims for safer, faster, lower-cost trajectories in highly non-convex settings.

Reviewed by Pith at T0; open to challenge. T0 means a machine referee read the full paper against a public rubric. the ladder, T0–T4 →

T0 review · deepseek-v4-flash

2026-08-02 02:01 UTC pith:2NDDB6LK

load-bearing objection Solid empirical integration of iALM + CFS in diffusion planning, but the safety proof doesn't cover the implemented algorithm or the returned trajectory. the 4 major comments →

arxiv 2607.14455 v1 pith:2NDDB6LK submitted 2026-07-16 cs.RO

Motion Planning with Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling

classification cs.RO
keywords model-based diffusionmotion planningconstraint optimizationaugmented Lagrangianconvex feasible setadaptive schedulingtrajectory optimizationsafe planning
verification ladder T0 review T1 audit T2 compute T3 formal T4 reserved

The pith

A machine-rendered reading of the paper's core claim, the machinery that carries it, and where it could break.

This paper tries to show that a diffusion-based motion planner can be made both safe and efficient by combining two mechanisms instead of choosing one: a soft feasibility prior keeps mildly violated trajectory samples informative, and a hard convex-feasible-set projection corrects the worst violations. On top of that, the planner adaptively adjusts how much safety pressure it applies at each denoising step, treating safety enforcement as a tunable resource that tracks the diffusion schedule. The authors demonstrate on non-convex 2D maps and a 7-DoF arm avoidance task that this combined and scheduled approach achieves higher safe-and-success rates, faster convergence, and lower final costs than prior model-based and model-free diffusion planners. A reader should care because the result points toward training-free diffusion planning that remains reliable under tight, nonconvex constraints.

Core claim

MD-COAS formulates single-robot motion planning as constrained trajectory optimization inside a model-based diffusion sampler. It replaces the hard-indicator feasibility prior with an inexact Augmented Lagrangian soft kernel, so mildly infeasible samples still receive nonzero Monte Carlo weights, and it adds a Convex Feasible Set (CFS) hard projection that solves a linearized QP to push violating rollouts back into a local inner approximation of the safe set. The safety-enforcement parameters, including the Lagrangian duals, penalty, projection probability, active-set size, and CFS iteration budget, are updated at every reverse diffusion step from a robust batch violation statistic and a com

What carries the argument

The central object is the diffusion-step-dependent soft feasibility kernel p(τ) ∝ exp(−[J(τ)+λ_k^T ḡ(τ)+ρ_k/2 ‖ḡ(τ)‖²]/β), which acts as the Monte Carlo score-ascent reweighting target, paired with the CFS hard projection operator P_{I,H}, a QP that minimizes control perturbation subject to linearized collision half-spaces a^T u ≥ b obtained from signed-distance gradients and rollout Jacobians. The adaptive scheduler couples these via a residual statistic r_k and a compute dual ν_k, updating (λ_k, ρ_k) by damped projected ascent and adjusting (p_k, I_k, H_k) so that hard projection is invoked only when violations persist and computation budget allows.

Load-bearing premise

The load-bearing premise is Assumption 3: every scheduled CFS projection contracts the batch violation residual by a factor (1−α_k) up to an inexactness floor, and the selected half-spaces are valid local inner approximations; that contraction is proven for a single batch-level gate, but Algorithm 1 samples a separate Bernoulli mask per candidate, so the certificate does not directly cover the implemented sampling.

What would settle it

Log the batch violation residual R(B_k) over reverse diffusion steps on the L10 or 7-DoF benchmark; if it plateaus well above the bound in Eq. (40) because the selected active-set half-spaces miss the actual collision geometry, the central safety claim collapses. Alternatively, replace the per-candidate Bernoulli mask in Algorithm 1 with a single batch-level gate (matching Theorem 3) and check whether the reported SSR and zero-violation results still hold; if they degrade, the proof and the algorithm are not aligned.

Watch this falsifier — get emailed when new claim-graph text bears on it.

If this is right

  • If the central claim holds, safe motion planning can be done with a training-free diffusion sampler that enforces dynamics and constraints without demonstration data.
  • Combining a soft prior with a hard projection may avoid both the weight collapse of hard-indicator priors and the overly restrictive feasible sets that slow down pure CBF-style filters.
  • Treating safety enforcement as a scheduled compute resource suggests that planners can trade runtime for safety adaptively, concentrating projection effort on denoising steps where violations are large.
  • The use of rollout Jacobians and signed-distance gradients means the same structure extends to higher-dimensional, nonlinear systems such as articulated robot arms.
  • The primal-dual view offers a principled way to co-optimize diffusion annealing and constraint enforcement, potentially useful for other diffusion-based optimization tasks.

Where Pith is reading between the lines

These are editorial extensions of the paper, not claims the author makes directly.

  • The paper's Theorem 3 proves a contraction bound for a single batch-level Bernoulli gate, while Algorithm 1 samples an independent per-candidate mask; closing this gap, either by rewriting the algorithm to use a batch gate or by proving a per-candidate contraction, is needed for the stated safety certificate to cover the implemented sampling.
  • The same residual-plus-compute-dual scheduling pattern could be applied to other safety mechanisms inside diffusion, such as CBF filters, so the contribution may extend beyond CFS projections.
  • Hard projection effort is most valuable when the active set actually captures the collision geometry; an experiment sweeping active-set size H in cluttered maps would test how sensitive the guarantees are to this choice.
  • Because the soft kernel is monotone in violations, the method suggests a generic design rule: any continuous, monotone, zero-tolerant feasibility kernel should prevent sample collapse while still guiding the sampler toward safe regions.

Editorial analysis

A structured set of objections, weighed in public.

Desk editor's note, referee report, simulated authors' rebuttal, and a circularity audit.

Referee Report

4 major / 4 minor

Summary. The paper proposes MD-COAS, a training-free model-based diffusion planner that combines an inexact Augmented Lagrangian (iALM) soft feasibility prior with a Convex Feasible Set (CFS) hard projection operator, and adaptively schedules both the soft prior weights and the hard-projection effort across reverse diffusion steps. The method is evaluated on 2D non-convex obstacle-avoidance benchmarks and a 7-DoF arm task, reporting improved safety/success rates and lower costs compared to MBD, EB-MBD, MDOC, SafeDiffuser, and DPCC-C. The appendix contains a theoretical analysis intended to certify safety of the batch residual under the scheduled projection.

Significance. If the empirical results and the claimed safety guarantee both hold, the paper is a useful contribution: it addresses a real limitation of existing model-based diffusion planners (soft-vs-hard safety enforcement) and introduces a compute-aware adaptive schedule that is plausible and well-motivated. The benchmark coverage is broad (multiple difficulty levels, seeds, and a 7-DoF task), and the paper provides hyperparameter tables and a code link, which are strengths for reproducibility. However, the central theoretical claim that the method 'certifies safety' is not established by the proof as written; the mismatch between the theorem and the implemented algorithm, together with the assumption-driven nature of the key contraction bound, means the theoretical contribution currently overreaches. The empirical evaluation alone may still justify publication, but the safety-certification claim must be either rigorously supported or substantially narrowed.

major comments (4)
  1. [Appendix E, Theorem 3 (Eqs. 37–43) vs. Algorithm 1 (line 9)] The theorem analyzes a single batch-level Bernoulli gate Z_k applied to the whole batch, giving the residual recursion in Eq. (43). Algorithm 1 instead samples an independent per-candidate mask δ_{k,m} for each trajectory. For per-candidate masks, the corrected batch is a mixture of projected and unprojected trajectories, and the 0.9-quantile residual of that mixture is not the convex combination used in Eq. (43). The proof therefore does not cover the algorithm that is actually run. Either modify Algorithm 1 to use a single batch-level gate, or provide a version of Theorem 3 that holds under independent per-candidate masks.
  2. [Theorem 3 and final output (Algorithm 1, lines 12–14)] Even if the batch-residual bound were correct, it applies to the candidate batch \hat B_k, not to the final returned trajectory τ_1. The final output is obtained by MCSA averaging of candidates (Eq. (5)) followed by the reverse update (Eq. (3)); this output is never passed through the CFS projection. Averaging safe candidates in a nonconvex environment does not preserve safety, so the 'certifying safety' claim in C1 does not follow for the object the planner actually returns. The paper should either prove a safety guarantee for the final output or explicitly restrict the claim to the projected batch candidates.
  3. [Assumption 3 (Appendix A) and Appendix F] Assumption 3 postulates the existence of α_k and δ_{q,k} such that the CFS projection contracts the residual, along with validity of the selected half-spaces as local inner approximations. This is very close to the conclusion of Theorem 3: the theorem derives a contraction bound whose strength depends on α_k, but nothing in the paper verifies or bounds α_k, δ_{q,k}, or the active-set coverage for the nonconvex benchmarks. Appendix F explicitly concedes that global nonconvex feasibility depends on the quality of the local half-spaces, active-set coverage, and repeated relinearization. The paper should therefore not state that the method 'certifies safety' without either providing a verifiable condition (e.g., a computable active-set certificate) or replacing the global certification wording with a local, assumption-relative statement.
  4. [Section IV-C (Adaptive scheduling formula)] The paper never specifies the exact mapping from the batch violation statistic v_k (or r_k) and the compute dual ν_k to the scheduled values (p_k, I_k, H_k). It only says projection effort is increased when violations persist and decreased otherwise, with c_k≈p_k I_k H_k and the budget constraint in Eq. (24). Since adaptive scheduling is contribution C2, this underspecification is a significant reproducibility gap. Please provide the precise update rule or pseudocode for the scheduler used in the experiments.
minor comments (4)
  1. [C1 (Introduction)] The phrase 'certifying safety in highly non-convex constrained environments' overstates what the appendix proves. Consider replacing it with 'locally enforcing safety via a convex-feasible-set projection' or adding an explicit caveat about the local nature of the guarantee.
  2. [Figure 2 caption] The caption states 'We use blue color to represent (sample) trajectories,' but the figure also shows orange and other colors for barriers/boundaries. Please clarify the color coding more precisely.
  3. [Table I] The standard deviations are given as right superscripts (e.g., 13.53^2.90), which is hard to parse. Standard notation such as 'mean ± std' or separate columns would be clearer.
  4. [Equation (24)] The augmented Lagrangian surrogate in Eq. (24) mixes the primal trajectory residual with the compute schedule variables. It would help to state explicitly that this is a heuristic surrogate rather than a formal Lagrangian for the original problem, since the compute variables are not part of the original trajectory optimization.

Circularity Check

0 steps flagged

No significant circularity: the core results are benchmark-driven and the theoretical limitations are explicit gaps rather than circular reductions.

full rationale

The paper's central empirical claim (higher SSR, faster convergence, lower cost than baselines) is evaluated against external planners on independently generated 2D and 7-DoF benchmarks; no quantity reported as a prediction is fitted from the same benchmark outcomes, so the fitted-input-called-prediction pattern does not apply. The CFS hard projection is imported from a peer-reviewed external method (Liu et al. [26]) rather than justified solely by the present paper, and the iALM soft prior is standard [30]; these are not load-bearing self-citations. The adaptive scheduler is a component of the method, not a renamed version of the evaluation metric. The main caveat is in Appendix A: Theorem 3 is conditional on Assumption 3, which already postulates the contraction inequality E[R(P(B̃_k))] ≤ (1−α_k)E[R(B̃_k)] + δ_{q,k}. This means the theorem propagates an assumed projection contraction through a gate rather than independently proving CFS safety; however, that is an unsupported assumption / overclaim, not a definitional circularity, because the paper does not claim to derive Assumption 3 from the theorem. Additionally, the proof analyzes a batch-level Bernoulli gate Z_k while Algorithm 1 samples per-candidate masks δ_{k,m}, and the final τ_1 is never CFS-projected; this is a theory-algorithm mismatch and a correctness gap, not a circular reduction. Appendix F explicitly concedes the local nature of the guarantee. No step in the derivation reduces to its own input by construction, so the appropriate finding is no significant circularity.

Axiom & Free-Parameter Ledger

6 free parameters · 6 axioms · 0 invented entities

The method is a composition of known optimization and diffusion tools with a large set of hand-tuned scheduler hyperparameters. The safety guarantee rests on Assumption 3, which essentially postulates the contraction that Theorem 3 derives; the formal analysis also uses a batch-level gate not implemented in Algorithm 1. No new physical entities are introduced.

free parameters (6)
  • initial iALM dual λ0 and penalty ρ0 = λ0=0, ρ0=1 for MD-COAS (Table VI)
    Initialization of the adaptive soft-prior schedule, chosen by hand rather than derived.
  • dead-zone tolerance ξ = 5×10^-4 (Table VII)
    Determines when violations count as negligible; directly controls when safety enforcement is active.
  • dual step sizes ηλ, ην = ηλ=0.02, ην=0.05 (Table VII)
    Update rates for iALM dual and compute-dual projected ascent; not derived from data.
  • penalty hysteresis thresholds and rates = r=1e-4, r̄=5e-4, γ↑=2.0, γ↓=1.5 (Table VII)
    Shaping the non-monotonic penalty schedule; these thresholds and factors are hand-picked.
  • expected compute budget B = B=8.0 with cost proxy c_k=p_k I_k H_k (Table VII)
    Global knob trading projection effort against runtime; fixed by hand before experiments.
  • CFS margins and tolerances per benchmark = CFS margin 0.05 (2D) vs 0.35 (7-DoF); max constraints per point 5 vs 8; QP tolerance 1e-5..1e-2 (Table VI)
    Environment-specific settings that likely affect the reported 1.00 SSR/zero-violation results; no sensitivity analysis is provided.
axioms (6)
  • domain assumption Known, exact dynamics f and signed-distance functions φ_j are available and differentiable (Assumption 1).
    Problem Definition 1 and Appendix A; the entire MCSA and CFS rollout rely on exact f and gradients.
  • domain assumption Rollout map and costs have bounded first derivatives on the planner's reachable set (Assumption 1).
    Appendix A-C; needed for local Lipschitz continuity and QP linearization validity.
  • ad hoc to paper MCSA proposal error is bounded as E[R(˜B_k)|F_k] ≤ R(B_k)+δ_{m,k} (Assumption 2).
    Introduced specifically to make Theorem 3 work; no evidence is supplied for the size of δ_{m,k}.
  • ad hoc to paper CFS projection is a valid local inner approximation with contraction factor α_k and floor δ_{q,k} (Assumption 3).
    This is essentially the effectiveness of the hard projection that the theorem claims to prove, making the theoretical support partly circular.
  • domain assumption Obstacles are convex (Eq. 12) while benchmarks include non-convex union obstacles; safety guarantees transfer by union-of-convex decomposition and active-set truncation.
    The formal problem statement assumes convex obstacles; non-convex maps rely on local convexification and top-H active-set selection, which is not covered by the formal guarantee.
  • standard math Score-based reverse update (3) and MCSA estimator (6) correctly approximate the reverse diffusion drift.
    From DDPM/MBD literature, accepted as background and not re-derived in this paper.

pith-pipeline@v1.3.0-alltime-deepseek · 19399 in / 14343 out tokens · 138327 ms · 2026-08-02T02:01:33.204465+00:00 · methodology

0 comments
read the original abstract

Single-Robot Motion Planning (SRMP) in highly non-convex constrained environments, where robots must satisfy collision-free guarantees, dynamic feasibility, and task-related constraints, is challenging under complex constraints and computational limits. Recent Model-Based Diffusion (MBD) approaches recast the SRMP as trajectory optimization that samples from a posterior over trajectories, using known dynamics, and analytically estimates the score function from rollout samples to guide diffusion denoising toward a low-cost, clean trajectory without demonstration learning. While existing works further adapt MBD to constrained environments and showcase promising performance, they are still limited by (1) enforcing safety either via soft feasibility diffusion priors or hard projection operators, but lack a unified framework to integrate both, and (2) fixing safety enforcement to neglect the changing of diffusion scheduling. Therefore, we introduce Model-Based Diffusion via Constraint Optimization and Adaptive Scheduling (MD-COAS) for SRMP that unifies the inexact Augmented Lagrangian Method (iALM) soft diffusion prior with a Convex Feasible Set (CFS)-based hard projection operator, and adaptively schedules and co-optimizes safety enforcement, along with diffusion scheduling. Experiments demonstrate that our method achieves higher safety \& success rates, faster convergence, and lower final costs than baseline planners on randomly generated highly non-convex 2D benchmarks and a 7-DoF robot arm avoidance task.

Figures

Figures reproduced from arXiv: 2607.14455 by Bowei Li, Changliu Liu, Jianlin Dou, Yuner Zhang, Zhilin He.

Figure 1
Figure 1. Figure 1: Overview of Constraint Optimization (CO) and Adaptive Scheduling (AS) (Bottom Left) and Model-Based Diffusion for [PITH_FULL_IMAGE:figures/full_fig_p002_1.png] view at source ↗
Figure 2
Figure 2. Figure 2: Illustrations of the diffusion process of model-based methods for 2D Constrained Obstacle environment at L10 (Union [PITH_FULL_IMAGE:figures/full_fig_p005_2.png] view at source ↗
Figure 4
Figure 4. Figure 4: Adaptive scheduling of safety-enforcement variables [PITH_FULL_IMAGE:figures/full_fig_p006_4.png] view at source ↗
Figure 3
Figure 3. Figure 3: Overall performance comparison of model-based [PITH_FULL_IMAGE:figures/full_fig_p006_3.png] view at source ↗

discussion (0)

Sign in with ORCID, Apple, or X to comment. Anyone can read and Pith papers without signing in.

Forward citations

Cited by 1 Pith paper

Reviewed papers in the Pith corpus that reference this work. Sorted by Pith novelty score.

  1. Distributed Model-Based Diffusion For Scalable Multi-Robot Trajectory Optimization

    cs.RO 2026-07 reject novelty 6.0

    DMBD splits multi-robot model-based diffusion into per-robot denoising conditioned on peers' trajectories; the paper's proof of score equivalence contains an algebraic error.

Reference graph

Works this paper leans on

37 extracted references · 10 linked inside Pith · cited by 1 Pith paper

  1. [1]

    Denoising diffusion probabilistic models,

    J. Ho, A. Jain, and P. Abbeel, “Denoising diffusion probabilistic models,” Advances in Neural Information Processing Systems, vol. 33, pp. 6840– 6851, 2020

  2. [2]

    Diffusion policy: Visuomotor policy learning via action diffusion,

    C. Chi, Z. Xu, S. Feng, E. Cousineau, Y . Du, B. Burchfiel, R. Tedrake, and S. Song, “Diffusion policy: Visuomotor policy learning via action diffusion,”The International Journal of Robotics Research, vol. 44, no. 10-11, pp. 1684–1704, 2025

  3. [3]

    Planning with dif- fusion for flexible behavior synthesis,

    M. Janner, Y . Du, J. B. Tenenbaum, and S. Levine, “Planning with dif- fusion for flexible behavior synthesis,”arXiv preprint arXiv:2205.09991, 2022

  4. [4]

    Proba- bilistic roadmaps for path planning in high-dimensional configuration spaces,

    L. E. Kavraki, P. Svestka, J.-C. Latombe, and M. H. Overmars, “Proba- bilistic roadmaps for path planning in high-dimensional configuration spaces,”IEEE transactions on Robotics and Automation, vol. 12, no. 4, pp. 566–580, 1996

  5. [5]

    Motion planning,

    S. M. La Valle, “Motion planning,”IEEE Robotics & Automation Magazine, vol. 18, no. 2, pp. 108–118, 2011

  6. [6]

    Rrt-connect: An efficient approach to single-query path planning,

    J. J. Kuffner and S. M. LaValle, “Rrt-connect: An efficient approach to single-query path planning,” inProceedings 2000 ICRA. Millennium conference. IEEE international conference on robotics and automation. Symposia proceedings (Cat. No. 00CH37065), vol. 2. IEEE, 2000, pp. 995–1001

  7. [7]

    Probabilistic completeness of rrt for geometric and kinodynamic planning with forward propagation,

    M. Kleinbort, K. Solovey, Z. Littlefield, K. E. Bekris, and D. Halperin, “Probabilistic completeness of rrt for geometric and kinodynamic planning with forward propagation,”IEEE Robotics and Automation Letters, vol. 4, no. 2, pp. i–vii, 2018

  8. [8]

    Anytime motion planning using the rrt,

    S. Karaman, M. R. Walter, A. Perez, E. Frazzoli, and S. Teller, “Anytime motion planning using the rrt,” inInternational Conference on Robotics and Automation. IEEE, 2011, pp. 1478–1483

  9. [9]

    Model-based diffusion for trajectory optimization,

    C. Pan, Z. Yi, G. Shi, and G. Qu, “Model-based diffusion for trajectory optimization,”Advances in Neural Information Processing Systems, vol. 37, pp. 57 914–57 943, 2024

  10. [10]

    The cross- entropy method for optimization,

    Z. I. Botev, D. P. Kroese, R. Y . Rubinstein, and P. L’ecuyer, “The cross- entropy method for optimization,” inHandbook of statistics. Elsevier, 2013, vol. 31, pp. 35–59

  11. [11]

    Information-theoretic model predictive control: Theory and applications to autonomous driving,

    G. Williams, P. Drews, B. Goldfain, J. M. Rehg, and E. A. Theodorou, “Information-theoretic model predictive control: Theory and applications to autonomous driving,”IEEE Transactions on Robotics, vol. 34, no. 6, pp. 1603–1622, 2018

  12. [12]

    Eb-mbd: Emerging-barrier model- based diffusion for safe trajectory optimization in highly constrained environments,

    R. Mishra and I. R. Manchester, “Eb-mbd: Emerging-barrier model- based diffusion for safe trajectory optimization in highly constrained environments,”arXiv preprint arXiv:2510.07700, 2025

  13. [13]

    Diffusion models for constrained domains,

    N. Fishman, L. Klarner, V . De Bortoli, E. Mathieu, and M. Hutchin- son, “Diffusion models for constrained domains,”arXiv preprint arXiv:2304.05364, 2023

  14. [14]

    Safe and stable control via lyapunov- guided diffusion models,

    X. Cheng, X. Tang, and Y . Yang, “Safe and stable control via lyapunov- guided diffusion models,”arXiv preprint arXiv:2509.25375, 2025

  15. [15]

    Model-based diffusion optimal control for multi-robot motion planning,

    Z. He, Y . Shaoul, and J. Li, “Model-based diffusion optimal control for multi-robot motion planning,”arXiv preprint arXiv:2607.12423, 2026

  16. [16]

    Motion planning diffusion: Learning and planning of robot motions with diffusion models,

    J. Carvalho, A. T. Le, M. Baierl, D. Koert, and J. Peters, “Motion planning diffusion: Learning and planning of robot motions with diffusion models,” inInternational Conference on Intelligent Robots and Systems. IEEE, 2023, pp. 1916–1923

  17. [17]

    Con- strained diffusers for safe planning and control,

    J. Zhang, L. Zhao, A. Papachristodoulou, and J. Umenberger, “Con- strained diffusers for safe planning and control,”arXiv preprint arXiv:2506.12544, 2025

  18. [18]

    Multi-agent path finding in continuous spaces with projected diffusion models,

    J. Liang, J. K. Christopher, S. Koenig, and F. Fioretto, “Multi-agent path finding in continuous spaces with projected diffusion models,”arXiv preprint arXiv:2412.17993, 2024

  19. [19]

    Diffusion predictive control with constraints,

    R. Römer, A. von Rohr, and A. P. Schoellig, “Diffusion predictive control with constraints,”arXiv preprint arXiv:2412.09342, 2024

  20. [20]

    Constrained synthesis with projected diffusion models,

    J. K. Christopher, S. Baek, and N. Fioretto, “Constrained synthesis with projected diffusion models,”Advances in Neural Information Processing Systems, vol. 37, pp. 89 307–89 333, 2024

  21. [21]

    Safediffuser: Safe planning with diffusion probabilistic models,

    W. Xiao, T.-H. Wang, C. Gan, R. Hasani, M. Lechner, and D. Rus, “Safediffuser: Safe planning with diffusion probabilistic models,” in International Conference on Learning Representations, 2023

  22. [22]

    Simultaneous multi-robot motion planning with projected diffusion models,

    J. Liang, J. K. Christopher, S. Koenig, and F. Fioretto, “Simultaneous multi-robot motion planning with projected diffusion models,”arXiv preprint arXiv:2502.03607, 2025

  23. [23]

    Motion planning with sequential convex optimization and convex collision checking,

    J. Schulman, Y . Duan, J. Ho, A. Lee, I. Awwal, H. Bradlow, J. Pan, S. Patil, K. Goldberg, and P. Abbeel, “Motion planning with sequential convex optimization and convex collision checking,”The International Journal of Robotics Research, vol. 33, no. 9, pp. 1251–1270, 2014

  24. [24]

    Gusto: Guaranteed sequential trajectory optimization via sequential convex programming,

    R. Bonalli, A. Cauligi, A. Bylard, and M. Pavone, “Gusto: Guaranteed sequential trajectory optimization via sequential convex programming,” inInternational Conference on Robotics and Automation. IEEE, 2019, pp. 6741–6747

  25. [25]

    J. T. Betts,Practical methods for optimal control and estimation using nonlinear programming. SIAM, 2010

  26. [26]

    The convex feasible set algorithm for real time optimization in motion planning,

    C. Liu, C.-Y . Lin, and M. Tomizuka, “The convex feasible set algorithm for real time optimization in motion planning,”SIAM Journal on Control and optimization, vol. 56, no. 4, pp. 2712–2733, 2018

  27. [27]

    Distributed motion coordination using convex feasible set based model predictive control,

    H. Zhou and C. Liu, “Distributed motion coordination using convex feasible set based model predictive control,” inInternational Conference on Robotics and Automation. IEEE, 2021, pp. 8330–8336

  28. [28]

    Control barrier function based quadratic programs for safety critical systems,

    A. D. Ames, X. Xu, J. W. Grizzle, and P. Tabuada, “Control barrier function based quadratic programs for safety critical systems,”IEEE Transactions on Automatic Control, vol. 62, no. 8, pp. 3861–3876, 2016

  29. [29]

    S. J. Wright,Primal-dual interior-point methods. SIAM, 1997

  30. [30]

    An inexact augmented lagrangian framework for nonconvex optimization with nonlinear constraints,

    M. F. Sahin, A. Alacaoglu, F. Latorre, V . Cevheret al., “An inexact augmented lagrangian framework for nonconvex optimization with nonlinear constraints,”Advances in Neural Information Processing Systems, vol. 32, 2019

  31. [31]

    Altro: A fast solver for constrained trajectory optimization,

    T. A. Howell, B. E. Jackson, and Z. Manchester, “Altro: A fast solver for constrained trajectory optimization,” inInternational Conference on Intelligent Robots and Systems. IEEE, 2019, pp. 7674–7679

  32. [32]

    Constrained differential dynamic programming: A primal-dual augmented lagrangian approach,

    W. Jallet, A. Bambade, N. Mansard, and J. Carpentier, “Constrained differential dynamic programming: A primal-dual augmented lagrangian approach,” inInternational Conference on Intelligent Robots and Systems. IEEE, 2022, pp. 13 371–13 378

  33. [33]

    Admm penalty parameter selection by residual balancing,

    B. Wohlberg, “Admm penalty parameter selection by residual balancing,” arXiv preprint arXiv:1704.06209, 2017

  34. [34]

    Admm without a fixed penalty parameter: Faster convergence with new adaptive penalization,

    Y . Xu, M. Liu, Q. Lin, and T. Yang, “Admm without a fixed penalty parameter: Faster convergence with new adaptive penalization,”Advances in Neural Information Processing Systems, vol. 30, 2017

  35. [35]

    Relative entropy policy search,

    J. Peters, K. Mulling, and Y . Altun, “Relative entropy policy search,” inAAAI conference on artificial intelligence, vol. 24, no. 1, 2010, pp. 1607–1612

  36. [36]

    Reinforcement learning and control as probabilistic inference: Tutorial and review,

    S. Levine, “Reinforcement learning and control as probabilistic inference: Tutorial and review,”arXiv preprint arXiv:1805.00909, 2018

  37. [37]

    Towards diverse behaviors: A benchmark for imitation learning with human demonstrations,

    X. Jia, D. Blessing, X. Jiang, M. Reuss, A. Donat, R. Lioutikov, and G. Neumann, “Towards diverse behaviors: A benchmark for imitation learning with human demonstrations,”arXiv preprint arXiv:2402.14606, 2024. APPENDIXA THEORETICALANALYSIS A. Proof Objective This appendix formalizes the safety-enforcement mechanism used by MD-COAS in the main text. The go...