In this chapter you already studied analytical and data-driven grasp generation and the notion of grasp quality in wrench space. Here we move from single grasp selection to sequences of grasps that transform an object from an initial to a desired pose using a dexterous hand (or multi-arm system). We distinguish:

• Re-grasp: sequences that break and form new contacts (e.g., place object, change grasp, pick again).
• In-hand manipulation: continuous motion where the object moves relative to the palm without leaving the hand, possibly with finger gaiting.

Let \( \mathcal{X} \) denote the continuous state space and \( \mathcal{M} \) the discrete contact mode space. A manipulation plan is a hybrid trajectory

\[ \gamma = \{(x_k, m_k)\}_{k=0}^K,\quad x_k \in \mathcal{X},\; m_k \in \mathcal{M}, \]

connecting an initial grasp-state \( (x_0,m_0) \) to a goal \( (x_K,m_K) \) while respecting kinematic, contact, and friction constraints. The cost of a plan may penalize path length, number of regrasps, and control effort.

flowchart TD
  S["Initial grasp (x0, m0)"] --> CAND["Generate candidate grasps / modes"]
  CAND --> GRAPH["Build grasp / regrasp graph"]
  GRAPH --> PLAN["Search for discrete path of grasps"]
  PLAN --> CONT["For each edge: plan continuous in-hand motion"]
  CONT --> VERIFY["Verify contact, friction, joint limits"]
  VERIFY --> EXEC["Execute with feedback (vision / tactile)"]
        

The remainder of the lesson makes each of these blocks mathematically precise and connects them to algorithms you can implement.

Consider a dexterous hand with \( n_h \) joints and an object with pose \( g_o \in SE(3) \) in the world frame. The continuous configuration state is

\[ \mathcal{C}_{\text{hand}} \subset \mathbb{R}^{n_h},\qquad \mathcal{C}_{\text{obj}} = SE(3),\qquad \mathcal{X} = \mathcal{C}_{\text{hand}} \times \mathcal{C}_{\text{obj}}. \]

We denote a state as \( x = (q_h, g_o) \), where \( q_h \) is the hand joint vector. A contact mode \( m \in \mathcal{M} \) encodes, for each potential contact, whether it is active or inactive and its regime (sticking, sliding, separating, etc.). For a set of potential contacts indexed by \( i = 1,\dots,N_c \), a simple mode encoding is

\[ m = (m_1,\dots,m_{N_c}),\quad m_i \in \{\text{inactive},\ \text{sticking},\ \text{sliding}\}. \]

A grasp is a tuple

\[ g = (x,m) = (q_h,g_o,m) \]

such that the active contacts in \( m \) are kinematically consistent with \( x \) and generate a stable equilibrium (e.g. force-closure or sufficient grasp quality in the wrench space defined in Chapter 7).

For in-hand manipulation, it is often convenient to express the object pose in the hand frame (palm frame) via a homogeneous transform \( T_{ho} \in SE(3) \). If \( T_{wh} \) and \( T_{wo} \) are the world-to-hand and world-to-object transforms, then

\[ T_{ho} = T_{wh}^{-1} T_{wo}. \]

A library of candidate grasps can therefore be represented as \( \mathcal{G} = \{ (T_{ho}^{(j)},q_h^{(j)},m^{(j)}) \}_{j=1}^N \), which we will use to build a regrasp graph.

Let \( \mathbf{v}_o \in \mathbb{R}^6 \) denote the spatial twist of the object in the world frame (linear and angular velocity), and \( \dot{\mathbf{q}}_h \in \mathbb{R}^{n_h} \) the joint velocities. For active contact \( i \), the relative velocity at the contact point, expressed in some frame, can be written as

\[ \mathbf{v}_{c,i}(x,\dot{\mathbf{q}}_h,\mathbf{v}_o) = \mathbf{J}_{h,i}(q_h)\,\dot{\mathbf{q}}_h - \mathbf{J}_{o,i}(g_o)\,\mathbf{v}_o, \]

