In the previous lesson, trajectory optimization was formulated as the minimization of a functional \( \mathcal{U}(\boldsymbol{\xi}) \) defined over trajectories \( \boldsymbol{\xi} : [0,1] \to \mathbb{R}^n \) in configuration space, subject to boundary conditions and collision constraints. CHOMP and STOMP instantiate this general setup with:

• A smoothness cost that penalizes large accelerations or jerks along the trajectory.
• An obstacle cost derived from a scalar potential over the workspace (e.g., a signed distance field).
• An iterative scheme that updates an initial trajectory (often a straight-line interpolation in configuration space) to reduce collisions while maintaining smoothness.

CHOMP uses gradient-based optimization with respect to a specially chosen inner product (covariant or natural gradient), while STOMP uses stochastic sampling of noisy trajectories and a path-integral-like update that does not require explicit gradients of the collision cost.

flowchart TD
  A[Start: initial trajectory xi0] --> B[Select optimizer: CHOMP or STOMP]
  B --> C[Evaluate smoothness and obstacle costs]
  C --> D[Compute trajectory update]
  D --> E[Update trajectory xikplus1]
  E --> F[Check collisions and convergence]
  F -->|not converged| C
  F -->|converged| G[Output optimized trajectory xi_star]
  

Both methods exploit the fact that trajectories for manipulators live in a relatively low-dimensional configuration space compared to the full state-space of dynamics. Dynamic constraints can later be incorporated (e.g., via additional cost terms or kinodynamic extensions), but in this lesson we focus on kinematic trajectory optimization.

Let \( \boldsymbol{\xi}(s) \in \mathbb{R}^n \), \( s \in [0,1] \) denote a path in joint space, with fixed boundary conditions \( \boldsymbol{\xi}(0)=\mathbf{q}_{\text{start}} \) and \( \boldsymbol{\xi}(1)=\mathbf{q}_{\text{goal}} \). CHOMP considers a cost functional of the form

\[ \mathcal{U}(\boldsymbol{\xi}) = \Phi_{\text{smooth}}(\boldsymbol{\xi}) + \lambda \, \Phi_{\text{obs}}(\boldsymbol{\xi}), \quad \lambda > 0, \]

where:

• Smoothness cost. A common choice penalizes squared acceleration:

  \[ \Phi_{\text{smooth}}(\boldsymbol{\xi}) = \frac{1}{2} \int_0^1 \left\| \frac{d^2 \boldsymbol{\xi}(s)}{ds^2} \right\|^2 ds. \]

• Obstacle cost. Let \( \mathbf{x}(s) = f(\boldsymbol{\xi}(s)) \in \mathbb{R}^3 \) be the forward kinematics mapping to a workspace control point and \( c(\mathbf{x}) \) a scalar cost (e.g., based on distance to obstacles). Then

  \[ \Phi_{\text{obs}}(\boldsymbol{\xi}) = \int_0^1 c\big(f(\boldsymbol{\xi}(s))\big)\, \left\| \frac{d\boldsymbol{\xi}(s)}{ds} \right\| \, ds. \]

The weight \( \lambda \) trades off collision avoidance against smoothness. The path-length factor \( \| d\boldsymbol{\xi}(s)/ds \| \) makes the obstacle cost approximately invariant to reparameterizations of \( s \).

In practice, we discretize the trajectory at \( K \) internal waypoints \( \mathbf{q}_1,\dots,\mathbf{q}_{K-2} \), with \( \mathbf{q}_0 = \mathbf{q}_{\text{start}} \) and \( \mathbf{q}_{K-1} = \mathbf{q}_{\text{goal}} \) fixed. Collect the internal waypoints into a stacked vector \( \mathbf{q} \in \mathbb{R}^{(K-2)n} \).

Assume a uniform parameterization with step size \( \Delta s = 1/(K-1) \). A second-order finite-difference approximation of the second derivative is

\[ \frac{d^2 \boldsymbol{\xi}(s_k)}{ds^2} \approx \frac{\mathbf{q}_{k-1} - 2\mathbf{q}_k + \mathbf{q}_{k+1}}{(\Delta s)^2}, \quad k = 1,\dots,K-2. \]

Let \( \mathbf{A} \) be the linear operator that maps the stacked vector of all waypoints (including endpoints) to discrete accelerations:

