admin/lerobot
1
0
Fork 0
mirror of https://github.com/huggingface/lerobot.git synced 2026-07-23 09:46:00 +00:00
Code Issues Packages Projects Releases Wiki Activity

switch to logging

Browse Source
This commit is contained in:
Martino Russi
2026-07-15 15:30:54 +02:00
parent c8e75da55f
commit 2492ce2c29
2 changed files with 48 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lerobot/robots/unitree_g1/controllers/sonic_pipeline.py
+41 -25
View File
@@ -55,6 +55,7 @@ worker · ``SonicPlanner`` · ``PlannerController`` · joystick input.
from __future__ import annotations from __future__ import annotations
import logging
import math import math
import os import os
import queue import queue
@@ -78,6 +79,8 @@ if TYPE_CHECKING or _onnxruntime_available:
else: else:
ort = None ort = None
logger = logging.getLogger(__name__)
# ── Constants ──────────────────────────────────────────────────────────────── # ── Constants ────────────────────────────────────────────────────────────────
# Robot/motor physical constants and the joint-order permutation tables. All # Robot/motor physical constants and the joint-order permutation tables. All
# 29-vectors are in IsaacLab joint order unless the name says ``_MUJOCO``. # 29-vectors are in IsaacLab joint order unless the name says ``_MUJOCO``.
@@ -799,27 +802,38 @@ class StandingEncoderDecoder:
target = DEFAULT_ANGLES + action_mj[ISAACLAB_TO_MUJOCO] * ACTION_SCALE target = DEFAULT_ANGLES + action_mj[ISAACLAB_TO_MUJOCO] * ACTION_SCALE
if debug: if debug:
delta = target - q delta = target - q
print( logger.debug(
f"token_norm={np.linalg.norm(self.token):.4f} action_norm={np.linalg.norm(action_mj):.4f} " "token_norm=%.4f action_norm=%.4f delta_max=%.4f delta_rms=%.4f",
f"delta_max={np.max(np.abs(delta)):.4f} delta_rms={np.sqrt(np.mean(delta**2)):.4f}" np.linalg.norm(self.token),
np.linalg.norm(action_mj),
np.max(np.abs(delta)),
np.sqrt(np.mean(delta**2)),
) )
return {f"{m.name}.q": float(target[m.value]) for m in G1_29_JointIndex} return {f"{m.name}.q": float(target[m.value]) for m in G1_29_JointIndex}
def print_input_diagnostics(self): def print_input_diagnostics(self):
"""Print sanity checks on the reference/anchor/gravity terms (debugging aid).""" """Log sanity checks on the reference/anchor/gravity terms (debugging aid)."""
print("\n[Diag] Standing reference checks")
names = {0: "g1", 1: "teleop", 2: "smpl"} names = {0: "g1", 1: "teleop", 2: "smpl"}
print(f" encoder mode: {self.encode_mode} ({names.get(self.encode_mode, 'unknown')})") anchor = self._anchor_6d(np.array([1, 0, 0, 0], np.float32), np.array([1, 0, 0, 0], np.float32))
print(f" DEFAULT_ANGLES range: [{DEFAULT_ANGLES.min():+.4f}, {DEFAULT_ANGLES.max():+.4f}]") gravity = get_gravity_orientation(np.array([1, 0, 0, 0], np.float32))
print(
f" anchor_6d(identity): {self._anchor_6d(np.array([1, 0, 0, 0], np.float32), np.array([1, 0, 0, 0], np.float32))}"
)
print(
f" gravity(identity): {get_gravity_orientation(np.array([1, 0, 0, 0], np.float32))} (expect [0,0,-1])"
)
dec0 = self.build_decoder_obs() dec0 = self.build_decoder_obs()
print(f" decoder q-delta max: {np.max(np.abs(dec0[94:384])):.6f}") logger.debug(
print(f" decoder dq max: {np.max(np.abs(dec0[384:674])):.6f}") "[Diag] Standing reference checks\n"
" encoder mode: %d (%s)\n"
" DEFAULT_ANGLES range: [%+.4f, %+.4f]\n"
" anchor_6d(identity): %s\n"
" gravity(identity): %s (expect [0,0,-1])\n"
" decoder q-delta max: %.6f\n"
" decoder dq max: %.6f",
self.encode_mode,
names.get(self.encode_mode, "unknown"),
DEFAULT_ANGLES.min(),
DEFAULT_ANGLES.max(),
anchor,
gravity,
np.max(np.abs(dec0[94:384])),
np.max(np.abs(dec0[384:674])),
)
# ── Planner motion buffer ───────────────────────────────────────────────────── # ── Planner motion buffer ─────────────────────────────────────────────────────
@@ -923,9 +937,11 @@ def _planner_worker(path, req_q, res_q, stop_evt, version, seed, use_gpu):
continue continue
motion = _resample_30_to_50(qpos, n) motion = _resample_30_to_50(qpos, n)
motion["gen_frame"] = gf motion["gen_frame"] = gf
print( logger.debug(
f"[Planner] inf={1000 * (t_inf - t0):.1f}ms total={1000 * (time.time() - t0):.1f}ms frames={n}", "[Planner] inf=%.1fms total=%.1fms frames=%d",
flush=True, 1000 * (t_inf - t0),
1000 * (time.time() - t0),
n,
) )
while not res_q.empty(): while not res_q.empty():
try: try:
@@ -933,8 +949,8 @@ def _planner_worker(path, req_q, res_q, stop_evt, version, seed, use_gpu):
except queue.Empty: except queue.Empty:
break break
res_q.put(motion) res_q.put(motion)
except Exception as e: except Exception:
print(f"[Planner] Error: {e}", flush=True) logger.exception("[Planner] worker error")
# ── SonicPlanner ────────────────────────────────────────────────────────────── # ── SonicPlanner ──────────────────────────────────────────────────────────────
@@ -1033,9 +1049,9 @@ class SonicPlanner:
qpos = qpos_out[0, :n] qpos = qpos_out[0, :n]
if np.any(np.isnan(qpos)): if np.any(np.isnan(qpos)):
raise RuntimeError("Planner initial output contains NaN") raise RuntimeError("Planner initial output contains NaN")
print(f"[Planner] Init: {n} frames @ 30 Hz") logger.info("[Planner] Init: %d frames @ 30 Hz", n)
self._load_motion_in_place(qpos, n) self._load_motion_in_place(qpos, n)
print(f"[Planner] Resampled to {self.motion_50hz.timesteps} frames @ 50 Hz") logger.info("[Planner] Resampled to %d frames @ 50 Hz", self.motion_50hz.timesteps)
return self.motion_50hz return self.motion_50hz
def request_replan(self, cursor, ms): def request_replan(self, cursor, ms):
@@ -1099,7 +1115,7 @@ class SonicPlanner:
name="sonic-planner", name="sonic-planner",
) )
self._planner_thread.start() self._planner_thread.start()
print(f"[Planner] Background thread started ({'GPU' if use_gpu else 'CPU'})") logger.info("[Planner] Background thread started (%s)", "GPU" if use_gpu else "CPU")
def stop_subprocess(self): def stop_subprocess(self):
"""Signal the planner thread to stop and join it.""" """Signal the planner thread to stop and join it."""
@@ -1107,7 +1123,7 @@ class SonicPlanner:
self._stop_evt.set() self._stop_evt.set()
if self._planner_thread is not None: if self._planner_thread is not None:
self._planner_thread.join(timeout=3.0) self._planner_thread.join(timeout=3.0)
print("[Planner] Background thread stopped") logger.info("[Planner] Background thread stopped")
self._planner_thread = None self._planner_thread = None
self._req_q = self._res_q = self._stop_evt = None self._req_q = self._res_q = self._stop_evt = None
@@ -1294,7 +1310,7 @@ class PlannerController(StandingEncoderDecoder):
self.delta_heading = 0.0 self.delta_heading = 0.0
self.first_motion = False self.first_motion = False
self.reinit_heading = False self.reinit_heading = False
print(f"[Heading] init quat: {self.heading_init_base_quat}") logger.debug("[Heading] init quat: %s", self.heading_init_base_quat)
return super().step(robot_obs, update_encoder=update_encoder, debug=debug) return super().step(robot_obs, update_encoder=update_encoder, debug=debug)
def advance_cursor(self): def advance_cursor(self):
src/lerobot/robots/unitree_g1/smpl_stream.py
+7 -3
View File
@@ -34,12 +34,15 @@ from __future__ import annotations
import contextlib import contextlib
import json import json
import logging
import time import time
from collections import deque from collections import deque
import numpy as np import numpy as np
import zmq import zmq
logger = logging.getLogger(__name__)
SMPL_TOPIC = "rt/smpl" SMPL_TOPIC = "rt/smpl"
DEFAULT_SMPL_HOST = "127.0.0.1" DEFAULT_SMPL_HOST = "127.0.0.1"
DEFAULT_SMPL_PORT = 5560 DEFAULT_SMPL_PORT = 5560
@@ -185,9 +188,10 @@ class SmplStream:
and not self._warned_stale and not self._warned_stale
and (now - self._last_recv_t) > self.stale_after_s and (now - self._last_recv_t) > self.stale_after_s
): ):
print( logger.warning(
f"[SmplStream] no {SMPL_TOPIC} frame for " "[SmplStream] no %s frame for %.2fs (holding last pose)",
f"{now - self._last_recv_t:.2f}s (holding last pose)" SMPL_TOPIC,
now - self._last_recv_t,
) )
self._warned_stale = True self._warned_stale = True