sync recent changes

additional_functions/check_loco_active.py

@@ -0,0 +1,52 @@
+#!/usr/bin/env python3
+"""
+Check if the Unitree locomotion DDS domain / LocoClient is already initialized.
+Does NOT send any commands — safe to run.
+"""
+
+import sys
+import psutil  # pip install psutil if missing
+import os
+
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactory
+
+def is_loco_domain_active():
+    """Detect if a ChannelFactory singleton is already initialized in this process."""
+    fac = ChannelFactory()
+    active = getattr(fac, "_ChannelFactory__initialized", False)
+    participant = getattr(fac, "_ChannelFactory__participant", None)
+    domain = getattr(fac, "_ChannelFactory__domain", None)
+    return active, participant, domain
+
+def check_other_python_processes():
+    """See if another Python process using the Unitree SDK is alive."""
+    procs = []
+    for p in psutil.process_iter(attrs=["pid", "name", "cmdline"]):
+        try:
+            if "python" in p.info["name"].lower() and any("unitree" in arg for arg in p.info["cmdline"]):
+                procs.append((p.info["pid"], " ".join(p.info["cmdline"])))
+        except Exception:
+            pass
+    return procs
+
+if __name__ == "__main__":
+    active, participant, domain = is_loco_domain_active()
+    print("=== Local DDS Factory State ===")
+    print(f"Factory initialized: {active}")
+    print(f"Participant: {participant}")
+    print(f"Domain: {domain}")
+    print()
+
+    print("=== Other Python processes using Unitree SDK ===")
+    procs = check_other_python_processes()
+    if not procs:
+        print("No other active Unitree-related Python processes found.")
+    else:
+        for pid, cmd in procs:
+            print(f"PID {pid}: {cmd}")
+
+    print()
+    if os.path.exists("/tmp/CycloneDDS"):
+        print("Warning: /tmp/CycloneDDS exists — could indicate leftover DDS shared memory.")

additional_functions/check_robot_connection.py

@@ -0,0 +1,92 @@
+#!/usr/bin/env python3
+"""
+Check if robot is reachable and responding to DDS messages.
+This helps diagnose the [ClientStub] send request error.
+"""
+
+import time
+import sys
+
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+
+def check_connection(network_interface="en7"):
+    """Check connection to robot step by step."""
+    print("=" * 60)
+    print("G1 Robot Connection Diagnostic")
+    print("=" * 60)
+    print(f"Network interface: {network_interface}")
+    print()
+    
+    # Step 1: Initialize DDS
+    print("[1/5] Initializing DDS ChannelFactory...")
+    try:
+        ChannelFactoryInitialize(0, network_interface)
+        print("      SUCCESS: ChannelFactory initialized")
+    except Exception as e:
+        print(f"      FAILED: {e}")
+        return False
+    
+    # Step 2: Wait for discovery
+    print("[2/5] Waiting for DDS discovery (5 seconds)...")
+    print("      (This allows DDS to find the robot on the network)")
+    time.sleep(5)
+    print("      SUCCESS: Discovery period complete")
+    
+    # Step 3: Create LocoClient
+    print("[3/5] Creating LocoClient...")
+    try:
+        loco_client = LocoClient()
+        print("      SUCCESS: LocoClient created")
+    except Exception as e:
+        print(f"      FAILED: {e}")
+        return False
+    
+    # Step 4: Set timeout and initialize
+    print("[4/5] Initializing LocoClient (timeout=10s)...")
+    try:
+        loco_client.SetTimeout(10.0)
+        loco_client.Init()
+        print("      SUCCESS: LocoClient initialized")
+    except Exception as e:
+        print(f"      FAILED: {e}")
+        return False
+    
+    # Step 5: Test command
+    print("[5/5] Sending test command (ZeroTorque)...")
+    print("      This will show if robot is listening...")
+    try:
+        result = loco_client.ZeroTorque()
+        print(f"      SUCCESS: Command sent, result: {result}")
+        print()
+        print("=" * 60)
+        print("CONNECTION TEST PASSED!")
+        print("=" * 60)
+        print("Your robot is connected and responding.")
+        print("The test_damp_zero_torque.py script should work now.")
+        return True
+    except Exception as e:
+        print(f"      FAILED: {e}")
+        print()
+        print("=" * 60)
+        print("CONNECTION TEST FAILED")
+        print("=" * 60)
+        print()
+        print("Troubleshooting:")
+        print("  1. Check robot is powered on")
+        print("  2. Check network connection:")
+        print(f"     sudo ifconfig {network_interface} 192.168.123.100 netmask 255.255.255.0")
+        print("  3. Ping robot: ping 192.168.123.164")
+        print("  4. Check robot mode (should be 'ai' mode)")
+        print("  5. Check if another program is controlling the robot")
+        return False
+
+if __name__ == "__main__":
+    if len(sys.argv) > 1:
+        interface = sys.argv[1]
+    else:
+        interface = "en7"
+    
+    check_connection(interface)

additional_functions/example_enhanced_sdk.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+"""
+Simple example using the Enhanced LocoClient with cleanup.
+
+This demonstrates the enhanced SDK with proper resource management.
+"""
+
+import sys
+import time
+
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.loco.g1_loco_client_enhanced import LocoClient
+
+
+def main():
+    network_interface = "en7" if len(sys.argv) < 2 else sys.argv[1]
+    
+    print("=" * 70)
+    print("G1 Enhanced SDK Example: Damp (1s) → Zero Torque (1s)")
+    print("=" * 70)
+    print(f"Network: {network_interface}\n")
+    
+    try:
+        # Initialize DDS
+        print("[1/4] Initializing DDS...")
+        ChannelFactoryInitialize(0, network_interface)
+        time.sleep(3)
+        print("      ✓ DDS ready\n")
+        
+        # Use context manager for automatic cleanup
+        print("[2/4] Connecting to robot...")
+        with LocoClient(auto_damp_on_close=True) as client:
+            client.SetTimeout(5.0)
+            print("      ✓ Connected\n")
+            
+            # Commands
+            print("[3/4] Executing commands...")
+            
+            print("      → DAMP mode...")
+            client.Damp()
+            time.sleep(1)
+            print("      ✓ Damp complete")
+            
+            print("      → ZERO TORQUE mode...")
+            client.ZeroTorque()
+            time.sleep(1)
+            print("      ✓ Zero torque complete\n")
+            
+            print("[4/4] Disconnecting...")
+            # Context manager will auto-cleanup and damp here
+        
+        print("      ✓ Disconnected safely\n")
+        print("=" * 70)
+        print("SUCCESS: All operations completed")
+        print("=" * 70)
+        
+    except KeyboardInterrupt:
+        print("\n⚠ Interrupted by user")
+        return 1
+        
+    except Exception as e:
+        print(f"\n❌ Error: {e}")
+        return 1
+    
+    return 0
+
+
+if __name__ == "__main__":
+    exit(main())

additional_functions/monitor_robot.py

@@ -0,0 +1,209 @@
+#!/usr/bin/env python3
+"""
+G1 Robot Monitor - IMU, Force/Torque, Temperature, and Battery
+
+Displays:
+- IMU data (orientation, gyro, accelerometer)
+- Motor force/torque (estimated from current)
+- Motor temperatures
+- Battery status (requires BMS topic, may not be available)
+
+Usage:
+    python monitor_robot.py <network_interface>
+    Example: python monitor_robot.py en7
+"""
+
+import sys
+import time
+import numpy as np
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+
+# Try to import BMS (may not be available on all systems)
+try:
+    from unitree_sdk2py.idl.unitree_hg.msg.dds_ import BmsState_
+    HAS_BMS = True
+except:
+    HAS_BMS = False
+    print("Note: BMS not available in this SDK version")
+
+
+class RobotMonitor:
+    def __init__(self):
+        self.latest_state = None
+        self.latest_bms = None
+        self.update_count = 0
+        
+    def state_handler(self, msg: LowState_):
+        """Handle low-level state updates (IMU, motors)"""
+        self.latest_state = msg
+        self.update_count += 1
+    
+    def bms_handler(self, msg):
+        """Handle battery management system updates"""
+        self.latest_bms = msg
+    
+    def print_status(self):
+        """Print formatted sensor data"""
+        if not self.latest_state:
+            print("Waiting for robot data...")
+            return
+        
+        state = self.latest_state
+        
+        # Clear screen and print header
+        print("\033[2J\033[H")  # Clear screen
+        print("=" * 80)
+        print(f"G1 ROBOT MONITOR - Updates: {self.update_count}")
+        print("=" * 80)
+        
+        # ============ IMU DATA ============
+        print("\n🧭 IMU DATA:")
+        imu = state.imu_state
+        
+        # Orientation (Euler angles)
+        roll_deg = np.degrees(imu.rpy[0])
+        pitch_deg = np.degrees(imu.rpy[1])
+        yaw_deg = np.degrees(imu.rpy[2])
+        print(f"  Orientation:")
+        print(f"    Roll:  {roll_deg:+7.2f}°")
+        print(f"    Pitch: {pitch_deg:+7.2f}°")
+        print(f"    Yaw:   {yaw_deg:+7.2f}°")
+        
+        # Angular velocity (gyroscope)
+        print(f"  Angular Velocity (rad/s):")
+        print(f"    X: {imu.gyroscope[0]:+7.3f}")
+        print(f"    Y: {imu.gyroscope[1]:+7.3f}")
+        print(f"    Z: {imu.gyroscope[2]:+7.3f}")
+        
+        # Linear acceleration
+        print(f"  Linear Acceleration (m/s²):")
+        print(f"    X: {imu.accelerometer[0]:+7.3f}")
+        print(f"    Y: {imu.accelerometer[1]:+7.3f}")
+        print(f"    Z: {imu.accelerometer[2]:+7.3f}")
+        
+        # Gravity magnitude (should be ~9.81 when stationary)
+        accel_mag = np.linalg.norm(imu.accelerometer)
+        print(f"  Acceleration Magnitude: {accel_mag:.2f} m/s² (gravity ~9.81)")
+        
+        print(f"  IMU Temperature: {imu.temperature}°C")
+        
+        # ============ ALL MOTORS: TORQUE & TEMPERATURE ============
+        print("\n⚡ ALL MOTORS - TORQUE & TEMPERATURE:")
+        print(f"  {'Motor':<8} {'Torque (Nm)':<14} {'Temp (°C)':<12} {'Status'}")
+        print(f"  {'-'*8} {'-'*14} {'-'*12} {'-'*20}")
+        
+        for i in range(35):
+            motor = state.motor_state[i]
+            torque = motor.tau_est
+            temp = motor.temperature[0]
+            
+            # Status indicators
+            status = []
+            if abs(torque) > 5.0:
+                status.append("⚡HIGH TORQUE")
+            if temp > 60:
+                status.append("🔥HOT")
+            elif temp > 50:
+                status.append("⚠️ WARM")
+            
+            status_str = " ".join(status) if status else "✓"
+            
+            print(f"  {i:2d}       {torque:+7.3f}        {temp:3d}          {status_str}")
+        
+        # Summary statistics
+        torques = [state.motor_state[i].tau_est for i in range(35)]
+        temps = [state.motor_state[i].temperature[0] for i in range(35)]
+        
+        print(f"\n  Statistics:")
+        print(f"    Torque:  min={min([abs(t) for t in torques]):.2f}  "
+              f"max={max([abs(t) for t in torques]):.2f}  "
+              f"mean={np.mean([abs(t) for t in torques]):.2f} Nm")
+        print(f"    Temp:    min={min(temps)}  max={max(temps)}  mean={np.mean(temps):.1f}°C")
+        
+        # ============ BATTERY ============
+        print("\n🔋 BATTERY:")
+        if self.latest_bms:
+            bms = self.latest_bms
+            print(f"  State of Charge: {bms.soc}%")
+            print(f"  State of Health: {bms.soh}%")
+            print(f"  Voltage: {bms.bmsvoltage[0] / 1000.0:.2f} V")
+            print(f"  Current: {bms.current / 1000.0:.2f} A")
+            
+            # Power calculation
+            power = (bms.bmsvoltage[0] / 1000.0) * (bms.current / 1000.0)
+            print(f"  Power: {power:.2f} W")
+            
+            # Temperature range
+            temps = [t / 10.0 for t in bms.temperature]
+            print(f"  Battery Temp: {min(temps):.1f}°C to {max(temps):.1f}°C")
+            print(f"  Charge Cycles: {bms.cycle}")
+        else:
+            print(f"  Battery data not available")
+            print(f"  (BMS topic may not be published or accessible)")
+        
+        # ============ SYSTEM INFO ============
+        print(f"\n⚙️  SYSTEM:")
+        print(f"  Tick: {state.tick}")
+        print(f"  Mode: machine={state.mode_machine}, pr={state.mode_pr}")
+        
+        print("\n" + "=" * 80)
+        print("Press Ctrl+C to stop")
+
+
+def main():
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} <network_interface>")
+        print(f"Example: python3 {sys.argv[0]} en7")
+        sys.exit(1)
+    
+    network_interface = sys.argv[1]
+    
+    print("=" * 80)
+    print("G1 ROBOT MONITOR")
+    print("=" * 80)
+    print(f"Initializing DDS on {network_interface}...")
+    ChannelFactoryInitialize(0, network_interface)
+    
+    # Create monitor
+    monitor = RobotMonitor()
+    
+    # Subscribe to low-level state (IMU, motors)
+    print("Subscribing to rt/lowstate...")
+    lowstate_sub = ChannelSubscriber("rt/lowstate", LowState_)
+    lowstate_sub.Init(monitor.state_handler, 10)
+    
+    # Try to subscribe to BMS (may not be available)
+    if HAS_BMS:
+        print("Attempting to subscribe to battery data...")
+        try:
+            # Common BMS topic names
+            for topic in ["rt/bms", "rt/battery", "rt/bms_state"]:
+                try:
+                    bms_sub = ChannelSubscriber(topic, BmsState_)
+                    bms_sub.Init(monitor.bms_handler, 10)
+                    print(f"  Subscribed to {topic}")
+                    break
+                except:
+                    pass
+        except Exception as e:
+            print(f"  Battery data not available: {e}")
+    
+    print("\nWaiting for data...")
+    time.sleep(1)
+    
+    print("Starting monitor...\n")
+    
+    try:
+        while True:
+            monitor.print_status()
+            time.sleep(0.5)  # Update display at 2 Hz
+    except KeyboardInterrupt:
+        print("\n\n" + "=" * 80)
+        print("Monitor stopped")
+        print("=" * 80)
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/read_joint_positions.py