where \( \mathbf{J}_{h,i} \) is the hand Jacobian mapping joint velocities to contact-point twist, and \( \mathbf{J}_{o,i} \) is the object Jacobian at that contact. For a sticking point contact we impose

\[ \mathbf{v}_{c,i} = \mathbf{0}. \]

Stacking the constraints for all sticking contacts yields a linear system

\[ \mathbf{A}(x,m) \begin{bmatrix} \dot{\mathbf{q}}_h \\ \mathbf{v}_o \end{bmatrix} = \mathbf{0}, \qquad \mathbf{A}(x,m) \in \mathbb{R}^{k \times (n_h+6)}, \]

where \( k \) is three times the number of sticking contacts for full 3D motion (or two in planar settings).

Under quasi-static assumptions, inertial terms are negligible and contact forces must balance external wrenches. Let \( \boldsymbol{\lambda} \in \mathbb{R}^{3N_c} \) stack the contact forces (or wrenches) at each active contact and \( \mathbf{w}_{\text{ext}} \in \mathbb{R}^6 \) be the external wrench (e.g. gravity). The grasp map \( \mathbf{G}(x,m) \in \mathbb{R}^{6 \times 3N_c} \) satisfies

\[ \mathbf{G}(x,m)\,\boldsymbol{\lambda} + \mathbf{w}_{\text{ext}} = \mathbf{0}. \]

Contact forces must lie in the Coulomb friction cones. A common polyhedral approximation is

\[ \mathcal{F}_i = \bigl\{ \mathbf{f}_i \in \mathbb{R}^3 \,\bigm|\, \mathbf{F}_i\,\mathbf{f}_i \le \mathbf{0} \bigr\}, \]

so that the stacked forces satisfy \( \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0} \). The instantaneous feasibility problem for in-hand motion can be posed as a quadratic program:

\[ \begin{aligned} \min_{\dot{\mathbf{q}}_h,\mathbf{v}_o,\boldsymbol{\lambda}} &\quad \|\mathbf{v}_o - \mathbf{v}_o^{\ast}\|_2^2 + \alpha \,\|\dot{\mathbf{q}}_h\|_2^2 \\ \text{s.t.} &\quad \mathbf{A}(x,m) \begin{bmatrix} \dot{\mathbf{q}}_h \\ \mathbf{v}_o \end{bmatrix} = \mathbf{0}, \\ &\quad \mathbf{G}(x,m)\,\boldsymbol{\lambda} + \mathbf{w}_{\text{ext}} = \mathbf{0}, \\ &\quad \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0}, \end{aligned} \]

where \( \mathbf{v}_o^{\ast} \) is a desired object twist and \( \alpha > 0 \) sets a trade-off between object motion tracking and joint effort. For fixed mode \( m \), this is a convex QP.

Suppose we have precomputed a finite set of stable grasps \( \mathcal{G} = \{g_1,\dots,g_N\} \) using the synthesis methods of Lesson 2. A grasp graph is a directed graph \( \mathcal{G}_{\text{graph}} = (V,E) \) with \( V = \{1,\dots,N\} \) and edges

\[ (i,j) \in E \quad\Longleftrightarrow\quad \text{there exists a feasible manipulation taking } g_i \text{ to } g_j. \]

Feasibility can mean an in-hand motion (no placement) or a place-and-regrasp maneuver via an intermediate stable placement on the environment. Let \( \pi = (i_0,\dots,i_K) \) be a path in the graph (vertex sequence). Its cost can be defined as

\[ J(\pi) = \sum_{k=0}^{K-1} c(g_{i_k},g_{i_{k+1}}), \]

where an edge cost may combine orientation and position change, number of fingers that change contact, and whether the move uses a placement:

\[ c(g_i,g_j) = w_p \,\|\mathbf{p}_i - \mathbf{p}_j\|_2 + w_R \,d_{SO(3)}(R_i,R_j) + w_{\text{mode}}\,\Delta n_f(i,j) + w_{\text{place}}\,\delta_{\text{place}}(i,j). \]

