Previous lessons introduced symbolic task planning, hybrid state spaces, and sampling-based TAMP. Optimization-based TAMP instead views the entire problem (symbolic decisions + continuous robot trajectories) as a single large optimization program. The high-level idea is to choose:

• a sequence of modes or actions (e.g. move, grasp, place), and
• a dynamically feasible, collision-free trajectory consistent with those modes,

so as to minimize a cost functional that encodes task objectives and motion quality. Let the configuration trajectory be \( \mathbf{q}_{0:T} = (\mathbf{q}_0, \dots, \mathbf{q}_T) \) and a discrete mode sequence \( \boldsymbol{\sigma}_{0:T} = (\sigma_0,\dots,\sigma_T) \). A prototypical cost is

\[ J(\mathbf{q}_{0:T},\boldsymbol{\sigma}_{0:T}) = \Phi(\mathbf{q}_T,\sigma_T) + \sum_{k=0}^{T-1} \ell_k(\mathbf{q}_k,\mathbf{q}_{k+1},\sigma_k), \]

where \( \Phi \) encodes terminal task success (e.g. object at goal, end-effector pose) and \( \ell_k \) encodes motion smoothness, control effort, and penalties for “uncomfortable” modes.

Optimization-based TAMP then searches over symbolic structures by repeatedly solving continuous trajectory optimization problems, or by embedding discrete decisions directly as integer variables in a single mixed-integer nonlinear program (MINLP).

flowchart TD
  A["Task description (symbols, goals)"] --> B["Candidate mode sequence / skeleton"]
  B --> C["Continuous optimization (trajectory, contacts, dynamics)"]
  C --> D["Feasibility check (collisions, joint limits, contacts)"]
  D -->|feasible| E["Execute / keep solution"]
  D -->|infeasible| F["Refine mode sequence or constraints"]
  F --> B
        

In this lesson we give a precise mathematical formulation of this hybrid problem, connect it to classical trajectory optimization from Chapter 4, and show how symbolic constraints are embedded into continuous optimization using logic–geometric and mixed-integer encodings.

We consider a fixed discretization of time steps \( k = 0,\dots,T \). Let \( \mathbf{q}_k \in \mathbb{R}^n \) denote the robot configuration (generalized coordinates) at step \( k \), and optionally \( \mathbf{u}_k \in \mathbb{R}^m \) denote control inputs if kinodynamic constraints are included.

Symbolic task decisions are encoded by discrete variables \( \mathbf{d}_k \) (e.g. which action is active, which object is grasped). For a finite action set \( \mathcal{A} = \{1,\dots,A\} \) we can use one-hot binary vectors

\[ \mathbf{d}_k = (d_{k,1},\dots,d_{k,A}) \in \{0,1\}^A,\quad \sum_{a=1}^A d_{k,a} = 1, \]

where \( d_{k,a}=1 \) means action \( a \) is chosen at time step \( k \).

Collect all continuous variables into \( \mathbf{x} = (\mathbf{q}_{0:T},\mathbf{u}_{0:T-1}) \) and all discrete variables into \( \mathbf{d} = (\mathbf{d}_0,\dots,\mathbf{d}_T) \). A generic optimization-based TAMP problem can be written as

\[ \begin{aligned} &\min_{\mathbf{x},\,\mathbf{d}} && J(\mathbf{x},\mathbf{d}) \\ &\text{s.t.} && \mathbf{f}_k(\mathbf{q}_k,\mathbf{q}_{k+1},\mathbf{u}_k,\mathbf{d}_k) = \mathbf{0}, \quad k=0,\dots,T-1,\\ &&& \mathbf{g}_k(\mathbf{q}_k,\mathbf{u}_k,\mathbf{d}_k) \le \mathbf{0}, \quad k=0,\dots,T,\\ &&& \mathbf{h}_k(\mathbf{q}_k,\mathbf{d}_k) \le \mathbf{0},\quad k=0,\dots,T,\\ &&& \mathbf{d}_k \in \{0,1\}^A,\ \sum_{a} d_{k,a}=1,\quad k=0,\dots,T. \end{aligned} \]

