Lesson 3.2.3 — Planning with World Models

Week 22

Prerequisite lessons: 3.2.1 (latent world models, RSSM, Dreamer), 3.2.2 (video prediction models as world models), 2.3.2 (motion planning for manipulation), 2.2.2 (model-based RL and MBPO), 1.1.4 (the full-stack pick-and-place pipeline), 2.3.3 (contact-rich manipulation).

Prior results used in this lesson. From 3.2.1: the RSSM’s prior $p_\theta(z_t|h_t)$, the imagination loop of Eq. (C.1), and the coverage-gap ratio of §L.D.3. From 3.2.2: the usable-horizon derivation from a power-law error curve and the pixel-tolerance arithmetic of §L.E.1. From 2.3.2: the collision-free path with a clearance margin. From 2.2.2: the short-rollout result. From 1.1.4: the seven-stage pipeline’s state machine and health checks. Each is restated at the point of use.

What this lesson hands forward. The receding-horizon planner, the shooting-method family (random, CEM, MPPI), the gradient-through-model planner, and the interaction between the planner and the 1.1.4 state machine. Module 4.2 uses the planner as the policy layer’s decision mechanism; Module 4.3 uses the failure taxonomy as an experiment template.

The running case in one sentence. The running arm, gripper, eye-in-hand camera, table, cube, bin, and divider are unchanged, and the RSSM of 3.2.1 is the forward model; this lesson asks how to query that model for the best action sequence, how to know the answer is trustworthy, and what to do when it is not.

§L.0 — Lesson map, objectives, and misconceptions

Observable outcomes. By the end of this lesson you can, without notes: (1) write the planning objective and state why it is a constrained stochastic optimization; (2) derive the random-shooting estimator’s variance and explain why it is inefficient; (3) write the CEM update and state its convergence properties; (4) write the MPPI update and state how it differs from CEM; (5) compute the temperature’s effect on the action distribution; (6) derive the gradient-through-model estimator and state the two requirements for it to be usable; (7) state the receding-horizon rule and the horizon/execution-length trade; (8) apply the 3.2.2 usable-horizon result to choose a planning horizon; (9) list the five planner failure modes and their signatures; (10) design the planner-with-model-flag architecture that protects the running case when the model is out of distribution.

Misconception inventory.

Everything is self-contained. Where a prerequisite lesson is used, the result is restated at the point of use.

§L.0.5 — Retrieval warm-up

Q1. In 3.2.1, the imagination loop rolls the RSSM forward using the actor’s actions. What changes when the planner chooses the actions instead?

Answer. The rollouts are conditioned on candidate action sequences rather than on the policy’s output, so the same model is queried as a forward model rather than as the actor’s training ground. Used in §L.A.1 to frame planning as optimization over action sequences.

Q2. In 3.2.2, the usable horizon is the horizon at which the rollout error crosses the planner’s tolerance, derived from the power-law fit. For the running case, the RSSM’s usable horizon is about 20 steps at the 16-pixel tolerance. What does that say about a 20-step planning horizon?

Answer. It is exactly at the edge of trust; a 20-step plan’s terminal state is at the tolerance, so the planner should use a horizon below 20 and bootstrap with the value, or use a shorter horizon with receding execution. Used in §L.D.1 to set the horizon.

Q3. In 2.3.2, the motion planner returns a collision-free path with a clearance margin. What does a world-model planner add to that result?

Answer. It evaluates candidate paths under the model’s dynamics and reward, so it can choose among them; the 2.3.2 planner supplies constraints and a feasible set, the world-model planner supplies the cost estimate. Used in §L.A.1 to define the planner’s role in the pipeline.

Q4. In 2.2.2, MBPO’s short rollouts start from real states. What is the world-model planner’s analogue of “starting from real states”?

Answer. Starting from the current posterior latent, which is conditioned on the current observation, so the first rollout step is on the model’s manifold. Used in §L.A.3.

Part A — Planning as optimization

§L.A.1 — The objective

The planner’s problem is to choose an action sequence $\mathbf{a}_{t:t+H} = (a_t, \ldots, a_{t+H-1})$ that maximizes the model’s predicted return from the current state, subject to the constraints the deployment imposes:

where $b_t$ is the current belief over the latent (represented by the RSSM’s posterior), $\hat{r}$ the reward model, $\mathcal{A}_H$ the set of admissible action sequences (bounded per-step magnitudes, rate limits, and the 2.3.2 clearance), and $\mathcal{F}$ the feasibility set from 2.3.2 and 2.3.3.

Three properties of Eq. (A.1) shape every algorithm in this lesson.

The expectation is over a belief, not a state. The posterior $b_t$ is a distribution because the RSSM’s $z_t$ is stochastic and the observation may be ambiguous. The planner must evaluate the return under this distribution, not at its mean, or the plan is overconfident.

The constraint set is not a box. The feasibility constraints include the 2.3.2 collision check and the 2.3.3 force limits, which are not differentiable and not convex. A planner that ignores them produces sequences the 1.1.4 state machine will reject.

The horizon is bounded by the model’s trust. From 3.2.2, the model’s error grows with $H$ and crosses the tolerance at the usable horizon. Beyond that, the maximization in Eq. (A.1) is maximizing model error, not return.

Self-explanation 1. Why must the planner evaluate the return under the belief rather than at the posterior’s mean?

Answer. Because the reward is generally not linear in the latent, so the mean reward at the mean latent is not the mean reward; a plan chosen at the mean can be systematically wrong when the belief is wide.

§L.A.2 — Why not the global optimum

