In classical configuration-space planning we represent the robot's pose by a configuration \( \mathbf{q} \in \mathcal{Q} \subset \mathbb{R}^n \). Obstacles in the workspace induce a configuration obstacle region \( \mathcal{Q}_{\text{obs}} \subset \mathcal{Q} \) and a free space \( \mathcal{Q}_{\text{free}} = \mathcal{Q} \setminus \mathcal{Q}_{\text{obs}} \). A geometric path is a continuous curve

\[ \sigma : [0,1] \to \mathcal{Q}, \quad \sigma(0) = \mathbf{q}_{\text{start}}, \; \sigma(1) \in \mathcal{Q}_{\text{goal}} , \]

and feasibility requires \( \sigma(s) \in \mathcal{Q}_{\text{free}} \) for all \( s \in [0,1] \). Algorithms such as PRM and RRT operate in this geometric setting: they generate curves in \( \mathcal{Q} \) and check collision, but they do not enforce dynamic feasibility (e.g., torque limits, nonholonomic constraints).

Real robots, however, have states that include velocities and often other dynamic quantities. Let \( \mathcal{X} \) denote the state space, e.g.

\[ \mathcal{X} = \mathcal{Q} \times \mathbb{R}^n, \quad \mathbf{x} = (\mathbf{q}, \dot{\mathbf{q}}), \]

and let \( \mathcal{U} \) be the space of admissible control inputs (joint torques, wheel velocities, thrust vectors, and so on). The robot's evolution is described by a dynamical system

\[ \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(0) = \mathbf{x}_0 , \]

derived from rigid-body dynamics (e.g., the Euler–Lagrange or Newton–Euler equations that you already know from basic robotics).

Kinodynamic planning asks for a control signal \( \mathbf{u} : [0,T] \to \mathcal{U} \) and a horizon \( T > 0 \) such that the resulting trajectory satisfies:

\[ \mathbf{x}(0) = \mathbf{x}_{\text{start}}, \quad \mathbf{x}(T) \in \mathcal{X}_{\text{goal}}, \quad \mathbf{x}(t) \in \mathcal{X}_{\text{free}}, \; \mathbf{u}(t) \in \mathcal{U}_{\text{adm}} \text{ for all } t \in [0,T]. \]

Compared to geometric planning, we have:

• A higher-dimensional search space \( \mathcal{X} \) (often double the dimension of \( \mathcal{Q} \)).
• Additional constraints encoded by the dynamics \( f \) and admissible controls \( \mathcal{U}_{\text{adm}} \).
• An explicit dependence on time \( t \) and horizon \( T \).

This already hints at why kinodynamic planning is much harder. The diagram below summarizes the conceptual shift.

flowchart TD
  Qgeom["Geometric planning: paths sigma(s) in Q_free"] --> Galg["Graph / sampling in configuration space"]
  Galg --> Gpath["Collision-free path (no dynamics)"]

  Kin["Kinodynamic planning: trajectories x(t) in X_free"] --> Kalg["Sampling in state and control spaces"]
  Kalg --> Sim["Forward simulate dynamics x_dot = f(x,u)"]
  Sim --> Ktraj["Dynamically feasible, collision-free trajectory"]

  Qgeom --- Kin
        

We now formulate kinodynamic planning precisely. Let:

• \( \mathcal{X} \subset \mathbb{R}^m \) be the state space,
• \( \mathcal{U} \subset \mathbb{R}^p \) be the control space,
• \( \mathcal{X}_{\text{obs}} \subset \mathcal{X} \) be the state-obstacle region,
• \( \mathcal{X}_{\text{free}} = \mathcal{X} \setminus \mathcal{X}_{\text{obs}} \) the free state space,
• \( \mathcal{X}_{\text{goal}} \subset \mathcal{X}_{\text{free}} \) the goal region,
• \( \mathcal{U}_{\text{adm}} \subset \mathcal{U} \) the set of admissible controls (e.g. input bounds).