\[ \mathbf{a} = \mathbf{A}\tilde{\mathbf{q}}, \quad \tilde{\mathbf{q}} = \begin{bmatrix} \mathbf{q}_0 \\ \mathbf{q}_1 \\ \vdots \\ \mathbf{q}_{K-1} \end{bmatrix}, \quad \mathbf{a} = \begin{bmatrix} \mathbf{a}_1 \\ \vdots \\ \mathbf{a}_{K-2} \end{bmatrix}. \]

Then the discrete smoothness cost can be expressed as

\[ \Phi_{\text{smooth}}(\mathbf{q}) \approx \frac{1}{2} \mathbf{a}^\top \mathbf{a} \Delta s = \frac{1}{2} \tilde{\mathbf{q}}^\top \mathbf{A}^\top \mathbf{A} \tilde{\mathbf{q}} \Delta s. \]

Eliminating fixed endpoints yields a quadratic form in the internal waypoints:

\[ \Phi_{\text{smooth}}(\mathbf{q}) = \frac{1}{2} \mathbf{q}^\top \mathbf{K} \mathbf{q} + \mathbf{b}^\top \mathbf{q} + \text{const}, \quad \mathbf{K} = \mathbf{A}_{\text{int}}^\top \mathbf{A}_{\text{int}} \Delta s, \]

where \( \mathbf{A}_{\text{int}} \) is the restriction of \( \mathbf{A} \) to internal waypoints. Since \( \mathbf{K} = \mathbf{A}_{\text{int}}^\top\mathbf{A}_{\text{int}} \), it is symmetric positive semidefinite. The gradient of the smoothness term is

\[ \nabla \Phi_{\text{smooth}}(\mathbf{q}) = \mathbf{K}\mathbf{q} + \mathbf{b}, \]

where \( \mathbf{b} \) collects contributions from fixed endpoints. This matrix structure is central to CHOMP: it not only defines the smoothness cost, but also the inner product with respect to which gradients are taken.

Consider a set of workspace control points \( \mathbf{x}_{k,\ell} \) on the robot at discrete configuration \( \mathbf{q}_k \) (e.g., points on links or end-effector). Let \( c(\mathbf{x}) \) denote a scalar obstacle cost (often a smoothed function of signed distance to obstacles). A simple discrete obstacle cost is

\[ \Phi_{\text{obs}}(\mathbf{q}) = \sum_{k=1}^{K-2} \sum_{\ell} w_{k,\ell} \, c(\mathbf{x}_{k,\ell}), \quad \mathbf{x}_{k,\ell} = f_{\ell}(\mathbf{q}_k), \]

with positive weights \( w_{k,\ell} \). The gradient with respect to \( \mathbf{q}_k \) is obtained by the chain rule:

\[ \nabla_{\mathbf{q}_k} \Phi_{\text{obs}}(\mathbf{q}) = \sum_{\ell} w_{k,\ell} \, \mathbf{J}_{k,\ell}^\top \nabla_{\mathbf{x}} c(\mathbf{x}_{k,\ell}), \quad \mathbf{J}_{k,\ell} = \frac{\partial f_{\ell}(\mathbf{q}_k)}{\partial \mathbf{q}_k}. \]

Stacking all internal waypoints we obtain \( \nabla \Phi_{\text{obs}}(\mathbf{q}) \in \mathbb{R}^{(K-2)n} \).

CHOMP does covariant gradient descent with respect to the inner product

\[ \langle \mathbf{u}, \mathbf{v} \rangle_{\mathbf{K}} = \mathbf{u}^\top \mathbf{K} \mathbf{v}. \]

The steepest descent direction under this inner product is

\[ \mathbf{d}_{\text{CHOMP}}(\mathbf{q}) = - \mathbf{K}^{-1} \nabla \mathcal{U}(\mathbf{q}), \quad \mathcal{U}(\mathbf{q}) = \Phi_{\text{smooth}}(\mathbf{q}) + \lambda \Phi_{\text{obs}}(\mathbf{q}). \]

The update rule is then

\[ \mathbf{q}^{(i+1)} = \mathbf{q}^{(i)} - \alpha \, \mathbf{K}^{-1} \nabla \mathcal{U}(\mathbf{q}^{(i)}), \quad \alpha > 0. \]

Intuitively, \( \mathbf{K}^{-1} \) transforms the Euclidean gradient into one that respects the smoothness metric, distributing local obstacle gradients over neighboring waypoints and yielding shape changes that remain smooth.

