diff --git a/src/reflex/embodiments/__init__.py b/src/reflex/embodiments/__init__.py index 3b02882..2729270 100644 --- a/src/reflex/embodiments/__init__.py +++ b/src/reflex/embodiments/__init__.py @@ -1,4 +1,4 @@ -"""Per-embodiment configs (Franka, SO-100, UR5, Trossen, Stretch, custom). +"""Per-embodiment configs (Franka, SO-100, UR5, Trossen, Stretch, Quadcopter, custom). Read by `reflex serve --embodiment ` so the runtime knows the robot's action space, normalization stats, gripper layout, control rate, and safety @@ -77,11 +77,17 @@ class EmbodimentConfig: embodiment: str action_space: dict[str, Any] normalization: dict[str, list[float]] - gripper: dict[str, Any] cameras: list[dict[str, Any]] control: dict[str, float | int] constraints: dict[str, Any] + # Optional end-effector concepts. Arms have a gripper; drones have a + # payload_release; future embodiments may have neither, both, or other + # actuators (sprayer, gimbal). Schema requires neither — embodiments + # without an end-effector simply omit the field. + gripper: dict[str, Any] = field(default_factory=dict) + payload_release: dict[str, Any] = field(default_factory=dict) + # Where this config came from (for debugging + audit trail). Not part # of the schema; populated by the loader. _source_path: str = field(default="") @@ -118,7 +124,8 @@ def from_dict(cls, d: dict[str, Any], source_path: str = "") -> EmbodimentConfig embodiment=d["embodiment"], action_space=d["action_space"], normalization=d["normalization"], - gripper=d["gripper"], + gripper=d.get("gripper", {}), + payload_release=d.get("payload_release", {}), cameras=d["cameras"], control=control, constraints=d["constraints"], @@ -155,17 +162,26 @@ def load_custom(cls, path: str) -> EmbodimentConfig: return cls.from_dict(data, source_path=str(p)) def to_dict(self) -> dict[str, Any]: - """Serialize back to a dict matching the schema (drops _source_path).""" - return { + """Serialize back to a dict matching the schema (drops _source_path). + + Empty optional fields (gripper, payload_release) are omitted so the + output round-trips cleanly through the JSON schema's + `additionalProperties: false` constraint. + """ + out: dict[str, Any] = { "schema_version": self.schema_version, "embodiment": self.embodiment, "action_space": self.action_space, "normalization": self.normalization, - "gripper": self.gripper, "cameras": self.cameras, "control": self.control, "constraints": self.constraints, } + if self.gripper: + out["gripper"] = self.gripper + if self.payload_release: + out["payload_release"] = self.payload_release + return out @property def action_dim(self) -> int: @@ -178,8 +194,19 @@ def state_dim(self) -> int: (inferred from mean_state).""" return len(self.normalization["mean_state"]) + @property + def has_gripper(self) -> bool: + """True if this embodiment has a gripper end-effector.""" + return bool(self.gripper) + @property def gripper_idx(self) -> int: + """Index into the action vector that controls the gripper. + + Raises KeyError if this embodiment has no gripper — callers must + check `has_gripper` first when the embodiment can be a drone or + other gripper-less robot. + """ return int(self.gripper["component_idx"]) diff --git a/src/reflex/embodiments/presets/quadcopter.json b/src/reflex/embodiments/presets/quadcopter.json new file mode 100644 index 0000000..3918a6a --- /dev/null +++ b/src/reflex/embodiments/presets/quadcopter.json @@ -0,0 +1,48 @@ +{ + "schema_version": 1, + "embodiment": "quadcopter", + "action_space": { + "type": "continuous", + "dim": 5, + "ranges": [ + [-3.1416, 3.1416], + [-3.1416, 3.1416], + [-3.1416, 3.1416], + [0.0, 1.0], + [0.0, 1.0] + ] + }, + "normalization": { + "mean_action": [0.0, 0.0, 0.0, 0.5, 0.0], + "std_action": [1.0, 1.0, 1.0, 0.25, 0.5], + "mean_state": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + "std_state": [10.0, 10.0, 10.0, 0.5, 0.5, 0.5, 0.5, 5.0, 5.0, 5.0] + }, + "payload_release": { + "component_idx": 4, + "trigger_threshold": 0.5 + }, + "cameras": [ + { + "name": "front", + "resolution": [640, 480], + "fps": 30.0, + "color_space": "rgb8" + }, + { + "name": "downward", + "resolution": [640, 480], + "fps": 30.0, + "color_space": "rgb8" + } + ], + "control": { + "frequency_hz": 50.0, + "chunk_size": 20, + "rtc_execution_horizon": 10 + }, + "constraints": { + "max_ee_velocity": 5.0, + "collision_check": true + } +} diff --git a/src/reflex/embodiments/schema.json b/src/reflex/embodiments/schema.json index 6f9594e..7461ea8 100644 --- a/src/reflex/embodiments/schema.json +++ b/src/reflex/embodiments/schema.json @@ -9,7 +9,6 @@ "embodiment", "action_space", "normalization", - "gripper", "cameras", "control", "constraints" @@ -23,7 +22,7 @@ }, "embodiment": { "type": "string", - "enum": ["franka", "so100", "ur5", "trossen", "stretch", "custom"], + "enum": ["franka", "so100", "ur5", "trossen", "stretch", "quadcopter", "custom"], "description": "Embodiment slug. Must match the file name minus .json for presets." }, "action_space": { @@ -86,6 +85,7 @@ "type": "object", "required": ["component_idx", "close_threshold", "inverted"], "additionalProperties": false, + "description": "Optional. Present for embodiments with a gripper end-effector (arms). Omitted for drones and other gripper-less robots.", "properties": { "component_idx": { "type": "integer", @@ -104,6 +104,25 @@ } } }, + "payload_release": { + "type": "object", + "required": ["component_idx", "trigger_threshold"], + "additionalProperties": false, + "description": "Optional. Present for embodiments with a payload-release actuator (drones with delivery, etc.). Mutually exclusive with `gripper` in practice — an embodiment has one end-effector type.", + "properties": { + "component_idx": { + "type": "integer", + "minimum": 0, + "description": "Index into the action vector that controls payload release." + }, + "trigger_threshold": { + "type": "number", + "minimum": 0.0, + "maximum": 1.0, + "description": "Action value above which payload is released." + } + } + }, "cameras": { "type": "array", "minItems": 1, @@ -155,20 +174,20 @@ }, "constraints": { "type": "object", - "required": ["max_ee_velocity", "max_gripper_velocity", "collision_check"], + "required": ["max_ee_velocity", "collision_check"], "additionalProperties": false, "properties": { "max_ee_velocity": { "type": "number", "exclusiveMinimum": 0, "maximum": 10.0, - "description": "End-effector velocity cap (m/s)." + "description": "End-effector velocity cap (m/s for arms; flight speed for aerial)." }, "max_gripper_velocity": { "type": "number", "exclusiveMinimum": 0, "maximum": 10.0, - "description": "Gripper open/close velocity cap (rad/s or normalized)." + "description": "Gripper open/close velocity cap (rad/s or normalized). Required when `gripper` is present; otherwise omitted." }, "collision_check": { "type": "boolean", diff --git a/src/reflex/embodiments/validate.py b/src/reflex/embodiments/validate.py index e6f0f54..c9cbec8 100644 --- a/src/reflex/embodiments/validate.py +++ b/src/reflex/embodiments/validate.py @@ -145,20 +145,54 @@ def validate_cross_field(cfg: EmbodimentConfig) -> list[ValidationError]: } ) - # gripper.component_idx must be inside [0, action_dim) - grip_idx = cfg.gripper.get("component_idx", -1) - if not 0 <= grip_idx < action_dim: - errors.append( - { - "slug": "gripper-idx-out-of-range", - "severity": "error", - "field": "gripper.component_idx", - "message": ( - f"component_idx {grip_idx} is outside action_space " - f"[0, {action_dim})" - ), - } - ) + # gripper.component_idx must be inside [0, action_dim) — only checked + # when a gripper is declared. Drones omit the gripper block entirely. + if cfg.gripper: + grip_idx = cfg.gripper.get("component_idx", -1) + if not 0 <= grip_idx < action_dim: + errors.append( + { + "slug": "gripper-idx-out-of-range", + "severity": "error", + "field": "gripper.component_idx", + "message": ( + f"component_idx {grip_idx} is outside action_space " + f"[0, {action_dim})" + ), + } + ) + # When a gripper is declared, the runtime needs a per-gripper + # velocity cap separate from max_ee_velocity — otherwise SafetyLimits + # falls back to broadcasting max_ee_velocity across all dims. + if "max_gripper_velocity" not in cfg.constraints: + errors.append( + { + "slug": "gripper-missing-velocity-cap", + "severity": "error", + "field": "constraints.max_gripper_velocity", + "message": ( + "constraints.max_gripper_velocity is required when " + "a `gripper` block is present" + ), + } + ) + + # payload_release.component_idx must be inside [0, action_dim) — only + # checked when payload_release is declared. + if cfg.payload_release: + pr_idx = cfg.payload_release.get("component_idx", -1) + if not 0 <= pr_idx < action_dim: + errors.append( + { + "slug": "payload-release-idx-out-of-range", + "severity": "error", + "field": "payload_release.component_idx", + "message": ( + f"component_idx {pr_idx} is outside action_space " + f"[0, {action_dim})" + ), + } + ) # control.rtc_execution_horizon is an INTEGER count of actions (per # ADR 2026-04-25-auto-calibration-architecture decision #8 — the legacy diff --git a/src/reflex/safety/guard.py b/src/reflex/safety/guard.py index 5283667..aa91e7d 100644 --- a/src/reflex/safety/guard.py +++ b/src/reflex/safety/guard.py @@ -104,11 +104,19 @@ def from_embodiment_config(cls, cfg: Any) -> SafetyLimits: action_space = cfg.action_space ranges = action_space["ranges"] action_dim = int(action_space["dim"]) - gripper_idx = cfg.gripper_idx constraints = cfg.constraints max_ee_vel = float(constraints["max_ee_velocity"]) - max_gripper_vel = float(constraints["max_gripper_velocity"]) + # max_gripper_velocity is only present for embodiments with a gripper + # (arms). For drones and other gripper-less embodiments, broadcast + # max_ee_velocity across all action dims. gripper_idx = -1 ensures + # no axis matches the gripper-specific path below. + if cfg.has_gripper: + gripper_idx = cfg.gripper_idx + max_gripper_vel = float(constraints["max_gripper_velocity"]) + else: + gripper_idx = -1 + max_gripper_vel = max_ee_vel # unused when gripper_idx == -1 position_min = [float(r[0]) for r in ranges] position_max = [float(r[1]) for r in ranges] diff --git a/tests/test_embodiments.py b/tests/test_embodiments.py index cce08b1..53106bc 100644 --- a/tests/test_embodiments.py +++ b/tests/test_embodiments.py @@ -1,9 +1,11 @@ """Tests for per-embodiment configs (B.1). -Three test classes: -- TestEmbodimentLoading — all 3 shipped presets load via load_preset() +Test classes: +- TestEmbodimentLoading — all 4 shipped presets load via load_preset() - TestSchemaValidation — JSON-schema layer rejects malformed configs - TestCrossValidation — Python-side cross-field rules catch mismatches +- TestPresetSemantics — per-embodiment invariants +- TestGripperOptional — drones and other gripper-less embodiments Style mirrors tests/test_config.py. """ @@ -20,11 +22,13 @@ validate_embodiment_config, ) -ALL_PRESETS = ["franka", "so100", "ur5"] +ALL_PRESETS = ["franka", "quadcopter", "so100", "ur5"] +ARM_PRESETS = ["franka", "so100", "ur5"] +GRIPPERLESS_PRESETS = ["quadcopter"] # --------------------------------------------------------------------------- -# TestEmbodimentLoading — the 3 shipped presets must load + validate +# TestEmbodimentLoading — the 4 shipped presets must load + validate # --------------------------------------------------------------------------- @@ -69,6 +73,16 @@ def test_ur5_action_dim_seven(self): cfg = EmbodimentConfig.load_preset("ur5") assert cfg.action_dim == 7 + def test_quadcopter_specifics(self): + """Quadcopter preset: 5-DOF action (3 body rates + thrust + payload), + 10-DOF state (pos + quat + vel), 50 Hz, no gripper, has payload_release.""" + cfg = EmbodimentConfig.load_preset("quadcopter") + assert cfg.action_dim == 5 + assert cfg.state_dim == 10 + assert cfg.control["frequency_hz"] == 50.0 + assert not cfg.has_gripper + assert cfg.payload_release["component_idx"] == 4 + def test_to_dict_round_trip(self): original = EmbodimentConfig.load_preset("franka") d = original.to_dict() @@ -96,10 +110,15 @@ def test_valid_franka_passes_schema(self): assert errors == [] def test_schema_rejects_missing_required_field(self): + # `action_space` is required (unlike `gripper`, which became optional + # to support drones — see test_quadcopter_validates_without_gripper). cfg = EmbodimentConfig.load_preset("franka").to_dict() - del cfg["gripper"] + del cfg["action_space"] errors = validate_against_schema(cfg) - assert any("gripper" in e["message"] or e["field"] == "gripper" for e in errors) + assert any( + "action_space" in e["message"] or e["field"] == "action_space" + for e in errors + ) def test_schema_rejects_unknown_embodiment_enum(self): cfg = EmbodimentConfig.load_preset("franka").to_dict() @@ -265,18 +284,103 @@ def test_collision_check_enabled(self, name): cfg = EmbodimentConfig.load_preset(name) assert cfg.constraints["collision_check"] is True - @pytest.mark.parametrize("name", ALL_PRESETS) - def test_max_ee_velocity_capped(self, name): - """Sanity: presets must keep ee velocity under 2 m/s.""" + @pytest.mark.parametrize("name", ARM_PRESETS) + def test_arm_max_ee_velocity_capped(self, name): + """Sanity: arm presets must keep end-effector velocity under 2 m/s. + + Drones use the same field for flight speed and have a separate, + looser cap — see test_quadcopter_flight_speed_capped. + """ cfg = EmbodimentConfig.load_preset(name) assert 0 < cfg.constraints["max_ee_velocity"] <= 2.0 + def test_quadcopter_flight_speed_capped(self): + """Quadcopter `max_ee_velocity` is reused as flight-speed cap. + Hard ceiling: 6 m/s (~13 mph) — well under the schema cap of 10.""" + cfg = EmbodimentConfig.load_preset("quadcopter") + assert 0 < cfg.constraints["max_ee_velocity"] <= 6.0 + @pytest.mark.parametrize("name", ALL_PRESETS) def test_chunk_size_reasonable(self, name): """Sanity: chunk_size between 10 and 100 (matches model output range).""" cfg = EmbodimentConfig.load_preset(name) assert 10 <= cfg.control["chunk_size"] <= 100 + +# --------------------------------------------------------------------------- +# TestGripperOptional — gripper-less embodiments (drones, future) must +# round-trip cleanly through schema + cross-field + SafetyLimits without +# special-casing at every call site. +# --------------------------------------------------------------------------- + + +class TestGripperOptional: + def test_quadcopter_omits_gripper(self): + """The shipped quadcopter preset must NOT include a `gripper` block.""" + cfg = EmbodimentConfig.load_preset("quadcopter") + assert not cfg.has_gripper + assert "gripper" not in cfg.to_dict() + + def test_quadcopter_validates_without_gripper(self): + """Schema must accept a preset with no `gripper` block (and no + `max_gripper_velocity`).""" + cfg = EmbodimentConfig.load_preset("quadcopter") + ok, errors = validate_embodiment_config(cfg) + blocking = [e for e in errors if e["severity"] == "error"] + assert ok and not blocking, f"quadcopter validation failed: {blocking}" + + def test_gripper_idx_raises_when_no_gripper(self): + """Accessing `gripper_idx` on a gripper-less embodiment raises + KeyError — callers must check `has_gripper` first.""" + cfg = EmbodimentConfig.load_preset("quadcopter") + with pytest.raises(KeyError): + _ = cfg.gripper_idx + + def test_arms_unchanged_have_gripper(self): + """Regression: existing arm presets must still report has_gripper=True.""" + for name in ARM_PRESETS: + cfg = EmbodimentConfig.load_preset(name) + assert cfg.has_gripper, f"{name} lost its gripper after refactor" + assert "gripper" in cfg.to_dict() + + def test_payload_release_idx_out_of_range_caught(self): + d = EmbodimentConfig.load_preset("quadcopter").to_dict() + d["payload_release"]["component_idx"] = 99 # action_dim=5 + cfg = EmbodimentConfig.from_dict(d) + errors = validate_cross_field(cfg) + assert any(e["slug"] == "payload-release-idx-out-of-range" for e in errors) + + def test_gripper_present_requires_max_gripper_velocity(self): + """If a `gripper` block is present, `max_gripper_velocity` must also + be present — otherwise SafetyLimits would crash at runtime.""" + d = EmbodimentConfig.load_preset("franka").to_dict() + del d["constraints"]["max_gripper_velocity"] + cfg = EmbodimentConfig.from_dict(d) + errors = validate_cross_field(cfg) + assert any(e["slug"] == "gripper-missing-velocity-cap" for e in errors) + + def test_safety_limits_builds_for_quadcopter(self): + """Regression: SafetyLimits.from_embodiment_config must work for a + gripper-less embodiment — broadcasts max_ee_velocity across all dims.""" + from reflex.safety.guard import SafetyLimits + cfg = EmbodimentConfig.load_preset("quadcopter") + limits = SafetyLimits.from_embodiment_config(cfg) + assert len(limits.velocity_max) == cfg.action_dim + # All axes should get the same (max_ee_velocity) cap — no gripper axis. + max_ee = cfg.constraints["max_ee_velocity"] + assert all(v == max_ee for v in limits.velocity_max) + + def test_safety_limits_arm_gripper_axis_uses_gripper_cap(self): + """Regression for arms: the gripper axis still gets max_gripper_velocity.""" + from reflex.safety.guard import SafetyLimits + cfg = EmbodimentConfig.load_preset("franka") + limits = SafetyLimits.from_embodiment_config(cfg) + max_ee = cfg.constraints["max_ee_velocity"] + max_grip = cfg.constraints["max_gripper_velocity"] + for i, v in enumerate(limits.velocity_max): + expected = max_grip if i == cfg.gripper_idx else max_ee + assert v == expected, f"axis {i}: expected {expected}, got {v}" + @pytest.mark.parametrize("name", ALL_PRESETS) def test_state_dim_matches_norm_arrays(self, name): """Cross-check: state_dim accessor uses normalization arrays."""