In earlier chapters, sampling-based planners such as PRM and RRT operated in configuration space \( \mathcal{C} \), connecting samples with straight-line interpolation (in joint space) and checking only geometric collision constraints. In contrast, kinodynamic planning must respect dynamics:

\[ \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(t) \in \mathcal{X} \subset \mathbb{R}^n,\; \mathbf{u}(t) \in \mathcal{U} \subset \mathbb{R}^m, \]

where the state typically includes configuration and velocity, \( \mathbf{x} = (\mathbf{q}, \dot{\mathbf{q}}) \), and the control is a torque, force, or velocity command. A kinodynamic planning problem specifies initial state \( \mathbf{x}_{\mathrm{init}} \), goal region \( \mathcal{X}_{\mathrm{goal}} \subset \mathcal{X} \), and a cost functional:

\[ J(\mathbf{u}(\cdot)) = \int_0^{T} \ell(\mathbf{x}(t), \mathbf{u}(t)) \, \mathrm{d}t \quad \text{subject to } \dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u}), \; \mathbf{x}(t) \in \mathcal{X}_{\mathrm{free}}, \; \mathbf{x}(T) \in \mathcal{X}_{\mathrm{goal}}. \]

Kinodynamic RRT* constructs a tree in state space \( \mathcal{X} \), such that each edge is a dynamically feasible trajectory generated by forward simulation under admissible controls. The key idea: sample states and connect them by integrating the dynamics, then rewire the tree as in RRT* to improve cost-to-come.

We formalize the kinodynamic planning problem as an infinite-dimensional optimal control problem. Let \( \mathcal{X}_{\mathrm{free}} \subset \mathcal{X} \) denote collision-free states and \( \mathcal{X}_{\mathrm{obs}} \) the obstacle states. Dynamics are:

\[ \dot{\mathbf{x}}(t) = f(\mathbf{x}(t), \mathbf{u}(t)), \quad \mathbf{x}(0) = \mathbf{x}_{\mathrm{init}}, \quad \mathbf{x}(t) \in \mathcal{X}_{\mathrm{free}} \;\forall t \in [0,T]. \]

For RRT*, the cost is typically additive in time (e.g. minimum time or minimum control effort):

\[ J(\sigma) = \int_0^{T} g(\mathbf{x}(t), \mathbf{u}(t)) \, \mathrm{d}t, \]

where \( \sigma \) denotes the trajectory and \( g \) is nonnegative and locally bounded (to ensure integrability). A kinodynamic RRT* constructs a sequence of trees \( \{\mathcal{T}_n\} \) with root at \( \mathbf{x}_{\mathrm{init}} \). Each vertex is a state \( \mathbf{x}_i \), each directed edge corresponds to a feasible trajectory segment \( \sigma_{ij} \) with cost \( c(\sigma_{ij}) \). The cost-to-come of a node \( \mathbf{x}_i \) is:

\[ J_n(\mathbf{x}_i) = \sum_{(k,\ell) \in \mathrm{path}(\mathbf{x}_{\mathrm{init}} \rightsquigarrow \mathbf{x}_i)} c(\sigma_{k\ell}). \]

The planner aims to approximate the optimal cost:

\[ J^{\star} = \inf \left\{ J(\sigma) \;\middle|\; \sigma \text{ is feasible and } \sigma(T) \in \mathcal{X}_{\mathrm{goal}} \right\}. \]

As in geometric RRT*, asymptotic optimality means that the best solution in the tree converges to \( J^{\star} \) with probability one:

\[ \lim_{n \to \infty} \left( \min_{\mathbf{x}_i \in \mathcal{T}_n \cap \mathcal{X}_{\mathrm{goal}}} J_n(\mathbf{x}_i) \right) = J^{\star} \quad \text{with probability one}. \]

Kinodynamic RRT* keeps the RRT* rewiring mechanism but changes the local steering operation: instead of a straight line segment, we integrate the dynamics with a (possibly discrete) control. A simplified discrete-time approximation with step \( \Delta t > 0 \) is:

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

We denote this numerical propagation operator by \( \Phi(\mathbf{x},\mathbf{u}) \), which returns the final state and the integrated cost for applying a piecewise-constant control sequence \( \mathbf{u}_{0:N-1} \).

High-level algorithm (simplified):