The dynamics are given by a (possibly nonlinear) ODE:

\[ \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(0) = \mathbf{x}_0 \in \mathcal{X}_{\text{free}}. \]

A trajectory is a pair \( (\mathbf{x}(\cdot), \mathbf{u}(\cdot)) \) such that \( \mathbf{u} : [0,T] \to \mathcal{U}_{\text{adm}} \), \( \mathbf{x} : [0,T] \to \mathcal{X}_{\text{free}} \), and the ODE holds almost everywhere. The planning problem is:

\[ \begin{aligned} &\text{Find } T > 0 \text{ and measurable } \mathbf{u}(\cdot) \text{ such that} \\ &\dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \ \mathbf{x}(0) = \mathbf{x}_0, \\ &\mathbf{x}(t) \in \mathcal{X}_{\text{free}},\; \mathbf{u}(t) \in \mathcal{U}_{\text{adm}} \text{ for all } t \in [0,T], \\ &\mathbf{x}(T) \in \mathcal{X}_{\text{goal}}. \end{aligned} \]

Often we also optimize a cost:

\[ J(\mathbf{u}(\cdot)) = \int_0^T \ell(\mathbf{x}(t), \mathbf{u}(t))\, dt + \Phi(\mathbf{x}(T)), \]

giving rise to an optimal kinodynamic planning problem: minimize \( J \) subject to the dynamic and collision constraints.

A fundamental difference from geometric planning is that the decision variable is an entire function \( \mathbf{u}(\cdot) \), i.e. an element of an infinite-dimensional function space (say, \( L^2([0,T],\mathbb{R}^p) \)). This infinite-dimensional nature is one of the core reasons why kinodynamic planning is difficult.

Under mild regularity assumptions (e.g. \( f \) locally Lipschitz in \( \mathbf{x} \), measurable in \( \mathbf{u} \)), there exists a unique trajectory for every admissible control input. This is good news analytically, but it does not by itself make the search problem easier.

3.1 Explosion of State and Time Dimensions

For an \( n \)-DOF manipulator with configuration \( \mathbf{q} \in \mathbb{R}^n \), a minimal state often includes joint velocities \( \dot{\mathbf{q}} \in \mathbb{R}^n \), so \( m = 2n \). If we include actuator states, floating-base 6D pose, or elastic modes, the state dimension grows further. Sampling in a \( 2n \)-dimensional space is dramatically harder than in \( n \) dimensions: the number of samples needed to maintain a given resolution scales exponentially in \( m \).

Moreover, time becomes an explicit variable. Some planners augment the state with time, forming \( \tilde{\mathbf{x}} = (\mathbf{x}, t) \) in \( \mathcal{X} \times [0,T] \). Searching in this augmented space further increases dimensionality.

3.2 Differential and Nonholonomic Constraints

Many robots (mobile bases, spacecraft, underactuated arms) are subject to nonholonomic constraints that restrict allowable velocities:

\[ A(\mathbf{q}) \dot{\mathbf{q}} = \mathbf{0}, \quad A(\mathbf{q}) \in \mathbb{R}^{k \times n}, \; k \lt n. \]

The rolling-without-slipping constraint of a car or differential-drive robot can be written in this Pfaffian form. Such constraints cannot be integrated into purely positional constraints; they restrict the tangent space of allowable motions at each configuration.

Additionally, many robots are underactuated: the number of independent control inputs \( p \) is strictly smaller than the configuration dimension \( n \). A simple model is

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}}) \dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{B}(\mathbf{q}) \mathbf{u}, \quad \mathbf{B}(\mathbf{q}) \in \mathbb{R}^{n \times p}, \; p \lt n. \]

Here the range of \( \mathbf{B}(\mathbf{q}) \) does not span all generalized forces, so not every acceleration \( \ddot{\mathbf{q}} \) is achievable. The robot can only accelerate within a subspace of the tangent space.

