mirror of
https://github.com/huggingface/lerobot.git
synced 2026-05-31 10:51:35 +00:00
sync recent changes
This commit is contained in:
Martino Russi
52
additional_functions/check_loco_active.py
Normal file
52
additional_functions/check_loco_active.py
Normal file
@@ -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.")
|
||||
92
additional_functions/check_robot_connection.py
Normal file
92
additional_functions/check_robot_connection.py
Normal file
@@ -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)
|
||||
71
additional_functions/example_enhanced_sdk.py
Normal file
71
additional_functions/example_enhanced_sdk.py
Normal file
@@ -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())
|
||||
209
additional_functions/monitor_robot.py
Normal file
209
additional_functions/monitor_robot.py
Normal file
@@ -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()
|
||||
|
||||
212
additional_functions/read_joint_positions.py
Normal file
212
additional_functions/read_joint_positions.py
Normal file
@@ -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()
|
||||
|
||||
58
additional_functions/sniff.py
Normal file
58
additional_functions/sniff.py
Normal file
@@ -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).")
|
||||
32
additional_functions/snifflonger.py
Normal file
32
additional_functions/snifflonger.py
Normal file
@@ -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()
|
||||
36
additional_functions/test.py
Normal file
36
additional_functions/test.py
Normal file
@@ -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)
|
||||
167
additional_functions/test_damp_zero_torque.py
Normal file
167
additional_functions/test_damp_zero_torque.py
Normal file
@@ -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()
|
||||
263
additional_functions/test_dummy_actions.py
Normal file
263
additional_functions/test_dummy_actions.py
Normal file
@@ -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()
|
||||
|
||||
275
additional_functions/test_full_pipeline.py
Normal file
275
additional_functions/test_full_pipeline.py
Normal file
@@ -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()
|
||||
|
||||
121
additional_functions/test_full_robot_state.py
Normal file
121
additional_functions/test_full_robot_state.py
Normal file
@@ -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()
|
||||
|
||||
87
additional_functions/test_led_control.py
Normal file
87
additional_functions/test_led_control.py
Normal file
@@ -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()
|
||||
|
||||
286
additional_functions/test_mode_sequence.py
Normal file
286
additional_functions/test_mode_sequence.py
Normal file
@@ -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()
|
||||
125
additional_functions/test_robot_mode.py
Normal file
125
additional_functions/test_robot_mode.py
Normal file
@@ -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()
|
||||
100
additional_functions/test_sensors.py
Normal file
100
additional_functions/test_sensors.py
Normal file
@@ -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()
|
||||
|
||||
79
additional_functions/test_speaker.py
Normal file
79
additional_functions/test_speaker.py
Normal file
@@ -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()
|
||||
|
||||
199
additional_functions/test_speaker_wav.py
Normal file
199
additional_functions/test_speaker_wav.py
Normal file
@@ -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()
|
||||
|
||||
187
additional_functions/test_teleop_only.py
Normal file
187
additional_functions/test_teleop_only.py
Normal file
@@ -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()
|
||||
|
||||
298
additional_functions/test_zmq_camera.py
Normal file
298
additional_functions/test_zmq_camera.py
Normal file
@@ -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()
|
||||
|
||||
144
additional_functions/view_camera.py
Normal file
144
additional_functions/view_camera.py
Normal file
@@ -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()
|
||||
215
additional_functions/wwwwdfv.py
Normal file
215
additional_functions/wwwwdfv.py
Normal file
@@ -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()
|
||||
61
eval_robot/assets/brainco_hand/brainco.yml
Normal file
61
eval_robot/assets/brainco_hand/brainco.yml
Normal file
@@ -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
|
||||
BIN
eval_robot/assets/brainco_hand/meshes/left_base_link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_base_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_index_distal_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_index_distal_Link.STL
Normal file
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_index_tip_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_index_tip_Link.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_middle_tip_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_middle_tip_Link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_pinky_distal_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_pinky_distal_Link.STL
Normal file
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_pinky_tip_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_pinky_tip_Link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_ring_distal_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_ring_distal_Link.STL
Normal file
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_ring_tip_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_ring_tip_Link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_thumb_distal_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_thumb_distal_Link.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/left_thumb_tip_Link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/left_thumb_tip_Link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_base_link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_base_link.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_index_tip.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_index_tip.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_middle_tip.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_middle_tip.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_pinky_tip.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_pinky_tip.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_ring_distal_link.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_ring_distal_link.STL
Normal file
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_ring_tip.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_ring_tip.STL
Normal file
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
BIN
eval_robot/assets/brainco_hand/meshes/right_thumb_tip.STL
Normal file
BIN
eval_robot/assets/brainco_hand/meshes/right_thumb_tip.STL
Normal file
Binary file not shown.
2
eval_robot/assets/g1/.gitignore
vendored
Normal file
2
eval_robot/assets/g1/.gitignore
vendored
Normal file
@@ -0,0 +1,2 @@
|
||||
*.gv
|
||||
*.pdf
|
||||
33
eval_robot/assets/g1/README.md
Normal file
33
eval_robot/assets/g1/README.md
Normal file
@@ -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)
|
||||
BIN
eval_robot/assets/g1/meshes/head_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/head_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_ankle_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_ankle_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_ankle_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_ankle_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_elbow_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_elbow_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_index_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_index_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_index_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_index_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_middle_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_middle_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_middle_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_middle_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_palm_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_palm_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_2_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hand_thumb_2_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hip_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hip_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hip_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_hip_yaw_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_hip_yaw_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_knee_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_knee_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_rubber_hand.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_rubber_hand.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_shoulder_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_shoulder_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_shoulder_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_shoulder_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_shoulder_yaw_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_shoulder_yaw_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_wrist_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_wrist_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_wrist_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_wrist_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_wrist_roll_rubber_hand.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_wrist_roll_rubber_hand.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/left_wrist_yaw_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/left_wrist_yaw_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/logo_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/logo_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/pelvis.STL
Normal file
BIN
eval_robot/assets/g1/meshes/pelvis.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/pelvis_contour_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/pelvis_contour_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_ankle_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_ankle_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_ankle_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_ankle_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_elbow_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_elbow_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_index_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_index_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_index_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_index_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_middle_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_middle_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_middle_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_middle_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_palm_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_palm_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_0_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_0_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_1_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_1_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_2_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hand_thumb_2_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hip_pitch_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hip_pitch_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hip_roll_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hip_roll_link.STL
Normal file
Binary file not shown.
BIN
eval_robot/assets/g1/meshes/right_hip_yaw_link.STL
Normal file
BIN
eval_robot/assets/g1/meshes/right_hip_yaw_link.STL
Normal file
Binary file not shown.
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user