33 lines
1.1 KiB
Python
33 lines
1.1 KiB
Python
"""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
|