Spaces:
Running
Running
File size: 5,371 Bytes
48c3baa | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 | """Python equivalent of the JS streaming benchmark.
Runs the exact same motion (mirror-symmetric antennas + head Y sin)
through the Python SDK while reading state at the same nominal rate.
Output CSV matches the JS schema so the two can be compared head-to-head.
Notes on parity vs JS:
- No lead-compensation here. The JS Playback shifts antenna commands
40 ms ahead and head 150 ms ahead. This script sends the raw motion
at wall-clock t. We're hunting for jitter/stutter events, which show
up the same way regardless of mean phase lag.
- The Python SDK exposes synchronous `get_current_head_pose()` /
`get_present_antenna_joint_positions()` that read a client-side
cache of the latest daemon state. Read timing is therefore set by
*our* loop, not by daemon-pushed events as in JS.
"""
import csv
import math
import time
import numpy as np
from reachy_mini import ReachyMini
# Match lib/benchmark.js BENCHMARK constants exactly.
CENTER_DEG = 30.0
AMP_DEG = 20.0
PHASE1_DUR = 5.0
PHASE1_HZ = 0.5
HOLD_DUR = 2.0
PHASE3_DUR = 5.0
PHASE3_HZ = 1.5
HEAD_AMP_M = 0.03
HEAD_PHASE1_HZ = 0.3
HEAD_PHASE3_HZ = 0.5
TOTAL_DUR = PHASE1_DUR + HOLD_DUR + PHASE3_DUR
LOOP_HZ = 50.0
PERIOD = 1.0 / LOOP_HZ
def right_at(t: float) -> float:
center = math.radians(CENTER_DEG)
amp = math.radians(AMP_DEG)
if t < 0:
return center
if t <= PHASE1_DUR:
return center + amp * math.sin(2 * math.pi * PHASE1_HZ * t)
if t <= PHASE1_DUR + HOLD_DUR:
return center
if t <= TOTAL_DUR:
return center + amp * math.sin(
2 * math.pi * PHASE3_HZ * (t - PHASE1_DUR - HOLD_DUR)
)
return center
def left_at(t: float) -> float:
return -right_at(t)
def head_y_at(t: float) -> float:
if t < 0:
return 0.0
if t <= PHASE1_DUR:
return HEAD_AMP_M * math.sin(2 * math.pi * HEAD_PHASE1_HZ * t)
if t <= PHASE1_DUR + HOLD_DUR:
return 0.0
if t <= TOTAL_DUR:
return HEAD_AMP_M * math.sin(
2 * math.pi * HEAD_PHASE3_HZ * (t - PHASE1_DUR - HOLD_DUR)
)
return 0.0
def head_pose_y(y_m: float) -> np.ndarray:
H = np.eye(4)
H[1, 3] = y_m
return H
def main() -> None:
samples: list[tuple[float, float, float, float, float, float, float]] = []
print("Connecting to Reachy Mini…")
with ReachyMini() as mini:
print("Connected. Going to initial pose…")
init_head = head_pose_y(head_y_at(0.0))
mini.goto_target(
init_head,
antennas=[left_at(0.0), right_at(0.0)],
duration=1.5,
)
time.sleep(0.2)
print(f"Running benchmark: {TOTAL_DUR:.1f} s at {LOOP_HZ:.0f} Hz…")
t0 = time.perf_counter()
tick = 0
while True:
now = time.perf_counter()
t = now - t0
if t > TOTAL_DUR:
break
cmd_l = left_at(t)
cmd_r = right_at(t)
cmd_hy = head_y_at(t)
mini.set_target(
head=head_pose_y(cmd_hy),
antennas=[cmd_l, cmd_r],
)
# Read the latest cached state right after sending. This is
# the closest analogue to what the JS path measures (where
# state events are pushed asynchronously). Read time is our
# wall-clock t.
try:
actual_head = mini.get_current_head_pose()
actual_ant = mini.get_present_antenna_joint_positions()
samples.append((
t,
math.degrees(cmd_l),
math.degrees(actual_ant[0]),
math.degrees(cmd_r),
math.degrees(actual_ant[1]),
cmd_hy * 100.0,
float(actual_head[1, 3]) * 100.0,
))
except Exception as e:
print(f" read error at t={t:.3f}: {e}")
tick += 1
next_tick = t0 + tick * PERIOD
sleep_for = next_tick - time.perf_counter()
if sleep_for > 0:
time.sleep(sleep_for)
print("Done. Returning to base…")
mini.goto_target(
np.eye(4),
antennas=[-0.1745, 0.1745],
duration=1.0,
)
time.sleep(1.1)
stamp = time.strftime("%Y-%m-%dT%H-%M-%S")
out = f"/Users/remi/Downloads/marionette-benchmark-python-{stamp}.csv"
with open(out, "w", newline="") as f:
w = csv.writer(f)
w.writerow([
"t_s",
"left_commanded_deg", "left_actual_deg",
"right_commanded_deg", "right_actual_deg",
"head_y_commanded_cm", "head_y_actual_cm",
])
for row in samples:
w.writerow([
f"{row[0]:.4f}",
f"{row[1]:.3f}", f"{row[2]:.3f}",
f"{row[3]:.3f}", f"{row[4]:.3f}",
f"{row[5]:.3f}", f"{row[6]:.3f}",
])
print(f"Wrote {out}")
print(f"Samples: {len(samples)}")
if samples:
dts = [(samples[i][0] - samples[i - 1][0]) * 1000 for i in range(1, len(samples))]
print(
f"State interval: mean {sum(dts)/len(dts):.1f} ms, "
f"max {max(dts):.0f} ms, "
f"p99 {sorted(dts)[int(len(dts)*0.99)]:.0f} ms"
)
if __name__ == "__main__":
main()
|