We consider a fixed-base manipulator with configuration \( q \in \mathcal{C} \subset \mathbb{R}^n \) operating over a table, manipulating a single rigid object. The object has pose \( p_o \in SE(3) \). The goal is to move the object from an initial stable placement to a target stable placement using a pick-and-place sequence: approach, grasp, transfer, and release, while remaining collision-free.

We introduce a discrete mode variable \( m \in \mathcal{M} := \{\text{transit}, \text{transfer}\} \) indicating whether the robot is holding the object. The hybrid state is

\[ x = (q, p_o, m) \in \mathcal{X} := \mathcal{C} \times SE(3) \times \mathcal{M}. \]

The lab objectives are:

• Formalize pick-and-place as a hybrid optimal planning problem.
• Define a symbolic task model (actions, predicates) consistent with the hybrid state.
• Implement a TAMP pipeline that: (i) finds a symbolic action skeleton, and (ii) refines it into continuous collision-free trajectories.
• Provide minimal but concrete code skeletons in multiple languages.

flowchart TD
  G["Task goal: 'object at target region'"] --> S["Symbolic planner: action skeleton"]
  S --> R["Sequence e.g. ['move_home', 'pick', 'move_goal', 'place']"]
  R --> C["For each action: sample grasps / placements / IK"]
  C --> M["Motion planner: collision-free paths"]
  M --> V["Validate full hybrid plan"]
  V --> OUT["Execute pick-and-place"]
        

We separate modes according to contact status. Let \( \mathcal{M} = \{m_0, m_1\} \) where:

• \( m_0 \): transit mode, gripper empty, object resting on table.
• \( m_1 \): transfer mode, object rigidly attached to gripper.

For each mode \( m \), define a feasible configuration manifold:

\[ \mathcal{X}_m = \{ (q, p_o, m) \mid \text{kinematics satisfied, collision-free, contacts consistent with } m \}. \]

We assume quasi-static motion and encode dynamics only as velocity bounds. A hybrid trajectory is a sequence of continuous paths and mode switches:

\[ \Gamma = \bigl( (m_1, \gamma_1), (m_2, \gamma_2), \dots, (m_K, \gamma_K) \bigr), \]

where each \( \gamma_k : [0,1] \to \mathcal{X}_{m_k} \) is continuous and successive paths connect at mode-switch states in \( \mathcal{X}_{m_k} \cap \mathcal{X}_{m_{k+1}} \) (e.g., at stable grasps or placements).

The cost of a hybrid plan is a sum of segment costs, e.g. path lengths:

\[ J(\Gamma) = \sum_{k=1}^{K} \int_0^{1} \ell\bigl(\gamma_k(t), \dot{\gamma}_k(t)\bigr)\, dt \]

subject to:

\[ \gamma_1(0) = x_{\text{init}}, \quad \gamma_K(1) \in X_{\text{goal}}, \quad \gamma_k(t) \in \mathcal{X}_{m_k} \ \forall t \in [0,1]. \]

A key structural fact (already used conceptually in earlier lessons) can be stated more rigorously:

Lemma (Transit/Transfer Decomposition). Assume:

• The environment is static and piecewise analytic.
• Contacts occur only at a finite set of stable placements and grasps.

Then any feasible pick-and-place hybrid trajectory admits a representation where mode switches occur only at stable placements and grasps, and modes alternate transit/transfer.

Sketch of justification. Contacts can be regularized so that any transition through intermediate, non-stable contacts can be deformed (while maintaining feasibility) into a sequence where contact onset/offset occurs only at isolated stable configurations. Between such configurations, no contact changes occur, hence the mode is constant, yielding the alternating decomposition.

We next define a symbolic abstraction consistent with the hybrid system. Let the discrete state consist of:

• Predicate \( \text{At}(o, s) \): object \( o \) is at surface pose symbol \( s \).
• Predicate \( \text{Holding}(o) \): gripper rigidly holds object \( o \).
• Predicate \( \text{EmptyHand} \): gripper is empty.

