TrajOpt (Trajectory Optimization) refers to a family of methods that directly optimize a parameterized trajectory of robot configurations \( \mathbf{q}_0, \dots, \mathbf{q}_N \) to minimize a cost (e.g., smoothness) while satisfying constraints (e.g., collision avoidance, joint limits). In this lesson we focus on the TrajOpt approach based on sequential convex optimization (SCO).

Consider a generic nonconvex trajectory optimization problem in vectorized form:

\[ \min_{\mathbf{x}\in\mathbb{R}^n} f(\mathbf{x}) \quad \text{s.t.}\quad g_i(\mathbf{x}) \le 0,\; i=1,\dots,m,\quad h_j(\mathbf{x}) = 0,\; j=1,\dots,p. \]

Here \( \mathbf{x} \) stacks all decision variables (e.g., all joint waypoints). In TrajOpt, \( f \) typically encodes smoothness and goal-tracking, while \( g_i \) encode collision and joint-limit constraints. These constraints are highly nonconvex in configuration space.

SCO proceeds by iteratively solving a sequence of convex approximations of the original nonconvex problem. At iteration \( \ell \), we linearize nonconvex constraints and possibly approximate the cost, obtaining a convex subproblem that we can solve efficiently using quadratic programming (QP) or second-order cone programming (SOCP).

flowchart TD
  A["Initial trajectory x^0 (e.g., straight line)"] --> B["Build convex approximation around x^ell"]
  B --> C["Convex subproblem (QP / SOCP)"]
  C --> D["Solve with numerical optimizer"]
  D --> E["Update trajectory x^(ell+1)"]
  E --> F{"Convergence check?"}
  F -->|no| B
  F -->|yes| G["Locally optimal trajectory"]
        

Compared with gradient-based CHOMP/STOMP, TrajOpt emphasizes hard constraints and uses mature convex optimization solvers at each iteration, making it attractive for high-DOF robot arms with complex geometry.

We assume the configuration of an n-DOF manipulator at time step \( k \) is \( \mathbf{q}_k \in \mathbb{R}^n \), for \( k = 0,\dots,N \). The stacked trajectory is \( \mathbf{x} = [\mathbf{q}_0^\top, \dots, \mathbf{q}_N^\top]^\top \).

A common smoothness cost penalizes discrete accelerations:

\[ J_{\text{smooth}}(\mathbf{x}) = \sum_{k=1}^{N-1} \left\| \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \right\|_2^2. \]

This is a quadratic function of \( \mathbf{x} \), hence convex:

\[ J_{\text{smooth}}(\mathbf{x}) = \mathbf{x}^\top \mathbf{H}_{\text{smooth}} \mathbf{x}, \quad \mathbf{H}_{\text{smooth}} \succeq 0. \]

We also include a goal-tracking term, e.g., for end-effector pose \( \mathbf{y}_N = \Phi(\mathbf{q}_N) \) and desired pose \( \mathbf{y}^\star \):

\[ J_{\text{goal}}(\mathbf{x}) = \left\| \Phi(\mathbf{q}_N) - \mathbf{y}^\star \right\|_2^2, \]

which is generally nonconvex because of the nonlinear forward kinematics \( \Phi \).

Joint limits and velocity limits can be enforced as simple linear constraints:

\[ \mathbf{q}_{\min} \le \mathbf{q}_k \le \mathbf{q}_{\max}, \quad \left\| \mathbf{q}_{k+1} - \mathbf{q}_k \right\|_\infty \le \Delta v_{\max}. \]

The collision avoidance constraints are more challenging. Let \( d(\mathbf{q}_k) \) denote the signed distance between the robot at configuration \( \mathbf{q}_k \) and the closest obstacle (positive outside obstacles, negative inside). A safety margin \( d_{\min} > 0 \) yields

\[ g_k(\mathbf{q}_k) := d_{\min} - d(\mathbf{q}_k) \le 0, \quad k=0,\dots,N. \]

The overall nonconvex problem is then

\[ \min_{\mathbf{x}} \; J_{\text{smooth}}(\mathbf{x}) + \lambda_{\text{goal}} J_{\text{goal}}(\mathbf{x}) \quad \text{s.t.}\quad g_k(\mathbf{q}_k) \le 0,\; \text{and linear box/velocity constraints.} \]

TrajOpt constructs convex approximations to the nonconvex pieces \( J_{\text{goal}} \) and \( g_k \) around the current trajectory iterate.

Consider again a generic constrained problem. We define a sequence of convex subproblems via local approximations. At iteration \( \ell \), we approximate:

• The cost \( f(\mathbf{x}) \) by a convex \( \hat{f}^{(\ell)}(\mathbf{x}) \).
• The constraint functions \( g_i(\mathbf{x}) \) by convex \( \hat{g}_i^{(\ell)}(\mathbf{x}) \), chosen to be conservative (feasible for the approximation implies feasible for the original).

For a differentiable constraint \( g(\mathbf{x}) \), linearization around \( \mathbf{x}^{(\ell)} \) is

\[ \hat{g}^{(\ell)}(\mathbf{x}) = g(\mathbf{x}^{(\ell)}) + \nabla g(\mathbf{x}^{(\ell)})^\top (\mathbf{x} - \mathbf{x}^{(\ell)}). \]

