Trajectory Planning

Step 1 — the chain rule. With $q = q(s(t))$, $\dot q = q'\dot s$ and $\ddot q = q''\dot s^2 + q'\ddot s$ (eqs. 11.2–11.3). The first is linear in $\dot s$; the second has a term in $\dot s^2$ and a term in $\ddot s$, and nothing else.

Step 2 — substitute. Into $M(q)\ddot q + \dot q\T\Gamma(q)\dot q + g(q) = u$:

Step 3 — group by powers of the clock. The velocity-product term is bilinear in $\dot q$, so the two factors of $\dot s$ come out as $\dot s^2$:

Every coefficient depends on $s$ alone. The $n$ equations now constrain one scalar unknown $\ddot s$ at each state $(s, \dot s)$ — that is the whole content of path-velocity decomposition.

The velocity-product piece of $b$ is exactly Chapter 17's velocityProduct(q, q′), the vector $C(q, \dot q)\dot q$ evaluated at $\dot q = q'$. No new physics is computed here; the library function PathDynamics::abc calls the Chapter 17 model and does the two matrix–vector products.

Why $b_i$ can vanish when $a_i$ does not, and vice versa (Choset Problem 11.5). $a_i = (Mq')_i$ is zero when the path's tangent is orthogonal, in the $M$-metric's $i$-th row, to joint $i$ — the RP arm's prismatic joint at the midpoint of a straight Cartesian line, where the radial velocity is momentarily zero. $b_i$ collects $M q''$ and the centrifugal/Coriolis terms, which can vanish on a straight segment ($q'' = 0$) with no Coriolis coupling even where $a_i \ne 0$. The micro-example below has $b_1 = 0$ and $a_1 = \pi$ at every $s$; Choset's RP arm has $a_2(\tfrac12) = 0$ and $b_2 \equiv 0$.

Step 1 — isolate $\ddot s$ in each row. Row $i$ of $u^{\min} \le a\ddot s + b\dot s^2 + c \le u^{\max}$ reads $u_i^{\min} - b_i\dot s^2 - c_i \le a_i\ddot s \le u_i^{\max} - b_i\dot s^2 - c_i$.

Step 2 — divide, and flip if negative. Dividing by $a_i > 0$ gives $\beta_i \le \ddot s \le \alpha_i$. Dividing by $a_i < 0$ reverses the inequality: $\alpha_i \le \ddot s \le \beta_i$. That swap is the whole content of eq. 11.8, and it is why the sign of $a_i$ matters: a joint whose coordinate decreases along the path has its "maximum torque" bound on the low side of $\ddot s$.

Step 3 — intersect the intervals. $\ddot s$ must satisfy all $n$ rows, so it lies in $[\max_i L_i, \min_i U_i] = [L, U]$. Each $L_i$ and $U_i$ is an affine function of $x = \dot s^2$ at fixed $s$: $L_i = p_i + m_i x$ with $m_i = -b_i/a_i$. The max of affine functions is convex and piecewise affine; the min is concave. Neither is smooth, so neither is $v$.

Step 4 — the boundary of admissibility. The cone is empty when $L > U$, i.e. when some $L_i > U_j$. At fixed $s$, as $\dot s$ grows from zero, the first value at which some pair crosses, $L_i(\dot s) = U_j(\dot s)$, is where admissibility is lost. Because each side is affine in $x$, the crossing is in closed form, $x_{ij} = (p^U_j - p^L_i)/(m^L_i - m^U_j)$, and $v(s) = \min$ over pairs of $\sqrt{x_{ij}}$ taken over positive roots. The minimum rule is exact rather than heuristic: if any $L_i = U_j$ then $L \ge L_i = U_j \ge U$, so the state is already on or past the boundary.

What the closed form assumes (Choset footnote 2). That $\dot s = 0$ is admissible for every $s$ — the robot can hold any configuration statically — and that admissibility is lost at most once as $\dot s$ grows. Friction, weak actuators, or velocity-dependent limits can violate this and create inadmissible islands in the plane. The algorithm below ignores islands; the library reports the first assumption's failure as an error rather than guessing.

A check that is not a frozen number. velocityLimit is compared against a brute-force scan of $L \le U$ with bisection on $150$ seeded random $(a, b, c)$ rows in two and three dimensions; the worst disagreement is $4 \times 10^{-14}$.

Step 1 — higher is faster. For two admissible curves $\dot s_a(s) \ge \dot s_b(s)$ on $[0, 1]$, $\int ds/\dot s_a \le \int ds/\dot s_b$. Time is monotone in the curve's height, so the optimum is the pointwise-highest admissible curve.

Step 2 — a tangent strictly inside the cone is wasteful. If at some state the curve's slope $d\dot s/ds = \ddot s/\dot s$ lies strictly between $L/\dot s$ and $U/\dot s$, one can raise the curve locally — accelerate harder before, brake harder after — without leaving admissibility, and the new curve is higher. So on an optimal curve the tangent is on the boundary almost everywhere.

Step 3 — the reachable set is bounded by two integral curves. From any state, the set of all curves with tangents inside the cones is bounded above by the $U$-integral curve and below by the $L$-integral curve (Choset's figure 11.2). The optimum is therefore a concatenation of pieces of $U$- and $L$-curves.

Step 4 — where the switches are. The pieces meet where a $U$-curve crosses an $L$-curve, or where the limit curve forces a change: a $U$-curve that would penetrate the amber must have been abandoned earlier for an $L$-curve that grazes the limit tangentially. Finding those points is the algorithm.

The Pontryagin reading. Chapter 19 derives the same result from the minimum principle: for a minimum-time problem the Hamiltonian is affine in the control, $\partial H/\partial u$ contains no $u$, and $H$ is minimized at a bound of the control set. Choset's §11.3.1 makes the connection explicit for the double integrator, where the switch is at $t_s = \sqrt{d/u_{\max}}$ — the same number the lattice reproduces below.

Step 1 — the row loses its $\ddot s$. With $a_i = 0$, row $i$ of eq. 11.7 reads $u_i^{\min} \le b_i\dot s^2 + c_i \le u_i^{\max}$. Nothing the actuator does changes $\ddot s$; the constraint is on $\dot s$ alone. Solving for $\dot s$ gives $\dot s^{\max}_{zip}$ (eq. 11.11). If $b_i = 0$ too, the row constrains nothing — provided the robot can hold the configuration, which is Choset's Problem 11.6 and the standing assumption.

Step 2 — a critical point is not a collapsed cone. On $v(s)$ the cone is a single vector, $L = U$. At a point of $\dot s^{\max}$ set by a zero-inertia bound, $L < U$: the cone is open, but it may point out of the admissible region.

Step 3 — clip to the tangent. Following $U$ forward from a singular point penetrates the limit curve; the fastest admissible acceleration is the one that keeps the state on the curve, which is the curve's tangent in the $(s, \dot s)$ plane: $d\dot s/dt = \ddot s = \dot s\,d\dot s^{\max}/ds$. Hence $\ddot s_{\max} = \min(\ddot s^{+}_{tangent}, U)$, with the right-hand derivative where the curve has a corner, and symmetrically $\ddot s_{\min} = \max(\ddot s^{-}_{tangent}, L)$ for the backward integration.

Step 4 — singular arcs. Critical points lie where $M(q)$ loses rank along the path's tangent, a lower-dimensional set; a path crossing it transversally has isolated critical points, a path moving along it has a critical arc. Integrating $\ddot s_{\max}$ and $\ddot s_{\min}$ instead of $U$ and $L$ lets the algorithm slide along such an arc with an acceleration strictly between $L$ and $U$, rather than chattering between them — and on a slide neither actuator is saturated.

In $x = \dot s^2$ the rule is a one-liner. The library integrates in $x$, where a stage from $s_k$ to $s_{k+1}$ with constant $\ddot s$ moves $x$ by $2\ddot s\,\Delta$. When the free step would land above $x_{\max}(s_{k+1})$, the stage acceleration that lands exactly on the curve is $u^* = (x_{\max} - x_k)/(2\Delta)$; if $u^*$ is inside the cone the stage slides, otherwise it penetrates. At a critical point that is Choset's rule; at an ordinary point of $v$ the collapsed cone makes the test pass only when the integral curve is tangent to the limit — which is the tangent-point condition of step 4 of the algorithm, found for free.

Step 1 — closure under the control set. Integrating $\ddot q_i = c\,a_{\max}$, $c \in \{-1, 0, 1\}$, for time $h$ from $(q_i, \dot q_i)$ gives $\dot q_i + c\,a_{\max}h$ and $q_i + \dot q_i h + \tfrac12 c\,a_{\max}h^2$. Write $\dot q_i = m\,a_{\max}h$ and $q_i = p\cdot\tfrac12 a_{\max}h^2$ with integers $m, p$; then the new indices are $m + c$ and $p + 2m + c$ — integers again. Every node of Choset's figure 11.8 lies on the grid of figure 11.9, and the tree is really a graph on that grid.

Step 2 — level is time. Each edge takes exactly $h$, so breadth-first expansion is uniform-cost search with unit edges, and the first level at which a node enters the goal region is the minimum number of steps: time-optimal for the chosen discretization of time and controls. A* with an admissible heuristic pops the same level with fewer expansions. The per-axis continuous bang-bang time $\max_i T_i$ is admissible because it ignores the other axes, the obstacles, the velocity limit and the discretization — every one of which can only make the lattice slower.

Step 3 — safety absorbs the discretization. Any continuous $\delta_v$-safe trajectory can be tracked by lattice moves within an error that shrinks with $h$; the margin $c_0 + c_1\|\dot q\|$ is spent on that error, leaving $(1 - \epsilon)\delta_v$. The two bounds on $h$ are what make the bookkeeping close — the first keeps $v_{\max}$ an integer multiple of $a_{\max}h$, the second ties the tracking error to the margin.

Step 4 — the grid is finite, so the time is bounded. The configuration-space diameter, the velocity bound and $h$ fix the number of lattice points, which is $O((1/\epsilon)^{3n})$ once $h$ is expressed through $\epsilon$. Polynomial in $\epsilon$, exponential in $n$: nobody has run this beyond a few degrees of freedom, and Choset says so.

What the theorem does not say. The $(1 - \epsilon)\delta_v$-safe trajectory the algorithm finds may look nothing like the true time-optimal $\delta_v$-safe one; the only promise is about its duration. And the proof — Donald and Xavier's — is beyond this chapter as it is beyond Choset's.

The manipulator generalization (Choset pp. 396–399). For $M(q)\ddot q + C\dot q + g = u$ with box torque limits, the feasible accelerations at a state form a parallelepiped $\mathcal{A}(q, \dot q)$ that changes during the step and collapses where $M^{-1}$ loses rank. A bound on the largest eigenvalue of $M$ keeps it from collapsing; a bound on $\dot M$ gives a conservative subset $\hat{\mathcal{A}} \subset \mathcal{A}$ feasible over the whole step; the controls are the $\ddot q$ grid points inside $\hat{\mathcal{A}}$. The completeness result survives with $(1 + \epsilon)T_{opt}$ in place of $T_{opt}$; the running time stays exponential in $n$.

Step 1 — the substitution. $\tfrac{d}{ds}\dot s^2 = 2\dot s\,\tfrac{d\dot s}{ds} = 2\ddot s$. In $x$ the kinematics are linear and a constant-$\ddot s$ stage is a straight line; the micro-example's switch at $s = \tfrac12$ lands on the grid exactly, and $\dot s = 0$ is no longer a singularity of the integration.

Step 2 — convexity of the stage constraint set. At fixed $s_k$ the $2n$ inequalities in $(x_k, u_k)$ are half-planes, and $x_{k+1} = x_k + 2u_k\Delta$ is affine. The feasible set is a convex polygon; its projection onto the $x_k$ axis is an interval.

Step 3 — propagate backward. $K_N = [x_f, x_f]$. Given $K_{k+1} = [\ell, h]$, add the two half-planes $\ell \le x_k + 2u_k\Delta \le h$ and take the minimum and maximum of $x_k$ over the polygon — two two-variable LPs, solved exactly by vertex enumeration since there are at most $2n + 6$ half-planes. The result is $K_k$, the set of states at $s_k$ from which the goal is still reachable: the complement of Choset's "doomed" states, computed without ever drawing a curve.

Step 4 — greedy forward is optimal. A larger $x_k$ never shrinks the reachable set downstream (controllability is monotone in $x$ under these constraints), so choosing the largest admissible $x_{k+1} \in K_{k+1}$ at each stage dominates every other choice pointwise, and pointwise-highest is fastest by the first derivation.

What it sidesteps. No tangent points, no root finding, no footnote-3 bisection, no special case for zero-inertia rows — a row with $a_i = 0$ is just a half-plane with no $u$ in it. The price is a grid: the answer is optimal for the discretization, and the stage constraints are enforced at $s_k$ only. On the chapter's seeded gravity-loaded splines at $N = 800$ the two constructions agree on $t_f$ to $0.38\%$ at worst, and they agree on which problems have no solution at all.