Here \( \mathbf{p}_i,R_i \) are position and rotation of the object relative to the hand in grasp \( g_i \), \( d_{SO(3)} \) is a metric on rotations (see Chapter 1), \( \Delta n_f \) counts the number of fingers that change contact set, and \( \delta_{\text{place}} \) is one if a placement is required.

Given an initial grasp index \( i_{\text{start}} \) and a target grasp \( i_{\text{goal}} \), regrasp planning reduces to a shortest-path problem:

\[ \pi^{\ast} = \arg\min_{\pi \in \mathcal{P}(i_{\text{start}},i_{\text{goal}})} J(\pi), \]

where \( \mathcal{P} \) is the set of all directed paths from \( i_{\text{start}} \) to \( i_{\text{goal}} \). Algorithms such as Dijkstra or A* (Chapter 2) can be applied directly once edge costs are available.

Remark (connectivity via placements). If every stable placement on the support surface admits at least one force-closure grasp and the object can be rotated arbitrarily in a finite set of placements, then the regrasp graph constructed from those placements and grasps is strongly connected (any initial grasp can reach any other) under mild assumptions on reachability. This justifies place-and-regrasp strategies in practical systems.

Instead of precomputing a discrete graph, one can directly plan a continuous in-hand trajectory driving the object from \( T_{ho}^{(0)} \) to \( T_{ho}^{(\text{goal})} \). Discretize time into \( k = 0,\dots,N \) with time step \( \Delta t \) and decision variables \( x_k = (q_h^k,g_o^k) \), control inputs \( \mathbf{u}_k \) (e.g. joint velocities or torques), and contact forces \( \boldsymbol{\lambda}_k \). For a fixed mode sequence \( (m_0,\dots,m_{N-1}) \), the problem can be written as

\[ \begin{aligned} \min_{\{x_k,\mathbf{u}_k,\boldsymbol{\lambda}_k\}} &\quad \sum_{k=0}^{N-1} \ell(x_k,\mathbf{u}_k) + \phi(x_N) \\ \text{s.t.} &\quad x_{k+1} = f(x_k,\mathbf{u}_k,m_k),\quad k=0,\dots,N-1, \\ &\quad \mathbf{A}(x_k,m_k) \begin{bmatrix} \dot{\mathbf{q}}_h^k \\ \mathbf{v}_o^k \end{bmatrix} = \mathbf{0}, \\ &\quad \mathbf{G}(x_k,m_k)\,\boldsymbol{\lambda}_k + \mathbf{w}_{\text{ext}}(x_k) = \mathbf{0}, \\ &\quad \mathbf{F}\boldsymbol{\lambda}_k \le \mathbf{0}, \\ &\quad x_0 = x_{\text{start}},\quad x_N \approx x_{\text{goal}}. \end{aligned} \]

The dynamics \( f \) can be a first-order integrator in joint space and object pose, or a more detailed quasi-static model derived from manipulator dynamics you studied in the kinematics and dynamics course. The major difficulty is that contact modes can change: formulation with mode decision variables leads to mixed-integer (MIQP/MILP) problems, while complementarity constraints (normal distance and normal force orthogonality) lead to nonlinear programs with complementarity, both hard in general.

Practical systems often combine a discrete search over mode sequences (e.g., finger gait patterns) with convex optimization for continuous variables within each mode. The regrasp graph in Section 4 supplies the discrete skeleton, and each edge is refined via a trajectory optimization of the above form.

We now implement a minimal regrasp graph planner in several languages. The example is deliberately simplified: nodes represent candidate in-hand object poses and edges represent feasible transitions if the orientation and position differences are below thresholds. In a real system these feasibility checks would call kinematics and contact-constraint solvers.

6.1 Python (with pinocchio and networkx)

import numpy as np
import networkx as nx
import pinocchio as pin  # robotics kinematics library

class Grasp:
    def __init__(self, idx, T_ho, quality):
        self.idx = idx
        self.T_ho = T_ho  # 4x4 homogeneous transform (hand frame to object)
        self.quality = quality

