sync recent changes

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)