Geometric planners ignore these constraints: they may generate a path in \( \mathcal{Q}_{\text{free}} \) that is dynamically unrealizable. Kinodynamic planners must search only among trajectories that satisfy these differential and actuation constraints, significantly reducing the set of reachable states but making search far harder.

3.3 Infinite-Dimensional Control Search

Even if state space is finite-dimensional, the decision variable \( \mathbf{u}(\cdot) \) is a function of time. To implement a planner, we must approximate this function. A standard approach is to discretize time with step size \( \Delta t \) and parameterize controls as piecewise constant:

\[ \mathbf{u}(t) = \mathbf{u}_k \quad \text{for } t \in [k \Delta t, (k+1)\Delta t), \quad k = 0,\dots,N-1, \]

where \( N = T / \Delta t \). The search space then becomes \( \mathcal{U}_{\text{adm}}^N \), whose size grows exponentially in \( N \) if we also discretize control values. Choosing \( \Delta t \) too large yields coarse, inaccurate trajectories; choosing it too small makes search intractable. This trade-off is at the heart of many kinodynamic planning algorithms.

3.4 Hardness Summary Diagram

flowchart LR
  Geo["Geometric path planning"] -->+Dim["Add velocities and time (higher dimension)"]
  Dim -->+Dyn["Add dynamics x_dot = f(x,u) and input bounds"]
  Dyn -->+Nonh["Add nonholonomic and underactuation constraints"]
  Nonh -->+Num["Add numerical integration and discretization error"]
  Num --> Hard["Kinodynamic planning: high-dimensional, \nconstrained, numerically sensitive search"]
        

Even before adding dynamics, exact motion planning in high-dimensional configuration spaces is known to be PSPACE-hard. Roughly, this means that deciding whether a path exists can encode any problem solvable by a polynomial-space Turing machine. The presence of obstacles that encode logical constraints leads to reductions from quantified Boolean formulas.

Kinodynamic planning is at least as hard as geometric planning, because we can embed a geometric problem as a special case of kinodynamic planning:

\[ \begin{aligned} &\text{Take } \mathcal{X} = \mathcal{Q}, \ \mathcal{U} = \mathcal{Q}, \\ &f(\mathbf{x},\mathbf{u}) = \mathbf{u}, \quad \mathcal{U}_{\text{adm}} = \{ \mathbf{u} : \|\mathbf{u}\| \le U_{\max} \},\\ &\mathcal{X}_{\text{obs}} = \mathcal{Q}_{\text{obs}}, \quad \mathcal{X}_{\text{goal}} = \mathcal{Q}_{\text{goal}}. \end{aligned} \]

Any geometric path \( \sigma(s) \) can be reparameterized into a trajectory \( \mathbf{x}(t) \) with a bounded-velocity control signal. Conversely, if the dynamics are \( \dot{\mathbf{x}} = \mathbf{u} \) with bounded control, reachable trajectories correspond to Lipschitz-continuous geometric paths. Thus geometric planning reduces to kinodynamic planning.

When optimality is added (e.g. time-optimal or energy-optimal trajectories), the problem usually becomes even harder. For many natural cost functionals, the resulting problem is at least NP-hard and, in continuous form, is related to optimal control and calculus of variations. There is typically no closed-form solution and numerical methods can get trapped in poor local minima or fail to converge.

Practically, these hardness results justify why kinodynamic planning algorithms (like kinodynamic RRT, state lattices, and trajectory optimization with dynamics) are heuristic and focus on asymptotic guarantees or probabilistic completeness rather than exact, worst-case optimal solutions.

Most kinodynamic planners rely on forward simulation of the dynamics to propagate states from a node using a candidate control. Suppose we use explicit Euler with step size \( \Delta t \):

\[ \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t\, f(\mathbf{x}_k, \mathbf{u}_k), \quad k = 0,\dots,N-1. \]

