By the end of this lab, you should be able to:

• Implement a pipeline that converts raw sensor data (point cloud or depth image) into an object pose estimate \( \hat{T}_{bo} \in SE(3) \) (base frame \( b \) to object frame \( o \)).
• Compose this estimate with a grasp defined in the object frame to obtain a target gripper pose \( \hat{T}_{bg} \), and solve inverse kinematics to command the arm.
• Run a closed loop where perception is repeated, pose is updated, and the commanded trajectory is corrected during the approach to the object.
• Reason mathematically about how pose estimation uncertainty affects grasp accuracy and closed-loop stability.

We assume:

• A calibrated RGB-D or 3D sensor rigidly attached to the robot or world.
• A known object model with pre-computed grasp candidates in the object frame:

  \[ \mathcal{G} = \{\, T_{og}^{(j)} \in SE(3) \mid j = 1,\dots,n_g \,\}, \]

  where \( g \) is a gripper frame attached to the hand palm.
• A motion planning and inverse-kinematics stack from previous chapters (e.g. RRT*/PRM-based planner, IK solver), abstracted as functions \( \mathrm{IK}(\cdot) \) and \( \mathrm{Plan}(\cdot) \).

The essential mapping in this lab is:

\[ \hat{T}_{bo} \;\stackrel{\text{compose with }T_{og}^{(j)}}{\longrightarrow}\; \hat{T}_{bg}^{(j)} \;\stackrel{\mathrm{IK+Plan}}{\longrightarrow}\; \mathbf{q}_{\text{traj}}^{(j)}, \]

followed by closed-loop corrections as new measurements arrive.

Let frames be:

• \( b \): robot base frame (world for the arm).
• \( c \): camera frame.
• \( o \): object frame.
• \( g \): gripper (hand) frame.

A homogeneous transform from frame \( b \) to frame \( a \) is:

\[ T_{ab} = \begin{bmatrix} R_{ab} & \mathbf{t}_{ab} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \quad R_{ab} \in SO(3),\; \mathbf{t}_{ab} \in \mathbb{R}^3. \]

If \( \mathbf{x}_b \) is a point in frame \( b \), its coordinates in frame \( a \) are:

\[ \mathbf{x}_a = R_{ab} \mathbf{x}_b + \mathbf{t}_{ab} = \begin{bmatrix} R_{ab} & \mathbf{t}_{ab} \end{bmatrix} \begin{bmatrix} \mathbf{x}_b \\ 1 \end{bmatrix} = T_{ab} \begin{bmatrix} \mathbf{x}_b \\ 1 \end{bmatrix}. \]

For three frames \( a,b,c \), composition follows:

\[ T_{ac} = T_{ab} T_{bc}. \]

Proof sketch. Start from \( \mathbf{x}_c \), transform to \( b \): \( \mathbf{x}_b = R_{bc} \mathbf{x}_c + \mathbf{t}_{bc} \). Then to \( a \): \( \mathbf{x}_a = R_{ab} \mathbf{x}_b + \mathbf{t}_{ab} \). Substitute and group terms to obtain \( \mathbf{x}_a = R_{ab} R_{bc} \mathbf{x}_c + (R_{ab}\mathbf{t}_{bc} + \mathbf{t}_{ab}) \), which is the affine transformation with homogeneous matrix \( T_{ab}T_{bc} \).

In this lab, the camera is rigidly mounted, so we treat the calibration \( T_{bc} \) as known. The pose estimator outputs \( \hat{T}_{co} \) (object in camera frame), and the grasp database gives \( T_{og}^{(j)} \). The commanded gripper pose in base frame is:

\[ \hat{T}_{bg}^{(j)} = T_{bc}\,\hat{T}_{co}\,T_{og}^{(j)}. \]

To obtain joint-space targets, we use the forward kinematics map \( f_{\mathrm{FK}} : \mathbb{R}^n \to SE(3) \) and solve the inverse kinematics problem:

\[ f_{\mathrm{FK}}(\mathbf{q}^\ast) = \hat{T}_{bg}^{(j)}, \]

using an IK routine covered in earlier chapters. The motion planner then finds a collision-free trajectory connecting the current joint configuration to \( \mathbf{q}^\ast \).