The global optimum of Eq. (A.1) is generally unaffordable, and a simple dimension count explains why. A 7-DoF action at $H = 10$ is a 70-dimensional continuous vector, and even with a coarse discretization of 10 bins per dimension the space is $10^{70}$. The planner has milliseconds to find a good action.

Two consequences follow. The planner is anytime: it returns a plan whenever the time budget expires, and the plan improves with more samples. And the acceptance criterion is not optimality but regret: the plan’s predicted return relative to the best plan it could have found with infinite samples, which the algorithms of Part B estimate.

The second consequence is that the planner’s quality is dominated by its sampling efficiency: how good a plan it finds per unit of model queries. The model queries are the expensive resource, because each query is a forward rollout of the RSSM.

§L.A.3 — The starting belief

The planner starts from the current posterior $q(z_t|h_t,o_t)$ (from 3.2.1 §L.B.2), not from a prior sample. The posterior is conditioned on the current observation, so the plan starts where the robot actually is; a prior sample would start the plan where the model thinks the robot might be, which is wrong when the observation is informative.

The planner samples $K$ latents from the posterior, rolls the same action sequence under each, and averages the returns. The sampling is what makes the plan robust to the belief’s width: a plan that succeeds under every sampled latent is preferable to one that succeeds only at the mean.

The average has a bias when the reward is non-linear in the latent, and the bias is worth stating: it is the same non-linearity that justifies the sampling, so the average is the correct expectation, not the mean’s reward.

Concept check. A planner evaluates candidate sequences at the posterior’s mean latent. The cube is 3 cm from the gripper but the posterior has two modes: one at the current cube pose and one 6 cm away (the cube could be on the near or far side of the divider). What does the planner choose, and what does the sampling-based planner choose?

Answer. The mean-latent planner sees the cube at about 4.5 cm, between the two modes, and plans a grasp that reaches neither. The sampling-based planner evaluates each mode and chooses a plan robust to both, typically a cautious approach that resolves the ambiguity before committing. [displaces M1]

Worked-example ladder.

WE-L.A.a (fully worked). The belief over the cube’s $x$-position is a two-component Gaussian, modes at 0.4 and 0.6 with equal weight and $\sigma = 0.02$. The reward is $r = -|x_{grip} - x_{cube}|$, and the gripper is at 0.5. Compute the reward at the mean latent and the expected reward under the belief.

Solution. The mean is 0.5, so $r(0.5) = -|0.5 - 0.5| = 0$. Under the belief, each mode gives $r = -|0.5 - 0.4| = -0.1$ and similarly $-0.1$, so the expectation is $-0.1$. The mean latent overestimates the reward by 0.1, and a planner optimizing at the mean picks an action that looks perfect and is not.

WE-L.A.b (completion). The gripper can move to 0.4 or 0.5. Compute the expected reward at each and choose. Completion. At 0.4: modes give $r(0.4) = 0$ and $r(0.4) = -0.2$, expectation $-0.1$. At 0.5: expectation $-0.1$. At 0.6: expectation $-0.1$. The problem is symmetric, so any single position is equally bad; the optimal plan is to resolve the ambiguity first, which requires a different action space — move the camera or the gripper to disambiguate — than a single fixed position.

WE-L.A.c (faded). Derive the feasibility constraint for the running case’s planner from the 2.3.2 clearance and the 2.3.3 force limit, and state why it is not a box. Faded step. The clearance is a distance constraint in Cartesian space; the force limit is a constraint on the contact wrench. Both are non-convex in the action because the distance and the wrench are nonlinear functions of the pose. Completion. The feasible set is the intersection of a non-convex reachable set, a clearance-constrained set, and a force-constrained set; the planner must sample in the interior or project onto it.

WE-L.A.d (contrastive near-miss). A learner evaluates plans at the mean latent and finds that the resulting grasp misses the cube by exactly the divider’s width, in a direction that depends on the day. Why it fails: the mean latent placed the cube between the two posterior modes, and the miss is the average of the two real misses. The fix: sample latents from the posterior and average the returns, which produces a cautious plan.

Comparative table: planning objectives.

Design principle P41.1. Plan under the belief, not at its mean; the sample cost of $K$ latents is small compared to the cost of a grasp that misses.

Consolidation checkpoint 1

(a) Three capabilities. You can write the planning objective and identify its three shaping properties. You can state why the mean latent overestimates and how sampling fixes it. You can write the feasibility constraint from the 2.3.2 and 2.3.3 results.

(b) Transfer probe. The posterior is unimodal and narrow. Does sampling still matter? Answer: marginally, but the cost is small and the insurance is valuable when the belief widens unexpectedly (e.g., during occlusion); the sampling is a general-purpose mechanism, not a special case.

(c) Remediation rule. If you cannot derive the reward error of WE-L.A.a, redo it before Part B.

Part B — Shooting methods

§L.B.1 — Random shooting

The simplest planner samples $N$ action sequences uniformly from the admissible set, rolls the model for each, and returns the best. Its estimate of the optimal return is the maximum of $N$ samples, which converges to the true maximum as $N \to \infty$ but very slowly in high dimensions.

The inefficiency is quantifiable. The maximum of $N$ samples from a distribution whose tail has exponent $\alpha$ converges at rate $N^{-1/\alpha}$. For a 70-dimensional action space the tail’s effective exponent is large, so the convergence is far slower than $1/N$. Random shooting is the baseline against which the others are measured, not a serious planner.

§L.B.2 — Cross-entropy method

The cross-entropy method (CEM; Rubinstein 1997, tier A, S) iterates three steps.

1. Sample $N$ action sequences from a distribution $\mathcal{N}(\mu, \Sigma)$ truncated to the admissible set.
2. Score each by its predicted return.
3. Refit $\mu, \Sigma$ to the mean and covariance of the top $K$ sequences (the elite set).

