Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
39 changes: 33 additions & 6 deletions src/reflex/embodiments/__init__.py
Original file line number Diff line number Diff line change
@@ -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 <name>` so the runtime knows the robot's
action space, normalization stats, gripper layout, control rate, and safety
Expand Down Expand Up @@ -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="")
Expand Down Expand Up @@ -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"],
Expand Down Expand Up @@ -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:
Expand All @@ -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"])


Expand Down
48 changes: 48 additions & 0 deletions src/reflex/embodiments/presets/quadcopter.json
Original file line number Diff line number Diff line change
@@ -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
}
}
29 changes: 24 additions & 5 deletions src/reflex/embodiments/schema.json
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@
"embodiment",
"action_space",
"normalization",
"gripper",
"cameras",
"control",
"constraints"
Expand All @@ -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": {
Expand Down Expand Up @@ -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",
Expand All @@ -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,
Expand Down Expand Up @@ -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",
Expand Down
62 changes: 48 additions & 14 deletions src/reflex/embodiments/validate.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
12 changes: 10 additions & 2 deletions src/reflex/safety/guard.py
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
Loading
Loading