Let \( \mathbf{x}(t) \) be the true solution and \( \mathbf{x}_k \) the numerical approximation at time \( t_k = k \Delta t \). Under a global Lipschitz condition \( \|f(\mathbf{x}_1,\mathbf{u}) - f(\mathbf{x}_2,\mathbf{u})\| \le L \|\mathbf{x}_1 - \mathbf{x}_2\| \), a standard stability argument (via discrete Grönwall inequality) yields an error bound

\[ \|\mathbf{x}(t_k) - \mathbf{x}_k\| \le C \Delta t \left( e^{L t_k} - 1 \right), \]

where \( C \) depends on bounds on higher derivatives. We see that:

• Local truncation error is \( O(\Delta t^2) \), but
• Global error grows roughly like \( O(\Delta t e^{L T}) \).

For long-horizon planning (large \( T \)) or stiff dynamics (large \( L \)), integration error can be substantial unless \( \Delta t \) is very small. Yet a small \( \Delta t \) increases the branching factor and depth of the search tree, exacerbating computational cost.

Another complication is that collision checking is usually performed only at discrete sample points along the simulated trajectory. If we step too coarsely in time, a trajectory can “jump” through a thin obstacle between two samples. Ensuring resolution completeness (i.e., finding a solution whenever one exists at the chosen resolution) requires careful coupling of spatial and temporal step sizes.

To make the preceding discussion concrete, consider a 1D double-integrator model, a common abstraction for a single joint or a prismatic actuator:

\[ \dot{x}(t) = v(t), \quad \dot{v}(t) = u(t), \quad |u(t)| \le u_{\max}. \]

The state is \( \mathbf{x} = (x,v) \in \mathbb{R}^2 \), the control is scalar \( u \in [-u_{\max}, u_{\max}] \). Below we implement a simple forward-simulation primitive in several languages. This primitive is a building block of many kinodynamic planners: from a node \( \mathbf{x}_k \), it simulates forward for a fixed time \( T_{\text{step}} \) under a constant control \( u \).

6.1 Python (with NumPy, stand-in for robotics libraries)

import numpy as np

def propagate_double_integrator(x0, u, dt=0.01, T_step=0.5, u_max=1.0):
    """
    x0: np.array([x, v])
    u:  scalar control (will be saturated to [-u_max, u_max])
    dt: integration step
    T_step: total propagation time
    """
    u = float(np.clip(u, -u_max, u_max))
    x = np.array(x0, dtype=float)
    trajectory = [x.copy()]

    N = int(T_step / dt)
    for k in range(N):
        # dynamics: x_dot = [v, u]
        x_dot = np.array([x[1], u])
        x = x + dt * x_dot
        trajectory.append(x.copy())
    return np.array(trajectory)

if __name__ == "__main__":
    x_start = np.array([0.0, 0.0])   # position, velocity
    u = 0.8
    traj = propagate_double_integrator(x_start, u)
    print("Final state:", traj[-1])
      

In practice, Python-based robotic stacks (e.g. through OMPL Python bindings or ROS-based simulation) wrap more complex dynamics and collision checking around this kind of forward propagation.

6.2 C++ (standalone; conceptually similar to OMPL propagators)

#include <iostream>
#include <vector>
#include <cmath>

struct State {
    double x; // position
    double v; // velocity
};

std::vector<State> propagateDoubleIntegrator(
        const State& x0, double u, double dt, double T_step, double u_max)
{
    double u_sat = std::max(-u_max, std::min(u, u_max));
    State x = x0;
    std::vector<State> traj;
    traj.push_back(x);

    int N = static_cast<int>(T_step / dt);
    for (int k = 0; k < N; ++k) {
        // Euler integration
        State xdot;
        xdot.x = x.v;
        xdot.v = u_sat;

        x.x += dt * xdot.x;
        x.v += dt * xdot.v;

        traj.push_back(x);
    }
    return traj;
}

