Sampling-Based Planning and Model Predictive Control › Constraints, Warm Starts, and Real-Time Budgets

We have so far treated the MPC problem as unconstrained except for the box on controls. Real systems often impose joint, velocity, geofence, or clearance constraints. Sampling-based planners have several options: reject infeasible samples, project or transform samples into a feasible parameterization, use a constrained optimizer, or add violation penalties to the objective. We use a finite penalty here. It converts a hard requirement into a soft preference; it neither diverges at the boundary nor guarantees feasibility.

Rejection preserves feasibility among retained samples, but its acceptance rate equals the proposal probability of the feasible set and can become very small in a narrow corridor. It still offers no guarantee that the best retained finite sample is globally optimal. A finite penalty always assigns a numerical cost, but the selected plan may remain infeasible. If violation is unacceptable, a soft-penalty planner needs a separate verified feasibility or safety mechanism rather than an assumption that a small violation is harmless.

Consider an obstacle at position $c$ with radius $r$ for a point-mass robot. A soft constraint for the obstacle is

where $w$ is a positive weight. Any evaluated point inside the disc contributes a quadratic penalty; points on or outside the boundary contribute zero. The squared hinge is continuously differentiable at the boundary, although its second derivative changes there. Because the terms are summed, several shallow violations can outweigh one deeper violation. The obstacle term itself does not reward extra clearance once outside, so the task and effort terms determine whether a plan grazes the boundary or takes a wider route.

Let us add an obstacle directly on the straight-line path from start to goal. The finite penalty makes routing around it compete with goal error and effort; feasibility is checked after optimization.

Notice the structure of make_constrained_cost. It wraps the base cost and adds one soft-penalty component; a truly hard constraint would require different handling. The obstacle penalty is also evaluated only at discrete rollout states. A segment can cross the disc between time steps even when both endpoints are outside, so tighter temporal discretization or segment-intersection checks are needed to rule that out. The numerical weight 5000 is large relative to this example's task term, but its physical units depend on how position and control costs are normalized. It is a tuning coefficient, not a dimensionless universal ratio of importance.

For this discretized example, the squared-hinge term makes sampled states inside the obstacle expensive. Increasing $w$ tends to prioritize reducing penetration, while decreasing it allows more tradeoff against goal and effort costs. No finite weight guarantees that an infeasible trajectory is never preferred, especially when no feasible sample is drawn. A practical sweep should therefore report both task performance and violation, followed by an explicit feasibility check on the selected trajectory.

The weight is not a parameter that can be set to "large" and forgotten, because it controls a tradeoff between clearance and goal progress. The cell below re-solves the same configured problem at nine penalty weights, changing $w$ while holding the start state, horizon, sample budget, and pseudorandom seed fixed. Ties are ordered by cost and then sample index in cem.

Read together, clearance and final goal error identify candidate weights that improve sampled-state feasibility without unacceptable task degradation in this run. They do not certify the continuous path. Weight selection should be repeated across seeds and starting conditions, followed by the safety checks appropriate to the deployment.

The figure shows the selected plan's sampled positions curving around the disc, while the plotted straight line intersects it. The detour is smooth under the acceleration dynamics and effort cost. Any visible margin is an outcome of this finite-sample run, not a robustness guarantee: because the obstacle penalty is zero outside the disc, it supplies no direct reward for additional clearance.

The second half of this section is warm starting. When the state, goal, and constraints change slowly between ticks, the shifted previous plan can be a useful initial proposal for the current problem:

We can pass this sequence as mu_init to CEM. When the prior remains relevant, it can improve finite-iteration plan quality or reduce the iterations needed to meet a target quality. It may also reduce between-tick variation by anchoring consecutive proposals. Abrupt goal, constraint, or dynamics changes can make the same prior harmful, so implementations should retain enough proposal variance or support a cold restart.

Let us demonstrate. We first solve a CEM problem at a previous state and apply its first action to obtain the next state. At that next state, we compare a one-iteration cold start against a one-iteration warm start initialized from the shifted prior plan.

With identical sample budgets and random-number seeds, the warm-started plan is lower-cost in this run after one iteration. The prior was computed at the previous state, shifted by the applied action, and evaluated at the resulting next state. The only configured difference between the two one-iteration calls is the initial proposal mean. This isolates the warm-start mechanism for the stated seed, but repeated disturbances and planner seeds are needed to estimate its typical benefit.

Warm starting can also reduce the number of refinement iterations needed for a target plan quality. A real-time controller must account for worst-case planner latency, because a missed deadline changes when an action is applied. Random shooting evaluates $N$ independent rollouts. CEM evaluates approximately $K_{\text{iter}}N$ rollouts plus elite selection and distribution updates. The product is therefore a first-order compute indicator, not a complete latency model. For a fixed product, the best split between iterations and samples depends on proposal dimension, elite fraction, hardware, and objective; more iterations do not win universally.

Let us measure the two planners and see the picture.

The numbers should be read with the stated budgets: random shooting performs 4,000 rollouts, while CEM performs 6,400 plus eight elite-selection and refit steps. These wall-clock measurements depend on the current machine, Python and NumPy versions, load, and array sizes. Neither the absolute times nor their ratio is generally transferable. A deployment should measure objective value and worst-case latency under equal operational budgets; warm-started CEM or MPPI are candidate designs, not automatic winners.

In the fixed-seed left panel, the warm proposal yields a lower plan cost at the same one-iteration budget. The right panel reports local wall-clock measurements for the two stated workloads; it is a hardware-specific timing check, not an algorithmic ranking.

A few practical notes on budgets. At 100 Hz, one control period is 10 ms, and sensing, state estimation, communication, and actuation consume part of it. Possible overrun policies include returning the best candidate found so far, reducing samples or horizon, or running planning at a lower rate than the low-level controller. For example, a planner at 25 Hz may supply four actions or a local policy to a 100 Hz loop; its planning horizon is a separate design choice and need not equal four. Reusing a previous action can itself be unsafe, so the deadline fallback must be designed and verified for the plant rather than treated as a universal rule.

The deadline is part of the controller model rather than a service-level preference. The plant evolves while planning, so an overrun changes the state and time at which the computed action is applied. The effect need not grow monotonically with every delay, but the implemented input schedule is no longer the one assumed by a synchronous discrete-time design. Timing validation must therefore include tail latency and the specified fallback behavior.