Proof (steepest descent under metric \( \mathbf{K} \)). Consider the problem of finding the direction \( \mathbf{d} \) with unit norm \( \langle \mathbf{d},\mathbf{d} \rangle_{\mathbf{K}} = 1 \) that maximizes decrease of the cost:

\[ \min_{\mathbf{d}} \nabla \mathcal{U}(\mathbf{q})^\top \mathbf{d} \quad \text{subject to } \mathbf{d}^\top \mathbf{K} \mathbf{d} = 1. \]

The Lagrangian is \( L(\mathbf{d},\lambda) = \nabla \mathcal{U}(\mathbf{q})^\top \mathbf{d} + \frac{\lambda}{2} (\mathbf{d}^\top \mathbf{K}\mathbf{d} - 1) \). Setting the derivative with respect to \( \mathbf{d} \) to zero gives

\[ \nabla_{\mathbf{d}} L = \nabla \mathcal{U}(\mathbf{q}) + \lambda \mathbf{K}\mathbf{d} = \mathbf{0} \quad \Rightarrow \quad \mathbf{d} \propto -\mathbf{K}^{-1}\nabla \mathcal{U}(\mathbf{q}). \]

After normalizing, the steepest descent direction is indeed proportional to \( -\mathbf{K}^{-1}\nabla \mathcal{U} \), which yields the CHOMP update rule.

STOMP avoids explicit gradient computations of the obstacle cost, which can be difficult when only a collision checker is available. Instead, it samples noisy trajectories around a current mean trajectory and moves in a direction that reduces expected cost.

Let the current trajectory parameters be \( \mathbf{q}^{(i)} \). STOMP draws \( M \) noise samples \( \boldsymbol{\epsilon}^{(m)} \sim \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}) \), forms perturbed trajectories

\[ \mathbf{q}^{(m)} = \mathbf{q}^{(i)} + \boldsymbol{\epsilon}^{(m)}, \quad m = 1,\dots,M, \]

and evaluates their costs \( C^{(m)} = \mathcal{U}(\mathbf{q}^{(m)}) \). Weights are assigned according to a soft-min rule, for example

\[ w^{(m)} = \exp\!\big(-\frac{1}{\eta} C^{(m)}\big), \quad \tilde{w}^{(m)} = \frac{w^{(m)}}{\sum_{j=1}^M w^{(j)}}, \]

with temperature parameter \( \eta > 0 \). The update is

\[ \Delta \mathbf{q} = \sum_{m=1}^M \tilde{w}^{(m)} \boldsymbol{\epsilon}^{(m)}, \quad \mathbf{q}^{(i+1)} = \mathbf{q}^{(i)} + \Delta \mathbf{q}. \]

By shaping the covariance \( \boldsymbol{\Sigma} \) with the same smoothness matrix \( \mathbf{K}^{-1} \) used in CHOMP, STOMP effectively biases perturbations toward smooth deformations of the trajectory. Under small-noise and small-step assumptions, one can show that the expected update \( \mathbb{E}[\Delta \mathbf{q}] \) is approximately proportional to a natural gradient direction, connecting STOMP to CHOMP in the limit.

flowchart TD
  S["Current trajectory q^i"] --> N["Sample M noise vectors epsilon^(m) ~ N(0,Sigma)"]
  N --> P["Perturb: q^(m) = q^i + epsilon^(m)"]
  P --> C["Evaluate cost C^(m) on each perturbed trajectory"]
  C --> W["Compute soft-min weights w^(m) from C^(m)"]
  W --> U["Update: Delta q = sum_m w^(m) * epsilon^(m)"]
  U --> NEXT["New trajectory q^(i+1) = q^i + Delta q"]
        

Unlike CHOMP, STOMP only requires black-box cost evaluation and can cope with non-differentiable collision costs, at the expense of higher sampling complexity per iteration.

We illustrate CHOMP and STOMP on a simple 2D point robot moving in a plane, where the configuration is the position itself. For a manipulator, one would replace the forward kinematics and Jacobians accordingly. In a full robotics stack, Python implementations often integrate with OMPL, MoveIt, and collision libraries exposed via ROS, but here we build the core logic from scratch.

import numpy as np

def build_K(num_points):
    """
    Build 1D smoothness matrix K for internal points using
    second-order finite differences. For 2D we will apply it per dimension.
    """
    K = np.zeros((num_points, num_points))
    for k in range(num_points):
        if k > 0:
            K[k, k-1] -= 1.0
            K[k-1, k] -= 1.0
        K[k, k] += 2.0
    # This approximates integral of squared second derivative up to scaling.
    return K