You have already seen ICP and point-to-point registration in Lesson 1 of this chapter. Here we recall the core least-squares formulation that our implementation will rely on (either directly, or through a library such as Open3D or PCL).

Suppose we have \( N \) 3D model points in the object frame, \( \mathbf{m}_i \in \mathbb{R}^3 \), and corresponding measured points in the camera frame, \( \mathbf{p}_i \in \mathbb{R}^3 \). The goal is to find rotation \( R \in SO(3) \) and translation \( \mathbf{t} \in \mathbb{R}^3 \) that minimize:

\[ J(R,\mathbf{t}) = \sum_{i=1}^{N} \bigl\| R \mathbf{m}_i + \mathbf{t} - \mathbf{p}_i \bigr\|_2^2. \]

Let centroids be \( \bar{\mathbf{m}} = \tfrac{1}{N}\sum_i \mathbf{m}_i \) and \( \bar{\mathbf{p}} = \tfrac{1}{N}\sum_i \mathbf{p}_i \), and define centered points \( \tilde{\mathbf{m}}_i = \mathbf{m}_i - \bar{\mathbf{m}} \), \( \tilde{\mathbf{p}}_i = \mathbf{p}_i - \bar{\mathbf{p}} \). Expanding the cost and differentiating with respect to \( \mathbf{t} \) gives:

\[ \frac{\partial J}{\partial \mathbf{t}} = 2 \sum_{i=1}^N \bigl( R \mathbf{m}_i + \mathbf{t} - \mathbf{p}_i \bigr) = 0. \]

Setting this derivative to zero yields:

\[ N \,\mathbf{t} + R \sum_i \mathbf{m}_i - \sum_i \mathbf{p}_i = 0 \quad\Rightarrow\quad \mathbf{t}^\ast = \bar{\mathbf{p}} - R \bar{\mathbf{m}}. \]

Therefore, the translation is determined by the rotation and centroids. Substituting \( \mathbf{t}^\ast \) back into the cost reduces the problem to:

\[ J(R) = \sum_{i=1}^{N} \bigl\| R \tilde{\mathbf{m}}_i - \tilde{\mathbf{p}}_i \bigr\|_2^2. \]

Define the cross-covariance matrix:

\[ \mathbf{S} = \sum_{i=1}^{N} \tilde{\mathbf{p}}_i \tilde{\mathbf{m}}_i^\top. \]

The optimal rotation is obtained via an SVD \( \mathbf{S} = \mathbf{U} \boldsymbol{\Sigma} \mathbf{V}^\top \):

\[ R^\ast = \mathbf{U} \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & \det(\mathbf{U}\mathbf{V}^\top) \end{bmatrix} \mathbf{V}^\top, \]

followed by \( \mathbf{t}^\ast = \bar{\mathbf{p}} - R^\ast \bar{\mathbf{m}} \). Iterative ICP variants update correspondences between points or surfaces and re-solve this problem until convergence.

In practice, you will rely on library implementations of ICP, but it is crucial to understand that each ICP iteration is solving a least-squares registration of this form.

Pose estimation is noisy. Let \( \mathbf{x} \in \mathbb{R}^6 \) be a minimal parameterization of the object pose (for example, a 3D translation and a 3D axis-angle vector). Assume a Gaussian posterior:

\[ \mathbf{x} \sim \mathcal{N}(\hat{\mathbf{x}}, \boldsymbol{\Sigma}_x). \]

A candidate grasp pose in the base frame is a deterministic function of the pose parameters:

\[ \mathbf{y} = f(\mathbf{x}) = \operatorname{Log}_{SE(3)}\!\bigl( T_{bc}\,\mathrm{Exp}_{SE(3)}(\mathbf{x})\,T_{og}^{(j)} \bigr), \]

where \( \mathbf{y} \in \mathbb{R}^6 \) is a minimal parameterization of the gripper pose, and \( \mathrm{Exp}_{SE(3)}, \operatorname{Log}_{SE(3)} \) are the exponential and logarithm maps (you can treat them abstractly here, since your implementation will often work directly with homogeneous matrices).