Iterating is what makes CEM efficient: the first iteration is random shooting, and each subsequent iteration shifts the sampling distribution toward the good region. After a few iterations, the distribution concentrates on a local optimum, which is good enough for planning in a few milliseconds.

The elite fraction $\rho = K/N$ controls the trade between exploration and exploitation. A small $\rho$ concentrates quickly but can lock onto a poor local optimum; a large $\rho$ explores more but converges slowly. Typical values are 0.1–0.2.

CEM’s variance update is the source of both its strength and its weakness. Refitting $\Sigma$ to the elite set shrinks the covariance toward the elite’s spread, which is what allows convergence. When the elite set is degenerate (e.g., five sequences that are nearly identical), $\Sigma$ collapses and the planner stops exploring. The mitigation is a variance floor: $\Sigma \leftarrow \max(\Sigma, \sigma_{min}^2 I)$.

§L.B.3 — MPPI

Model Predictive Path Integral control (MPPI; Williams et al. 2017, tier A, S) is the smooth alternative to CEM. Rather than truncating to the elite, MPPI weights every sample by an exponential utility:

where $G_i$ is the return of sample $i$ and $\lambda$ is a temperature. The update is a weighted average of the samples, so every sample contributes, and the weights concentrate as $\lambda$ falls.

MPPI’s two advantages over CEM. The update is smooth in the returns, so a slightly better sample receives a slightly higher weight, which avoids the discontinuity of an elite cutoff. The covariance is the samples’ covariance, which is not refit and therefore does not collapse; MPPI maintains exploration even as the distribution concentrates.

The disadvantage is that the temperature $\lambda$ must be tuned to the return scale. A $\lambda$ that is small relative to the returns produces near-one-hot weights and behaves like random shooting; a $\lambda$ that is large produces a near-uniform average and converges slowly. DreamerV3’s CEM is not MPPI, and the running case uses MPPI for its smoothness.

The free-energy interpretation is worth stating because it explains the temperature: MPPI’s update is the minimizer of the KL between the weighting and the return’s exponential tilt, so $\lambda$ is the temperature at which the planner is willing to concentrate. It is the same $\lambda$ that appears in the soft-Bellman equation of 2.2.5, applied to planning.

Self-explanation 2. Why does MPPI maintain exploration where CEM collapses?

Answer. Because MPPI does not refit a covariance from a truncated elite set; the weights’ spread is the samples’ spread, and a uniform-ish weight gives a wide effective distribution.

Concept check. MPPI’s temperature $\lambda$ is set very large. What does the update become, and what is the failure?

Answer. Large $\lambda$ makes the weights near-uniform, so the update is the mean of all samples, which is the mean of the truncated Gaussian — essentially no learning from the returns. The planner behaves like a prior sample and ignores the model. [displaces M3]

Worked-example ladder.

WE-L.B.a (fully worked). Four samples have returns $G = [1, 2, 3, 4]$ and the action sequences are scalars $\mathbf{a}_i = [0, 1, 2, 3]$. Compute MPPI’s update at $\lambda = 1$.

Solution. $w_i \propto \exp(G_i/1)$: $[e, e^2, e^3, e^4]/ \sum = [2.72, 7.39, 20.1, 54.6]/84.8 = [0.032, 0.087, 0.237, 0.644]$. The weighted action is $0.032 \cdot 0 + 0.087 \cdot 1 + 0.237 \cdot 2 + 0.644 \cdot 3 = 0 + 0.087 + 0.474 + 1.932 = 2.49$. The update is near the best sample but not at it.

WE-L.B.b (completion). Compute the update at $\lambda = 0.1$. Completion. $w_i \propto \exp(10 G_i)$ gives weights $[4.5\times10^{-14}, 2.1\times10^{-9}, 9.7\times10^{-5}, 0.9999]$, so the update is approximately 3.0 — essentially random shooting’s argmax. The completion shows the temperature’s role: small $\lambda$ is exploitation, large is exploration.

WE-L.B.c (faded). Derive CEM’s covariance update for the same four samples with an elite fraction of 0.5. Faded step. The elite is the top two, actions 2 and 3, so $\mu = 2.5$ and $\Sigma = 0.25$. Completion. The covariance shrinks by the elite’s spread, which is the mechanism that concentrates the search; the variance floor is what stops it from collapsing to zero when the elite is a single sequence.

WE-L.B.d (contrastive near-miss). A learner runs CEM with an elite fraction of 0.01 on a 20-sample budget, so the elite is zero or one samples, and the covariance collapses on the first iteration. The plan is stuck at the first sample’s neighbourhood. Why it fails: the elite fraction times the sample count must be at least several samples for the covariance to be meaningful; $\rho N < 2$ is degenerate. The fix: raise $N$ or raise $\rho$; the practical rule is $N \geq 50$ and $\rho \in [0.1, 0.2]$.

Comparative table: shooting methods.

Design principle P41.2. CEM for fast concentration on a good local optimum, MPPI for smooth updates that do not collapse; never set $\rho N$ below a handful.

Consolidation checkpoint 2

(a) Three capabilities. You can write the CEM update and the variance floor. You can write the MPPI update and the temperature’s effect. You can compute a weighted action from a return vector.

(b) Transfer probe. The returns are all nearly equal, and MPPI’s $\lambda$ is small. What does the update do? Answer: the weights are near-one-hot on the small differences, which are noise, so the update chases noise; the fix is to raise $\lambda$ until the differences are comparable to it.

(c) Remediation rule. If you cannot compute the MPPI update of WE-L.B.a, redo it before Part C.