Here:

• \( \mathbf{f}_k \) represent dynamic constraints (e.g. Euler integration of manipulator dynamics from Chapter 4).
• \( \mathbf{g}_k \) represent continuous constraints such as joint limits and collision avoidance.
• \( \mathbf{h}_k \) encode task-dependent geometric predicates, such as “object is on table” or “gripper is in contact with object”, whose activation depends on \( \mathbf{d}_k \).

This is a mixed-integer nonlinear program (MINLP). For a fixed sequence of discrete actions \( \bar{\mathbf{d}} = (\bar{\mathbf{d}}_0,\dots,\bar{\mathbf{d}}_T) \), the problem reduces to a pure trajectory optimization:

\[ V(\bar{\mathbf{d}}) = \min_{\mathbf{x}} J(\mathbf{x},\bar{\mathbf{d}}) \ \text{s.t.}\ \mathbf{f}_k(\cdot,\bar{\mathbf{d}}_k)=\mathbf{0},\ \mathbf{g}_k(\cdot,\bar{\mathbf{d}}_k)\le \mathbf{0},\ \mathbf{h}_k(\cdot,\bar{\mathbf{d}}_k)\le \mathbf{0}. \]

TAMP can thus be viewed as a discrete search over \( \bar{\mathbf{d}} \) with value function \( V(\bar{\mathbf{d}}) \), where \( V(\bar{\mathbf{d}}) \) itself is defined by a continuous optimization problem structurally similar to the trajectory optimizations studied in Chapter 4.

Logic–Geometric Programming treats logical predicates and geometric constraints in a unified optimization problem. Let \( \mathcal{P} = \{p_1,\dots,p_P\} \) be a set of symbolic predicates (e.g. On(obj, table), Grasped(obj)). For each predicate \( p_j \) we introduce a binary variable \( y_{j,k} \in \{0,1\} \) indicating whether \( p_j \) holds at time \( k \).

Geometric semantics are encoded via constraint functions \( c_j(\mathbf{q}_k) \), such that “\( p_j \) holds” is approximated by

\[ \|c_j(\mathbf{q}_k)\|_2 \le \varepsilon, \]

for some small tolerance \( \varepsilon \). To couple logic and geometry, we use implications such as “if \( y_{j,k} = 1 \), then \( p_j \) must hold geometrically”. In optimization form this is encoded with big-\( M \) constraints:

\[ \|c_j(\mathbf{q}_k)\|_2 \le \varepsilon + M\,(1-y_{j,k}),\quad j=1,\dots,P,\ k=0,\dots,T, \]

where \( M \) is a sufficiently large constant. When \( y_{j,k}=1 \), the constraint reduces to the geometric predicate; when \( y_{j,k}=0 \), the right-hand side is effectively relaxed.

Action schemas from classical planning (e.g. STRIPS operators) have preconditions and effects expressed in terms of these predicates. If an action \( a \) at time \( k \) has preconditions \( p_{j_1},\dots,p_{j_r} \) and effects \( p_{j_{r+1}},\dots,p_{j_s} \), we can write linear constraints relating \( \mathbf{d}_k \) and \( \mathbf{y}_k \), for example:

\[ y_{j_\ell,k} \ge d_{k,a},\quad \ell=1,\dots,r, \]

ensuring that preconditions of action \( a \) are true whenever the action is selected. Effects can be enforced between \( \mathbf{y}_k \) and \( \mathbf{y}_{k+1} \). The resulting LGP problem is again a MINLP, but with structure closely mirroring the logical task description.

A common practical strategy is to first generate a discrete symbolic skeleton (a sequence of actions and modes) by solving a purely logical planning problem, then fix this skeleton and solve the geometric subproblem as a nonlinear program. If the geometric subproblem is infeasible, the symbolic plan is refined.