Linearizing \( f \) about \( \hat{\mathbf{x}} \), we have:

\[ f(\mathbf{x}) \approx f(\hat{\mathbf{x}}) + \mathbf{J}_f(\hat{\mathbf{x}})\, (\mathbf{x} - \hat{\mathbf{x}}), \]

where \( \mathbf{J}_f(\hat{\mathbf{x}}) \in \mathbb{R}^{6 \times 6} \) is the Jacobian of \( f \) with respect to pose parameters. It follows that:

\[ \operatorname{Cov}[\mathbf{y}] \approx \boldsymbol{\Sigma}_y = \mathbf{J}_f(\hat{\mathbf{x}}) \,\boldsymbol{\Sigma}_x\, \mathbf{J}_f(\hat{\mathbf{x}})^\top. \]

This covariance describes the spatial uncertainty of the gripper target pose induced by pose estimation noise. In practice:

• Large eigenvalues of \( \boldsymbol{\Sigma}_y \) indicate directions in which the grasp pose is poorly constrained.
• You can reject grasp candidates for which the probability of remaining inside the friction-cone-consistent region is low under this uncertainty.

A simple approximation for translational uncertainty is to track only the covariance of the grasp center position \( \mathbf{p}_g \), obtained by propagating the translational part of the Jacobian.

Let \( \mathbf{e}_k \in \mathbb{R}^6 \) be the pose error between the desired grasp pose and the current end-effector pose at the \( k \)-th perception update, expressed in some error coordinate (for example, twist coordinates in the gripper frame).

A simple discrete-time correction law is:

\[ \mathbf{e}_{k+1} = ( \mathbf{I} - \mathbf{K} )\,\mathbf{e}_k + \mathbf{w}_k, \]

where \( \mathbf{K} \in \mathbb{R}^{6 \times 6} \) is a gain matrix, and \( \mathbf{w}_k \) captures pose estimation noise and unmodeled disturbances. In the noise-free case \( \mathbf{w}_k = 0 \), the solution is:

\[ \mathbf{e}_k = (\mathbf{I} - \mathbf{K})^k \mathbf{e}_0. \]

For a scalar one-dimensional error with gain \( K = k \in \mathbb{R} \) (so the update is \( e_{k+1} = (1 - k)e_k \)), we have:

\[ e_k = (1 - k)^k e_0. \]

Asymptotic stability requires \( |1 - k| < 1 \), i.e., \( 0 < k < 2 \). In the 6D case, the condition generalizes to requiring the spectral radius \( \rho(\mathbf{I} - \mathbf{K}) < 1 \), so that all eigenvalues lie strictly inside the unit circle. In the presence of noise \( \mathbf{w}_k \), a stationary covariance \( \boldsymbol{\Sigma}_e \) solves the discrete Lyapunov equation:

\[ \boldsymbol{\Sigma}_e = (\mathbf{I} - \mathbf{K})\, \boldsymbol{\Sigma}_e\, (\mathbf{I} - \mathbf{K})^\top + \boldsymbol{\Sigma}_w, \]

where \( \boldsymbol{\Sigma}_w \) is the covariance of \( \mathbf{w}_k \). This shows the classic trade-off: larger gains reduce deterministic error faster but amplify measurement noise in steady state.

In your implementation, you will not explicitly solve this Lyapunov equation; instead, you will tune the gains and observation rate and verify empirically that the approach is stable and robust for the given object and robot.

The perception-to-grasp loop combines pose estimation, grasp selection, motion planning, and execution with re-perception. A high-level view is:

flowchart TD
  A["Start loop (k = 0)"] --> B["Capture depth frame / point cloud"]
  B --> C["Segment object point cloud"]
  C --> D["Estimate pose T_co(k) via registration"]
  D --> E["Compose T_bg_j(k) = T_bc * T_co(k) * T_og_j"]
  E --> F["Score grasps Q_j and select argmax_j Q_j"]
  F --> G["Plan motion and send trajectory to controller"]
  G --> H["Execute approach and close gripper"]
  H --> I{"Success detected?"}
  I -->|yes| J["Lift and place object"]
  I -->|no| K["Increment k, optionally update view"]
  K --> B
        

