Initial commit

s10_supermimic/core/command_curriculum.py

@@ -0,0 +1,32 @@
+"""Velocity range presets. V4 is time-based; v5/v6/v7 are stair-gated.
+
+S10 v7 uses ``V7_STAGES`` as the 1.5/0.8/1.2 cap, but stage advancement comes
+from ``stair_curriculum.next_stage`` (actual up-stair pass rate). Only v4
+advances by iteration count alone.
+"""
+STAGES = (
+  (0, (-.3,.8), (-.2,.2), (-.8,.8)),
+  (1000, (-.5,1.), (-.4,.4), (-1.,1.)),
+  (2500, (-.8,1.5), (-.6,.6), (-1.5,1.5)),
+  (5000, (-1.5,1.8), (-.8,.8), (-2.,2.)),
+  (7500, (-2.,2.), (-1.,1.), (-3.,3.)),
+)
+V7_STAGES = (
+  (0, (-.3,.8), (-.2,.2), (-.5,.5)),
+  (1000, (-.5,1.), (-.4,.4), (-.7,.7)),
+  (2500, (-.8,1.2), (-.6,.6), (-.9,.9)),
+  (5000, (-1.2,1.4), (-.7,.7), (-1.1,1.1)),
+  (7500, (-1.5,1.5), (-.8,.8), (-1.2,1.2)),
+)
+
+
+def stages_for(revision):
+  return V7_STAGES if revision >= 7 else STAGES
+
+
+def set_command_stage(cfg, iteration, stage=None, stages=STAGES):
+  index = stage if stage is not None else max(
+    i for i, item in enumerate(stages) if max(0, iteration) >= item[0])
+  _, x, y, yaw = stages[index]
+  cfg.ranges.lin_vel_x, cfg.ranges.lin_vel_y, cfg.ranges.ang_vel_z = x, y, yaw
+  return index