1. Sample \( \mathbf{x}_{\mathrm{rand}} \sim \mathcal{U}(\mathcal{X}_{\mathrm{free}}) \).
2. Find nearest node: \( \mathbf{x}_{\mathrm{near}} = \arg\min_{\mathbf{x}_i \in \mathcal{T}} d(\mathbf{x}_i, \mathbf{x}_{\mathrm{rand}}) \) for a suitable metric \( d \).
3. Select a control sequence \( \mathbf{u}_{0:N-1} \) (from a discrete control set or via steering) and propagate: \( (\mathbf{x}_{\mathrm{new}}, c_{\mathrm{edge}}) = \Phi(\mathbf{x}_{\mathrm{near}},\mathbf{u}_{0:N-1}) \).
4. If the trajectory stays in \( \mathcal{X}_{\mathrm{free}} \), add \( \mathbf{x}_{\mathrm{new}} \) to the tree.
5. Rewire within a radius \( r_n \) of \( \mathbf{x}_{\mathrm{new}} \) using local propagation costs.

flowchart TD
  A["Sample x_rand in X_free"] --> B["Find x_near in tree (metric in state space)"]
  B --> C["Forward propagate dynamics with candidate controls"]
  C --> D{"Trajectory collision free?"}
  D -->|no| A
  D -->|yes| E["Add x_new as node, store parent and cost"]
  E --> F["Find neighbors in radius r_n"]
  F --> G["Choose best parent via minimal cost-to-come"]
  G --> H["Rewire neighbors if going via x_new reduces their cost"]
  H --> A
        

The radius \( r_n \) is chosen analogously to geometric RRT*:

\[ r_n = \gamma \left( \frac{\log n}{n} \right)^{1/d}, \]

where \( d \) is the dimension of \( \mathcal{X} \) and \( \gamma \) is a constant that depends on the measure of \( \mathcal{X}_{\mathrm{free}} \) and properties of feasible trajectories. The use of this radius ensures connectivity of the underlying random geometric graph induced by feasible kinodynamic edges.

As in geometric RRT*, asymptotic optimality of kinodynamic RRT* relies on:

• The existence of an \( \varepsilon \)-robust optimal trajectory: an optimal solution whose state-time tube of radius \( \varepsilon \) lies within \( \mathcal{X}_{\mathrm{free}} \).
• Lipschitz continuity of \( f \) and of the cost density \( g \) with respect to the state and control.
• The steering / propagation operator \( \Phi \) being able to approximate any short feasible segment with cost distortion bounded by a factor tending to one as segment length tends to zero.

Under these conditions and using the radius \( r_n \) above, one can show that:

\[ \mathbb{P} \left( \limsup_{n \to \infty} \left| J_n^{\star} - J^{\star} \right| = 0 \right) = 1, \]

where \( J_n^{\star} \) is the best cost-to-come to any goal node in \( \mathcal{T}_n \).

Probabilistic completeness follows from the fact that random samples almost surely intersect any reachable neighborhood of the robust optimal trajectory. The reachable set at finite time from a state \( \mathbf{x}_0 \) is:

\[ \mathcal{R}_T(\mathbf{x}_0) = \left\{ \mathbf{x}(T) \;\middle|\; \dot{\mathbf{x}} = f(\mathbf{x},\mathbf{u}),\; \mathbf{x}(0) = \mathbf{x}_0,\; \mathbf{u}(\cdot) \in \mathcal{U} \right\}. \]

If for every point along an optimal trajectory the reachable set in a small time interval has nonzero volume in \( \mathcal{X}_{\mathrm{free}} \), then samples and local kinodynamic edges can approximate that trajectory arbitrarily closely as \( n \to \infty \).

A practical kinodynamic RRT* must choose:

• A state-space metric \( d(\mathbf{x}_1,\mathbf{x}_2) \) that accounts for both configuration and velocity. A common choice is a weighted Euclidean metric:

\[ d(\mathbf{x}_1,\mathbf{x}_2)^2 = \left\| W_q(\mathbf{q}_1 - \mathbf{q}_2) \right\|^2 + \left\| W_v(\dot{\mathbf{q}}_1 - \dot{\mathbf{q}}_2) \right\|^2, \]

• A control discretization \( \mathcal{U}_{\mathrm{disc}} \subset \mathcal{U} \), e.g. a finite set of accelerations or steering commands. For a double integrator:

\[ \mathcal{U}_{\mathrm{disc}} = \left\{ -u_{\max}, 0, u_{\max} \right\}^m. \]

• A stable numerical integrator (e.g. explicit Euler or Runge–Kutta) with step size \( \Delta t \) small enough to approximate dynamics and collision constraints.

