Overview

# guard(robot)  # optional: uncomment this and the import to stop on your rule
try:
    # Returns once the robot reports STAND and has been upright for 0.5 s (armed): the
    # firmware enters MOVE only after that.
    robot.stand()
except (NotReadyError, WaitTimeoutError) as exc:  # no live state, a fault, or not armed
    print(exc)  # what the robot reports and what to do
    sys.exit(1)
print(f"robot mode {robot.get_state().mode.name}, armed {robot.armed}")

# guard(robot)  # optional: uncomment this and the import to stop on your rule
try:
    robot.balance()  # returns once the robot reports MOVE
except (NotReadyError, WaitTimeoutError) as exc:  # no live state, a fault, or not MOVE
    print(exc)  # what the robot reports and what to do
    sys.exit(1)
print(f"robot mode {robot.get_state().mode.name}, balancing in place")

# guard(robot)  # optional: uncomment this and the import to stop on your rule
mode = robot.get_state().mode
if mode is not Mode.MOVE:  # this script's own rule: walk from MOVE only
    print(f"robot mode {mode.name}: walk.py walks in MOVE only; run balance.py first")
    sys.exit(1)
try:
    # Held and re-sent at 10 Hz for DURATION_S, then zero velocity; returns after that.
    sent = robot.set_velocity(vx=VX, vy=VY, vyaw=VYAW, duration=DURATION_S)
except NotReadyError as exc:  # no live state (nothing sent), or a fault ended the walk
    print(exc)
    sys.exit(1)
if sent.clamped:
    print("the SDK reduced the speed to the robot's limits:", sent.command)
print(f"robot mode {robot.get_state().mode.name}, balancing in place")

# robot.get_state() is the latest sample the robot sent. It is a snapshot: read it again
# for newer values. A field the robot does not report is None, never a guess.
s = robot.get_state()

# Robot mode: DAMP (no actuator holds a position), STAND (a held standing pose, no
# balance) or MOVE (the walking policy balances the robot, at zero or any velocity).
print(f"robot mode   {s.mode.name}")

# Armed: the firmware accepts MOVE only after STAND has been held upright for 0.5 s.
# None means the SDK cannot tell (no gravity vector reported).
print(f"armed        {robot.armed}")

# Faulted: a critical alert latched DAMP. The robot stays in DAMP, and error_flags stay
# set, until the firmware restarts. The preflight check names what latched.
print(f"faulted      {s.faulted}")
for problem in robot.preflight("stand").problems:
    if problem.code == "faulted":
        print(f"  {problem.message}")

# Battery, from the battery management system. None when the robot reports none.
if s.battery is not None:
    b = s.battery
    print(
        f"battery      {b.soc_percent:.0f} %, {b.voltage_v:.1f} V, {b.current_a:+.1f} A, "
        f"warmest cell {b.max_cell_temp_c:.0f} °C, protecting {b.protecting}"
    )
else:
    print("battery      not reported")

# Actuators: one Joint per actuator, with the firmware's name, position (rad), velocity
# (rad/s), current (A) and temperature (°C). The firmware warns at 60 °C and latches
# DAMP at 80 °C.
temps = sorted(((j.temp, j.name) for j in s.joints if j.temp is not None), reverse=True)
if temps:
    hottest = ", ".join(f"{name} {temp:.0f} °C" for temp, name in temps[:HOTTEST])
    print(f"hottest      {hottest}")
else:
    print("hottest      actuator temperatures not reported")

# IMU: gravity as the body sees it. z is close to -1 when the robot is upright;
# upright is True below -0.8. euler is (roll, pitch, yaw) in rad from the IMU quaternion.
if s.gravity is not None:
    gx, gy, gz = s.gravity
    print(f"gravity      ({gx:+.2f}, {gy:+.2f}, {gz:+.2f}), upright {s.upright}")
if s.euler is not None:
    roll, pitch, yaw = s.euler
    print(f"orientation  roll {roll:+.2f}, pitch {pitch:+.2f}, yaw {yaw:+.2f} rad")

# Alerts the firmware reports now. Severity 0 is critical: it latches DAMP.
alerts = [f"{a.name}{' (critical)' if a.critical else ''}" for a in s.alerts]
print(f"alerts       {', '.join(alerts) or 'none'}")

# Freshness: how old the sample is. Decisions to move are made on samples at most
# 0.5 s old; robot.preflight() reports stale_state beyond that.
print(f"state age    {s.age_s * 1000:.0f} ms")

# The same fields as a stream, a few times a second.
for _ in range(STREAM_LINES):
    time.sleep(STREAM_PERIOD_S)
    s = robot.get_state()
    now = time.monotonic()
    rate = sum(1 for t in tuple(arrivals) if now - t <= 1.0)
    battery = f"{s.battery.soc_percent:.0f} %" if s.battery else "n/a"
    print(
        f"{s.mode.name:5}  armed {robot.armed}  upright {s.upright}  faulted {s.faulted}  "
        f"battery {battery}  age {s.age_s * 1000:.0f} ms  rate {rate:.0f} Hz"
    )

if robot.has("camera"):
    photo = robot.camera.photo()  # the next fresh frame, rgb8
    Path(PHOTO).write_bytes(photo.to_jpeg())
    print(f"saved {PHOTO}, {photo.width}x{photo.height}")
    clip = robot.camera.capture_clip(CLIP_S, audio=robot.has("microphone"))
    frames = clip.save_frames(CLIP_DIR)
    print(f"saved {len(frames)} frames to {CLIP_DIR}/ at {clip.fps:.1f} fps")
    if clip.audio:
        print("saved", clip.save_wav(CLIP_WAV))
else:
    print("this connection carries no camera; use hybrid or livekit")

if robot.has("speaker"):
    # 16-bit mono PCM: a sine wave at TONE_HZ for TONE_S.
    tone = b"".join(
        struct.pack("<h", int(8000 * math.sin(2 * math.pi * TONE_HZ * i / SAMPLE_RATE_HZ)))
        for i in range(int(SAMPLE_RATE_HZ * TONE_S))
    )
    robot.speaker.play_pcm(tone, sample_rate_hz=SAMPLE_RATE_HZ)
    print(f"played {TONE_S:.0f} s at {TONE_HZ:.0f} Hz")