If we have a distance constraint \( d(\mathbf{x}) \ge d_{\min} \) and \( d \) is convex, then using the first-order inequality for convex functions:

\[ d(\mathbf{x}) \ge d(\mathbf{x}^{(\ell)}) + \nabla d(\mathbf{x}^{(\ell)})^\top(\mathbf{x}-\mathbf{x}^{(\ell)}), \]

the conservative linear constraint

\[ d(\mathbf{x}^{(\ell)}) + \nabla d(\mathbf{x}^{(\ell)})^\top(\mathbf{x}-\mathbf{x}^{(\ell)}) \ge d_{\min} \]

guarantees that any \( \mathbf{x} \) satisfying the linearized constraint also satisfies the original distance constraint, since the left-hand side is a lower bound on the true distance.

To ensure each subproblem remains well-posed, TrajOpt uses trust-region constraints that restrict the step:

\[ \left\| \mathbf{x} - \mathbf{x}^{(\ell)} \right\|_\infty \le \Delta^{(\ell)}. \]

The resulting convex subproblem at iteration \( \ell \) is of the form

\[ \begin{aligned} \min_{\mathbf{x}} \;& \hat{f}^{(\ell)}(\mathbf{x}) \\ \text{s.t.}\;& \hat{g}_i^{(\ell)}(\mathbf{x}) \le 0,\; i=1,\dots,m,\\ & \mathbf{A}\mathbf{x} \le \mathbf{b},\; \mathbf{C}\mathbf{x} = \mathbf{d},\\ & \left\| \mathbf{x} - \mathbf{x}^{(\ell)} \right\|_\infty \le \Delta^{(\ell)}, \end{aligned} \]

which is a QP if \( \hat{f}^{(\ell)} \) is quadratic and constraints are linear.

flowchart TD
  S["Nonconvex problem f,g"] --> L1["Linearize nonconvex constraints at x^ell"]
  L1 --> C1["Form convex QP: quadratic cost + linear constraints"]
  C1 --> SOL["Call QP solver (e.g. OSQP, quadprog)"]
  SOL --> UPD["Obtain x^(ell+1)"]
  UPD --> TR["Adjust trust region Delta^(ell+1)"]
  TR --> L1
        

Under standard conditions (e.g., gradient consistency, sufficient descent, bounded trust-region radius), SCO iterates converge to a first-order stationary point satisfying the Karush–Kuhn–Tucker (KKT) conditions of the original problem.

TrajOpt represents geometry by convex shapes or unions of convex pieces and uses signed distance functions between robot link geometries and environment objects. Let \( \mathbf{p}(\mathbf{q}) \) be a point on a link surface, and \( \mathbf{p}_o \) a point on an obstacle surface with outward normal \( \mathbf{n} \). Local signed distance along the normal is

\[ d(\mathbf{q}) = \mathbf{n}^\top \left( \mathbf{p}(\mathbf{q}) - \mathbf{p}_o \right). \]

Using the manipulator Jacobian \( \mathbf{J}_p(\mathbf{q}) \) for the point \( \mathbf{p}(\mathbf{q}) \), a small configuration change \( \delta \mathbf{q} \) yields

\[ \mathbf{p}(\mathbf{q} + \delta \mathbf{q}) \approx \mathbf{p}(\mathbf{q}) + \mathbf{J}_p(\mathbf{q}) \,\delta \mathbf{q}. \]

Thus, the distance at the perturbed configuration is approximated by

\[ \begin{aligned} d(\mathbf{q} + \delta \mathbf{q}) &\approx \mathbf{n}^\top \left( \mathbf{p}(\mathbf{q}) + \mathbf{J}_p(\mathbf{q})\,\delta \mathbf{q} - \mathbf{p}_o \right) \\ &= d(\mathbf{q}) + \mathbf{n}^\top \mathbf{J}_p(\mathbf{q})\,\delta\mathbf{q}. \end{aligned} \]

To enforce \( d(\mathbf{q}+\delta\mathbf{q}) \ge d_{\min} \), we can impose the linear constraint

\[ d(\mathbf{q}) + \mathbf{n}^\top \mathbf{J}_p(\mathbf{q})\,\delta\mathbf{q} \ge d_{\min}. \]

If \( d(\mathbf{q}) \) is convex in \( \mathbf{q} \) (e.g., distance to a convex obstacle), then this linearization is conservative in the sense discussed in the previous section. In practice, TrajOpt enforces such linearized constraints at multiple support points per link and time step to approximate continuous-time safety.

For multiple collision pairs indexed by \( c \) and time steps \( k \), we obtain a family of linear inequalities

\[ d_{c,k}(\mathbf{q}_k^{(\ell)}) + \mathbf{a}_{c,k}^{(\ell)\top} (\mathbf{q}_k - \mathbf{q}_k^{(\ell)}) \ge d_{\min}, \quad \mathbf{a}_{c,k}^{(\ell)} = \mathbf{J}_{p,c}(\mathbf{q}_k^{(\ell)})^\top \mathbf{n}_{c,k}, \]

which together define a convex polyhedron in the space of configurations for each waypoint \( k \).