The hybrid state \( x = (q, p_o, m) \) is mapped to these symbols via an abstraction map \( \alpha : \mathcal{X} \to \mathcal{S} \), where \( \mathcal{S} \) is the set of symbolic states. For example, if \( p_o \) lies within tolerance of surface patch \( s \) and \( m = m_0 \), we set \( \text{At}(o, s) \) to true.

Consider actions move, pick, and place with PDDL-like schemas:

# PDDL-like schemas (conceptual, not executable)
(:action move
 :parameters (?r - robot ?from - conf ?to - conf)
 :precondition (and (Reachable ?r ?from ?to))
 :effect (and (AtConf ?r ?to) (not (AtConf ?r ?from))))

(:action pick
 :parameters (?r - robot ?o - object ?s - surface ?g - grasp)
 :precondition (and (At ?o ?s)
                    (EmptyHand)
                    (GraspReachable ?r ?o ?s ?g))
 :effect (and (Holding ?o)
              (not (At ?o ?s))
              (not (EmptyHand))))

(:action place
 :parameters (?r - robot ?o - object ?s - surface ?g - grasp)
 :precondition (and (Holding ?o)
                    (PlaceReachable ?r ?o ?s ?g))
 :effect (and (At ?o ?s)
              (EmptyHand)
              (not (Holding ?o))))
      

The lab uses any off-the-shelf classical planner (e.g., from earlier task-planning exercises) to generate an action skeleton:

\[ \sigma = (a_1, \dots, a_T) \in \mathcal{A}^T \]

such as \( \sigma = (\text{move_home}, \text{pick}, \text{move_goal}, \text{place}) \). This skeleton is sound at the symbolic level if each action is applicable under the abstract preconditions and yields the desired goal formula.

For each symbolic action instance we must choose continuous parameters: robot configurations, grasp poses, and object placements. Let \( \theta \) denote the collection of these continuous parameters (e.g., grasp frames, pre-grasp offsets).

For a pick action, we require:

• A grasp transform \( T_{go} \in SE(3) \) in the object frame.
• An IK solution \( q_{\text{grasp}} \) such that the end-effector pose matches \( T_{we}(q_{\text{grasp}}) = T_{wo} T_{go} \).
• A collision-free approach path \( \gamma_{\text{approach}} \) from the current configuration to \( q_{\text{grasp}} \) in mode \( m_0 \).

Similarly, for place we require a target placement pose \( p_o^{\text{goal}} \) on the table and a transfer path in mode \( m_1 \). These become motion-planning subproblems of the form:

\[ \text{Find } \gamma : [0,1] \to \mathcal{C}_m \text{ such that } \gamma(0) = q_{\text{start}},\ \gamma(1) = q_{\text{goal}}, \]

where \( \mathcal{C}_m \subset \mathcal{C} \) is the mode-specific collision-free configuration space (with the object either static on the table or attached to the gripper).

In this lab we reuse sampling-based planners from previous chapters (e.g., RRT-Connect or RRT*) as black-box solvers for each such subproblem, while still reasoning about their probabilistic completeness: if a solution exists and the planner samples configurations dense in \( \mathcal{C}_m \), the probability of failure after \( N \) samples tends to zero as \( N \to \infty \).

We now couple the symbolic skeleton and geometric refinement. Let \( \Pi_{\text{sym}} \) denote the symbolic planner and \( \Pi_{\text{geo}} \) the geometric planner (for each mode). A simple hierarchical algorithm is:

1. Use \( \Pi_{\text{sym}} \) to obtain a candidate skeleton \( \sigma \).
2. For each action \( a_t \in \sigma \), sample continuous parameters \( \theta_t \) (grasps, placements).
3. Call \( \Pi_{\text{geo}} \) to find paths \( \gamma_t \) under the corresponding mode.
4. If any geometric refinement fails, record a geometric dead-end and request an alternative skeleton or different continuous parameters.
5. If all refinements succeed, concatenate to obtain a hybrid plan \( \Gamma \).

flowchart TD
  A["Symbolic planner"] --> B["Action skeleton sigma"]
  B --> C["Sample continuous params for each action"]
  C --> D["Geometric planner per action"]
  D --> E["All segments feasible?"]
  E -->|yes| F["Concatenate to hybrid plan Gamma"]
  E -->|no| G["Record failure, backtrack / resample"]
  G --> B
  

