From 5d9266b024c3ac1624e4ef552fa3bb67e663d897 Mon Sep 17 00:00:00 2001 From: Martino Russi Date: Wed, 26 Nov 2025 15:37:04 +0100 Subject: [PATCH] simplify implementation --- .../robots/unitree_g1/config_unitree_g1.py | 22 +------- .../unitree_g1/robot_kinematic_processor.py | 50 ------------------- src/lerobot/robots/unitree_g1/robot_server.py | 9 +--- 3 files changed, 4 insertions(+), 77 deletions(-) diff --git a/src/lerobot/robots/unitree_g1/config_unitree_g1.py b/src/lerobot/robots/unitree_g1/config_unitree_g1.py index 6efd12ebd..a7d56b705 100644 --- a/src/lerobot/robots/unitree_g1/config_unitree_g1.py +++ b/src/lerobot/robots/unitree_g1/config_unitree_g1.py @@ -25,7 +25,6 @@ from ..config import RobotConfig @dataclass class UnitreeG1Config(RobotConfig): # id: str = "unitree_g1" - motion_mode: bool = False simulation_mode: bool = True kp_high = 40.0 kd_high = 3.0 @@ -41,12 +40,9 @@ class UnitreeG1Config(RobotConfig): arm_velocity_limit = 100.0 control_dt = 1.0 / 250.0 - speed_gradual_max: bool = False gradual_start_time: float | None = None gradual_time: float | None = None - audio_client: bool = True - freeze_body: bool = False gravity_compensation: bool = True @@ -56,28 +52,14 @@ class UnitreeG1Config(RobotConfig): # This robot class ONLY uses sockets to communicate with a bridge on the Orin # Run 'python dds_to_socket.py' on the Orin first, then set this to the Orin's IP # Example: socket_host="192.168.123.164" (Orin's wlan0 IP) - socket_host: str | None = None + socket_host: str | None = None# = "172.18.129.215" socket_port: int | None = None # Locomotion control - locomotion_control: bool = True + locomotion_control: bool = False #policy_path: str = "src/lerobot/robots/unitree_g1/assets/g1/locomotion/motion.pt" policy_path: str = "src/lerobot/robots/unitree_g1/assets/g1/locomotion/GR00T-WholeBodyControl-Walk.onnx" - # Motion imitation (dance_102, gangnam_style, etc) - motion_imitation_control: bool = False - motion_policy_path: str = "unitree_rl_lab/deploy/robots/g1_29dof/config/policy/mimic/dance_102/exported/policy.onnx" # Use policy - #motion_policy_path: str = None # Set to None for direct playback mode (no policy) - motion_file_path: str = "unitree_rl_lab/deploy/robots/g1_29dof/config/policy/mimic/dance_102/params/G1_Take_102.bvh_60hz.csv" - motion_fps: float = 60.0 - motion_control_dt: float = 0.02 - - motion_joint_ids_map: list = field(default_factory=lambda: [0, 6, 12, 1, 7, 13, 2, 8, 14, 3, 9, 15, 22, 4, 10, 16, 23, 5, 11, 17, 24, 18, 25, 19, 26, 20, 27, 21, 28]) - motion_stiffness: list = field(default_factory=lambda: [40.2, 99.1, 40.2, 99.1, 28.5, 28.5, 40.2, 99.1, 40.2, 99.1, 28.5, 28.5, 40.2, 28.5, 28.5, 14.3, 14.3, 14.3, 14.3, 14.3, 16.8, 16.8, 14.3, 14.3, 14.3, 14.3, 14.3, 16.8, 16.8]) - motion_damping: list = field(default_factory=lambda: [2.56, 6.31, 2.56, 6.31, 1.81, 1.81, 2.56, 6.31, 2.56, 6.31, 1.81, 1.81, 2.56, 1.81, 1.81, 0.907, 0.907, 0.907, 0.907, 0.907, 1.07, 1.07, 0.907, 0.907, 0.907, 0.907, 0.907, 1.07, 1.07]) - motion_default_joint_pos: list = field(default_factory=lambda: [-0.302, -0.319, 0.00124, 0.000442, 0.00489, 0.00191, 0.00929, 0.00796, 0.00546, 0.672, 0.67, 0.2, 0.202, -0.368, -0.355, 0.194, -0.196, -0.00644, 0.00976, 0.00258, -0.00029, 0.605, 0.596, 0.00818, 0.00322, 0.00293, -0.00339, -0.00955, -0.00715]) - motion_action_scale: list = field(default_factory=lambda: [0.548, 0.548, 0.548, 0.351, 0.351, 0.439, 0.548, 0.548, 0.439, 0.351, 0.351, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.439, 0.0745, 0.0745, 0.0745, 0.0745]) - # Locomotion parameters (from g1.yaml) locomotion_control_dt: float = 0.02 diff --git a/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py b/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py index 7a2ff282e..bd81d756a 100644 --- a/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py +++ b/src/lerobot/robots/unitree_g1/robot_kinematic_processor.py @@ -56,56 +56,6 @@ class WeightedMovingFilter: return self._filtered_data -def visualize_filter_comparison(filter_params, steps): - import time - - t = np.linspace(0, 4 * np.pi, steps) - original_data = np.array( - [np.sin(t + i) + np.random.normal(0, 0.2, len(t)) for i in range(35)] - ).T # sin wave with noise, shape is [len(t), 35] - - plt.figure(figsize=(14, 10)) - - for idx, weights in enumerate(filter_params): - filter = WeightedMovingFilter(weights, 14) - data_2b_filtered = original_data.copy() - filtered_data = [] - - time1 = time.time() - - for i in range(steps): - filter.add_data(data_2b_filtered[i][13:27]) # step i, columns 13 to 26 (total:14) - data_2b_filtered[i][13:27] = filter.filtered_data - filtered_data.append(data_2b_filtered[i]) - - time2 = time.time() - print(f"filter_params:{filter_params[idx]}, time cosume:{time2 - time1}") - - filtered_data = np.array(filtered_data) - - # col0 should not 2b filtered - plt.subplot(len(filter_params), 2, idx * 2 + 1) - plt.plot(filtered_data[:, 0], label=f"Filtered (Window {filter._window_size})") - plt.plot(original_data[:, 0], "r--", label="Original", alpha=0.5) - plt.title("Joint 1 - Should not to be filtered.") - plt.xlabel("Step") - plt.ylabel("Value") - plt.legend() - - # col13 should 2b filtered - plt.subplot(len(filter_params), 2, idx * 2 + 2) - plt.plot(filtered_data[:, 13], label=f"Filtered (Window {filter._window_size})") - plt.plot(original_data[:, 13], "r--", label="Original", alpha=0.5) - plt.title(f"Joint 13 - Window {filter._window_size}, Weights {weights}") - plt.xlabel("Step") - plt.ylabel("Value") - plt.legend() - - plt.tight_layout() - plt.show() - - - class G1_29_ArmIK: def __init__(self, Unit_Test=False, Visualization=False): np.set_printoptions(precision=5, suppress=True, linewidth=200) diff --git a/src/lerobot/robots/unitree_g1/robot_server.py b/src/lerobot/robots/unitree_g1/robot_server.py index 5c720de97..42a93636c 100644 --- a/src/lerobot/robots/unitree_g1/robot_server.py +++ b/src/lerobot/robots/unitree_g1/robot_server.py @@ -11,7 +11,6 @@ from unitree_sdk2py.utils.crc import CRC from unitree_sdk2py.comm.motion_switcher.motion_switcher_client import MotionSwitcherClient kTopicLowCommand_Debug = "rt/lowcmd" -kTopicLowCommand_Motion = "rt/arm_sdk" kTopicLowState = "rt/lowstate" LOWCMD_PORT = 6000 # laptop -> robot @@ -43,7 +42,7 @@ def state_forward_loop(lowstate_sub, lowstate_sock, state_period: float): last_state_time = now -def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, lowcmd_pub_motion, crc: CRC): +def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, crc: CRC): """ read lowcmd from laptop (zmq) and push to dds. runs in its own thread. @@ -58,8 +57,6 @@ def cmd_forward_loop(lowcmd_sock, lowcmd_pub_debug, lowcmd_pub_motion, crc: CRC) if topic == kTopicLowCommand_Debug: lowcmd_pub_debug.Write(cmd) - elif topic == kTopicLowCommand_Motion: - lowcmd_pub_motion.Write(cmd) else: # ignore unknown topics pass @@ -84,9 +81,7 @@ def main(): # dds publishers / subscriber lowcmd_pub_debug = ChannelPublisher(kTopicLowCommand_Debug, hg_LowCmd) - lowcmd_pub_motion = ChannelPublisher(kTopicLowCommand_Motion, hg_LowCmd) lowcmd_pub_debug.Init() - lowcmd_pub_motion.Init() lowstate_sub = ChannelSubscriber(kTopicLowState, hg_LowState) lowstate_sub.Init() @@ -112,7 +107,7 @@ def main(): ) t_cmd = threading.Thread( target=cmd_forward_loop, - args=(lowcmd_sock, lowcmd_pub_debug, lowcmd_pub_motion, crc), + args=(lowcmd_sock, lowcmd_pub_debug, crc), daemon=True, )