OCT - NOV PROGRESS REPORT

DIGITAL TWIN &
SHADOW CONTROL

A drift-free, math-engine powered control strategy for the Unitree G1. Bridging simulation and reality with PyBullet and Human-in-the-Loop perception.

Watch Demos Read Architecture
Eng. Omer Larranaga
Project Lead | CTO OceanBuilders LLC
01

Digital Twin Architecture

This system uses a digital twin where a simulated robot in PyBullet mirrors and computes control for the physical Unitree G1. The simulation acts as a real-time math engine.

The Math Engine

Instead of analytical IK, we use PyBullet in p.DIRECT mode:

  • Step 1: Move simulated arm. PyBullet computes joint angles (q_integrator).
  • Step 2: Mirror angles to real robot via Unitree SDK.

No Drift Logic

Typical IK solvers bias toward a nominal pose. Here, the system holds the last valid configuration. Result: Arm stays fixed when idle.

SYNCED: SIM vs REALITY
omeris@albox:~/IK_Solver
Mode DoF Function
Free Mode 3 (XYZ) Positioning without orientation constraint.
Locked Mode 6 (Pose) Full placement with fixed RPY.
Vision Pipeline

Perception & Fusion

Combines slow semantic understanding (VLM) with fast geometric tracking (ArUco) to maintain real-time responsiveness.

Perception View
VLM: ~0.9s ArUco: 30FPS
src: realsense_receive_rgb_min.py

Vision Language Model

SEMANTIC

Answers "Where is the shampoo?" providing bounding boxes for initialization.

ArUco Tracker

GEOMETRIC

High-speed tracking of the hand marker for closed-loop control.

Servo Logic
error_px = target_px - hand_px
depth_req = client.probe_depth()
03

Automating Visual Servoing

Control Loop Diagram

Figure 3.1: IBVS Architecture

Controller.py 
if state.auto_mode:
    # 1. Get Error Vector
    dx, dy = received_error_px
    
    # 2. Hand-Eye Mapping (Cam X -> Robot Y)
    vy = -dx * GAIN_XY
    vz = -dy * GAIN_XY
    
    # 3. Depth Correction
    vx = (target_depth - hand_depth) * GAIN_Z
    
    robot.set_velocity([vx, vy, vz])
Trajectory Interpolation
MOTION PLAN VERIFIED

15-point spline interpolation showing arm trajectory avoiding obstacles.

System Specifications

ZeroMQ / MQTT

Low-latency transport of dpx + depth data.

Hand-Eye Mapping

Static transform: +X_img → ±Y_robot

Spatial Awareness

Navigation & SLAM

ROS 2 LIDAR A* STAR

Real-time environmental reconstruction for path planning. The system runs a background SLAM thread (live_slam).

3D Voxel Map

Persistent point cloud colored by height (Turbo colormap) to distinguish floor from obstacles.

2D Occupancy Grid

Uses percentile filtering and exponential smoothing for clear navigation.

Tactile Sensors
Active

Active feedback on fingertips

slam_visualizer
3D Voxel Map SLAM
nav_planner_node
2D Occupancy Grid Route

Optimization Roadmap

Latency Reduction

VLM currently ~0.98s. Too slow for dynamic objects.

Switch to KCF/CSRT Tracker

Offset Calibration

Marker is on hand back. Grasp center is ~10cm forward.

Apply Static Vector Offset

Safety Protocol

Prevent unintended motion during vision spikes.

Implement Deadman Switch

Live Demonstrations

Comprehensive archive of test runs and capabilities.

Collision Avoidance

Arm trajectory safety testing.

Distance Probing

Depth sensor calibration test.

VLM & ArUco Fusion

Hybrid tracking system demo.

Pick and Place

Standard manipulation cycle.

Visual Servoing

Real-time object tracking.

Auto follow

Remote operator interface.

Precision Grasping

Fine motor control test.

Dynamic Motion

Speed and acceleration checks.

Sim-to-Real Sync

Digital twin synchronization.

Advanced Manipulation

Complex trajectory execution.

Object ID: Bottle

Specific object training set.

Depth Stream

Raw RealSense output.

VLM Pointer Logic

Language-to-coordinate mapping.

HRI Interaction

Human-Robot Interaction tests.

Calibration Hand to Eye

Startup and calibration sequence.