For robots with velocity and acceleration limits, the dynamics often take the form of a second-order system:

\[ \ddot{\mathbf{q}} = h(\mathbf{q},\dot{\mathbf{q}},\mathbf{u}), \qquad \|\mathbf{u}\| \leq u_{\max}. \]

An explicit Euler discretization for the state \( \mathbf{x} = (\mathbf{q},\dot{\mathbf{q}}) \) is:

\[ \begin{aligned} \mathbf{q}_{k+1} &= \mathbf{q}_k + \Delta t \, \dot{\mathbf{q}}_k, \\ \dot{\mathbf{q}}_{k+1} &= \dot{\mathbf{q}}_k + \Delta t \, h(\mathbf{q}_k,\dot{\mathbf{q}}_k,\mathbf{u}_k). \end{aligned} \]

This transition is used inside the propagation operator \( \Phi \) in each edge expansion of the tree.

We implement a minimal kinodynamic RRT* in Python for a point mass in the plane with double-integrator dynamics:

\[ \dot{x} = v_x,\quad \dot{y} = v_y,\quad \dot{v}_x = u_x,\quad \dot{v}_y = u_y,\quad \|\mathbf{u}\|_{\infty} \leq u_{\max}. \]

Libraries such as ompl (via Python bindings) or pybullet can be used to integrate with full robot models, but here we show a from-scratch implementation using NumPy.

import numpy as np

class Node:
    def __init__(self, state, parent=None, cost=0.0):
        self.state = np.array(state, dtype=float)
        self.parent = parent
        self.cost = float(cost)

def distance(x1, x2, w_pos=1.0, w_vel=0.1):
    # x = [x, y, vx, vy]
    dp = x1[:2] - x2[:2]
    dv = x1[2:] - x2[2:]
    return np.sqrt(w_pos * np.dot(dp, dp) + w_vel * np.dot(dv, dv))

def forward_propagate(x0, u, dt, steps):
    """Explicit Euler integration for double integrator."""
    x = np.array(x0, dtype=float)
    cost = 0.0
    traj = [x.copy()]
    for _ in range(steps):
        x[0] += dt * x[2]
        x[1] += dt * x[3]
        x[2] += dt * u[0]
        x[3] += dt * u[1]
        cost += dt  # time cost
        traj.append(x.copy())
    return x, cost, np.array(traj)

def collision_free(traj, obstacles):
    # obstacles: list of (cx, cy, r)
    for x in traj:
        for (cx, cy, r) in obstacles:
            if (x[0] - cx) ** 2 + (x[1] - cy) ** 2 <= r ** 2:
                return False
    return True

def find_nearest(tree, x_rand):
    d_min = float("inf")
    best = None
    for node in tree:
        d = distance(node.state, x_rand)
        if d < d_min:
            d_min = d
            best = node
    return best

def find_neighbors(tree, x_new, r_n):
    return [node for node in tree if distance(node.state, x_new) <= r_n]

def kinodynamic_rrt_star(x_init, goal_region, bounds, obstacles,
                         u_set, dt=0.1, steps=5, n_iter=500,
                         gamma=30.0):
    """
    x_init: initial state [x,y,vx,vy]
    goal_region: lambda x -> bool
    bounds: [xmin, xmax, ymin, ymax, vmax]
    u_set: list of candidate accelerations [(ux, uy), ...]
    """
    dim = 4
    X_free_measure = (bounds[1] - bounds[0]) * (bounds[3] - bounds[2])
    tree = [Node(x_init)]
    best_goal = None

    for n in range(1, n_iter + 1):
        # sample state (velocity bounded)
        x_rand = np.array([
            np.random.uniform(bounds[0], bounds[1]),
            np.random.uniform(bounds[2], bounds[3]),
            np.random.uniform(-bounds[4], bounds[4]),
            np.random.uniform(-bounds[4], bounds[4])
        ])

        x_near_node = find_nearest(tree, x_rand)
        x_near = x_near_node.state

        # choose best control among discrete set
        best_x_new, best_traj = None, None
        best_edge_cost = float("inf")
        for u in u_set:
            x_new, edge_cost, traj = forward_propagate(x_near, u, dt, steps)
            if not (bounds[0] <= x_new[0] <= bounds[1] and
                    bounds[2] <= x_new[1] <= bounds[3]):
                continue
            if not collision_free(traj, obstacles):
                continue
            if edge_cost < best_edge_cost:
                best_edge_cost = edge_cost
                best_x_new = x_new
                best_traj = traj

        if best_x_new is None:
            continue

        r_n = min(
            (gamma * (np.log(n) / n)) ** (1.0 / dim),
            max(bounds[1] - bounds[0], bounds[3] - bounds[2])
        )

        neighbors = find_neighbors(tree, best_x_new, r_n)
        # choose parent that minimizes cost-to-come
        x_new_parent = x_near_node
        new_cost = x_near_node.cost + best_edge_cost

        for node in neighbors:
            # approximate local propagation cost via distance as heuristic
            cand_x_new, cand_cost, cand_traj = forward_propagate(
                node.state, u_set[0], dt, steps
            )
            # for simplicity, assume same cost as best_edge_cost if collision-free
            if collision_free(cand_traj, obstacles):
                cand_total = node.cost + best_edge_cost
                if cand_total < new_cost:
                    new_cost = cand_total
                    x_new_parent = node

        new_node = Node(best_x_new, parent=x_new_parent, cost=new_cost)
        tree.append(new_node)

        # rewiring step
        for node in neighbors:
            # naive local cost estimate (reuse best_edge_cost)
            if new_node.cost + best_edge_cost < node.cost:
                # check collision-free local edge (skipped detail here)
                node.parent = new_node
                node.cost = new_node.cost + best_edge_cost

        if goal_region(new_node.state):
            if best_goal is None or new_node.cost < best_goal.cost:
                best_goal = new_node

    return tree, best_goal
      