Part C — Gradient-based planning

§L.C.1 — Backprop through the model

The model is differentiable, so the return is differentiable with respect to the action sequence, and the planner can use gradient ascent:

The gradient is computed by backpropagation through the RSSM’s recurrence, the prior, and the reward model — the same computational graph as 3.2.1’s actor training, with the action as the parameter instead of the policy’s weights.

The advantage is sample efficiency: one backward pass gives a gradient direction, whereas shooting methods need hundreds of forward passes. The disadvantage is that the gradient is over the action sequence, not over a policy, so each planning step requires a fresh backward pass, and the cost scales with the horizon and the action dimension.

§L.C.2 — The two requirements

Gradient-based planning works only if two conditions hold.

Differentiability through the stochastic latent. The RSSM’s latent is a categorical sample, which is not differentiable. The straight-through estimator of 3.2.1 §L.C.2 supplies a biased gradient; the bias is acceptable when the downstream computation is smooth, and it is not when the latent’s contribution is sharp (e.g., a contact event represented by one categorical variable). The operational test is to compare the gradient-based plan with a CEM plan on the same state; if they diverge systematically, the gradient is biased in a way that matters.

A smooth model. The gradient follows the model’s sharpest directions, and if those directions are artefacts — a decoder that reconstructs the table’s texture with high-frequency detail — the planner will exploit the artefacts. The mitigation is a model trained with a task-relevant auxiliary loss, as in 3.2.2 §L.C.2, or a regularization on the gradient’s norm.

A third, practical condition: the gradient’s scale must be compatible with the optimizer’s step size. The RSSM’s reward model has a scale set by the reward, and the gradient’s norm varies with the state; a step size tuned for one state is too large or too small for another. The mitigation is normalized gradient descent or a trust region.

§L.C.3 — Model exploitation

Both families exploit model error, and the gradient family exploits it faster. The planner finds the action sequence that maximizes the model’s predicted return, which is a combination of the true return and the model’s error; if the model’s error is larger in some directions, the planner will prefer those directions.

The mitigation is not a more accurate model — an accurate model is also exploitable, just in a smaller neighbourhood. It is three things. A short horizon, so the exploited error has less time to accumulate. An uncertainty penalty, so the return is discounted where the model is uncertain. A trust region around the data, so the planner’s actions stay where the model is trained.

The uncertainty penalty requires a model that reports uncertainty. The RSSM’s stochastic latent is not calibrated for this purpose by default; the ensemble is the standard mechanism (Chua et al. 2018, tier A, S; Janner et al. 2019, tier A, S), where $M$ models are trained and their disagreement is the uncertainty. The running case’s single RSSM does not supply this, which is why §L.E.3 adds the model flag of 3.2.1 §L.D.3 rather than relying on the planner to avoid out-of-distribution states on its own.

Self-explanation 3. Why is the uncertainty penalty a return modification rather than a gradient modification?

Answer. Because the penalty changes which actions are preferred, not how the gradient is computed; it is a term in the objective, and the gradient follows from the modified objective.

Concept check. A planner with a gradient ascent finds a plan whose predicted return is 50 and whose CEM counterpart is 20. The real return is 5. Diagnose and name two mitigations.

Answer. The plan exploits model error: the gradient found a direction where the model is overconfident. The mitigations are a shorter horizon (so the exploitation has less time) and a trust region or uncertainty penalty that penalizes the off-distribution action sequence. [displaces M2]

Worked-example ladder.

WE-L.C.a (fully worked). A one-step return is $\hat{r}(a) = -a^2 + \epsilon(a)$, where the model’s error is $\epsilon(a) = 0.1 \sin(10 a)$. At the true optimum $a = 0$, the model’s return is 0. At $a \approx 0.15$, the error contributes about $0.1 \sin(1.5) \approx 0.1$, so the model’s return is $-0.0225 + 0.1 \approx 0.077$, greater than at the true optimum. Compute the gradient at $a=0.15$ and describe the planner’s behaviour.

Solution. $\hat{r}'(a) = -2a + 0.1 \cdot 10 \cos(10a) = -0.3 + \cos(1.5) \approx -0.3 + 0.07 = -0.23$. The gradient is negative, so the planner moves to smaller $a$, toward 0. The small error has not created a local maximum that dominates the true optimum at this scale, but with a larger $\epsilon$ amplitude it can.

WE-L.C.b (completion). Increase the error amplitude to 0.5. Recompute the model’s return at $a \approx 0.15$ and at $a=0$ and state whether the planner prefers the artefact. Completion. At $a \approx 0.15$: $-0.0225 + 0.5 \sin(1.5) \approx -0.0225 + 0.499 = 0.476$. At $a = 0$: 0. The planner prefers the artefact, and the true return there is about $-0.0225$. The completion shows the exploitation threshold: the error amplitude relative to the true curvature.

WE-L.C.c (faded). Write the uncertainty penalty for an ensemble of $M$ models and show it is a variance term. Faded step. $\hat{r}_{pen}(a) = \hat{r}_{mean}(a) - \beta \, \mathrm{Var}_m[\hat{r}_m(a)]$. Completion. With $M$ models, the variance is the ensemble disagreement; the penalty shrinks the return where the disagreement is large, which is where the model is unreliable.

WE-L.C.d (contrastive near-miss). A learner uses gradient-based planning with a 50-step horizon and finds that the plan drives the arm through the divider in the model. The real 2.3.2 planner would have rejected the plan, but the model’s collision penalty was not trained on divider collisions. Why it fails: the model’s collision representation is a learned feature, and the training data did not include divider contacts; the gradient found the model’s blind spot. The fix: the 2.3.2 collision check is applied to the plan after it is generated, rejecting plans that cross the divider, and the model is retrained with divider-contact data.