Consider a single object to be moved from an initial pose to a goal pose using a manipulator with parallel-jaw gripper. For each time step \( k \), define actions \( a \in \{\text{move},\text{grasp},\text{place}\} \). Let \( d_{k,\text{move}}, d_{k,\text{grasp}}, d_{k,\text{place}} \) be the corresponding one-hot binary variables.

The kinematic pose of the gripper is \( \mathbf{p}_\text{grip}(\mathbf{q}_k) \), and the object pose is \( \mathbf{p}_\text{obj}(\mathbf{q}_k) \) (if carried) or a fixed pose (if on table). We want:

• “grasp” to enforce contact between gripper and object,
• “place” to enforce that the object coincides with the goal pose,
• continuous motion otherwise.

Let \( \mathbf{p}_\text{goal} \) be the goal pose. We encode:

\[ \|\mathbf{p}_\text{grip}(\mathbf{q}_k) - \mathbf{p}_\text{obj}(\mathbf{q}_k)\|_2 \le \varepsilon_\text{grasp} + M\,(1-d_{k,\text{grasp}}), \]

\[ \|\mathbf{p}_\text{obj}(\mathbf{q}_k) - \mathbf{p}_\text{goal}\|_2 \le \varepsilon_\text{goal} + M\,(1-d_{k,\text{place}}), \]

which constrain the configuration only when the corresponding action is selected. To model that the object moves rigidly with the gripper after a successful grasp until a place action, we introduce a binary variable \( h_k \in \{0,1\} \) indicating whether the object is currently held. Then we can write the logical evolution:

\[ h_{k+1} = h_k + d_{k,\text{grasp}} - d_{k,\text{place}}, \quad h_0 = 0. \]

Geometric consistency of a held object can be expressed as

\[ \|\mathbf{p}_\text{obj}(\mathbf{q}_k) - \mathbf{p}_\text{grip}(\mathbf{q}_k)\|_2 \le \varepsilon_\text{hold} + M\,(1-h_k). \]

Together with dynamics, collision constraints, and a smoothness or energy cost, this yields a concrete MINLP that simultaneously chooses grasp and place times and computes a feasible trajectory that respects them.

The MINLP structure is in general NP-hard. A widely used strategy is to perform a branch-and-bound search over discrete decisions (e.g. action sequences) while solving continuous relaxations or full nonlinear programs (NLPs) at each node to compute bounds. For a fixed partial assignment of discrete variables \( \mathbf{d}_{0:k} \) we solve a continuous subproblem:

\[ \underline{V}(\mathbf{d}_{0:k}) = \min_{\mathbf{x},\,\mathbf{d}_{k+1:T}} J(\mathbf{x},\mathbf{d}) \ \text{s.t. current discrete assignments and relaxations}, \]

which provides a lower bound on any completion of the branch.

flowchart TD
  R["Root: no discrete decisions fixed"] --> N1["Node 1: fix action at step 0"]
  R --> N2["Node 2: alternative action at step 0"]
  N1 --> N1a["Solve NLP: lower bound for Node 1"]
  N2 --> N2a["Solve NLP: lower bound for Node 2"]
  N1a --> N1b["Branch on step 1 if promising"]
  N2a --> PRUNE["Prune if lower bound worse than incumbent"]
        

For each fixed discrete assignment, the continuous subproblem is a constrained NLP. Let us denote all equality constraints by \( \mathbf{f}(\mathbf{x}) = \mathbf{0} \) and inequality constraints (active for the fixed discrete decisions) by \( \mathbf{g}(\mathbf{x}) \le \mathbf{0} \). The Lagrangian is

\[ \mathcal{L}(\mathbf{x},\boldsymbol{\lambda},\boldsymbol{\mu}) = J(\mathbf{x},\bar{\mathbf{d}}) + \boldsymbol{\lambda}^\top \mathbf{f}(\mathbf{x}) + \boldsymbol{\mu}^\top \mathbf{g}(\mathbf{x}), \]