This implementation is intentionally simplified (e.g. we reuse a single candidate control in neighbor evaluation). In a more faithful implementation, one would perform local propagation from each neighbor to the new state and recompute the exact edge cost and collision check.

In C++, the Open Motion Planning Library (OMPL) provides ompl::control::RRTstar that already supports kinodynamic planning with generic control spaces and state-space representations. Below is a minimal from-scratch skeleton for a planar double integrator, illustrating the data structures and main loop (without full OMPL integration).

#include <vector>
#include <cmath>
#include <limits>
#include <random>

struct State {
    double x, y, vx, vy;
};

struct Node {
    State state;
    int parent;    // index of parent in vector<Node>
    double cost;
};

double distance(const State& a, const State& b) {
    double dx = a.x - b.x;
    double dy = a.y - b.y;
    double dvx = a.vx - b.vx;
    double dvy = a.vy - b.vy;
    double w_pos = 1.0;
    double w_vel = 0.1;
    return std::sqrt(w_pos * (dx * dx + dy * dy) +
                     w_vel * (dvx * dvx + dvy * dvy));
}

State propagate(const State& s, double ux, double uy,
                double dt, int steps) {
    State x = s;
    for (int k = 0; k < steps; ++k) {
        x.x  += dt * x.vx;
        x.y  += dt * x.vy;
        x.vx += dt * ux;
        x.vy += dt * uy;
    }
    return x;
}

// Collision and neighbor search code omitted for brevity.

int main() {
    std::mt19937 gen(123);
    std::uniform_real_distribution<double> unif01(0.0, 1.0);

    // Bounds
    double xmin = 0.0, xmax = 10.0;
    double ymin = 0.0, ymax = 10.0;
    double vmax = 2.0;

    std::vector<Node> tree;
    Node root;
    root.state = {1.0, 1.0, 0.0, 0.0};
    root.parent = -1;
    root.cost = 0.0;
    tree.push_back(root);

    std::vector<std::pair<double, double>> u_set = {
        {-1.0, 0.0}, {1.0, 0.0},
        {0.0, -1.0}, {0.0, 1.0},
        {0.0, 0.0}
    };

    const int maxIter = 1000;
    const double dt = 0.1;
    const int steps = 5;
    const int dim = 4;
    const double gamma = 30.0;

    for (int n = 1; n <= maxIter; ++n) {
        State x_rand;
        x_rand.x  = xmin + (xmax - xmin) * unif01(gen);
        x_rand.y  = ymin + (ymax - ymin) * unif01(gen);
        x_rand.vx = -vmax + 2.0 * vmax * unif01(gen);
        x_rand.vy = -vmax + 2.0 * vmax * unif01(gen);

        // Find nearest
        int idx_near = 0;
        double best_d = std::numeric_limits<double>::infinity();
        for (int i = 0; i < (int)tree.size(); ++i) {
            double d = distance(tree[i].state, x_rand);
            if (d < best_d) {
                best_d = d;
                idx_near = i;
            }
        }

        // Pick best control (naive)
        const State& x_near = tree[idx_near].state;
        State x_new;
        double best_edge_cost = std::numeric_limits<double>::infinity();
        for (const auto& u : u_set) {
            State cand = propagate(x_near, u.first, u.second, dt, steps);
            // Check bounds and collisions here ...
            double edge_cost = dt * steps;
            if (edge_cost < best_edge_cost) {
                best_edge_cost = edge_cost;
                x_new = cand;
            }
        }

        if (!std::isfinite(best_edge_cost)) {
            continue;
        }

        // Compute radius r_n
        double rn = std::pow(gamma * std::log((double)n) / (double)n, 1.0 / dim);

        // TODO: find neighbors within rn, choose best parent, and rewire.

        Node newNode;
        newNode.state = x_new;
        newNode.parent = idx_near;
        newNode.cost = tree[idx_near].cost + best_edge_cost;
        tree.push_back(newNode);
    }

    return 0;
}
      

