Series: AI Agent Pipeline cho Robot Manipulation — Bài 5/5 (Cuối)
Bài toán của việc ra khỏi simulation
Bốn bài trước, chúng ta đã xây dựng một pipeline hoàn chỉnh: ManiAgent phân tích scene, AnyGrasp tính grasp pose, ALRM viết code điều khiển, và SAP Protocol với Temporal Verifier đảm bảo pipeline phục hồi khi thất bại. Kết quả: 79.6% success rate trên LIBERO benchmark.
Nhưng LIBERO là simulation. Và simulation luôn "tốt hơn" thực tế một cách giả tạo.
Khi bạn chuyển sang robot vật lý, ba vấn đề xuất hiện cùng lúc:
1. Domain gap về visual: Ánh sáng thực tế thay đổi. Shadow, reflection, glare — những thứ simulation giả vờ không tồn tại. Model được train trên ảnh render sạch sẽ, nhưng phải inference trên ảnh camera với noise, blur và color drift.
2. Kinematic mismatch: Joint limits, friction, gear backlash — mỗi robot thật có "tính cách" riêng. LIBERO dùng Franka Panda mô phỏng lý tưởng; robot Franka thật của bạn sẽ khác. Không nhiều, nhưng đủ để grasp pose lệch 3–5mm và khiến thất bại.
3. Latency và timing: Simulation chạy deterministic. Robot thật có network latency, motor control loop delay, và camera sync drift. SAP Protocol giả định timing nhất định — khi deploy thật, những giả định này có thể bị vi phạm.
Bài này giải quyết cả ba vấn đề.
Kiến trúc deploy thực tế
Trước khi đi vào từng bước, nhìn tổng thể pipeline deploy:
┌─────────────────────────────────────────────────────────┐
│ 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) │ │
│ └──────────────┘ └──────────────┘ └─────────────┘ │
└─────────────────────────────────────────────────────────┘
Workstation chạy các model nặng (DeepSeek, Qwen, OpenVLA). Robot controller xử lý real-time control. Giao tiếp qua ROS 2 hoặc ZMQ với latency < 10ms trên local network.
Bước 1: Camera-to-Robot Calibration
Đây là bước đầu tiên và quan trọng nhất. Nếu calibrate sai, mọi grasp pose đều sẽ lệch — không phải lỗi của model, mà lỗi của transform.
Hand-eye calibration với 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 cho RealSense D435i trên wrist.
Sử dụng AprilTag 36h11, size 50mm.
"""
def __init__(self, robot_interface, camera):
self.robot = robot_interface
self.camera = camera
self.T_ee_cam = None # Transform từ EE frame đến 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:
"""
Di chuyển robot đến n_poses vị trí ngẫu nhiên,
ghi lại (T_base_ee, T_cam_tag) cho mỗi vị trí.
"""
pose_pairs = []
# Vị trí sampling: around nominal home pose
nominal_joints = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
for i in range(n_poses):
# Jitter ngẫu nhiên ±15° trên mỗi 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.
Trả về 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 bằng cách project một điểm biết vị trí 3D.
Trả về reprojection error (mm).
"""
# Di chuyển đến 5 pose mới, không có trong calibration set
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()
# Transform tag pose sang base frame
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: đặt tag cố định ở vị trí biết trước
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. Nếu > 5mm, re-calibrate. Nếu > 10mm, kiểm tra camera mounting và tag flatness.
Lưu ý quan trọng khi calibrate
- Lighting consistency: Calibrate trong điều kiện ánh sáng giống lúc deploy. AprilTag detection nhạy cảm với shadow.
- Tag placement: Tag phải phẳng tuyệt đối. Mảnh giấy dán trên hộp carton là không đủ tốt — dùng aluminum composite board.
- Thermal drift: Camera RealSense có thermal drift nhỏ sau 20–30 phút. Warm up camera 10 phút trước khi calibrate.
- Joint reproducibility: Franka Panda có joint angle reproducibility ±0.1°. Khi verify, luôn approach từ cùng một hướng (avoid backlash).
Bước 2: Xử lý Domain Gap
Domain gap là lý do lớn nhất khiến model "sim-trained" fail trên real robot. Ba kỹ thuật chính:
2a. Real-world data augmentation (inference-time)
Thay vì retrain, apply augmentation lúc inference để làm ảnh thật "giống" ảnh sim hơn:
# deployment/visual_preprocessor.py
import torch
import torchvision.transforms as T
from PIL import Image, ImageEnhance
import numpy as np
class RealWorldPreprocessor:
"""
Chuẩn hóa ảnh real camera về distribution gần với LIBERO training data.
"""
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
"""
# Step 1: Bilateral denoising (preserve edges)
import cv2
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."""
# Load LIBERO reference (pre-computed mean image)
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]
# Build lookup table
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. Background randomization (training-time nếu có data thật)
Nếu bạn có thể thu thập dữ liệu thật (dù ít), trộn với LIBERO data theo tỷ lệ 20% real / 80% sim:
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:
# Sample từ real data
real_idx = idx % len(self.real_dataset)
return self.real_dataset[real_idx]
return self.sim_dataset[idx]
Chỉ 500–1000 real demonstrations đủ để cải thiện sim-to-real transfer đáng kể.
2c. Depth-guided grasp refinement
AnyGrasp hoạt động tốt hơn với depth data thực tế so với sim depth. Nhưng RealSense D435i có structured light noise ở vùng texture-less (bề mặt đen mờ, kính, vải). Xử lý:
# deployment/depth_processor.py
import numpy as np
import cv2
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 filters."""
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:
"""
Kiểm tra depth quality trong ROI trước khi grasp.
Reject nếu > 20% pixels là invalid (0 hoặc 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
Bước 3: Adapt SAP Timing cho Real Robot
SAP Protocol trong LIBERO dùng stride=5 (check verifier mỗi 5 steps). Với robot thật, "step" có nghĩa khác:
| LIBERO (sim) | Franka (real) | |
|---|---|---|
| Action frequency | 20 Hz | 15 Hz (camera limit) |
| Step duration | 50ms | ~67ms |
| Verifier latency | ~5ms (no network) | 200–800ms (LLM call) |
| stride=5 = check every | 250ms | 335ms |
Verifier latency trên real robot phụ thuộc vào model placement. Nếu Qwen2.5-VL chạy local GPU, latency ~200ms. Nếu dùng cloud API, có thể lên đến 1–2 giây.
# deployment/sap_real_world.py
import time
from typing import Optional
import threading
class RealWorldSAPOrchestrator:
"""
SAP Protocol adapter cho real-world deployment.
Key changes vs simulation:
1. Async verifier calls (không block action loop)
2. Adaptive stride dựa trên actual timing
3. Emergency stop integration với Franka FCI
"""
def __init__(
self,
robot_controller,
verifier,
decomposer,
target_action_freq: float = 15.0, # Hz
verifier_stride_ms: float = 300.0 # ms giữa các lần verify
):
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[bool] = None
self._verifier_thread: Optional[threading.Thread] = None
self._last_verify_time: float = 0
def execute_task(self, task_instruction: str) -> bool:
"""Main execution loop."""
# Phase 1: Subgoal decomposition
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 một subgoal với async verification."""
# Generate action sequence từ OpenVLA
actions = self.openvla.generate_actions(subgoal, horizon=50)
action_buffer = list(actions)
verifier_window = [] # Sliding window for verifier
step_count = 0
while action_buffer:
step_start = time.time()
action = action_buffer.pop(0)
# Execute action
obs = self.robot.step(action)
verifier_window.append(obs)
# Maintain sliding window K=2 (2 frames)
if len(verifier_window) > 2:
verifier_window.pop(0)
step_count += 1
# Check if async verifier returned 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 needed
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
# Regenerate actions nếu buffer gần hết
if len(action_buffer) < 5:
new_actions = self.openvla.generate_actions(
subgoal, horizon=20, obs=obs
)
action_buffer.extend(new_actions)
# Maintain action frequency
elapsed = time.time() - step_start
sleep_time = (1.0 / self.action_freq) - elapsed
if sleep_time > 0:
time.sleep(sleep_time)
# Buffer exhausted without completion
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: gripper về safe position, retry."""
print("Recovery: lifting gripper to safe position...")
self.robot.move_to_safe_position()
time.sleep(1.0)
# Retry subgoal một lần
return self._execute_subgoal(failed_subgoal)
Bước 4: Latency Budget và Real-Time Constraints
Với Franka FCI, robot controller loop chạy ở 1kHz (1ms cycle). Pipeline của chúng ta không thể can thiệp vào loop này — chúng ta chỉ gửi target positions, không phải torque commands.
Phân tích latency budget:
┌─────────────────────────────────────────────┐
│ 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 là bottleneck lớn nhất: 35ms trên RTX 3090. Nếu bạn dùng GPU yếu hơn (RTX 3060: ~55ms), action frequency phải giảm xuống 10 Hz.
Tối ưu inference với TorchScript và fp16
# optimization/openvla_optimized.py
import torch
class OptimizedOpenVLA:
def __init__(self, model_path: str, device: str = "cuda"):
self.device = device
# Load model
from transformers import AutoModelForVision2Seq
model = AutoModelForVision2Seq.from_pretrained(model_path)
# Convert to fp16 — giảm ~40% memory, ~30% faster
model = model.half()
# Compile với torch.compile (PyTorch 2.0+)
# mode="reduce-overhead" tốt cho inference loop
self.model = torch.compile(model.to(device), mode="reduce-overhead")
# Warm up (tránh JIT compilation delay lúc đầu)
self._warmup()
def _warmup(self):
"""Chạy 5 dummy inferences để compile graph."""
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:
"""Generate action sequence. fp16 + compile = ~35ms on RTX 3090."""
image_tensor = obs["rgb_tensor"].half().to(self.device)
# Tokenize instruction
inputs = self.processor(
text=instruction,
images=image_tensor,
return_tensors="pt"
).to(self.device)
# Cast inputs to fp16
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)
actions = self._decode_actions(output)
return actions
Bước 5: Kết quả thực nghiệm trên robot thật
Paper Agentic Robot báo cáo kết quả trên Franka Panda với 3 task thực:
| Task | Sim (LIBERO) | Real Robot | Gap |
|---|---|---|---|
| Pick-and-place (rigid) | 89.0% | 81.3% | -7.7% |
| Pour liquid | 73.2% | 61.8% | -11.4% |
| Multi-step assembly | 61.6% | 47.2% | -14.4% |
Domain gap nhỏ nhất ở task đơn giản (pick-and-place rigid objects), lớn nhất ở task phức tạp (multi-step assembly) — đúng như kỳ vọng.
So sánh với baseline không có SAP
| System | Real-world avg | Notes |
|---|---|---|
| Plain OpenVLA | 42.1% | No recovery |
| OpenVLA + AnyGrasp | 58.6% | Better grasp, no recovery |
| Full SAP pipeline | 63.4% | Full system |
| Human teleoperation | 91.0% | Upper bound |
+21.3% improvement từ pipeline đầy đủ so với OpenVLA đơn thuần trên real robot.
Failure analysis
Trong các trường hợp thất bại của SAP pipeline trên real robot:
- 42%: Grasp pose lệch do depth noise (vật thể có surface texture thấp)
- 28%: Verifier false negative (không phát hiện failure kịp thời)
- 19%: Kinematic singularity — robot joint limit gần reach workspace boundary
- 11%: Network/timing issues (verifier call timeout)
Insight: Đầu tư vào depth quality (better depth camera, better surface treatment cho objects) sẽ giải quyết 42% failure cases.
Checklist deploy cho team mới
Tổng hợp những gì cần làm trước khi chạy thử lần đầu:
Hardware setup:
- Camera mount cứng (không rung khi robot di chuyển)
- AprilTag board phẳng trên bề mặt rigid
- Workstation GPU ≥ RTX 3060 (8GB VRAM minimum)
- Local network latency < 5ms giữa workstation và robot
Calibration:
- Hand-eye calibration với ≥ 15 pose pairs
- Verify reprojection error < 2mm
- Re-calibrate nếu camera mount thay đổi
Software:
- RealSense SDK + post-processing filters configured
- OpenVLA compiled với fp16 + torch.compile
- AnyGrasp server running trên GPU
- Franka FCI operational mode enabled
Testing (theo thứ tự):
- Camera calibration test (project known point, check error)
- Grasp pose test với single rigid object, flat surface
- Full pipeline test với easy task (simple pick-place)
- Verifier test: manually cause failure, confirm detection
- Full SAP test với complex multi-step task
Kết luận series
Chúng ta đã đi từ câu hỏi đơn giản — tại sao VLA đơn thuần không đủ? — đến một system hoàn chỉnh có thể deploy trên robot thật.
Tổng kết 5 bài:
| Bài | Component | Điểm chính |
|---|---|---|
| 1 | ManiAgent overview | Multi-agent đánh bại monolithic VLA: 86.8% vs 55.7% |
| 2 | Perception + Grasp | Florence-v2 detect → AnyGrasp pose → Franka execute |
| 3 | ALRM reasoning | Code-as-Policy vs Tool-as-Policy trong ReAct loop |
| 4 | SAP Verifier | Temporal sliding window phát hiện failure sớm |
| 5 | Real-world deploy | Calibration + domain gap + async timing |
Pipeline đầy đủ đạt 63.4% trên robot thật so với 42.1% của plain OpenVLA — +21.3% cải thiện mà không cần retrain model nào từ đầu.
Điều thú vị nhất: mỗi component đều có thể swap độc lập. DeepSeek-V3 → bất kỳ LLM nào. OpenVLA → π0 hoặc RoboVLMs. AnyGrasp → GraspNet hoặc contact-GraspNet. Framework là thứ tồn tại lâu hơn các model cụ thể.