def random_transform(max_pos=0.05):
    """Small random transform around the palm."""
    T = np.eye(4)
    T[:3, 3] = max_pos * (np.random.rand(3) - 0.5)
    # small random rotation via axis-angle
    axis = np.random.rand(3) - 0.5
    axis /= np.linalg.norm(axis) + 1e-9
    theta = 0.3 * (np.random.rand() - 0.5)
    R = pin.exp3(axis * theta)
    T[:3, :3] = R
    return T

def pose_distance(T1, T2, w_rot=1.0, w_pos=1.0):
    dp = np.linalg.norm(T1[:3, 3] - T2[:3, 3])
    R_rel = T1[:3, :3].T @ T2[:3, :3]
    tr = np.trace(R_rel)
    tr = np.clip(tr, -1.0, 3.0)
    dtheta = np.arccos((tr - 1.0) / 2.0)
    return w_pos * dp + w_rot * dtheta

# Build synthetic grasp set
np.random.seed(0)
N = 6
grasps = []
for i in range(N):
    T = random_transform()
    quality = np.random.rand()
    grasps.append(Grasp(i, T, quality))

# Build directed regrasp graph
G = nx.DiGraph()
for g in grasps:
    G.add_node(g.idx)

max_step = 0.08  # admissible pose distance for direct regrasp
for i in range(N):
    for j in range(N):
        if i == j:
            continue
        d = pose_distance(grasps[i].T_ho, grasps[j].T_ho)
        if d <= max_step:
            # edge cost: pose distance plus penalty for quality loss
            cost = d + max(0.0, grasps[i].quality - grasps[j].quality)
            G.add_edge(i, j, cost=cost)

start_id = 0
goal_id = N - 1
if nx.has_path(G, start_id, goal_id):
    path = nx.shortest_path(G, source=start_id, target=goal_id, weight="cost")
    print("Regrasp sequence:", path)
else:
    print("No path between grasps in this random example.")
      

In a full implementation, each edge would also store a continuous in-hand trajectory (planned via a QP as in Section 3) and low-level commands for the hand controller.

6.2 C++ (with Eigen and pinocchio)

#include <vector>
#include <queue>
#include <limits>
#include <iostream>
#include <Eigen/Dense>
#include <pinocchio/multibody/model.hpp> // robotics kinematics library

struct Edge {
    int to;
    double cost;
};

using Graph = std::vector< std::vector<Edge> >;

std::vector<int> dijkstra(const Graph& G, int start, int goal) {
    const double INF = std::numeric_limits<double>::infinity();
    int n = static_cast<int>(G.size());
    std::vector<double> dist(n, INF);
    std::vector<int> parent(n, -1);
    using Node = std::pair<double,int>;
    auto cmp = [](const Node& a, const Node& b) { return a.first > b.first; };
    std::priority_queue<Node, std::vector<Node>, decltype(cmp)> pq(cmp);

    dist[start] = 0.0;
    pq.push({0.0, start});

    while (!pq.empty()) {
        auto [d,u] = pq.top();
        pq.pop();
        if (d > dist[u]) continue;
        if (u == goal) break;
        for (const auto& e : G[u]) {
            double nd = d + e.cost;
            if (nd < dist[e.to]) {
                dist[e.to] = nd;
                parent[e.to] = u;
                pq.push({nd, e.to});
            }
        }
    }

    std::vector<int> path;
    if (dist[goal] == INF) return path;
    for (int v = goal; v != -1; v = parent[v]) {
        path.push_back(v);
    }
    std::reverse(path.begin(), path.end());
    return path;
}

int main() {
    // Example: 4 grasps with a few feasible transitions
    Graph G(4);
    G[0].push_back({1, 1.0});
    G[1].push_back({2, 1.0});
    G[0].push_back({2, 2.5});
    G[2].push_back({3, 1.0});

    auto path = dijkstra(G, 0, 3);
    if (path.empty()) {
        std::cout << "No regrasp path.\n";
    } else {
        std::cout << "Path:";
        for (int v : path) {
            std::cout << " " << v;
        }
        std::cout << std::endl;
    }
    return 0;
}
      