In a robotics codebase, this logic would typically be wrapped in a planning module, and integrated with a dynamics engine (e.g. via a physics engine such as Bullet or the dynamic model from a robotics framework). OMPL provides ready-made state and control space abstractions that can replace the hand-written State/propagate functions.

Java is less central in mainstream motion-planning libraries, but Java-based robotics projects (e.g. for educational robots or simulation front-ends) can still implement kinodynamic RRT* with standard collections. Below is a minimal skeleton for the same planar double-integrator model.

import java.util.ArrayList;
import java.util.List;
import java.util.Random;

class State {
    double x, y, vx, vy;
    State(double x, double y, double vx, double vy) {
        this.x = x; this.y = y; this.vx = vx; this.vy = vy;
    }
}

class Node {
    State state;
    Node parent;
    double cost;
    Node(State s, Node p, double c) {
        this.state = s; this.parent = p; this.cost = c;
    }
}

public class KinodynamicRRTStar {
    private final List<Node> tree = new ArrayList<>();
    private final Random rand = new Random(1234);

    double distance(State a, State b) {
        double dx = a.x - b.x;
        double dy = a.y - b.y;
        double dvx = a.vx - b.vx;
        double dvy = a.vy - b.vy;
        double wPos = 1.0, wVel = 0.1;
        return Math.sqrt(wPos * (dx * dx + dy * dy)
                       + wVel * (dvx * dvx + dvy * dvy));
    }

    State propagate(State s, double ux, double uy, double dt, int steps) {
        double x = s.x, y = s.y, vx = s.vx, vy = s.vy;
        for (int k = 0; k < steps; ++k) {
            x  += dt * vx;
            y  += dt * vy;
            vx += dt * ux;
            vy += dt * uy;
        }
        return new State(x, y, vx, vy);
    }

    public void run(State init, int maxIter) {
        tree.add(new Node(init, null, 0.0));

        double xmin = 0.0, xmax = 10.0;
        double ymin = 0.0, ymax = 10.0;
        double vmax = 2.0;
        double[][] uSet = {
            {-1.0, 0.0}, {1.0, 0.0},
            {0.0, -1.0}, {0.0, 1.0}, {0.0, 0.0}
        };
        double dt = 0.1;
        int steps = 5;
        int dim = 4;
        double gamma = 30.0;

        for (int n = 1; n <= maxIter; ++n) {
            State xRand = new State(
                xmin + (xmax - xmin) * rand.nextDouble(),
                ymin + (ymax - ymin) * rand.nextDouble(),
                -vmax + 2.0 * vmax * rand.nextDouble(),
                -vmax + 2.0 * vmax * rand.nextDouble()
            );

            // nearest
            Node nearest = tree.get(0);
            double best = Double.POSITIVE_INFINITY;
            for (Node node : tree) {
                double d = distance(node.state, xRand);
                if (d < best) {
                    best = d;
                    nearest = node;
                }
            }

            State xNew = null;
            double bestEdgeCost = Double.POSITIVE_INFINITY;
            for (double[] u : uSet) {
                State cand = propagate(nearest.state, u[0], u[1], dt, steps);
                // bounds check (collision omitted)
                if (cand.x < xmin || cand.x > xmax ||
                    cand.y < ymin || cand.y > ymax) {
                    continue;
                }
                double edgeCost = dt * steps;
                if (edgeCost < bestEdgeCost) {
                    bestEdgeCost = edgeCost;
                    xNew = cand;
                }
            }
            if (xNew == null) continue;

            // radius (not used here for simplicity)
            double rn = Math.pow(
                gamma * Math.log((double)n) / (double)n,
                1.0 / (double)dim
            );

            Node newNode = new Node(xNew, nearest,
                                    nearest.cost + bestEdgeCost);
            tree.add(newNode);
            // neighbor-based rewiring omitted
        }
    }