The loop terminates either when a grasp succeeds (tactile/force/pose-based criterion) or when a maximum number of attempts is reached.

In Python we can combine numpy for linear algebra, open3d for point-cloud ICP, and a ROS-based motion planning stack (e.g. MoveIt Python bindings). Below is a simplified skeleton that illustrates the main elements of the loop.

import numpy as np
import open3d as o3d

# Assume: T_bc is known (4x4 numpy array)
# Assume: grasps_og is a list of 4x4 numpy arrays T_og_j

def estimate_pose_icp(model_pcd, scene_pcd, init_T=None):
    if init_T is None:
        init_T = np.eye(4)
    reg = o3d.pipelines.registration.registration_icp(
        model_pcd,
        scene_pcd,
        max_correspondence_distance=0.01,
        init=init_T,
        estimation_method=o3d.pipelines.registration.TransformationEstimationPointToPlane(),
    )
    return reg.transformation, reg.fitness, reg.inlier_rmse

def transform_grasp_candidates(T_bc, T_co, grasps_og):
    T_bg_list = []
    for T_og in grasps_og:
        T_bg = T_bc @ T_co @ T_og
        T_bg_list.append(T_bg)
    return T_bg_list

def score_grasp(T_bg):
    # Placeholder for a score based on reachability, clearance, wrench metric, etc.
    # For this lab, you might plug in a precomputed grasp quality stored with T_og.
    return 1.0

def select_best_grasp(T_bc, T_co, grasps_og):
    T_bg_list = transform_grasp_candidates(T_bc, T_co, grasps_og)
    scores = [score_grasp(T_bg) for T_bg in T_bg_list]
    idx_best = int(np.argmax(scores))
    return T_bg_list[idx_best], scores[idx_best]

def ik_solve(T_bg_target):
    # Interface to your IK solver from earlier chapters.
    # Returns joint vector q_star or None if infeasible.
    raise NotImplementedError

def plan_and_execute(q_star):
    # Interface to your planner and controller (e.g. MoveIt).
    # Should send a trajectory to the robot and wait for completion.
    raise NotImplementedError

def capture_scene_point_cloud():
    # Interface to a ROS subscriber or simulator API that returns an Open3D point cloud
    raise NotImplementedError

def run_pose_grasp_loop(
    model_pcd,
    T_bc,
    grasps_og,
    max_attempts=5,
):
    T_co_init = np.eye(4)
    for k in range(max_attempts):
        scene_pcd = capture_scene_point_cloud()
        T_co, fitness, rmse = estimate_pose_icp(model_pcd, scene_pcd, init_T=T_co_init)

        T_bg_best, score = select_best_grasp(T_bc, T_co, grasps_og)

        q_star = ik_solve(T_bg_best)
        if q_star is None:
            continue

        plan_and_execute(q_star)

        # Here you would check success via tactile / force / post-grasp pose
        success = False  # replace with real condition
        if success:
            break

        # Use latest pose as initialization for the next ICP call
        T_co_init = T_co
      

The key robotics-specific components are delegated to specialized subsystems: pose estimation via ICP, grasp scoring via grasp metrics, and planning via your existing motion planner. The loop itself is conceptually simple but must be carefully engineered to handle failures and outliers.

In C++, we typically use Eigen for linear algebra and PCL for 3D perception, with a motion planning stack such as MoveIt. The snippet below abstracts away the ROS integration and focuses on transformations and function interfaces.

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

// Forward declarations for perception and planning hooks
Eigen::Matrix4f estimatePoseICP(
    const Eigen::MatrixXf& model_points,
    const Eigen::MatrixXf& scene_points,
    const Eigen::Matrix4f& T_init
);

bool ikSolve(
    const Eigen::Matrix4f& T_bg_target,
    Eigen::VectorXf& q_star
);

bool planAndExecute(
    const Eigen::VectorXf& q_star
);

// Compose base-to-gripper transform from base-to-camera, camera-to-object,
// and object-to-gripper
Eigen::Matrix4f composeGrasp(
    const Eigen::Matrix4f& T_bc,
    const Eigen::Matrix4f& T_co,
    const Eigen::Matrix4f& T_og
) {
    return T_bc * T_co * T_og;
}

