Consider an \( n \)-DOF manipulator with joint trajectory \( \mathbf{q}(t) \in \mathbb{R}^n \), sampled at discrete times \( t_k = k h \), \( k = 0,\dots,N \), with step size \( h > 0 \). We denote \( \mathbf{q}_k = \mathbf{q}(t_k) \). A typical optimization-based planner searches for internal waypoints \( \mathbf{q}_1,\dots,\mathbf{q}_{N-1} \), given fixed boundary conditions \( \mathbf{q}_0 = \mathbf{q}_{\mathrm{start}} \) and \( \mathbf{q}_N = \mathbf{q}_{\mathrm{goal}} \), by solving

\[ \min_{\mathbf{q}_1,\dots,\mathbf{q}_{N-1}} \ J(\mathbf{q}_0,\dots,\mathbf{q}_N) = \lambda_{\mathrm{smooth}} J_{\mathrm{smooth}} + \lambda_{\mathrm{col}} J_{\mathrm{col}} \]

subject to boundary conditions and possibly joint, velocity, and torque limits. The two key contributions to the cost are:

• Smoothness cost \( J_{\mathrm{smooth}} \), typically penalizing velocities, accelerations, or jerks to obtain dynamically reasonable motions.
• Collision cost \( J_{\mathrm{col}} \), penalizing proximity to obstacles or enforcing hard separation constraints to maintain safety margins.

These terms interact strongly: aggressive collision avoidance without smoothness can produce jerky, dynamically infeasible motions; excessive smoothing can reduce clearance. Optimization-based planners explicitly trade these effects by a cost functional and its gradient.

flowchart TD
  A["Start and goal configurations q_start, q_goal"] --> B["Initial discrete path \n(e.g. straight in joint space)"]
  B --> C["Evaluate smoothness cost J_smooth"]
  B --> D["Evaluate collision cost J_col \nusing signed distance"]
  C --> E["Combine into total \ncost J = lambda_smooth * J_smooth + lambda_col * J_col"]
  D --> E
  E --> F["Compute gradients wrt all waypoints"]
  F --> G["Update waypoints and \nenforce joint limits"]
  G --> H["Check collision-free \nand convergence criteria"]
  H -->|not converged| C
  H -->|converged| I["Output optimized trajectory"]
        

A standard continuous-time smoothness functional penalizes squared accelerations:

\[ J_{\mathrm{smooth}}^{\mathrm{cont}}(\mathbf{q}) = \frac{1}{2}\int_0^T \left\Vert \ddot{\mathbf{q}}(t) \right\Vert^2 \,\mathrm{d}t. \]

Using a second-order finite-difference approximation \( \ddot{\mathbf{q}}(t_k) \approx (\mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1}) / h^2 \) we obtain a discrete smoothness cost

\[ J_{\mathrm{smooth}} = \frac{1}{2} \sum_{k=1}^{N-1} \left\Vert \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \right\Vert^2. \]

Stacking all waypoints (including boundaries) into a single vector \( \mathbf{q} \in \mathbb{R}^{n(N+1)} \), we may write

\[ J_{\mathrm{smooth}} = \frac{1}{2}\,\mathbf{q}^\top \mathbf{H} \mathbf{q} \]

where \( \mathbf{H} \) is a symmetric, banded, positive semidefinite matrix constructed from the finite-difference operator. When the boundary waypoints \( \mathbf{q}_0, \mathbf{q}_N \) are fixed and only the internal waypoints are optimized, the corresponding reduced Hessian is positive definite.

Proof sketch (positive definiteness on internal nodes).

1. Let \( \Delta^2 \mathbf{q}_k = \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \). Then \( J_{\mathrm{smooth}} = 0 \) implies \( \Delta^2 \mathbf{q}_k = \mathbf{0} \) for all \( k = 1,\dots,N-1 \).
2. The recurrence \( \Delta^2 \mathbf{q}_k = \mathbf{0} \) implies that each joint trajectory component is affine in \( k \), i.e., \( \mathbf{q}_k = \mathbf{a} k + \mathbf{b} \).
3. Fixing \( \mathbf{q}_0 \) and \( \mathbf{q}_N \) uniquely determines \( \mathbf{a}, \mathbf{b} \), leaving no degrees of freedom among internal nodes. Thus, among trajectories respecting boundary conditions, the only vector with \( J_{\mathrm{smooth}} = 0 \) is the unique straight-line interpolation, and the reduced Hessian is positive definite.

This guarantees that \( J_{\mathrm{smooth}} \) defines a strictly convex quadratic penalty on internal waypoints. In practice, the banded structure of \( \mathbf{H} \) is crucial for efficient solvers (e.g., Cholesky factorization in \( \mathcal{O}(N n^3) \) instead of \( \mathcal{O}(N^3 n^3) \)).