    public static void main(String[] args) {
        KinodynamicRRTStar planner = new KinodynamicRRTStar();
        planner.run(new State(1.0, 1.0, 0.0, 0.0), 500);
        System.out.println("Tree size: " + planner.tree.size());
    }
}
      

For Java-based robotics projects (e.g. mobile robots or manipulators controlled via ROS 2 Java clients), this logic would be connected to a dynamics / collision-check backend and executed within a planning node.

MATLAB is common in control and robotics. We can keep the RRT* logic in MATLAB, while using Simulink to model the continuous dynamics as a plant. Suppose we build a Simulink model named double_integrator_2d that takes input \( \mathbf{u}(t) \) and outputs state \( \mathbf{x}(t) \).

function [tree, bestGoal] = kinodynamic_rrt_star_matlab()
    % State: [x; y; vx; vy]
    xInit = [1; 1; 0; 0];
    bounds = [0 10 0 10 2]; % [xmin xmax ymin ymax vmax]
    uSet = [ -1 0; 1 0; 0 -1; 0 1; 0 0 ];
    dt = 0.1; steps = 5;
    maxIter = 500; gamma = 30.0; dim = 4;

    node.state = xInit;
    node.parent = 0;
    node.cost = 0;
    tree = node;
    bestGoal = [];

    for n = 1:maxIter
        xRand = [ ...
            bounds(1) + (bounds(2) - bounds(1)) * rand; ...
            bounds(3) + (bounds(4) - bounds(3)) * rand; ...
            -bounds(5) + 2 * bounds(5) * rand; ...
            -bounds(5) + 2 * bounds(5) * rand ];

        % nearest
        dMin = inf; idxNear = 1;
        for i = 1:numel(tree)
            d = state_distance(tree(i).state, xRand);
            if d < dMin
                dMin = d; idxNear = i;
            end
        end

        xNear = tree(idxNear).state;
        bestEdgeCost = inf;
        bestXnew = [];

        for ui = 1:size(uSet,1)
            u = uSet(ui,:).';
            % forward simulate via Simulink
            [xNew, edgeCost] = propagate_with_simulink( ...
                xNear, u, dt, steps);
            if isempty(xNew), continue; end

            if xNew(1) < bounds(1) || xNew(1) > bounds(2) || ...
               xNew(2) < bounds(3) || xNew(2) > bounds(4)
                continue;
            end

            if edgeCost < bestEdgeCost
                bestEdgeCost = edgeCost;
                bestXnew = xNew;
            end
        end

        if isempty(bestXnew), continue; end

        rn = (gamma * (log(n) / n))^(1/dim); %#ok<NASGU>

        newNode.state = bestXnew;
        newNode.parent = idxNear;
        newNode.cost = tree(idxNear).cost + bestEdgeCost;
        tree(end+1) = newNode; %#ok<AGROW>

        if is_in_goal(bestXnew)
            if isempty(bestGoal) || newNode.cost < bestGoal.cost
                bestGoal = newNode;
            end
        end
    end
end

function d = state_distance(x1, x2)
    wPos = 1.0; wVel = 0.1;
    dp = x1(1:2) - x2(1:2);
    dv = x1(3:4) - x2(3:4);
    d = sqrt(wPos * (dp.'*dp) + wVel * (dv.'*dv));
end

function [xNew, edgeCost] = propagate_with_simulink(xInit, u, dt, steps)
    Tfinal = dt * steps;
    % Assume Simulink model 'double_integrator_2d' with input u and initial state xInit
    simOut = sim("double_integrator_2d", ...
                 "StopTime", num2str(Tfinal), ...
                 "LoadInitialState", "on", ...
                 "InitialState", xInit, ...
                 "SrcWorkspace", "current");
    xTraj = simOut.xout.signals.values;
    xNew = xTraj(end,:).';
    edgeCost = Tfinal; % time cost
end

function flag = is_in_goal(x)
    goalCenter = [9; 9];
    goalR = 0.5;
    flag = norm(x(1:2) - goalCenter) <= goalR;
end
      

The Simulink model encapsulates the physics, while MATLAB manages sampling, neighbor search, and rewiring. In more complex robotic systems, the plant model can be the manipulator dynamics with torque saturation and contact forces.