with Lagrange multipliers \( \boldsymbol{\lambda} \) (unconstrained) and \( \boldsymbol{\mu} \ge \mathbf{0} \). Stationary points satisfy the Karush–Kuhn–Tucker (KKT) conditions:

\[ \begin{aligned} &\nabla_{\mathbf{x}} \mathcal{L}(\mathbf{x}^\star,\boldsymbol{\lambda}^\star,\boldsymbol{\mu}^\star) = \mathbf{0},\\ &\mathbf{f}(\mathbf{x}^\star) = \mathbf{0},\\ &\mathbf{g}(\mathbf{x}^\star) \le \mathbf{0},\quad \boldsymbol{\mu}^\star \ge \mathbf{0},\\ &\mu_i^\star\,g_i(\mathbf{x}^\star) = 0 \ \text{for all inequality constraints}. \end{aligned} \]

Practical solvers (SQP, interior-point) approximate solutions of these conditions numerically. In optimization-based TAMP, these NLP solves are nested inside the outer discrete search, which motivates efficient warm-starting and reuse of gradients, as well as coarse-to-fine relaxations of collision and contact constraints.

We illustrate a minimal optimization-based TAMP implementation in Python for a 2D planar point robot that must move around a circular obstacle and either push or pick-and-place a point-mass object to a goal. We use a discrete set of candidate mode sequences (skeletons) and solve a trajectory optimization for each skeleton using scipy.optimize.minimize.

import numpy as np
from scipy.optimize import minimize

# Robot and environment constants
dt = 0.1
T = 20                      # number of steps
q_start = np.array([0.0, 0.0])
q_goal  = np.array([1.0, 1.0])
obs_center = np.array([0.5, 0.5])
obs_radius = 0.2

def obstacle_penalty(q):
    # soft penalty if inside obstacle
    d = np.linalg.norm(q - obs_center) - obs_radius
    return max(0.0, -d)**2

def smoothness_cost(traj):
    # sum of squared velocities
    q = traj.reshape(T+1, 2)
    vel = np.diff(q, axis=0) / dt
    return np.sum(vel**2)

def goal_cost(traj):
    q = traj.reshape(T+1, 2)
    return np.linalg.norm(q[-1] - q_goal)**2

def collision_cost(traj):
    q = traj.reshape(T+1, 2)
    return sum(obstacle_penalty(q[k]) for k in range(T+1))

def make_push_cost(weight_push):
    def cost(traj):
        # weight_push penalizes deviation between object and robot
        q = traj.reshape(T+1, 2)
        obj = np.copy(q)          # object is pushed by contact with robot
        # encouraging contact: distance between robot and object small
        contact_pen = sum(np.linalg.norm(q[k] - obj[k])**2 for k in range(T+1))
        return smoothness_cost(traj) + goal_cost(traj) \
               + 10.0 * collision_cost(traj) \
               + weight_push * contact_pen
    return cost

def optimize_skeleton(mode):
    # mode can be "push" or "pick_place"
    if mode == "push":
        cost_fun = make_push_cost(weight_push=0.1)
    elif mode == "pick_place":
        # approximated by stronger contact requirement and a "lift" penalty
        cost_fun = make_push_cost(weight_push=10.0)
    else:
        raise ValueError("unknown mode")

    # decision variables: flattened trajectory (T+1 points in R^2)
    x0 = np.linspace(q_start, q_goal, T+1).reshape(-1)  # initial guess

    def eq_start_end(traj):
        q = traj.reshape(T+1, 2)
        return np.concatenate([q[0] - q_start])

    constraints = ({
        "type": "eq",
        "fun": eq_start_end,
    },)

    res = minimize(cost_fun, x0, method="SLSQP", constraints=constraints)
    return res

modes = ["push", "pick_place"]
best_val = np.inf
best_mode = None
best_traj = None
for mode in modes:
    res = optimize_skeleton(mode)
    if res.success and res.fun < best_val:
        best_val = res.fun
        best_mode = mode
        best_traj = res.x.reshape(T+1, 2)