TrajOpt often uses a penalized formulation of inequality constraints, e.g.,

\[ F_\rho(\mathbf{x}) = f(\mathbf{x}) + \rho \sum_{i=1}^m \left[\max\{0, g_i(\mathbf{x})\}\right]^2, \]

where \( \rho > 0 \) is a penalty parameter. For a large enough \( \rho \), minimizers of \( F_\rho \) approximate constrained minimizers. In practice, we solve a sequence of convex approximations of \( F_\rho \) with increasing penalty.

At a local optimum \( \mathbf{x}^\star \) of the constrained problem, KKT conditions state that there exist multipliers \( \lambda_i \ge 0 \) and \( \nu_j \) such that

\[ \begin{aligned} &\nabla f(\mathbf{x}^\star) + \sum_{i=1}^m \lambda_i \nabla g_i(\mathbf{x}^\star) + \sum_{j=1}^p \nu_j \nabla h_j(\mathbf{x}^\star) = \mathbf{0}, \\ &g_i(\mathbf{x}^\star) \le 0,\quad \lambda_i \ge 0,\quad \lambda_i g_i(\mathbf{x}^\star) = 0,\; i=1,\dots,m, \\ &h_j(\mathbf{x}^\star)=0,\; j=1,\dots,p. \end{aligned} \]

Sequential convex programming algorithms can be shown (under regularity assumptions) to generate a sequence \( \mathbf{x}^{(\ell)} \) whose limit points satisfy these KKT conditions. Intuitively, linearized constraints and convex cost approximations enforce local stationarity and feasibility, while the trust region controls the validity of the approximations.

We now implement a simplified TrajOpt for a point robot in 2D moving from \( \mathbf{q}_0 \) to \( \mathbf{q}_N \) around circular obstacles. We use numpy and cvxpy. For manipulators, the same pattern applies with forward kinematics and Jacobians (available from libraries like pyrobot, pinocchio, or ros2_control stacks).

import numpy as np
import cvxpy as cp

def signed_distance_point_circle(q, center, radius):
    """
    Signed distance from point q to a circle with given center and radius.
    Positive outside, zero on boundary, negative inside.
    """
    return np.linalg.norm(q - center) - radius

def distance_linearization(q, center, radius):
    """
    Linear approximation of the signed distance at q:
      d(q + dq) approx d(q) + grad_d(q)^T dq
    For a circle, grad_d(q) = (q - center) / ||q - center||.
    """
    diff = q - center
    norm = np.linalg.norm(diff)
    if norm < 1e-8:
        # Avoid division by zero; use arbitrary unit vector
        grad = np.array([1.0, 0.0])
        d = -radius
    else:
        grad = diff / norm
        d = norm - radius
    return d, grad

def trajopt_sco(q_start, q_goal, obstacles, N=20,
                d_min=0.1, lambda_smooth=1.0,
                max_iters=10, trust_radius=0.3):
    """
    Simple SCO TrajOpt for a 2D point robot.
    obstacles: list of (center, radius) pairs.
    """
    # Initial guess: straight line interpolation
    qs = np.linspace(0.0, 1.0, N+1)[:, None]
    traj = (1 - qs) * q_start[None, :] + qs * q_goal[None, :]

    for it in range(max_iters):
        Q = cp.Variable((N+1, 2))

        # Smoothness cost (discrete acceleration)
        smooth_terms = []
        for k in range(1, N):
            smooth_terms.append(cp.sum_squares(Q[k+1] - 2*Q[k] + Q[k-1]))
        smooth_cost = lambda_smooth * cp.sum(smooth_terms)

        # Goal term
        goal_cost = cp.sum_squares(Q[N] - q_goal)

        # Collision constraints via linearization
        constraints = []
        constraints.append(Q[0] == q_start)
        constraints.append(Q[N] == q_goal)

        # Trust region around current trajectory
        constraints.append(cp.norm_inf(Q - traj) <= trust_radius)

        # Linearized signed distance constraints
        for k in range(N+1):
            qk = traj[k]
            for (center, radius) in obstacles:
                d, grad = distance_linearization(qk, center, radius)
                # d(qk) + grad^T (q - qk) >= d_min
                constraints.append(d + grad @ (Q[k] - qk) >= d_min)

        prob = cp.Problem(cp.Minimize(smooth_cost + goal_cost), constraints)
        prob.solve(solver=cp.OSQP, warm_start=True, verbose=False)

        if prob.status not in ["optimal", "optimal_inaccurate"]:
            print("Iteration", it, ": QP not solved to optimality.")
            break

        new_traj = Q.value
        diff = np.linalg.norm(new_traj - traj)
        print(f"Iteration {it}, step norm = {diff:.4f}")
        traj = new_traj
        if diff < 1e-3:
            break

    return traj

if __name__ == "__main__":
    q_start = np.array([0.0, 0.0])
    q_goal = np.array([1.0, 1.0])
    obstacles = [
        (np.array([0.5, 0.5]), 0.2),
    ]
    traj = trajopt_sco(q_start, q_goal, obstacles)
    print("Final trajectory shape:", traj.shape)
      