int main() {
    State x0{0.0, 0.0};
    double u = 0.8;
    double dt = 0.01;
    double T_step = 0.5;
    double u_max = 1.0;

    auto traj = propagateDoubleIntegrator(x0, u, dt, T_step, u_max);
    std::cout << "Final state: x = "
              << traj.back().x
              << ", v = " << traj.back().v << std::endl;
    return 0;
}
      

Libraries like OMPL expose a similar interface: the planner calls a user-provided state propagation callback that integrates the dynamics for small time steps.

6.3 Java (e.g. for educational simulators)

public class DoubleIntegrator {

    public static class State {
        public double x;
        public double v;
        public State(double x, double v) {
            this.x = x;
            this.v = v;
        }
    }

    public static java.util.List<State> propagate(
            State x0, double u, double dt, double T_step, double uMax) {

        double uSat = Math.max(-uMax, Math.min(u, uMax));
        State x = new State(x0.x, x0.v);
        java.util.List<State> traj = new java.util.ArrayList<>();
        traj.add(new State(x.x, x.v));

        int N = (int)(T_step / dt);
        for (int k = 0; k < N; ++k) {
            double xdot = x.v;
            double vdot = uSat;

            x.x += dt * xdot;
            x.v += dt * vdot;

            traj.add(new State(x.x, x.v));
        }
        return traj;
    }

    public static void main(String[] args) {
        State x0 = new State(0.0, 0.0);
        double u = 0.8;
        java.util.List<State> trajectory =
            propagate(x0, u, 0.01, 0.5, 1.0);

        State xf = trajectory.get(trajectory.size() - 1);
        System.out.println("Final state: x = " + xf.x + ", v = " + xf.v);
    }
}
      

6.4 MATLAB / Simulink

function traj = propagate_double_integrator(x0, u, dt, T_step, u_max)
% x0 = [x; v]
% u: scalar control
    if nargin < 3, dt = 0.01; end
    if nargin < 4, T_step = 0.5; end
    if nargin < 5, u_max = 1.0; end

    u_sat = max(-u_max, min(u, u_max));
    x = x0(:);
    N = floor(T_step / dt);
    traj = zeros(2, N+1);
    traj(:,1) = x;

    for k = 1:N
        xdot = [x(2); u_sat];   % [x_dot; v_dot]
        x = x + dt * xdot;
        traj(:,k+1) = x;
    end
end

% In Simulink:
% - Create an Integrator block chain for v and x.
% - Feed a Saturation block with constant u into the acceleration input.
% - Log x and v using "To Workspace" blocks for analysis.
      

6.5 Wolfram Mathematica

(* Double-integrator dynamics: x'(t) = v(t), v'(t) = u0 with |u0| <= umax *)
Clear[simulateDoubleIntegrator]

simulateDoubleIntegrator[x0_, v0_, u0_, tmax_] := Module[
  {uSat, sol},
  uSat = Clip[u0, {-1.0, 1.0}];
  sol = NDSolve[
    {
      x'[t] == v[t],
      v'[t] == uSat,
      x[0] == x0,
      v[0] == v0
    },
    {x, v},
    {t, 0, tmax}
  ];
  {x[tmax] /. sol[[1]], v[tmax] /. sol[[1]]}
];

simulateDoubleIntegrator[0.0, 0.0, 0.8, 0.5]
      

All these snippets expose the same conceptual interface: given a state, a control, and integration parameters, they return a trajectory. In a full kinodynamic planner we would add:

• Sampling or optimization over control values and durations,
• Collision checking along the trajectory,
• Graph/tree search in the state space built from these propagated segments.

Problem 1 (Geometric Planning as a Special Case): Show rigorously that geometric path planning in configuration space can be formulated as a special case of kinodynamic planning by an appropriate choice of state space, control space, and dynamics.

Solution:

Let the configuration space be \( \mathcal{Q} \subset \mathbb{R}^n \) with free space \( \mathcal{Q}_{\text{free}} \) and goal region \( \mathcal{Q}_{\text{goal}} \subset \mathcal{Q}_{\text{free}} \). Define:

\[ \begin{aligned} &\mathcal{X} = \mathcal{Q}, \quad \mathcal{X}_{\text{free}} = \mathcal{Q}_{\text{free}}, \quad \mathcal{X}_{\text{goal}} = \mathcal{Q}_{\text{goal}}, \\ &\mathcal{U} = \mathcal{Q}, \quad f(\mathbf{x},\mathbf{u}) = \mathbf{u}. \end{aligned} \]

A control signal \( \mathbf{u} : [0,T] \to \mathcal{Q} \) with bounded norm induces the trajectory \( \mathbf{x}(t) = \mathbf{x}_0 + \int_0^t \mathbf{u}(s)\, ds \). If \( \mathbf{u} \) is continuous and \( \mathcal{X}_{\text{free}} \) is open, then \( \mathbf{x}(\cdot) \) is a continuous curve in \( \mathcal{Q}_{\text{free}} \). Conversely, any continuous path \( \sigma : [0,1] \to \mathcal{Q}_{\text{free}} \) can be reparameterized in time: set \( \mathbf{x}(t) = \sigma(t/T) \), and define \( \mathbf{u}(t) = \dot{\mathbf{x}}(t) \) where it exists. Thus the existence of a geometric path is equivalent to the existence of a kinodynamic trajectory under these dynamics. Therefore geometric planning reduces to kinodynamic planning.

Problem 2 (Reachable Set of Double Integrator): For the 1D double-integrator model \( \dot{x} = v, \ \dot{v} = u, \ |u| \le u_{\max} \), starting from \( x(0) = 0, v(0) = 0 \), characterize the set of reachable states \( (x(T), v(T)) \) at a fixed time \( T > 0 \).

Solution:

For a constant control \( u(t) = u_0 \) with \( |u_0| \le u_{\max} \), integrating gives

\[ v(T) = u_0 T, \quad x(T) = \tfrac{1}{2} u_0 T^2. \]

Thus under constant controls the terminal state lies on the parabola \( x = \tfrac{1}{2} T v \) with \( |v| \le u_{\max} T \). Allowing time-varying controls \( u(t) \) with \( |u(t)| \le u_{\max} \), we can write

\[ \begin{aligned} v(T) &= \int_0^T u(s)\, ds, \\ x(T) &= \int_0^T (T - s) u(s)\, ds. \end{aligned} \]

The mapping from \( u(\cdot) \) to \( (x(T),v(T)) \) is linear, and the constraint \( |u(s)| \le u_{\max} \) for all \( s \) describes an infinite-dimensional hypercube. The reachable set is therefore the image of that hypercube under a linear map; hence it is a convex, centrally symmetric subset of \( \mathbb{R}^2 \). The extreme points occur at bang-bang controls \( u(s) = \pm u_{\max} \), so the reachable set is the convex hull of the endpoints corresponding to bang-bang profiles. A more detailed geometric analysis shows that the reachable set is a lens-shaped convex region bounded by two parabolic arcs with slopes proportional to \( u_{\max} \).

Problem 3 (Error Bound for Explicit Euler): Let \( \dot{\mathbf{x}} = f(\mathbf{x}) \) with \( f \) globally Lipschitz, \( \|f(\mathbf{x}_1) - f(\mathbf{x}_2)\| \le L \|\mathbf{x}_1 - \mathbf{x}_2\| \). Suppose we approximate on \( [0,T] \) using explicit Euler with step \( \Delta t \). Show that there exists a constant \( C \) such that \[ \|\mathbf{x}(t_k) - \mathbf{x}_k\| \le C \Delta t (e^{L t_k} - 1). \]

Solution (sketch):

Let \( \mathbf{e}_k = \mathbf{x}(t_k) - \mathbf{x}_k \) and \( t_k = k \Delta t \). A standard local truncation error analysis gives \( \|\mathbf{x}(t_{k+1}) - \mathbf{x}(t_k) - \Delta t f(\mathbf{x}(t_k))\| \le K \Delta t^2 \) for some constant \( K \). Then