def obstacle_cost(x):
    """Simple radial obstacle around origin."""
    d = np.linalg.norm(x)
    d0 = 0.5  # obstacle radius
    if d < d0:
        return 0.5 * (d0 - d)**2
    return 0.0

def obstacle_grad(x):
    """Gradient of obstacle_cost."""
    d = np.linalg.norm(x)
    d0 = 0.5
    if d < 1e-8:
        return np.zeros_like(x)
    if d < d0:
        return - (d0 - d) * (x / d)
    return np.zeros_like(x)

def chomp_step(q, K, alpha=0.1, lam=1.0):
    """
    q: array of shape (N, 2) internal waypoints (2D point robot).
    K: N x N smoothness matrix.
    """
    N = q.shape[0]

    # Smoothness gradient: flatten and apply K kron I_2
    q_flat = q.reshape(N * 2)
    K_big = np.kron(K, np.eye(2))
    grad_smooth = K_big @ q_flat

    # Obstacle gradient: per waypoint
    grad_obs = np.zeros_like(q)
    for k in range(N):
        grad_obs[k, :] = obstacle_grad(q[k, :])
    grad_obs_flat = grad_obs.reshape(N * 2)

    grad_total = grad_smooth + lam * grad_obs_flat

    # Covariant update: solve K_big * delta = grad_total
    delta = np.linalg.solve(K_big + 1e-6 * np.eye(N * 2), grad_total)
    q_new_flat = q_flat - alpha * delta
    return q_new_flat.reshape(N, 2)

def stomp_step(q, K, num_rollouts=20, noise_std=0.05, lam=1.0, eta=0.1):
    """
    STOMP step around current trajectory q (N x 2).
    """
    N = q.shape[0]
    q_flat = q.reshape(N * 2)

    # Shape noise using smoothness inverse K^{-1}
    K_big = np.kron(K, np.eye(2))
    K_inv = np.linalg.inv(K_big + 1e-6 * np.eye(N * 2))

    epsilons = []
    costs = []
    for m in range(num_rollouts):
        # Sample white noise then smooth it
        e_raw = np.random.randn(N * 2) * noise_std
        e_smooth = K_inv @ e_raw
        q_pert = (q_flat + e_smooth).reshape(N, 2)

        # Compute cost (smoothness + obstacle)
        smooth = 0.5 * float(q_pert.reshape(N * 2).T @ (K_big @ q_pert.reshape(N * 2)))
        obs = 0.0
        for k in range(N):
            obs += obstacle_cost(q_pert[k, :])
        cost = smooth + lam * obs

        epsilons.append(e_smooth)
        costs.append(cost)

    costs = np.array(costs)
    # Soft-min weights
    c_min = np.min(costs)
    weights = np.exp(-(costs - c_min) / eta)
    weights /= np.sum(weights)

    delta = np.zeros_like(q_flat)
    for w, e in zip(weights, epsilons):
        delta += w * e

    q_new_flat = q_flat + delta
    return q_new_flat.reshape(N, 2)

# Example usage:
if __name__ == "__main__":
    np.random.seed(0)
    N = 30  # internal waypoints
    K = build_K(N)

    start = np.array([-1.0, 0.0])
    goal = np.array([1.0, 0.0])
    # Initialize straight line
    alphas = np.linspace(0.0, 1.0, N + 2)[1:-1]
    q = np.outer(1 - alphas, start) + np.outer(alphas, goal)

    for it in range(50):
        q = chomp_step(q, K, alpha=0.05, lam=5.0)
    # Alternatively:
    # for it in range(50):
    #     q = stomp_step(q, K)
      

In a manipulator setting, obstacle_cost and obstacle_grad would use forward kinematics and distance fields. Robotics-focused Python stacks usually delegate collision and kinematics to pybullet, ROS with MoveIt, or pinocchio, while reusing the CHOMP/STOMP update logic above.

In C++, CHOMP is frequently implemented within ROS/MoveIt using Eigen for linear algebra and collision libraries like FCL. The snippet below shows a minimal CHOMP step on a 2D point robot; for a manipulator, one would integrate forward kinematics and Jacobians.

#include <Eigen/Dense>
#include <vector>

using Eigen::MatrixXd;
using Eigen::VectorXd;

