From 8e6a978416bdc42456dcadeb6e04212c1877bc88 Mon Sep 17 00:00:00 2001 From: Furkan Davulcu Date: Tue, 10 Nov 2026 18:10:46 +0100 Subject: [PATCH] feat(mcap,lerobot_v3): carry JointState joint names into info.json Co-Authored-By: Claude Code --- forge/core/models.py | 4 ++ forge/formats/lerobot_v3/writer.py | 22 ++++++++- forge/formats/mcap/extractors.py | 11 +++++ forge/formats/mcap/reader.py | 41 ++++++++++++++++- tests/test_lerobot_v3_writer.py | 49 ++++++++++++++++++++ tests/test_mcap_reader.py | 72 ++++++++++++++++++++++++++++++ 6 files changed, 196 insertions(+), 3 deletions(-) diff --git a/forge/core/models.py b/forge/core/models.py index e0d4f26..ebd8973 100644 --- a/forge/core/models.py +++ b/forge/core/models.py @@ -273,6 +273,8 @@ class Episode: cameras: Dictionary mapping camera names to CameraInfo. state_dim: Dimensionality of state vector. action_dim: Dimensionality of action vector. + state_names: Name of each state dimension, in order, when the source names them. + action_names: Name of each action dimension, in order, when the source names them. fps: Frames per second. """ @@ -288,6 +290,8 @@ class Episode: cameras: dict[str, CameraInfo] = field(default_factory=dict) state_dim: int | None = None action_dim: int | None = None + state_names: list[str] | None = None + action_names: list[str] | None = None fps: float | None = None # Frame access - lazy iterator diff --git a/forge/formats/lerobot_v3/writer.py b/forge/formats/lerobot_v3/writer.py index 638fe63..97c6b20 100644 --- a/forge/formats/lerobot_v3/writer.py +++ b/forge/formats/lerobot_v3/writer.py @@ -346,6 +346,21 @@ def _flush_chunk(self, output_path: Path) -> None: self._current_chunk_videos = {} self._episodes_in_current_chunk = 0 + def _vector_names( + self, episode: Episode, key: str, names: list[str] | None + ) -> list[str] | None: + """Return the names a state or action feature records. Every episode must agree, since each value is one joint.""" + names = list(names) if names else None + feature = self._features.get(key) + if feature is not None and feature["names"] != names: + raise ConversionError( + "source", + "lerobot-v3", + f"episode {episode.episode_id!r} lists {key} names {names}, but earlier episodes list " + f"{feature['names']}; convert episodes with the same joints into one dataset", + ) + return names + def write_episode( self, episode: Episode, @@ -395,6 +410,9 @@ def write_episode( self._flush_chunk(output_path) self._current_chunk_index = chunk_index + state_names = self._vector_names(episode, "observation.state", episode.state_names) + action_names = self._vector_names(episode, "action", episode.action_names) + # Track video frame index within the chunk's video file video_frame_offset = len(self._current_chunk_frames) @@ -420,7 +438,7 @@ def write_episode( self._features["observation.state"] = { "dtype": "float32", "shape": list(frame.state.shape), - "names": None, + "names": state_names, "fps": float(fps), } @@ -431,7 +449,7 @@ def write_episode( self._features["action"] = { "dtype": "float32", "shape": list(frame.action.shape), - "names": None, + "names": action_names, "fps": float(fps), } diff --git a/forge/formats/mcap/extractors.py b/forge/formats/mcap/extractors.py index c573d3d..c84c8ab 100644 --- a/forge/formats/mcap/extractors.py +++ b/forge/formats/mcap/extractors.py @@ -51,6 +51,17 @@ def _read_attr(msg: Any, candidates: tuple[str, ...]) -> Any: raise AttributeError(f"none of {candidates} found on {type(msg).__name__}") +# ROS2 JointState names its joints in .name, Foxglove proto in .joint_names. +_JOINT_NAME_FIELDS = ("name", "joint_names") + + +def extract_joint_names(msg: Any, schema_name: str) -> list[str]: + """Return the joint names a JointState (ROS2 or proto) lists, or an empty list for any other schema.""" + if schema_name not in _JOINT_STATE_SCHEMAS: + return [] + return [str(name) for name in _read_attr(msg, _JOINT_NAME_FIELDS)] + + def extract_joint_field(msg: Any, field: str, dtype: str | None) -> NDArray[Any]: """Pull position / velocity / effort from JointState (ROS2 or proto).""" candidates = _JOINT_STATE_FIELDS.get(field, (field,)) diff --git a/forge/formats/mcap/reader.py b/forge/formats/mcap/reader.py index 3f0ea70..2e42816 100644 --- a/forge/formats/mcap/reader.py +++ b/forge/formats/mcap/reader.py @@ -27,7 +27,7 @@ ) from forge.formats.mcap import episode_split, sync from forge.formats.mcap.decoders import DecodedMessage, iter_decoded -from forge.formats.mcap.extractors import extract +from forge.formats.mcap.extractors import extract, extract_joint_names from forge.formats.mcap.inspect import generate_config, inspect_mcap from forge.formats.mcap.topic_config import FieldMapping, TopicConfig from forge.formats.mcap.video_decode import decode_packet_stream, looks_like_video_format @@ -205,6 +205,8 @@ def read_episodes( cameras = self._infer_cameras(secondary_data) state_dim = self._infer_dim(secondary_data, "observation.state") action_dim = self._infer_dim(secondary_data, "action") + state_names = self._infer_joint_names(streams, config, "observation.state", state_dim) + action_names = self._infer_joint_names(streams, config, "action", action_dim) fps = self._infer_fps(primary_ts_ns) # ---- Task description (one-shot lookup) ---- @@ -240,6 +242,8 @@ def loader() -> Iterator[Frame]: cameras=cameras, state_dim=state_dim, action_dim=action_dim, + state_names=state_names, + action_names=action_names, fps=fps, _frame_loader=_make_loader(), ) @@ -500,6 +504,41 @@ def _infer_dim( return int(sample.size) return None + @staticmethod + def _infer_joint_names( + streams: dict[str, list[DecodedMessage]], + config: TopicConfig, + field_name: str, + dim: int | None, + ) -> list[str] | None: + """Return the joint names every message of a JointState field lists, or None when none list any. + + Values are read by index, so a stream whose names change between messages is refused. + """ + mapping = config.fields.get(field_name) + if mapping is None: + return None + topic = mapping.topic + distinct = { + tuple(extract_joint_names(m.message, m.schema_name)) for m in streams.get(topic, []) + } + if distinct <= {()}: + return None + if len(distinct) > 1: + first, second = sorted(distinct)[:2] + raise ValueError( + f"{topic!r} lists joint names {list(first)} in one message and {list(second)} in " + f"another; {field_name} values are read by index, so record the topic with a fixed " + "joint list" + ) + names = list(distinct.pop()) + if len(names) != dim or len(set(names)) != len(names): + raise ValueError( + f"{topic!r} lists {len(names)} joint names {names} for {dim} {field_name} values; " + "each value needs one unique name" + ) + return names + def _infer_fps(self, primary_ts_ns: list[int]) -> float | None: if len(primary_ts_ns) < 2: return None diff --git a/tests/test_lerobot_v3_writer.py b/tests/test_lerobot_v3_writer.py index c76e0ff..0ace354 100644 --- a/tests/test_lerobot_v3_writer.py +++ b/tests/test_lerobot_v3_writer.py @@ -6,6 +6,7 @@ import numpy as np import pytest +from forge.core.exceptions import ConversionError from forge.core.models import CameraInfo, Episode, Frame, LazyImage from forge.formats.lerobot_v3.writer import LeRobotV3Writer, LeRobotV3WriterConfig @@ -817,3 +818,51 @@ def png(value: int) -> bytes: assert features[cam_col]["dtype"] == "image" assert features[cam_col]["shape"] == [8, 12, 3] assert features["observation.state"]["shape"] == [3] + + +JOINTS = ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"] + + +@pytest.mark.skipif( + not _check_image_mode_dependencies_available(), + reason="PIL or PyArrow not installed", +) +class TestLeRobotV3WriterVectorNames: + """State and action joint names reach info.json, and every episode must agree on them.""" + + def _write(self, tmp_path: Path, episodes: list[Episode]) -> dict: + config = LeRobotV3WriterConfig(fps=30.0, robot_type="test_robot", video=False) + LeRobotV3Writer(config).write_dataset(iter(episodes), tmp_path / "output") + with open(tmp_path / "output" / "meta" / "info.json") as f: + return json.load(f)["features"] + + def test_names_are_written(self, tmp_path: Path, sample_episodes: list[Episode]): + for episode in sample_episodes: + episode.state_names = list(JOINTS) + episode.action_names = list(reversed(JOINTS)) + features = self._write(tmp_path, sample_episodes) + assert features["observation.state"]["names"] == JOINTS + assert features["action"]["names"] == list(reversed(JOINTS)) + + def test_unnamed_episodes_write_null_names( + self, tmp_path: Path, sample_episodes: list[Episode] + ): + features = self._write(tmp_path, sample_episodes) + assert features["observation.state"]["names"] is None + assert features["action"]["names"] is None + + def test_episodes_that_disagree_on_names_are_refused( + self, tmp_path: Path, sample_episodes: list[Episode] + ): + for episode in sample_episodes: + episode.state_names = list(JOINTS) + sample_episodes[1].state_names = list(reversed(JOINTS)) + with pytest.raises(ConversionError, match="episode 'ep_001' lists observation.state names"): + self._write(tmp_path, sample_episodes) + + def test_a_named_episode_after_an_unnamed_one_is_refused( + self, tmp_path: Path, sample_episodes: list[Episode] + ): + sample_episodes[1].state_names = list(JOINTS) + with pytest.raises(ConversionError, match="earlier episodes list None"): + self._write(tmp_path, sample_episodes) diff --git a/tests/test_mcap_reader.py b/tests/test_mcap_reader.py index 5df8596..74c516a 100644 --- a/tests/test_mcap_reader.py +++ b/tests/test_mcap_reader.py @@ -10,6 +10,7 @@ import logging from pathlib import Path +from types import SimpleNamespace import numpy as np import pytest @@ -17,6 +18,7 @@ pytest.importorskip("mcap") from forge.formats.mcap import MCAPReader, generate_config # noqa: E402 +from forge.formats.mcap.decoders import DecodedMessage # noqa: E402 from forge.formats.mcap.topic_config import ( # noqa: E402 EpisodeSplit, FieldMapping, @@ -95,6 +97,11 @@ def test_state_and_action_are_numpy(self): assert isinstance(f.action, np.ndarray) assert f.action.shape == (8,) + def test_proto_joint_states_without_names_give_none(self): + ep = next(MCAPReader().read_episodes(TROSSEN, config=_trossen_config())) + assert ep.state_names is None + assert ep.action_names is None + def test_images_are_lazy(self): ep = next(MCAPReader().read_episodes(TROSSEN, config=_trossen_config())) f = ep.load_frames()[5] @@ -141,6 +148,71 @@ def test_reads_with_generated_config(self): assert isinstance(f0.state, np.ndarray) assert f0.state.shape == (4,) + def test_episode_carries_the_joint_state_names(self): + ep = next(iter(MCAPReader().read_episodes(ROS2))) + assert ep.state_names == ["shoulder_joint", "elbow_joint", "wrist_joint", "gripper_joint"] + + +class TestJointNames: + """MCAPReader._infer_joint_names on hand-built JointState streams.""" + + @staticmethod + def _names(per_message: list[list[str]], dim: int | None = 2) -> list[str] | None: + stream = [ + DecodedMessage( + topic="/joint_states", + timestamp_ns=i, + publish_time_ns=i, + schema_name="sensor_msgs/msg/JointState", + schema_encoding="ros2msg", + message=SimpleNamespace(name=names, position=[0.0] * len(names)), + ) + for i, names in enumerate(per_message) + ] + config = TopicConfig( + source=ROS2, + fields={"observation.state": FieldMapping(topic="/joint_states", field="position")}, + ) + return MCAPReader._infer_joint_names( + {"/joint_states": stream}, config, "observation.state", dim + ) + + def test_a_non_joint_state_stream_gives_none(self): + stream = [ + DecodedMessage( + topic="/command", + timestamp_ns=0, + publish_time_ns=0, + schema_name="std_msgs/msg/Float64MultiArray", + schema_encoding="ros2msg", + message=SimpleNamespace(data=[0.0, 0.0]), + ) + ] + config = TopicConfig( + source=ROS2, fields={"action": FieldMapping(topic="/command", field="data")} + ) + assert MCAPReader._infer_joint_names({"/command": stream}, config, "action", 2) is None + + def test_a_field_absent_from_the_config_gives_none(self): + config = TopicConfig(source=ROS2, fields={}) + assert MCAPReader._infer_joint_names({}, config, "action", None) is None + + def test_names_that_change_between_messages_are_refused(self): + with pytest.raises(ValueError, match="in one message and"): + self._names([["a", "b"], ["b", "a"]]) + + def test_a_stream_mixing_named_and_unnamed_messages_is_refused(self): + with pytest.raises(ValueError, match="in one message and"): + self._names([["a", "b"], []]) + + def test_names_that_do_not_match_the_values_are_refused(self): + with pytest.raises(ValueError, match="each value needs one unique name"): + self._names([["a", "b"]], dim=3) + + def test_duplicate_names_are_refused(self): + with pytest.raises(ValueError, match="each value needs one unique name"): + self._names([["a", "a"]]) + # --------------------------------------------------------------------------- # Galileo — images only, no JointState (state_dim should be None)