In purely geometric planning (earlier chapters), a path is feasible if it lies in the obstacle-free configuration space. Kinodynamic planning additionally requires that paths respect the robot dynamics and actuator limits. Let \( x(t)\in \mathbb{R}^n \) denote the state and \( u(t)\in \mathbb{R}^m \) the control input. A general control-affine model is

\[ \dot{x}(t) = f(x(t)) + \sum_{i=1}^m g_i(x(t))\,u_i(t), \quad x(0)=x_0. \]

We also have state constraints \( x(t)\in X_{\text{free}} \subset \mathbb{R}^n \), and control constraints \( u(t)\in U_{\text{adm}} \subset \mathbb{R}^m \). A feasible kinodynamic trajectory is a triple \( (x(\cdot),u(\cdot),T) \) such that

\[ \begin{aligned} \dot{x}(t) &= f(x(t)) + \sum_{i=1}^m g_i(x(t))\,u_i(t), \\ x(0) &= x_0, \quad x(T)\in X_{\text{goal}}, \\ x(t) &\in X_{\text{free}},\quad u(t)\in U_{\text{adm}}, \quad \forall t\in[0,T]. \end{aligned} \]

The additional requirements on derivatives and inputs are called differential constraints. They can be written in implicit form

\[ c(x(t),\dot{x}(t),u(t)) = 0,\quad h(x(t),\dot{x}(t),u(t)) \le 0, \]

where \( c \) encodes equalities (e.g., nonholonomic constraints) and \( h \) encodes inequalities (e.g., velocity, acceleration, or torque bounds).

flowchart TD
  G["Geometric path in C-space"] --> DYN["Add dynamics xdot = f(x,u)"]
  DYN --> CONS["Enforce state/control limits"]
  CONS --> FEAS["Feasible trajectory (x(t), u(t))"]
  CONS --> INFEAS["Infeasible: violates differential constraints"]
        

In kinodynamic planners (e.g., kinodynamic RRT* in Lesson 3), sampling happens in state space but edges of the graph or tree must correspond to feasible trajectories for the given differential constraints.

To even define feasibility, we need existence and uniqueness of solutions to the differential equations. A standard sufficient condition is a global Lipschitz bound on \( f \) and \( g_i \) in \( x \). Suppose there exists \( L > 0 \) such that

\[ \big\| f(x_1) - f(x_2) \big\| \le L \|x_1 - x_2\|, \quad \big\| g_i(x_1) - g_i(x_2) \big\| \le L \|x_1 - x_2\|, \]

for all \( x_1,x_2 \) and all \( i \), and control inputs are measurable and bounded, \( \|u(t)\|\le M \). Then by the Picard–Lindelöf theorem:

• For any initial state \( x_0 \) and input \( u(\cdot) \), there exists a unique solution \( x(\cdot) \) at least on some time interval.
• Feasibility becomes a question of whether the solution remains inside \( X_{\text{free}} \) and respects input bounds for the entire horizon.

Define the feasible set from \( x_0 \):

\[ \mathcal{R}(x_0) = \Big\{ x(T) \,\Big|\, \exists T > 0,\ \exists u(\cdot)\in \mathcal{U}_{\text{adm}} : x(\cdot) \text{ satisfies dynamics and constraints} \Big\}. \]

A goal region \( X_{\text{goal}} \) is reachable from \( x_0 \) iff \( X_{\text{goal}}\cap \mathcal{R}(x_0)\neq \emptyset \). Much of kinodynamic planning can be seen as sampling-based approximations of \( \mathcal{R}(x_0) \).

Let \( q\in\mathbb{R}^n \) denote the configuration of a robot (without velocities), and \( \dot{q} \) its time derivative. Holonomic constraints limit configurations by equalities \( \Phi(q)=0 \). In contrast, many mobile robots are subject to nonholonomic constraints that restrict velocities but not the reachable set of positions directly.

A common representation is a Pfaffian constraint

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

where each row of \( A(q) \) encodes an instantaneous velocity constraint. The constraint is holonomic if there exists a function \( \Phi(q) \) such that \( A(q) = \nabla\Phi(q) \); otherwise it is nonholonomic.

Example: a unicycle or car-like robot with configuration \( q = (x,y,\theta) \) and controls \( u=(v,\omega) \):