MatrixXd buildK(int N) {
    MatrixXd K = MatrixXd::Zero(N, N);
    for (int k = 0; k < N; ++k) {
        K(k, k) += 2.0;
        if (k > 0) {
            K(k, k-1) -= 1.0;
            K(k-1, k) -= 1.0;
        }
    }
    return K;
}

double obstacleCost(const Eigen::Vector2d& x) {
    double d0 = 0.5;
    double d = x.norm();
    if (d < d0) {
        double diff = d0 - d;
        return 0.5 * diff * diff;
    }
    return 0.0;
}

Eigen::Vector2d obstacleGrad(const Eigen::Vector2d& x) {
    double d0 = 0.5;
    double d = x.norm();
    if (d < 1e-8) {
        return Eigen::Vector2d::Zero();
    }
    if (d < d0) {
        double coeff = -(d0 - d) / d;
        return coeff * x;
    }
    return Eigen::Vector2d::Zero();
}

void chompStep(std::vector<Eigen::Vector2d>& q,
               const MatrixXd& K, double alpha, double lambda) {
    int N = static_cast<int>(q.size());
    VectorXd q_flat(2 * N);
    for (int k = 0; k < N; ++k) {
        q_flat(2*k)     = q;
        q_flat(2*k + 1) = q;
    }

    MatrixXd Kbig = Eigen::kroneckerProduct(K, MatrixXd::Identity(2, 2)).eval();
    VectorXd grad_smooth = Kbig * q_flat;

    VectorXd grad_obs = VectorXd::Zero(2 * N);
    for (int k = 0; k < N; ++k) {
        Eigen::Vector2d g = obstacleGrad(q[k]);
        grad_obs(2*k)     = g(0);
        grad_obs(2*k + 1) = g(1);
    }

    VectorXd grad_total = grad_smooth + lambda * grad_obs;

    // Solve (Kbig + eps I) delta = grad_total
    MatrixXd A = Kbig + 1e-6 * MatrixXd::Identity(2 * N, 2 * N);
    VectorXd delta = A.ldlt().solve(grad_total);

    VectorXd q_new_flat = q_flat - alpha * delta;
    for (int k = 0; k < N; ++k) {
        q = q_new_flat(2*k);
        q = q_new_flat(2*k + 1);
    }
}
      

In ROS, this CHOMP core would be embedded into a loop that interacts with a kinematics library (e.g., KDL, MoveIt kinematics plugins) and a collision world representation (e.g., FCL with occupancy grids or octomaps).

Java is less common for trajectory optimization in robotics, but with numerical libraries like EJML, one can implement STOMP-style updates. The snippet below uses basic arrays for clarity. In practice, collision checking would be handled by a Java binding of a 3D engine or robotics middleware.

import java.util.Random;

public class Stomp2D {

    private final Random rng = new Random(0L);

    private double obstacleCost(double[] x) {
        double d0 = 0.5;
        double d = Math.sqrt(x[0]*x[0] + x[1]*x[1]);
        if (d < d0) {
            double diff = d0 - d;
            return 0.5 * diff * diff;
        }
        return 0.0;
    }

    public double[][] stompStep(double[][] q, int numRollouts,
                                double noiseStd, double lambda, double eta) {
        int N = q.length;
        int dim = 2;

        double[][] eps = new double[numRollouts][N * dim];
        double[] costs = new double[numRollouts];

        for (int m = 0; m < numRollouts; ++m) {
            // White noise
            double[] e = new double[N * dim];
            for (int i = 0; i < N * dim; ++i) {
                e[i] = rng.nextGaussian() * noiseStd;
            }
            eps[m] = e;

            // Perturbed trajectory
            double[][] qPert = new double[N][dim];
            for (int k = 0; k < N; ++k) {
                qPert[k][0] = q[k][0] + e[2*k];
                qPert[k][1] = q[k][1] + e[2*k + 1];
            }

            // Cost: smoothness (finite differences) + obstacle
            double smooth = 0.0;
            for (int k = 1; k < N-1; ++k) {
                double ax = qPert[k-1][0] - 2.0*qPert[k][0] + qPert[k+1][0];
                double ay = qPert[k-1][1] - 2.0*qPert[k][1] + qPert[k+1][1];
                smooth += 0.5 * (ax*ax + ay*ay);
            }
            double obs = 0.0;
            for (int k = 0; k < N; ++k) {
                obs += obstacleCost(qPert[k]);
            }
            costs[m] = smooth + lambda * obs;
        }

        // Soft-min weights
        double cmin = Double.POSITIVE_INFINITY;
        for (double c : costs) {
            if (c < cmin) cmin = c;
        }
        double[] w = new double[numRollouts];
        double wsum = 0.0;
        for (int m = 0; m < numRollouts; ++m) {
            w[m] = Math.exp(-(costs[m] - cmin) / eta);
            wsum += w[m];
        }
        for (int m = 0; m < numRollouts; ++m) {
            w[m] /= wsum;
        }

        // Update
        double[] delta = new double[N * dim];
        for (int m = 0; m < numRollouts; ++m) {
            for (int i = 0; i < N * dim; ++i) {
                delta[i] += w[m] * eps[m][i];
            }
        }

        double[][] qNew = new double[N][dim];
        for (int k = 0; k < N; ++k) {
            qNew[k][0] = q[k][0] + delta[2*k];
            qNew[k][1] = q[k][1] + delta[2*k + 1];
        }
        return qNew;
    }
}
      

