Sim-to-Real Deploy: Moving the SAP Pipeline from LIBERO to Physical Robots | AI Manipulation Agents #5

Series: AI Agent Pipeline for Robot Manipulation — Part 5/5 (Final)

The Problem with Leaving Simulation

Over the previous four posts, we built a complete pipeline: ManiAgent analyzes the scene, AnyGrasp computes grasp poses, ALRM writes control code, and the SAP Protocol with Temporal Verifier ensures recovery from failures. The result: 79.6% success rate on the LIBERO benchmark.

But LIBERO is a simulation. And simulation is always artificially better than reality.

When you move to a physical robot, three problems appear simultaneously:

1. Visual domain gap: Real-world lighting changes. Shadows, reflections, glare — things simulation pretends don't exist. Models trained on clean renders must now inference on camera images with noise, blur, and color drift.

2. Kinematic mismatch: Joint limits, friction, gear backlash — every physical robot has its own "personality." LIBERO uses an idealized simulated Franka Panda; your real Franka will differ. Not by much, but enough to shift grasp poses by 3–5mm and cause failures.

3. Latency and timing: Simulation runs deterministically. Physical robots have network latency, motor control loop delays, and camera sync drift. The SAP Protocol assumes certain timing; those assumptions can be violated in deployment.

This post addresses all three problems.

Real-World Deployment Architecture

Before diving into each step, here's the full deployment pipeline overview:

┌─────────────────────────────────────────────────────────┐
│                     Workstation PC                        │
│  ┌──────────────┐   ┌──────────────┐   ┌─────────────┐  │
│  │  Decomposer  │   │   Verifier   │   │  ALRM Code  │  │
│  │ DeepSeek-V3  │──▶│  Qwen2.5-VL  │   │  Generator  │  │
│  │  (Cloud API) │   │  (Local GPU) │   │             │  │
│  └──────────────┘   └──────────────┘   └─────────────┘  │
│          │                │                   │          │
│          └────────────────┴───────────────────┘          │
│                           │ SAP Orchestrator             │
└───────────────────────────┼─────────────────────────────┘
                            │ ROS 2 / ZMQ
┌───────────────────────────┼─────────────────────────────┐
│                    Robot Controller                       │
│  ┌──────────────┐   ┌─────┴────────┐   ┌─────────────┐  │
│  │  RealSense   │   │  Franka FCI  │   │  AnyGrasp   │  │
│  │   D435i      │──▶│  Controller  │   │  Server     │  │
│  │  (Camera)    │   │  (1kHz loop) │   │  (GPU)      │  │
│  └──────────────┘   └──────────────┘   └─────────────┘  │
└─────────────────────────────────────────────────────────┘

The workstation runs heavy models (DeepSeek, Qwen, OpenVLA). The robot controller handles real-time control. They communicate over ROS 2 or ZMQ with < 10ms local network latency.

Step 1: Camera-to-Robot Calibration

This is the first and most critical step. A miscalibrated transform means every grasp pose will be off — not a model error, but a coordinate transform error.

Hand-eye calibration with AprilTag

# calibration/hand_eye_calib.py
import numpy as np
import cv2
from scipy.spatial.transform import Rotation
import roboticstoolbox as rtb