@@ -0,0 +1,212 @@
+#!/usr/bin/env python3
+"""
+G1 Robot Joint Position Reader
+
+Displays real-time joint positions, velocities, and accelerations for all 35 motors.
+
+Usage:
+    python read_joint_positions.py <network_interface>
+    Example: python read_joint_positions.py en7
+"""
+
+import sys
+import time
+import numpy as np
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+
+
+# G1 Joint Names (35 motors)
+JOINT_NAMES = {
+    # Legs (0-11)
+    0: "left_hip_pitch",
+    1: "left_hip_roll", 
+    2: "left_hip_yaw",
+    3: "left_knee",
+    4: "left_ankle_pitch",
+    5: "left_ankle_roll",
+    6: "right_hip_pitch",
+    7: "right_hip_roll",
+    8: "right_hip_yaw", 
+    9: "right_knee",
+    10: "right_ankle_pitch",
+    11: "right_ankle_roll",
+    
+    # Waist (12-14)
+    12: "waist_yaw",
+    13: "waist_roll",
+    14: "waist_pitch",
+    
+    # Head (15-16)
+    15: "head_yaw",
+    16: "head_pitch",
+    
+    # Left Arm (17-23)
+    17: "left_shoulder_pitch",
+    18: "left_shoulder_roll",
+    19: "left_shoulder_yaw",
+    20: "left_elbow_pitch",
+    21: "left_elbow_roll",
+    22: "left_wrist_yaw",
+    23: "left_wrist_pitch",
+    
+    # Right Arm (24-30)
+    24: "right_shoulder_pitch",
+    25: "right_shoulder_roll",
+    26: "right_shoulder_yaw",
+    27: "right_elbow_pitch",
+    28: "right_elbow_roll",
+    29: "right_wrist_yaw",
+    30: "right_wrist_pitch",
+    
+    # Hands (31-34) - if applicable
+    31: "left_hand",
+    32: "right_hand",
+    33: "reserved_33",
+    34: "reserved_34",
+}
+
+
+class JointReader:
+    def __init__(self):
+        self.latest_state = None
+        self.update_count = 0
+        
+    def state_handler(self, msg: LowState_):
+        """Handle low-level state updates"""
+        self.latest_state = msg
+        self.update_count += 1
+    
+    def print_positions(self, mode='all'):
+        """
+        Print joint positions
+        mode: 'all', 'legs', 'arms', 'compact'
+        """
+        if not self.latest_state:
+            print("Waiting for robot data...")
+            return
+        
+        state = self.latest_state
+        
+        # Clear screen and print header
+        print("\033[2J\033[H")  # Clear screen
+        print("=" * 100)
+        print(f"G1 JOINT POSITIONS - Updates: {self.update_count}")
+        print("=" * 100)
+        
+        if mode == 'compact':
+            self._print_compact(state)
+        else:
+            self._print_detailed(state, mode)
+        
+        print("\n" + "=" * 100)
+        print("Press Ctrl+C to stop")
+    
+    def _print_detailed(self, state, mode):
+        """Print detailed joint information"""
+        print(f"\n{'ID':<4} {'Joint Name':<25} {'Position (rad)':<16} {'Position (deg)':<16} {'Velocity (rad/s)':<18} {'Accel (rad/s²)'}")
+        print("-" * 100)
+        
+        for i in range(35):
+            # Filter by mode
+            if mode == 'legs' and i >= 12:
+                continue
+            elif mode == 'arms' and not (17 <= i <= 30):
+                continue
+            
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            
+            position_rad = motor.q
+            position_deg = np.degrees(position_rad)
+            velocity = motor.dq
+            acceleration = motor.ddq
+            
+            print(f"{i:<4} {joint_name:<25} {position_rad:+8.4f}        {position_deg:+8.2f}°        "
+                  f"{velocity:+8.4f}          {acceleration:+8.4f}")
+    
+    def _print_compact(self, state):
+        """Print compact view - positions only"""
+        print("\n🦿 LEGS:")
+        for i in range(12):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+        
+        print("\n🦴 WAIST:")
+        for i in range(12, 15):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+        
+        print("\n🗣️  HEAD:")
+        for i in range(15, 17):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+        
+        print("\n🦾 LEFT ARM:")
+        for i in range(17, 24):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+        
+        print("\n🦾 RIGHT ARM:")
+        for i in range(24, 31):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+        
+        print("\n👋 HANDS:")
+        for i in range(31, 35):
+            motor = state.motor_state[i]
+            joint_name = JOINT_NAMES.get(i, f"motor_{i}")
+            print(f"  [{i:2d}] {joint_name:<20} {motor.q:+7.4f} rad  ({np.degrees(motor.q):+7.2f}°)")
+
+
+def main():
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} <network_interface> [mode]")
+        print(f"Example: python3 {sys.argv[0]} en7 compact")
+        print(f"\nModes:")
+        print(f"  all      - Show all joints with full details (default)")
+        print(f"  compact  - Organized by body part")
+        print(f"  legs     - Only leg joints")
+        print(f"  arms     - Only arm joints")
+        sys.exit(1)
+    
+    network_interface = sys.argv[1]
+    mode = sys.argv[2] if len(sys.argv) > 2 else 'compact'
+    
+    print("=" * 100)
+    print("G1 JOINT POSITION READER")
+    print("=" * 100)
+    print(f"Initializing DDS on {network_interface}...")
+    ChannelFactoryInitialize(0, network_interface)
+    
+    # Create reader
+    reader = JointReader()
+    
+    # Subscribe to low-level state
+    print("Subscribing to rt/lowstate...")
+    lowstate_sub = ChannelSubscriber("rt/lowstate", LowState_)
+    lowstate_sub.Init(reader.state_handler, 10)
+    
+    print("\nWaiting for data...")
+    time.sleep(1)
+    
+    print(f"Starting monitor (mode: {mode})...\n")
+    
+    try:
+        while True:
+            reader.print_positions(mode)
+            time.sleep(0.1)  # Update display at 10 Hz
+    except KeyboardInterrupt:
+        print("\n\n" + "=" * 100)
+        print("Reader stopped")
+        print("=" * 100)
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/sniff.py

@@ -0,0 +1,58 @@
+import time, threading, numpy as np
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowCmd_ as HGLowCmd
+
+TOPIC = "rt/lowcmd"
+N = 35
+
+class LowCmdSniffer:
+    def __init__(self, iface="en7"):
+        ChannelFactoryInitialize(0, iface)
+        self.sub = ChannelSubscriber(TOPIC, HGLowCmd)
+        # Use a queue depth; passing None as handler is fine for polling
+        self.sub.Init(None, 20)
+
+        self._lock = threading.Lock()
+        self._have = False
+        self.q   = np.zeros(N)
+        self.dq  = np.zeros(N)
+        self.kp  = np.zeros(N)
+        self.kd  = np.zeros(N)
+        self.tau = np.zeros(N)
+
+        threading.Thread(target=self._loop, daemon=True).start()
+
+    def _loop(self):
+        while True:
+            msg = self.sub.Read()
+            if msg is not None:
+                with self._lock:
+                    for i in range(N):
+                        mc = msg.motor_cmd[i]
+                        self.q[i]   = getattr(mc, "q", 0.0)
+                        self.dq[i]  = getattr(mc, "qd", 0.0)  # named qd in HG
+                        self.kp[i]  = getattr(mc, "kp", 0.0)
+                        self.kd[i]  = getattr(mc, "kd", 0.0)
+                        self.tau[i] = getattr(mc, "tau", 0.0)
+                    self._have = True
+            time.sleep(0.001)
+
+    def latest(self):
+        with self._lock:
+            return self._have, self.q.copy(), self.dq.copy(), self.kp.copy(), self.kd.copy(), self.tau.copy()
+
+if __name__ == "__main__":
+    sniffer = LowCmdSniffer(iface="en7")
+    t0 = time.time()
+    while time.time() - t0 < 5:
+        ok, q, dq, kp, kd, tau = sniffer.latest()
+        if ok:
+            print("kp :", kp.round(2))
+            print("kd :", kd.round(2))
+            print("q  :", q.round(4))
+            print("dq :", dq.round(4))
+            print("tau:", tau.round(3))
+            break
+        time.sleep(0.01)
+    else:
+        print("No LowCmd messages seen on rt/lowcmd in 5s (check topic/type/interface).")

additional_functions/snifflonger.py

@@ -0,0 +1,32 @@
+# snifflonger.py
+import csv, time, sys
+from sniff import LowCmdSniffer  # uses latest() -> ok, q, dq, kp, kd, tau
+
+IFACE = sys.argv[1] if len(sys.argv) > 1 else "en7"
+DT    = float(sys.argv[2]) if len(sys.argv) > 2 else 0.01  # seconds
+
+N = 35
+
+def header(prefix):
+    return [f"{prefix}{i}" for i in range(N)]
+
+def main():
+    sniffer = LowCmdSniffer(iface=IFACE)
+
+    with open("lowcmd_log.csv", "w", newline="") as f:
+        w = csv.writer(f)
+        w.writerow(["t"] + header("kp") + header("kd") + header("q") + header("dq") + header("tau"))
+        try:
+            while True:
+                ok, q, dq, kp, kd, tau = sniffer.latest()
+                if ok:
+                    t = time.time()
+                    # ensure lists (in case numpy arrays)
+                    w.writerow([t, *kp.tolist(), *kd.tolist(), *q.tolist(), *dq.tolist(), *tau.tolist()])
+                    f.flush()
+                time.sleep(DT)
+        except KeyboardInterrupt:
+            pass
+
+if __name__ == "__main__":
+    main()

additional_functions/test.py

@@ -0,0 +1,36 @@
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1, G1_29_JointIndex
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+
+from lerobot.datasets.lerobot_dataset import LeRobotDataset
+import time
+config = UnitreeG1Config(
+    motion_mode=False,
+    simulation_mode=False
+)
+
+robot = UnitreeG1(config)
+#print(robot.get_observation())
+#robot.calibrate()
+#print(robot.get_observation())
+while True:
+   observation = robot.get_observation()
+   robot.send_action(observation)
+   #print(robot.get_observation())
+   time.sleep(0.01)
+# print(observation)
+# time.sleep(0.1)
+
+# dataset = LeRobotDataset(repo_id='nepyope/unitree_box_push_30fps_actions_fix', root="", episodes=[0])
+
+# actions = dataset.hf_dataset.select_columns("action")
+# print(actions)
+# episode_idx = 0
+# episode_info = dataset.meta.episodes[episode_idx]
+
+# from_idx = episode_info["dataset_from_index"]
+# to_idx = episode_info["dataset_to_index"]
+
+
+# for idx in range(from_idx, to_idx):
+#             robot.send_action(actions[idx]["action"].numpy()[:14])
+#             time.sleep(1.0 / 30)

additional_functions/test_damp_zero_torque.py

@@ -0,0 +1,167 @@
+#!/usr/bin/env python3
+"""
+Test script to switch between damp and zero torque mode every 2 seconds.
+This script demonstrates switching between the safest robot modes.
+"""
+
+import time
+import sys
+import signal
+
+# Add the unitree_sdk2_python path to sys.path
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+
+
+class DampZeroTorqueTester:
+    def __init__(self):
+        # Initialize locomotion client directly (like the working examples)
+        self.loco_client = LocoClient()
+        self.loco_client.SetTimeout(10.0)  # Increased timeout to allow robot to connect
+        self.loco_client.Init()
+        self.running = True
+
+        # Set up signal handler for graceful shutdown
+        signal.signal(signal.SIGINT, self.signal_handler)
+
+    def signal_handler(self, sig, frame):
+        """Handle Ctrl+C gracefully."""
+        print("\nReceived interrupt signal. Shutting down...")
+        self.running = False
+
+    def switch_to_damp(self):
+        """Switch robot to damping mode."""
+        try:
+            print("[Command] Switching to DAMP mode (motors off)...")
+            result = self.loco_client.Damp()
+            print(f"[Result] Damp command result: {result}")
+            return True
+        except Exception as e:
+            print(f"[Error] Failed to switch to damp mode: {e}")
+            return False
+
+    def switch_to_zero_torque(self):
+        """Switch robot to zero torque mode."""
+        try:
+            print("[Command] Switching to ZERO TORQUE mode (motors on, no torque)...")
+            result = self.loco_client.ZeroTorque()
+            print(f"[Result] ZeroTorque command result: {result}")
+            return True
+        except Exception as e:
+            print(f"[Error] Failed to switch to zero torque mode: {e}")
+            return False
+
+    def run_test(self):
+        """Run exactly 2 cycles: damp → zero_torque → damp → zero_torque."""
+        print("=" * 60)
+        print("G1 Robot Damp/Zero Torque Mode Switcher")
+        print("=" * 60)
+        print("Running exactly 2 cycles:")
+        print("   Cycle 1: DAMP → ZERO TORQUE")
+        print("   Cycle 2: ZERO TORQUE → DAMP → ZERO TORQUE")
+        print("   Final mode: ZERO TORQUE")
+        print("\nWARNING: Ensure robot is in a safe position!")
+        print("Press Ctrl+C to stop safely")
+        print("=" * 60)
+
+        # Wait for user confirmation
+        input("Press Enter to start the test...")
+
+        start_time = time.time()
+
+        # Initial setup - start in damp mode
+        print(f"\nStarting test...")
+        if not self.switch_to_damp():
+            print("Failed to initialize damp mode. Aborting.")
+            return
+
+        # Cycle 1: DAMP → ZERO TORQUE
+        print(f"\nCycle 1: DAMP → ZERO TORQUE")
+        print("Waiting 2 seconds...")
+        time.sleep(2)
+
+        if not self.running:
+            return
+
+        if not self.switch_to_zero_torque():
+            print("Failed to switch to zero torque in cycle 1.")
+            return
+
+        # Cycle 2: ZERO TORQUE → DAMP → ZERO TORQUE
+    
+        print(f"\nCycle 2: ZERO TORQUE → DAMP → ZERO TORQUE")
+        print("Waiting 2 seconds...")
+        time.sleep(2)
+
+        if not self.running:
+            return
+
+        if not self.switch_to_damp():
+            print("Failed to switch to damp in cycle 2.")
+            return
+
+        print("Waiting 2 seconds...")
+        time.sleep(2)
+
+        if not self.running:
+            return
+
+        if not self.switch_to_zero_torque():
+            print("Failed to switch to zero torque in cycle 2.")
+            return
+
+        # Test completed - robot ends in zero torque mode
+        print(f"\nTest completed after 2 cycles")
+        print("Final mode: ZERO TORQUE")
+
+        print("\nTest finished!")
+        print(f"Total cycles: 2")
+        print(f"Total time: {time.time() - start_time:.1f} seconds")
+
+
+def main():
+    print("G1 Robot Damp/Zero Torque Mode Switcher")
+    print("This script will run exactly 2 cycles ending in zero torque mode.")
+
+    # Get network interface from command line or use default
+    if len(sys.argv) > 1:
+        network_interface = sys.argv[1]
+        print(f"Using network interface: {network_interface}")
+    else:
+        network_interface = "en7"  # Default based on your setup
+        print(f"Using default network interface: {network_interface}")
+
+    print("\nInitializing connection...")
+
+    tester = None
+    try:
+        # Initialize the channel factory
+        ChannelFactoryInitialize(0, network_interface)
+        print("Channel factory initialized")
+        
+        # Wait for DDS to discover robot (CRITICAL!)
+        print("Waiting for DDS discovery (3 seconds)...")
+        time.sleep(3)
+        print("Discovery complete")
+
+        # Create tester
+        tester = DampZeroTorqueTester()
+        print("LocoClient created and ready")
+
+        # Run the test
+        tester.run_test()
+        del tester #calls close()
+
+    except Exception as e:
+        print(f"Error during initialization: {e}")
+        print("\nTroubleshooting:")
+        print("   - Make sure you're connected to the robot's network")
+        print("   - Try: sudo ifconfig en7 192.168.123.100 netmask 255.255.255.0")
+        print("   - Verify robot is powered on and accessible")
+        print("   - Run check_robot_connection.py for detailed diagnostics")
+
+
+if __name__ == "__main__":
+    main()