This implementation demonstrates the core pattern: linearized distance constraints, trust regions, and a quadratic smoothness objective. For manipulators, replace the 2D position with joint-space vectors and use collision distances returned by a geometry engine (e.g., FCL, Bullet) plus Jacobians from a kinematics library.

In C++, TrajOpt-style methods are typically implemented using Eigen (for linear algebra) and a QP solver such as OSQP, qpOASES, or Gurobi. The following sketch shows how to form a quadratic cost and linear constraints for one SCO iteration.

#include <iostream>
#include <vector>
#include <Eigen/Dense>
// Include your QP solver headers, e.g. OSQP

struct Obstacle {
  Eigen::Vector2d center;
  double radius;
};

struct TrajOptProblem {
  int N; // number of segments
  double d_min;
  double lambda_smooth;
  double trust_radius;
  Eigen::Vector2d q_start;
  Eigen::Vector2d q_goal;
  std::vector<Obstacle> obstacles;
};

void buildQP(const TrajOptProblem& prob,
             const Eigen::MatrixXd& traj,
             Eigen::MatrixXd& H,
             Eigen::VectorXd& f,
             Eigen::MatrixXd& A,
             Eigen::VectorXd& lb,
             Eigen::VectorXd& ub)
{
  const int N = prob.N;
  const int dim = 2;
  const int nvar = (N + 1) * dim;

  H.setZero(nvar, nvar);
  f.setZero(nvar);

  // Smoothness: sum ||q_{k+1} - 2 q_k + q_{k-1}||^2
  for (int k = 1; k < N; ++k) {
    for (int d = 0; d < dim; ++d) {
      int i_prev = (k - 1) * dim + d;
      int i_curr = k * dim + d;
      int i_next = (k + 1) * dim + d;

      // Contribution of (q_next - 2 q_curr + q_prev)^2
      H(i_prev, i_prev) += prob.lambda_smooth;
      H(i_curr, i_curr) += 4.0 * prob.lambda_smooth;
      H(i_next, i_next) += prob.lambda_smooth;

      H(i_prev, i_curr) += -2.0 * prob.lambda_smooth;
      H(i_curr, i_prev) += -2.0 * prob.lambda_smooth;

      H(i_curr, i_next) += -2.0 * prob.lambda_smooth;
      H(i_next, i_curr) += -2.0 * prob.lambda_smooth;

      H(i_prev, i_next) += 1.0 * prob.lambda_smooth;
      H(i_next, i_prev) += 1.0 * prob.lambda_smooth;
    }
  }

  // Goal term: ||q_N - q_goal||^2
  for (int d = 0; d < dim; ++d) {
    int idx = N * dim + d;
    H(idx, idx) += 1.0;
    f(idx) += -2.0 * prob.q_goal[d];
  }

  // We now assemble linear constraints of the form
  // lb <= A x <= ub
  // including equality constraints for q_0, q_N,
  // trust-region, and linearized collision constraints.

  // For brevity, we omit the detailed construction.
  // In practice, you would stack each linear constraint row-by-row.

  // Example: q_0 == q_start
  // A(row, idx_of_q0_d) = 1; lb(row) = ub(row) = q_start[d];

  // Example: trust region around traj
  // For each component i: -trust_radius <= x_i - traj_i <= trust_radius
  // A(row, i) = 1; lb(row) = traj_i - trust_radius; ub(row) = traj_i + trust_radius;

  // Example: linearized collision constraint
  // d + grad^T (q_k - qk_curr) >= d_min
  // Rewritten as: grad^T q_k >= d_min - d + grad^T qk_curr

  // After constructing H, f, A, lb, ub, call the QP solver.
}

int main() {
  TrajOptProblem prob;
  prob.N = 20;
  prob.d_min = 0.1;
  prob.lambda_smooth = 1.0;
  prob.trust_radius = 0.3;
  prob.q_start = Eigen::Vector2d(0.0, 0.0);
  prob.q_goal  = Eigen::Vector2d(1.0, 1.0);
  prob.obstacles.push_back({Eigen::Vector2d(0.5, 0.5), 0.2});

  // Initial straight-line trajectory
  Eigen::MatrixXd traj(prob.N + 1, 2);
  for (int k = 0; k <= prob.N; ++k) {
    double s = static_cast<double>(k) / prob.N;
    traj.row(k) = (1.0 - s) * prob.q_start + s * prob.q_goal;
  }

  // SCO outer loop (omitting convergence tests for brevity)
  for (int it = 0; it < 10; ++it) {
    Eigen::MatrixXd H;
    Eigen::VectorXd f;
    Eigen::MatrixXd A;
    Eigen::VectorXd lb, ub;

    buildQP(prob, traj, H, f, A, lb, ub);

    // Solve QP: minimize 0.5 x^T H x + f^T x  s.t. lb <= A x <= ub
    // Replace with call to your chosen solver.
    // ...

    // Update traj from solution vector x
    // ...
  }

  return 0;
}
      

For full robot arms, replace the 2D point with joint-space vectors and use forward kinematics from Eigen-based kinematics libraries or ROS-based stacks, plus collision distances from e.g. FCL or Bullet.

In Java, linear algebra can be handled by libraries such as EJML, while ojAlgo provides quadratic programming utilities. The example below illustrates a single SCO iteration for a 1D trajectory (for clarity).

