In pure motion planning, the state is typically a configuration \( \mathbf{q} \in \mathcal{C} \), and the goal is a subset \( \mathcal{C}_{\text{goal}} \subset \mathcal{C} \). In TAMP, we also need to capture discrete information such as “which object is held?”, “which contacts are active?”, or “which doors are open?”. A natural representation is a hybrid state \( s = (m, x) \), where:

• \( m \in \mathcal{M} \) is a discrete mode (symbolic or contact state).
• \( x \in \mathcal{X}_m \subset \mathbb{R}^n \) is a continuous state (robot joint angles, object poses, etc.) consistent with mode \( m \).

For example, in a pick-and-place task with a single object, we might use modes \( m \in \{\text{transit}, \text{transfer}\} \): in \( \text{transit} \) the robot moves with an empty gripper, while in \( \text{transfer} \) the object is rigidly attached to the end-effector. Each mode induces different geometric constraints and collision sets.

A TAMP algorithm then searches for a hybrid trajectory \( t \mapsto (m(t), x(t)) \) that starts from a hybrid initial state, obeys mode constraints and transitions, and reaches a goal condition that may involve both symbolic facts and geometric relationships.

flowchart TD
  TS["Task specification (logical goals)"] --> MS["Search over mode sequence"]
  MS --> GP["Continuous planner inside each mode"]
  GP --> CHK["Check guards and geometric constraints"]
  CHK -->|success| PLAN["Hybrid plan (m(t), x(t))"]
  CHK -->|failure| REF["Refine mode sequence or samples"]
  REF --> MS
        

We formalize a hybrid model tailored to TAMP. Let \( \mathcal{M} = \{ m_1, \dots, m_K \} \) be a finite set of modes. For each mode \( m \in \mathcal{M} \) we have a mode-specific continuous state space \( \mathcal{X}_m \subset \mathbb{R}^n \).

\[ \mathcal{X} \;=\; \bigcup_{m \in \mathcal{M}} \{m\} \times \mathcal{X}_m. \]

Each \( \mathcal{X}_m \) is typically defined by smooth equality and inequality constraints:

\[ \mathcal{X}_m \;=\; \{\, x \in \mathbb{R}^n \mid h_m(x) = 0,\; g_m(x) \le 0 \,\}, \]

where \( h_m : \mathbb{R}^n \to \mathbb{R}^{p_m} \) encodes holonomic constraints (e.g., contact kinematics, grasp frames) and \( g_m : \mathbb{R}^n \to \mathbb{R}^{q_m} \) encodes inequality constraints such as non-penetration or joint limits.

To include dynamics (if needed), each mode possesses a vector field \( f_m \):

\[ \dot{x}(t) \;=\; f_m(x(t), u(t)), \quad x(t) \in \mathcal{X}_m, \quad u(t) \in \mathcal{U}_m, \]

with \( \mathcal{U}_m \) the admissible controls in mode \( m \). In many manipulator TAMP problems we restrict attention to quasi-static or kinematic models, so \( f_m \) is derived from the kinematic relationship between joint velocities and task-space velocities.

A TAMP-specific hybrid system can be summarized as the tuple \( H = (\mathcal{M}, \{\mathcal{X}_m\}_{m\in\mathcal{M}}, \mathcal{U}, f, \mathcal{G}, R) \), where:

• \( \mathcal{M} \): finite mode set.
• \( \mathcal{X}_m \): continuous state manifold per mode.
• \( \mathcal{U} = \bigcup_{m} \mathcal{U}_m \): control set.
• \( f = \{ f_m \}_{m\in\mathcal{M}} \): mode-dependent dynamics or kinematics.
• \( \mathcal{G} \), \( R \): guard sets and reset maps (described next).