Let \( \mathcal{X} \subset \mathbb{R}^3 \) be the workspace with obstacles \( \mathcal{O} \subset \mathcal{X} \). A signed distance field is a function \( d : \mathcal{X} \to \mathbb{R} \) such that

• \( d(\mathbf{x}) > 0 \) if \( \mathbf{x} \) is in free space,
• \( d(\mathbf{x}) = 0 \) on the obstacle boundary,
• \( d(\mathbf{x}) < 0 \) inside obstacles.

For each waypoint \( \mathbf{q}_k \), choose a finite set of body points \( \mathbf{x}_{k,i} = f_i(\mathbf{q}_k) \) obtained by forward kinematics. For a clearance parameter \( \varepsilon > 0 \), define a scalar penalty function

\[ \phi(d) = \begin{cases} 0, & d \ge \varepsilon,\\[4pt] \frac{1}{2}(\varepsilon - d)^2, & d < \varepsilon. \end{cases} \]

The collision cost is then

\[ J_{\mathrm{col}} = \sum_{k=0}^N \sum_{i} \phi\big( d(\mathbf{x}_{k,i}) \big). \]

Assuming \( d \) is differentiable in a neighborhood of any \( \mathbf{x}_{k,i} \) with \( d(\mathbf{x}_{k,i}) \le \varepsilon \), and letting \( \mathbf{n}_{k,i} = \nabla_{\mathbf{x}} d(\mathbf{x}_{k,i}) \) denote the outward unit normal to the obstacle boundary, we compute the gradient with respect to a waypoint \( \mathbf{q}_k \) as