import org.ejml.simple.SimpleMatrix;
import org.ojalgo.optimisation.Optimisation;
import org.ojalgo.optimisation.QuadraticSolver;
import org.ojalgo.optimisation.QuadraticSolver.Builder;

public class TrajOpt1D {

    public static void main(String[] args) {
        int N = 10;
        double lambdaSmooth = 1.0;
        double trustRadius = 0.2;

        // Initial straight-line trajectory q_k in 1D
        double qStart = 0.0;
        double qGoal = 1.0;
        SimpleMatrix q = new SimpleMatrix(N + 1, 1);
        for (int k = 0; k <= N; ++k) {
            double s = (double) k / N;
            q.set(k, 0, (1.0 - s) * qStart + s * qGoal);
        }

        int nvar = N + 1;
        SimpleMatrix H = new SimpleMatrix(nvar, nvar);
        SimpleMatrix g = new SimpleMatrix(nvar, 1);

        // Build Hessian for smoothness cost sum (q_{k+1} - 2 q_k + q_{k-1})^2
        for (int k = 1; k < N; ++k) {
            int iPrev = k - 1;
            int iCurr = k;
            int iNext = k + 1;

            H.set(iPrev, iPrev, H.get(iPrev, iPrev) + lambdaSmooth);
            H.set(iCurr, iCurr, H.get(iCurr, iCurr) + 4.0 * lambdaSmooth);
            H.set(iNext, iNext, H.get(iNext, iNext) + lambdaSmooth);

            H.set(iPrev, iCurr, H.get(iPrev, iCurr) - 2.0 * lambdaSmooth);
            H.set(iCurr, iPrev, H.get(iCurr, iPrev) - 2.0 * lambdaSmooth);

            H.set(iCurr, iNext, H.get(iCurr, iNext) - 2.0 * lambdaSmooth);
            H.set(iNext, iCurr, H.get(iNext, iCurr) - 2.0 * lambdaSmooth);

            H.set(iPrev, iNext, H.get(iPrev, iNext) + 1.0 * lambdaSmooth);
            H.set(iNext, iPrev, H.get(iNext, iPrev) + 1.0 * lambdaSmooth);
        }

        // Goal term (q_N - qGoal)^2
        H.set(N, N, H.get(N, N) + 1.0);
        g.set(N, 0, -2.0 * qGoal);

        // Trust region: |q_k - q_k_current| <= trustRadius
        // This yields linear inequality constraints: q_k_current - trustRadius <= q_k <= q_k_current + trustRadius

        // Use ojAlgo builder
        Builder builder = QuadraticSolver.getBuilder(nvar);
        for (int i = 0; i < nvar; ++i) {
            for (int j = 0; j < nvar; ++j) {
                builder.quadratic(i, j, H.get(i, j) * 0.5); // ojAlgo uses 0.5 x^T Q x
            }
            builder.linear(i, g.get(i, 0));
            double qi = q.get(i, 0);
            builder.lower(i, qi - trustRadius);
            builder.upper(i, qi + trustRadius);
        }

        // Fix endpoints
        builder.lower(0, qStart);
        builder.upper(0, qStart);
        builder.lower(N, qGoal);
        builder.upper(N, qGoal);

        QuadraticSolver solver = builder.build();
        Optimisation.Result result = solver.solve();
        System.out.println("Solver state: " + result.getState());

        double[] xOpt = result.toRawCopy1D();
        System.out.println("Optimized trajectory:");
        for (int i = 0; i < xOpt.length; ++i) {
            System.out.println("q[" + i + "] = " + xOpt[i]);
        }
    }
}
      

Collision constraints can be added similarly as linear inequalities based on linearized signed distance; for manipulators, use a Java robotics/kinematics library to compute link positions and Jacobians.

MATLAB provides quadprog for QP and the Robotics System Toolbox for kinematics and collision checking. The following example sets up one SCO iteration for a planar manipulator trajectory using joint-space smoothness and linearized distance constraints.

function traj = trajopt_sco_matlab(robot, qStart, qGoal, obstacles)
% TRAJOPT_SCO_MATLAB
%   robot     : rigidBodyTree from Robotics System Toolbox
%   qStart    : 1xn start configuration
%   qGoal     : 1xn goal configuration
%   obstacles : struct array with fields center (1x3), radius

N = 20;
lambdaSmooth = 1.0;
trustRadius  = 0.3;
dMin         = 0.05;
n            = numel(qStart);

% Initial trajectory (linear interpolation)
traj = zeros(N+1, n);
for k = 0:N
    s = k / N;
    traj(k+1,:) = (1 - s) * qStart + s * qGoal;
end