Formally, denote by \( \mathcal{F}(\sigma) \) the event that there exists at least one assignment of continuous parameters such that all subproblems are feasible. The integrated algorithm is probabilistically complete under assumptions:

• The symbolic planner is complete over a finite state/action space.
• For any geometrically feasible skeleton, the sampling measures for \( \theta_t \) have support on a set of positive volume in the feasible parameter set.
• The motion planners \( \Pi_{\text{geo}} \) for each mode are probabilistically complete.

Under these conditions, the probability that the algorithm fails to find a solution, if one exists, tends to zero as the number of symbolic/continuous samples grows.

We now sketch a Python implementation. We assume that:

• rrt_connect was implemented in Chapter 3 (sampling-based planning lab).
• A simple collision checker and kinematics interface are available.
• A symbolic planner (e.g. pyperplan, or a custom STRIPS planner) can return action skeletons.

import numpy as np

# ------------------------------
# Geometric layer abstractions
# ------------------------------

class World:
    def __init__(self, robot_model, obstacles, table, object_mesh):
        self.robot_model = robot_model
        self.obstacles = obstacles
        self.table = table
        self.object_mesh = object_mesh

    def is_collision(self, q, mode, object_pose):
        # mode == "transit" or "transfer"
        # Implement robot-object-environment collision check
        return False  # placeholder

    def fk_end_effector(self, q):
        # Forward kinematics for the end-effector frame
        raise NotImplementedError

    def ik(self, target_pose, q_seed):
        # Return an IK solution if possible, else None
        raise NotImplementedError


class GeometricPlanner:
    def __init__(self, world):
        self.world = world

    def plan_path(self, q_start, q_goal, mode, object_pose):
        from rrt_connect import rrt_connect  # your previous implementation

        def sampler():
            # Sample joint configuration in limits
            low, high = self.world.robot_model.joint_limits()
            return np.random.uniform(low, high)

        def is_free(q):
            return not self.world.is_collision(q, mode, object_pose)

        path = rrt_connect(q_start, q_goal, sampler, is_free)
        return path  # list of configurations or None

    def sample_grasp_and_ik(self, object_pose, q_seed):
        # Very simple top-down grasp sampling
        grasp_poses = []
        for angle in np.linspace(0.0, 2.0 * np.pi, 8):
            grasp_poses.append(self._top_down_grasp(object_pose, angle))

        for gp in grasp_poses:
            q_grasp = self.world.ik(gp, q_seed)
            if q_grasp is not None and not self.world.is_collision(q_grasp, "transit", object_pose):
                return gp, q_grasp
        return None, None

    def _top_down_grasp(self, object_pose, angle):
        # Construct a simple SE(3) transform for top-down grasp at rotation "angle"
        # around the object z-axis.
        raise NotImplementedError


# ------------------------------
# Symbolic layer abstractions
# ------------------------------

class SymbolicState:
    def __init__(self, at_surface, holding):
        self.at_surface = at_surface  # e.g., "start_region" or "goal_region"
        self.holding = holding        # object name or None

class SymbolicPlanner:
    def plan(self, init_state, goal_predicate):
        # Use any classical planner to get an action skeleton
        # For this lab, return a hard-coded skeleton:
        return [
            ("move", "home", "pre_pick"),
            ("pick", "object", "start_region"),
            ("move", "pre_pick", "pre_place"),
            ("place", "object", "goal_region")
        ]


# ------------------------------
# TAMP Integration
# ------------------------------

