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
4 changes: 4 additions & 0 deletions forge/core/models.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
"""

Expand All @@ -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
Expand Down
22 changes: 20 additions & 2 deletions forge/formats/lerobot_v3/writer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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)

Expand All @@ -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),
}

Expand All @@ -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),
}

Expand Down
11 changes: 11 additions & 0 deletions forge/formats/mcap/extractors.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,))
Expand Down
41 changes: 40 additions & 1 deletion forge/formats/mcap/reader.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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) ----
Expand Down Expand Up @@ -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(),
)
Expand Down Expand Up @@ -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
Expand Down
49 changes: 49 additions & 0 deletions tests/test_lerobot_v3_writer.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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)
72 changes: 72 additions & 0 deletions tests/test_mcap_reader.py
Original file line number Diff line number Diff line change
Expand Up @@ -10,13 +10,15 @@

import logging
from pathlib import Path
from types import SimpleNamespace

import numpy as np
import pytest

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,
Expand Down Expand Up @@ -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]
Expand Down Expand Up @@ -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)
Expand Down
Loading