class HandEyeCalibrator:
    """
    Eye-in-hand calibration for RealSense D435i mounted on wrist.
    Uses AprilTag 36h11, 50mm size.
    """
    def __init__(self, robot_interface, camera):
        self.robot = robot_interface
        self.camera = camera
        self.T_ee_cam = None  # Transform from EE frame to Camera frame
        
        # AprilTag detector
        self.detector = cv2.aruco.ArucoDetector(
            cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_APRILTAG_36h11),
            cv2.aruco.DetectorParameters()
        )
        
    def collect_poses(self, n_poses: int = 20) -> list:
        """
        Move robot to n_poses random configurations,
        recording (T_base_ee, T_cam_tag) pairs for each.
        """
        pose_pairs = []
        
        # Sampling around nominal home pose
        nominal_joints = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
        
        for i in range(n_poses):
            # Random jitter ±15° per joint
            delta = np.random.uniform(-0.26, 0.26, 7)
            target_joints = nominal_joints + delta
            
            # Move robot
            self.robot.move_joints(target_joints, speed=0.1)
            import time; time.sleep(0.5)  # Wait for settle
            
            # Get robot EE pose
            T_base_ee = self.robot.get_ee_transform()
            
            # Detect AprilTag
            frame = self.camera.get_rgb()
            corners, ids, _ = self.detector.detectMarkers(frame)
            
            if ids is not None and len(ids) > 0:
                # Estimate tag pose
                rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
                    corners, 0.05,  # 50mm tag
                    self.camera.intrinsics_matrix,
                    self.camera.distortion_coeffs
                )
                T_cam_tag = self._rvec_tvec_to_matrix(rvec[0], tvec[0])
                pose_pairs.append((T_base_ee, T_cam_tag))
                print(f"Pose {i+1}/{n_poses} collected ✓")
            else:
                print(f"Pose {i+1}/{n_poses} - Tag not detected, skipping")
        
        return pose_pairs
    
    def calibrate(self, pose_pairs: list) -> np.ndarray:
        """
        Solve AX = XB hand-eye calibration.
        Returns T_ee_cam (4x4 homogeneous matrix).
        """
        R_gripper2base = [p[0][:3,:3] for p in pose_pairs]
        t_gripper2base = [p[0][:3,3] for p in pose_pairs]
        R_target2cam = [p[1][:3,:3] for p in pose_pairs]
        t_target2cam = [p[1][:3,3] for p in pose_pairs]
        
        R_cam2gripper, t_cam2gripper = cv2.calibrateHandEye(
            R_gripper2base, t_gripper2base,
            R_target2cam, t_target2cam,
            method=cv2.CALIB_HAND_EYE_TSAI
        )
        
        T_ee_cam = np.eye(4)
        T_ee_cam[:3,:3] = R_cam2gripper
        T_ee_cam[:3,3] = t_cam2gripper.flatten()
        
        self.T_ee_cam = T_ee_cam
        np.save("calibration/T_ee_cam.npy", T_ee_cam)
        print(f"Calibration saved. Translation: {t_cam2gripper.flatten()}")
        return T_ee_cam
    
    def verify_calibration(self, T_ee_cam: np.ndarray) -> float:
        """
        Verify by projecting a point with known 3D position.
        Returns reprojection error (mm).
        """
        errors = []
        for _ in range(5):
            self.robot.move_joints(
                np.random.uniform(-0.3, 0.3, 7) + [0, -0.785, 0, -2.356, 0, 1.571, 0.785],
                speed=0.1
            )
            T_base_ee = self.robot.get_ee_transform()
            
            frame = self.camera.get_rgb()
            corners, ids, _ = self.detector.detectMarkers(frame)
            if ids is not None:
                rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
                    corners, 0.05,
                    self.camera.intrinsics_matrix,
                    self.camera.distortion_coeffs
                )
                T_cam_tag = self._rvec_tvec_to_matrix(rvec[0], tvec[0])
                T_base_tag_estimated = T_base_ee @ T_ee_cam @ T_cam_tag
                
                # Ground truth: tag at known fixed position
                T_base_tag_gt = np.array([...])  # Known position
                
                error_mm = np.linalg.norm(
                    T_base_tag_estimated[:3,3] - T_base_tag_gt[:3,3]
                ) * 1000
                errors.append(error_mm)
        
        mean_error = np.mean(errors)
        print(f"Calibration verification: {mean_error:.2f}mm mean error")
        return mean_error

Target: reprojection error < 2mm. If > 5mm, re-calibrate. If > 10mm, inspect camera mounting and tag flatness.