void poseGraspLoop(
    const Eigen::MatrixXf& model_points,
    const Eigen::Matrix4f& T_bc,
    const std::vector<Eigen::Matrix4f>& grasps_og,
    int max_attempts
) {
    Eigen::Matrix4f T_co_init = Eigen::Matrix4f::Identity();

    for (int k = 0; k < max_attempts; ++k) {
        Eigen::MatrixXf scene_points;
        // TODO: fill scene_points from sensor or simulator

        Eigen::Matrix4f T_co = estimatePoseICP(model_points, scene_points, T_co_init);

        // For simplicity, choose the first grasp; in practice score them.
        Eigen::Matrix4f T_bg = composeGrasp(T_bc, T_co, grasps_og[0]);

        Eigen::VectorXf q_star;
        bool ik_ok = ikSolve(T_bg, q_star);
        if (!ik_ok) {
            T_co_init = T_co;
            continue;
        }

        bool exec_ok = planAndExecute(q_star);
        if (exec_ok) {
            break;
        }

        T_co_init = T_co;
    }
}
      

In your project code, you would implement estimatePoseICP using PCL registration routines, and planAndExecute using your preferred motion planning library.

Java is less common for mainstream manipulation, but matrix-based implementations can be built with libraries such as EJML. Below is a minimal example using plain 2D arrays to emphasize the structure rather than the specific library.

public class PoseGraspLoop {

    // 4x4 matrix multiplication
    public static double[][] mul4x4(double[][] A, double[][] B) {
        double[][] C = new double[4][4];
        for (int i = 0; i != 4; ++i) {
            for (int j = 0; j != 4; ++j) {
                double s = 0.0;
                for (int k = 0; k != 4; ++k) {
                    s += A[i][k] * B[k][j];
                }
                C[i][j] = s;
            }
        }
        return C;
    }

    public static double[][] composeGrasp(
        double[][] T_bc,
        double[][] T_co,
        double[][] T_og
    ) {
        return mul4x4(mul4x4(T_bc, T_co), T_og);
    }

    // Placeholders for perception and planning
    public static double[][] estimatePoseICP(
        double[][] modelPoints,
        double[][] scenePoints,
        double[][] T_init
    ) {
        // Call into native or library code
        return T_init;
    }

    public static double[] ikSolve(double[][] T_bg_target) {
        // Call IK solver; return joint configuration or null if infeasible
        return null;
    }

    public static boolean planAndExecute(double[] qStar) {
        // Send trajectory to robot controller
        return false;
    }

    public static void runLoop(
        double[][] modelPoints,
        double[][] T_bc,
        double[][][] grasps_og,
        int maxAttempts
    ) {
        double[][] T_co_init = new double[4][4];
        for (int i = 0; i != 4; ++i) {
            T_co_init[i][i] = 1.0;
        }

        for (int k = 0; k != maxAttempts; ++k) {
            double[][] scenePoints = {}; // fill from sensor or simulator

            double[][] T_co = estimatePoseICP(modelPoints, scenePoints, T_co_init);

            double[][] T_bg = composeGrasp(T_bc, T_co, grasps_og[0]);

            double[] qStar = ikSolve(T_bg);
            if (qStar == null) {
                T_co_init = T_co;
                continue;
            }

            boolean execOk = planAndExecute(qStar);
            if (execOk) {
                break;
            }

            T_co_init = T_co;
        }
    }
}
      

In a real system you would replace the stubs with JNI bindings to robotics libraries or a Java-native robotics framework.

MATLAB provides convenient robotics toolboxes and Simulink integration for real-time execution. Here is a script that:

• Composes base-to-gripper transforms.
• Uses an inverse kinematics solver for a rigidBodyTree.
• Sketches how to integrate the perception-to-grasp loop in Simulink.

% Assume: robot is a robotics.RigidBodyTree
%        T_bc (4x4), T_co (4x4), grasps_og cell array of 4x4 matrices

function T_bg = composeGrasp(T_bc, T_co, T_og)
    T_bg = T_bc * T_co * T_og;