Here we used C++ and Eigen/pinocchio only for linear algebra and kinematics support; a full system would call forward kinematics and contact checks when building Graph G.

6.3 Java (graph abstraction for regrasp planning)

import java.util.*;

// Lightweight linear algebra can be provided by EJML or similar libraries.
class GraspGraph {
    static class Edge {
        int to;
        double cost;
        Edge(int t, double c) { to = t; cost = c; }
    }

    private final List<List<Edge>> adj;

    public GraspGraph(int n) {
        adj = new ArrayList<>(n);
        for (int i = 0; i < n; ++i) {
            adj.add(new ArrayList<>());
        }
    }

    public void addEdge(int from, int to, double cost) {
        adj.get(from).add(new Edge(to, cost));
    }

    public List<Integer> dijkstra(int start, int goal) {
        int n = adj.size();
        double[] dist = new double[n];
        int[] parent = new int[n];
        Arrays.fill(dist, Double.POSITIVE_INFINITY);
        Arrays.fill(parent, -1);

        class Node {
            int v; double d;
            Node(int v, double d) { this.v = v; this.d = d; }
        }

        PriorityQueue<Node> pq = new PriorityQueue<>(
            Comparator.comparingDouble(a -> a.d)
        );

        dist[start] = 0.0;
        pq.add(new Node(start, 0.0));

        while (!pq.isEmpty()) {
            Node cur = pq.poll();
            if (cur.d > dist[cur.v]) continue;
            if (cur.v == goal) break;
            for (Edge e : adj.get(cur.v)) {
                double nd = cur.d + e.cost;
                if (nd < dist[e.to]) {
                    dist[e.to] = nd;
                    parent[e.to] = cur.v;
                    pq.add(new Node(e.to, nd));
                }
            }
        }

        List<Integer> path = new ArrayList<>();
        if (Double.isInfinite(dist[goal])) return path;
        for (int v = goal; v != -1; v = parent[v]) {
            path.add(v);
        }
        Collections.reverse(path);
        return path;
    }

    public static void main(String[] args) {
        GraspGraph gg = new GraspGraph(4);
        gg.addEdge(0, 1, 1.0);
        gg.addEdge(1, 2, 0.8);
        gg.addEdge(2, 3, 0.9);
        gg.addEdge(0, 3, 3.5);

        List<Integer> path = gg.dijkstra(0, 3);
        System.out.println("Regrasp path: " + path);
    }
}
      

6.4 MATLAB / Simulink

% Simple directed regrasp graph and shortest path in MATLAB

% Node indices for candidate grasps
nodes = 1:4;

% Edges: from, to, cost
E_from = [1 2 1 3];
E_to   = [2 3 4 4];
E_cost = [1.0 1.0 1.0 2.5];

G = digraph(E_from, E_to, E_cost, numel(nodes));
[startNode, goalNode] = deal(1, 4);

[path, totalCost] = shortestpath(G, startNode, goalNode, "Method", "positive");
disp("Regrasp path:");
disp(path);
disp("Total cost:");
disp(totalCost);

% In a full implementation, each node would carry the hand configuration q_h
% and object pose T_ho, and feasibility of edges would be checked using
% Robotics System Toolbox (e.g., rigidBodyTree, inverseKinematics).
      

In Simulink, a common architecture is:

• A Stateflow chart encoding contact modes and high-level regrasp sequences.
• A MATLAB Function block that, given the current mode, solves the instantaneous QP of Section 3 to generate joint velocity commands.
• A kinematic chain built from Rigid Transform and joint blocks (or a rigidBodyTree-based Simscape model) to simulate the hand and object motion.

6.5 Wolfram Mathematica

(* Regrasp graph and shortest path in Mathematica *)

graspVertices = {1, 2, 3, 4};
edges = {1 -> 2, 2 -> 3, 1 -> 3, 3 -> 4};
weights = {1.0, 1.0, 2.0, 1.0};