\[ \begin{aligned} \mathbf{e}_{k+1} &= \mathbf{x}(t_{k+1}) - \mathbf{x}_{k+1} \\ &= \mathbf{x}(t_k) + \Delta t f(\mathbf{x}(t_k)) + \boldsymbol{\tau}_k - \bigl(\mathbf{x}_k + \Delta t f(\mathbf{x}_k)\bigr), \end{aligned} \]

where \( \|\boldsymbol{\tau}_k\| \le K \Delta t^2 \). Using the Lipschitz property,

\[ \|\mathbf{e}_{k+1}\| \le (1 + L \Delta t) \|\mathbf{e}_k\| + K \Delta t^2. \]

Iterating this recursion and applying the discrete Grönwall inequality, one obtains \( \|\mathbf{e}_k\| \le C \Delta t (e^{L t_k} - 1) \) for a constant \( C \) depending on \( K \) and \( L \), as claimed.

Problem 4 (Underactuation and Accessible Accelerations): Consider the manipulator dynamics \( \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q}) = \mathbf{B}(\mathbf{q}) \mathbf{u} \) with \( \mathbf{B}(\mathbf{q}) \in \mathbb{R}^{n \times p} \), \( p \lt n \). Assume \( \mathbf{M}(\mathbf{q}) \) is invertible. Show that for any fixed \( (\mathbf{q},\dot{\mathbf{q}}) \), the set of achievable accelerations \( \ddot{\mathbf{q}} \) at that state is an affine subspace of dimension at most \( p \).

Solution:

For a fixed \( (\mathbf{q},\dot{\mathbf{q}}) \), rearrange the dynamics:

\[ \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} \bigl( \mathbf{B}(\mathbf{q}) \mathbf{u} - \mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} - \mathbf{g}(\mathbf{q}) \bigr). \]

The map \( \mathbf{u} \mapsto \ddot{\mathbf{q}} \) is affine: \( \ddot{\mathbf{q}} = \mathbf{M}(\mathbf{q})^{-1} \mathbf{B}(\mathbf{q}) \mathbf{u} + \mathbf{b} \), with \( \mathbf{b} = -\mathbf{M}(\mathbf{q})^{-1} (\mathbf{C}(\mathbf{q},\dot{\mathbf{q}})\dot{\mathbf{q}} + \mathbf{g}(\mathbf{q})) \) independent of \( \mathbf{u} \). The linear part has image contained in \( \mathcal{S} = \mathbf{M}(\mathbf{q})^{-1}\text{Im}(\mathbf{B}(\mathbf{q})) \), which is a subspace of dimension at most \( \text{rank}(\mathbf{B}(\mathbf{q})) \le p \). Therefore the set of achievable accelerations is an affine subspace \( \mathbf{b} + \mathcal{S} \subset \mathbb{R}^n \) of dimension at most \( p \). This shows explicitly how underactuation restricts instantaneous motion directions, contributing to the difficulty of kinodynamic planning.

In this lesson we formalized the kinodynamic planning problem and analyzed why it is substantially harder than geometric configuration-space planning. The key reasons are:

• State spaces are higher-dimensional and often include velocities, leading to exponential growth in sampling complexity.
• Differential, nonholonomic, and underactuation constraints restrict reachable motions in subtle, highly nonlinear ways.
• The search variable is an entire control function, making the problem infinite-dimensional; any finite parameterization trades accuracy against tractability.
• Numerical integration and collision checking introduce discretization error and further complicate guarantees.
• Complexity-theoretic results show that kinodynamic planning is at least as hard as geometric planning and often NP/PSPACE-hard in the worst case.

Subsequent lessons in this chapter will introduce state lattices, motion primitives, and kinodynamic sampling-based planners that cope with these difficulties by carefully structuring the search space and exploiting dynamic feasibility at the local level.