print("Best skeleton:", best_mode, "cost:", best_val)
      

This code does not yet implement full logical preconditions, but it illustrates the characteristic TAMP pattern:

• Enumerate or search over discrete mode sequences.
• For each, perform continuous trajectory optimization.
• Choose the best feasible combination.

In a full robotics stack, collision penalties would be computed via a geometry engine (e.g. fcl through pybullet or pinocchio bindings), and symbolic constraints would be used to generate more structured skeletons before calling trajectory optimizers such as CHOMP, TrajOpt, or direct collocation from Chapter 4.

In C++, optimization-based TAMP is often implemented by combining Eigen for linear algebra, a collision library (e.g. fcl), and a nonlinear solver such as IPOPT or ifopt. The core step is to define an objective and constraints that evaluate cost and feasibility for a fixed mode sequence; the outer loop then enumerates or branches over different sequences.

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

struct SkeletonStep {
  std::string action; // "move", "grasp", "place", ...
};

struct TAMPProblem {
  int T;
  Eigen::Vector2d q_start;
  Eigen::Vector2d q_goal;
  std::vector<SkeletonStep> skeleton;

  double smoothnessCost(const Eigen::VectorXd& x) const {
    double cost = 0.0;
    int dim = 2;
    for (int k = 0; k < T; ++k) {
      Eigen::Vector2d qk   = x.segment(dim * k, dim);
      Eigen::Vector2d qkp1 = x.segment(dim * (k + 1), dim);
      Eigen::Vector2d v = (qkp1 - qk);
      cost += v.squaredNorm();
    }
    return cost;
  }

  double goalCost(const Eigen::VectorXd& x) const {
    int dim = 2;
    Eigen::Vector2d qT = x.segment(dim * T, dim);
    return (qT - q_goal).squaredNorm();
  }

  double contactCost(const Eigen::VectorXd& x) const {
    // simple mode-dependent penalty (placeholder for real kinematics)
    double cost = 0.0;
    int dim = 2;
    for (int k = 0; k < T; ++k) {
      const auto& step = skeleton[k];
      Eigen::Vector2d qk = x.segment(dim * k, dim);
      if (step.action == "grasp") {
        // encourage qk to be near the object's pose
        Eigen::Vector2d obj(0.5, 0.5);
        cost += (qk - obj).squaredNorm();
      }
    }
    return cost;
  }

  double objective(const Eigen::VectorXd& x) const {
    return smoothnessCost(x) + goalCost(x) + contactCost(x);
  }
};

// An IPOPT or ifopt wrapper would call TAMPProblem::objective(...) and define
// constraint evaluations for joint limits, collisions, dynamics, etc.
// The outer TAMP layer would instantiate multiple TAMPProblem instances with
// different SkeletonStep sequences and choose the best feasible solution.
      

In a full implementation, the constraint interface would also query collision distances via a library such as fcl and impose equality constraints for grasp and place events, while the outer loop would be integrated with a symbolic planner (e.g. using PDDL) that proposes candidate skeletons.

8.1 Java Sketch using an Optimization Library

In Java, continuous trajectory optimization can be implemented with numerical libraries such as ojAlgo or Apache Commons Math. Symbolic task planning can be integrated via a planner or constraint solver (e.g. OptaPlanner), while continuous feasibility is delegated to an optimizer.

public class TAMPObjective {

    private int T;
    private double[] qStart;
    private double[] qGoal;
    private String[] actions; // length T, e.g. "move", "grasp"

    public TAMPObjective(int T, double[] qStart, double[] qGoal, String[] actions) {
        this.T = T;
        this.qStart = qStart;
        this.qGoal = qGoal;
        this.actions = actions;
    }