additional_functions/test_dummy_actions.py

@@ -0,0 +1,263 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Test script with dummy actions to isolate robot communication issues.
+Sends constant 0.5 values for all actions without reading teleoperator.
+
+Example:
+
+```shell
+python test_dummy_actions.py \
+    --robot.type=unitree_g1 \
+    --teleop.type=custom \
+    --fps=60 \
+    --duration=10.0
+```
+
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pprint import pformat
+
+from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig  # noqa: F401
+from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig  # noqa: F401
+from lerobot.cameras.zmq.configuration_zmq import ZMQCameraConfig  # noqa: F401
+from lerobot.configs import parser
+from lerobot.processor import (
+    make_default_processors,
+)
+from lerobot.robots import (  # noqa: F401
+    Robot,
+    RobotConfig,
+    bi_so100_follower,
+    hope_jr,
+    koch_follower,
+    make_robot_from_config,
+    so100_follower,
+    so101_follower,
+    unitree_g1,
+)
+from lerobot.teleoperators import (  # noqa: F401
+    Teleoperator,
+    TeleoperatorConfig,
+    bi_so100_leader,
+    gamepad,
+    homunculus,
+    koch_leader,
+    make_teleoperator_from_config,
+    so100_leader,
+    so101_leader,
+    custom
+)
+from lerobot.utils.import_utils import register_third_party_devices
+from lerobot.utils.robot_utils import busy_wait
+from lerobot.utils.utils import init_logging, move_cursor_up
+
+
+@dataclass
+class TestDummyConfig:
+    teleop: TeleoperatorConfig
+    robot: RobotConfig
+    # Limit the maximum frames per second.
+    fps: int = 60
+    # Test duration in seconds
+    duration: float = 10.0
+
+
+def test_dummy_loop(
+    teleop: Teleoperator,
+    robot: Robot,
+    fps: int,
+    duration: float,
+):
+    """
+    Test loop that sends dummy actions (all 0.5) to the robot.
+
+    Args:
+        teleop: The teleoperator device instance (used only to get action features).
+        robot: The robot instance.
+        fps: The target frequency for the control loop in frames per second.
+        duration: The duration of the test in seconds.
+    """
+
+    teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
+
+    # Create dummy action with all values set to 0.5
+    dummy_action = {key: 0.5 for key in teleop.action_features.keys()}
+    
+    print(f"\nDummy action (all 0.5):")
+    for key in list(dummy_action.keys())[:5]:
+        print(f"  {key}: {dummy_action[key]}")
+    if len(dummy_action) > 5:
+        print(f"  ... and {len(dummy_action) - 5} more")
+
+    start = time.perf_counter()
+    iteration = 0
+    
+    # Timing accumulators
+    total_obs_time = 0
+    total_process_time = 0
+    total_robot_send_time = 0
+    
+    min_obs_time = float('inf')
+    max_obs_time = 0
+    min_process_time = float('inf')
+    max_process_time = 0
+    min_robot_send_time = float('inf')
+    max_robot_send_time = 0
+    
+    print(f"\nStarting dummy action test for {duration}s at {fps} FPS target...")
+    print(f"Robot: {robot.__class__.__name__}")
+    print(f"Sending constant actions: all values = 0.5")
+    print("-" * 80)
+
+    while True:
+        loop_start = time.perf_counter()
+
+        # 1. Get robot observation
+        t1 = time.perf_counter()
+        obs = robot.get_observation()
+        obs_time = time.perf_counter() - t1
+        
+        # 2. Use dummy action (no teleop read)
+        raw_action = dummy_action
+        
+        # 3. Process action through pipeline
+        t3 = time.perf_counter()
+        teleop_action = teleop_action_processor((raw_action, obs))
+        robot_action_to_send = robot_action_processor((teleop_action, obs))
+        process_time = time.perf_counter() - t3
+        
+        # 4. Send action to robot
+        t5 = time.perf_counter()
+        result = robot.send_action(robot_action_to_send)
+        robot_send_time = time.perf_counter() - t5
+        
+        # Track statistics
+        total_obs_time += obs_time
+        total_process_time += process_time
+        total_robot_send_time += robot_send_time
+        
+        min_obs_time = min(min_obs_time, obs_time)
+        max_obs_time = max(max_obs_time, obs_time)
+        min_process_time = min(min_process_time, process_time)
+        max_process_time = max(max_process_time, process_time)
+        min_robot_send_time = min(min_robot_send_time, robot_send_time)
+        max_robot_send_time = max(max_robot_send_time, robot_send_time)
+        
+        iteration += 1
+
+        # Display timing breakdown
+        total_component_time = obs_time + process_time + robot_send_time
+        
+        print(f"\nIteration: {iteration}")
+        print(f"1. robot.get_observation():     {obs_time * 1e3:>7.2f}ms")
+        print(f"2. action processing:           {process_time * 1e3:>7.2f}ms")
+        print(f"3. robot.send_action():         {robot_send_time * 1e3:>7.2f}ms")
+        print(f"   Total component time:        {total_component_time * 1e3:>7.2f}ms")
+        print(f"   send_action() result: {result}")
+        
+        # Show sent values
+        print(f"\nSent actions:")
+        print(f"  kLeftWristRoll.pos:  {robot_action_to_send.get('kLeftWristRoll.pos', 'N/A')}")
+        print(f"  kRightWristRoll.pos: {robot_action_to_send.get('kRightWristRoll.pos', 'N/A')}")
+        
+        move_cursor_up(11)
+
+        # Wait to maintain target FPS
+        dt_s = time.perf_counter() - loop_start
+        busy_wait(1 / fps - dt_s)
+        
+        loop_time = time.perf_counter() - loop_start
+        
+        # Check if duration reached
+        elapsed = time.perf_counter() - start
+        if elapsed >= duration:
+            break
+
+    # Print final statistics
+    print("\n" + "=" * 80)
+    print("FINAL TIMING BREAKDOWN (DUMMY ACTIONS)")
+    print("=" * 80)
+    print(f"Total iterations: {iteration}")
+    print(f"Total time:       {elapsed:.2f}s")
+    print(f"Actual FPS:       {iteration / elapsed:.2f}")
+    print(f"Target FPS:       {fps}")
+    print("\n" + "-" * 80)
+    print(f"{'Component':<35} {'Avg (ms)':>10} {'Min (ms)':>10} {'Max (ms)':>10} {'% of time':>10}")
+    print("-" * 80)
+    
+    avg_obs = total_obs_time / iteration * 1e3
+    avg_process = total_process_time / iteration * 1e3
+    avg_robot_send = total_robot_send_time / iteration * 1e3
+    
+    total_avg = avg_obs + avg_process + avg_robot_send
+    
+    print(f"{'1. robot.get_observation()':<35} {avg_obs:>10.2f} {min_obs_time*1e3:>10.2f} {max_obs_time*1e3:>10.2f} {avg_obs/total_avg*100:>9.1f}%")
+    print(f"{'2. action processing':<35} {avg_process:>10.2f} {min_process_time*1e3:>10.2f} {max_process_time*1e3:>10.2f} {avg_process/total_avg*100:>9.1f}%")
+    print(f"{'3. robot.send_action()':<35} {avg_robot_send:>10.2f} {min_robot_send_time*1e3:>10.2f} {max_robot_send_time*1e3:>10.2f} {avg_robot_send/total_avg*100:>9.1f}%")
+    print("-" * 80)
+    print(f"{'TOTAL':<35} {total_avg:>10.2f}")
+    print("=" * 80)
+
+
+@parser.wrap()
+def test_dummy(cfg: TestDummyConfig):
+    init_logging()
+    logging.info(pformat(asdict(cfg)))
+
+    print("\nInitializing teleoperator (for action features only)...")
+    teleop = make_teleoperator_from_config(cfg.teleop)
+    
+    print("Initializing robot...")
+    robot = make_robot_from_config(cfg.robot)
+    
+    print("Connecting teleoperator...")
+    teleop.connect()
+    
+    print("Connecting robot...")
+    robot.connect()
+    
+    print(f"\nTeleoperator connected: {teleop.is_connected}")
+    print(f"Robot connected: {robot.is_connected}")
+    print(f"Action features count: {len(teleop.action_features)}")
+
+    try:
+        test_dummy_loop(
+            teleop=teleop,
+            robot=robot,
+            fps=cfg.fps,
+            duration=cfg.duration,
+        )
+    except KeyboardInterrupt:
+        print("\nTest interrupted by user")
+    finally:
+        print("\nDisconnecting...")
+        teleop.disconnect()
+        robot.disconnect()
+
+
+def main():
+    register_third_party_devices()
+    test_dummy()
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_full_pipeline.py