class TAMPPickAndPlace:
    def __init__(self, world, geo_planner, sym_planner):
        self.world = world
        self.geo = geo_planner
        self.sym = sym_planner

    def plan(self, q_init, object_pose_init, goal_region_pose):
        init_state = SymbolicState(at_surface="start_region", holding=None)
        goal_predicate = ("At", "object", "goal_region")

        skeleton = self.sym.plan(init_state, goal_predicate)
        object_pose = object_pose_init
        q_current = q_init
        hybrid_plan = []

        for (action, *params) in skeleton:
            if action == "move":
                q_from, q_to_name = params
                # For simplicity, map symbolic names to concrete configs
                q_goal = self._named_config(q_to_name)
                path = self.geo.plan_path(q_current, q_goal, mode="transit", object_pose=object_pose)
                if path is None:
                    return None
                hybrid_plan.append(("move", path))
                q_current = q_goal

            elif action == "pick":
                obj, surf = params
                grasp_pose, q_grasp = self.geo.sample_grasp_and_ik(object_pose, q_current)
                if q_grasp is None:
                    return None

                path_to_grasp = self.geo.plan_path(q_current, q_grasp, mode="transit", object_pose=object_pose)
                if path_to_grasp is None:
                    return None

                hybrid_plan.append(("pick", path_to_grasp, grasp_pose))
                q_current = q_grasp

            elif action == "place":
                obj, surf = params
                object_pose = goal_region_pose
                q_place = self._named_config("place_pose")
                path_transfer = self.geo.plan_path(q_current, q_place, mode="transfer", object_pose=object_pose)
                if path_transfer is None:
                    return None
                hybrid_plan.append(("place", path_transfer, goal_region_pose))
                q_current = q_place

        return hybrid_plan

    def _named_config(self, name):
        # Map a symbolic name to a robot configuration, e.g. from a database
        raise NotImplementedError
      

Students should fill in the world-specific functions (is_collision, fk_end_effector, ik, and _top_down_grasp), and connect to either a simulation environment (e.g. PyBullet) or a real manipulator.

We now mirror the structure in C++. We assume a configuration-space planner similar to OMPL's geometric::SimpleSetup has been wrapped for our manipulator, and that kinematics/collision-checking are available.

// High-level skeleton (non-compilable without your specific headers)

struct PoseSE3 {
    Eigen::Matrix3d R;
    Eigen::Vector3d t;
};

enum class Mode { Transit, Transfer };

class World {
public:
    bool isCollision(const Eigen::VectorXd& q, Mode mode, const PoseSE3& objectPose) const;
    bool ikSolve(const PoseSE3& target, const Eigen::VectorXd& qSeed, Eigen::VectorXd& qSol) const;
};

class GeometricPlanner {
public:
    GeometricPlanner(const World& world) : world_(world) {}

    bool planPath(const Eigen::VectorXd& qStart,
                  const Eigen::VectorXd& qGoal,
                  Mode mode,
                  const PoseSE3& objectPose,
                  std::vector<Eigen::VectorXd>& path) const {
        // Wrap OMPL or your own RRT here
        // Set state validity checker using world_.isCollision
        return true; // placeholder
    }

    bool sampleGraspAndIk(const PoseSE3& objectPose,
                          const Eigen::VectorXd& qSeed,
                          PoseSE3& graspPose,
                          Eigen::VectorXd& qGrasp) const {
        const int numSamples = 8;
        for (int i = 0; i != numSamples; ++i) {
            double angle = 2.0 * M_PI * static_cast<double>(i) / static_cast<double>(numSamples);
            graspPose = topDownGrasp(objectPose, angle);
            if (world_.ikSolve(graspPose, qSeed, qGrasp) &&
                !world_.isCollision(qGrasp, Mode::Transit, objectPose)) {
                return true;
            }
        }
        return false;
    }

private:
    PoseSE3 topDownGrasp(const PoseSE3& objectPose, double angle) const {
        PoseSE3 g;
        // Construct a grasp pose relative to objectPose
        return g;
    }

    const World& world_;
};

struct SymbolicAction {
    std::string name;
    std::vector<std::string> params;
};

class SymbolicPlanner {
public:
    std::vector<SymbolicAction> plan() const {
        std::vector<SymbolicAction> sigma;
        sigma.push_back({"move", {"home", "pre_pick"}});
        sigma.push_back({"pick", {"object", "start_region"}});
        sigma.push_back({"move", {"pre_pick", "pre_place"}});
        sigma.push_back({"place", {"object", "goal_region"}});
        return sigma;
    }
};

class TAMPPipeline {
public:
    TAMPPipeline(const World& w,
                 const GeometricPlanner& g,
                 const SymbolicPlanner& s)
      : world_(w), geo_(g), sym_(s) {}