end

% Example IK call
function qStar = solveIK(robot, T_bg_target)
    ik = inverseKinematics("RigidBodyTree", robot);
    weights = [1 1 1 1 1 1];
    initialGuess = robot.homeConfiguration;
    [configSol, ~] = ik(robot.EndEffector, T_bg_target, weights, initialGuess);
    qStar = [configSol.JointPosition];
end

% High-level loop (to be called from a Simulink MATLAB Function block)
function [qCmd, success] = poseGraspStep( ...
    robot, T_bc, T_co, grasps_og, maxAttempts, k)

    persistent T_co_init
    if isempty(T_co_init)
        T_co_init = eye(4);
    end

    % In practice, T_co would come from a ROS topic or sensor block
    % Here we take T_co as input to this step

    T_bg = composeGrasp(T_bc, T_co, grasps_og{1});

    qStar = solveIK(robot, T_bg);

    % Plan a simple joint-space interpolation (for illustration)
    qCurrent = [robot.homeConfiguration.JointPosition];
    nSteps = 50;
    qTrajectory = zeros(numel(qStar), nSteps);
    for i = 1:numel(qStar)
        qTrajectory(i, :) = linspace(qCurrent(i), qStar(i), nSteps);
    end

    qCmd = qTrajectory(:, end);  % last point as instantaneous command
    success = true;

    T_co_init = T_co;
end

% Sketch of Simulink integration (to be built graphically):
% 1) ROS Subscribe block for point cloud
% 2) MATLAB Function block "PoseEstimationICP" that outputs T_co
% 3) MATLAB Function block "PoseGraspStep" (above) that outputs qCmd
% 4) ROS Publish block for joint trajectory or joint targets
      

In Simulink, this logic is implemented as a combination of ROS I/O blocks, MATLAB Function blocks for ICP and grasp composition, and a controller that runs at a fixed sample time.

Wolfram Mathematica is well suited for prototyping symbolic computations and for verifying transformation and uncertainty formulas before coding them in a real-time system.

(* Homogeneous transform from rotation matrix R (3x3) and translation t (3x1) *)
HomogeneousTransform[R_, t_] := ArrayFlatten[{
  {R, t},
  
}];

(* Compose transforms: T_ab * T_bc *)
ComposeTransform[T1_, T2_] := T1 . T2;

(* Example: base-to-camera, camera-to-object, object-to-gripper *)
Tbc = IdentityMatrix[4];
Tco = HomogeneousTransform[
  RotationMatrix[Pi/4, {0, 0, 1}],
  { {0.1}, {0.0}, {0.2} }
];
Tog = HomogeneousTransform[
  RotationMatrix[Pi/2, {0, 1, 0}],
  { {0.0}, {0.0}, {0.05} }
];

Tbg = ComposeTransform[Tbc, ComposeTransform[Tco, Tog]] // Simplify;