for it = 1:10
    % Decision vector x stacks all q_k
    nvar = (N+1) * n;
    H = zeros(nvar, nvar);
    f = zeros(nvar, 1);

    % Smoothness term
    for k = 2:N
        for j = 1:n
            idxPrev = (k-2)*n + j;
            idxCurr = (k-1)*n + j;
            idxNext = k*n + j;

            H(idxPrev, idxPrev) = H(idxPrev, idxPrev) + lambdaSmooth;
            H(idxCurr, idxCurr) = H(idxCurr, idxCurr) + 4*lambdaSmooth;
            H(idxNext, idxNext) = H(idxNext, idxNext) + lambdaSmooth;

            H(idxPrev, idxCurr) = H(idxPrev, idxCurr) - 2*lambdaSmooth;
            H(idxCurr, idxPrev) = H(idxCurr, idxPrev) - 2*lambdaSmooth;

            H(idxCurr, idxNext) = H(idxCurr, idxNext) - 2*lambdaSmooth;
            H(idxNext, idxCurr) = H(idxNext, idxCurr) - 2*lambdaSmooth;

            H(idxPrev, idxNext) = H(idxPrev, idxNext) + lambdaSmooth;
            H(idxNext, idxPrev) = H(idxNext, idxPrev) + lambdaSmooth;
        end
    end

    % Goal term ||q_N - qGoal||^2
    for j = 1:n
        idx = N*n + j;
        H(idx, idx) = H(idx, idx) + 1;
        f(idx)      = f(idx) - 2*qGoal(j);
    end

    A = [];
    b = [];
    Aeq = [];
    beq = [];

    % Equality constraints for endpoints
    % q_0 = qStart, q_N = qGoal
    Aeq = zeros(2*n, nvar);
    beq = zeros(2*n, 1);
    % q_0
    for j = 1:n
        idx = j;
        Aeq(j, idx) = 1;
        beq(j)      = qStart(j);
    end
    % q_N
    for j = 1:n
        idx = N*n + j;
        Aeq(n+j, idx) = 1;
        beq(n+j)      = qGoal(j);
    end

    % Bounds (trust region)
    lb = zeros(nvar,1);
    ub = zeros(nvar,1);
    for k = 0:N
        for j = 1:n
            idx = k*n + j;
            qk  = traj(k+1,j);
            lb(idx) = qk - trustRadius;
            ub(idx) = qk + trustRadius;
        end
    end

    % Linearized collision constraints: A_coll * x >= b_coll
    % quadprog expects A*x <= b, so we multiply by -1.
    A_coll = [];
    b_coll = [];

    for k = 0:N
        qk = traj(k+1,:);
        % Compute collision points and normals using Robotics System Toolbox
        % (e.g., use checkCollision or custom signed-distance queries)
        for obs = obstacles
            % Simple point on end effector for illustration:
            [pose, J] = forwardKinematics(robot, qk); %#ok<NASGU>
            % Here, pose and J would be obtained via getTransform and geometricJacobian.
            % Suppose we have distance d and normal n such that
            % d(q) approx d(qk) + n * J * (q - qk)'
            % For brevity, we assume d and n are given:
            d = 0.1; % placeholder
            nvec = [1 0 0]; % placeholder

            a = nvec * J;       % row vector 1xn
            rhs = dMin - d + a*qk';

            row = zeros(1, nvar);
            for j = 1:n
                idx = k*n + j;
                row(idx) = a(j);
            end
            A_coll = [A_coll; -row];
            b_coll = [b_coll; -rhs];
        end
    end

    A = [A; A_coll];
    b = [b; b_coll];

    options = optimoptions('quadprog','Display','off');
    [xOpt,~,exitflag] = quadprog(H, f, A, b, Aeq, beq, lb, ub, [], options);
    if exitflag <= 0
        warning('quadprog failed at iteration %d', it);
        break;
    end

    traj_new = reshape(xOpt, [n, N+1])';
    if norm(traj_new - traj, 'fro') < 1e-3
        traj = traj_new;
        break;
    end
    traj = traj_new;
end
end
      

To integrate with Simulink, one typical pattern is to:

1. Represent the robot dynamics and sensors in Simulink using the Robotics System Toolbox blocks.
2. Use a MATLAB Function block that calls a TrajOpt routine like trajopt_sco_matlab to generate reference joint trajectories.
3. Feed the optimized trajectory into trajectory-following controllers (e.g., PID or computed-torque) within Simulink.

In Wolfram Mathematica, we can implement a basic SCO-like loop by repeatedly calling NMinimize on convexified costs and constraints. The example below optimizes a 2D point trajectory with smoothness and distance-penalty terms.

ClearAll["Global`*"];

(* Discrete smoothness cost for 2D points *)
smoothnessCost[qs_List] := Module[{N = Length[qs] - 1},
  Sum[Norm[qs[[k + 1]] - 2 qs[[k]] + qs[[k - 1]]]^2,
    {k, 2, N}]
];

(* Circle signed distance and linearization *)
signedDistancePointCircle[q_, center_, radius_] :=
  Norm[q - center] - radius;

linearizedDistanceConstraint[qSym_, qCurr_, center_, radius_, dMin_] :=
 Module[{diff, norm, grad, d0},
  diff = qCurr - center;
  norm = Norm[diff];
  grad = If[norm < 10^-8, {1., 0.}, diff/norm];
  d0   = norm - radius;
  grad.(qSym - qCurr) + d0 - dMin
];