    bool plan(const Eigen::VectorXd& qInit,
              const PoseSE3& objectPoseInit,
              const PoseSE3& goalRegionPose) {
        auto sigma = sym_.plan();
        PoseSE3 objectPose = objectPoseInit;
        Eigen::VectorXd qCurrent = qInit;

        for (const auto& a : sigma) {
            if (a.name == "move") {
                Eigen::VectorXd qGoal = namedConfig(a.params[1]);
                std::vector<Eigen::VectorXd> path;
                if (!geo_.planPath(qCurrent, qGoal, Mode::Transit, objectPose, path)) {
                    return false;
                }
                qCurrent = qGoal;
                // store path in hybrid plan structure
            } else if (a.name == "pick") {
                PoseSE3 graspPose;
                Eigen::VectorXd qGrasp;
                if (!geo_.sampleGraspAndIk(objectPose, qCurrent, graspPose, qGrasp)) {
                    return false;
                }
                std::vector<Eigen::VectorXd> path;
                if (!geo_.planPath(qCurrent, qGrasp, Mode::Transit, objectPose, path)) {
                    return false;
                }
                qCurrent = qGrasp;
                // insert pick segment
            } else if (a.name == "place") {
                objectPose = goalRegionPose;
                Eigen::VectorXd qPlace = namedConfig("place_pose");
                std::vector<Eigen::VectorXd> path;
                if (!geo_.planPath(qCurrent, qPlace, Mode::Transfer, objectPose, path)) {
                    return false;
                }
                qCurrent = qPlace;
                // insert place segment
            }
        }
        return true;
    }

private:
    Eigen::VectorXd namedConfig(const std::string& name) const {
        // Lookup named configuration
        return Eigen::VectorXd();
    }

    const World& world_;
    const GeometricPlanner& geo_;
    const SymbolicPlanner& sym_;
};
      

This skeleton emphasizes the separation of concerns: World for geometry, GeometricPlanner for continuous planning, SymbolicPlanner for discrete planning, and TAMPPipeline for integration.

Java is often used when integrating with large scale middleware. The structure mirrors Python and C++ but stresses interfaces and encapsulation. Assume that you have existing classes for kinematics and collision checking.

public enum Mode {
    TRANSIT,
    TRANSFER
}

public class PoseSE3 {
    // Rotation and translation stored as 4x4 homogeneous matrix
    public double[][] T;
}

public interface World {
    boolean isCollision(double[] q, Mode mode, PoseSE3 objectPose);
    double[] ikSolve(PoseSE3 target, double[] qSeed); // returns null if no solution
}

public interface GeoPlanner {
    double[][] planPath(double[] qStart, double[] qGoal, Mode mode, PoseSE3 objectPose);
    GraspIkResult sampleGraspAndIk(PoseSE3 objectPose, double[] qSeed);
}

public class GraspIkResult {
    public PoseSE3 graspPose;
    public double[] qGrasp;
}

public class SymbolicAction {
    public String name;
    public String[] params;

    public SymbolicAction(String name, String... params) {
        this.name = name;
        this.params = params;
    }
}

public interface SymbolicPlanner {
    java.util.List<SymbolicAction> plan();
}

public class SimpleSymbolicPlanner implements SymbolicPlanner {
    @Override
    public java.util.List<SymbolicAction> plan() {
        java.util.List<SymbolicAction> sigma = new java.util.ArrayList<SymbolicAction>();
        sigma.add(new SymbolicAction("move", "home", "pre_pick"));
        sigma.add(new SymbolicAction("pick", "object", "start_region"));
        sigma.add(new SymbolicAction("move", "pre_pick", "pre_place"));
        sigma.add(new SymbolicAction("place", "object", "goal_region"));
        return sigma;
    }
}

public class TAMPPipeline {
    private final World world;
    private final GeoPlanner geo;
    private final SymbolicPlanner sym;

    public TAMPPipeline(World world, GeoPlanner geo, SymbolicPlanner sym) {
        this.world = world;
        this.geo = geo;
        this.sym = sym;
    }