\[ \dot{x} = v\cos\theta,\quad \dot{y} = v\sin\theta,\quad \dot{\theta} = \omega. \]

The no-side-slip constraint can be written as

\[ -\sin\theta\,\dot{x} + \cos\theta\,\dot{y} = 0, \]

which is nonholonomic: there is no scalar function \( \Phi(x,y,\theta) \) whose gradient is \( (-\sin\theta,\cos\theta,0) \). A geometric planner that proposes a path requiring instantaneous sideways motion of the car (violating this constraint) cannot be time-parameterized into a feasible trajectory.

Define the allowable velocity distribution

\[ \Delta(q) = \big\{ \dot{q}\in\mathbb{R}^n \,\big|\, A(q)\,\dot{q}=0 \big\}. \]

For driftless control-affine systems \( \dot{q} = \sum_{i=1}^m g_i(q)\,u_i \), \( \Delta(q) = \text{span}\{g_1(q),\dots,g_m(q)\} \). Feasible velocities must belong to \( \Delta(q) \) at every time.

A manipulator with configuration \( q\in\mathbb{R}^n \) and joint torques \( \tau\in\mathbb{R}^m \) has dynamics

\[ M(q)\,\ddot{q} + C(q,\dot{q})\,\dot{q} + g(q) = B(q)\,\tau. \]

If \( B(q) \) is square and invertible, the system is fully actuated: any sufficiently smooth \( q(t) \) can be tracked (respecting torque bounds) by suitable \( \tau(t) \). If \( \operatorname{rank} B(q) < n \), the system is underactuated, and not every geometric path is dynamically realizable.

For driftless underactuated systems \( \dot{x} = \sum_{i=1}^m g_i(x)\,u_i \), controllability is linked to Lie brackets. The Lie bracket of two vector fields \( g_i,g_j \) is

\[ [g_i,g_j](x) = \frac{\partial g_j}{\partial x}(x)\,g_i(x) - \frac{\partial g_i}{\partial x}(x)\,g_j(x), \]

which captures the second-order motion generated by alternating the controls \( u_i \) and \( u_j \). Let

\[ \mathcal{L}(g_1,\dots,g_m)(x) = \text{span}\big\{ g_i(x),\ [g_i,g_j](x),\ [g_i,[g_j,g_k]](x),\dots \big\}. \]

The Lie algebra rank condition (LARC) states that if \( \mathcal{L}(g_1,\dots,g_m)(x) = \mathbb{R}^n \) for all \( x \), the system is small-time locally controllable around those states. This is a sufficient condition for the existence of feasible trajectories between nearby states.

For the unicycle, we can set

\[ g_1(x,y,\theta) = \begin{bmatrix}\cos\theta \\ \sin\theta \\ 0\end{bmatrix},\quad g_2(x,y,\theta) = \begin{bmatrix}0 \\ 0 \\ 1\end{bmatrix}, \]

and compute

\[ [g_1,g_2](x,y,\theta) = \begin{bmatrix}\sin\theta \\ -\cos\theta \\ 0\end{bmatrix}. \]

The three vectors \( g_1,g_2,[g_1,g_2] \) are linearly independent for all \( \theta \), so \( \mathcal{L}(g_1,g_2)(x) = \mathbb{R}^3 \) and the unicycle is small-time locally controllable. Underactuation does not imply infeasibility, but it shapes which motions are reachable and how planners should generate motion primitives.

Many planners first compute a geometric path in configuration space and then try to time-parameterize it to respect dynamics. Let \( q:[0,1]\to\mathbb{R}^n \) be a differentiable geometric path. A time-parameterization is a strictly increasing map \( s:[0,T]\to[0,1] \) with \( s(0)=0 \) and \( s(T)=1 \). The realized motion is \( \tilde{q}(t)=q(s(t)) \) with

\[ \dot{\tilde{q}}(t) = q'(s(t))\,\dot{s}(t),\quad \ddot{\tilde{q}}(t) = q''(s(t))\,\dot{s}(t)^2 + q'(s(t))\,\ddot{s}(t). \]

For driftless systems \( \dot{q} = \sum_{i=1}^m g_i(q)\,u_i \), we obtain a necessary condition for path feasibility:

\[ q'(s) \in \Delta(q(s)) \quad \text{for all } s\in[0,1],\quad \Delta(q) = \text{span}\{g_1(q),\dots,g_m(q)\}. \]

Proof (sketch). Suppose there exists a feasible trajectory following the path: \( \tilde{q}(t)=q(s(t)) \) for some monotone \( s(t) \). Feasibility implies \( \dot{\tilde{q}}(t)\in \Delta(\tilde{q}(t)) \) for all \( t \), hence

\[ q'(s(t))\,\dot{s}(t) \in \Delta(q(s(t))). \]

Since \( \dot{s}(t) > 0 \), we conclude \( q'(s(t))\in \Delta(q(s(t))) \) for all \( t \), which implies the condition in \( s \)-space. If there exists some \( s^\star \) such that \( q'(s^\star) \notin \Delta(q(s^\star)) \), then no feasible time-parameterization exists. □

For fully actuated manipulators, feasibility often reduces to satisfying inequality bounds (e.g., on velocity, acceleration, or torque). For example, with a maximum joint speed \( \|\dot{q}\|\le v_{\max} \), we must enforce

\[ \|q'(s)\|\,\dot{s}(t) \le v_{\max} \quad \Rightarrow \quad \dot{s}(t) \le \frac{v_{\max}}{\|q'(s(t))\|}. \]

Time-scaling methods (e.g., due to Bobrow and co-authors) treat \( \dot{s} \) as a new control and integrate feasible \( \dot{s}(t) \) and \( \ddot{s}(t) \) subject to such bounds along the path. Feasibility then becomes a 1D constrained integration problem.

Sampling-based kinodynamic planners use numerical integration to propagate dynamics over finite steps. With step size \( h > 0 \), explicit Euler integration gives

\[ x_{k+1} = x_k + h\,f(x_k,u_k) + \mathcal{O}(h^2). \]

A planner typically decides feasibility on the basis of the discrete trajectory sequence \( \{x_k\} \) and the associated controls \( \{u_k\} \), checking:

• State constraints: \( x_k\in X_{\text{free}} \).
• Control constraints: \( u_k\in U_{\text{adm}} \).
• Differential constraints: e.g., nonholonomic constraints \( A(x_k)\,f(x_k,u_k)=0 \) or bounded numerical violation.

The discretization error \( \mathcal{O}(h^2) \) implies that the continuous trajectory can slightly deviate from the discrete points. In practice, planners use a safety margin: they require \( x_k \) to be at some distance from obstacles and enforce stricter bounds on \( u_k \) so that the continuous solution remains feasible.

flowchart TD
  S["Sample state x_rand"] --> N["Nearest state x_near"]
  N --> U["Sample control u and step size h"]
  U --> INT["Integrate xdot = f(x,u) for horizon"]
  INT --> C["Check constraints along discrete states"]
  C -->|ok| ADD["Add new state/edge to tree"]
  C -->|violated| REJ["Reject and resample control"]
        

We now implement a simple feasibility checker for a unicycle model in Python. We consider the state \( x=(x,y,\theta) \) and controls \( u=(v,\omega) \) with bounds \( |v|\le v_{\max} \), \( |\omega|\le \omega_{\max} \).

import math
from typing import List, Tuple

State = Tuple[float, float, float]   # (x, y, theta)
Control = Tuple[float, float]        # (v, omega)

def unicycle_dynamics(x: State, u: Control) -> State:
    """Continuous-time dynamics xdot = f(x,u)."""
    px, py, th = x
    v, w = u
    return (v * math.cos(th),  # xdot
            v * math.sin(th),  # ydot
            w)                 # thetadot

def step_euler(x: State, u: Control, dt: float) -> State:
    """Explicit Euler integration for one step."""
    dx, dy, dth = unicycle_dynamics(x, u)
    return (x[0] + dt * dx,
            x[1] + dt * dy,
            x[2] + dt * dth)

def within_bounds(u: Control,
                  v_max: float,
                  w_max: float) -> bool:
    v, w = u
    if abs(v) > v_max:
        return False
    if abs(w) > w_max:
        return False
    return True

def is_state_valid(x: State) -> bool:
    """
    Placeholder for collision checking and other state constraints.
    Replace with calls to a collision checker using the robot geometry
    and environment representation.
    """
    # Example: keep robot inside a square workspace
    px, py, _ = x
    if abs(px) > 5.0 or abs(py) > 5.0:
        return False
    return True

def simulate_trajectory(x0: State,
                        controls: List[Control],
                        dt: float,
                        v_max: float,
                        w_max: float) -> Tuple[bool, List[State]]:
    """
    Simulate a sequence of piecewise-constant controls and check feasibility.
    Returns (is_feasible, trajectory).
    """
    traj = [x0]
    x = x0
    for u in controls:
        if not within_bounds(u, v_max, w_max):
            return False, traj
        x_next = step_euler(x, u, dt)
        if not is_state_valid(x_next):
            return False, traj
        traj.append(x_next)
        x = x_next
    return True, traj

if __name__ == "__main__":
    x0 = (0.0, 0.0, 0.0)
    dt = 0.1
    v_max = 1.0
    w_max = 2.0

    # Simple "arc" trajectory: forward with constant v and omega
    controls = [(0.8, 1.0) for _ in range(50)]
    feasible, traj = simulate_trajectory(x0, controls, dt, v_max, w_max)

    print("Feasible:", feasible)
    print("Final state:", traj[-1])
      

In a kinodynamic RRT, the function simulate_trajectory would be used as a local propagator to generate candidate edges, and is_state_valid would wrap a collision checker implemented using a robotics library (e.g., fcl bindings or pybullet in Python).

The same feasibility logic can be implemented efficiently in C++, suitable for integration into real-time planners or simulators. Below we use a minimal header-only style; in practice, one would combine this with a collision library and linear algebra (e.g., Eigen).

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

struct State {
    double x;
    double y;
    double theta;
};

struct Control {
    double v;
    double w;
};

State dynamics(const State& s, const Control& u) {
    State dx;
    dx.x = u.v * std::cos(s.theta);
    dx.y = u.v * std::sin(s.theta);
    dx.theta = u.w;
    return dx;
}

State eulerStep(const State& s, const Control& u, double dt) {
    State dx = dynamics(s, u);
    State sn;
    sn.x = s.x + dt * dx.x;
    sn.y = s.y + dt * dx.y;
    sn.theta = s.theta + dt * dx.theta;
    return sn;
}

bool withinBounds(const Control& u,
                  double v_max,
                  double w_max) {
    if (std::fabs(u.v) > v_max) return false;
    if (std::fabs(u.w) > w_max) return false;
    return true;
}

bool isStateValid(const State& s) {
    // Example: bounded workspace
    if (std::fabs(s.x) > 5.0) return false;
    if (std::fabs(s.y) > 5.0) return false;
    return true;
}

bool simulateTrajectory(const State& x0,
                        const std::vector<Control>& controls,
                        double dt,
                        double v_max,
                        double w_max,
                        std::vector<State>& out_traj) {
    out_traj.clear();
    out_traj.push_back(x0);
    State x = x0;
    for (const auto& u : controls) {
        if (!withinBounds(u, v_max, w_max)) {
            return false;
        }
        State xn = eulerStep(x, u, dt);
        if (!isStateValid(xn)) {
            return false;
        }
        out_traj.push_back(xn);
        x = xn;
    }
    return true;
}

int main() {
    State x0{0.0, 0.0, 0.0};
    double dt = 0.1;
    double v_max = 1.0;
    double w_max = 2.0;

    std::vector<Control> controls(50, Control{0.8, 1.0});
    std::vector<State> traj;

    bool feasible = simulateTrajectory(x0, controls, dt, v_max, w_max, traj);
    std::cout << "Feasible: " << feasible << std::endl;
    if (feasible) {
        const State& xf = traj.back();
        std::cout << "Final state: "
                  << xf.x << " "
                  << xf.y << " "
                  << xf.theta << std::endl;
    }
    return 0;
}
      

This structure mirrors typical kinodynamic planning implementations: dynamics and eulerStep form the local integrator, while withinBounds and isStateValid enforce feasibility.

Java-based robotics stacks (or Android-based robots) can use similar abstractions. Below is a minimal implementation of a unicycle feasibility checker.

public class UnicycleKinodynamics {

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

    public static class Control {
        public double v, w;
        public Control(double v, double w) {
            this.v = v;
            this.w = w;
        }
    }

    public static State dynamics(State s, Control u) {
        double dx = u.v * Math.cos(s.theta);
        double dy = u.v * Math.sin(s.theta);
        double dth = u.w;
        return new State(dx, dy, dth);
    }

    public static State eulerStep(State s, Control u, double dt) {
        State dx = dynamics(s, u);
        return new State(
            s.x + dt * dx.x,
            s.y + dt * dx.y,
            s.theta + dt * dx.theta
        );
    }

    public static boolean withinBounds(Control u,
                                       double vMax,
                                       double wMax) {
        if (Math.abs(u.v) > vMax) return false;
        if (Math.abs(u.w) > wMax) return false;
        return true;
    }

    public static boolean isStateValid(State s) {
        // Example: bounded workspace
        if (Math.abs(s.x) > 5.0) return false;
        if (Math.abs(s.y) > 5.0) return false;
        return true;
    }

    public static boolean simulateTrajectory(
            State x0,
            java.util.List<Control> controls,
            double dt,
            double vMax,
            double wMax,
            java.util.List<State> outTraj) {

        outTraj.clear();
        outTraj.add(x0);
        State x = x0;

        for (Control u : controls) {
            if (!withinBounds(u, vMax, wMax)) {
                return false;
            }
            State xn = eulerStep(x, u, dt);
            if (!isStateValid(xn)) {
                return false;
            }
            outTraj.add(xn);
            x = xn;
        }
        return true;
    }

    public static void main(String[] args) {
        State x0 = new State(0.0, 0.0, 0.0);
        double dt = 0.1;
        double vMax = 1.0, wMax = 2.0;

        java.util.List<Control> controls =
            new java.util.ArrayList<>();
        for (int k = 0; k < 50; ++k) {
            controls.add(new Control(0.8, 1.0));
        }

        java.util.List<State> traj =
            new java.util.ArrayList<>();
        boolean feasible = simulateTrajectory(
            x0, controls, dt, vMax, wMax, traj
        );
        System.out.println("Feasible: " + feasible);
    }
}
      

This pattern can be adapted to more complex underactuated systems by replacing the unicycle dynamics with appropriate dynamic models, and by integrating with Java bindings to robotics libraries or middleware.

MATLAB is frequently used in robotics to prototype motion planners and then export logic to Simulink for block-diagram simulation and code generation. Below is a MATLAB script that checks feasibility of a unicycle trajectory using ode45.

function [feasible, T, X] = check_unicycle_feasibility(x0, v_fun, w_fun, ...
                                                       tspan, v_max, w_max)
% x0    : initial state [x0; y0; theta0]
% v_fun : handle @(t) v(t)
% w_fun : handle @(t) w(t)
% tspan : [t0 tf]
% v_max, w_max : bounds

    function dx = dyn(t, x)
        v = v_fun(t);
        w = w_fun(t);
        if abs(v) > v_max || abs(w) > w_max
            % Hard constraint violation: encode NaNs
            dx = [NaN; NaN; NaN];
            return;
        end
        dx = [v * cos(x(3));
              v * sin(x(3));
              w];
    end

    opts = odeset('RelTol',1e-6,'AbsTol',1e-8);
    [T, X] = ode45(@dyn, tspan, x0, opts);

    if any(any(isnan(X)))
        feasible = false;
        return;
    end

    % Example workspace constraint
    if any(abs(X(:,1)) > 5.0) || any(abs(X(:,2)) > 5.0)
        feasible = false;
        return;
    end

    feasible = true;
end
      

In Simulink, one would encapsulate dyn into a MATLAB Function block, connect it to integrator blocks, and add assertion blocks for the constraints.

Mathematica (Wolfram Language) provides symbolic and numeric tools for such feasibility checks. The following snippet uses NDSolve to integrate a control schedule.

(* Unicycle dynamics and feasibility check in Mathematica *)

v[t_] := 0.8;
w[t_] := 1.0;
vMax = 1.0;
wMax = 2.0;

dynEqns = {
  x'[t] == v[t] Cos[theta[t]],
  y'[t] == v[t] Sin[theta[t]],
  theta'[t] == w[t],
  x[0] == 0,
  y[0] == 0,
  theta[0] == 0
};

sol = NDSolve[dynEqns, {x, y, theta}, {t, 0, 5}][[1]];

(* Sample trajectory and check bounds *)
pts = Table[
   {tVal, x[tVal] /. sol, y[tVal] /. sol, theta[tVal] /. sol},
   {tVal, 0, 5, 0.1}
];

feasibleBoundsQ =
  And @@ (Abs[#2] <= 5.0 && Abs[#3] <= 5.0 & @@@ pts);

Print["Feasible workspace bounds: ", feasibleBoundsQ];
      

Mathematica also enables symbolic manipulation of constraints (e.g., verifying nonholonomic Pfaffian forms, computing Lie brackets symbolically), which is useful for theoretical analysis of feasibility conditions.

Problem 1 (Tangent Condition for Driftless Feasibility). Consider a driftless system \( \dot{q} = \sum_{i=1}^m g_i(q)\,u_i \) in \( \mathbb{R}^n \) with distribution \( \Delta(q) = \text{span}\{g_1(q),\dots,g_m(q)\} \). Let \( q:[0,1]\to\mathbb{R}^n \) be a differentiable path. Prove that if there exists a feasible time-parameterized trajectory following this path, then \( q'(s)\in\Delta(q(s)) \) for all \( s\in[0,1] \).

Solution. A feasible trajectory following the path can be written as \( \tilde{q}(t)=q(s(t)) \) for some strictly increasing \( s:[0,T]\to[0,1] \). Differentiating gives

\[ \dot{\tilde{q}}(t) = q'(s(t))\,\dot{s}(t). \]

Feasibility implies there exist inputs \( u_i(t) \) such that

\[ q'(s(t))\,\dot{s}(t) = \sum_{i=1}^m g_i(q(s(t)))\,u_i(t) \in \Delta(q(s(t))). \]

Since \( \dot{s}(t) > 0 \) for all \( t \), we can divide by \( \dot{s}(t) \), yielding \( q'(s(t))\in \Delta(q(s(t))) \) for all \( t \), hence for all \( s \). Therefore, a necessary condition for feasibility is that the path tangent always lies in the allowed velocity distribution. □

Problem 2 (Infeasible Lateral Path for a Car). A car-like robot is modeled as a unicycle with configuration \( q=(x,y,\theta) \) and no-side-slip constraint as in Section 3. Consider the geometric path \( q(s) = (0,s,\pi/2) \) for \( s\in[0,1] \). Show that this path is not feasible.

Solution. We have \( q'(s) = (0,1,0) \). For the unicycle, the distribution is \( \Delta(q)=\text{span}\{g_1,g_2\} \) with

\[ g_1 = (\cos\theta,\sin\theta,0)^\top,\quad g_2 = (0,0,1)^\top. \]

At \( \theta=\pi/2 \), \( g_1=(0,1,0)^\top \) and \( g_2=(0,0,1)^\top \). Then \( q'(s)=(0,1,0)^\top \) is in the span of \( g_1 \), so the instantaneous tangent condition is satisfied. However, the entire path keeps \( \theta=\pi/2 \); that is, the car remains always pointing along the positive \( y \) axis. The path moves strictly forward along the current heading, which is allowed by the dynamics. This path is actually feasible and can be realized with constant \( v>0,\omega=0 \). A truly infeasible path would require motion orthogonal to the heading, such as \( q(s)=(s,0,\pi/2) \), whose tangent \( (1,0,0)^\top \) is never in \( \Delta(q) \) at \( \theta=\pi/2 \). □

Problem 3 (Lie Brackets for the Unicycle). For the unicycle vector fields \( g_1,g_2 \) given in Section 4, compute \( [g_1,g_2] \) and show that \( \mathcal{L}(g_1,g_2)(x)=\mathbb{R}^3 \).

Solution. We have \( g_1(x,y,\theta)=(\cos\theta,\sin\theta,0)^\top \) and \( g_2(x,y,\theta)=(0,0,1)^\top \). Their Jacobians are

\[ \frac{\partial g_1}{\partial x} = 0,\quad \frac{\partial g_1}{\partial y} = 0,\quad \frac{\partial g_1}{\partial \theta} = (-\sin\theta,\cos\theta,0)^\top,\quad \frac{\partial g_2}{\partial x} = \frac{\partial g_2}{\partial y} = \frac{\partial g_2}{\partial \theta} = 0. \]

Using the definition \( [g_1,g_2] = \tfrac{\partial g_2}{\partial x}g_1 - \tfrac{\partial g_1}{\partial x}g_2 \), we obtain

\[ [g_1,g_2](x,y,\theta) = -\frac{\partial g_1}{\partial \theta} \cdot 1 = (\sin\theta,-\cos\theta,0)^\top. \]

At any \( \theta \), the three vectors \( g_1=(\cos\theta,\sin\theta,0)^\top \), \( g_2=(0,0,1)^\top \), and \( [g_1,g_2]=(\sin\theta,-\cos\theta,0)^\top \) are linearly independent. Hence they span \( \mathbb{R}^3 \), and the Lie algebra rank condition holds. The unicycle is therefore small-time locally controllable. □

Problem 4 (Time-Scaling with Velocity Bounds). Let \( q:[0,1]\to\mathbb{R}^n \) be a path with \( q'(s)\neq 0 \) for all \( s \). Suppose we require \( \|\dot{q}(t)\|\le v_{\max} \). Show that any time-parameterization \( s(t) \) must satisfy \( \dot{s}(t)\le v_{\max}/\|q'(s(t))\| \). Then, argue why integrating the scalar inequality \( 0\le \dot{s}(t)\le v_{\max}/\|q'(s(t))\| \) is sufficient to guarantee kinematic feasibility with respect to this bound.

Solution. We have \( \dot{q}(t)=q'(s(t))\dot{s}(t) \), so \( \|\dot{q}(t)\| = \|q'(s(t))\|\dot{s}(t) \) because \( \dot{s}(t)\ge 0 \). The bound \( \|\dot{q}(t)\|\le v_{\max} \) implies

\[ \|q'(s(t))\|\,\dot{s}(t) \le v_{\max} \quad \Rightarrow \quad \dot{s}(t)\le \frac{v_{\max}}{\|q'(s(t))\|}. \]

Conversely, if we choose any measurable \( \dot{s}(t) \) satisfying \( 0\le \dot{s}(t)\le v_{\max}/\|q'(s(t))\| \) for all \( t \) and integrate to obtain \( s(t) \), then the resulting motion automatically respects the velocity bound. Thus the time-scaling problem reduces to a 1D inequality-constrained integration along the geometric path. □

Problem 5 (Euler Discretization Error). Consider the explicit Euler scheme \( x_{k+1}=x_k+h\,f(x_k,u_k) \) for a system \( \dot{x}=f(x,u) \) with \( f \) Lipschitz in \( x \). Show that, under standard assumptions, the global discretization error after \( N \) steps with \( Nh = T \) satisfies \( \|x(T)-x_N\|\le C h \) for some constant \( C \) independent of \( h \), and explain the implication for feasibility checking in planners.

Solution. Classical numerical analysis results for explicit Euler guarantee that the local truncation error per step is \( \mathcal{O}(h^2) \). Under Lipschitz and boundedness assumptions, Grönwall-type inequalities yield an \( \mathcal{O}(h) \) global error:

\[ \max_{0\le k\le N}\big\|x(kh) - x_k\big\| \le C h \]

for some constant \( C \) depending on the Lipschitz constant of \( f \) but not on \( h \). For planners, this means that if the discrete states \( x_k \) remain at least a distance \( \delta \) from obstacles and constraint boundaries, choosing \( h \) small enough so that \( C h < \delta \) ensures that the continuous trajectory is also feasible. Hence, feasibility in discrete-time becomes a conservative approximation of continuous feasibility. □

In this lesson we formalized differential constraints as restrictions on \( (x,\dot{x},u) \) and embedded them into the kinodynamic planning framework. We distinguished holonomic from nonholonomic constraints using Pfaffian forms and Lie-bracket controllability, showed how underactuation shapes the reachable set, and derived tangent and time-scaling conditions for feasibility of geometric paths. We also connected continuous-time dynamics with discrete-time approximations used in sampling-based planners, and implemented feasibility checks for a unicycle model in Python, C++, Java, MATLAB/Simulink, and Mathematica. These concepts will be crucial in the next lesson, where we explicitly impose velocity/acceleration limits in planning algorithms.