(* One SCO iteration using NMinimize *)
trajOptIteration[trajCurr_List, center_, radius_, dMin_, lambdaSmooth_] :=
 Module[{N = Length[trajCurr] - 1,
   qVars, obj, cons, k, qk, qSym, linCons},
  qVars = Array[q, {N + 1, 2}];

  obj = lambdaSmooth * smoothnessCost[qVars];

  cons = {
    qVars[[1]] == trajCurr[[1]],
    qVars[[N + 1]] == trajCurr[[N + 1]]
  };

  (* Linearized distance constraints at each waypoint *)
  For[k = 1, k <= N + 1, k++,
    qk = trajCurr[[k]];
    qSym = qVars[[k]];
    linCons = linearizedDistanceConstraint[qSym, qk, center, radius, dMin];
    AppendTo[cons, linCons >= 0];
  ];

  First@
   NMinimize[{obj, cons}, Flatten[qVars]]
 ];

(* Example usage *)
Nsteps = 20;
qStart = {0., 0.};
qGoal  = {1., 1.};
traj0  = Table[(1. - s) qStart + s qGoal,
   {s, 0., 1., 1./Nsteps}];

center = {0.5, 0.5};
radius = 0.2;
dMin   = 0.1;
lambdaSmooth = 1.0;

traj = traj0;
Do[
  {val, flatQ} = trajOptIteration[traj, center, radius, dMin, lambdaSmooth];
  traj = Partition[flatQ, 2];
  Print["Iteration ", it, " cost = ", val];
  If[Norm[traj - traj0, "Frobenius"] < 10^-3, Break[]];
  ,
  {it, 1, 5}
];

ListLinePlot[traj, PlotRange -> All,
  AxesLabel -> {"x", "y"},
  PlotMarkers -> Automatic]
      

For manipulator arms, the same pattern can be used by mapping joint configurations to workspace positions via GeometricTransformation or symbolic kinematics, then forming signed distances to obstacles.

Problem 1 (Hessian of the Smoothness Term): Consider the discrete smoothness cost \( J_{\text{smooth}}(\mathbf{x}) = \sum_{k=1}^{N-1} \left\| \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \right\|_2^2 \) with \( \mathbf{x} = [\mathbf{q}_0^\top,\dots,\mathbf{q}_N^\top]^\top \). Show that \( J_{\text{smooth}}(\mathbf{x}) = \mathbf{x}^\top \mathbf{H}\mathbf{x} \) for some symmetric positive semidefinite block tridiagonal matrix \( \mathbf{H} \), and derive the pattern of its blocks.

Solution:

Let \( \Delta^2 \mathbf{q}_k = \mathbf{q}_{k+1} - 2\mathbf{q}_k + \mathbf{q}_{k-1} \). Then

\[ J_{\text{smooth}}(\mathbf{x}) = \sum_{k=1}^{N-1} \left( \Delta^2 \mathbf{q}_k \right)^\top \left( \Delta^2 \mathbf{q}_k \right) = \left\| \mathbf{A} \mathbf{x} \right\|_2^2 = \mathbf{x}^\top \mathbf{A}^\top \mathbf{A} \mathbf{x}, \]

where \( \mathbf{A} \) is the block matrix implementing the second-difference operator, with rows corresponding to \( \Delta^2 \mathbf{q}_k \). Thus, \( \mathbf{H} = \mathbf{A}^\top \mathbf{A} \succeq 0 \).

Inspecting \( \Delta^2 \mathbf{q}_k \) shows that each term couples only \( \mathbf{q}_{k-1}, \mathbf{q}_k, \mathbf{q}_{k+1} \); therefore \( \mathbf{H} \) is block tridiagonal with block coefficients \( +\mathbf{I}, -2\mathbf{I}, +\mathbf{I} \) in a three-point stencil. Each block is scaled by the dimension of \( \mathbf{q}_k \), and the overall Hessian is symmetric and positive semidefinite.

Problem 2 (Conservativeness of Linearized Distance Constraints): Let \( d:\mathbb{R}^n\to\mathbb{R} \) be a convex signed distance function and consider the constraint \( d(\mathbf{x}) \ge d_{\min} \). Show that the linearized constraint at \( \mathbf{x}_0 \),

\[ d(\mathbf{x}_0) + \nabla d(\mathbf{x}_0)^\top (\mathbf{x}-\mathbf{x}_0) \ge d_{\min}, \]

is conservative, i.e., any \( \mathbf{x} \) satisfying the linearized constraint also satisfies the original distance constraint.

Solution:

For a convex function \( d \), we have for all \( \mathbf{x},\mathbf{x}_0 \):

\[ d(\mathbf{x}) \ge d(\mathbf{x}_0) + \nabla d(\mathbf{x}_0)^\top(\mathbf{x}-\mathbf{x}_0). \]

Suppose \( \mathbf{x} \) satisfies the linearized constraint, so that

\[ d(\mathbf{x}_0) + \nabla d(\mathbf{x}_0)^\top(\mathbf{x}-\mathbf{x}_0) \ge d_{\min}. \]

Then

\[ d(\mathbf{x}) \ge d(\mathbf{x}_0) + \nabla d(\mathbf{x}_0)^\top(\mathbf{x}-\mathbf{x}_0) \ge d_{\min}, \]

so \( \mathbf{x} \) is feasible for the original constraint. Hence the linearization defines an inner approximation of the feasible set.