    public boolean plan(double[] qInit, PoseSE3 objectPoseInit, PoseSE3 goalRegionPose) {
        java.util.List<SymbolicAction> sigma = sym.plan();
        PoseSE3 objectPose = objectPoseInit;
        double[] qCurrent = qInit;

        for (SymbolicAction a : sigma) {
            if (a.name.equals("move")) {
                double[] qGoal = namedConfig(a.params[1]);
                double[][] path = geo.planPath(qCurrent, qGoal, Mode.TRANSIT, objectPose);
                if (path == null) {
                    return false;
                }
                qCurrent = qGoal;
            } else if (a.name.equals("pick")) {
                GraspIkResult res = geo.sampleGraspAndIk(objectPose, qCurrent);
                if (res == null) {
                    return false;
                }
                double[][] path = geo.planPath(qCurrent, res.qGrasp, Mode.TRANSIT, objectPose);
                if (path == null) {
                    return false;
                }
                qCurrent = res.qGrasp;
            } else if (a.name.equals("place")) {
                objectPose = goalRegionPose;
                double[] qPlace = namedConfig("place_pose");
                double[][] path = geo.planPath(qCurrent, qPlace, Mode.TRANSFER, objectPose);
                if (path == null) {
                    return false;
                }
                qCurrent = qPlace;
            }
        }
        return true;
    }

    private double[] namedConfig(String name) {
        // Resolve symbolic name to configuration
        return new double[0];
    }
}
      

Students should implement an actual GeoPlanner backed by a Java-based sampling planner and fill in the World interface with kinematics and collision logic.

MATLAB is convenient for prototyping planning algorithms; Simulink can be used to simulate execution of the generated trajectories. Below is a simplified MATLAB script organizing the motion layer; it assumes the symbolic skeleton has already been computed.

function hybridPlan = tamp_pick_and_place(world, skeleton, qInit, objectPoseInit, goalRegionPose)
% world: struct with handles for collision, kinematics, etc.

qCurrent = qInit;
objectPose = objectPoseInit;
hybridPlan = {};

for k = 1:numel(skeleton)
    action = skeleton{k};
    name = action.name;
    params = action.params;

    switch name
        case 'move'
            qGoal = named_config(params{2});
            path = rrt_connect_matlab(world, qCurrent, qGoal, 'transit', objectPose);
            if isempty(path)
                hybridPlan = {};
                return;
            end
            hybridPlan{end + 1} = struct('type', 'move', 'path', {path});
            qCurrent = qGoal;

        case 'pick'
            [graspPose, qGrasp] = sample_grasp_and_ik(world, objectPose, qCurrent);
            if isempty(qGrasp)
                hybridPlan = {};
                return;
            end
            pathToGrasp = rrt_connect_matlab(world, qCurrent, qGrasp, 'transit', objectPose);
            if isempty(pathToGrasp)
                hybridPlan = {};
                return;
            end
            hybridPlan{end + 1} = struct('type', 'pick', ...
                                         'path', {pathToGrasp}, ...
                                         'graspPose', graspPose);
            qCurrent = qGrasp;

        case 'place'
            objectPose = goalRegionPose;
            qPlace = named_config('place_pose');
            pathTransfer = rrt_connect_matlab(world, qCurrent, qPlace, 'transfer', objectPose);
            if isempty(pathTransfer)
                hybridPlan = {};
                return;
            end
            hybridPlan{end + 1} = struct('type', 'place', ...
                                         'path', {pathTransfer}, ...
                                         'goalPose', goalRegionPose);
            qCurrent = qPlace;
    end
end
end

function qGoal = named_config(name)
% Map a symbolic name to a configuration, e.g. loaded from a MAT file
qGoal = []; % TODO
end

function path = rrt_connect_matlab(world, qStart, qGoal, mode, objectPose)
% Thin MATLAB wrapper around your Chapter 3 planner
path = {}; % TODO
end

function [graspPose, qGrasp] = sample_grasp_and_ik(world, objectPose, qSeed)
% Sample a few top-down grasps and call IK
graspPose = [];
qGrasp = [];
end
      

To connect this with Simulink, export each path segment as a time-parameterized joint trajectory (e.g. via cubic splines) and feed it into a Simulink model that contains:

• A joint-space trajectory generator block.
• A manipulator dynamics block (or a kinematic block if dynamics are abstracted).
• Visualization or scope blocks to monitor joint tracking.

Mathematica can be used for the discrete search over grasp and placement samples and for verifying feasibility via external calls to simulators or kinematic libraries. Below is an abstracted notebook-style function.

ClearAll[tampPickAndPlace]

tampPickAndPlace[
  world_,
  skeleton_List,
  qInit_List,
  objectPoseInit_,
  goalRegionPose_] := Module[
  {qCurrent = qInit, objectPose = objectPoseInit, hybridPlan = {}, res,
   action, name, params, path, graspPose, qGrasp, qPlace},

  Do[
    action = skeleton[[k]];
    name = action["name"];
    params = action["params"];

    If[name === "move",
      qPlace = namedConfig[params[[2]]];
      path = rrtConnectWorld[world, qCurrent, qPlace, "transit", objectPose];
      If[path === $Failed, Return[$Failed]];
      AppendTo[hybridPlan, <|"type" -> "move", "path" -> path|>];
      qCurrent = qPlace;,
    Null];

    If[name === "pick",
      res = sampleGraspAndIk[world, objectPose, qCurrent];
      If[res === $Failed, Return[$Failed]];
      graspPose = res["graspPose"];
      qGrasp = res["qGrasp"];
      path = rrtConnectWorld[world, qCurrent, qGrasp, "transit", objectPose];
      If[path === $Failed, Return[$Failed]];
      AppendTo[hybridPlan, <|"type" -> "pick",
        "path" -> path, "graspPose" -> graspPose|>];
      qCurrent = qGrasp;,
    Null];

    If[name === "place",
      objectPose = goalRegionPose;
      qPlace = namedConfig["placePose"];
      path = rrtConnectWorld[world, qCurrent, qPlace, "transfer", objectPose];
      If[path === $Failed, Return[$Failed]];
      AppendTo[hybridPlan, <|"type" -> "place",
        "path" -> path, "goalPose" -> goalRegionPose|>];
      qCurrent = qPlace;,
    Null];

    ,
    {k, Length[skeleton]}
  ];

  hybridPlan
]

namedConfig[name_String] := (* lookup table for named configurations *) {}

rrtConnectWorld[world_, qStart_, qGoal_, mode_, objectPose_] :=
  (* wrapper around an external planner, returning a list of configurations *) $Failed

sampleGraspAndIk[world_, objectPose_, qSeed_] :=
  (* call into kinematics code to sample grasps and solve IK *) $Failed
      

This code uses association structures and assumes external functions rrtConnectWorld and sampleGraspAndIk that could be implemented via ExternalEvaluate or library linking to existing robotics software.

Problem 1 (Mode Sequence Cardinality Bound). Suppose we discretize the set of stable placements of the object into \( N_p \) candidates and the set of grasps into \( N_g \) candidates. For single-object pick-and-place with an initial and final placement and a single pick and place, bound the number of distinct mode sequences and continuous parameter combinations the TAMP pipeline must consider under this discretization.

Solution. The symbolic mode sequence is fixed up to renaming: transit → transfer → transit, so there is only one mode pattern. However, each pattern is parameterized by:

• A choice of grasp \( g \in \{1, \dots, N_g\} \).
• A choice of initial placement (fixed by the problem) and final placement \( p \in \{1, \dots, N_p\} \).

Thus the number of parameter combinations is at most \( N_g N_p \). This bounds the number of distinct hybrid path families the algorithm needs to sample over (ignoring path discretization). If we allow re-grasps at intermediate placements, the bound increases combinatorially in the number of intermediate placements but remains finite under a finite discretization.

Problem 2 (Soundness of Geometric Refinement). Consider a TAMP pipeline that first computes a symbolic plan \( \sigma \) and then performs geometric refinement. Assume that for each symbolic action instance all geometric preconditions are correctly encoded in the abstract precondition (by existential quantification over continuous parameters). Prove that if the geometric refinement succeeds for every action in \( \sigma \), then the resulting hybrid execution satisfies the original goal predicate.