Wolfram Mathematica is well suited for prototyping kinodynamic planners, thanks to symbolic and numeric integration routines like NDSolve. The snippet below illustrates a simple kinodynamic RRT-like expansion step for a double integrator in one dimension, easily generalizable to two dimensions.

ClearAll["Global`*"];

stateDistance[{x1_, v1_}, {x2_, v2_}] :=
  Sqrt[(x1 - x2)^2 + 0.1 (v1 - v2)^2];

propagateState[{x0_, v0_}, u_, dt_, steps_] := Module[
  {x = x0, v = v0, t},
  Do[
    v = v + dt u;
    x = x + dt v;
    ,
    {steps}
  ];
  { {x, v}, dt steps }
];

kinodynamicRRTStep[tree_, uSet_, bounds_, dt_, steps_] := Module[
  {xrand, nearest, bestEdgeCost, bestState, uCand, dist, node},
  xrand = {
    RandomReal[{bounds[[1]], bounds[[2]]}],
    RandomReal[{-bounds[[3]], bounds[[3]]}]
  };
  nearest = First@MinimalBy[tree, stateDistance[#["state"], xrand] &];
  bestEdgeCost = Infinity;
  bestState = None;
  Do[
    {uCand} = u;
    {newState, edgeCost} =
      propagateState[nearest["state"], uCand, dt, steps];
    If[newState[[1]] < bounds[[1]] || newState[[1]] > bounds[[2]],
      Continue[];
    ];
    If[edgeCost < bestEdgeCost,
      bestEdgeCost = edgeCost;
      bestState = newState;
    ];
    ,
    {u, uSet}
  ];
  If[bestState === None,
    tree,
    Append[tree, <|
      "state" -> bestState,
      "parent" -> nearest,
      "cost" -> nearest["cost"] + bestEdgeCost
    |>]
  ]
];

(* Example usage *)
bounds = {0, 10, 2}; (* xmin, xmax, vmax *)
dt = 0.1; steps = 5;
uSet = { {-1.0}, {0.0}, {1.0} };
tree = {<|"state" -> {1.0, 0.0}, "parent" -> None, "cost" -> 0.0|>};

Do[
  tree = kinodynamicRRTStep[tree, uSet, bounds, dt, steps];
  ,
  {50}
];

Length[tree]
      

Mathematica's symbolic capabilities can also be used to analyze reachable sets and cost functionals for simple dynamical systems, providing insights before coding a large-scale planner in C++ or Python.

Problem 1 (Discrete-Time Double Integrator Dynamics): Consider the planar double integrator with state \( \mathbf{x} = (x,y,v_x,v_y) \) and control \( \mathbf{u} = (u_x,u_y) \), with dynamics \( \dot{x} = v_x \), \( \dot{y} = v_y \), \( \dot{v}_x = u_x \), \( \dot{v}_y = u_y \). Derive the first-order explicit Euler discretization with step \( \Delta t \).

Solution:

Explicit Euler updates state using \( \mathbf{x}_{k+1} = \mathbf{x}_k + \Delta t \, f(\mathbf{x}_k,\mathbf{u}_k) \). Here, \( f(\mathbf{x},\mathbf{u}) = (v_x, v_y, u_x, u_y) \). Therefore

\[ \begin{aligned} x_{k+1} &= x_k + \Delta t \, v_{x,k}, \\ y_{k+1} &= y_k + \Delta t \, v_{y,k}, \\ v_{x,k+1} &= v_{x,k} + \Delta t \, u_{x,k}, \\ v_{y,k+1} &= v_{y,k} + \Delta t \, u_{y,k}. \end{aligned} \]

This is exactly the update used in the code implementations; as \( \Delta t \to 0 \), the discrete trajectory converges to the continuous one under regularity assumptions on the control sequence.

Problem 2 (Rewiring Radius Dimension Dependence): Let \( \mathcal{X} \subset \mathbb{R}^d \) be a compact state space with positive Lebesgue measure. Show why the RRT* rewiring radius \( r_n = \gamma (\log n / n)^{1/d} \) shrinks to zero as \( n \to \infty \), and explain intuitively why a smaller radius must be used in higher-dimensional state spaces.

Solution:

We have \( \log n / n \to 0 \) as \( n \to \infty \). Since \( 0 < \log n / n < 1 \) for large \( n \) and the function \( x \mapsto x^{1/d} \) is continuous and strictly increasing on \( (0,1) \), we obtain

\[ \lim_{n \to \infty} r_n = \lim_{n \to \infty} \gamma \left( \frac{\log n}{n} \right)^{1/d} = \gamma \cdot 0 = 0. \]

For fixed \( n \), increasing dimension \( d \) decreases \( r_n \) because the exponent \( 1/d \) becomes smaller, i.e. the radius must shrink faster to maintain the right balance between sparsity and connectivity in high dimensions. Intuitively, volume grows rapidly with radius in large \( d \), so a small radius already covers a large neighborhood; using a radius too large would create overly dense neighbor sets and excessive rewiring costs.

Problem 3 (Feasible vs Infeasible Straight-Line Edges): For a unicycle model with state \( (x,y,\theta) \) and control \( (v,\omega) \), dynamics are \( \dot{x} = v \cos\theta \), \( \dot{y} = v \sin\theta \), \( \dot{\theta} = \omega \). Explain why straight-line interpolation in \( (x,y,\theta) \)-space is not dynamically feasible in general, and why kinodynamic RRT* must use forward integration instead.

Solution:

Straight-line interpolation in state space between \( (x_0,y_0,\theta_0) \) and \( (x_1,y_1,\theta_1) \) typically yields an intermediate path where \( (x(t),y(t)) \) and \( \theta(t) \) are linear in time. For a unicycle satisfying \( \dot{x} = v \cos\theta \), \( \dot{y} = v \sin\theta \), the velocity vector \( (\dot{x},\dot{y}) \) must be aligned with heading \( \theta \). In a linear interpolation, the velocity vector induced by the straight segment between two intermediate points need not satisfy this alignment. Hence, the straight-line segment is not a trajectory of the unicycle dynamics for any admissible \( v,\omega \). Kinodynamic RRT* therefore uses forward integration of the differential equations with admissible controls to generate edges.

Problem 4 (Cost-to-Come Monotonicity): Show that in kinodynamic RRT*, if each edge cost \( c(\sigma_{ij}) \) is strictly positive, then along any path from root to a node \( \mathbf{x}_i \), the sequence of node costs \( J(\mathbf{x}_0), J(\mathbf{x}_1), \dots, J(\mathbf{x}_i) \) is strictly increasing.

Solution:

Let \( \mathbf{x}_0 = \mathbf{x}_{\mathrm{init}} \) have cost \( J(\mathbf{x}_0) = 0 \). Suppose a node \( \mathbf{x}_k \) is added with parent \( \mathbf{x}_{k-1} \) via an edge with cost \( c(\sigma_{k-1,k}) > 0 \). By definition, \( J(\mathbf{x}_k) = J(\mathbf{x}_{k-1}) + c(\sigma_{k-1,k}) \). Since \( c(\sigma_{k-1,k}) > 0 \), we have \( J(\mathbf{x}_k) > J(\mathbf{x}_{k-1}) \). By induction along the path, each node has strictly higher cost than its parent. Even after rewiring, the new cost remains parent cost plus positive edge cost, so the monotonicity property holds.

Problem 5 (Goal-Biased Sampling Effect): Suppose in kinodynamic RRT* we replace uniform state sampling by a mixture: with probability \( p \in (0,1) \) sample uniformly from \( \mathcal{X}_{\mathrm{free}} \), and with probability \( 1-p \) sample uniformly from a small goal region \( \mathcal{X}_{\mathrm{goal}} \). Explain qualitatively how this affects convergence speed and asymptotic optimality.

Solution:

The mixture sampling increases the fraction of samples near the goal, which accelerates the first discovery of a feasible solution (reduced expected time to find a goal-reaching path). However, asymptotic optimality requires that samples remain dense in \( \mathcal{X}_{\mathrm{free}} \), i.e. the distribution has support equal to \( \mathcal{X}_{\mathrm{free}} \). As long as \( p > 0 \), the uniform component guarantees that every region of \( \mathcal{X}_{\mathrm{free}} \) receives infinitely many samples with probability one, so asymptotic optimality is preserved. If instead we set \( p = 0 \) (only sampling in the goal region), the planner would not explore the rest of the space and could fail to approximate the global optimal solution.

This lesson lifted sampling-based planning from configuration space to full state space, where edges must satisfy the robot dynamics. We formalized the kinodynamic RRT* problem as an optimal control problem, emphasized the role of numerical integration and control discretization, and examined how rewiring and radius selection extend to state spaces with velocities and accelerations. We also presented practical implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing the ground for the next lesson on differential constraints and feasibility in more complex underactuated systems.