g = Graph[
  graspVertices,
  Thread[edges, Rule],
  EdgeWeight -> weights,
  DirectedEdges -> True
];

HighlightGraph[
  g,
  PathGraph@FindShortestPath[g, 1, 4],
  GraphLayout -> "LayeredDigraphEmbedding"
]

(* In a more detailed model, vertices would store SE(3) poses and contact modes.
   Feasibility of edges would be determined by solving quasi-static equilibrium
   and friction constraints using LinearProgramming or NMinimize. *)
      

Problem 1 (Planar Rolling Contact Kinematics). Consider a planar finger with a single revolute joint at the origin and link length \( L \). It contacts a circular object of radius \( R \) at a point on the boundary. The finger joint angle is \( q \), the object center has position \( (x_o,y_o) \), and its orientation is \( \theta_o \). Assume pure rolling contact (no slip). Derive a linear relation between \( \dot{q} \) and the object twist \( \mathbf{v}_o = (\dot{x}_o,\dot{y}_o,\dot{\theta}_o) \) at the contact.

Solution: The fingertip position in world coordinates is \( \mathbf{p}_f = (L\cos q,\,L\sin q) \). Its velocity is

\[ \dot{\mathbf{p}}_f = \frac{\partial \mathbf{p}_f}{\partial q}\,\dot{q} = (-L\sin q,\,L\cos q)\,\dot{q}. \]

For the circular object, the contact point in the object frame is \( \mathbf{p}_{c,o} = (R,0) \). In the world frame this is

\[ \mathbf{p}_c = \mathbf{p}_o + R \begin{bmatrix} \cos \theta_o \\ \sin \theta_o \end{bmatrix}, \qquad \mathbf{p}_o = (x_o,y_o). \]

The velocity of the contact point due to object motion is the sum of translational and rotational contributions:

\[ \dot{\mathbf{p}}_c = \begin{bmatrix} \dot{x}_o \\ \dot{y}_o \end{bmatrix} + \dot{\theta}_o \begin{bmatrix} -R\sin \theta_o \\ R\cos \theta_o \end{bmatrix}. \]

Pure rolling implies equality of contact point velocities: \( \dot{\mathbf{p}}_f = \dot{\mathbf{p}}_c \). This yields

\[ \begin{bmatrix} -L\sin q \\ L\cos q \end{bmatrix}\dot{q} = \begin{bmatrix} \dot{x}_o \\ \dot{y}_o \end{bmatrix} + \dot{\theta}_o \begin{bmatrix} -R\sin \theta_o \\ R\cos \theta_o \end{bmatrix}. \]

This can be written in the standard Jacobian form

\[ \mathbf{J}_{h}(q)\,\dot{q} - \mathbf{J}_{o}(\theta_o)\,\mathbf{v}_o = \mathbf{0}, \]

where \( \mathbf{J}_h(q) = (-L\sin q,\,L\cos q) \) and \( \mathbf{J}_o(\theta_o) \) is obtained from the coefficients multiplying \( (\dot{x}_o,\dot{y}_o,\dot{\theta}_o) \). This is the planar analogue of the general 3D contact kinematics in Section 3.

Problem 2 (Convexity of Quasi-Static Force Feasibility). Consider a fixed state \( x \) and mode \( m \) with grasp map \( \mathbf{G} \), external wrench \( \mathbf{w}_{\text{ext}} \), and polyhedral friction constraints \( \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0} \). Show that the set of feasible contact force vectors

\[ \mathcal{L} = \Bigl\{\boldsymbol{\lambda} \,\bigm|\, \mathbf{G}\boldsymbol{\lambda} + \mathbf{w}_{\text{ext}} = \mathbf{0},\; \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0}\Bigr\} \]

is a convex polyhedron and explain why deciding whether a mode is statically feasible can be reduced to a linear program.