@@ -0,0 +1,275 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Test script to measure each component of the teleoperation pipeline separately.
+
+Example:
+
+```shell
+python test_full_pipeline.py \
+    --robot.type=unitree_g1 \
+    --teleop.type=custom \
+    --fps=60 \
+    --duration=10.0
+```
+
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pprint import pformat
+
+from lerobot.cameras.opencv.configuration_opencv import OpenCVCameraConfig  # noqa: F401
+from lerobot.cameras.realsense.configuration_realsense import RealSenseCameraConfig  # noqa: F401
+from lerobot.cameras.zmq.configuration_zmq import ZMQCameraConfig  # noqa: F401
+from lerobot.configs import parser
+from lerobot.processor import (
+    make_default_processors,
+)
+from lerobot.robots import (  # noqa: F401
+    Robot,
+    RobotConfig,
+    bi_so100_follower,
+    hope_jr,
+    koch_follower,
+    make_robot_from_config,
+    so100_follower,
+    so101_follower,
+    unitree_g1,
+)
+from lerobot.teleoperators import (  # noqa: F401
+    Teleoperator,
+    TeleoperatorConfig,
+    bi_so100_leader,
+    gamepad,
+    homunculus,
+    koch_leader,
+    make_teleoperator_from_config,
+    so100_leader,
+    so101_leader,
+    custom
+)
+from lerobot.utils.import_utils import register_third_party_devices
+from lerobot.utils.robot_utils import busy_wait
+from lerobot.utils.utils import init_logging, move_cursor_up
+
+
+@dataclass
+class TestPipelineConfig:
+    teleop: TeleoperatorConfig
+    robot: RobotConfig
+    # Limit the maximum frames per second.
+    fps: int = 60
+    # Test duration in seconds
+    duration: float = 10.0
+
+
+def test_pipeline_loop(
+    teleop: Teleoperator,
+    robot: Robot,
+    fps: int,
+    duration: float,
+):
+    """
+    Test loop that measures timing of each pipeline component separately.
+
+    Args:
+        teleop: The teleoperator device instance.
+        robot: The robot instance.
+        fps: The target frequency for the control loop in frames per second.
+        duration: The duration of the test in seconds.
+    """
+
+    teleop_action_processor, robot_action_processor, robot_observation_processor = make_default_processors()
+
+    start = time.perf_counter()
+    iteration = 0
+    
+    # Timing accumulators
+    total_obs_time = 0
+    total_teleop_read_time = 0
+    total_teleop_process_time = 0
+    total_robot_process_time = 0
+    total_robot_send_time = 0
+    
+    min_obs_time = float('inf')
+    max_obs_time = 0
+    min_teleop_read_time = float('inf')
+    max_teleop_read_time = 0
+    min_teleop_process_time = float('inf')
+    max_teleop_process_time = 0
+    min_robot_process_time = float('inf')
+    max_robot_process_time = 0
+    min_robot_send_time = float('inf')
+    max_robot_send_time = 0
+    
+    print(f"\nStarting full pipeline test for {duration}s at {fps} FPS target...")
+    print(f"Robot: {robot.__class__.__name__}")
+    print(f"Teleoperator: {teleop.__class__.__name__}")
+    print("-" * 80)
+
+    while True:
+        loop_start = time.perf_counter()
+
+        # 1. Get robot observation
+        t1 = time.perf_counter()
+        obs = robot.get_observation()
+        obs_time = time.perf_counter() - t1
+        
+        # 2. Get teleop action
+        t2 = time.perf_counter()
+        raw_action = teleop.get_action()
+        teleop_read_time = time.perf_counter() - t2
+        
+        # 3. Process teleop action
+        t3 = time.perf_counter()
+        teleop_action = teleop_action_processor((raw_action, obs))
+        teleop_process_time = time.perf_counter() - t3
+        
+        # 4. Process action for robot
+        t4 = time.perf_counter()
+        robot_action_to_send = robot_action_processor((teleop_action, obs))
+        robot_process_time = time.perf_counter() - t4
+        
+        # 5. Send action to robot
+        t5 = time.perf_counter()
+        _ = robot.send_action(robot_action_to_send)
+        robot_send_time = time.perf_counter() - t5
+        
+        # Track statistics
+        total_obs_time += obs_time
+        total_teleop_read_time += teleop_read_time
+        total_teleop_process_time += teleop_process_time
+        total_robot_process_time += robot_process_time
+        total_robot_send_time += robot_send_time
+        
+        min_obs_time = min(min_obs_time, obs_time)
+        max_obs_time = max(max_obs_time, obs_time)
+        min_teleop_read_time = min(min_teleop_read_time, teleop_read_time)
+        max_teleop_read_time = max(max_teleop_read_time, teleop_read_time)
+        min_teleop_process_time = min(min_teleop_process_time, teleop_process_time)
+        max_teleop_process_time = max(max_teleop_process_time, teleop_process_time)
+        min_robot_process_time = min(min_robot_process_time, robot_process_time)
+        max_robot_process_time = max(max_robot_process_time, robot_process_time)
+        min_robot_send_time = min(min_robot_send_time, robot_send_time)
+        max_robot_send_time = max(max_robot_send_time, robot_send_time)
+        
+        iteration += 1
+
+        # Display timing breakdown
+        total_component_time = obs_time + teleop_read_time + teleop_process_time + robot_process_time + robot_send_time
+        
+        print(f"\nIteration: {iteration}")
+        print(f"1. robot.get_observation():     {obs_time * 1e3:>7.2f}ms")
+        print(f"2. teleop.get_action():         {teleop_read_time * 1e3:>7.2f}ms")
+        print(f"3. teleop_action_processor:     {teleop_process_time * 1e3:>7.2f}ms")
+        print(f"4. robot_action_processor:      {robot_process_time * 1e3:>7.2f}ms")
+        print(f"5. robot.send_action():         {robot_send_time * 1e3:>7.2f}ms")
+        print(f"   Total component time:        {total_component_time * 1e3:>7.2f}ms")
+        
+        # Show wrist roll values
+        print(f"\nkLeftWristRoll.pos:  {robot_action_to_send.get('kLeftWristRoll.pos', 'N/A')}")
+        print(f"kRightWristRoll.pos: {robot_action_to_send.get('kRightWristRoll.pos', 'N/A')}")
+        
+        move_cursor_up(11)
+
+        # Wait to maintain target FPS
+        dt_s = time.perf_counter() - loop_start
+        busy_wait(1 / fps - dt_s)
+        
+        loop_time = time.perf_counter() - loop_start
+        
+        # Check if duration reached
+        elapsed = time.perf_counter() - start
+        if elapsed >= duration:
+            break
+
+    # Print final statistics
+    print("\n" + "=" * 80)
+    print("FINAL TIMING BREAKDOWN")
+    print("=" * 80)
+    print(f"Total iterations: {iteration}")
+    print(f"Total time:       {elapsed:.2f}s")
+    print(f"Actual FPS:       {iteration / elapsed:.2f}")
+    print(f"Target FPS:       {fps}")
+    print("\n" + "-" * 80)
+    print(f"{'Component':<35} {'Avg (ms)':>10} {'Min (ms)':>10} {'Max (ms)':>10} {'% of time':>10}")
+    print("-" * 80)
+    
+    avg_obs = total_obs_time / iteration * 1e3
+    avg_teleop_read = total_teleop_read_time / iteration * 1e3
+    avg_teleop_proc = total_teleop_process_time / iteration * 1e3
+    avg_robot_proc = total_robot_process_time / iteration * 1e3
+    avg_robot_send = total_robot_send_time / iteration * 1e3
+    
+    total_avg = avg_obs + avg_teleop_read + avg_teleop_proc + avg_robot_proc + avg_robot_send
+    
+    print(f"{'1. robot.get_observation()':<35} {avg_obs:>10.2f} {min_obs_time*1e3:>10.2f} {max_obs_time*1e3:>10.2f} {avg_obs/total_avg*100:>9.1f}%")
+    print(f"{'2. teleop.get_action()':<35} {avg_teleop_read:>10.2f} {min_teleop_read_time*1e3:>10.2f} {max_teleop_read_time*1e3:>10.2f} {avg_teleop_read/total_avg*100:>9.1f}%")
+    print(f"{'3. teleop_action_processor':<35} {avg_teleop_proc:>10.2f} {min_teleop_process_time*1e3:>10.2f} {max_teleop_process_time*1e3:>10.2f} {avg_teleop_proc/total_avg*100:>9.1f}%")
+    print(f"{'4. robot_action_processor':<35} {avg_robot_proc:>10.2f} {min_robot_process_time*1e3:>10.2f} {max_robot_process_time*1e3:>10.2f} {avg_robot_proc/total_avg*100:>9.1f}%")
+    print(f"{'5. robot.send_action()':<35} {avg_robot_send:>10.2f} {min_robot_send_time*1e3:>10.2f} {max_robot_send_time*1e3:>10.2f} {avg_robot_send/total_avg*100:>9.1f}%")
+    print("-" * 80)
+    print(f"{'TOTAL':<35} {total_avg:>10.2f}")
+    print("=" * 80)
+
+
+@parser.wrap()
+def test_pipeline(cfg: TestPipelineConfig):
+    init_logging()
+    logging.info(pformat(asdict(cfg)))
+
+    print("\nInitializing teleoperator...")
+    teleop = make_teleoperator_from_config(cfg.teleop)
+    
+    print("Initializing robot...")
+    robot = make_robot_from_config(cfg.robot)
+    
+    print("Connecting teleoperator...")
+    teleop.connect()
+    
+    print("Connecting robot...")
+    robot.connect()
+    
+    print(f"\nTeleoperator connected: {teleop.is_connected}")
+    print(f"Teleoperator calibrated: {teleop.is_calibrated}")
+    print(f"Robot connected: {robot.is_connected}")
+
+    try:
+        test_pipeline_loop(
+            teleop=teleop,
+            robot=robot,
+            fps=cfg.fps,
+            duration=cfg.duration,
+        )
+    except KeyboardInterrupt:
+        print("\nTest interrupted by user")
+    finally:
+        print("\nDisconnecting...")
+        teleop.disconnect()
+        robot.disconnect()
+
+
+def main():
+    register_third_party_devices()
+    test_pipeline()
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_full_robot_state.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python3
+"""
+Test script for get_full_robot_state() function.
+Displays IMU data and motor states from the G1 robot.
+
+Usage:
+    python test_full_robot_state.py
+"""
+
+import time
+import logging
+import numpy as np
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+
+# Set up logging
+logging.basicConfig(
+    level=logging.INFO,
+    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
+)
+
+
+def print_robot_state(state: dict):
+    """Print robot state in a readable format"""
+    
+    print("\n" + "=" * 70)
+    print("G1 FULL ROBOT STATE")
+    print("=" * 70)
+    
+    # IMU Data
+    imu = state['imu']
+    print(f"\n🧭 IMU:")
+    print(f"  Orientation (deg): Roll={np.degrees(imu['rpy'][0]):+.1f}°, "
+          f"Pitch={np.degrees(imu['rpy'][1]):+.1f}°, Yaw={np.degrees(imu['rpy'][2]):+.1f}°")
+    print(f"  Gyroscope (rad/s): x={imu['gyroscope'][0]:+.3f}, "
+          f"y={imu['gyroscope'][1]:+.3f}, z={imu['gyroscope'][2]:+.3f}")
+    print(f"  Accel (m/s²):      x={imu['accelerometer'][0]:+.3f}, "
+          f"y={imu['accelerometer'][1]:+.3f}, z={imu['accelerometer'][2]:+.3f}")
+    print(f"  Quaternion:        w={imu['quaternion'][0]:.3f}, "
+          f"x={imu['quaternion'][1]:+.3f}, y={imu['quaternion'][2]:+.3f}, z={imu['quaternion'][3]:+.3f}")
+    print(f"  Temperature:       {imu['temperature']}°C")
+    
+    # Motor Data - show first 5 motors
+    motors = state['motors']
+    print(f"\n🦾 Motors (showing first 5 of {len(motors)}):")
+    for motor in motors[:5]:
+        print(f"  Motor {motor['id']:2d}: pos={motor['q']:+.3f} rad, "
+              f"vel={motor['dq']:+.3f} rad/s, "
+              f"torque={motor['tau_est']:+.2f} Nm, "
+              f"temp={motor['temperature']}°C")
+    
+    # Motor Statistics
+    print(f"\n📊 Motor Statistics (all {len(motors)} motors):")
+    temps = [m['temperature'] for m in motors]
+    torques = [abs(m['tau_est']) for m in motors]
+    velocities = [abs(m['dq']) for m in motors]
+    
+    print(f"  Temperature: min={min(temps)}°C, max={max(temps)}°C, avg={sum(temps)/len(temps):.1f}°C")
+    print(f"  Torque:      min={min(torques):.2f}Nm, max={max(torques):.2f}Nm, avg={sum(torques)/len(torques):.2f}Nm")
+    print(f"  Velocity:    min={min(velocities):.3f}rad/s, max={max(velocities):.3f}rad/s")
+    
+    # High torque warning
+    high_torque = [(m['id'], m['tau_est']) for m in motors if abs(m['tau_est']) > 5.0]
+    if high_torque:
+        print(f"\n⚠️  Motors with high torque (>5.0 Nm):")
+        for motor_id, torque in high_torque[:10]:  # Show up to 10
+            print(f"     Motor {motor_id:2d}: {torque:+.2f} Nm")
+    
+    print("\nPress Ctrl+C to stop")
+
+
+def main():
+    print("="*70)
+    print("G1 Robot Full State Test")
+    print("="*70)
+    
+    # Create robot config
+    config = UnitreeG1Config(
+        cameras={},  # No cameras needed for state test
+        motion_mode=False,
+        simulation_mode=False,
+    )
+    
+    # Initialize robot
+    print("\nInitializing robot...")
+    robot = UnitreeG1(config)
+    
+    # Give it a moment to start receiving data
+    time.sleep(1)
+    
+    try:
+        print("\nReading robot state (updating every 0.5 seconds)...")
+        print("This demonstrates the get_full_robot_state() function")
+        
+        while True:
+            # Get full robot state
+            state = robot.get_full_robot_state()
+            
+            # Print it
+            print_robot_state(state)
+            
+            # Wait before next update
+            time.sleep(0.5)
+            
+    except KeyboardInterrupt:
+        print("\n\nStopped by user")
+    
+    except Exception as e:
+        print(f"\n✗ Error: {e}")
+        import traceback
+        traceback.print_exc()
+    
+    finally:
+        print("\nDisconnecting robot...")
+        robot.disconnect()
+        print("Done!")
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_led_control.py

@@ -0,0 +1,87 @@
+#!/usr/bin/env python3
+"""
+Test script for G1 robot LED control using the new audio_control function.
+
+Usage:
+    python test_led_control.py
+"""
+
+import time
+import logging
+from lerobot.robots.unitree_g1.unitree_g1 import UnitreeG1
+from lerobot.robots.unitree_g1.config_unitree_g1 import UnitreeG1Config
+
+# Set up logging
+logging.basicConfig(
+    level=logging.INFO,
+    format='%(asctime)s - %(name)s - %(levelname)s - %(message)s'
+)
+
+def main():
+    print("="*60)
+    print("G1 Robot LED Control Test")
+    print("="*60)
+    
+    # Create robot config
+    config = UnitreeG1Config(
+        cameras={},  # No cameras needed for LED test
+        motion_mode=False,
+        simulation_mode=False,
+    )
+    
+    # Initialize robot
+    print("\n[1/4] Initializing robot...")
+    robot = UnitreeG1(config)
+    
+    try:
+        # Test LED: Red
+        print("\n[2/4] Setting LED to RED (255, 0, 0)...")
+        robot.audio_control((255, 0, 0))
+        time.sleep(2)
+        
+        # Test LED: Green
+        print("\n[3/4] Setting LED to GREEN (0, 255, 0)...")
+        robot.audio_control((0, 255, 0))
+        time.sleep(2)
+        
+        # Test LED: Blue
+        print("\n[4/4] Setting LED to BLUE (0, 0, 255)...")
+        robot.audio_control((0, 0, 255))
+        time.sleep(2)
+        
+        # Test TTS
+        print("\n[Bonus 1] Testing text-to-speech...")
+        robot.audio_control("LED test complete!", volume=80)
+        time.sleep(3)
+        
+        # Test WAV playback
+        print("\n[Bonus 2] Testing WAV file playback...")
+        wav_file = "out.wav"
+        import os
+        if os.path.exists(wav_file):
+            robot.audio_control(wav_file, volume=80)
+            print(f"Playing {wav_file}... (waiting for playback to complete)")
+            # Wait a bit for audio to play (adjust based on your file length)
+            time.sleep(5)
+        else:
+            print(f"⚠ Warning: {wav_file} not found, skipping WAV playback test")
+            print(f"  To test WAV playback, place a 16kHz mono 16-bit WAV file named 'out.wav' in this directory")
+        
+        print("\n" + "="*60)
+        print("✓ LED control test completed successfully!")
+        print("="*60)
+        
+    except Exception as e:
+        print(f"\n✗ Error during test: {e}")
+        import traceback
+        traceback.print_exc()
+    
+    finally:
+        print("\nDisconnecting robot...")
+        robot.disconnect()
+        print("Done!")
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_mode_sequence.py