Comparative table: planners and their exploitation risk.

Design principle P41.3. Plan with a gradient only inside a trust region; outside it, the gradient is a model-error detector.

Consolidation checkpoint 3

(a) Three capabilities. You can write the gradient-based update and the two requirements. You can state the exploitation mechanism and its three mitigations. You can write the uncertainty penalty for an ensemble.

(b) Transfer probe. A model has low error in-distribution and a sharp texture artefact. Which planner exploits it faster? Answer: the gradient planner, because the gradient follows the artefact’s direction in one pass, while CEM would need samples near the artefact.

(c) Remediation rule. If you cannot state why an accurate model is still exploitable, re-read §L.C.3 before Part D.

Part D — Receding horizon and safety

§L.D.1 — The receding-horizon rule

The planner computes a plan of length $H$ and executes only the first $E$ actions, then re-plans from the new observation. The receding-horizon rule with $E \le H$ is what makes the planner robust to model error: the plan beyond $E$ is a look-ahead that informs the first action but is not executed.

The horizon $H$ is bounded by the usable horizon from 3.2.2: beyond it, the model’s error dominates. The execution length $E$ is bounded by the latency of the planning loop: a plan must be recomputed before the executed actions are exhausted. For the running case at 10 Hz control with 20 ms planning latency, $E$ can be as high as the number of steps the plan’s first segment remains valid, typically 1–5.

The trade: a longer $E$ amortizes planning cost and reduces jitter from re-planning, but reduces reactivity to observations. A shorter $E$ is more reactive and more expensive.

§L.D.2 — The safety layer

The plan is checked before execution against the constraints of Eq. (A.1).

Force limit. The 2.3.3 impedance controller’s peak force limit is applied to the plan’s predicted contact forces; any plan that would exceed it is rejected and the planner re-samples.

Collision. The 2.3.2 clearance check is applied to the plan’s Cartesian trajectory; a plan that violates the clearance is rejected.

Rate limit. The per-step action magnitude is checked against the actuator’s limits; a plan that exceeds them is clipped or rejected.

Model flag. The plan’s terminal state is checked against the model’s coverage: if the terminal latent’s nearest training latent is beyond a threshold, the plan is flagged as out-of-distribution and the state machine switches to a conservative policy.

The four checks are applied in order, and the first rejection stops the plan. The order matters: force first (physical safety), collision second (workspace safety), rate third (hardware safety), model flag last (information safety).

§L.D.3 — The state machine

The 1.1.4 state machine gains a PLAN state between perception and control.

Entry condition. The 2.1.2 pose estimate is fresh and the 3.2.1 posterior is conditioned on the current observation.

Exit condition. A plan is generated that passes all four safety checks, or the planner’s time budget expires and the conservative policy takes over.

Health checks. The plan’s predicted return, its terminal coverage flag, and the planner’s sample count and elite fraction are logged at the state boundary. A plan with an unusually low predicted return, or a flag, triggers review.

Recovery. If the planner fails for three consecutive cycles, the state machine transitions to a conservative APPROACH policy — the 2.3.3 impedance controller with a fixed nominal plan — which is safe but slow.

The state machine’s health checks are what make the planner auditable: without them, a planner that produces a bad plan that happens to pass the safety checks is invisible until the grasp fails.

Self-explanation 1. Why is the execution length $E$ not equal to the horizon $H$?

Answer. Because the plan beyond $E$ is based on the current observation and will be stale after $E$ steps; executing it without re-planning discards the opportunity to correct.

Concept check. The planner’s usable horizon is 20, the execution length is 5, and the control rate is 10 Hz. How often does the planner re-plan, and what is the most the state can have changed when it does?

Answer. It re-plans every 0.5 s. In 0.5 s at a nominal 10 cm/s the arm moves 5 cm, so the state can have changed by a grasp-relevant amount; the 2.1.2 pose and the 3.2.1 posterior must be refreshed at the re-plan, not cached. [displaces M5]

Worked-example ladder.

WE-L.D.a (fully worked). The planner runs at 50 Hz with 20 ms latency and the control runs at 10 Hz. What is the maximum $E$ that the latency allows?

Solution. At 10 Hz, each control step is 100 ms. At 20 ms planning latency, the plan can be recomputed within one control step, so $E$ can be as high as 1 without stalling; $E = 5$ would mean the planner must complete in 500 ms, which it does comfortably. The binding constraint is the usable horizon (20) and the reactivity requirement, not the latency.

WE-L.D.b (completion). The planner’s latency rises to 200 ms at a 30-sample CEM with 5 iterations. Recompute the maximum $E$ and state whether the deployment is viable. Completion. 200 ms is two control steps at 10 Hz, so $E \ge 2$ is required for the planner to keep up; $E = 2$ is viable but leaves no margin for jitter, so the sample count should be reduced or the control rate raised.

WE-L.D.c (faded). Derive the model-flag threshold from the coverage-gap ratio of 3.2.1 §L.D.3. Faded step. The threshold is the distance at which the held-out error exceeds the budget; compute it on the training latents and the flagged states. Completion. The threshold is a radius in latent space, and the flag is a binary query of whether the plan’s terminal latent falls inside it.

WE-L.D.d (contrastive near-miss). A planner passes all four safety checks and produces a plan whose terminal latent is just inside the coverage radius, but the path to it crosses a region the model has never seen. Why it fails: the flag checks the terminal state, not the path; the model’s uncertainty along the path is invisible to the check. The fix: check the flag at every step of the plan, not only at the terminal, or use an ensemble’s disagreement as a per-step signal.