Key calibration notes

• Lighting consistency: Calibrate under the same lighting conditions as deployment. AprilTag detection is sensitive to shadows.
• Tag flatness: The tag must be perfectly flat. Paper taped to a cardboard box is not good enough — use aluminum composite board.
• Thermal drift: RealSense cameras exhibit small thermal drift after 20–30 minutes. Warm up the camera for 10 minutes before calibrating.
• Joint reproducibility: Franka Panda has joint angle reproducibility of ±0.1°. When verifying, always approach from the same direction to avoid backlash effects.

Step 2: Handling Domain Gap

Domain gap is the primary reason sim-trained models fail on real robots. Three main techniques:

2a. Real-world data augmentation (inference-time)

Rather than retraining, apply augmentation at inference time to make real images "look more like" sim training data:

# deployment/visual_preprocessor.py
import torch
import torchvision.transforms as T
from PIL import Image
import numpy as np

class RealWorldPreprocessor:
    """
    Normalizes real camera images to match LIBERO training data distribution.
    """
    def __init__(self, target_stats_path: str = "data/libero_stats.json"):
        import json
        with open(target_stats_path) as f:
            stats = json.load(f)
        
        # LIBERO training data statistics
        self.libero_mean = torch.tensor(stats["mean"])  # [3]
        self.libero_std = torch.tensor(stats["std"])    # [3]
        
    def preprocess(self, raw_frame: np.ndarray) -> torch.Tensor:
        """
        Pipeline:
        1. Denoising (bilateral filter)
        2. White balance correction
        3. Histogram matching to LIBERO distribution
        4. Normalize to model input format
        """
        import cv2
        # Step 1: Bilateral denoising (preserves edges)
        denoised = cv2.bilateralFilter(raw_frame, d=5, sigmaColor=75, sigmaSpace=75)
        
        # Step 2: White balance (gray world assumption)
        balanced = self._white_balance(denoised)
        
        # Step 3: Histogram matching
        matched = self._histogram_match(balanced)
        
        # Step 4: Normalize
        tensor = torch.from_numpy(matched).float().permute(2, 0, 1) / 255.0
        tensor = (tensor - self.libero_mean[:,None,None]) / self.libero_std[:,None,None]
        
        return tensor
    
    def _white_balance(self, img: np.ndarray) -> np.ndarray:
        """Simple gray world white balance."""
        result = img.copy().astype(np.float32)
        avg = result.mean(axis=(0,1))
        gray = avg.mean()
        scale = gray / (avg + 1e-6)
        result = np.clip(result * scale, 0, 255).astype(np.uint8)
        return result
    
    def _histogram_match(self, img: np.ndarray) -> np.ndarray:
        """Match histogram to LIBERO reference image statistics."""
        reference = np.load("data/libero_reference_mean.npy")
        
        matched = np.zeros_like(img)
        for channel in range(3):
            matched[:,:,channel] = self._match_channel(
                img[:,:,channel], 
                reference[:,:,channel]
            )
        return matched
    
    def _match_channel(self, src: np.ndarray, ref: np.ndarray) -> np.ndarray:
        """Match CDF of src channel to ref channel."""
        src_hist, _ = np.histogram(src.flatten(), 256, [0,256])
        ref_hist, _ = np.histogram(ref.flatten(), 256, [0,256])
        
        src_cdf = src_hist.cumsum()
        ref_cdf = ref_hist.cumsum()
        
        src_cdf_norm = src_cdf / src_cdf[-1]
        ref_cdf_norm = ref_cdf / ref_cdf[-1]
        
        lut = np.zeros(256, dtype=np.uint8)
        j = 0
        for i in range(256):
            while j < 255 and ref_cdf_norm[j] < src_cdf_norm[i]:
                j += 1
            lut[i] = j
        
        return lut[src]

2b. Mixed dataset fine-tuning (if real data is available)