@@ -0,0 +1,286 @@
+#!/usr/bin/env python3
+"""
+Test script to transition through robot modes: zero torque → damp → start → balance stand.
+This script demonstrates a sequence of robot mode transitions.
+"""
+
+import time
+import sys
+import signal
+
+# Add the unitree_sdk2_python path to sys.path
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+
+
+class ModeSequenceTester:
+    def __init__(self):
+        # Initialize motion switcher client first
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+        
+        # Initialize locomotion client
+        self.loco_client = LocoClient()
+        self.loco_client.SetTimeout(10.0)
+        self.loco_client.Init()
+        self.running = True
+        
+        # Set up signal handler for graceful shutdown
+        signal.signal(signal.SIGINT, self.signal_handler)
+    
+    def signal_handler(self, sig, frame):
+        """Handle Ctrl+C gracefully."""
+        print("\nReceived interrupt signal. Shutting down...")
+        self.running = False
+        # Just exit immediately
+        exit(0)
+    
+    def reset_robot_state(self):
+        """Reset robot to a clean state before starting sequence."""
+        try:
+            print("Resetting robot to clean state...")
+            
+            # Check current mode first
+            try:
+                status, result = self.msc.CheckMode()
+                if status == 0:
+                    print(f"📊 Current mode: {result.get('name', 'unknown')}")
+                    # Release current mode if any
+                    if result.get('name'):
+                        print(" Releasing current mode...")
+                        self.msc.ReleaseMode()
+                        time.sleep(2.0)
+            except Exception as e:
+                print(f" Could not check/release mode: {e}")
+            
+            # Force reset through locomotion
+            print(" Setting to damp mode...")
+            self.loco_client.Damp()
+            time.sleep(2.0)
+            
+            print(" Setting to zero torque...")
+            self.loco_client.ZeroTorque()
+            time.sleep(2.0)
+            
+            print(" Back to damp mode...")
+            self.loco_client.Damp()
+            time.sleep(2.0)
+            
+            print(" Robot state reset complete")
+        except Exception as e:
+            print(f" Warning: Could not fully reset robot state: {e}")
+            # Try minimal reset
+            try:
+                print(" Attempting minimal reset...")
+                self.loco_client.Damp()
+                time.sleep(1.0)
+                print(" Minimal reset complete")
+            except Exception as e2:
+                print(f" Even minimal reset failed: {e2}")
+    
+    def cleanup_and_reset(self):
+        """Clean up and reset robot after sequence."""
+        try:
+            print(" Cleaning up and resetting robot...")
+            
+            # Release any held modes (but don't force damp mode)
+            try:
+                print(" Releasing any held modes...")
+                self.msc.ReleaseMode()
+                time.sleep(1.0)
+            except Exception as e:
+                print(f" Could not release mode: {e}")
+            
+            print(" Cleanup complete - robot ready for next commands")
+        except Exception as e:
+            print(f" Warning: Could not clean up robot state: {e}")
+            print(" Cleanup attempted - robot may still be ready")
+    
+    def switch_to_zero_torque(self):
+        """Switch robot to zero torque mode."""
+        try:
+            print(" Switching to ZERO TORQUE mode (FSM ID: 0)...")
+            self.loco_client.ZeroTorque()
+            return True
+        except Exception as e:
+            print(f" Error switching to zero torque mode: {e}")
+            return False
+    
+    def switch_to_damp(self):
+        """Switch robot to damping mode."""
+        try:
+            print(" Switching to DAMP mode (FSM ID: 1)...")
+            self.loco_client.Damp()
+            return True
+        except Exception as e:
+            print(f" Error switching to damp mode: {e}")
+            return False
+    
+    def switch_to_start(self):
+        """Switch robot to start mode."""
+        try:
+            print(" Switching to START mode (FSM ID: 200)...")
+            self.loco_client.Start()
+            return True
+        except Exception as e:
+            print(f" Error switching to start mode: {e}")
+            return False
+    
+    def switch_to_high_stand(self):
+        """Switch robot to high stand mode."""
+        try:
+            print(" Switching to HIGH STAND mode (max height)...")
+            self.loco_client.HighStand()
+            return True
+        except Exception as e:
+            print(f" Error switching to high stand mode: {e}")
+            return False
+    
+    
+    def switch_to_low_stand(self):
+        """Switch robot to low stand mode."""
+        try:
+            print(" Switching to LOW STAND mode...")
+            self.loco_client.LowStand()
+            return True
+        except Exception as e:
+            print(f" Error switching to low stand mode: {e}")
+            return False
+    
+    def switch_to_squat2standup(self):
+        """Switch robot to squat2standup mode."""
+        try:
+            print(" Switching to SQUAT2STANDUP mode (FSM ID: 706)...")
+            self.loco_client.Squat2StandUp()
+            return True
+        except Exception as e:
+            print(f" Error switching to squat2standup mode: {e}")
+            return False
+    
+    def switch_to_lie2standup(self):
+        """Switch robot to lie2standup mode."""
+        try:
+            print(" Switching to LIE2STANDUP mode (FSM ID: 702)...")
+            self.loco_client.Lie2StandUp()
+            return True
+        except Exception as e:
+            print(f" Error switching to lie2standup mode: {e}")
+            return False
+    
+    def switch_to_wave_hand(self, turn_flag=False):
+        """Switch robot to wave hand mode."""
+        try:
+            print(f" Switching to WAVE HAND mode (turn_flag: {turn_flag})...")
+            self.loco_client.WaveHand(turn_flag)
+            return True
+        except Exception as e:
+            print(f" Error switching to wave hand mode: {e}")
+            return False
+    
+    def switch_to_shake_hand(self, stage=-1):
+        """Switch robot to shake hand mode."""
+        try:
+            print(f" Switching to SHAKE HAND mode (stage: {stage})...")
+            self.loco_client.ShakeHand(stage)
+            return True
+        except Exception as e:
+            print(f" Error switching to shake hand mode: {e}")
+            return False
+    
+    def interactive_control(self):
+        """Interactive keyboard control for robot modes."""
+        print("=" * 70)
+        print("G1 Robot Interactive Control")
+        print("=" * 70)
+        print("Commands:")
+        print("   0 = Zero Torque (FSM ID: 0)")
+        print("   1 = Damp (FSM ID: 1)")
+        print("   2 = Start (FSM ID: 200)")
+        print("   3 = High Stand")
+        print("   4 = Low Stand")
+        print("   5 = Squat2StandUp")
+        print("   6 = Lie2StandUp")
+        print("   7 = Wave Hand")
+        print("   8 = Shake Hand")
+        print("   q = Quit")
+        print("\nWARNING: Ensure robot is in a safe position!")
+        print("=" * 70)
+        
+        while self.running:
+            try:
+                command = input("\nEnter command (0-8 or q): ").strip()
+                
+                if command == 'q':
+                    print("Quitting...")
+                    break
+                elif command == '0':
+                    self.switch_to_zero_torque()
+                elif command == '1':
+                    self.switch_to_damp()
+                elif command == '2':
+                    self.switch_to_start()
+                elif command == '3':
+                    self.switch_to_high_stand()
+                elif command == '4':
+                    self.switch_to_low_stand()
+                elif command == '5':
+                    self.switch_to_squat2standup()
+                elif command == '6':
+                    self.switch_to_lie2standup()
+                elif command == '7':
+                    self.switch_to_wave_hand()
+                elif command == '8':
+                    self.switch_to_shake_hand()
+                else:
+                    print("Invalid command. Try 0-8 or q.")
+                    
+            except KeyboardInterrupt:
+                print("\nReceived interrupt signal. Shutting down...")
+                break
+            except Exception as e:
+                print(f"Error: {e}")
+        
+        print("Interactive control finished.")
+
+
+def main():
+    print("G1 Robot Interactive Control")
+    print("This script provides keyboard control for robot modes")
+    
+    # Get network interface from command line or use default
+    if len(sys.argv) > 1:
+        network_interface = sys.argv[1]
+        print(f"Using network interface: {network_interface}")
+    else:
+        network_interface = "en7"  # Default based on your setup
+        print(f"Using default network interface: {network_interface}")
+    
+    print("\nInitializing connection...")
+    
+    try:
+        # Initialize the channel factory
+        ChannelFactoryInitialize(0, network_interface)
+        print(" Channel factory initialized")
+        
+        # Create tester
+        tester = ModeSequenceTester()
+        print(" LocoClient created")
+        
+        # Run interactive control
+        tester.interactive_control()
+        
+    except Exception as e:
+        print(f" Error during initialization: {e}")
+        print("\n🔧 Troubleshooting:")
+        print("   - Make sure you're connected to the robot's network")
+        print("   - Try: sudo ifconfig en7 192.168.123.100 netmask 255.255.255.0")
+        print("   - Verify robot is powered on and accessible")
+        print("   - Check if robot is in 'ai' mode")
+
+
+if __name__ == "__main__":
+    main()

additional_functions/test_robot_mode.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python3
+"""
+Test script to check G1 robot's current mode and print it to screen.
+This script uses MotionSwitcherClient to check the robot's control mode.
+"""
+
+import time
+import sys
+import json
+
+# Add the unitree_sdk2_python path to sys.path
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient
+
+
+class RobotModeChecker:
+    def __init__(self):
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+    
+    def check_mode(self):
+        """Check the robot's current mode and return the result."""
+        try:
+            status, result = self.msc.CheckMode()
+            return status, result
+        except Exception as e:
+            print(f"Error checking mode: {e}")
+            return -1, None
+    
+    def print_mode_info(self):
+        """Print detailed mode information to screen."""
+        print("=" * 50)
+        print("G1 Robot Mode Checker")
+        print("=" * 50)
+        
+        print("\nChecking robot mode...")
+        status, result = self.check_mode()
+        
+        if status == 0 and result is not None:
+            print("✅ Successfully connected to robot!")
+            print(f"📊 Status Code: {status}")
+            print(f"📋 Mode Result: {json.dumps(result, indent=2)}")
+            
+            # Extract mode name if available
+            if isinstance(result, dict) and 'name' in result:
+                mode_name = result['name']
+                if mode_name:
+                    print(f"🤖 Current Mode: {mode_name}")
+                    
+                    # Provide mode interpretation
+                    mode_interpretations = {
+                        'ai': 'AI/Autonomous Mode - Ready for external commands',
+                        'normal': 'Normal Mode - Basic operation mode',
+                        'advanced': 'Advanced Mode - Advanced control mode',
+                        'ai-w': 'AI-Wheeled Mode - For wheeled robots',
+                        '': 'Safe Mode - No active control mode'
+                    }
+                    
+                    interpretation = mode_interpretations.get(mode_name, 'Unknown Mode')
+                    print(f"💡 Interpretation: {interpretation}")
+                else:
+                    print("🤖 Current Mode: Safe Mode (No active mode)")
+                    print("💡 Interpretation: Robot is in safe state, ready to accept commands")
+            else:
+                print("🤖 Current Mode: Unknown (check result structure)")
+                
+        elif status == -1:
+            print("❌ Failed to connect to robot")
+            print("🔧 Troubleshooting:")
+            print("   - Check network connection")
+            print("   - Verify robot is powered on")
+            print("   - Ensure correct network interface (try en7)")
+            print("   - Check if robot IP is 192.168.123.164")
+        else:
+            print(f"❌ Error checking mode. Status: {status}")
+            if result:
+                print(f"📋 Result: {result}")
+        
+        print("\n" + "=" * 50)
+
+
+def main():
+    print("G1 Robot Mode Checker")
+    print("This script will check the robot's current control mode.")
+    
+    # Get network interface from command line or use default
+    if len(sys.argv) > 1:
+        network_interface = sys.argv[1]
+        print(f"Using network interface: {network_interface}")
+    else:
+        network_interface = "en7"  # Default based on your setup
+        print(f"Using default network interface: {network_interface}")
+    
+    print("\nInitializing connection...")
+    
+    try:
+        # Initialize the channel factory
+        ChannelFactoryInitialize(0, network_interface)
+        print("✅ Channel factory initialized")
+        
+        # Create mode checker
+        checker = RobotModeChecker()
+        print("✅ MotionSwitcherClient created")
+        
+        # Check and print mode
+        checker.print_mode_info()
+        
+        # Optionally check mode multiple times
+        print("\n🔄 Checking mode again in 2 seconds...")
+        time.sleep(2)
+        checker.print_mode_info()
+        
+    except Exception as e:
+        print(f"❌ Error during initialization: {e}")
+        print("\n🔧 Troubleshooting:")
+        print("   - Make sure you're connected to the robot's network")
+        print("   - Try: sudo ifconfig en7 192.168.123.100 netmask 255.255.255.0")
+        print("   - Verify robot is powered on and accessible")
+
+
+if __name__ == "__main__":
+    main()

additional_functions/test_sensors.py

@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+"""
+Quick G1 sensor test script.
+Displays IMU, motor states, and system info.
+
+Usage:
+    python test_sensors.py <network_interface>
+    Example: python test_sensors.py en7
+"""
+
+import sys
+import time
+import numpy as np
+from unitree_sdk2py.core.channel import ChannelSubscriber, ChannelFactoryInitialize
+from unitree_sdk2py.idl.unitree_hg.msg.dds_ import LowState_
+
+
+def print_sensor_data(msg: LowState_):
+    """Print sensor data in a readable format"""
+    
+    print("\n" + "=" * 70)
+    print(f"G1 SENSOR DATA @ tick {msg.tick}")
+    print("=" * 70)
+    
+    # IMU
+    imu = msg.imu_state
+    print(f"\n🧭 IMU:")
+    print(f"  Orientation (deg): Roll={np.degrees(imu.rpy[0]):+.1f}°, "
+          f"Pitch={np.degrees(imu.rpy[1]):+.1f}°, Yaw={np.degrees(imu.rpy[2]):+.1f}°")
+    print(f"  Gyroscope (rad/s): x={imu.gyroscope[0]:+.3f}, "
+          f"y={imu.gyroscope[1]:+.3f}, z={imu.gyroscope[2]:+.3f}")
+    print(f"  Accel (m/s²):      x={imu.accelerometer[0]:+.3f}, "
+          f"y={imu.accelerometer[1]:+.3f}, z={imu.accelerometer[2]:+.3f}")
+    print(f"  Quaternion:        w={imu.quaternion[0]:.3f}, "
+          f"x={imu.quaternion[1]:+.3f}, y={imu.quaternion[2]:+.3f}, z={imu.quaternion[3]:+.3f}")
+    print(f"  Temperature:       {imu.temperature}°C")
+    
+    # Motors - show first 5 and summary
+    print(f"\n🦾 Motors (showing first 5 of 35):")
+    for i in range(min(5, 35)):
+        motor = msg.motor_state[i]
+        print(f"  Motor {i:2d}: pos={motor.q:+.3f} rad, "
+              f"vel={motor.dq:+.3f} rad/s, "
+              f"torque={motor.tau_est:+.2f} Nm, "
+              f"temp={motor.temperature[0]}°C")
+    
+    # Motor statistics
+    print(f"\n📊 Motor Statistics (all 35):")
+    temps = [msg.motor_state[i].temperature[0] for i in range(35)]
+    torques = [abs(msg.motor_state[i].tau_est) for i in range(35)]
+    velocities = [abs(msg.motor_state[i].dq) for i in range(35)]
+    
+    print(f"  Temperature: min={min(temps)}°C, max={max(temps)}°C, avg={sum(temps)/len(temps):.1f}°C")
+    print(f"  Torque:      min={min(torques):.2f}Nm, max={max(torques):.2f}Nm, avg={sum(torques)/len(torques):.2f}Nm")
+    print(f"  Velocity:    min={min(velocities):.3f}rad/s, max={max(velocities):.3f}rad/s")
+    
+    # High torque warning
+    high_torque = [(i, msg.motor_state[i].tau_est) for i in range(35) 
+                   if abs(msg.motor_state[i].tau_est) > 5.0]
+    if high_torque:
+        print(f"\n⚠️  Motors with high torque (>5.0 Nm):")
+        for motor_id, torque in high_torque[:10]:  # Show up to 10
+            print(f"     Motor {motor_id:2d}: {torque:+.2f} Nm")
+    
+    # System
+    print(f"\n⚙️  System Info:")
+    print(f"  Mode: machine={msg.mode_machine}, pr={msg.mode_pr}")
+    print(f"  Version: {msg.version[0]}.{msg.version[1]}")
+    
+    print("\nPress Ctrl+C to stop")
+
+
+def main():
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} <network_interface>")
+        print(f"Example: python3 {sys.argv[0]} en7")
+        sys.exit(1)
+    
+    network_interface = sys.argv[1]
+    
+    print(f"Initializing DDS on {network_interface}...")
+    ChannelFactoryInitialize(0, network_interface)
+    
+    print("Subscribing to rt/lowstate...")
+    lowstate_sub = ChannelSubscriber("rt/lowstate", LowState_)
+    lowstate_sub.Init(print_sensor_data, 10)
+    
+    print("Receiving sensor data...")
+    print("(Data will print each time it's received)")
+    
+    try:
+        while True:
+            time.sleep(1)  # Keep alive, handler prints on each message
+    except KeyboardInterrupt:
+        print("\n\nStopped")
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_speaker.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+"""
+Simple test script to use the G1 robot's speaker.
+Demonstrates TTS (text-to-speech) and volume control.
+
+Usage:
+    python test_speaker.py en7
+    (replace 'en7' with your network interface)
+"""
+
+import sys
+import time
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient
+
+
+def main():
+    if len(sys.argv) < 2:
+        print(f"Usage: python3 {sys.argv[0]} <network_interface>")
+        print("Example: python3 test_speaker.py en7")
+        sys.exit(1)
+
+    # Initialize DDS communication
+    network_interface = sys.argv[1]
+    print(f"Initializing communication on {network_interface}...")
+    ChannelFactoryInitialize(0, network_interface)
+
+    # Create audio client
+    audio_client = AudioClient()
+    audio_client.SetTimeout(10.0)
+    audio_client.Init()
+    print("Audio client initialized!")
+
+    # Get current volume
+    code, volume_data = audio_client.GetVolume()
+    if code == 0:
+        print(f"Current volume: {volume_data}")
+    
+    # Set volume to 80%
+    print("Setting volume to 80%...")
+    audio_client.SetVolume(100)
+    time.sleep(0.5)
+
+    # # Test English TTS
+    # print("Speaking (English)...")
+    audio_client.LedControl(255, 0, 0)
+    audio_client.TtsMaker("Hello!", 0)
+    exit()
+    # time.sleep(5)
+
+    # # Test Chinese TTS
+    # print("Speaking (Chinese)...")
+    # audio_client.TtsMaker("大家好！我是宇树科技人形机器人。", 0)
+    # time.sleep(5)
+
+    # # Test LED control (bonus!)
+    # print("Testing LED strip...")
+    # audio_client.TtsMaker("Now testing LED lights.", 0)
+    # time.sleep(2)
+    
+    print("LED: Red")
+      # Red
+    time.sleep(1)
+    
+    print("LED: Green")
+    #audio_client.LedControl(0, 255, 0)  # Green
+    time.sleep(1)
+    
+    print("LED: Blue")
+    #audio_client.LedControl(0, 0, 255)  # Blue
+    time.sleep(1)
+
+
+    print("Done!")
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_speaker_wav.py