Comparative table: planner parameters and their bounds.

Design principle P41.4. Re-plan before the plan is stale; check the path, not just the endpoint; the safety layer’s four checks are necessary and, in the running case, not sufficient without the path flag.

Consolidation checkpoint 4

(a) Three capabilities. You can compute the maximum execution length from latency and control rate. You can order the four safety checks and state each one’s role. You can write the PLAN state’s entry, exit, and health checks.

(b) Transfer probe. The planner’s latency doubles and the control rate is fixed. What changes and what is the fix? Answer: the maximum $E$ doubles or the margin shrinks; the fix is to reduce $N$ or $H$, or to raise the control rate.

(c) Remediation rule. If you cannot state why the path flag matters when the terminal flag passes, re-read §L.D.3 before Part E.

Part E — The running case

§L.E.1 — The plan and its inputs

The running case’s planner is a CEM planner over 7-DoF end-effector deltas with horizon $H = 10$, sample count $N = 200$, elite fraction $\rho = 0.1$, five iterations, at 20 ms per iteration, giving a total planning time of about 100 ms. The planner runs at 10 Hz with $E = 3$, re-planning every 300 ms.

The forward model is the 3.2.1 RSSM, extended with 3,000 grasping transitions per the 3.2.1 §L.E.2 recommendation. Its usable horizon from 3.2.2 is about 20 steps at the 16-pixel tolerance; the planner’s $H = 10$ is within the usable horizon, with a factor-2 margin.

The starting belief is the current posterior $q(z_t|h_t,o_t)$, sampled 8 times per planning query. The reward is the negative distance to the target grasp pose, plus a success term.

§L.E.2 — The coverage gap in planning

The 3.2.1 §L.D.3 result is that the reaching-trained model’s coverage gap on grasping is a ratio of about 4.7, which is above the 3 threshold; the 3,000-transition extension brings it to about 2.0. The planner operates on the extended model, but the extension is small and the model’s confidence on the grasping states is not calibrated.

The planner’s behaviour under this gap has a specific signature. When the plan’s terminal latent falls near the edge of the extended data’s coverage, the model’s predicted return is overconfident — it extrapolates the reward — and the planner prefers actions that terminate at the edge. The signature is a predicted return that is high while the terminal coverage flag is near its boundary, which the health check catches.

The mitigation is the model flag of §L.D.3 applied per step: if any step of the plan enters the uncovered region, the plan is rejected and the planner samples again with a penalty on the uncovered region. In practice the planner converges to plans that stay inside the coverage, which is conservative but safe; the conservatism is the cost of the small data.

§L.E.3 — The planner-state-machine composition

The composition in the running case:

• State PERCEIVE: 2.1.2 pose and 3.2.1 posterior.
• State PLAN: CEM over $H=10$, $N=200$, with the four checks and the per-step model flag.
• State EXECUTE: the first 3 actions to the 2.3.3 impedance controller.
• State MONITOR: the health checks and the 1.1.4 state machine’s error recovery.

The composition’s failure modes are the planner’s (§L.C.2) and the model’s (3.2.1 §L.D), and they are caught at different points: the planner’s by the safety checks, the model’s by the per-step flag.

Self-explanation 2. Why is the CEM planner the right choice for the running case rather than the gradient planner?

Answer. Because the RSSM’s latent is categorical and its straight-through gradient is biased at contact events, and the running case’s task is contact-rich; CEM is robust to the gradient’s bias and the sample cost is affordable at 200 samples and 5 iterations.

Worked-example ladder.

WE-L.E.a (fully worked). The plan’s predicted return is 12.3 and its terminal coverage flag is 0.7 of the threshold, but one intermediate step’s flag is 1.4 of the threshold. State the decision.

Solution. The per-step flag is above threshold at an intermediate step, so the plan is rejected. The planner re-samples with a penalty on the uncovered region, which biases toward plans that stay inside the coverage.

WE-L.E.b (completion). After 10 rejected plans in 100 ms, what is the state machine’s action? Completion. The state machine transitions to APPROACH, the conservative impedance controller with a fixed nominal plan, because the planner has failed for the cycle. The approach is slower but safe, and the next cycle retries the planner.

WE-L.E.c (faded). Design the reward’s success term so that the planner prefers a conservative grasp inside the coverage over a risky one at the boundary. Faded step. Penalize the terminal state’s distance to the coverage boundary, and weight the success term below the penalty’s gradient norm. Completion. The success term must be large enough to motivate the grasp and small enough that the coverage penalty dominates at the boundary; the balance is the 3.1.2 gradient-norm rule applied to planning.

WE-L.E.d (contrastive near-miss). A learner uses the coverage-flag threshold as a hard cutoff and finds that the planner almost always rejects, because the edge of the coverage is where the useful grasps are. Why it fails: the coverage’s edge is where the model is least confident, but it is also where the task’s grasps live; a hard cutoff is too conservative. The fix: use a soft penalty that grows toward the boundary, so the planner prefers interior plans when they are comparable and accepts edge plans when they are much better.

Comparative table: the running case’s planner components.

Design principle P41.5. Plan inside the model’s coverage; the coverage’s edge is where the reward and the error both grow, and a soft penalty is the right instrument.

Consolidation checkpoint 5

(a) Three capabilities. You can state the running case’s planner parameters and their bounds. You can apply the per-step coverage flag. You can design the state machine’s fallback.

(b) Transfer probe. The control rate rises to 20 Hz. What changes and what is the fix? Answer: the maximum $E$ doubles if latency is unchanged, but the planning time per step halves, so the sample count must fall; the balance is $N \approx 100$ at 50 ms, which is feasible.