A mode switch is allowed only on a guard set, and it is accompanied (in general) by a reset map on the continuous state. For a pair of modes \( (m, m') \) we define:

\[ \mathcal{G}_{m,m'} \;\subset\; \mathcal{X}_m, \qquad R_{m,m'} : \mathcal{G}_{m,m'} \to \mathcal{X}_{m'}. \]

Intuitively, \( \mathcal{G}_{m,m'} \) captures the set of states in which an action (e.g., pick, place, open-door) is both symbolically applicable and geometrically feasible, while \( R_{m,m'} \) encodes instantaneous changes in state (such as switching from free motion to rigidly attached motion).

A hybrid trajectory over horizon \( [0, T] \) is a pair of functions \( (m(\cdot), x(\cdot)) \) with a finite sequence of switching times:

\[ 0 = t_0 < t_1 < \cdots < t_K = T, \]

such that:

\[ \begin{aligned} &m(t) = m_k &&\text{for } t \in [t_k, t_{k+1}), \\[0.5em] &\dot{x}(t) = f_{m_k}(x(t), u(t)) &&\text{for } t \in (t_k, t_{k+1}),\quad x(t) \in \mathcal{X}_{m_k}, \\[0.5em] &x(t_k^+) = R_{m_{k-1}, m_k}(x(t_k^-)) &&\text{with } x(t_k^-)\in \mathcal{G}_{m_{k-1}, m_k}. \end{aligned} \]

The TAMP problem typically adds a performance criterion (cost functional) on hybrid trajectories. A common form is:

\[ J[m(\cdot), x(\cdot), u(\cdot)] \;=\; \sum_{k=0}^{K-1} \int_{t_k}^{t_{k+1}} L_{m_k}(x(t), u(t))\,\mathrm{d}t \;+\; \sum_{k=1}^{K-1} c_{m_{k-1}, m_k}\!\big(x(t_k^-)\big), \]

where \( L_{m_k} \) is a running cost (e.g., energy or path length in mode \( m_k \)) and \( c_{m_{k-1}, m_k} \) penalizes mode switches (e.g., for minimizing grasp changes).

Consider a rigid manipulator in a workspace with a single movable object. Let \( \mathbf{q} \in \mathcal{C}_R \) denote the robot configuration and \( p_o \in \mathbb{R}^6 \) a minimal parameterization of the object pose. We define the continuous state \( x = (\mathbf{q}, p_o) \in \mathbb{R}^{n+6} \).

We use two primary modes: \( m_{\text{transit}} \) (object resting on the table), and \( m_{\text{transfer}} \) (object rigidly grasped).

Mode 1: Transit.

\[ \mathcal{X}_{\text{transit}} \;=\; \{\, (\mathbf{q}, p_o) \mid \phi_{\text{table}}(p_o) = 0,\; \psi_{\text{collision}}(\mathbf{q}, p_o) \le 0 \,\}, \]

where \( \phi_{\text{table}}(p_o) = 0 \) encodes the object resting stably on the support surface and \( \psi_{\text{collision}} \le 0 \) enforces collision-free constraints between robot, object, and environment.

Mode 2: Transfer.

Let \( T_{\text{ee}}(\mathbf{q}) \in SE(3) \) be the pose of the end-effector and \( p_{\text{grasp}} \in SE(3) \) a fixed transform representing the grasp frame on the object. The transfer mode enforces a rigid grasp:

\[ \mathcal{X}_{\text{transfer}} \;=\; \{\, (\mathbf{q}, p_o) \mid T_{\text{ee}}(\mathbf{q})^{-1} p_o = p_{\text{grasp}},\; \psi_{\text{collision}}(\mathbf{q}, p_o) \le 0 \,\}. \]

Pick guard and reset. A pick action is admissible when the end-effector and object are at a pre-grasp configuration, captured by a guard set:

\[ \mathcal{G}_{\text{transit},\text{transfer}} \;=\; \{\, (\mathbf{q}, p_o) \in \mathcal{X}_{\text{transit}} \mid \chi_{\text{pregrasp}}(\mathbf{q}, p_o) = 0 \,\}, \]

where \( \chi_{\text{pregrasp}} \) encodes a small admissible region around a chosen grasp pose. The reset \( R_{\text{transit},\text{transfer}} \) often leaves the continuous state unchanged (it merely reinterprets the constraints):

\[ R_{\text{transit},\text{transfer}}(\mathbf{q}, p_o) \;=\; (\mathbf{q}, p_o). \]

Analogously, the place guard \( \mathcal{G}_{\text{transfer},\text{transit}} \) enforces object pose compatibility with the support surface and releases the grasp constraint. In practice, many TAMP algorithms first search over a mode skeleton such as \( [\text{transit}, \text{transfer}, \text{transit}] \), then solve continuous motions within each segment.

The set of modes and possible switches can be summarized by a mode graph. Nodes are modes \( m \in \mathcal{M} \) and directed edges represent non-empty guard sets:

\[ (m, m') \in E \quad \Longleftrightarrow \quad \mathcal{G}_{m,m'} \neq \emptyset. \]

A mode sequence or mode skeleton is a finite walk \( (m_0, m_1, \dots, m_K) \) in this graph, starting from an initial mode \( m_0 \) and ending in a goal-compatible mode \( m_K \). For each consecutive pair \( (m_k, m_{k+1}) \) we must find states \( x_k \in \mathcal{X}_{m_k} \) and \( x_k^+ \in \mathcal{X}_{m_{k+1}} \) with \( x_k \in \mathcal{G}_{m_k, m_{k+1}} \) and \( x_k^+ = R_{m_k,m_{k+1}}(x_k) \).

Let \( P_m \) be a motion planner (e.g., PRM or RRT) that operates inside mode \( m \) and returns a continuous path inside \( \mathcal{X}_m \) or failure. Assume:

• (A1) The set of modes reachable from the initial mode is finite.
• (A2) Each planner \( P_m \) is resolution-complete: if a path exists between two states in \( \mathcal{X}_m \), then as sampling resolution is refined, \( P_m \) eventually returns a valid path.
• (A3) The TAMP algorithm enumerates mode sequences in non-decreasing length and tries to connect successive segments using the appropriate \( P_m \).

Theorem (Resolution completeness in hybrid space).

Under assumptions (A1)–(A3), if there exists a feasible hybrid trajectory that reaches the goal set, then there exists a finite resolution such that the TAMP algorithm will eventually find a hybrid plan.

Sketch of proof.

Let \( (m^\star(\cdot), x^\star(\cdot)) \) be a feasible hybrid trajectory with switching times \( 0 = t_0^\star < t_1^\star < \dots < t_{K^\star}^\star = T^\star \) and mode sequence \( (m_0^\star, \dots, m_{K^\star}^\star) \). By (A1), this sequence is among the finite set of reachable mode sequences. By (A3), there exists some finite iteration where the algorithm enumerates exactly this sequence. On each interval \( [t_k^\star, t_{k+1}^\star] \) the trajectory lies in \( \mathcal{X}_{m_k^\star} \), so a continuous path exists. By (A2), there is a resolution at which \( P_{m_k^\star} \) will succeed in reproducing a path that stays within \( \mathcal{X}_{m_k^\star} \). Concatenating these paths and applying the reset maps at the guards yields a valid hybrid solution with the same mode sequence. Hence for sufficiently fine resolution the algorithm must find a plan.

In practice, completeness can be lost through pruning or heuristic search over modes, but the theorem clarifies which assumptions are needed to lift continuous resolution-completeness to the hybrid level.

flowchart TD
  H0["Initial hybrid state (m0, x0)"] --> EXP["Expand successor modes"]
  EXP --> GUARD["Filter by guard sets G_{m,m'}"]
  GUARD --> PLANSEG["Call motion planner in each candidate mode"]
  PLANSEG --> GOALH["Goal hybrid state reached?"]
  GOALH -->|yes| DONE["Return concatenated hybrid plan"]
  GOALH -->|no| FRONT["Insert new states in search frontier"]
  FRONT --> EXP
        

To illustrate hybrid state search concretely, we implement a simple grid-world TAMP problem. A point robot moves on a \( W \times H \) grid with one object. Modes: \( \text{TRANSIT} \) (robot not holding the object) and \( \text{TRANSFER} \) (robot holding the object). The hybrid state is: \( s = (m, x_r, y_r, x_o, y_o) \). Guards correspond to “pick” and “place” events.

from collections import deque

# Discrete modes
MODE_TRANSIT = 0    # robot moves without object
MODE_TRANSFER = 1   # robot carries object

W, H = 7, 7  # grid dimensions

FREE = 0
OBST = 1

# Simple environment
grid = [[FREE for _ in range(W)] for _ in range(H)]
grid[3][3] = OBST  # one obstacle cell

start_robot = (0, 0)
start_obj   = (2, 2)
goal_obj    = (6, 6)

class HybridState:
    __slots__ = ("mode", "rx", "ry", "ox", "oy")

    def __init__(self, mode, rx, ry, ox, oy):
        self.mode = mode
        self.rx = rx
        self.ry = ry
        self.ox = ox
        self.oy = oy

    def key(self):
        return (self.mode, self.rx, self.ry, self.ox, self.oy)

    def __repr__(self):
        return f"HybridState(mode={self.mode}, r=({self.rx},{self.ry}), o=({self.ox},{self.oy}))"

def in_bounds(x, y):
    return 0 <= x and x < W and 0 <= y and y < H

def is_free(x, y):
    return in_bounds(x, y) and grid[y][x] == FREE

def neighbors(state):
    """Generate successors of a hybrid state."""
    succ = []

    # 4-connected motion
    for dx, dy in [(1,0), (-1,0), (0,1), (0,-1)]:
        nx = state.rx + dx
        ny = state.ry + dy
        if not is_free(nx, ny):
            continue

        if state.mode == MODE_TRANSIT:
            # object stays where it is
            ns = HybridState(MODE_TRANSIT, nx, ny, state.ox, state.oy)
        else:
            # object moves with robot
            ns = HybridState(MODE_TRANSFER, nx, ny, nx, ny)
        succ.append(("move", ns))

    # pick: transit -> transfer
    if state.mode == MODE_TRANSIT and (state.rx, state.ry) == (state.ox, state.oy):
        ns = HybridState(MODE_TRANSFER, state.rx, state.ry, state.rx, state.ry)
        succ.append(("pick", ns))

    # place: transfer -> transit at goal
    if state.mode == MODE_TRANSFER and (state.rx, state.ry) == goal_obj:
        ns = HybridState(MODE_TRANSIT, state.rx, state.ry, state.rx, state.ry)
        succ.append(("place", ns))

    return succ

def is_goal(state):
    return state.mode == MODE_TRANSIT and (state.ox, state.oy) == goal_obj

def bfs_hybrid():
    start = HybridState(MODE_TRANSIT, start_robot[0], start_robot[1],
                        start_obj[0], start_obj[1])
    frontier = deque([start])
    parent = {start.key(): (None, None)}  # key -> (parent_key, action)

    while frontier:
        s = frontier.popleft()
        if is_goal(s):
            # reconstruct plan
            plan = []
            cur = s.key()
            while parent[cur][0] is not None:
                par, act = parent[cur]
                plan.append((act, cur))
                cur = par
            plan.reverse()
            return s, plan

        for act, ns in neighbors(s):
            k = ns.key()
            if k not in parent:
                parent[k] = (s.key(), act)
                frontier.append(ns)
    return None, []

if __name__ == "__main__":
    goal_state, plan = bfs_hybrid()
    if goal_state is None:
        print("No hybrid plan found.")
    else:
        print("Goal state:", goal_state)
        print("Number of steps:", len(plan))
        for act, key in plan:
            print(act, key)
      

This example implements a fully discrete approximation of the hybrid TAMP problem: continuous spaces are discretized into grid cells, and guards are simple equalities (robot at object, robot at goal). Nevertheless, the same hybrid abstractions (modes, guards, reset maps) carry over to continuous C-space planners.

Below is a C++ skeleton that mirrors the Python example. It defines a HybridState structure, an enum for modes, and a simple BFS over the hybrid state space. Collision checking and environment setup can be adapted to a full 2D or configuration-space planner using robotics libraries such as OMPL.

#include <iostream>
#include <vector>
#include <queue>
#include <unordered_map>
#include <tuple>

enum class Mode { Transit, Transfer };

struct HybridState {
    Mode mode;
    int rx, ry;  // robot position
    int ox, oy;  // object position

    bool operator==(const HybridState& other) const {
        return mode == other.mode &&
               rx == other.rx && ry == other.ry &&
               ox == other.ox && oy == other.oy;
    }
};

struct HybridStateHash {
    std::size_t operator()(HybridState const& s) const noexcept {
        std::size_t h1 = std::hash<int>{}(static_cast<int>(s.mode));
        std::size_t h2 = std::hash<int>{}(s.rx);
        std::size_t h3 = std::hash<int>{}(s.ry);
        std::size_t h4 = std::hash<int>{}(s.ox);
        std::size_t h5 = std::hash<int>{}(s.oy);
        return (((h1 ^ (h2 << 1)) ^ (h3 << 1)) ^ (h4 << 1)) ^ (h5 << 1);
    }
};

const int W = 7;
const int H = 7;
int grid[H][W];

bool inBounds(int x, int y) {
    return 0 <= x && x < W && 0 <= y && y < H;
}

bool isFree(int x, int y) {
    return inBounds(x, y) && grid[y][x] == 0;
}

std::vector<std::pair<std::string, HybridState>>
neighbors(const HybridState& s, std::pair<int,int> goalObj) {
    std::vector<std::pair<std::string, HybridState>> succ;

    // 4-connected motion
    const int dirs[4][2] = { {1,0},{-1,0},{0,1},{0,-1} };
    for (auto& d : dirs) {
        int nx = s.rx + d[0];
        int ny = s.ry + d[1];
        if (!isFree(nx, ny)) continue;

        HybridState ns;
        if (s.mode == Mode::Transit) {
            ns = {Mode::Transit, nx, ny, s.ox, s.oy};
        } else {
            ns = {Mode::Transfer, nx, ny, nx, ny};
        }
        succ.push_back({"move", ns});
    }

    // pick
    if (s.mode == Mode::Transit && s.rx == s.ox && s.ry == s.oy) {
        HybridState ns{Mode::Transfer, s.rx, s.ry, s.rx, s.ry};
        succ.push_back({"pick", ns});
    }

    // place
    if (s.mode == Mode::Transfer &&
        s.rx == goalObj.first && s.ry == goalObj.second) {
        HybridState ns{Mode::Transit, s.rx, s.ry, s.rx, s.ry};
        succ.push_back({"place", ns});
    }

    return succ;
}

int main() {
    // initialize grid (0 = free, 1 = obstacle)
    for (int y = 0; y < H; ++y)
        for (int x = 0; x < W; ++x)
            grid[y][x] = 0;
    grid[3][3] = 1;

    std::pair<int,int> startRobot{0,0};
    std::pair<int,int> startObj{2,2};
    std::pair<int,int> goalObj{6,6};

    HybridState start{Mode::Transit, startRobot.first, startRobot.second,
                      startObj.first, startObj.second};

    std::queue<HybridState> frontier;
    frontier.push(start);

    std::unordered_map<HybridState,
        std::pair<HybridState, std::string>, HybridStateHash> parent;
    parent[start] = {start, "start"};

    auto isGoal = [&goalObj](const HybridState& s) {
        return s.mode == Mode::Transit &&
               s.ox == goalObj.first && s.oy == goalObj.second;
    };

    bool found = false;
    HybridState goal = start;

    while (!frontier.empty()) {
        HybridState s = frontier.front();
        frontier.pop();

        if (isGoal(s)) {
            found = true;
            goal = s;
            break;
        }

        for (auto& [act, ns] : neighbors(s, goalObj)) {
            if (parent.find(ns) == parent.end()) {
                parent[ns] = {s, act};
                frontier.push(ns);
            }
        }
    }

    if (!found) {
        std::cout << "No hybrid plan found.\n";
        return 0;
    }

    // Reconstruct and print plan
    std::vector<std::pair<std::string, HybridState>> plan;
    HybridState cur = goal;
    while (!(cur == start)) {
        auto it = parent.find(cur);
        const HybridState& par = it->second.first;
        const std::string& act = it->second.second;
        plan.push_back({act, cur});
        cur = par;
    }
    std::reverse(plan.begin(), plan.end());

    std::cout << "Plan length: " << plan.size() << "\n";
    for (auto& step : plan) {
        std::cout << step.first << " : "
                  << "mode=" << (step.second.mode == Mode::Transit ? "T" : "C")
                  << " r=(" << step.second.rx << "," << step.second.ry << ")"
                  << " o=(" << step.second.ox << "," << step.second.oy << ")"
                  << "\n";
    }

    return 0;
}
      

This code can be extended to higher-dimensional configurations by replacing grid-based int coordinates with continuous vectors and using a continuous motion planner (e.g., via OMPL) within each mode.

The following Java code sketches a similar hybrid BFS. In a full TAMP system, one would replace the grid world with a configuration-space representation and plug in a geometric planner inside each mode.

import java.util.*;

enum Mode {
    TRANSIT,
    TRANSFER
}

final class HybridState {
    final Mode mode;
    final int rx, ry;
    final int ox, oy;

    HybridState(Mode mode, int rx, int ry, int ox, int oy) {
        this.mode = mode;
        this.rx = rx;
        this.ry = ry;
        this.ox = ox;
        this.oy = oy;
    }

    @Override
    public boolean equals(Object o) {
        if (!(o instanceof HybridState)) return false;
        HybridState s = (HybridState) o;
        return mode == s.mode &&
               rx == s.rx && ry == s.ry &&
               ox == s.ox && oy == s.oy;
    }

    @Override
    public int hashCode() {
        return Objects.hash(mode, rx, ry, ox, oy);
    }

    @Override
    public String toString() {
        return String.format("HybridState(%s, r=(%d,%d), o=(%d,%d))",
                             mode, rx, ry, ox, oy);
    }
}

public class HybridGridPlanner {
    static final int W = 7, H = 7;
    static final int FREE = 0, OBST = 1;
    static int[][] grid = new int[H][W];

    static boolean inBounds(int x, int y) {
        return 0 <= x && x < W && 0 <= y && y < H;
    }

    static boolean isFree(int x, int y) {
        return inBounds(x, y) && grid[y][x] == FREE;
    }

    static List<AbstractMap.SimpleEntry<String, HybridState>>
    neighbors(HybridState s, int gx, int gy) {
        List<AbstractMap.SimpleEntry<String, HybridState>> succ =
            new ArrayList<>();

        int[][] dirs = { {1,0},{-1,0},{0,1},{0,-1} };
        for (int[] d : dirs) {
            int nx = s.rx + d[0];
            int ny = s.ry + d[1];
            if (!isFree(nx, ny)) continue;

            HybridState ns;
            if (s.mode == Mode.TRANSIT) {
                ns = new HybridState(Mode.TRANSIT, nx, ny, s.ox, s.oy);
            } else {
                ns = new HybridState(Mode.TRANSFER, nx, ny, nx, ny);
            }
            succ.add(new AbstractMap.SimpleEntry<>("move", ns));
        }

        if (s.mode == Mode.TRANSIT && s.rx == s.ox && s.ry == s.oy) {
            HybridState ns = new HybridState(Mode.TRANSFER, s.rx, s.ry, s.rx, s.ry);
            succ.add(new AbstractMap.SimpleEntry<>("pick", ns));
        }

        if (s.mode == Mode.TRANSFER && s.rx == gx && s.ry == gy) {
            HybridState ns = new HybridState(Mode.TRANSIT, s.rx, s.ry, s.rx, s.ry);
            succ.add(new AbstractMap.SimpleEntry<>("place", ns));
        }

        return succ;
    }

    public static void main(String[] args) {
        // Initialize grid
        for (int y = 0; y < H; ++y)
            Arrays.fill(grid[y], FREE);
        grid[3][3] = OBST;

        int sx = 0, sy = 0;
        int ox = 2, oy = 2;
        int gx = 6, gy = 6;

        HybridState start = new HybridState(Mode.TRANSIT, sx, sy, ox, oy);

        Queue<HybridState> frontier = new ArrayDeque<>();
        frontier.add(start);

        Map<HybridState, AbstractMap.SimpleEntry<HybridState, String>> parent =
            new HashMap<>();
        parent.put(start, new AbstractMap.SimpleEntry<>(start, "start"));

        HybridState goal = null;

        while (!frontier.isEmpty()) {
            HybridState s = frontier.remove();
            if (s.mode == Mode.TRANSIT && s.ox == gx && s.oy == gy) {
                goal = s;
                break;
            }

            for (AbstractMap.SimpleEntry<String, HybridState> e :
                    neighbors(s, gx, gy)) {
                String act = e.getKey();
                HybridState ns = e.getValue();
                if (!parent.containsKey(ns)) {
                    parent.put(ns, new AbstractMap.SimpleEntry<>(s, act));
                    frontier.add(ns);
                }
            }
        }

        if (goal == null) {
            System.out.println("No hybrid plan found.");
            return;
        }

        List<AbstractMap.SimpleEntry<String, HybridState>> plan = new ArrayList<>();
        HybridState cur = goal;
        while (!cur.equals(start)) {
            AbstractMap.SimpleEntry<HybridState, String> par = parent.get(cur);
            plan.add(new AbstractMap.SimpleEntry<>(par.getValue(), cur));
            cur = par.getKey();
        }
        Collections.reverse(plan);

        System.out.println("Plan length: " + plan.size());
        for (AbstractMap.SimpleEntry<String, HybridState> step : plan) {
            System.out.println(step.getKey() + " : " + step.getValue());
        }
    }
}
      

MATLAB can represent hybrid states using structures and implement mode switching either in plain code or via Stateflow (for graphical hybrid automata). The following script mirrors the grid-based hybrid search in MATLAB.

function hybrid_tamp_grid
    W = 7; H = 7;
    FREE = 0; OBST = 1;

    grid = FREE * ones(H, W);
    grid(4,4) = OBST; % MATLAB indices are 1-based

    startRobot = [1, 1];   % (x,y)
    startObj   = [3, 3];
    goalObj    = [7, 7];

    MODE_TRANSIT  = 0;
    MODE_TRANSFER = 1;

    start.mode = MODE_TRANSIT;
    start.rx = startRobot(1); start.ry = startRobot(2);
    start.ox = startObj(1);   start.oy = startObj(2);

    % BFS frontier
    frontier = {start};
    parent = containers.Map();
    parent_key = state_key(start);
    parent(parent_key) = struct('parent', '', 'action', 'start');

    goalFound = false;
    goalState = start;

    while ~isempty(frontier)
        s = frontier{1};
        frontier(1) = [];

        if is_goal(s, goalObj)
            goalFound = true;
            goalState = s;
            break;
        end

        succ = neighbors(s, grid, goalObj, MODE_TRANSIT, MODE_TRANSFER, FREE);
        for i = 1:numel(succ)
            ns = succ(i).state;
            act = succ(i).action;
            k = state_key(ns);
            if ~isKey(parent, k)
                parent(k) = struct('parent', state_key(s), 'action', act);
                frontier{end+1} = ns;
            end
        end
    end

    if ~goalFound
        fprintf('No hybrid plan found.\n');
        return;
    end

    % Reconstruct plan
    plan = {};
    curKey = state_key(goalState);
    while ~strcmp(parent(curKey).parent, '')
        par = parent(curKey);
        plan{end+1} = struct('action', par.action, 'key', curKey); %#ok<AGROW>
        curKey = par.parent;
    end
    plan = fliplr(plan);

    fprintf('Plan length: %d\n', numel(plan));
    for i = 1:numel(plan)
        fprintf('%s : %s\n', plan{i}.action, plan{i}.key);
    end
end

function flag = is_goal(s, goalObj)
    flag = (s.mode == 0) && (s.ox == goalObj(1)) && (s.oy == goalObj(2));
end

function k = state_key(s)
    k = sprintf('m%d_rx%d_ry%d_ox%d_oy%d', s.mode, s.rx, s.ry, s.ox, s.oy);
end

function S = neighbors(s, grid, goalObj, MODE_TRANSIT, MODE_TRANSFER, FREE)
    dirs = [1 0; -1 0; 0 1; 0 -1];
    [H, W] = size(grid);
    S = struct('action', {}, 'state', {});

    for i = 1:size(dirs,1)
        nx = s.rx + dirs(i,1);
        ny = s.ry + dirs(i,2);
        if nx < 1 || nx > W || ny < 1 || ny > H
            continue;
        end
        if grid(ny, nx) ~= FREE
            continue;
        end

        ns = s;
        ns.rx = nx; ns.ry = ny;
        if s.mode == MODE_TRANSIT
            % object stays
        else
            ns.ox = nx; ns.oy = ny;
        end
        S(end+1).action = 'move'; %#ok<AGROW>
        S(end).state = ns;
    end

    if s.mode == MODE_TRANSIT && s.rx == s.ox && s.ry == s.oy
        ns = s;
        ns.mode = MODE_TRANSFER;
        S(end+1).action = 'pick'; %#ok<AGROW>
        S(end).state = ns;
    end

    if s.mode == MODE_TRANSFER && s.rx == goalObj(1) && s.ry == goalObj(2)
        ns = s;
        ns.mode = MODE_TRANSIT;
        S(end+1).action = 'place'; %#ok<AGROW>
        S(end).state = ns;
    end
end
      

In Simulink/Stateflow, the same logic can be expressed graphically as a chart whose states correspond to TRANSIT and TRANSFER, with transitions guarded by logical expressions such as (rx == ox && ry == oy) for a pick.

Mathematica can represent hybrid systems by combining discrete state variables with piecewise dynamics and event handling. The following snippet encodes a simple 1D hybrid system with two modes and an event-triggered switch; the same pattern can be extended to manipulator TAMP settings.

(* Discrete mode variable: 0 = transit, 1 = transfer *)
ClearAll[mode, x, u, t];

(* Simple kinematic dynamics in each mode *)
f[m_, x_, u_] := Which[
  m == 0, u,
  m == 1, 0.5 u,
  True, 0
];

(* Continuous control input *)
uControl[t_] := 1.0;

(* Hybrid simulation with one mode switch at x == 1.0 *)
sol = NDSolve[
  {
    x'[t] == f[mode[t], x[t], uControl[t]],
    x[0] == 0.0,
    mode[0] == 0,
    WhenEvent[x[t] >= 1.0 && mode[t] == 0,
      {
        mode[t] -> 1
      }
    ]
  },
  {x, mode},
  {t, 0, 5},
  DiscreteVariables -> {mode}
];

Plot[Evaluate[x[t] /. sol], {t, 0, 5},
  AxesLabel -> {"t", "x(t)"}
]
      

Here mode[t] is treated as a discrete variable whose value changes when the guard x[t] >= 1.0 is satisfied. In a TAMP context, the guard would correspond to contact or grasp conditions, and the dynamics f would be derived from the robot's kinematics or dynamics model in each mode.

Problem 1 (Hybrid state space as a disjoint union). Let \( \mathcal{M} = \{m_1,\dots,m_K\} \) and \( \mathcal{X}_{m_k} \subset \mathbb{R}^n \) be pairwise disjoint subsets. Show that the hybrid space \( \mathcal{X} = \bigcup_{m\in\mathcal{M}} \{m\} \times \mathcal{X}_m \) is a disjoint union and that each element has a unique mode.

Solution.

Take any element \( s \in \mathcal{X} \). By definition, \( s \in \{m\} \times \mathcal{X}_m \) for some \( m \in \mathcal{M} \). Hence we can write \( s = (m, x) \) with \( x \in \mathcal{X}_m \). Suppose also \( s = (m', x') \in \{m'\} \times \mathcal{X}_{m'} \). Equality of ordered pairs implies \( m = m' \) and \( x = x' \). Since the sets \( \mathcal{X}_m \) are pairwise disjoint, this is the only possibility. Therefore any element of \( \mathcal{X} \) belongs to exactly one component \( \{m\} \times \mathcal{X}_m \), giving a disjoint union and a unique mode label.

Problem 2 (Soundness of mode-skeleton TAMP). Consider a sequence of modes \( (m_0,\dots,m_K) \) and states \( x_k \in \mathcal{X}_{m_k} \) such that \( x_k \in \mathcal{G}_{m_{k},m_{k+1}} \) and \( x_{k+1} = R_{m_k,m_{k+1}}(x_k) \). Assume for each \( k \) there exists a continuous path \( \gamma_k : [0,1] \to \mathcal{X}_{m_k} \) with \( \gamma_k(0) = x_k \) and \( \gamma_k(1) = x_{k+1} \). Show that concatenating these paths and reset maps yields a valid hybrid trajectory.

Solution.

Define switching times by \( t_k = k \) for \( k = 0,\dots,K \), so that \( [t_k,t_{k+1}] = [k,k+1] \). For \( t \in [t_k,t_{k+1}] \) set \( m(t) = m_k \) and \( x(t) = \gamma_k(t-k) \). Then \( x(t) \in \mathcal{X}_{m_k} \) for all such \( t \) by construction of \( \gamma_k \). At switching time \( t_{k+1} \) we have \( x(t_{k+1}^-) = \gamma_k(1) = x_{k+1} \). Since \( x_k \in \mathcal{G}_{m_k,m_{k+1}} \) and \( x_{k+1} = R_{m_k,m_{k+1}}(x_k) \), the reset condition holds. The resulting \( (m(\cdot), x(\cdot)) \) satisfies the hybrid trajectory definition with modes \( m_k \), continuous segments \( \gamma_k \), and reset maps \( R_{m_k,m_{k+1}} \), hence is a valid hybrid solution.

Problem 3 (Contact-mode constraints in pick-and-place). In the pick-and-place example, let \( q \) be the robot configuration and \( p_o \) the object pose. Assume the table surface is the plane \( z = 0 \) in world coordinates and the object must rest upright when on the table. Write explicit equality constraints for \( \phi_{\text{table}}(p_o) \) and for a grasp constraint \( T_{\text{ee}}(q)^{-1} p_o = p_{\text{grasp}} \).

Solution.

Let \( p_o = (x_o, y_o, z_o, \theta_o, \phi_o, \psi_o) \) be a minimal 6D parameterization of the object pose (position and Euler angles). For an upright object resting on the table at height zero, we can define:

\[ \phi_{\text{table}}(p_o) \;=\; \begin{bmatrix} z_o \\ \theta_o \\ \phi_o \end{bmatrix}, \]

and require \( \phi_{\text{table}}(p_o) = 0 \). This enforces that the center of mass lies in the plane and that the object has no roll or pitch relative to the table frame.

For the grasp constraint, let \( T_{\text{ee}}(q) \in SE(3) \) and \( p_{\text{grasp}} \in SE(3) \) be homogeneous transforms. The equality \( T_{\text{ee}}(q)^{-1} p_o = p_{\text{grasp}} \) can be written in coordinates as six scalar equations (three for position, three for orientation). For example, if \( T_{\text{ee}}(q)^{-1} p_o \) has position component \( t = (t_x,t_y,t_z) \) and Euler angles \( (\alpha,\beta,\gamma) \), then:

\[ h_{\text{grasp}}(q, p_o) \;=\; \begin{bmatrix} t_x - t_x^{\text{grasp}} \\ t_y - t_y^{\text{grasp}} \\ t_z - t_z^{\text{grasp}} \\ \alpha - \alpha^{\text{grasp}} \\ \beta - \beta^{\text{grasp}} \\ \gamma - \gamma^{\text{grasp}} \end{bmatrix} \;=\; 0, \]

where the superscript “grasp” denotes the desired relative pose in the gripper frame. Thus \( \mathcal{X}_{\text{transfer}} \) is defined by \( h_{\text{grasp}}(q,p_o)=0 \) together with collision inequalities.

Problem 4 (Hybrid cost decomposition). Let a hybrid trajectory with modes \( m_0,\dots,m_K \) and switching times \( t_0 < \cdots < t_K \) have cost

\[ J = \sum_{k=0}^{K-1} \int_{t_k}^{t_{k+1}} L_{m_k}(x(t),u(t))\,\mathrm{d}t + \sum_{k=1}^{K-1} c_{m_{k-1},m_k}\!\big(x(t_k^-)\big). \]

Suppose we fix the sequence of modes and switching times, and that the reset maps are identity. Show that minimizing \( J \) over controls \( u(\cdot) \) decouples into \( K \) separate optimal control problems, one per mode.

Solution.

With fixed \( m_k \) and \( t_k \), the only variables are the controls (and thus the induced continuous paths) on each sub-interval. The discrete switching costs \( c_{m_{k-1},m_k}(x(t_k^-)) \) become fixed once the boundary states \( x(t_k^-) \) are fixed. If the reset maps are identity and the boundary states are specified, then \( c_{m_{k-1},m_k}(x(t_k^-)) \) is independent of the interior controls on each interval. Hence:

\[ J \;=\; \sum_{k=0}^{K-1} J_k \;+\; \underbrace{\sum_{k=1}^{K-1} c_{m_{k-1},m_k}\!\big(x(t_k^-)\big)}_{\text{constant}}, \quad J_k \;=\; \int_{t_k}^{t_{k+1}} L_{m_k}(x(t),u(t))\,\mathrm{d}t. \]

Minimizing \( J \) is then equivalent to minimizing each \( J_k \) separately, because the sum of constants does not affect the argmin. Thus, under the stated assumptions, hybrid optimal control decomposes into independent optimizations in each mode.

In this lesson we introduced hybrid state spaces for TAMP, where discrete modes (symbolic or contact-level) are coupled with continuous state manifolds and mode-specific constraints. We defined guard sets and reset maps that govern admissible mode switches, and characterized hybrid trajectories and cost functionals. We then showed how a finite mode graph and resolution-complete continuous planners can yield resolution-complete TAMP algorithms under suitable assumptions. Finally, we implemented simple hybrid planners in Python, C++, Java, MATLAB/Simulink, and Mathematica, illustrating how hybrid search can be realized algorithmically even in discrete grid approximations. These constructs provide the mathematical backbone for sampling-based and optimization-based TAMP algorithms developed in later lessons.