This STOMP-style update augments existing Java-based robotics frameworks (e.g., those used in industrial control) by providing a sampling-based trajectory optimizer where gradients are not explicitly available.

MATLAB is widely used in control and robotics for prototyping. The Robotics System Toolbox provides kinematics and collision-checking capabilities that integrate naturally with CHOMP and STOMP. Below is a minimal CHOMP step for a 2D point robot; in practice, one would call robotics.RigidBodyTree for kinematics and collisionBox, etc., for obstacles.

function q = chomp_2d_step(q, K, alpha, lambda)
% q: N x 2 internal waypoints
% K: N x N smoothness matrix

N = size(q, 1);
q_flat = q';
q_flat = q_flat(:);

Kbig = kron(K, eye(2));
grad_smooth = Kbig * q_flat;

grad_obs = zeros(size(q));
for k = 1:N
    grad_obs(k, :) = obstacle_grad(q(k, :));
end
grad_obs_flat = grad_obs';
grad_obs_flat = grad_obs_flat(:);

grad_total = grad_smooth + lambda * grad_obs_flat;

A = Kbig + 1e-6 * eye(2 * N);
delta = A \ grad_total;

q_new_flat = q_flat - alpha * delta;
q_new = reshape(q_new_flat, 2, N)';
q = q_new;
end

function c = obstacle_cost(x)
d0 = 0.5;
d = norm(x);
if d < d0
    diff = d0 - d;
    c = 0.5 * diff^2;
else
    c = 0.0;
end
end

function g = obstacle_grad(x)
d0 = 0.5;
d = norm(x);
if d < 1e-8
    g = [0.0, 0.0];
elseif d < d0
    coeff = -(d0 - d) / d;
    g = coeff * x;
else
    g = [0.0, 0.0];
end
end
      

In Simulink, CHOMP or STOMP can be embedded into a MATLAB Function block that updates a reference trajectory for a lower-level tracking controller (e.g., joint-space PD or model-based control) implemented in continuous time.

Mathematica is convenient for symbolic exploration of CHOMP's functional gradient, as well as for numerical optimization. Below is a small script for a 1D trajectory optimizing a smoothness plus obstacle cost functional.

(* Discretized 1D trajectory: internal points q[1..N] between q0 and qT *)
N = 20;
q0 = -1.0; qT = 1.0;

(* Smoothness matrix K *)
buildK[n_] := Module[{K},
  K = ConstantArray[0.0, {n, n}];
  Do[
    K[[k, k]] += 2.0;
    If[k > 1, K[[k, k - 1]] -= 1.0; K[[k - 1, k]] -= 1.0;],
    {k, 1, n}
  ];
  K
];

K = buildK[N];

obstacleCost[x_] := Module[{d0 = 0.5, d = Norm[{x}]},
  If[d < d0,
    0.5 (d0 - d)^2,
    0.0
  ]
];

totalCost[q_List, lambda_] := Module[{qAll, smooth, obs},
  qAll = Join[{q0}, q, {qT}];
  (* Smoothness: finite differences *)
  smooth = 0.0;
  Do[
    With[{acc = qAll[[k]] - 2.0 qAll[[k + 1]] + qAll[[k + 2]]},
      smooth += 0.5 acc^2;
    ],
    {k, 1, N}
  ];
  obs = Total[obstacleCost /@ q];
  smooth + lambda obs
];

lambda = 5.0;