(c) Remediation rule. If you cannot state why a hard coverage cutoff is too conservative, re-read WE-L.E.d before the lab.

Failure taxonomy

The table covers the three lab corruptions plus three failures not exercised in the lab: elite collapse, reward flatness, and flag over-tightness.

The lab

Task. Implement a CEM planner and an MPPI planner over a synthetic latent model, evaluate them against a ground-truth dynamics on a 2-D reaching task, and demonstrate the coverage flag’s effect on the planner’s conservatism.

Conditions. Python 3.10+, NumPy, CPU sufficient. The synthetic dynamics is a 2-D point mass with damping; the model is the RSSM from the 3.2.1 lab, pre-trained. Episode length 100, task to reach a target from a random start. Seed all RNGs.

Controls. (i) CEM with $\rho=0.1$, $N=100$; (ii) MPPI with $\lambda=1.0$, $N=100$; (iii) CEM with a hard coverage flag; (iv) CEM with a soft coverage penalty.

Metrics.

Pre-registered hypotheses.

H1. CEM and MPPI achieve comparable reach success at the same sample count, because both concentrate the search; CEM is faster to converge and MPPI is smoother in the action.

H2. The hard coverage flag rejects a substantial fraction of plans near the task’s boundary, and the soft penalty recovers most of the success at slightly higher conservatism.

H3. The planner’s predicted return exceeds the executed return by a factor that grows with the horizon, the exploitation signature.

H4. The elite-collapse control ($N=20$, $\rho=0.05$) fails to improve over random shooting.

Break-it sequence.

1. Collapse the elite silently. Set $N=20$, $\rho=0.05$, and observe that the plan does not improve across iterations. Silent: the return curve is flat.
2. Extend the horizon beyond the usable horizon silently. Set $H=40$. The predicted return rises and the executed success falls. Silent: the plan looks better by its own metric.
3. Disable the per-step flag silently. The planner finds plans that terminate at the coverage’s edge with inflated predicted return. Silent: the plan passes the safety checks.

Acceptance criteria.

What good looks like. A good artefact has five properties. (1) The two planners are compared on the same states and the same model, so the comparison is clean. (2) The predicted-versus-executed return is plotted against horizon, not reported as a single ratio. (3) The flag’s rejection rate and the conservatism are reported together, so the trade is visible. (4) The break-it results are a table. (5) The plan’s execution length $E$ is a parameter, not a constant.

A minimal planner core, seeded and annotated:

# lab_3_2_3.py — CEM and MPPI planners over a synthetic latent model
import numpy as np
np.random.seed(0)

$D_ACT$, H, N, $N_ITER$, RHO, LAM = 2, 10, 100, 5, 0.1, 1.0

def model_step(z, a):                                   # stand-in for RSSM prior
    return z + a * 0.1 - 0.1 * z                         # damped point mass

def reward(z, target):                                  # negative distance
    return -np.linalg.norm(z - target)

def rollout(z0, actions, target):                       # batched rollout
    z = z0.copy(); G = np.zeros(actions.shape[0])
    for t in range(actions.shape[1]):
        z = model_step(z, actions[:, t, :]) if z.ndim > 1 else \
            np.array([model_step(z[i], actions[i, t]) for i in range(len(z))])
        G += reward(z, target)
    return G, z

def cem(z0, target, mu=None, sigma=1.0):
    mu = np.zeros((H, $D_ACT$)) if mu is None else mu
    for _ in $range(N_ITER$):
        samples = mu + sigma * np.random.randn(N, H, $D_ACT$)   # sample
        G, _ = rollout(z0, samples, target)                    # score
        elite = samples[np.argsort(-G)[:int(RHO * N)]]         # elite
        mu = elite.mean(axis=0)                                # refit mean
        sigma = max(elite.std(), 0.05)                         # variance floor
    return mu

def mppi(z0, target):
    samples = np.random.randn(N, H, $D_ACT$)                     # prior samples
    G, _ = rollout(z0, samples, target)
    w = np.exp((G - G.max()) / LAM); w /= w.sum()              # Eq. (B.1)
    return (w[:, None, None] * samples).sum(axis=0)

def flag(z, train_latents, radius=0.5):                        # coverage check
    return np.linalg.norm(z - train_latents, axis=1).min() > radius

# Evaluation
targets = np.random.randn(50, 2) * 2.0
train_latents = np.random.randn(500, 2) * 1.0                  # stand-in for data
res = {"cem": 0, "mppi": 0, "cem_flag": 0}
for tgt in targets:
    z0 = np.random.randn(2) * 0.5
    $a_cem$ = cem(z0, tgt); $a_mppi$ = mppi(z0, tgt)
    _, $z_cem$  = rollout(z0, $a_cem[None$], tgt)
    _, $z_mppi$ = rollout(z0, $a_mppi[None$], tgt)
    res["cem"]  += float(np.linalg.$norm(z_cem[0$]  - tgt) < 0.3)
    res["mppi"] += float(np.linalg.$norm(z_mppi[0$] - tgt) < 0.3)
    if not $flag(z_cem[0$], train_latents):                   # per-step in practice
        res["cem_flag"] += float(np.linalg.$norm(z_cem[0$] - tgt) < 0.3)
print(res)

Line 7 is the stand-in for the RSSM’s prior; the stand-in is what makes the lab runnable while preserving the structure of the planner’s query. Lines 13–17 are the batched rollout, which is the model’s forward pass. Lines 19–26 are CEM: sample, score, elite, refit, with the variance floor. Lines 28–31 are MPPI: sample, score, exponential weights, weighted mean. Line 33 is the coverage flag; in the running case it is applied at every step, not only the terminal. Lines 36–49 evaluate the three controls and print the success counts.

