Optimization Methods for Prioritized Control, Sampling Design, and Motion Planning in Robotics
June 13, 2026
- unstructured environments
- hardware failures
- communication errors
- out-of-distribution scenarios
Real World Challenges
Motivation
Grid refinement Horizon adaptation Grid adaptation
Motivation
Direct transcription of an OCP fixes a time grid — almost always uniform.
- A uniform grid enforces constant resolution, regardless of the trajectory.
- Stiff dynamics and short constraint-active windows need fine resolution locally.
- Refining everywhere inflates the QP size and the cost of repeated MPC solves.
Results — Stiff System
Eigenvalues \(\{-10, -0.1, -0.01\}\) (stiffness \(\sim 10^3\)): non-uniform sampling reaches a lower cost with far fewer intervals and dominates the cost–time trade-off.
Results — Cart-Pole
Parameters: \(\require{siunitx} m_\text{pole}=0.1 kg\), \(m_\text{cart}=1 kg\), \(l_\text{pole}=0.5 m\)
- The state trajectories respect the constraints
- The non-uniform grid matches the dense uniform-grid cost with far fewer sampling instants.
Results — Efficiency
Matrix exponential + multiple shooting scales best — the exponential is \(\sim 5\)–\(30\times\) faster than numerical integration.
Introduction
Research Goal: optimize non-uniform sampling outside a convex inner QP.
Contributions:
- A two-level (bilevel) differentiable optimization framework.
- Exact zero-order-hold discretization (Van Loan) + temperature-scaled simplex reparametrization.
- Optional r-adaptive merge–split moves for discrete mesh restructuring.
Differentiable Optimization
The inner QP is solved exactly; differentiate its KKT conditions implicitly: \[ \frac{\partial \mathcal{L}}{\partial \varphi} = \frac{\partial \mathcal{L}}{\partial \Delta t}\, \frac{\partial \Delta t}{\partial \varphi}, \qquad \frac{\partial \mathcal{L}}{\partial \Delta t} = \big(\, \mathrm{KKT}^{-1} \circ \partial \mathcal{L} \,\big) \]
- Exact sensitivities — no finite differences.
- Update \(\varphi\) with a first-order optimizer (Adam).
- Inner convexity is preserved: \(A_{d,k}, B_{d,k}, W_k\) enter the QP as fixed parameters.
Simplex Reparametrization
Map an unconstrained \(\varphi\) to positive intervals summing to \(T\): \[ \Delta t_k(\varphi, \tau) = \epsilon + (T - N\epsilon)\, \frac{e^{\varphi_k / \tau}}{\sum_j e^{\varphi_j / \tau}} \]
- Positivity and \(\sum_k \Delta t_k = T\) hold by construction — no projection.
- Temperature \(\tau\) annealed high \(\to\) low: explore, then commit.
Results — Learned Non-Uniform Grids
The loss decreases monotonically, and the time budget concentrates where the dynamics are fast, unstable, or the input saturates — coarsening over the passive tail.
Results — Efficiency
Comparison With Joint Sampling Times Optimization
Decoupling time from control scales better than the joint NLP (right).
Quadratic Programming (QP)
Subset of Convex Programming.
Both efficient to solve and descriptive.
\[ \begin{aligned} & \min_x \; && x^T Q x + p^T x \\ & \text{s.t.} \; && A x = b \\ & && C x \leq d \end{aligned} \] with Q symmetric positive semidefinite.
Problem 🤕
- Robots are high-dimensional systems
- Several duties must be fulfilled simultaneously
- Trade-off among antagonistic requirements is necessary
Solution Approaches
Techniques to deal with redundancy:
- Standard approach
- Singularity-robust approach
- Reverse priority approach
- Hierarchical Quadratic Programming (HQP)
Prioritized vs. Weighted
Scenario: five partially-conflicting tasks on a single unicycle, decreasing importance.
- Low \(\kappa\) → fails everything.
- High \(\kappa\) → only the top task is satisfied.
- No weight combination replicates HQP behavior.
- HQP completes tasks sequentially when conflicting, simultaneously when not.
Motivation
- Soft terrains
- Passive articulated feet
- Compliant feet
Introduction
Research Goal: consider the contact compliance in the control design.
Contributions:
- The Soft Bilinear Inverted Pendulum (SBIP) model is proposed and used in the Motion Planner design
- A Whole Body Controller is developed using HQP and a soft contact model
- The improvements granted by considering the softness in the MP and the WBC are evaluated
- The proposed approach is tested in simulations and experiments
Soft Bilinear Inverted Pendulum Model
Equations of motion of the SBIP model: \[ \begin{aligned} \ddot{x}_{CoM} &= \frac{g + \ddot{z}_{CoM}}{h_{CoM}} (x_{CoM} - x_{ZMP}) \\ \ddot{y}_{CoM} &= \frac{g + \ddot{z}_{CoM}}{h_{CoM}} (y_{CoM} - y_{ZMP}) \\ \ddot{z}_{CoM} &= - \ddot{\delta} = - g + 1/m (k_p \delta + k_d \dot{\delta}) \end{aligned} \]
Simplified model used to plan the base and feet trajectories over a long horizon.
The planner is a QP-based MPC that tracks a base velocity reference.
Motion Planner
QP MPC plans \(N\) footstep positions (ZMP) and CoM states in a trotting gait; solved independently for \(x\) and \(y\).
The closed-form horizontal SBIP solution links consecutive footsteps: \[ x_{CoM,i} = A(T_s) \, x_{CoM,i-1} + B(T_s) \, x_{ZMP,i}, \qquad \omega = \sqrt{g / h_{CoM}} \]
optimizing over the horizon variables \[ x_{opt} = \begin{bmatrix} x_{ZMP,1} & x_{CoM,1}^T & \cdots & x_{ZMP,N} & x_{CoM,N}^T \end{bmatrix}^T \]
\[ \begin{aligned} \min_{x_{opt}} \ \tfrac{1}{2} \sum_{i=1}^{N} \big(\ &\lVert x_{CoM,i} - A(T_s) x_{CoM,i-1} - B(T_s) x_{ZMP,i} \rVert^2_{Q_{dyn}} \\ + \ &\lVert \dot{x}_{CoM,i} - \dot{x}^{des} \rVert^2_{Q_{des}} + \lVert x_{ZMP,i} - x_{ZMP,i-1} \rVert^2_{Q_{min}} \ \big) \\ \text{s.t.} \ \ &x_{ZMP,i} - x_{ZMP,i-1} \le d \qquad \qquad \textbf{Max step length} \end{aligned} \]
- Dynamics consistency (\(Q_{dyn}\))
- velocity-reference tracking (\(Q_{des}\)) ·
- step regularization (\(Q_{min}\)); constraint = max step length \(d\).
Tracking Controller
Full robot model used to plan over a short horizon with higher fidelity.
The equations of motion of a legged robot are \[ M(q) \ddot{q} + h(q, \dot{q}) = S^T \tau + J^T (q) f \] which can be split into the underactuated and actuated part as \[ \begin{aligned} M_a (q) \ddot{v} + h_a (q, v) &= \tau + J_a^T (q) f \\ M_u (q) \ddot{q} + h_u (q, v) &= J_u^T (q) f \end{aligned} \]
The optimization vector is \[ x_{opt} = \begin{bmatrix} \dot{v}^T & f^T & \delta_d^T \end{bmatrix}^T \]
From the optimization vector and the robot model, the joint torques are \[ \tau^* = M_a (q) \dot{v}^* + h_a (q, v) - J_a^T (q) f^* \]
| Priority | Tasks |
|---|---|
| 1 | Physical consistency |
| 2 | Actuation torque limits |
| Contact friction cone limits | |
| Force modulation | |
| 3 | Contact constraints |
| 4 | Base linear trajectory tracking |
| Base angular trajectory tracking | |
| Swing feet trajectory tracking | |
| 5 | Energy minimization |
| Contact forces minimization |
Comparison in Simulation
Four controller configurations compared using four KPIs and two different scenarios.
Motivation
Actuators overheating during prolonged use.
Introduction
Research Goal: guarantee thermal safety and optimal performance using control.
Contributions:
- Consider the thermal dynamics in the control design.
- HQP-based approach for constraints prioritization.
- Ablation study to evaluate the impact on performance.
Control Architecture
First order thermal model \[ \dot{T} = - \alpha (T - T_\text{amb}) + \beta | \tau | \]
Are you sure??
Control Architecture
Optimization vector: \[ x = \begin{bmatrix} q^T & \dot{q}^T & T^T & \tau_s^T \end{bmatrix}^T \]
Tasks:
- Dynamic consistency, Thermal model, Epigraph reformulation
- Thermal safety, Safety limits
- Primary task (e.g., end-effector pose)
- Secondary tasks (e.g., control energy minimization)
Results – Reach and Hold
Results – Circle
Results – Lemniscate
Introduction
Research Goal: coordinate heterogeneous robot fleets executing multiple, possibly conflicting tasks under a strict priority order.
Contributions:
- Hierarchical Quadratic Programming embedded in an MPC loop for multi-robot control.
- Tasks encoded as linear equality/inequality constraints — modular and parameter-free.
- Robust to robot heterogeneity (unicycle, omnidirectional, …).
- Validated on a library of multi-robot tasks (coverage, formation, collision avoidance, …).
Distributed HQP — Motivation
The previous approach is centralized:
- One process collects all robot states.
- Solves a single large QP per priority level.
- Communication failure → no control.
- Computational cost scales with fleet size.
Goal: keep the priority guarantees while distributing the computation over the fleet.
dHQP — Formation with Obstacle
Nine omnidirectional robots: collision avoidance > velocity reference > formation control.
dHQP — Narrow Gap & Coverage
Ten agents through a narrow gap.
Heterogeneous fleet (unicycle + omni) covering a square.
dHQP — Scalability
dHQP scales better than centralized HQP as the fleet grows.
Optimality Gap
People 🧑🤝🧑👭👫👬