\[ \nabla_{\mathbf{q}_k} J_{\mathrm{col}} = \sum_{i : d(\mathbf{x}_{k,i}) < \varepsilon} \phi'\big(d(\mathbf{x}_{k,i})\big) \, \mathbf{J}_{k,i}^\top \mathbf{n}_{k,i}, \]

where \( \mathbf{J}_{k,i} = \frac{\partial \mathbf{x}_{k,i}}{\partial \mathbf{q}_k} \) is the Jacobian of the body point with respect to the joint coordinates, and \( \phi'(d) = -( \varepsilon - d ) \) for \( d < \varepsilon \), zero otherwise.

This structure is characteristic of CHOMP-like planners: the collision gradient is the sum over body points of jacobian-transposed workspace forces \( \mathbf{n}_{k,i} \) scaled by penetration depth \( \varepsilon - d \).

Combining the previous sections and treating only the internal waypoints as decision variables, we obtain a finite-dimensional nonlinear program:

\[ \begin{aligned} \min_{\mathbf{q}_1,\dots,\mathbf{q}_{N-1}} \quad & \lambda_{\mathrm{smooth}} \frac{1}{2} \sum_{k=1}^{N-1} \left\Vert \mathbf{q}_{k+1}-2\mathbf{q}_k+\mathbf{q}_{k-1} \right\Vert^2 + \lambda_{\mathrm{col}} \sum_{k,i} \phi\big(d(\mathbf{x}_{k,i})\big)\\[4pt] \text{s.t.} \quad & \mathbf{q}_0 = \mathbf{q}_{\mathrm{start}},\quad \mathbf{q}_N = \mathbf{q}_{\mathrm{goal}},\\ & \mathbf{q}_{\min} \le \mathbf{q}_k \le \mathbf{q}_{\max}, \quad k=0,\dots,N,\\ & \text{(optional) dynamic and torque limits at discrete times.} \end{aligned} \]

The Karush–Kuhn–Tucker (KKT) optimality conditions couple:

• the stationarity condition \( \nabla J + \text{constraint gradients}^\top \boldsymbol{\lambda} = 0 \),
• primal feasibility for equality and inequality constraints,
• complementary slackness for inequalities.

In many trajectory optimizers used in robotics, inequality constraints such as joint limits and obstacle clearance are handled via penalty functions or barrier methods, trading strict feasibility for numerical robustness and implementation simplicity.

Let \( \mathbf{q} \) denote the stacked vector of internal waypoints, and suppose \( J_{\mathrm{smooth}} = \frac{1}{2}\mathbf{q}^\top \mathbf{H} \mathbf{q} \) with banded positive-definite \( \mathbf{H} \), and \( J_{\mathrm{col}} \) is differentiable. Then

\[ \nabla J(\mathbf{q}) = \lambda_{\mathrm{smooth}} \mathbf{H}\mathbf{q} + \lambda_{\mathrm{col}} \nabla J_{\mathrm{col}}(\mathbf{q}). \]

A simple gradient descent update is

\[ \mathbf{q}^{(\ell+1)} = \mathbf{q}^{(\ell)} - \alpha \nabla J(\mathbf{q}^{(\ell)}), \quad \alpha > 0. \]

CHOMP introduces a Riemannian metric based on the smoothness functional, using a preconditioned update

\[ \mathbf{q}^{(\ell+1)} = \mathbf{q}^{(\ell)} - \alpha \mathbf{H}^{-1} \nabla J(\mathbf{q}^{(\ell)}), \]

which accelerates convergence by approximately solving a local quadratic subproblem defined by \( \mathbf{H} \). The system \( \mathbf{H} \mathbf{s} = \nabla J \) can be solved efficiently thanks to bandedness.

After each iteration, joint limits and boundary conditions are enforced by projection:

\[ \mathbf{q}_k^{(\ell+1)} \leftarrow \min\big( \mathbf{q}_{\max}, \max(\mathbf{q}_{\min},\,\mathbf{q}_k^{(\ell+1)}) \big), \]

with \( \mathbf{q}_0 \) and \( \mathbf{q}_N \) reset to the prescribed boundary conditions at every step.

flowchart TD
  S["Initial path q(0) (feasible or nearly feasible)"] --> G1["Compute gradients: H q and grad J_col"]
  G1 --> L1["Solve banded system H s = grad J for preconditioned step"]
  L1 --> U1["Update waypoints: q_new = q_old - alpha * s"]
  U1 --> P1["Project onto joint limits and fix endpoints"]
  P1 --> C1["Check cost decrease and collision status"]
  C1 -->|not converged| G1
  C1 -->|converged| DONE["Return optimized trajectory"]
        

While penalty terms are convenient, certain constraints must be enforced strictly, e.g., joint limits and minimum clearance margins. A common approach is to:

• treat joint limits via projection after each iterate (as above),
• treat collision margins via a combination of soft penalties and inflated obstacles (add a buffer to the signed distance field).

For instance, instead of requiring \( d(\mathbf{x}) \ge 0 \), one may enforce \( d(\mathbf{x}) \ge \delta \) with \( \delta > 0 \) and use the penalty function \( \phi \) only for \( d(\mathbf{x}) \le \delta \). This ensures that minimizers of the penalized problem lie strictly inside a safety tube around obstacles.

Alternatively, a logarithmic barrier

\[ \psi(d) = -\log(d - \delta) \quad \text{for } d > \delta \]

enforces strict feasibility \( d(\mathbf{x}) > \delta \) by sending the cost to infinity as the clearance approaches \( \delta \). Barrier methods are slightly more delicate numerically but provide better theoretical guarantees of feasibility.

We now implement a simple gradient-based optimizer in Python for a planar 2-DOF arm with one circular obstacle. In practice, you would use libraries such as roboticstoolbox-python or pinocchio for forward kinematics and Jacobians, but here we implement them from scratch to expose all mathematical components.

import numpy as np

# Robot parameters: planar 2-DOF arm
L1, L2 = 0.7, 0.7  # link lengths

def fk_end_effector(q):
    """
    Forward kinematics for planar 2R arm.
    q: array-like of shape (2,)
    returns np.array([x, y])
    """
    q1, q2 = q
    x = L1 * np.cos(q1) + L2 * np.cos(q1 + q2)
    y = L1 * np.sin(q1) + L2 * np.sin(q1 + q2)
    return np.array([x, y])

def jacobian_end_effector(q):
    """
    Geometric Jacobian for the end-effector in 2D.
    """
    q1, q2 = q
    s1 = np.sin(q1)
    c1 = np.cos(q1)
    s12 = np.sin(q1 + q2)
    c12 = np.cos(q1 + q2)

    # J is 2x2
    J = np.zeros((2, 2))
    J[0, 0] = -L1 * s1 - L2 * s12
    J[0, 1] = -L2 * s12
    J[1, 0] =  L1 * c1 + L2 * c12
    J[1, 1] =  L2 * c12
    return J

# Obstacle: circle centered at (0.5, 0.0) with radius 0.25
obs_center = np.array([0.5, 0.0])
obs_radius = 0.25
clearance = 0.05  # desired clearance

def signed_distance_circle(x):
    """
    Signed distance from a point x in R^2 to a circle.
    Positive outside, negative inside.
    """
    diff = x - obs_center
    norm = np.linalg.norm(diff)
    return norm - obs_radius

def collision_cost_and_grad(path):
    """
    path: ndarray of shape (N+1, 2) with waypoints (including endpoints).
    Returns J_col, grad_col (same shape as path).
    Only internal waypoints are penalized for simplicity.
    """
    N_plus_1, dof = path.shape
    grad = np.zeros_like(path)
    cost = 0.0

    for k in range(1, N_plus_1 - 1):
        qk = path[k]
        xk = fk_end_effector(qk)
        d = signed_distance_circle(xk)
        if d < clearance:
            # hinge penalty phi(d) = 0.5 * (clearance - d)^2
            phi = 0.5 * (clearance - d) ** 2
            cost += phi

            # gradient wrt q_k: phi'(d) * dd/dq
            # phi'(d) = -(clearance - d)
            dphi_dd = -(clearance - d)

            diff = xk - obs_center
            norm = np.linalg.norm(diff)
            if norm > 1e-6:
                n = diff / norm  # workspace normal
                J = jacobian_end_effector(qk)  # 2x2
                # dd/dq = n^T J
                grad_k = dphi_dd * (J.T @ n)
                grad[k] += grad_k
    return cost, grad

def smoothness_cost_and_grad(path):
    """
    Quadratic acceleration penalty using second differences.
    path: ndarray of shape (N+1, dof).
    """
    N_plus_1, dof = path.shape
    grad = np.zeros_like(path)
    cost = 0.0

    for k in range(1, N_plus_1 - 1):
        ddq = path[k + 1] - 2.0 * path[k] + path[k - 1]
        cost += 0.5 * np.dot(ddq, ddq)

        # Gradient contributions:
        grad[k - 1] += -ddq
        grad[k]     +=  2.0 * ddq
        grad[k + 1] += -ddq

    return cost, grad

def project_joint_limits(path, q_min, q_max, q_start, q_goal):
    """
    Clamp each waypoint to joint limits and fix endpoints.
    """
    path[0] = q_start
    path[-1] = q_goal
    path[1:-1] = np.minimum(q_max, np.maximum(q_min, path[1:-1]))
    return path

# Trajectory initialization
N = 40  # number of segments
q_start = np.array([-0.5, 1.0])
q_goal  = np.array([1.0, -0.5])
path = np.linspace(q_start, q_goal, N + 1)

q_min = np.array([-np.pi, -np.pi])
q_max = np.array([ np.pi,  np.pi])

lambda_smooth = 1.0
lambda_col = 10.0
alpha = 0.01

for it in range(200):
    Js, gs = smoothness_cost_and_grad(path)
    Jc, gc = collision_cost_and_grad(path)

    J_total = lambda_smooth * Js + lambda_col * Jc
    grad_total = lambda_smooth * gs + lambda_col * gc

    # Gradient descent on internal nodes only
    path[1:-1] -= alpha * grad_total[1:-1]

    # Enforce limits and boundaries
    path = project_joint_limits(path, q_min, q_max, q_start, q_goal)

    if it % 20 == 0:
        print(f"Iter {it}: J = {J_total:.4f}, J_smooth = {Js:.4f}, J_col = {Jc:.4f}")
      

This toy example already illustrates the interplay between smoothness and collision costs. Adding a banded linear solver for the smoothness term and using a more complete set of body points would bring the code closer to CHOMP and TrajOpt-style implementations.

In C++, banded quadratic terms and collision distances are often implemented with Eigen for linear algebra and FCL or similar libraries for collision queries. The following sketch focuses on constructing the smoothness gradient and embedding collision forces returned from a signed distance query.

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

// Waypoint trajectory for an n-DOF robot
struct Trajectory {
    std::vector<Eigen::VectorXd> q; // size N_plus_1, each of dim n
};

// Build smoothness cost and gradient using second differences
double smoothnessCostAndGrad(const Trajectory& traj,
                             std::vector<Eigen::VectorXd>& grad) {
    const int N_plus_1 = static_cast<int>(traj.q.size());
    const int n = static_cast<int>(traj.q[0].size());
    grad.assign(N_plus_1, Eigen::VectorXd::Zero(n));

    double cost = 0.0;
    for (int k = 1; k < N_plus_1 - 1; ++k) {
        Eigen::VectorXd ddq = traj.q[k + 1] - 2.0 * traj.q[k] + traj.q[k - 1];
        cost += 0.5 * ddq.squaredNorm();

        grad[k - 1] += -ddq;
        grad[k]     +=  2.0 * ddq;
        grad[k + 1] += -ddq;
    }
    return cost;
}

// Collision interface (to be implemented using, e.g., FCL)
struct CollisionResult {
    double cost;
    std::vector<Eigen::VectorXd> grad; // same size as traj.q
};

// Placeholder: in a real system, call FCL or another library
CollisionResult collisionCostAndGrad(const Trajectory& traj) {
    CollisionResult res;
    const int N_plus_1 = static_cast<int>(traj.q.size());
    const int n = static_cast<int>(traj.q[0].size());
    res.cost = 0.0;
    res.grad.assign(N_plus_1, Eigen::VectorXd::Zero(n));

    // Example: assume we have a function
    //   querySignedDistanceAndDirection(q, d, normal)
    // for each waypoint that returns signed distance and workspace normal.
    //
    // Here we just leave a stub to show how the gradient would be used.
    //
    // double clearance = 0.05;
    // for (int k = 1; k < N_plus_1 - 1; ++k) {
    //     double d;
    //     Eigen::VectorXd grad_q = Eigen::VectorXd::Zero(n);
    //     // fill d and grad_q using collision library + robot Jacobians
    //     if (d < clearance) {
    //         double phi = 0.5 * (clearance - d) * (clearance - d);
    //         res.cost += phi;
    //         double dphi_dd = -(clearance - d);
    //         res.grad[k] += dphi_dd * grad_q;
    //     }
    // }

    return res;
}

int main() {
    const int N = 40;
    const int n = 2;

    Trajectory traj;
    traj.q.resize(N + 1);
    Eigen::Vector2d q_start(-0.5, 1.0);
    Eigen::Vector2d q_goal(1.0, -0.5);

    for (int k = 0; k <= N; ++k) {
        double alpha = static_cast<double>(k) / N;
        traj.q[k] = (1.0 - alpha) * q_start + alpha * q_goal;
    }

    double lambda_smooth = 1.0;
    double lambda_col = 10.0;
    double alpha_step = 0.01;

    std::vector<Eigen::VectorXd> grad_s, grad_total;
    for (int it = 0; it < 100; ++it) {
        double Js = smoothnessCostAndGrad(traj, grad_s);
        CollisionResult col = collisionCostAndGrad(traj);

        grad_total = grad_s;
        for (int k = 0; k <= N; ++k) {
            grad_total[k] =
                lambda_smooth * grad_s[k] + lambda_col * col.grad[k];
        }

        // Gradient descent on internal nodes
        for (int k = 1; k < N; ++k) {
            traj.q[k] -= alpha_step * grad_total[k];
        }

        std::cout << "Iter " << it
                  << ": Js = " << Js
                  << ", Jc = " << col.cost << std::endl;
    }

    return 0;
}
      

In a full implementation, the collision function would call FCL to compute signed distances and contact normals between robot links and obstacles, and use the robot Jacobian to map workspace forces back to joint-space gradients.

Java is less commonly used for low-level manipulator control, but the same optimization ideas apply. Here we implement a 2D point robot with smoothness and collision penalties, using plain arrays. Linear algebra libraries such as EJML or ojAlgo can be used for more complex robots.

public class PointTrajectoryOptimizer {

    static class Path {
        double[][] p; // shape (N_plus_1, 2)
    }

    static double[] circleCenter = new double[]{0.5, 0.0};
    static double circleRadius = 0.25;
    static double clearance = 0.05;

    static double smoothnessCostAndGrad(Path path, double[][] grad) {
        int N_plus_1 = path.p.length;
        double cost = 0.0;

        for (int k = 1; k < N_plus_1 - 1; ++k) {
            double ddx = path.p[k + 1][0] - 2.0 * path.p[k][0] + path.p[k - 1][0];
            double ddy = path.p[k + 1][1] - 2.0 * path.p[k][1] + path.p[k - 1][1];

            double ddNorm2 = ddx * ddx + ddy * ddy;
            cost += 0.5 * ddNorm2;

            grad[k - 1][0] += -ddx;
            grad[k - 1][1] += -ddy;

            grad[k][0]     +=  2.0 * ddx;
            grad[k][1]     +=  2.0 * ddy;

            grad[k + 1][0] += -ddx;
            grad[k + 1][1] += -ddy;
        }
        return cost;
    }

    static double collisionCostAndGrad(Path path, double[][] grad) {
        int N_plus_1 = path.p.length;
        double cost = 0.0;

        for (int k = 1; k < N_plus_1 - 1; ++k) {
            double dx = path.p[k][0] - circleCenter[0];
            double dy = path.p[k][1] - circleCenter[1];
            double norm = Math.sqrt(dx * dx + dy * dy);
            double d = norm - circleRadius;

            if (d < clearance) {
                double phi = 0.5 * (clearance - d) * (clearance - d);
                cost += phi;
                double dphi_dd = -(clearance - d);

                if (norm > 1e-6) {
                    double nx = dx / norm;
                    double ny = dy / norm;

                    // dd/dx = nx, dd/dy = ny
                    grad[k][0] += dphi_dd * nx;
                    grad[k][1] += dphi_dd * ny;
                }
            }
        }
        return cost;
    }

    public static void main(String[] args) {
        int N = 40;
        Path path = new Path();
        path.p = new double[N + 1][2];

        double[] start = new double[]{-0.5, 0.0};
        double[] goal  = new double[]{ 1.0, 0.0};

        for (int k = 0; k <= N; ++k) {
            double alpha = (double) k / N;
            path.p[k][0] = (1.0 - alpha) * start[0] + alpha * goal[0];
            path.p[k][1] = (1.0 - alpha) * start[1] + alpha * goal[1];
        }

        double lambdaSmooth = 1.0;
        double lambdaCol = 10.0;
        double alphaStep = 0.01;

        for (int it = 0; it < 200; ++it) {
            double[][] grad = new double[N + 1][2];

            double Js = smoothnessCostAndGrad(path, grad);
            double Jc = collisionCostAndGrad(path, grad);

            // Gradient descent on internal points
            for (int k = 1; k < N; ++k) {
                path.p[k][0] -= alphaStep * (lambdaSmooth * grad[k][0] + lambdaCol * grad[k][0]);
                path.p[k][1] -= alphaStep * (lambdaSmooth * grad[k][1] + lambdaCol * grad[k][1]);
            }

            if (it % 20 == 0) {
                System.out.println("Iter " + it + ": Js = " + Js + ", Jc = " + Jc);
            }
        }
    }
}
      

Extending this to manipulators requires replacing direct workspace coordinates with forward kinematics and Jacobians, as in the Python and C++ examples, but the optimization structure remains identical.

MATLAB, with the Robotics System Toolbox, provides high-level functions for kinematics and collision checking (e.g., rigidBodyTree, checkCollision). Simulink can wrap the optimization loop using MATLAB Function blocks. Below is a MATLAB script illustrating one iteration of a trajectory optimization in configuration space.

% Define a 2-DOF planar manipulator in MATLAB
robot = rigidBodyTree("DataFormat","row","MaxNumBodies",3);

L1 = 0.7; L2 = 0.7;

body1 = rigidBody("link1");
joint1 = rigidBodyJoint("joint1","revolute");
setFixedTransform(joint1,trvec2tform([0 0 0]));
body1.Joint = joint1;
addBody(robot,body1,"base");

body2 = rigidBody("link2");
joint2 = rigidBodyJoint("joint2","revolute");
setFixedTransform(joint2,axang2tform([0 0 1 0]) * trvec2tform([L1 0 0]));
body2.Joint = joint2;
addBody(robot,body2,"link1");

tool = rigidBody("tool");
setFixedTransform(tool.Joint,trvec2tform([L2 0 0]));
addBody(robot,tool,"link2");

% Obstacle as collision object
obs = collisionSphere(0.25);
obs.Pose = trvec2tform([0.5 0 0]);

% Trajectory discretization
N = 40;
qStart = [-0.5 1.0];
qGoal  = [ 1.0 -0.5];
q = zeros(N+1,2);
for k = 1:(N+1)
    alpha = (k-1)/N;
    q(k,:) = (1-alpha)*qStart + alpha*qGoal;
end

lambdaSmooth = 1.0;
lambdaCol = 10.0;
alphaStep = 0.01;
clearance = 0.05;

for it = 1:200
    grad = zeros(size(q));
    J_s = 0;
    J_c = 0;

    % Smoothness term
    for k = 2:N
        ddq = q(k+1,:) - 2*q(k,:) + q(k-1,:);
        J_s = J_s + 0.5 * ddq * ddq.';
        grad(k-1,:) = grad(k-1,:) - ddq;
        grad(k,:)   = grad(k,:)   + 2*ddq;
        grad(k+1,:) = grad(k+1,:) - ddq;
    end

    % Collision term
    for k = 2:N
        config = q(k,:);
        [isColliding,sepDist,~,~] = checkCollision(robot,config,obs, ...
            "IgnoreSelfCollision","on","Exhaustive","off");
        d = sepDist;  % signed distance; < 0 if penetrating

        if d < clearance
            phi = 0.5*(clearance - d)^2;
            J_c = J_c + phi;

            % Approximate gradient using numerical differentiation on joint space
            epsFD = 1e-5;
            gq = zeros(1,2);
            for j = 1:2
                dq = zeros(1,2); dq(j) = epsFD;
                [~,dPlus]  = checkCollision(robot,config + dq,obs);
                [~,dMinus] = checkCollision(robot,config - dq,obs);
                dd_dq = (dPlus - dMinus) / (2*epsFD);
                gq(j) = -(clearance - d) * dd_dq;
            end
            grad(k,:) = grad(k,:) + gq;
        end
    end

    % Gradient step on internal nodes
    for k = 2:N
        q(k,:) = q(k,:) - alphaStep * (lambdaSmooth * grad(k,:) + lambdaCol * grad(k,:));
    end

    % Fix boundary conditions
    q(1,:)   = qStart;
    q(end,:) = qGoal;

    if mod(it,20) == 0
        fprintf("Iter %d: J_s = %.4f, J_c = %.4f\n",it,J_s,J_c);
    end
end
      

In Simulink, the above loop can be encapsulated in a MATLAB Function block and triggered at a lower rate than the real-time control loop, yielding an online trajectory refinement scheme.

Mathematica can express the smoothness and collision functionals symbolically and pass them to NMinimize or FindMinimum. Below is a simple point-robot example that mirrors the Java implementation and demonstrates how to encode both the cost and inequality constraints.

(* Number of segments and waypoints *)
N = 10;
(* Variables: 2D positions at each waypoint *)
qVars = Table[{Subscript[x, k], Subscript[y, k]}, {k, 0, N}];

(* Boundary conditions *)
start = {-0.5, 0.};
goal  = { 1.0, 0.};

(* Smoothness cost: second differences *)
smoothness[q_] := Module[{cost = 0.},
  Do[
    With[{pPrev = q[[k]], p = q[[k + 1]], pNext = q[[k + 2]]},
      With[{dd = pNext - 2 p + pPrev},
        cost += 0.5 Norm[dd]^2;
      ]
    ],
    {k, 1, N - 1}
  ];
  cost
];

(* Collision cost: circle obstacle *)
circleCenter = {0.5, 0.};
circleRadius = 0.25;
clearance = 0.05;

signedDistanceCircle[p_] := Norm[p - circleCenter] - circleRadius;

collision[q_] := Module[{cost = 0., d},
  Do[
    d = signedDistanceCircle[q[[k]]];
    If[d < clearance,
      cost += 0.5 (clearance - d)^2;
    ],
    {k, 1, N}
  ];
  cost
];

lambdaSmooth = 1.;
lambdaCol = 10.;

totalCost[q_] := lambdaSmooth*smoothness[q] + lambdaCol*collision[q];

(* Assemble variable list and constraints *)
vars = Flatten[qVars];

constraints = Join[
  Thread[qVars[[1]] == start],
  Thread[qVars[[-1]] == goal]
];

sol = NMinimize[
  {totalCost[qVars], constraints},
  vars
];

sol
      

For manipulators, each \( \mathbf{q}_k \) becomes a joint vector, and the collision term is built from forward kinematics and signed distances as in previous sections. Mathematica is particularly useful for symbolic derivations of gradients and Hessians, which can then be exported to other languages.

Problem 1 (Convexity of the Discrete Smoothness Cost). Consider the discrete smoothness functional \( J_{\mathrm{smooth}} = \frac{1}{2}\sum_{k=1}^{N-1} \left\Vert \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \right\Vert^2 \), with fixed boundary points \( \mathbf{q}_0 \) and \( \mathbf{q}_N \). Show that \( J_{\mathrm{smooth}} \) is strictly convex in the internal waypoints \( \mathbf{q}_1,\dots,\mathbf{q}_{N-1} \).

Solution. We already saw that \( J_{\mathrm{smooth}} \) is quadratic in the stacked vector of all waypoints, hence convex. Strict convexity on the internal waypoints follows from the fact that \( J_{\mathrm{smooth}}(\mathbf{q}) = 0 \) implies \( \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} = \mathbf{0} \) for all \( k \), so each joint trajectory component is affine in \( k \). With fixed boundary values, there is a unique affine trajectory, hence the reduced Hessian has trivial nullspace and is positive definite. Therefore \( J_{\mathrm{smooth}} \) is strictly convex in internal waypoints.

Problem 2 (Gradient of a Signed-Distance Collision Penalty). Let \( \mathbf{x}(\mathbf{q}) \) be a differentiable body point of a robot, with Jacobian \( \mathbf{J}(\mathbf{q}) = \frac{\partial \mathbf{x}}{\partial \mathbf{q}} \). Let \( d(\mathbf{x}) \) be a differentiable signed distance field with gradient \( \nabla_{\mathbf{x}} d(\mathbf{x}) = \mathbf{n}(\mathbf{x}) \). For the scalar penalty \( c(\mathbf{q}) = \phi(d(\mathbf{x}(\mathbf{q}))) \), where \( \phi \) is differentiable on the region of interest, derive \( \nabla_{\mathbf{q}} c(\mathbf{q}) \).

Solution. Apply the chain rule:

\[ \nabla_{\mathbf{q}} c(\mathbf{q}) = \phi'\big(d(\mathbf{x}(\mathbf{q}))\big) \nabla_{\mathbf{q}} d(\mathbf{x}(\mathbf{q})) = \phi'\big(d(\mathbf{x}(\mathbf{q}))\big) \mathbf{J}(\mathbf{q})^\top \nabla_{\mathbf{x}} d(\mathbf{x}(\mathbf{q})) = \phi'(d)\,\mathbf{J}(\mathbf{q})^\top \mathbf{n}(\mathbf{x}). \]

In particular, for the hinge penalty \( \phi(d) = \frac{1}{2}(\varepsilon - d)^2 \) when \( d < \varepsilon \) and zero otherwise, we have \( \phi'(d) = -(\varepsilon - d) \) in the active region.

Problem 3 (Projection onto Box Joint Limits). Let \( \mathcal{B} = \{\mathbf{q} \in \mathbb{R}^n : \mathbf{q}_{\min} \le \mathbf{q} \le \mathbf{q}_{\max} \} \) be a box of joint limits. The projection \( P_{\mathcal{B}}: \mathbb{R}^n \to \mathcal{B} \) is defined componentwise by \( (P_{\mathcal{B}}(\mathbf{q}))_i = \min(q_{\max,i},\max(q_{\min,i},q_i)) \). Show that \( P_{\mathcal{B}} \) is nonexpansive, i.e., \( \Vert P_{\mathcal{B}}(\mathbf{q}) - P_{\mathcal{B}}(\mathbf{p}) \Vert \le \Vert \mathbf{q} - \mathbf{p} \Vert \) for all \( \mathbf{q},\mathbf{p} \).

Solution. It suffices to check nonexpansiveness componentwise in one dimension. For \( P(x) = \min(b,\max(a,x)) \), the derivative \( P'(x) \) exists almost everywhere and satisfies \( |P'(x)| \le 1 \) (zero outside the interval \( [a,b] \), one inside). Hence \( |P(x) - P(y)| \le |x - y| \) for all \( x,y \). By the Pythagorean theorem, this inequality extended componentwise yields

\[ \Vert P_{\mathcal{B}}(\mathbf{q}) - P_{\mathcal{B}}(\mathbf{p}) \Vert^2 = \sum_i \big(P(q_i) - P(p_i)\big)^2 \le \sum_i (q_i - p_i)^2 = \Vert \mathbf{q} - \mathbf{p} \Vert^2. \]

Taking square roots proves nonexpansiveness.

Problem 4 (Log-Barrier Gradient for Clearance Constraints). For a body point with signed distance \( d(\mathbf{x}(\mathbf{q})) \), consider the barrier \( \psi(\mathbf{q}) = -\log(d(\mathbf{x}(\mathbf{q})) - \delta) \) defined on the domain \( d(\mathbf{x}(\mathbf{q})) > \delta \). Derive the gradient \( \nabla_{\mathbf{q}} \psi(\mathbf{q}) \).

Solution. Again by the chain rule,

\[ \nabla_{\mathbf{q}} \psi(\mathbf{q}) = -\frac{1}{d(\mathbf{x}(\mathbf{q})) - \delta} \nabla_{\mathbf{q}} d(\mathbf{x}(\mathbf{q})) = -\frac{1}{d(\mathbf{x}(\mathbf{q})) - \delta} \mathbf{J}(\mathbf{q})^\top \nabla_{\mathbf{x}} d(\mathbf{x}(\mathbf{q})) = -\frac{1}{d - \delta}\,\mathbf{J}(\mathbf{q})^\top \mathbf{n}(\mathbf{x}). \]

As \( d \to \delta^+ \), the norm of this gradient diverges, strongly repelling the trajectory from the clearance boundary.

Problem 5 (Discretization of Velocity vs Acceleration Costs). Consider the continuous velocity-based cost \( J_v^{\mathrm{cont}} = \frac{1}{2}\int_0^T \Vert \dot{\mathbf{q}}(t) \Vert^2 \,\mathrm{d}t \) and the acceleration-based cost \( J_a^{\mathrm{cont}} = \frac{1}{2}\int_0^T \Vert \ddot{\mathbf{q}}(t) \Vert^2 \,\mathrm{d}t \). Using first-order finite differences \( \dot{\mathbf{q}}(t_k) \approx (\mathbf{q}_{k+1}-\mathbf{q}_k)/h \) and second-order differences as before, write discrete approximations \( J_v \) and \( J_a \) and compare the bandwidth of their corresponding Hessians.

Solution. The velocity-based discretization is

\[ J_v \approx \frac{h}{2}\sum_{k=0}^{N-1} \left\Vert \frac{\mathbf{q}_{k+1}-\mathbf{q}_k}{h} \right\Vert^2 = \frac{1}{2h}\sum_{k=0}^{N-1} \Vert \mathbf{q}_{k+1}-\mathbf{q}_k \Vert^2, \]

whose Hessian couples only neighboring waypoints, yielding a tridiagonal structure. The acceleration-based discretization \( J_a = J_{\mathrm{smooth}} \) couples triplets of waypoints via \( \mathbf{q}_{k-1},\mathbf{q}_k,\mathbf{q}_{k+1} \), leading to a Hessian with bandwidth 2 (i.e., five nonzero diagonals). Thus, acceleration-based smoothing yields a slightly denser but still banded Hessian, and typically stronger smoothing of curvature than velocity-based penalties.

In this lesson we formalized collision and smoothness terms for optimization-based trajectory planning. The discrete smoothness functional based on second-order finite differences yields a strictly convex quadratic cost with a banded Hessian, enabling efficient preconditioned gradient methods. Collision penalties constructed from signed distance fields and robot Jacobians produce workspace-normal forces mapped into joint space, as in CHOMP and TrajOpt.

We also discussed practical constraint handling via projection and barrier methods, and instantiated the theory across multiple programming environments: Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These ideas are central to modern trajectory optimizers used in advanced manipulators and will be reused throughout subsequent chapters.