Cheat sheet

Exercises with solutions

E 3.2.3.1. Write the planning objective and identify its three shaping properties.

S 3.2.3.1. Maximize the expected return under the belief subject to feasibility. The three properties: the expectation is over the belief not the mean; the constraint set is not a box; the horizon is bounded by the model’s trust. The lesson: each property changes the algorithm, and a planner that ignores any of them is wrong in a specific way.

E 3.2.3.2 (transfer). A posterior over the cube’s $y$-position has modes at 0.1 and 0.3 with equal weight and $\sigma=0.02$. The gripper is at 0.2. Compute the reward at the mean and under the belief with $r = -|y_{grip}-y_{cube}|$.

S 3.2.3.2. At the mean 0.2, $r = 0$. Under the belief, each mode gives $-0.1$, so the expectation is $-0.1$. The mean overestimates by 0.1. The lesson: the mean-latent planner is systematically optimistic under a multimodal belief.

E 3.2.3.3. Compute the MPPI weights for returns $G = [0, 1, 2]$ at $\lambda = 0.5$.

S 3.2.3.3. $\exp(G/\lambda) = [1, e^2, e^4] = [1, 7.39, 54.6]$, sum 62.99, weights $[0.016, 0.117, 0.867]$. The lesson: $\lambda$ controls the concentration; at $\lambda=0.5$ the best sample receives most of the weight.

E 3.2.3.4 (diagnostic). A CEM planner’s elite variance falls to 0.001 after one iteration and the plan does not improve. Diagnose and fix.

S 3.2.3.4. The elite set is degenerate: $\rho N$ is too small or the elite set is highly correlated. The fix is to raise $N$ or $\rho$, or add a variance floor. The lesson: the elite fraction times the sample count must be at least a handful, and the floor prevents the collapse.

E 3.2.3.5. State the two requirements for a usable gradient planner and give one diagnostic for each.

S 3.2.3.5. (1) Differentiability through the stochastic latent: diagnostic is to compare the gradient plan with CEM on the same state; systematic divergence flags a bias. (2) A smooth model: diagnostic is to compare the gradient’s norm across states; a sharp increase flags an artefact. The lesson: the gradient’s usability is measurable, not a matter of trust.

E 3.2.3.6 (transfer). The usable horizon is 15 and the control rate is 10 Hz with 20 ms planning latency. Choose $H$ and $E$, and justify.

S 3.2.3.6. $H = 10$, below the usable horizon with margin; $E = 3$, at 300 ms re-planning, which is well within the 100 ms latency and gives reactivity. The lesson: $H$ and $E$ are chosen from independent constraints — the model’s trust and the loop’s latency.

E 3.2.3.7 (diagnostic). A gradient planner finds a plan whose predicted return is 50 and whose executed return is 5. Name two mitigations and the discriminating test.

S 3.2.3.7. (1) Shorter horizon: test by reducing $H$ and observing whether the gap shrinks. (2) Uncertainty penalty or trust region: test by adding the penalty and observing whether the plan moves toward the data. The lesson: exploitation is a horizon-and-distribution problem, not a gradient problem.

E 3.2.3.8. Design the safety-check order and justify it.

S 3.2.3.8. Force, collision, rate, model flag. Force is first because a force violation is a physical hazard; collision is next because it is a workspace hazard; rate is a hardware hazard; the flag is informational and cheapest to check last. The lesson: the order reflects the severity and reversibility of the failure.

E 3.2.3.9 (transfer). A hard coverage flag rejects 60% of plans near the task’s boundary; a soft penalty recovers 90% of the success. State the trade and the choice.

S 3.2.3.9. The hard flag trades conservatism for safety, and the soft penalty trades a small amount of conservatism for most of the success. The choice is the soft penalty with a monitoring rule: if the executed return falls below the prediction by more than a threshold, tighten the penalty. The lesson: the flag is a knob, not a binary, and the monitoring rule closes the loop.

Exit test

You are ready for Module 3.3 when you can, without notes:

1. Write the planning objective and its three shaping properties.
2. Derive the mean-latent overestimate under a multimodal belief.
3. Write the CEM update and state the variance floor’s role.
4. Write the MPPI update and state the temperature’s role.
5. Compute a weighted action from a return vector at a given temperature.
6. State the two requirements for a usable gradient planner.
7. State the exploitation mechanism and its three mitigations.
8. Write the uncertainty penalty for an ensemble.
9. State the receding-horizon rule and choose $H$ from the usable horizon.
10. Choose $E$ from the planning latency and control rate.
11. Order the four safety checks and justify the order.
12. Write the PLAN state’s entry, exit, and health checks.
13. Design the running case’s planner parameters and their bounds.
14. Apply the per-step coverage flag and explain why terminal-only is insufficient.
15. State the soft-penalty trade and the monitoring rule.

Cross-references. 3.2.1 supplies the RSSM prior, posterior, and coverage ratio restated in §L.A.3 and §L.E.2. 3.2.2 supplies the usable-horizon derivation restated in §L.D.1. 2.3.2 supplies the clearance constraint restated in §L.A.1 and §L.D.2. 2.3.3 supplies the force limits restated in §L.D.2. 2.2.2 supplies the short-rollout result restated in §L.D.1. 1.1.4 supplies the state machine extended in §L.D.3. Module 3.3 uses the planner as the layer beneath the LLM reasoning layer; Module 4.2 uses the planner as the policy layer’s decision mechanism.

Ready. The next lesson to write is 3.3.1 — LLM Task Planning and Decomposition. Say “next” to generate it.