(* Linearization of pose-to-grasp map with respect to small perturbations dx *)
(* Here we treat the pose parameters as symbolic variables *)
dx = Array[Subscript[dx, #] &, 6];
(* Symbolic Jacobian J_f could be derived by choosing a specific parameterization
   (e.g., axis-angle for rotation, t for translation) and differentiating the
   elements of Tbg with respect to those parameters. *)

(* Example numerical evaluation of propagated covariance *)
SigmaX = DiagonalMatrix[{1.*10^-4, 1.*10^-4, 1.*10^-4, 1.*10^-4, 1.*10^-4, 1.*10^-4}];
Jf = IdentityMatrix[6]; (* placeholder for actual Jacobian *)
SigmaY = Jf . SigmaX . Transpose[Jf];
Eigenvalues[SigmaY]
      

Symbolic differentiation in Mathematica can be used to derive closed-form Jacobians for the pose-to-grasp map, which can then be exported to other languages if needed.

The next diagram emphasizes the feedback nature of the loop, where pose errors guide new control commands and new sensor views.

flowchart TD
  S["Sensor frame data (point cloud / depth)"] --> P["Pose estimator: T_co(k)"]
  P --> GSEL["Grasp mapping and selection"]
  GSEL --> CTRL["IK + planner: joint trajectory"]
  CTRL --> ROBOT["Robot execution (approach, close, lift)"]
  ROBOT --> OBS["Success / failure detection"]
  OBS -->|failure| VIEW["Update view / camera pose"]
  VIEW --> S
  OBS -->|success| DONE["End loop"]
        

This architecture connects naturally to active perception (from the previous lesson): when failures occur, the robot may deliberately move to poses that reduce occlusions and improve the conditioning of the pose estimation problem.

Problem 1 (Frame Composition Identity): Let \( T_{bc} \) be the base-to-camera transform, \( T_{co} \) the camera-to-object transform, and \( T_{og} \) the object-to-gripper transform for a candidate grasp. Show that the base-to-gripper transform is \( T_{bg} = T_{bc} T_{co} T_{og} \), and verify this by writing the corresponding affine transformations on a point \( \mathbf{x}_g \) in the gripper frame.

Solution: A point in the gripper frame \( \mathbf{x}_g \) is mapped to the object frame as \( \mathbf{x}_o = R_{og} \mathbf{x}_g + \mathbf{t}_{og} \), to the camera frame as \( \mathbf{x}_c = R_{co} \mathbf{x}_o + \mathbf{t}_{co} \), and to the base frame as \( \mathbf{x}_b = R_{bc} \mathbf{x}_c + \mathbf{t}_{bc} \).

Substituting, we have:

\[ \mathbf{x}_b = R_{bc}\bigl( R_{co}(R_{og}\mathbf{x}_g + \mathbf{t}_{og}) + \mathbf{t}_{co} \bigr) + \mathbf{t}_{bc}. \]

Expanding gives:

\[ \mathbf{x}_b = (R_{bc} R_{co} R_{og})\,\mathbf{x}_g + \bigl( R_{bc} R_{co} \mathbf{t}_{og} + R_{bc} \mathbf{t}_{co} + \mathbf{t}_{bc} \bigr). \]

This is the affine transformation associated with the homogeneous matrix \( T_{bg} = T_{bc} T_{co} T_{og} \). Thus, composing the three transforms yields the correct base-to-gripper transform.

Problem 2 (Translation Closed Form in Least Squares Pose): In the pose estimation problem of Section 3, derive explicitly the optimal translation \( \mathbf{t}^\ast \) given a fixed rotation \( R \).

Solution: The cost is \( J(R,\mathbf{t}) = \sum_i \lVert R\mathbf{m}_i + \mathbf{t} - \mathbf{p}_i \rVert_2^2 \). Differentiating:

\[ \frac{\partial J}{\partial \mathbf{t}} = 2 \sum_i (R\mathbf{m}_i + \mathbf{t} - \mathbf{p}_i). \]

Setting \( \frac{\partial J}{\partial \mathbf{t}} = 0 \) gives:

\[ N\mathbf{t} + R \sum_i \mathbf{m}_i - \sum_i \mathbf{p}_i = 0 \quad\Rightarrow\quad \mathbf{t}^\ast = \bar{\mathbf{p}} - R \bar{\mathbf{m}}, \]

where \( \bar{\mathbf{m}} = \tfrac{1}{N}\sum_i \mathbf{m}_i \) and \( \bar{\mathbf{p}} = \tfrac{1}{N}\sum_i \mathbf{p}_i \).

Problem 3 (Scalar Gain Condition): Consider the scalar closed-loop error update \( e_{k+1} = (1 - k)e_k \) from Section 5, with \( k \in \mathbb{R} \) and no noise. Prove that \( e_k \to 0 \) as \( k \to \infty \) if and only if \( 0 < k < 2 \).

Solution: The closed-form solution is \( e_k = (1 - k)^k e_0 \). This converges to zero if and only if the magnitude of the base is strictly less than one: \( |1 - k| < 1 \). Solving the inequality:

\[ |1 - k| < 1 \quad\Leftrightarrow\quad -1 < 1 - k < 1 \quad\Leftrightarrow\quad 0 < k < 2. \]

Thus, the scalar proportional gain must satisfy \( 0 < k < 2 \) for asymptotic stability.

Problem 4 (First-Order Uncertainty Propagation): Let \( \mathbf{x} \sim \mathcal{N}(\hat{\mathbf{x}}, \boldsymbol{\Sigma}_x) \) and \( \mathbf{y} = f(\mathbf{x}) \), where \( f \) is differentiable at \( \hat{\mathbf{x}} \). Show that under first-order linearization, \( \mathbf{y} \) is approximately Gaussian with covariance \( \boldsymbol{\Sigma}_y \approx \mathbf{J}_f(\hat{\mathbf{x}})\, \boldsymbol{\Sigma}_x\,\mathbf{J}_f(\hat{\mathbf{x}})^\top \).

Solution: Linearizing:

\[ \mathbf{y} = f(\mathbf{x}) \approx f(\hat{\mathbf{x}}) + \mathbf{J}_f(\hat{\mathbf{x}})\, (\mathbf{x} - \hat{\mathbf{x}}). \]

Let \( \delta\mathbf{x} = \mathbf{x} - \hat{\mathbf{x}} \), so \( \delta\mathbf{x} \sim \mathcal{N}(\mathbf{0}, \boldsymbol{\Sigma}_x) \). Then the perturbation in \( \mathbf{y} \) is:

\[ \delta\mathbf{y} \approx \mathbf{J}_f(\hat{\mathbf{x}})\,\delta\mathbf{x}, \]

which is a linear transformation of a Gaussian vector. Therefore, \( \delta\mathbf{y} \) is Gaussian with zero mean and covariance:

\[ \boldsymbol{\Sigma}_y = \mathbf{J}_f(\hat{\mathbf{x}}) \,\boldsymbol{\Sigma}_x\, \mathbf{J}_f(\hat{\mathbf{x}})^\top. \]

Hence, \( \mathbf{y} \approx \mathcal{N}(f(\hat{\mathbf{x}}), \boldsymbol{\Sigma}_y) \).

Problem 5 (Pose Error and Grasp Offset): Suppose the true object pose is \( T_{bo}^{\text{true}} \) and the estimate used for control is \( \hat{T}_{bo} = T_{bo}^{\text{true}} \Delta T \), where \( \Delta T \in SE(3) \) is a right-multiplicative error close to identity. For a fixed object-frame grasp \( T_{og} \), show that the commanded gripper pose \( \hat{T}_{bg} = \hat{T}_{bo} T_{og} \) differs from the ideal pose \( T_{bg}^{\text{true}} = T_{bo}^{\text{true}} T_{og} \) by the same group error \( \Delta T \).

Solution: By definition,

\[ \hat{T}_{bg} = \hat{T}_{bo} T_{og} = T_{bo}^{\text{true}} \Delta T T_{og} = T_{bo}^{\text{true}} T_{og} \bigl( T_{og}^{-1} \Delta T T_{og} \bigr). \]

The ideal gripper pose is \( T_{bg}^{\text{true}} = T_{bo}^{\text{true}} T_{og} \). Thus:

\[ \hat{T}_{bg} = T_{bg}^{\text{true}} \,\Delta T_g, \quad \Delta T_g = T_{og}^{-1} \Delta T T_{og}. \]

Hence, the gripper pose error is the conjugation of \( \Delta T \) by \( T_{og} \). For small pose errors, the twist representation of \( \Delta T_g \) is approximately the same as that of \( \Delta T \), up to a frame rotation, so the magnitude of the pose error directly reflects the magnitude of the estimation error.

In this lab you integrated perception and manipulation by:

• Using 3D registration (ICP or related methods) to estimate object pose \( \hat{T}_{bo} \) from point clouds.
• Composing this estimate with object-frame grasp templates \( T_{og}^{(j)} \) to obtain gripper targets \( \hat{T}_{bg}^{(j)} \).
• Feeding these targets into IK and motion planning routines to produce grasp execution trajectories.
• Closing the loop with repeated perception and a simple discrete-time correction law, and analyzing stability and uncertainty propagation.

This pose estimation plus grasp execution loop is a central building block of advanced manipulation systems. Subsequent chapters build on this structure with learning from demonstration, reinforcement learning, and sim-to-real transfer.