@@ -0,0 +1,199 @@
+#!/usr/bin/env python3
+"""
+Play a WAV file through the G1 robot's speaker.
+
+Requirements:
+    - WAV file must be 16kHz, mono, 16-bit PCM
+    
+Usage:
+    python test_speaker_wav.py en7 path/to/audio.wav
+"""
+
+import sys
+import time
+import struct
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.audio.g1_audio_client import AudioClient
+
+
+def read_wav(filename):
+    """Read WAV file and return PCM data."""
+    try:
+        with open(filename, 'rb') as f:
+            def read(fmt):
+                return struct.unpack(fmt, f.read(struct.calcsize(fmt)))
+
+            # Read RIFF header
+            chunk_id, = read('<I')
+            if chunk_id != 0x46464952:  # "RIFF"
+                print(f"[ERROR] Not a valid WAV file (invalid RIFF header)")
+                return [], -1, -1, False
+
+            _chunk_size, = read('<I')
+            format_tag, = read('<I')
+            if format_tag != 0x45564157:  # "WAVE"
+                print(f"[ERROR] Not a valid WAV file (invalid WAVE format)")
+                return [], -1, -1, False
+
+            # Read fmt chunk
+            subchunk1_id, = read('<I')
+            subchunk1_size, = read('<I')
+
+            # Skip JUNK chunk if present
+            if subchunk1_id == 0x4B4E554A:  # "JUNK"
+                f.seek(subchunk1_size, 1)
+                subchunk1_id, = read('<I')
+                subchunk1_size, = read('<I')
+
+            if subchunk1_id != 0x20746D66:  # "fmt "
+                print(f"[ERROR] Invalid fmt chunk")
+                return [], -1, -1, False
+
+            if subchunk1_size not in [16, 18]:
+                print(f"[ERROR] Unsupported fmt chunk size: {subchunk1_size}")
+                return [], -1, -1, False
+
+            audio_format, = read('<H')
+            if audio_format != 1:
+                print(f"[ERROR] Only PCM format supported, got format {audio_format}")
+                return [], -1, -1, False
+
+            num_channels, = read('<H')
+            sample_rate, = read('<I')
+            _byte_rate, = read('<I')
+            _block_align, = read('<H')
+            bits_per_sample, = read('<H')
+
+            if bits_per_sample != 16:
+                print(f"[ERROR] Only 16-bit samples supported, got {bits_per_sample}-bit")
+                return [], -1, -1, False
+
+            if subchunk1_size == 18:
+                extra_size, = read('<H')
+                if extra_size != 0:
+                    f.seek(extra_size, 1)
+
+            # Find data chunk
+            while True:
+                subchunk2_id, subchunk2_size = read('<II')
+                if subchunk2_id == 0x61746164:  # "data"
+                    break
+                f.seek(subchunk2_size, 1)
+
+            # Read PCM data
+            raw_pcm = f.read(subchunk2_size)
+            if len(raw_pcm) != subchunk2_size:
+                print("[ERROR] Failed to read full PCM data")
+                return [], -1, -1, False
+
+            return list(raw_pcm), sample_rate, num_channels, True
+
+    except Exception as e:
+        print(f"[ERROR] Failed to read WAV file: {e}")
+        return [], -1, -1, False
+
+
+def play_pcm_stream(client, pcm_list, app_name="example", chunk_size=96000):
+    """
+    Play PCM audio in chunks.
+    
+    Args:
+        client: AudioClient instance
+        pcm_list: List of PCM bytes
+        app_name: Application name for this audio stream
+        chunk_size: Bytes per chunk (96000 = ~3 seconds at 16kHz)
+    """
+    pcm_data = bytes(pcm_list)
+    stream_id = str(int(time.time() * 1000))
+    offset = 0
+    chunk_index = 0
+    total_size = len(pcm_data)
+
+    print(f"Playing audio: {total_size} bytes in {(total_size // chunk_size) + 1} chunks")
+
+    while offset < total_size:
+        remaining = total_size - offset
+        current_chunk_size = min(chunk_size, remaining)
+        chunk = pcm_data[offset:offset + current_chunk_size]
+
+        # Send chunk
+        ret_code, _ = client.PlayStream(app_name, stream_id, chunk)
+        if ret_code != 0:
+            print(f"[ERROR] Failed to send chunk {chunk_index}, return code: {ret_code}")
+            break
+        else:
+            print(f"[INFO] Sent chunk {chunk_index}/{(total_size // chunk_size)}")
+
+        offset += current_chunk_size
+        chunk_index += 1
+        time.sleep(1.0)  # Wait between chunks
+
+
+def main():
+    if len(sys.argv) < 3:
+        print(f"Usage: python3 {sys.argv[0]} <network_interface> <wav_file>")
+        print("Example: python3 test_speaker_wav.py en7 audio.wav")
+        print("\nWAV file requirements:")
+        print("  - Sample rate: 16000 Hz")
+        print("  - Channels: 1 (mono)")
+        print("  - Bit depth: 16-bit")
+        sys.exit(1)
+
+    network_interface = sys.argv[1]
+    wav_path = sys.argv[2]
+
+    # Initialize communication
+    print(f"Initializing on {network_interface}...")
+    ChannelFactoryInitialize(0)
+
+    # Create audio client
+    audio_client = AudioClient()
+    audio_client.SetTimeout(10.0)
+    audio_client.Init()
+    print("Audio client initialized!")
+
+    # Read WAV file
+    print(f"Reading WAV file: {wav_path}")
+    pcm_list, sample_rate, num_channels, is_ok = read_wav(wav_path)
+    
+    if not is_ok:
+        print("[ERROR] Failed to read WAV file")
+        sys.exit(1)
+
+    print(f"WAV info:")
+    print(f"  - Sample rate: {sample_rate} Hz")
+    print(f"  - Channels: {num_channels}")
+    print(f"  - Size: {len(pcm_list)} bytes")
+
+    # Verify format
+    if sample_rate != 16000:
+        print(f"[ERROR] Sample rate must be 16000 Hz, got {sample_rate} Hz")
+        print("Use ffmpeg to convert:")
+        print(f"  ffmpeg -i input.wav -ar 16000 -ac 1 -sample_fmt s16 output.wav")
+        sys.exit(1)
+
+    if num_channels != 1:
+        print(f"[ERROR] Must be mono (1 channel), got {num_channels} channels")
+        print("Use ffmpeg to convert:")
+        print(f"  ffmpeg -i input.wav -ar 16000 -ac 1 -sample_fmt s16 output.wav")
+        sys.exit(1)
+
+    # Play audio
+    print("Playing audio...")
+    play_pcm_stream(audio_client, pcm_list, "test_wav")
+    
+    # Wait for playback to finish
+    duration_seconds = len(pcm_list) / (16000 * 2)  # 16kHz, 16-bit (2 bytes)
+    print(f"Waiting {duration_seconds:.1f} seconds for playback...")
+    time.sleep(duration_seconds + 1)
+
+    # Stop playback
+    print("Stopping playback...")
+    audio_client.PlayStop("test_wav")
+    
+    print("Done!")
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_teleop_only.py