Solution: The feasible set is the intersection of (i) an affine subspace defined by \( \mathbf{G}\boldsymbol{\lambda} + \mathbf{w}_{\text{ext}} = \mathbf{0} \) and (ii) a polyhedral cone defined by \( \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0} \). Both are convex; the intersection of convex sets is convex, hence \( \mathcal{L} \) is convex. Furthermore, each constraint is linear in \( \boldsymbol{\lambda} \), so \( \mathcal{L} \) is precisely a (possibly unbounded) convex polyhedron. Deciding feasibility is equivalent to checking whether the linear program

\[ \min_{\boldsymbol{\lambda}} 0 \quad\text{s.t.}\quad \mathbf{G}\boldsymbol{\lambda} + \mathbf{w}_{\text{ext}} = \mathbf{0},\; \mathbf{F}\boldsymbol{\lambda} \le \mathbf{0} \]

is feasible, which can be done with any LP solver. This yields a computational test for mode feasibility in regrasp/in-hand planning.

Problem 3 (Small Regrasp Graph and Path). A robot has three stable grasps on an object: \( g_0 \), \( g_1 \), and \( g_2 \). It can execute the following feasible transitions:

• \( g_0 \to g_1 \) with cost 1 (in-hand motion),
• \( g_1 \to g_2 \) with cost 1 (in-hand motion),
• \( g_0 \to g_2 \) with cost 3 (place-and-regrasp).

(a) Build the corresponding directed regrasp graph. (b) Compute the minimum-cost path from \( g_0 \) to \( g_2 \). (c) Explain in words why place-and-regrasp edges are still useful even if they are more expensive.

Solution: (a) The graph is

flowchart TD
  G0["g0"] -->|1| G1["g1"]
  G1 -->|1| G2["g2"]
  G0 -->|3| G2
        

(b) There are two paths from \( g_0 \) to \( g_2 \): direct \( g_0 \to g_2 \) with cost 3, and \( g_0 \to g_1 \to g_2 \) with cost \( 1+1=2 \). The minimum-cost path is therefore \( g_0 \to g_1 \to g_2 \).

(c) In other situations, the in-hand edges may not exist because of joint limits or self-collisions, while place-and-regrasp edges are still possible. Even if they are more expensive, they preserve connectivity of the regrasp graph, ensuring that a path exists whenever the task is physically solvable via placements.

Problem 4 (Hybrid Cost Functional). Let \( \gamma = \{(x_k,m_k)\}_{k=0}^K \) be a hybrid manipulation plan. Suppose the continuous part of the cost penalizes squared object velocity and the discrete part penalizes mode switches. Propose a cost functional of the form \( J(\gamma) = J_{\text{cont}} + J_{\text{disc}} \) and explain how the discrete term relates to the regrasp graph formulation of Section 4.

Solution: A suitable choice is

\[ J_{\text{cont}} = \sum_{k=0}^{K-1} \|\mathbf{v}_o^k\|_2^2 \,\Delta t, \qquad J_{\text{disc}} = \beta \sum_{k=0}^{K-1} \mathbf{1}[m_{k+1} \ne m_k], \]

where \( \mathbf{v}_o^k \) is the object twist at step \( k \), \( \Delta t \) is the time step, and \( \beta > 0 \) weighs mode switches. In the regrasp graph abstraction each edge corresponds to a segment of \( \gamma \); its cost \( c(g_i,g_j) \) can be viewed as a local approximation to \( J_{\text{cont}} + J_{\text{disc}} \) over that segment, aggregating both the continuous effort and the number/type of mode switches incurred by the transition from \( g_i \) to \( g_j \).

In this lesson we lifted grasp synthesis from single configurations to sequences of grasps and modes needed for regrasp and in-hand manipulation. We formalized a hybrid state space combining hand configuration, object pose, and discrete contact modes; derived kinematic and quasi-static constraints for instantaneous in-hand motion; introduced grasp and regrasp graphs as discrete abstractions of the manipulation problem; and presented optimization-based formulations that refine graph edges into dynamically and contact-feasible trajectories. The multi-language examples showed how to encode regrasp graphs and run shortest-path algorithms, paving the way for simulation labs in subsequent lessons.