Solution. Let \( s_0 \) be the initial symbolic state and \( s_t \) the forward application of \( a_t \) in \( \sigma \) under the symbolic transition relation. Because the symbolic planner is sound, the final state \( s_T \) satisfies the goal formula by construction. By hypothesis, each geometric refinement selects continuous parameters that satisfy the corresponding geometric preconditions, so the abstraction map \( \alpha \) of the resulting continuous trajectory segment matches the symbolic transition. Inductively, after applying all actions with their refined trajectories, the hybrid state maps to \( s_T \), so the goal predicate holds for the hybrid execution as well.

Problem 3 (Failure Certificates). Suppose the geometric planner proves that for all grasps in a finite set \( G \) and placements in a finite set \( P \), no collision-free path exists for a particular symbolic skeleton \( \sigma \). Explain how this can be used as a refinement constraint on the symbolic planner to prune future skeletons.

Solution. The negative result can be viewed as a clause of the form “for any instantiation of actions in \( \sigma \) whose parameters are restricted to \( G \) and \( P \), geometric feasibility fails.” This condition can be expressed as an additional constraint on the preconditions of certain actions, e.g. by forbidding particular combinations of GraspReachable and PlaceReachable facts corresponding to \( G \times P \). When fed back to the symbolic planner, subsequent searches will avoid producing skeletons that require these infeasible combinations, reducing redundant geometric effort.

Problem 4 (Probabilistic Completeness of the Pipeline). Assume:

• The symbolic planner is complete over a finite domain.
• Each geometric planner for mode-specific motion is probabilistically complete.
• Grasp and placement samplers draw from distributions with full support on the true feasible sets (restricted to a finite discrete approximation).

Argue that the overall TAMP pipeline for pick-and-place is probabilistically complete.

Solution. Let \( \Gamma^\star \) be a feasible hybrid solution with skeleton \( \sigma^\star \) and continuous parameters \( \theta^\star \). Because the discrete domain is finite and the symbolic planner is complete, the probability that the planner never returns \( \sigma^\star \) tends to zero as the number of symbolic searches grows. Conditional on \( \sigma^\star \), the continuous parameters are drawn with positive probability in any neighborhood of \( \theta^\star \) due to full-support sampling. For each subproblem, probabilistic completeness of the geometric planner implies that the probability that it fails to find a path when one exists tends to zero as samples increase. Combining these events across a finite number of segments yields that the probability of failing to discover \( \Gamma^\star \) tends to zero as both symbolic and geometric sampling budgets grow, establishing probabilistic completeness.

Problem 5 (Cost Decomposition). Suppose each transit and transfer path cost is the standard path length in joint space:

\[ J(\Gamma) = \sum_{k=1}^{K} \int_0^{1} \bigl\|\dot{q}_k(t)\bigr\|_2\, dt. \]

Show that for fixed grasp and placement parameters, minimizing \( J(\Gamma) \) over hybrid paths decomposes into independent minimizations over each segment \( \gamma_k \).

Solution. For fixed grasp and placement parameters, mode switch states (segment endpoints) are fixed. The admissible set of hybrid trajectories is then the Cartesian product of admissible sets for each segment. The cost is a sum of segment functionals \( J(\Gamma) = \sum_k J_k(\gamma_k) \) where \( J_k(\gamma_k) = \int_0^1 \|\dot{q}_k(t)\|_2\, dt \). Since there is no cross-coupling term between segments, the global minimum is attained by minimizing each \( J_k \) independently over its own feasible set, yielding an optimal hybrid trajectory composed of segment-wise optimal paths.

This lab instantiated the abstract TAMP concepts of earlier lessons in a concrete pick-and-place setting. We:

• Modeled pick-and-place as a hybrid planning problem over transit and transfer modes.
• Defined a consistent symbolic abstraction with preconditions and effects for move, pick, and place.
• Coupled symbolic skeleton generation with geometric refinement through sampling of grasps, placements, and collision-free trajectories.
• Explored implementation skeletons across Python, C++, Java, MATLAB/Simulink, and Mathematica.

Subsequent lessons can extend this pipeline to multiple objects, re-grasps, and more complex long-horizon tasks, as well as connect to learning-based components for grasp synthesis or heuristic guidance.