@@ -0,0 +1,187 @@
+#!/usr/bin/env python
+
+# Copyright 2025 The HuggingFace Inc. team. All rights reserved.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+"""
+Test script to measure teleoperator read performance in isolation.
+This script ONLY reads from the teleoperator without initializing or communicating with a robot.
+
+Example:
+
+```shell
+python test_teleop_only.py \
+    --teleop.type=custom \
+    --fps=60 \
+    --duration=10.0
+```
+
+"""
+
+import logging
+import time
+from dataclasses import asdict, dataclass
+from pprint import pformat
+
+from lerobot.configs import parser
+from lerobot.teleoperators import (  # noqa: F401
+    Teleoperator,
+    TeleoperatorConfig,
+    bi_so100_leader,
+    gamepad,
+    homunculus,
+    koch_leader,
+    make_teleoperator_from_config,
+    so100_leader,
+    so101_leader,
+    custom
+)
+from lerobot.utils.import_utils import register_third_party_devices
+from lerobot.utils.robot_utils import busy_wait
+from lerobot.utils.utils import init_logging, move_cursor_up
+
+
+@dataclass
+class TestTeleopConfig:
+    teleop: TeleoperatorConfig
+    # Limit the maximum frames per second.
+    fps: int = 60
+    # Test duration in seconds
+    duration: float = 10.0
+    # Display detailed timing stats
+    display_stats: bool = True
+
+
+def test_teleop_loop(
+    teleop: Teleoperator,
+    fps: int,
+    duration: float,
+    display_stats: bool = True,
+):
+    """
+    Test loop that only reads from teleoperator and measures performance.
+
+    Args:
+        teleop: The teleoperator device instance providing control actions.
+        fps: The target frequency for the control loop in frames per second.
+        duration: The duration of the test in seconds.
+        display_stats: If True, displays detailed timing statistics.
+    """
+
+    start = time.perf_counter()
+    iteration = 0
+    total_read_time = 0
+    min_read_time = float('inf')
+    max_read_time = 0
+    
+    print(f"\nStarting teleoperator read test for {duration}s at {fps} FPS target...")
+    print(f"Teleoperator: {teleop.__class__.__name__}")
+    print(f"Action features: {list(teleop.action_features.keys())}")
+    print(f"Number of actions: {len(teleop.action_features)}")
+    print("-" * 80)
+
+    while True:
+        loop_start = time.perf_counter()
+
+        # Measure just the get_action call
+        read_start = time.perf_counter()
+        action = teleop.get_action()
+        read_time = time.perf_counter() - read_start
+        
+        # Track statistics
+        total_read_time += read_time
+        min_read_time = min(min_read_time, read_time)
+        max_read_time = max(max_read_time, read_time)
+        iteration += 1
+
+        if display_stats:
+            # Display timing information
+            avg_read_time = total_read_time / iteration
+            print(f"\nIteration: {iteration}")
+            print(f"Read time:     {read_time * 1e3:>7.2f}ms")
+            print(f"Avg read time: {avg_read_time * 1e3:>7.2f}ms")
+            print(f"Min read time: {min_read_time * 1e3:>7.2f}ms")
+            print(f"Max read time: {max_read_time * 1e3:>7.2f}ms")
+            
+            # Show only the wrist roll actions
+            print("\nWrist Roll Actions:")
+            print(f"  kLeftWristRoll.pos:  {action.get('kLeftWristRoll.pos', 'N/A'):.4f}")
+            print(f"  kRightWristRoll.pos: {action.get('kRightWristRoll.pos', 'N/A'):.4f}")
+            
+            move_cursor_up(9)
+
+        # Wait to maintain target FPS
+        dt_s = time.perf_counter() - loop_start
+        busy_wait(1 / fps - dt_s)
+        
+        loop_time = time.perf_counter() - loop_start
+        actual_fps = 1 / loop_time if loop_time > 0 else 0
+        
+        # Check if duration reached
+        elapsed = time.perf_counter() - start
+        if elapsed >= duration:
+            break
+
+    # Print final statistics
+    print("\n" + "=" * 80)
+    print("FINAL STATISTICS")
+    print("=" * 80)
+    print(f"Total iterations: {iteration}")
+    print(f"Total time:       {elapsed:.2f}s")
+    print(f"Actual FPS:       {iteration / elapsed:.2f}")
+    print(f"Target FPS:       {fps}")
+    print(f"\nget_action() timing:")
+    print(f"  Average:        {total_read_time / iteration * 1e3:.2f}ms")
+    print(f"  Min:            {min_read_time * 1e3:.2f}ms")
+    print(f"  Max:            {max_read_time * 1e3:.2f}ms")
+    print(f"  Total:          {total_read_time:.2f}s ({total_read_time/elapsed*100:.1f}% of loop time)")
+    print("=" * 80)
+
+
+@parser.wrap()
+def test_teleop(cfg: TestTeleopConfig):
+    init_logging()
+    logging.info(pformat(asdict(cfg)))
+
+    print("\nInitializing teleoperator...")
+    teleop = make_teleoperator_from_config(cfg.teleop)
+    
+    print("Connecting teleoperator...")
+    teleop.connect()
+    
+    print(f"Connected: {teleop.is_connected}")
+    print(f"Calibrated: {teleop.is_calibrated}")
+
+    try:
+        test_teleop_loop(
+            teleop=teleop,
+            fps=cfg.fps,
+            duration=cfg.duration,
+            display_stats=cfg.display_stats,
+        )
+    except KeyboardInterrupt:
+        print("\nTest interrupted by user")
+    finally:
+        print("\nDisconnecting teleoperator...")
+        teleop.disconnect()
+
+
+def main():
+    register_third_party_devices()
+    test_teleop()
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/test_zmq_camera.py

@@ -0,0 +1,298 @@
+#!/usr/bin/env python3
+"""
+Comprehensive test script for ZMQ camera integration.
+Tests all camera functionalities: find_cameras, connect, read, async_read, disconnect.
+"""
+
+import time
+import numpy as np
+from pathlib import Path
+
+from lerobot.cameras.zmq import ZMQCamera, ZMQCameraConfig
+from lerobot.cameras.configs import ColorMode
+
+
+def test_find_cameras():
+    """Test 1: Camera Discovery"""
+    print("\n" + "="*60)
+    print("TEST 1: Camera Discovery (find_cameras)")
+    print("="*60)
+    
+    cameras = ZMQCamera.find_cameras()
+    
+    if not cameras:
+        print("❌ No ZMQ cameras found!")
+        print("   Make sure you have configured cameras in ~/.lerobot/zmq_cameras.json")
+        print("   or set LEROBOT_ZMQ_CAMERAS environment variable")
+        return None
+    
+    print(f"✅ Found {len(cameras)} ZMQ camera(s):")
+    for i, cam in enumerate(cameras):
+        print(f"\n  Camera {i}:")
+        for key, value in cam.items():
+            if key == "default_stream_profile":
+                print(f"    {key}:")
+                for sub_key, sub_value in value.items():
+                    print(f"      {sub_key}: {sub_value}")
+            else:
+                print(f"    {key}: {value}")
+    
+    return cameras[0] if cameras else None
+
+
+def test_connect_disconnect(cam_info):
+    """Test 2: Connect and Disconnect"""
+    print("\n" + "="*60)
+    print("TEST 2: Connect and Disconnect")
+    print("="*60)
+    
+    config = ZMQCameraConfig(
+        server_address=cam_info["server_address"],
+        port=cam_info["port"],
+        camera_name=cam_info["camera_name"],
+        color_mode=ColorMode.RGB,
+    )
+    
+    camera = ZMQCamera(config)
+    
+    # Test is_connected before connection
+    print(f"Before connect - is_connected: {camera.is_connected}")
+    assert not camera.is_connected, "❌ Camera should not be connected initially"
+    
+    # Test connect
+    print("Connecting to camera...")
+    start = time.time()
+    camera.connect(warmup=True)
+    connect_time = time.time() - start
+    print(f"✅ Connected in {connect_time:.2f}s")
+    print(f"   Camera resolution: {camera.width}x{camera.height}")
+    assert camera.is_connected, "❌ Camera should be connected after connect()"
+    
+    # Test disconnect
+    print("Disconnecting camera...")
+    camera.disconnect()
+    print("✅ Disconnected")
+    assert not camera.is_connected, "❌ Camera should not be connected after disconnect()"
+    
+    return config
+
+
+def test_synchronous_read(config):
+    """Test 3: Synchronous Read"""
+    print("\n" + "="*60)
+    print("TEST 3: Synchronous Read (read)")
+    print("="*60)
+    
+    camera = ZMQCamera(config)
+    camera.connect(warmup=False)
+    
+    print("Reading 10 frames synchronously...")
+    read_times = []
+    
+    for i in range(10):
+        start = time.time()
+        frame = camera.read()
+        read_time = (time.time() - start) * 1000  # ms
+        read_times.append(read_time)
+        
+        # Validate frame
+        assert isinstance(frame, np.ndarray), "❌ Frame should be numpy array"
+        assert frame.ndim == 3, "❌ Frame should be 3D (H, W, C)"
+        assert frame.shape[2] == 3, "❌ Frame should have 3 channels"
+        
+        if i == 0:
+            print(f"   Frame shape: {frame.shape}")
+            print(f"   Frame dtype: {frame.dtype}")
+            print(f"   Frame range: [{frame.min()}, {frame.max()}]")
+    
+    avg_time = np.mean(read_times)
+    std_time = np.std(read_times)
+    fps = 1000 / avg_time if avg_time > 0 else 0
+    
+    print(f"✅ Read 10 frames successfully")
+    print(f"   Average read time: {avg_time:.2f} ± {std_time:.2f} ms")
+    print(f"   Estimated FPS: {fps:.1f}")
+    
+    camera.disconnect()
+
+
+def test_color_mode_conversion(config):
+    """Test 4: Color Mode Conversion"""
+    print("\n" + "="*60)
+    print("TEST 4: Color Mode Conversion")
+    print("="*60)
+    
+    # Test RGB mode
+    config_rgb = ZMQCameraConfig(
+        server_address=config.server_address,
+        port=config.port,
+        camera_name=config.camera_name,
+        color_mode=ColorMode.RGB,
+    )
+    camera_rgb = ZMQCamera(config_rgb)
+    camera_rgb.connect(warmup=False)
+    frame_rgb = camera_rgb.read()
+    camera_rgb.disconnect()
+    
+    # Test BGR mode
+    config_bgr = ZMQCameraConfig(
+        server_address=config.server_address,
+        port=config.port,
+        camera_name=config.camera_name,
+        color_mode=ColorMode.BGR,
+    )
+    camera_bgr = ZMQCamera(config_bgr)
+    camera_bgr.connect(warmup=False)
+    frame_bgr = camera_bgr.read()
+    camera_bgr.disconnect()
+    
+    # Frames should be different (color channels swapped)
+    assert not np.array_equal(frame_rgb, frame_bgr), "❌ RGB and BGR frames should differ"
+    # But shapes should be the same
+    assert frame_rgb.shape == frame_bgr.shape, "❌ RGB and BGR frames should have same shape"
+    
+    print("✅ RGB mode works correctly")
+    print("✅ BGR mode works correctly")
+    print(f"   Frame shapes match: {frame_rgb.shape}")
+
+
+def test_asynchronous_read(config):
+    """Test 5: Asynchronous Read"""
+    print("\n" + "="*60)
+    print("TEST 5: Asynchronous Read (async_read)")
+    print("="*60)
+    
+    camera = ZMQCamera(config)
+    camera.connect(warmup=False)
+    
+    print("Reading 10 frames asynchronously...")
+    read_times = []
+    
+    for i in range(10):
+        start = time.time()
+        frame = camera.async_read(timeout_ms=1000)
+        read_time = (time.time() - start) * 1000  # ms
+        read_times.append(read_time)
+        
+        # Validate frame
+        assert isinstance(frame, np.ndarray), "❌ Frame should be numpy array"
+        assert frame.ndim == 3, "❌ Frame should be 3D (H, W, C)"
+        
+        if i == 0:
+            print(f"   Frame shape: {frame.shape}")
+            print(f"   Background thread alive: {camera.thread.is_alive()}")
+    
+    avg_time = np.mean(read_times)
+    std_time = np.std(read_times)
+    fps = 1000 / avg_time if avg_time > 0 else 0
+    
+    print(f"✅ Read 10 frames asynchronously")
+    print(f"   Average read time: {avg_time:.2f} ± {std_time:.2f} ms")
+    print(f"   Estimated FPS: {fps:.1f}")
+    
+    camera.disconnect()
+
+
+def test_reconnection(config):
+    """Test 6: Reconnection"""
+    print("\n" + "="*60)
+    print("TEST 6: Reconnection")
+    print("="*60)
+    
+    camera = ZMQCamera(config)
+    
+    # Connect, read, disconnect cycle 1
+    print("Connection cycle 1...")
+    camera.connect(warmup=False)
+    frame1 = camera.read()
+    camera.disconnect()
+    
+    # Wait a bit
+    time.sleep(0.5)
+    
+    # Connect, read, disconnect cycle 2
+    print("Connection cycle 2...")
+    camera.connect(warmup=False)
+    frame2 = camera.read()
+    camera.disconnect()
+    
+    # Frames should be valid
+    assert frame1.shape == frame2.shape, "❌ Frames should have consistent shape"
+    
+    print("✅ Reconnection works correctly")
+    print(f"   Both frames have shape: {frame1.shape}")
+
+
+def test_timeout_handling(config):
+    """Test 7: Timeout Handling"""
+    print("\n" + "="*60)
+    print("TEST 7: Timeout Handling")
+    print("="*60)
+    
+    # Create config with very short timeout
+    config_short = ZMQCameraConfig(
+        server_address=config.server_address,
+        port=config.port,
+        camera_name=config.camera_name,
+        timeout_ms=50,  # Very short timeout
+    )
+    
+    camera = ZMQCamera(config_short)
+    camera.connect(warmup=False)
+    
+    try:
+        # This should work if server is fast enough
+        frame = camera.read()
+        print(f"✅ Read succeeded even with {config_short.timeout_ms}ms timeout")
+    except TimeoutError:
+        print(f"⚠️  Timeout occurred (expected with {config_short.timeout_ms}ms timeout)")
+    
+    camera.disconnect()
+
+
+def main():
+    """Run all tests"""
+    print("\n" + "="*60)
+    print("ZMQ CAMERA COMPREHENSIVE TEST SUITE")
+    print("="*60)
+    
+    try:
+        # Test 1: Discovery
+        cam_info = test_find_cameras()
+        if not cam_info:
+            print("\n❌ Cannot proceed without configured cameras")
+            return
+        
+        # Test 2: Connect/Disconnect
+        config = test_connect_disconnect(cam_info)
+        
+        # Test 3: Synchronous Read
+        test_synchronous_read(config)
+        
+        # Test 4: Color Mode
+        test_color_mode_conversion(config)
+        
+        # Test 5: Asynchronous Read
+        test_asynchronous_read(config)
+        
+        # Test 6: Reconnection
+        test_reconnection(config)
+        
+        # Test 7: Timeout
+        test_timeout_handling(config)
+        
+        # Summary
+        print("\n" + "="*60)
+        print("✅ ALL TESTS PASSED!")
+        print("="*60)
+        print("\nZMQ Camera integration is working correctly! 🎉")
+        
+    except Exception as e:
+        print(f"\n❌ TEST FAILED: {e}")
+        import traceback
+        traceback.print_exc()
+
+
+if __name__ == "__main__":
+    main()
+

additional_functions/view_camera.py

@@ -0,0 +1,144 @@
+"""
+Bigger display version of camera viewer.
+Use --scale to control how big the camera windows are.
+Example:
+    python unitree_lerobot/eval_robot/view_camera.py --scale 1.5
+"""
+
+import time
+import cv2
+import argparse
+import numpy as np
+from unitree_lerobot.eval_robot.make_robot import setup_image_client
+
+import logging_mp
+
+logging_mp.basic_config(level=logging_mp.INFO)
+logger_mp = logging_mp.get_logger(__name__)
+
+
+def resize_for_display(image: np.ndarray, scale: float) -> np.ndarray:
+    """Resize image by a scale factor for larger display."""
+    h, w = image.shape[:2]
+    new_w = int(w * scale)
+    new_h = int(h * scale)
+    return cv2.resize(image, (new_w, new_h))
+
+
+def view_camera_main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("--scale", type=float, default=1.5, help="Resize factor for display windows")
+    args = parser.parse_args()
+
+    # Create minimal config (no robot control needed)
+    cfg = argparse.Namespace(
+        sim=False,      # Real robot (not simulation)
+        arm="G1_29",    # Doesn't matter for camera-only
+        ee="dex3",      # Doesn't matter for camera-only
+        motion=False
+    )
+
+    scale_factor = args.scale
+
+    logger_mp.info("Setting up image client...")
+    logger_mp.info("Make sure image_server.py is running on the robot!")
+    logger_mp.info("Default robot IP: 192.168.123.164:5555")
+
+    image_info = setup_image_client(cfg)
+    tv_img_array = image_info["tv_img_array"]
+    wrist_img_array = image_info["wrist_img_array"]
+    tv_img_shape = image_info["tv_img_shape"]
+    wrist_img_shape = image_info["wrist_img_shape"]
+    is_binocular = image_info["is_binocular"]
+    has_wrist_cam = image_info["has_wrist_cam"]
+
+    logger_mp.info(f"Camera config:")
+    logger_mp.info(f"  - Head camera: {tv_img_shape}, binocular: {is_binocular}")
+    logger_mp.info(f"  - Wrist camera: {wrist_img_shape if has_wrist_cam else 'Not available'}")
+    logger_mp.info("")
+    logger_mp.info("Waiting for first frame...")
+
+    time.sleep(2)
+
+    logger_mp.info("=" * 60)
+    logger_mp.info("CAMERA VIEWER ACTIVE")
+    logger_mp.info("=" * 60)
+    logger_mp.info(f"Scale factor: {scale_factor}")
+    logger_mp.info("Controls:")
+    logger_mp.info("  - Press 'q' to quit")
+    logger_mp.info("  - Press 's' to save current frame as image")
+    logger_mp.info("=" * 60)
+
+    frame_count = 0
+
+    try:
+        while True:
+            head_image = tv_img_array.copy()
+
+            # HEAD CAMERA
+            if head_image is not None and head_image.size > 0:
+                if np.any(head_image):
+                    if is_binocular:
+                        h, w = head_image.shape[:2]
+                        left_head = head_image[:, :w // 2]
+                        right_head = head_image[:, w // 2:]
+
+                        left_display = resize_for_display(left_head, scale_factor)
+                        right_display = resize_for_display(right_head, scale_factor)
+
+                        cv2.imshow('Left Head Camera', left_display)
+                        cv2.imshow('Right Head Camera', right_display)
+                    else:
+                        display_img = resize_for_display(head_image, scale_factor)
+                        cv2.imshow('Head Camera', display_img)
+                else:
+                    logger_mp.warning("Received empty frame from head camera")
+
+            # WRIST CAMERA
+            if has_wrist_cam and wrist_img_array is not None:
+                wrist_image = wrist_img_array.copy()
+                if wrist_image is not None and wrist_image.size > 0 and np.any(wrist_image):
+                    h, w = wrist_image.shape[:2]
+                    left_wrist = wrist_image[:, :w // 2]
+                    right_wrist = wrist_image[:, w // 2:]
+
+                    left_wrist_display = resize_for_display(left_wrist, scale_factor)
+                    right_wrist_display = resize_for_display(right_wrist, scale_factor)
+
+                    cv2.imshow('Left Wrist Camera', left_wrist_display)
+                    cv2.imshow('Right Wrist Camera', right_wrist_display)
+
+            # Keyboard input
+            key = cv2.waitKey(1) & 0xFF
+            if key == ord('q'):
+                logger_mp.info("Quit command received")
+                break
+            elif key == ord('s'):
+                timestamp = time.strftime("%Y%m%d_%H%M%S")
+                cv2.imwrite(f'head_camera_{timestamp}.jpg', head_image)
+                logger_mp.info(f"Saved: head_camera_{timestamp}.jpg")
+                if has_wrist_cam and wrist_img_array is not None:
+                    wrist_image = wrist_img_array.copy()
+                    if wrist_image is not None and wrist_image.size > 0:
+                        cv2.imwrite(f'wrist_camera_{timestamp}.jpg', wrist_image)
+                        logger_mp.info(f"Saved: wrist_camera_{timestamp}.jpg")
+
+            frame_count += 1
+            if frame_count % 30 == 0:
+                logger_mp.info(f"Frames received: {frame_count}")
+
+            time.sleep(0.033)
+
+    except KeyboardInterrupt:
+        logger_mp.info("\nInterrupted by user (Ctrl+C)")
+
+    finally:
+        logger_mp.info("Closing camera viewer...")
+        cv2.destroyAllWindows()
+        #from lerobot.robots.unitree_g1.eval_robot.utils.utils import cleanup_resources
+        #cleanup_resources(image_info)
+        logger_mp.info("Done!")
+
+
+if __name__ == "__main__":
+    view_camera_main()

additional_functions/wwwwdfv.py

@@ -0,0 +1,215 @@
+#!/usr/bin/env python3
+"""
+Test script to switch between damp and zero torque mode every 2 seconds.
+This script demonstrates switching between the safest robot modes.
+"""
+
+import time
+import sys
+import signal
+
+# Add the unitree_sdk2_python path to sys.path
+sys.path.append('/Users/nepyope/Documents/unitree/unitree_IL_lerobot/unitree_sdk2_python')
+
+from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+from unitree_sdk2py.g1.loco.g1_loco_client import LocoClient
+
+
+class DampZeroTorqueTester:
+    def __init__(self):
+        # Initialize motion switcher client first
+        self.msc = MotionSwitcherClient()
+        self.msc.SetTimeout(5.0)
+        self.msc.Init()
+        
+        # Check current robot mode
+        print("Checking robot mode...")
+        status, result = self.msc.CheckMode()
+        if status == 0:
+            print(f"Current robot mode: {result.get('name', 'unknown')}")
+            if result.get('name'):
+                print("Releasing current mode...")
+                self.msc.ReleaseMode()
+                time.sleep(2.0)
+        else:
+            print(f"Could not check robot mode. Status: {status}")
+        
+        # Ensure robot is in 'ai' mode for locomotion commands
+        print("Setting robot to 'ai' mode...")
+        try:
+            self.msc.SelectMode("ai")
+            time.sleep(1.0)
+            print("Robot set to 'ai' mode successfully")
+        except Exception as e:
+            print(f"Warning: Could not set robot to 'ai' mode: {e}")
+            print("Trying to continue anyway...")
+        
+        # Initialize locomotion client
+        self.loco_client = LocoClient()
+        self.loco_client.SetTimeout(10.0)
+        self.loco_client.Init()
+        self.running = True
+        
+        # Set up signal handler for graceful shutdown
+        signal.signal(signal.SIGINT, self.signal_handler)
+    
+    def signal_handler(self, sig, frame):
+        """Handle Ctrl+C gracefully."""
+        print("\n🛑 Received interrupt signal. Shutting down safely...")
+        self.running = False
+        # Release robot control
+        try:
+            self.msc.ReleaseMode()
+            print("🔓 Robot control released")
+        except:
+            pass
+    
+    def switch_to_damp(self):
+        """Switch robot to damping mode."""
+        try:
+            print("🔒 Switching to DAMP mode (motors off)...")
+            self.loco_client.Damp()
+            return True
+        except Exception as e:
+            print(f"❌ Error switching to damp mode: {e}")
+            return False
+    
+    def switch_to_zero_torque(self):
+        """Switch robot to zero torque mode."""
+        try:
+            print("⚡ Switching to ZERO TORQUE mode (motors on, no torque)...")
+            self.loco_client.ZeroTorque()
+            return True
+        except Exception as e:
+            print(f"❌ Error switching to zero torque mode: {e}")
+            return False
+    
+    def run_test(self):
+        """Run exactly 2 cycles: damp → zero_torque → damp → zero_torque."""
+        print("=" * 60)
+        print("🤖 G1 Robot Damp/Zero Torque Mode Switcher")
+        print("=" * 60)
+        print("🔄 Running exactly 2 cycles:")
+        print("   Cycle 1: DAMP → ZERO TORQUE")
+        print("   Cycle 2: ZERO TORQUE → DAMP → ZERO TORQUE")
+        print("   Final mode: ZERO TORQUE")
+        print("\n⚠️  WARNING: Ensure robot is in a safe position!")
+        print("🛑 Press Ctrl+C to stop safely")
+        print("=" * 60)
+        
+        # Wait for user confirmation
+        input("Press Enter to start the test...")
+        
+        start_time = time.time()
+        
+        # Initial setup - start in damp mode
+        print(f"\n🚀 Starting test...")
+        if not self.switch_to_damp():
+            print("❌ Failed to initialize damp mode. Aborting.")
+            return
+        
+        # Cycle 1: DAMP → ZERO TORQUE
+        print(f"\n📊 Cycle 1: DAMP → ZERO TORQUE")
+        print("⏳ Waiting 2 seconds...")
+        time.sleep(2)
+        
+        if not self.running:
+            return
+            
+        if not self.switch_to_zero_torque():
+            print("❌ Failed to switch to zero torque in cycle 1.")
+            return
+        
+        # Cycle 2: ZERO TORQUE → DAMP → ZERO TORQUE
+        print(f"\n📊 Cycle 2: ZERO TORQUE → DAMP → ZERO TORQUE")
+        print("⏳ Waiting 2 seconds...")
+        time.sleep(2)
+        
+        if not self.running:
+            return
+            
+        if not self.switch_to_damp():
+            print("❌ Failed to switch to damp in cycle 2.")
+            return
+            
+        print("⏳ Waiting 2 seconds...")
+        time.sleep(2)
+        
+        if not self.running:
+            return
+            
+        if not self.switch_to_zero_torque():
+            print("❌ Failed to switch to zero torque in cycle 2.")
+            return
+        
+        # Test completed - robot ends in zero torque mode
+        print(f"\n🏁 Test completed after 2 cycles")
+        print("⚡ Final mode: ZERO TORQUE")
+        
+        # Release robot control to allow other programs to connect
+        print("🔓 Releasing robot control...")
+        try:
+            self.msc.ReleaseMode()
+            time.sleep(1.0)
+            # Release again to ensure complete cleanup
+            self.msc.ReleaseMode()
+            print("✅ Robot control released successfully")
+        except Exception as e:
+            print(f"⚠️ Warning: Could not release robot control: {e}")
+        
+        # Final check - ensure robot is in a free state
+        print("🔍 Final mode check...")
+        try:
+            status, result = self.msc.CheckMode()
+            if status == 0:
+                print(f"Final robot mode: {result.get('name', 'unknown')}")
+                if result.get('name'):
+                    print("🔓 Performing final release...")
+                    self.msc.ReleaseMode()
+            else:
+                print(f"Could not check final mode. Status: {status}")
+        except Exception as e:
+            print(f"Warning: Could not check final mode: {e}")
+        
+        print("\n✅ Test finished!")
+        print(f"📈 Total cycles: 2")
+        print(f"⏱️  Total time: {time.time() - start_time:.1f} seconds")
+
+
+def main():
+    print("G1 Robot Damp/Zero Torque Mode Switcher")
+    print("This script will run exactly 2 cycles ending in zero torque mode.")
+    
+    # Get network interface from command line or use default
+    if len(sys.argv) > 1:
+        network_interface = sys.argv[1]
+        print(f"Using network interface: {network_interface}")
+    else:
+        network_interface = "en7"  # Default based on your setup
+        print(f"Using default network interface: {network_interface}")
+    
+    print("\nInitializing connection...")
+    
+    try:
+        # Initialize the channel factory
+        ChannelFactoryInitialize(0, network_interface)
+        print("✅ Channel factory initialized")
+        
+        # Create tester
+        tester = DampZeroTorqueTester()
+        print("✅ LocoClient created")
+        
+        # Run the test
+        tester.run_test()
+        
+    except Exception as e:
+        print(f"❌ Error during initialization: {e}")
+        print("\n🔧 Troubleshooting:")
+        print("   - Make sure you're connected to the robot's network")
+        print("   - Try: sudo ifconfig en7 192.168.123.100 netmask 255.255.255.0")
+        print("   - Verify robot is powered on and accessible")
+        print("   - Check if robot is in 'ai' mode")
+
+
+if __name__ == "__main__":
+    main()

eval_robot/assets/brainco_hand/brainco.yml

@@ -0,0 +1,61 @@
+left:
+  type: DexPilot # or vector
+  urdf_path: brainco_hand/brainco_left.urdf
+
+  # Target refers to the retargeting target, which is the robot hand
+  target_joint_names: 
+    [
+      "left_thumb_metacarpal_joint",
+      "left_thumb_proximal_joint",
+      "left_index_proximal_joint",
+      "left_middle_proximal_joint",
+      "left_ring_proximal_joint",
+      "left_pinky_proximal_joint",
+    ]
+
+  # for DexPilot type
+  wrist_link_name: "base_link"
+  finger_tip_link_names: [ "left_thumb_tip",  "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ]
+  # If you do not know exactly how it is used, please leave it to None for default.
+  target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24,  0,  0,  0,  0,  0], [ 4,  4,  4,  4,  9, 9,  9, 14, 14, 19,  4,  9, 14, 19, 24]]
+
+  # for vector type
+  target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"]
+  target_task_link_names: [ "left_thumb_tip",  "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ]
+  target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ]
+
+  # Scaling factor for vector retargeting only
+  # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6
+  scaling_factor: 0.90
+  # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+  low_pass_alpha: 0.2
+
+right:
+  type: DexPilot # or vector
+  urdf_path: brainco_hand/brainco_right.urdf
+
+  # Target refers to the retargeting target, which is the robot hand
+  target_joint_names: 
+    [
+      "right_thumb_metacarpal_joint",
+      "right_thumb_proximal_joint",
+      "right_index_proximal_joint",
+      "right_middle_proximal_joint",
+      "right_ring_proximal_joint",
+      "right_pinky_proximal_joint",
+    ]
+  # for DexPilot type
+  wrist_link_name: "base_link"
+  finger_tip_link_names: [ "right_thumb_tip",  "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ]
+  target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24,  0,  0,  0,  0,  0], [ 4,  4,  4,  4,  9, 9,  9, 14, 14, 19,  4,  9, 14, 19, 24]]
+  
+  # for vector type
+  target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"]
+  target_task_link_names: [ "right_thumb_tip",  "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ]
+  target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ]
+
+  # Scaling factor for vector retargeting only
+  # For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6
+  scaling_factor: 0.90
+  # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+  low_pass_alpha: 0.2

eval_robot/assets/g1/.gitignore

@@ -0,0 +1,2 @@
+*.gv
+*.pdf

eval_robot/assets/g1/README.md

@@ -0,0 +1,33 @@
+# Unitree G1 Description (URDF & MJCF)
+
+## Overview
+
+This package includes a universal humanoid robot description (URDF & MJCF) for the [Unitree G1](https://www.unitree.com/g1/), developed by [Unitree Robotics](https://www.unitree.com/).
+
+MJCF/URDF for the G1 robot:
+
+| MJCF/URDF file name           | `mode_machine` | Hip roll reduction ratio | Update status | dof#leg | dof#waist | dof#arm | dof#hand |
+| ----------------------------- | :------------: | :----------------------: | ------------- | :-----: | :-------: | :-----: | :------: |
+| `g1_23dof`                    |       1        |           14.5           | Beta          |   6*2   |     1     |   5*2   |    0     |
+| `g1_29dof`                    |       2        |           14.5           | Beta          |   6*2   |     3     |   7*2   |    0     |
+| `g1_29dof_with_hand`          |       2        |           14.5           | Beta          |   6*2   |     3     |   7*2   |   7*2    |
+| `g1_29dof_lock_waist`         |       3        |           14.5           | Beta          |   6*2   |     1     |   7*2   |    0     |
+| `g1_23dof_rev_1_0`            |       4        |           22.5           | Up-to-date    |   6*2   |     1     |   5*2   |    0     |
+| `g1_29dof_rev_1_0`            |       5        |           22.5           | Up-to-date    |   6*2   |     3     |   7*2   |    0     |
+| `g1_29dof_with_hand_rev_1_0`  |       5        |           22.5           | Up-to-date    |   6*2   |     3     |   7*2   |   7*2    |
+| `g1_29dof_lock_waist_rev_1_0` |       6        |           22.5           | Up-to-date    |   6*2   |     1     |   7*2   |    0     |
+| `g1_dual_arm`                 |       9        |           null           | Up-to-date    |    0    |     0     |   7*2   |    0     |
+
+## Visulization with [MuJoCo](https://github.com/google-deepmind/mujoco)
+
+1. Open MuJoCo Viewer
+
+   ```bash
+   pip install mujoco
+   python -m mujoco.viewer
+   ```
+
+2. Drag and drop the MJCF/URDF model file (`g1_XXX.xml`/`g1_XXX.urdf`) to the MuJoCo Viewer.
+
+## Note for teleoperate
+g1_body29_hand14 is modified from [g1_29dof_with_hand_rev_1_0](https://github.com/unitreerobotics/unitree_ros/blob/master/robots/g1_description/g1_29dof_with_hand_rev_1_0.urdf)