If you can collect real demonstrations (even a small amount), blend with LIBERO at a 20% real / 80% sim ratio:

class MixedDataset(torch.utils.data.Dataset):
    def __init__(self, sim_dataset, real_dataset, real_ratio=0.2):
        self.sim_dataset = sim_dataset
        self.real_dataset = real_dataset
        self.real_ratio = real_ratio
        
    def __len__(self):
        return len(self.sim_dataset)
    
    def __getitem__(self, idx):
        if np.random.random() < self.real_ratio and len(self.real_dataset) > 0:
            real_idx = idx % len(self.real_dataset)
            return self.real_dataset[real_idx]
        return self.sim_dataset[idx]

Even 500–1000 real demonstrations significantly improves sim-to-real transfer.

2c. Depth-guided grasp refinement

AnyGrasp performs better with real depth data than simulated depth, but the RealSense D435i has structured light noise on textureless surfaces (matte black, glass, fabric). Process accordingly:

# deployment/depth_processor.py
import numpy as np
import cv2
import pyrealsense2 as rs

class DepthProcessor:
    def __init__(self):
        self.hole_filling = rs.hole_filling_filter()
        self.spatial = rs.spatial_filter()
        self.temporal = rs.temporal_filter()
        
    def process(self, depth_frame):
        """Apply RealSense post-processing pipeline."""
        filtered = self.spatial.process(depth_frame)
        filtered = self.temporal.process(filtered)
        filtered = self.hole_filling.process(filtered)
        return filtered
    
    def validate_depth(self, depth_image: np.ndarray, roi: tuple) -> bool:
        """
        Check depth quality in ROI before grasping.
        Reject if > 20% pixels are invalid (0 or beyond max range).
        """
        x1, y1, x2, y2 = roi
        roi_depth = depth_image[y1:y2, x1:x2]
        
        invalid_mask = (roi_depth == 0) | (roi_depth > 2000)  # > 2m
        invalid_ratio = invalid_mask.sum() / roi_depth.size
        
        if invalid_ratio > 0.2:
            print(f"WARNING: {invalid_ratio:.1%} invalid depth in ROI")
            return False
        return True

Step 3: Adapting SAP Timing for Real Robots

The SAP Protocol in LIBERO uses stride=5 (check verifier every 5 steps). On a physical robot, "step" has a different meaning:

Verifier latency on a real robot depends on model placement. Local GPU (Qwen2.5-VL): ~200ms. Cloud API: up to 1–2 seconds.

# deployment/sap_real_world.py
import time
from typing import Optional
import threading

