ceca51b0d8
Default humanoid intake to command-following gait and support bounded Cartesian target dragging for arms. Carry model and actuator semantics through calibration, native verification and packaging. Document reusable intake decisions and cover them with regression tests.
83 lines
2.7 KiB
Python
83 lines
2.7 KiB
Python
"""Generate operator documents for named native command axes."""
|
|
|
|
from dataclasses import dataclass
|
|
from pathlib import Path
|
|
|
|
import yaml
|
|
|
|
WALKING_CONTROLS = Path(__file__).with_name("walking.yaml")
|
|
|
|
|
|
@dataclass(frozen=True)
|
|
class AxisBinding:
|
|
source: str
|
|
sign: int
|
|
plus: str
|
|
minus: str
|
|
unit: str
|
|
positive: str
|
|
|
|
|
|
_VELOCITY = {
|
|
"lin_vel_x": AxisBinding("Ly", -1, "key_w", "key_s", "m/s", "forward"),
|
|
"lin_vel_y": AxisBinding("Lx", -1, "key_a", "key_d", "m/s", "left"),
|
|
"ang_vel_z": AxisBinding("Rx", -1, "key_j", "key_l", "rad/s", "turn left"),
|
|
}
|
|
|
|
|
|
def walking_controls(axes) -> dict:
|
|
"""Bind exactly the body-velocity axes the policy can observe."""
|
|
layout = yaml.safe_load(WALKING_CONTROLS.read_text(encoding="utf-8"))
|
|
term = layout["command"][0]
|
|
by_name = {axis["name"]: axis for axis in term["axes"]}
|
|
term["axes"] = [by_name[name] for name in axes]
|
|
for device in layout["devices"].values():
|
|
device["axes"] = {name: device["axes"][name] for name in axes}
|
|
return layout
|
|
|
|
|
|
def velocity_controls(axes, *, differential: bool = False) -> dict:
|
|
bindings = {axis: _VELOCITY[axis] for axis in axes}
|
|
if differential and "ang_vel_z" in bindings:
|
|
bindings["ang_vel_z"] = AxisBinding("Rx", -1, "key_a", "key_d", "rad/s", "turn left")
|
|
return controls_document(bindings, term="twist", feeds="velocity_commands", frame="body")
|
|
|
|
|
|
def controls_document(bindings: dict[str, AxisBinding], *, term: str, feeds: str, frame: str):
|
|
"""Build a document for validation through controller.controls.load_controls."""
|
|
return {
|
|
"schema_version": 2,
|
|
"command": [
|
|
{
|
|
"term": term,
|
|
"feeds": feeds,
|
|
"frame": frame,
|
|
"axes": [
|
|
{"name": axis, "unit": spec.unit, "positive": spec.positive}
|
|
for axis, spec in bindings.items()
|
|
],
|
|
"scaling": {"rule": "full_deflection_is_range_edge"},
|
|
}
|
|
],
|
|
"devices": {
|
|
"gamepad": {
|
|
"scheme": "absolute",
|
|
"layout": "xbox",
|
|
"deadzone": "device_reported_rescaled",
|
|
"release_button": "B",
|
|
"axes": {
|
|
axis: {"source": spec.source, "sign": spec.sign}
|
|
for axis, spec in bindings.items()
|
|
},
|
|
},
|
|
"keyboard": {
|
|
"scheme": "keys",
|
|
"full_after_s": 0.5,
|
|
"release": ["key_escape"],
|
|
"axes": {
|
|
axis: {"+": [spec.plus], "-": [spec.minus]} for axis, spec in bindings.items()
|
|
},
|
|
},
|
|
},
|
|
}
|