@@ -1,46 +1,33 @@
import logging
import time
import struct
from functools import cached_property
from typing import Any
from pathlib import Path
from lerobot . cameras . utils import make_cameras_from_configs
import json
from . . robot import Robot
from . config_unitree_g1 import UnitreeG1Config
import numpy as np
import logging
import select
import struct
import sys
import termios
import threading
import time
from enum import IntEnum
import sys
import select
import termios
import tty
from collections import deque
from functools import cached_property
from pathlib import Path
from typing import Any
from typing import Union
import numpy as np
import time
import torch
import onnxruntime as ort
from unitree_sdk2py . idl . unitree_hg . msg . dds_ import LowCmd_ as hg_LowCmd , LowState_ as hg_LowState # idl for g1, h1_2
from unitree_sdk2py . idl . default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py . utils . crc import CRC
from unitree_sdk2py . comm . motion_switcher . motion_switcher_client import (
MotionSwitcherClient ,
)
from lerobot . envs . factory import make_env
from scipy . spatial . transform import Rotation as R
import struct
import torch
from scipy . spatial . transform import Rotation as R
from unitree_sdk2py . idl . default import unitree_hg_msg_dds__LowCmd_
from unitree_sdk2py . idl . unitree_hg . msg . dds_ import (
LowCmd_ as hg_LowCmd ,
LowState_ as hg_LowState ,
) # idl for g1, h1_2
from unitree_sdk2py . utils . crc import CRC
from lerobot . cameras . utils import make_cameras_from_configs
from lerobot . robots . unitree_g1 . g1_utils import G1_29_JointArmIndex , G1_29_JointIndex
from . . robot import Robot
from . config_unitree_g1 import UnitreeG1Config
logger = logging . getLogger ( __name__ )
@@ -76,6 +63,7 @@ class G1_29_LowState:
self . imu_state = IMUState ( )
self . wireless_remote = None # Raw wireless remote data
class DataBuffer :
def __init__ ( self ) :
self . data = None
@@ -89,8 +77,8 @@ class DataBuffer:
with self . lock :
self . data = data
class UnitreeG1 ( Robot ) :
class UnitreeG1 ( Robot ) :
config_class = UnitreeG1Config
name = " unitree_g1 "
@@ -128,8 +116,11 @@ class UnitreeG1(Robot):
self . calibrated = False
from lerobot . robots . unitree_g1 . unitree_sdk2_socket import ChannelPublisher , ChannelSubscriber , ChannelFactoryInitialize # dds
from lerobot . robots . unitree_g1 . unitree_sdk2_socket import (
ChannelFactoryInitialize ,
ChannelPublisher ,
ChannelSubscriber ,
) # dds
ChannelFactoryInitialize ( 0 )
@@ -163,7 +154,7 @@ class UnitreeG1(Robot):
logger . info ( f " Current two arms motor state q: \n { self . get_current_dual_arm_q ( ) } \n " )
logger . info ( " Lock all joints except two arms... \n " )
arm_indices = set ( member . value for member in G1_29_JointArmIndex )
arm_indices = { member . value for member in G1_29_JointArmIndex }
for id in G1_29_JointIndex :
self . msg . motor_cmd [ id ] . mode = 1
if id . value in arm_indices :
@@ -217,18 +208,18 @@ class UnitreeG1(Robot):
logger . warning ( f " Loading locomotion policy from { config . policy_path } " )
# Check file extension and load accordingly
if config . policy_path . endswith ( ' .pt ' ) :
if config . policy_path . endswith ( " .pt " ) :
logger . warning ( " Detected TorchScript (.pt) policy " )
self . policy = torch . jit . load ( config . policy_path )
self . policy_type = ' torchscript '
self . policy_type = " torchscript "
logger . info ( " TorchScript policy loaded successfully " )
elif config . policy_path . endswith ( ' .onnx ' ) :
elif config . policy_path . endswith ( " .onnx " ) :
logger . warning ( " Detected ONNX (.onnx) policy " )
# For GR00T-style policies, load both Balance and Walk policies
# Balance policy for standing (low velocity commands)
# Walk policy for locomotion (high velocity commands)
balance_policy_path = config . policy_path . replace ( ' Walk.onnx ' , ' Balance.onnx ' )
balance_policy_path = config . policy_path . replace ( " Walk.onnx " , " Balance.onnx " )
walk_policy_path = config . policy_path
if Path ( balance_policy_path ) . exists ( ) and Path ( walk_policy_path ) . exists ( ) :
@@ -238,8 +229,12 @@ class UnitreeG1(Robot):
self . policy = None # Not used when dual policies are loaded
logger . info ( f " Balance policy loaded from: { balance_policy_path } " )
logger . info ( f " Walk policy loaded from: { walk_policy_path } " )
logger . info ( f " ONNX input: { self . policy_balance . get_inputs ( ) [ 0 ] . name } , shape: { self . policy_balance . get_inputs ( ) [ 0 ] . shape } " )
logger . info ( f " ONNX out put: { self . policy_balance . get_outputs ( ) [ 0 ] . name } , shape: { self . policy_balance . get_outputs ( ) [ 0 ] . shape } " )
logger . info (
f " ONNX in put: { self . policy_balance . get_inputs ( ) [ 0 ] . name } , shape: { self . policy_balance . get_inputs ( ) [ 0 ] . shape } "
)
logger . info (
f " ONNX output: { self . policy_balance . get_outputs ( ) [ 0 ] . name } , shape: { self . policy_balance . get_outputs ( ) [ 0 ] . shape } "
)
else :
# Fallback to single policy
logger . info ( " Loading single ONNX policy " )
@@ -247,12 +242,18 @@ class UnitreeG1(Robot):
self . policy_balance = None
self . policy_walk = None
logger . info ( " ONNX policy loaded successfully " )
logger . info ( f " ONNX input: { self . policy . get_inputs ( ) [ 0 ] . name } , shape: { self . policy . get_inputs ( ) [ 0 ] . shape } " )
logger . info ( f " ONNX out put: { self . policy . get_outputs ( ) [ 0 ] . name } , shape: { self . policy . get_outputs ( ) [ 0 ] . shape } " )
logger . info (
f " ONNX in put: { self . policy . get_inputs ( ) [ 0 ] . name } , shape: { self . policy . get_inputs ( ) [ 0 ] . shape } "
)
logger . info (
f " ONNX output: { self . policy . get_outputs ( ) [ 0 ] . name } , shape: { self . policy . get_outputs ( ) [ 0 ] . shape } "
)
self . policy_type = ' onnx '
self . policy_type = " onnx "
else :
raise ValueError ( f " Unsupported policy format: { config . policy_path } . Only .pt (TorchScript) and .onnx (ONNX) are supported. " )
raise ValueError (
f " Unsupported policy format: { config . policy_path } . Only .pt (TorchScript) and .onnx (ONNX) are supported. "
)
# Initialize locomotion variables
self . remote_controller = self . RemoteController ( )
@@ -264,7 +265,7 @@ class UnitreeG1(Robot):
self . locomotion_cmd = np . array ( [ 0.0 , 0.0 , 0.0 ] , dtype = np . float32 )
# GR00T-specific variables (for ONNX policies with 29 joints)
if self . policy_type == ' onnx ' :
if self . policy_type == " onnx " :
self . groot_qj_all = np . zeros ( 29 , dtype = np . float32 ) # All 29 joints
self . groot_dqj_all = np . zeros ( 29 , dtype = np . float32 )
self . groot_action = np . zeros ( 15 , dtype = np . float32 ) # 15D action (legs + waist)
@@ -284,7 +285,7 @@ class UnitreeG1(Robot):
self . start_keyboard_controls ( )
# Use different init based on policy type
if self . policy_type == ' onnx ' :
if self . policy_type == " onnx " :
self . init_groot_locomotion ( )
else :
self . init_locomotion ( )
@@ -292,7 +293,6 @@ class UnitreeG1(Robot):
# Even without locomotion, provide keyboard feedback in sim
logger . info ( " Simulation mode active (locomotion disabled) " )
logger . info ( " Initialize G1 OK! \n " )
def _subscribe_motor_state ( self ) :
@@ -326,6 +326,27 @@ class UnitreeG1(Robot):
sleep_time = max ( 0 , ( self . control_dt - all_t_elapsed ) ) # maintina constant control dt
time . sleep ( sleep_time )
def ctrl_dual_arm_go_home ( self ) :
""" Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero. """
logger . info ( " [G1_29_ArmController] ctrl_dual_arm_go_home start... " )
max_attempts = 100
current_attempts = 0
with self . ctrl_lock :
self . q_target = np . zeros ( 14 )
# self.q_target[G1_29_JointIndex.kLeftElbow] = 0.5
# self.tauff_target = np.zeros(14)
tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements
while current_attempts < max_attempts :
current_q = self . get_current_dual_arm_q ( )
if np . all ( np . abs ( current_q ) < tolerance ) :
if self . motion_mode :
for weight in np . linspace ( 1 , 0 , num = 101 ) :
self . msg . motor_cmd [ G1_29_JointIndex . kNotUsedJoint0 ] . q = weight
time . sleep ( 0.02 )
logger . info ( " [G1_29_ArmController] both arms have reached the home position. " )
break
current_attempts + = 1
time . sleep ( 0.05 )
def clip_arm_q_target ( self , target_q , velocity_limit ) :
current_q = self . get_current_dual_arm_q ( )
@@ -346,7 +367,9 @@ class UnitreeG1(Robot):
if self . simulation_mode :
cliped_arm_q_target = arm_q_target
else :
cliped_arm_q_target = self . clip_arm_q_target ( arm_q_target , velocity_limit = self . arm_velocity_limit )
cliped_arm_q_target = self . clip_arm_q_target (
arm_q_target , velocity_limit = self . arm_velocity_limit
)
for idx , id in enumerate ( G1_29_JointArmIndex ) :
self . msg . motor_cmd [ id ] . q = cliped_arm_q_target [ idx ]
@@ -396,7 +419,6 @@ class UnitreeG1(Robot):
""" Return current state dq of the left and right arm motors. """
return np . array ( [ self . lowstate_buffer . GetData ( ) . motor_state [ id ] . dq for id in G1_29_JointArmIndex ] )
def _Is_weak_motor ( self , motor_index ) :
weak_motors = [
G1_29_JointIndex . kLeftAnklePitch . value ,
@@ -430,7 +452,7 @@ class UnitreeG1(Robot):
return { f " { G1_29_JointArmIndex ( motor ) . name } .pos " : float for motor in G1_29_JointArmIndex }
def calibrate ( self ) - > None :
self . calibration = json . load ( open ( ' src/lerobot/robots/unitree_g1/arm_calibration.json ' ) )
self . calibration = json . load ( open ( " src/lerobot/robots/unitree_g1/arm_calibration.json " ) )
self . calibrated = True
def configure ( self ) - > None :
@@ -448,7 +470,7 @@ class UnitreeG1(Robot):
cam . disconnect ( )
# Close MuJoCo environment if in simulation mode
if self . simulation_mode and hasattr ( self , ' mujoco_env ' ) :
if self . simulation_mode and hasattr ( self , " mujoco_env " ) :
logger . info ( " Closing MuJoCo environment... " )
print ( self . mujoco_env )
self . mujoco_env [ " hub_env " ] [ 0 ] . envs [ 0 ] . kill_sim ( )
@@ -470,33 +492,40 @@ class UnitreeG1(Robot):
# Extract IMU data
imu_data = {
' quaternion ' : lowstate . imu_state . quaternion , # [w, x, y, z]
' gyroscope ' : lowstate . imu_state . gyroscope , # [x, y, z] rad/s
' accelerometer ' : lowstate . imu_state . accelerometer , # [x, y, z] m/s²
' rpy ' : lowstate . imu_state . rpy , # [roll, pitch, yaw] rad
' temperature ' : lowstate . imu_state . temperature , # °C
" quaternion " : lowstate . imu_state . quaternion , # [w, x, y, z]
" gyroscope " : lowstate . imu_state . gyroscope , # [x, y, z] rad/s
" accelerometer " : lowstate . imu_state . accelerometer , # [x, y, z] m/s²
" rpy " : lowstate . imu_state . rpy , # [roll, pitch, yaw] rad
" temperature " : lowstate . imu_state . temperature , # °C
}
# Extract motor data
motors_data = [ ]
for i in range ( G1_29_Num_Motors ) :
motor = lowstate . motor_state [ i ]
motors_data . append ( {
' id ' : i ,
' q ' : motor . q , # position (rad)
' dq ' : motor . dq , # velocity (rad/s )
' tau_est ' : motor . tau_est , # estimated torque (Nm )
' temperature ' : motor . temperature [ 0 ] if isinstance ( motor . temperature , ( list , tuple ) ) else motor . temperature , # °C
} )
motors_data . append (
{
" id " : i ,
" q " : motor . q , # position (rad)
" dq " : motor . dq , # velocity (rad/s )
" tau_est " : motor . tau_est , # estimated torque (Nm)
" temperature " : motor . temperature [ 0 ]
if isinstance ( motor . temperature , ( list , tuple ) )
else motor . temperature , # °C
}
)
return {
' imu ' : imu_data ,
' motors ' : motors_data ,
" imu " : imu_data ,
" motors " : motors_data ,
}
def get_observation ( self ) - > dict [ str , Any ] :
obs_array = self . get_current_dual_arm_q ( )
obs_dict = { f " { G1_29_JointArmIndex ( motor ) . name } .pos " : val for motor , val in zip ( G1_29_JointArmIndex , obs_array , strict = True ) }
obs_dict = {
f " { G1_29_JointArmIndex ( motor ) . name } .pos " : val
for motor , val in zip ( G1_29_JointArmIndex , obs_array , strict = True )
}
# Capture images from cameras
for cam_key , cam in self . cameras . items ( ) :
@@ -545,7 +574,9 @@ class UnitreeG1(Robot):
# check if action is within bounds
for key , value in action . items ( ) :
if value < self . calibration [ key ] [ " range_min " ] or value > self . calibration [ key ] [ " range_max " ] :
raise ValueError ( f " Action value { value } for { key } is out of bounds, actions are not normalized " )
raise ValueError (
f " Action value { value } for { key } is out of bounds, actions are not normalized "
)
if self . freeze_body :
arm_joint_indices = set ( range ( 15 , 29 ) ) # 15–
for jid in G1_29_JointIndex :
@@ -585,7 +616,6 @@ class UnitreeG1(Robot):
return calibrated
def invert_calibration ( self , action : dict [ str , float ] ) - > dict [ str , float ] :
""" Map [0, 1] actions back to motor ranges. """
calibrated = { }
@@ -697,7 +727,6 @@ class UnitreeG1(Robot):
time . sleep ( self . config . locomotion_control_dt )
logger . info ( " Finished holding default leg position " )
class RemoteController :
def __init__ ( self ) :
self . lx = 0
@@ -796,13 +825,17 @@ class UnitreeG1(Robot):
# Debug: print remote controller values every 50 iterations (~1 second at 50Hz)
if self . locomotion_counter % 50 == 0 :
logger . debug ( f " Remote controller - lx: { self . remote_controller . lx : .2f } , ly: { self . remote_controller . ly : .2f } , rx: { self . remote_controller . rx : .2f } " )
logger . debug (
f " Remote controller - lx: { self . remote_controller . lx : .2f } , ly: { self . remote_controller . ly : .2f } , rx: { self . remote_controller . rx : .2f } "
)
# Build observation vector
num_actions = self . config . num_locomotion_actions
self . locomotion_obs [ : 3 ] = ang_vel
self . locomotion_obs [ 3 : 6 ] = gravity_orientation
self . locomotion_obs [ 6 : 9 ] = self . locomotion_cmd * np . array ( self . config . cmd_scale ) * np . array ( self . config . max_cmd )
self . locomotion_obs [ 6 : 9 ] = (
self . locomotion_cmd * np . array ( self . config . cmd_scale ) * np . array ( self . config . max_cmd )
)
self . locomotion_obs [ 9 : 9 + num_actions ] = qj_obs
self . locomotion_obs [ 9 + num_actions : 9 + num_actions * 2 ] = dqj_obs
self . locomotion_obs [ 9 + num_actions * 2 : 9 + num_actions * 3 ] = self . locomotion_action
@@ -812,10 +845,10 @@ class UnitreeG1(Robot):
# Get action from policy network
obs_tensor = torch . from_numpy ( self . locomotion_obs ) . unsqueeze ( 0 )
if self . policy_type == ' torchscript ' :
if self . policy_type == " torchscript " :
# TorchScript inference
self . locomotion_action = self . policy ( obs_tensor ) . detach ( ) . numpy ( ) . squeeze ( )
elif self . policy_type == ' onnx ' :
elif self . policy_type == " onnx " :
# ONNX inference
ort_inputs = { self . policy . get_inputs ( ) [ 0 ] . name : obs_tensor . cpu ( ) . numpy ( ) }
ort_outs = self . policy . run ( None , ort_inputs )
@@ -824,7 +857,10 @@ class UnitreeG1(Robot):
raise ValueError ( f " Unknown policy type: { self . policy_type } " )
# Transform action to target joint positions
target_dof_pos = np . array ( self . config . default_leg_angles ) + self . locomotion_action * self . config . locomotion_action_scale
target_dof_pos = (
np . array ( self . config . default_leg_angles )
+ self . locomotion_action * self . config . locomotion_action_scale
)
# Send commands to LEG motors only
for i in range ( len ( self . config . leg_joint2motor_idx ) ) :
@@ -888,7 +924,9 @@ class UnitreeG1(Robot):
if self . config . locomotion_imu_type == " torso " :
waist_yaw = lowstate . motor_state [ 12 ] . q # Waist yaw index
waist_yaw_omega = lowstate . motor_state [ 12 ] . dq
quat , ang_vel_3d = self . locomotion_transform_imu_data ( waist_yaw , waist_yaw_omega , quat , np . array ( [ ang_vel ] ) )
quat , ang_vel_3d = self . locomotion_transform_imu_data (
waist_yaw , waist_yaw_omega , quat , np . array ( [ ang_vel ] )
)
ang_vel = ang_vel_3d . flatten ( )
# Create observation
@@ -903,11 +941,40 @@ class UnitreeG1(Robot):
# Subtract default angles for legs + waist (15 joints)
# GR00T default_angles: [-0.1, 0.0, 0.0, 0.3, -0.2, 0.0, -0.1, 0.0, 0.0, 0.3, -0.2, 0.0, 0.0, 0.0, 0.0]
groot_default_angles = np . array ( [ - 0.1 , 0.0 , 0.0 , 0.3 , - 0.2 , 0.0 , # left leg
- 0.1 , 0.0 , 0.0 , 0.3 , - 0.2 , 0.0 , # right leg
0.0 , 0.0 , 0.0 , # waist
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , # left arm (zeroed)
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 , 0.0 ] , dtype = np . float32 ) # right arm (zeroed)
groot_default_angles = np . array (
[
- 0.1 ,
0.0 ,
0.0 ,
0.3 ,
- 0.2 ,
0.0 , # left leg
- 0.1 ,
0.0 ,
0.0 ,
0.3 ,
- 0.2 ,
0.0 , # right leg
0.0 ,
0.0 ,
0.0 , # waist
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 , # left arm (zeroed)
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 ,
0.0 ,
] ,
dtype = np . float32 ,
) # right arm (zeroed)
qj_obs = ( qj_obs - groot_default_angles ) * self . config . dof_pos_scale
dqj_obs = dqj_obs * self . config . dof_vel_scale
@@ -920,7 +987,9 @@ class UnitreeG1(Robot):
self . locomotion_cmd [ 2 ] = self . remote_controller . rx * - 1
# Build 86D single frame observation (GR00T format)
self . groot_obs_single [ : 3 ] = self . locomotion_cmd * np . array ( self . config . groot_cmd_scale ) # cmd - use GR00T scaling!
self . groot_obs_single [ : 3 ] = self . locomotion_cmd * np . array (
self . config . groot_cmd_scale
) # cmd - use GR00T scaling!
self . groot_obs_single [ 3 ] = self . groot_height_cmd # height_cmd
self . groot_obs_single [ 4 : 7 ] = self . groot_orientation_cmd # roll, pitch, yaw cmd
self . groot_obs_single [ 7 : 10 ] = ang_vel_scaled # angular velocity
@@ -966,7 +1035,9 @@ class UnitreeG1(Robot):
self . groot_action [ 14 ] = 0.0 # Waist pitch
# Transform action to target joint positions (15D: legs + waist, but waist actions are zeroed)
target_dof_pos_15 = groot_default_angles [ : 15 ] + self . groot_action * self . config . locomotion_action_scale
target_dof_pos_15 = (
groot_default_angles [ : 15 ] + self . groot_action * self . config . locomotion_action_scale
)
# Send commands to LEG motors (0-11)
for i in range ( 12 ) :
@@ -983,7 +1054,9 @@ class UnitreeG1(Robot):
waist_roll_action_idx = 13 # In the 15D action
self . msg . motor_cmd [ waist_roll_idx ] . q = target_dof_pos_15 [ waist_roll_action_idx ]
self . msg . motor_cmd [ waist_roll_idx ] . qd = 0
self . msg . motor_cmd [ waist_roll_idx ] . kp = self . config . locomotion_arm_waist_kps [ 1 ] # index 1 is waist roll
self . msg . motor_cmd [ waist_roll_idx ] . kp = self . config . locomotion_arm_waist_kps [
1
] # index 1 is waist roll
self . msg . motor_cmd [ waist_roll_idx ] . kd = self . config . locomotion_arm_waist_kds [ 1 ]
self . msg . motor_cmd [ waist_roll_idx ] . tau = 0
@@ -1001,7 +1074,6 @@ class UnitreeG1(Robot):
self . msg . motor_cmd [ joint_idx ] . kd = self . kd_wrist
self . msg . motor_cmd [ joint_idx ] . tau = 0
# Send command
self . msg . crc = self . crc . Crc ( self . msg )
self . lowcmd_publisher . Write ( self . msg )
@@ -1013,7 +1085,7 @@ class UnitreeG1(Robot):
start_time = time . time ( )
try :
# Use different run function based on policy type
if self . policy_type == ' onnx ' :
if self . policy_type == " onnx " :
self . groot_locomotion_run ( )
else :
self . locomotion_run ( )
@@ -1079,25 +1151,25 @@ class UnitreeG1(Robot):
key = sys . stdin . read ( 1 ) . lower ( )
# Velocity commands
if key == ' w ' :
if key == " w " :
self . locomotion_cmd [ 0 ] + = 0.4 # Forward
elif key == ' s ' :
elif key == " s " :
self . locomotion_cmd [ 0 ] - = 0.4 # Backward
elif key == ' a ' :
elif key == " a " :
self . locomotion_cmd [ 1 ] + = 0.25 # Left
elif key == ' d ' :
elif key == " d " :
self . locomotion_cmd [ 1 ] - = 0.25 # Right
elif key == ' q ' :
elif key == " q " :
self . locomotion_cmd [ 2 ] + = 0.5 # Rotate left
elif key == ' e ' :
elif key == " e " :
self . locomotion_cmd [ 2 ] - = 0.5 # Rotate right
elif key == ' z ' :
elif key == " z " :
self . locomotion_cmd [ : ] = 0.0 # Stop
# Height commands (only for GR00T ONNX policies)
elif key == ' r ' :
elif key == " r " :
self . groot_height_cmd + = 0.05 # Raise 5cm
elif key == ' f ' :
elif key == " f " :
self . groot_height_cmd - = 0.05 # Lower 5cm
# Clamp commands to reasonable limits
@@ -1106,12 +1178,15 @@ class UnitreeG1(Robot):
self . locomotion_cmd [ 2 ] = np . clip ( self . locomotion_cmd [ 2 ] , - 1.0 , 1.0 ) # yaw_rate
# Clamp height (reasonable range: 0.5m to 1.0m)
if hasattr ( self , ' groot_height_cmd ' ) :
if hasattr ( self , " groot_height_cmd " ) :
self . groot_height_cmd = np . clip ( self . groot_height_cmd , 0.50 , 1.00 )
# Print current commands
print ( f " [VEL CMD] vx= { self . locomotion_cmd [ 0 ] : .2f } , vy= { self . locomotion_cmd [ 1 ] : .2f } , yaw= { self . locomotion_cmd [ 2 ] : .2f } " , end = " " )
if hasattr ( self , ' groot_height_cmd ' ) :
print (
f " [VEL CMD] vx= { self . locomotion_cmd [ 0 ] : .2f } , vy= { self . locomotion_cmd [ 1 ] : .2f } , yaw= { self . locomotion_cmd [ 2 ] : .2f } " ,
end = " " ,
)
if hasattr ( self , " groot_height_cmd " ) :
print ( f " | [HEIGHT] { self . groot_height_cmd : .3f } m " , end = " " )
print ( ) # Newline
@@ -1147,7 +1222,6 @@ class UnitreeG1(Robot):
self . keyboard_thread . join ( timeout = 2.0 )
logger . info ( " Keyboard controls stopped " )
def init_locomotion ( self ) :
""" Test locomotion control sequence: home arms -> move legs to default -> start policy thread. """
if not self . config . locomotion_control :
@@ -1156,7 +1230,6 @@ class UnitreeG1(Robot):
logger . info ( " Starting locomotion test sequence... " )
# 2. Move legs to default position
self . locomotion_move_to_default_pos ( )
@@ -1196,70 +1269,3 @@ class UnitreeG1(Robot):
logger . info ( " GR00T locomotion initialization complete! Policy is now running. " )
logger . info ( " 516D observations (86D × " )
class G1_29_JointArmIndex ( IntEnum ) :
# Left arm
kLeftShoulderPitch = 15
kLeftShoulderRoll = 16
kLeftShoulderYaw = 17
kLeftElbow = 18
kLeftWristRoll = 19
kLeftWristPitch = 20
kLeftWristyaw = 21
# Right arm
kRightShoulderPitch = 22
kRightShoulderRoll = 23
kRightShoulderYaw = 24
kRightElbow = 25
kRightWristRoll = 26
kRightWristPitch = 27
kRightWristYaw = 28
class G1_29_JointIndex ( IntEnum ) :
# Left leg
kLeftHipPitch = 0
kLeftHipRoll = 1
kLeftHipYaw = 2
kLeftKnee = 3
kLeftAnklePitch = 4
kLeftAnkleRoll = 5
# Right leg
kRightHipPitch = 6
kRightHipRoll = 7
kRightHipYaw = 8
kRightKnee = 9
kRightAnklePitch = 10
kRightAnkleRoll = 11
kWaistYaw = 12 #we're c
kWaistRoll = 13
kWaistPitch = 14
# Left arm
kLeftShoulderPitch = 15
kLeftShoulderRoll = 16
kLeftShoulderYaw = 17
kLeftElbow = 18
kLeftWristRoll = 19
kLeftWristPitch = 20
kLeftWristyaw = 21
# Right arm
kRightShoulderPitch = 22
kRightShoulderRoll = 23
kRightShoulderYaw = 24
kRightElbow = 25
kRightWristRoll = 26
kRightWristPitch = 27
kRightWristYaw = 28
# not used
kNotUsedJoint0 = 29
kNotUsedJoint1 = 30
kNotUsedJoint2 = 31
kNotUsedJoint3 = 32
kNotUsedJoint4 = 33
kNotUsedJoint5 = 34
Martino Russi