    // traj is flat array of length 2*(T+1): [q0x,q0y,q1x,q1y,...]
    public double evaluate(double[] traj) {
        double cost = 0.0;

        // smoothness
        for (int k = 0; k < T; ++k) {
            double qkx = traj[2 * k];
            double qky = traj[2 * k + 1];
            double qkx1 = traj[2 * (k + 1)];
            double qky1 = traj[2 * (k + 1) + 1];
            double dx = qkx1 - qkx;
            double dy = qky1 - qky;
            cost += dx * dx + dy * dy;
        }

        // goal
        double qTx = traj[2 * T];
        double qTy = traj[2 * T + 1];
        double dxg = qTx - qGoal[0];
        double dyg = qTy - qGoal[1];
        cost += dxg * dxg + dyg * dyg;

        // simple "grasp" cost
        for (int k = 0; k < T; ++k) {
            if (actions[k].equals("grasp")) {
                double qkx = traj[2 * k];
                double qky = traj[2 * k + 1];
                double objx = 0.5;
                double objy = 0.5;
                double dx = qkx - objx;
                double dy = qky - objy;
                cost += 10.0 * (dx * dx + dy * dy);
            }
        }
        return cost;
    }
}
      

An external solver wrapper would call evaluate inside a numerical optimization loop and add constraints for start configuration, joint limits, and collision avoidance using your robotics kinematics library on the JVM (or via native bindings to C++ code).

8.2 MATLAB/Simulink Example via fmincon

MATLAB provides fmincon and the Robotics System Toolbox for kinematics and collision checking. A minimal continuous TAMP subproblem for a fixed skeleton can be written as:

function tamp_matlab_example()
    T = 20;
    dt = 0.1;
    qStart = [0; 0];
    qGoal  = [1; 1];

    x0 = zeros(2*(T+1),1);
    for k = 0:T
        alpha = k / T;
        x0(2*k+1:2*k+2) = (1-alpha)*qStart + alpha*qGoal;
    end

    function J = costFun(x)
        J = 0;
        for k = 1:T
            qk   = x(2*(k-1)+1:2*(k-1)+2);
            qkp1 = x(2*k+1:2*k+2);
            v = (qkp1 - qk) / dt;
            J = J + v.' * v;
        end
        qT = x(2*T+1:2*T+2);
        diffGoal = qT - qGoal;
        J = J + diffGoal.' * diffGoal;
    end

    function [c, ceq] = nonlinConstraints(x)
        % Example: start configuration fixed
        q0 = x(1:2);
        ceq = [q0 - qStart];
        % c contains inequality constraints (e.g. collision distances >= 0)
        c = [];
    end

    options = optimoptions('fmincon','Algorithm','sqp','Display','iter');
    lb = -inf(2*(T+1),1);
    ub =  inf(2*(T+1),1);

    [xOpt, Jstar] = fmincon(@costFun, x0, [], [], [], [], lb, ub, @nonlinConstraints, options);
    fprintf('Optimal cost: %f\n', Jstar);

    % In a full TAMP pipeline, nonlinConstraints would call collision checking
    % and geometric grasp/place constraints, and Simulink could be used to
    % simulate closed-loop tracking of the optimized trajectory.
end
      

8.3 Mathematica Mixed-Integer Example with NMinimize

Mathematica can solve small mixed-integer trajectory problems directly using NMinimize with integer and boolean variables:

T = 4;
qvars = Table[q[t], {t, 0, T}];
holdvars = Table[h[t], {t, 0, T}];

cost[q_List] := Sum[(q[[t + 2]] - q[[t + 1]])^2, {t, 0, T - 1}] +
                 (q[[-1]] - 1)^2;

constraints = {
  q[0] == 0,
  (* logical evolution of hold variable *)
  holdvars[[1]] == 0,
  Table[
    holdvars[[t + 2]] == holdvars[[t + 1]] + grasp[t] - place[t],
    {t, 0, T - 1}
  ],
  (* binary decisions *)
  Table[grasp[t] ∈ {0, 1}, {t, 0, T - 1}],
  Table[place[t] ∈ {0, 1}, {t, 0, T - 1}]
};