class RealWorldSAPOrchestrator:
    """
    SAP Protocol adapted for real-world deployment.
    Key changes vs simulation:
    1. Async verifier calls (don't block the action loop)
    2. Adaptive stride based on actual timing
    3. Emergency stop integration with Franka FCI
    """
    
    def __init__(
        self,
        robot_controller,
        verifier,
        decomposer,
        target_action_freq: float = 15.0,  # Hz
        verifier_stride_ms: float = 300.0  # ms between verify calls
    ):
        self.robot = robot_controller
        self.verifier = verifier
        self.decomposer = decomposer
        self.action_freq = target_action_freq
        self.verifier_stride_ms = verifier_stride_ms
        
        # Async verifier state
        self._verifier_result: Optional[str] = None
        self._verifier_thread: Optional[threading.Thread] = None
        self._last_verify_time: float = 0
        
    def execute_task(self, task_instruction: str) -> bool:
        """Main task execution loop."""
        subgoals = self.decomposer.decompose(task_instruction)
        print(f"Decomposed into {len(subgoals)} subgoals: {subgoals}")
        
        for sg_idx, subgoal in enumerate(subgoals):
            print(f"\n[Subgoal {sg_idx+1}/{len(subgoals)}]: {subgoal}")
            
            success = self._execute_subgoal(subgoal)
            if not success:
                print(f"Failed at subgoal {sg_idx+1}. Attempting recovery...")
                recovered = self._recover(subgoal)
                if not recovered:
                    print("Recovery failed. Aborting task.")
                    return False
        
        return True
    
    def _execute_subgoal(self, subgoal: str) -> bool:
        """Execute one subgoal with async verification."""
        actions = self.openvla.generate_actions(subgoal, horizon=50)
        action_buffer = list(actions)
        verifier_window = []
        
        while action_buffer:
            step_start = time.time()
            
            action = action_buffer.pop(0)
            obs = self.robot.step(action)
            verifier_window.append(obs)
            
            if len(verifier_window) > 2:
                verifier_window.pop(0)
            
            # Check if async verifier returned a result
            if self._verifier_result is not None:
                result = self._verifier_result
                self._verifier_result = None
                
                if result == "subgoal_complete":
                    return True
                elif result == "failure_detected":
                    return False
                # "in_progress" → continue
            
            # Launch new async verifier call if stride elapsed
            now = time.time()
            if (now - self._last_verify_time) * 1000 > self.verifier_stride_ms:
                if self._verifier_thread is None or not self._verifier_thread.is_alive():
                    frames = [w["rgb"] for w in verifier_window]
                    self._launch_async_verify(subgoal, frames)
                    self._last_verify_time = now
            
            # Refill action buffer before it empties
            if len(action_buffer) < 5:
                new_actions = self.openvla.generate_actions(
                    subgoal, horizon=20, obs=obs
                )
                action_buffer.extend(new_actions)
            
            # Maintain target action frequency
            elapsed = time.time() - step_start
            sleep_time = (1.0 / self.action_freq) - elapsed
            if sleep_time > 0:
                time.sleep(sleep_time)
        
        return False
    
    def _launch_async_verify(self, subgoal: str, frames: list):
        """Launch verifier call in background thread."""
        def _verify():
            result = self.verifier.check(subgoal, frames)
            self._verifier_result = result
        
        self._verifier_thread = threading.Thread(target=_verify, daemon=True)
        self._verifier_thread.start()
    
    def _recover(self, failed_subgoal: str) -> bool:
        """Recovery: move gripper to safe position, retry."""
        print("Recovery: lifting gripper to safe position...")
        self.robot.move_to_safe_position()
        time.sleep(1.0)
        return self._execute_subgoal(failed_subgoal)

Step 4: Latency Budget and Real-Time Constraints

With Franka FCI, the robot controller loop runs at 1kHz (1ms cycle). Our pipeline cannot interfere with this loop — we only send target positions, not torque commands.

Latency budget analysis:

┌─────────────────────────────────────────────┐
│ Total budget: 67ms (15 Hz action frequency)  │
├─────────────────────────────────────────────┤
│ Camera capture:        8ms                   │
│ Depth processing:      5ms                   │
│ Visual preprocessing:  3ms                   │
│ OpenVLA inference:    35ms (RTX 3090)         │
│ Action post-process:   3ms                   │
│ Robot command send:    2ms                   │
│ Buffer:               11ms                   │
├─────────────────────────────────────────────┤
│ Total:                67ms ✓                 │
└─────────────────────────────────────────────┘

OpenVLA inference is the biggest bottleneck: 35ms on RTX 3090. With a weaker GPU (RTX 3060: ~55ms), action frequency must drop to 10 Hz.

Optimizing inference with TorchScript and fp16

# optimization/openvla_optimized.py
import torch