(* Optimize numerically *)
qInit = Table[q0 + (qT - q0) (k/(N + 1)), {k, 1, N}];
sol = FindMinimum[
   totalCost[q, lambda],
   Table[{q[[k]], qInit[[k]]}, {k, 1, N}]
][[2]];

qOpt = q /. sol;
      

This code directly optimizes the discrete trajectory using FindMinimum, which internally performs gradient-based or quasi-Newton updates similar in spirit to CHOMP, but without explicitly exposing the covariance structure \( \mathbf{K}^{-1} \).

Problem 1 (Gradient of Discrete Smoothness Term). Consider a scalar 1D trajectory with internal waypoints \( q_1,\dots,q_{K-2} \) and fixed endpoints \( q_0, q_{K-1} \). Define

\[ \Phi_{\text{smooth}}(\mathbf{q}) = \frac{1}{2} \sum_{k=1}^{K-2} \big(q_{k-1} - 2q_k + q_{k+1}\big)^2. \]

(a) Show that \( \Phi_{\text{smooth}}(\mathbf{q}) \) can be written as \( \frac{1}{2}\mathbf{q}^\top \mathbf{K}\mathbf{q} + \mathbf{b}^\top \mathbf{q} + \text{const} \) with a symmetric tridiagonal matrix \( \mathbf{K} \). (b) Compute the gradient \( \nabla \Phi_{\text{smooth}}(\mathbf{q}) \).

Solution. Introduce the difference operator \( D_k = q_{k-1} - 2q_k + q_{k+1} \). The cost is \( \Phi_{\text{smooth}} = \frac{1}{2}\sum_k D_k^2 \). Each \( D_k \) depends linearly on \( q_{k-1},q_k,q_{k+1} \), so the overall expression is a quadratic form in \( \mathbf{q} \). Collecting coefficients, one finds that \( \mathbf{K} \) is tridiagonal with entries (for interior indices)

\[ K_{kk} = 6,\quad K_{k,k-1} = K_{k-1,k} = -4,\quad K_{k,k-2} = K_{k-2,k} = 1, \]

up to boundary modifications. Fixed endpoints contribute to the linear term \( \mathbf{b} \) and constant offset. Differentiating the quadratic form gives

\[ \nabla \Phi_{\text{smooth}}(\mathbf{q}) = \mathbf{K}\mathbf{q} + \mathbf{b}. \]

This matches the general expression used in CHOMP, where \( \mathbf{K} = \mathbf{A}_{\text{int}}^\top\mathbf{A}_{\text{int}} \) for an appropriate difference matrix \( \mathbf{A}_{\text{int}} \).

Problem 2 (Covariant Steepest Descent Direction). Let \( \mathcal{U}(\mathbf{q}) \) be a differentiable cost function and define the inner product \( \langle \mathbf{u},\mathbf{v} \rangle_{\mathbf{K}} = \mathbf{u}^\top\mathbf{K}\mathbf{v} \) for a symmetric positive definite matrix \( \mathbf{K} \). Show that the direction of steepest descent of \( \mathcal{U} \) with unit \( \mathbf{K} \)-norm is proportional to \( -\mathbf{K}^{-1}\nabla \mathcal{U}(\mathbf{q}) \).

Solution. We seek \( \mathbf{d} \) that minimizes \( \nabla \mathcal{U}(\mathbf{q})^\top \mathbf{d} \) subject to \( \mathbf{d}^\top\mathbf{K}\mathbf{d}=1 \). The Lagrangian is \( L(\mathbf{d},\lambda) = \nabla \mathcal{U}(\mathbf{q})^\top \mathbf{d} + \frac{\lambda}{2}(\mathbf{d}^\top\mathbf{K}\mathbf{d}-1) \). Setting \( \nabla_{\mathbf{d}}L = \mathbf{0} \) yields

\[ \nabla \mathcal{U}(\mathbf{q}) + \lambda \mathbf{K}\mathbf{d} = \mathbf{0} \quad \Rightarrow \quad \mathbf{d} = -\lambda^{-1}\mathbf{K}^{-1}\nabla\mathcal{U}(\mathbf{q}). \]

Normalizing to satisfy the constraint changes only the scalar \( \lambda^{-1} \), so the steepest descent direction is proportional to \( -\mathbf{K}^{-1}\nabla\mathcal{U}(\mathbf{q}) \), which is precisely the CHOMP update direction.