vars = Join[qvars, holdvars, Table[grasp[t], {t, 0, T - 1}],
            Table[place[t], {t, 0, T - 1}]];

NMinimize[{cost[qvars], constraints}, vars,
          Integers -> Table[grasp[t], {t, 0, T - 1}] ~Join~
                      Table[place[t], {t, 0, T - 1}]]
      

Here we sketched the structure of a 1D pick-and-place problem with discrete grasp and place decisions. In a more realistic setting, the cost and constraints would be vector-valued in joint space and would encode geometric predicates for contacts and collisions.

Problem 1 (Reduction to Trajectory Optimization). Consider the hybrid TAMP formulation of Section 2. Show that for any fixed discrete sequence \( \bar{\mathbf{d}} \), the optimization over \( \mathbf{x} \) is equivalent to a (possibly nonconvex) trajectory optimization problem as studied in Chapter 4. Explicitly characterize the variables, objective, and constraints of this reduced problem.

Solution. For a fixed discrete sequence \( \bar{\mathbf{d}} \), the mixed problem

\[ \min_{\mathbf{x}} J(\mathbf{x},\bar{\mathbf{d}}) \ \text{s.t.}\ \mathbf{f}_k(\mathbf{q}_k,\mathbf{q}_{k+1},\mathbf{u}_k,\bar{\mathbf{d}}_k)=\mathbf{0},\ \mathbf{g}_k(\mathbf{q}_k,\mathbf{u}_k,\bar{\mathbf{d}}_k)\le\mathbf{0},\ \mathbf{h}_k(\mathbf{q}_k,\bar{\mathbf{d}}_k)\le\mathbf{0} \]

has only continuous decision variables \( \mathbf{x} = (\mathbf{q}_{0:T},\mathbf{u}_{0:T-1}) \), with objective \( J(\mathbf{x},\bar{\mathbf{d}}) \) and equality and inequality constraints exactly of the type considered in standard trajectory optimization (dynamics, joint limits, collision avoidance). The dependence on \( \bar{\mathbf{d}} \) only appears as fixed parameters in \( \mathbf{f}_k,\mathbf{g}_k,\mathbf{h}_k \). Therefore, for each \( \bar{\mathbf{d}} \) we obtain an ordinary finite-dimensional constrained NLP in the continuous variables, i.e., a trajectory optimization problem.

Problem 2 (Correctness of Big-“M” Encoding). Let \( c(\mathbf{q}) \) be a scalar geometric predicate such that the constraint \( c(\mathbf{q}) \le 0 \) should hold only when a binary variable \( y \) equals 1. Consider the big-\( M \) encoding

\[ c(\mathbf{q}) \le M\,(1-y),\quad y \in \{0,1\}. \]

(a) Show that if \( y = 1 \), then the original constraint \( c(\mathbf{q}) \le 0 \) is enforced. (b) Give a sufficient condition on \( M \) under which the constraint is inactive when \( y = 0 \).

Solution. (a) If \( y = 1 \), then the inequality becomes \( c(\mathbf{q}) \le M\,(1-1) = 0 \), which is exactly the original constraint. (b) If \( y = 0 \), the inequality becomes \( c(\mathbf{q}) \le M \). To ensure that this imposes no restriction, it is sufficient that \( M \ge \sup_{\mathbf{q}\in\mathcal{X}} c(\mathbf{q}) \), where \( \mathcal{X} \) is the feasible set of interest for \( \mathbf{q} \). In that case, every feasible \( \mathbf{q} \) satisfies \( c(\mathbf{q}) \le M \), so the constraint is vacuous when \( y = 0 \).

Problem 3 (KKT Conditions for a Fixed Skeleton). For a fixed discrete mode sequence, consider the trajectory optimization subproblem

\[ \min_{\mathbf{x}} J(\mathbf{x}) \ \text{s.t.}\ \mathbf{f}(\mathbf{x}) = \mathbf{0},\ \mathbf{g}(\mathbf{x}) \le \mathbf{0}. \]