class OptimizedOpenVLA:
    def __init__(self, model_path: str, device: str = "cuda"):
        self.device = device
        
        from transformers import AutoModelForVision2Seq
        model = AutoModelForVision2Seq.from_pretrained(model_path)
        
        # fp16: ~40% memory reduction, ~30% faster
        model = model.half()
        
        # torch.compile with reduce-overhead mode (good for inference loops)
        self.model = torch.compile(model.to(device), mode="reduce-overhead")
        
        self._warmup()
    
    def _warmup(self):
        """Run 5 dummy inferences to trigger JIT compilation."""
        dummy_image = torch.randn(1, 3, 224, 224, device=self.device, dtype=torch.float16)
        dummy_tokens = torch.zeros(1, 50, dtype=torch.long, device=self.device)
        
        with torch.no_grad():
            for _ in range(5):
                _ = self.model(pixel_values=dummy_image, input_ids=dummy_tokens)
        
        print("OpenVLA warmed up. Ready for real-time inference.")
    
    @torch.inference_mode()
    def generate_actions(self, instruction: str, obs: dict, horizon: int = 10) -> list:
        """fp16 + compile = ~35ms on RTX 3090."""
        image_tensor = obs["rgb_tensor"].half().to(self.device)
        
        inputs = self.processor(
            text=instruction,
            images=image_tensor,
            return_tensors="pt"
        ).to(self.device)
        
        inputs = {k: v.half() if v.dtype == torch.float32 else v 
                  for k, v in inputs.items()}
        
        output = self.model.generate(**inputs, max_new_tokens=horizon * 7)
        return self._decode_actions(output)

Step 5: Real-World Experimental Results

The Agentic Robot paper reports results on a physical Franka Panda with three real-world tasks:

Domain gap is smallest for simple tasks (rigid pick-and-place) and largest for complex multi-step tasks — exactly as expected.

Comparison with non-SAP baselines

+21.3% improvement from the full pipeline over plain OpenVLA on the physical robot.

Failure mode analysis

Among the SAP pipeline failures on the physical robot:

• 42%: Grasp pose offset due to depth noise (low-texture object surfaces)
• 28%: Verifier false negatives (failure not detected in time)
• 19%: Kinematic singularity — joint limit near workspace boundary
• 11%: Network/timing issues (verifier call timeout)

Key insight: Investing in depth quality (better camera, better surface treatment for target objects) would eliminate 42% of failure cases.

Deployment Checklist for New Teams

A summary of what to do before the first real-world run:

Hardware setup:

• Camera mount is rigid (no vibration when robot moves)
• AprilTag board is flat on a rigid surface
• Workstation GPU ≥ RTX 3060 (8GB VRAM minimum)
• Local network latency < 5ms between workstation and robot

Calibration:

• Hand-eye calibration with ≥ 15 pose pairs
• Verified reprojection error < 2mm
• Re-calibrate whenever camera mount changes

Software:

• RealSense SDK + post-processing filters configured
• OpenVLA compiled with fp16 + torch.compile
• AnyGrasp server running on GPU
• Franka FCI operational mode enabled

Testing (in order):

• Camera calibration test (project known point, check error)
• Grasp pose test with single rigid object on flat surface
• Full pipeline test with easy task (simple pick-and-place)
• Verifier test: manually cause failure, confirm detection
• Full SAP test with complex multi-step task

Series Conclusion

We started with a simple question — why isn't a monolithic VLA enough? — and ended with a complete system deployable on physical robots.

Summary across all five parts:

The complete pipeline achieves 63.4% on a physical robot versus 42.1% for plain OpenVLA — a +21.3% improvement without retraining any model from scratch.

The most interesting takeaway: every component is independently swappable. DeepSeek-V3 → any LLM. OpenVLA → π0 or RoboVLMs. AnyGrasp → GraspNet or contact-GraspNet. The framework outlasts the specific models inside it.

Related Posts

• Part 4: SAP Protocol + Temporal Verifier — 79.6% LIBERO
• Part 3: ALRM — Code-as-Policy vs Tool-as-Policy in ReAct
• Part 2: Perception Agent — Florence-v2 + AnyGrasp
• Part 1: Why Multi-Agent Outperforms Monolithic VLA?
• PlotJuggler G1 Debug Series — Real Robot Debugging