Problem 3 (Obstacle Gradient via Jacobian). For a manipulator with configuration \( \mathbf{q} \in \mathbb{R}^n \), let \( \mathbf{x} = f(\mathbf{q}) \) be the end-effector position and \( c(\mathbf{x}) \) a smooth obstacle cost. Show that the gradient of \( c(f(\mathbf{q})) \) with respect to \( \mathbf{q} \) is \( \mathbf{J}^\top \nabla_{\mathbf{x}} c(\mathbf{x}) \), where \( \mathbf{J} = \partial f / \partial \mathbf{q} \) is the Jacobian.

Solution. The chain rule in multiple dimensions gives

\[ \nabla_{\mathbf{q}} c(f(\mathbf{q})) = \left(\frac{\partial f(\mathbf{q})}{\partial \mathbf{q}}\right)^\top \nabla_{\mathbf{x}} c(\mathbf{x}) = \mathbf{J}^\top \nabla_{\mathbf{x}} c(\mathbf{x}). \]

In the discrete CHOMP obstacle cost, this expression is summed over control points and time steps, leading to the stacked gradient \( \nabla \Phi_{\text{obs}}(\mathbf{q}) \).

Problem 4 (STOMP Update as Approximate Gradient Step). Consider the STOMP update

\[ \Delta \mathbf{q} = \sum_{m=1}^M \tilde{w}^{(m)} \boldsymbol{\epsilon}^{(m)}, \quad \tilde{w}^{(m)} \propto \exp\big(-\tfrac{1}{\eta} \mathcal{U}( \mathbf{q} + \boldsymbol{\epsilon}^{(m)})\big), \]

where \( \boldsymbol{\epsilon}^{(m)} \) are i.i.d. samples from \( \mathcal{N}(\mathbf{0},\boldsymbol{\Sigma}) \). Argue (formally, using a Taylor expansion) that for small noise and small \( \eta \), the expected update \( \mathbb{E}[\Delta \mathbf{q}] \) is approximately proportional to \( -\boldsymbol{\Sigma}\nabla \mathcal{U}(\mathbf{q}) \).

Solution (sketch). Expand \( \mathcal{U}(\mathbf{q} + \boldsymbol{\epsilon}) \) around \( \mathbf{q} \):

\[ \mathcal{U}(\mathbf{q} + \boldsymbol{\epsilon}) \approx \mathcal{U}(\mathbf{q}) + \nabla \mathcal{U}(\mathbf{q})^\top \boldsymbol{\epsilon} + \frac{1}{2} \boldsymbol{\epsilon}^\top \mathbf{H}\boldsymbol{\epsilon}, \]

where \( \mathbf{H} \) is the Hessian. Then \( \exp(-\mathcal{U}(\mathbf{q} + \boldsymbol{\epsilon})/\eta) \) is approximately

\[ \exp(-\mathcal{U}(\mathbf{q})/\eta) \exp\!\left( -\frac{1}{\eta} \nabla \mathcal{U}(\mathbf{q})^\top\boldsymbol{\epsilon} \right), \]

neglecting quadratic terms for small noise. This defines an exponentially tilted Gaussian distribution whose mean shift is proportional to \( -\boldsymbol{\Sigma}\nabla \mathcal{U}(\mathbf{q}) \). Thus, the expected value of \( \boldsymbol{\epsilon} \) under this tilted distribution is approximately \( -c\,\boldsymbol{\Sigma}\nabla \mathcal{U}(\mathbf{q}) \) for some positive scalar \( c \) depending on \( \eta \), so \( \mathbb{E}[\Delta \mathbf{q}] \propto -\boldsymbol{\Sigma}\nabla \mathcal{U}(\mathbf{q}) \). This connects STOMP with a preconditioned gradient descent method, similar to CHOMP.

In this lesson we specialized the general trajectory optimization formulation to two concrete planners: CHOMP and STOMP. We derived CHOMP's smoothness and obstacle cost functionals, expressed the smoothness term as a quadratic form defined by a banded matrix \( \mathbf{K} \), and showed why the covariant gradient direction \( -\mathbf{K}^{-1}\nabla \mathcal{U} \) is the steepest descent under a geometry induced by smoothness. We then described STOMP's sampling-based update rule, and argued that, in the small-noise limit, it recovers a preconditioned gradient descent behavior without explicitly computing gradients. Finally, we sketched implementations in multiple programming environments, establishing a bridge between the mathematical formulation and practical planners for manipulators and other robots.