Derive the KKT conditions and explain the complementarity condition \( \mu_i\,g_i(\mathbf{x}) = 0 \) in terms of contact constraints in a manipulation problem.

Solution. Introducing Lagrange multipliers \( \boldsymbol{\lambda} \) for equality and \( \boldsymbol{\mu} \ge \mathbf{0} \) for inequality constraints, the Lagrangian is

\[ \mathcal{L}(\mathbf{x},\boldsymbol{\lambda},\boldsymbol{\mu}) = J(\mathbf{x}) + \boldsymbol{\lambda}^\top \mathbf{f}(\mathbf{x}) + \boldsymbol{\mu}^\top \mathbf{g}(\mathbf{x}). \]

The KKT conditions are:

\[ \nabla_{\mathbf{x}}\mathcal{L}(\mathbf{x}^\star,\boldsymbol{\lambda}^\star,\boldsymbol{\mu}^\star) = \mathbf{0},\quad \mathbf{f}(\mathbf{x}^\star)=\mathbf{0},\quad \mathbf{g}(\mathbf{x}^\star)\le\mathbf{0},\quad \boldsymbol{\mu}^\star\ge\mathbf{0},\quad \mu_i^\star\,g_i(\mathbf{x}^\star)=0. \]

The complementarity condition states that each inequality is either inactive (\( g_i(\mathbf{x}^\star) < 0 \)), in which case the multiplier is zero (\( \mu_i^\star = 0 \)), or active (\( g_i(\mathbf{x}^\star)=0 \)), in which case the corresponding multiplier may be positive. For a contact constraint \( g_i(\mathbf{x}) \le 0 \) describing signed distance between bodies, this expresses that there is either a separating gap with zero contact force (\( g_i < 0,\ \mu_i=0 \)) or a closed contact with possibly nonzero normal force (\( g_i=0,\ \mu_i \ge 0 \)).

Problem 4 (Value Function as a Lower Bound in Branch-and-Bound). Let \( \mathbf{d}_{0:k} \) be a partial assignment of discrete actions for steps \( 0,\dots,k \). Define the value function

\[ V^\star(\mathbf{d}_{0:k}) = \min_{\mathbf{x},\,\mathbf{d}_{k+1:T}} J(\mathbf{x},\mathbf{d}) \ \text{s.t. consistency with } \mathbf{d}_{0:k}. \]

Argue why \( V^\star(\mathbf{d}_{0:k}) \) is a lower bound on the cost of any complete extension of \( \mathbf{d}_{0:k} \) and how this justifies pruning nodes in branch-and-bound.

Solution. Any complete assignment \( \tilde{\mathbf{d}} \) that extends \( \mathbf{d}_{0:k} \) is feasible for the optimization defining \( V^\star(\mathbf{d}_{0:k}) \); the latter minimizes over all such extensions. Therefore

\[ J(\tilde{\mathbf{x}},\tilde{\mathbf{d}}) \ge V^\star(\mathbf{d}_{0:k}) \]

for any feasible pair \( (\tilde{\mathbf{x}},\tilde{\mathbf{d}}) \) with the given prefix. If an incumbent solution with cost \( J_\text{inc} \) is known and \( V^\star(\mathbf{d}_{0:k}) \ge J_\text{inc} \), no completion of \( \mathbf{d}_{0:k} \) can yield a better solution, so the branch can be safely pruned without losing optimality.

In this lesson we formulated Task and Motion Planning as a hybrid discrete–continuous optimization problem, highlighted logic–geometric and mixed-integer encodings of symbolic constraints, and connected TAMP to the trajectory optimization methods studied earlier in the course. We discussed solution approaches based on branch-and-bound over nonlinear trajectory optimizers and examined KKT conditions for the continuous subproblems. Finally, we sketched small implementations across several programming languages, illustrating how optimization-based TAMP can be integrated into real robot software stacks.