Problem 3 (KKT Conditions for a TrajOpt Subproblem): Consider a single convex QP TrajOpt subproblem:

\[ \begin{aligned} \min_{\mathbf{x}} \;& \tfrac{1}{2}\mathbf{x}^\top \mathbf{H} \mathbf{x} + \mathbf{c}^\top \mathbf{x} \\ \text{s.t.}\;& \mathbf{A}\mathbf{x} \le \mathbf{b},\quad \mathbf{A}_{\text{eq}}\mathbf{x} = \mathbf{b}_{\text{eq}}. \end{aligned} \]

Derive the KKT conditions and explain why any solution satisfying them is globally optimal (assuming \( \mathbf{H} \succeq 0 \) and Slater feasibility conditions).

Solution:

The Lagrangian is

\[ \mathcal{L}(\mathbf{x},\boldsymbol{\lambda},\boldsymbol{\nu}) = \tfrac{1}{2}\mathbf{x}^\top \mathbf{H} \mathbf{x} + \mathbf{c}^\top \mathbf{x} + \boldsymbol{\lambda}^\top(\mathbf{A}\mathbf{x}-\mathbf{b}) + \boldsymbol{\nu}^\top(\mathbf{A}_{\text{eq}}\mathbf{x}-\mathbf{b}_{\text{eq}}), \]

with \( \boldsymbol{\lambda} \ge \mathbf{0} \). The KKT conditions are

\[ \begin{aligned} \text{(Stationarity)}\;& \mathbf{H}\mathbf{x}^\star + \mathbf{c} + \mathbf{A}^\top \boldsymbol{\lambda}^\star + \mathbf{A}_{\text{eq}}^\top \boldsymbol{\nu}^\star = \mathbf{0}, \\ \text{(Primal feasibility)}\;& \mathbf{A}\mathbf{x}^\star \le \mathbf{b},\quad \mathbf{A}_{\text{eq}}\mathbf{x}^\star = \mathbf{b}_{\text{eq}}, \\ \text{(Dual feasibility)}\;& \boldsymbol{\lambda}^\star \ge \mathbf{0}, \\ \text{(Complementarity)}\;& \lambda_i^\star (\mathbf{A}_i \mathbf{x}^\star - b_i) = 0 \;\; \forall i. \end{aligned} \]

Because the objective is convex and the constraints define a convex feasible set, any point satisfying KKT is a global optimum. Slater's condition guarantees strong duality, hence KKT conditions are both necessary and sufficient.

Problem 4 (One-Step TrajOpt for a Single Intermediate Waypoint): Consider a 1D trajectory with \( q_0 = 0 \), \( q_2 = 1 \), and unknown intermediate waypoint \( q_1 \). Let the smoothness cost be \( (q_2 - 2q_1 + q_0)^2 \) and suppose there is a linearized obstacle constraint \( q_1 \ge 0.6 \). Solve the convex subproblem explicitly.

Solution:

The objective is

\[ J(q_1) = (1 - 2q_1 + 0)^2 = (1-2q_1)^2. \]

This is a convex quadratic. Unconstrained minimization yields \( J'(q_1) = 2(1-2q_1)(-2) = -4(1-2q_1) \), setting this to zero gives \( 1-2q_1 = 0 \), hence \( q_1^\star = 0.5 \).

However, the constraint \( q_1 \ge 0.6 \) is violated. Therefore the constrained minimizer is at the boundary \( q_1 = 0.6 \), since the objective increases symmetrically away from \( 0.5 \). Hence the optimal \( q_1^\star = 0.6 \).

Problem 5 (Effect of Trust Region Size): Explain qualitatively how the trust-region radius \( \Delta^{(\ell)} \) affects the convergence behavior of SCO-based TrajOpt. What are the effects of choosing \( \Delta^{(\ell)} \) too small or too large?

Solution:

The trust region bounds the step \( \mathbf{x} - \mathbf{x}^{(\ell)} \) within a region where linearizations are reliable. If \( \Delta^{(\ell)} \) is too small, each subproblem changes the trajectory only slightly, leading to very slow progress (many iterations). In the extreme, the method may stagnate near an initial poor guess if the region is too restrictive.

If \( \Delta^{(\ell)} \) is too large, the linearized constraints and cost may be poor approximations far from \( \mathbf{x}^{(\ell)} \), causing infeasible or unstable iterates, or even divergence. Adaptive trust-region strategies enlarge \( \Delta^{(\ell)} \) when the model predicts actual improvement well and shrink it when the model proves inaccurate.

In this lesson, we formulated optimization-based trajectory planning as a nonconvex problem over discretized robot configurations with smoothness and collision-avoidance terms. We introduced sequential convex optimization as the core mathematical framework for TrajOpt, deriving convex QP subproblems via linearization of signed distance constraints and goal-tracking objectives. We discussed trust-region mechanisms, KKT conditions, and convergence intuition, and implemented simplified TrajOpt pipelines in Python, C++, Java, MATLAB/Simulink, and Wolfram Mathematica. These tools provide a powerful foundation for sophisticated trajectory planners that can exploit modern convex optimization solvers while handling complex robot geometries and constraints.