From acdb6f5a0e6b70e420a327d7e529d00c12c1bd41 Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Mon, 7 Sep 2026 13:55:22 +0800 Subject: [PATCH 1/3] feat(simulation): opt-in ground-truth object pose stream for eval scoring MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit MujocoSimModule gains publish_ground_truth (default off) and ground_truth_hz (20 Hz). When enabled, world poses of every free-joint scene body (robot root excluded — odom already covers it) publish on a new gt_object_poses Out[PoseStamped] stream, frame_id = body name. Ground truth is privileged scoring data for the evals framework (issue #3594): never consumed by the agent. Part 1 of 2; part 2 adds the eval-side gt_store() and predicate library. --- dimos/simulation/engines/mujoco_sim_module.py | 73 ++++++++++++ .../engines/test_mujoco_sim_module.py | 110 +++++++++++++++++- 2 files changed, 182 insertions(+), 1 deletion(-) diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index 87ff5a0a21..e90dde801b 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -18,6 +18,8 @@ - camera streams (Out ports), replacing ``MujocoCamera`` - joint state via shared memory, consumed by ``ShmMujocoAdapter`` inside ``ControlCoordinator`` +- optionally, ground-truth world poses of free-joint scene bodies + (``publish_ground_truth``), for eval scoring only This avoids the prior pattern of sharing engines via a global in-process registry, which was fragile when ``WorkerManager`` places the adapter and @@ -84,6 +86,29 @@ def _find_sensor_slice(model: mujoco.MjModel, *names: str, dim: int = 3) -> slic _RX180 = R.from_euler("x", 180, degrees=True) +_MJJNT_FREE = int(mujoco.mjtJoint.mjJNT_FREE) # type: ignore[attr-defined] + + +def _resolve_gt_bodies(model: mujoco.MjModel, root_qpos_adr: int | None) -> list[tuple[str, int]]: + """(body name, qpos adr) for every free-joint body except the robot root. + + The robot root pose already flows on ``odom``; ground truth covers the + remaining scene bodies so eval scorers can check physical outcomes. + """ + bodies: list[tuple[str, int]] = [] + for joint_id in range(model.njnt): + if int(model.jnt_type[joint_id]) != _MJJNT_FREE: + continue + qpos_adr = int(model.jnt_qposadr[joint_id]) + if root_qpos_adr is not None and qpos_adr == root_qpos_adr: + continue + body_id = int(model.jnt_bodyid[joint_id]) + raw_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, body_id) or f"body_{body_id}" + # Attached submodels get a leading "/" namespace separator on newer + # MuJoCo; strip it so frame_id stays a stable plain body name. + bodies.append((raw_name.lstrip("/"), qpos_adr)) + return bodies + def _pose_matrix( position: NDArray[np.float64], rotation: NDArray[np.float64] @@ -251,6 +276,11 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): spawn_z: float | None = None spawn_yaw: float | None = None reset_joint_positions: list[float] | None = None + # Opt-in privileged ground truth for eval scoring: world poses of all + # free-joint scene bodies (robot root excluded, odom covers it). Never + # consumed by the agent. Off = zero overhead, zero behavior change. + publish_ground_truth: bool = False + ground_truth_hz: float = 20.0 headless: bool = False dof: int = 7 @@ -332,6 +362,10 @@ class MujocoSimModule( # root. Published every step; consumers like the viser viewer use # this to translate the robot in world space. odom: Out[PoseStamped] + # Ground-truth world poses of free-joint scene bodies, throttled to + # config.ground_truth_hz. Only publishes when config.publish_ground_truth + # is set; frame_id carries the MuJoCo body name. Eval scoring only. + gt_object_poses: Out[PoseStamped] tf: Out[TFMessage] def __init__(self, **kwargs: Any) -> None: @@ -360,6 +394,11 @@ def __init__(self, **kwargs: Any) -> None: self._root_base_qpos_adr: int | None = None self._root_spawn_clearance_z: float | None = None + # (body name, qpos adr) pairs for ground-truth publishing, resolved + # once at start. Stays empty unless config.publish_ground_truth. + self._gt_bodies: list[tuple[str, int]] = [] + self._gt_last_publish_monotonic = 0.0 + @property def _camera_link(self) -> str: return f"{self.config.camera_name}_link" @@ -551,6 +590,14 @@ def add_camera( self._imu_base_qpos_slice = None self._root_spawn_clearance_z = self._compute_root_spawn_clearance_z() + if self.config.publish_ground_truth: + self._gt_bodies = _resolve_gt_bodies(self._engine.model, self._root_base_qpos_adr) + logger.info( + "MujocoSimModule: ground-truth publishing enabled", + bodies=[name for name, _ in self._gt_bodies], + hz=self.config.ground_truth_hz, + ) + # Wire SHM bridge hooks. self._sim_hooks = _WholeBodySimHooks( self._shm, @@ -806,6 +853,32 @@ def _publish_shm_and_lcm(self, engine: MujocoEngine) -> None: ) ) + # Ground truth - throttled; ``_gt_bodies`` is empty unless + # config.publish_ground_truth, so the disabled path costs one check. + if self._gt_bodies: + now = time.monotonic() + if now - self._gt_last_publish_monotonic >= 1.0 / self.config.ground_truth_hz: + self._gt_last_publish_monotonic = now + ts = time.time() + for name, qpos_adr in self._gt_bodies: + body_pos = data.qpos[qpos_adr : qpos_adr + 3] + body_quat = data.qpos[qpos_adr + 3 : qpos_adr + 7] # (w, x, y, z) + self.gt_object_poses.publish( + PoseStamped( + ts=ts, + frame_id=name, + position=Vector3( + float(body_pos[0]), float(body_pos[1]), float(body_pos[2]) + ), + orientation=Quaternion( + float(body_quat[1]), + float(body_quat[2]), + float(body_quat[3]), + float(body_quat[0]), + ), # PoseStamped uses x,y,z,w + ) + ) + # IMU - only if MJCF declared the sensors. if ( self._imu_quat_slice is None diff --git a/dimos/simulation/engines/test_mujoco_sim_module.py b/dimos/simulation/engines/test_mujoco_sim_module.py index 3fa20048b5..470d3a5cc5 100644 --- a/dimos/simulation/engines/test_mujoco_sim_module.py +++ b/dimos/simulation/engines/test_mujoco_sim_module.py @@ -26,7 +26,11 @@ from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.simulation.engines.mujoco_engine import CameraFrame, MujocoEngine -from dimos.simulation.engines.mujoco_sim_module import MujocoSimModule, MujocoSimModuleConfig +from dimos.simulation.engines.mujoco_sim_module import ( + MujocoSimModule, + MujocoSimModuleConfig, + _resolve_gt_bodies, +) class _FakeData: @@ -564,3 +568,107 @@ def test_publish_loop_pacing_is_independent_of_frame_timestamp_magnitude(base_ts assert elapsed >= (len(frame_ts) - 1) / fps finally: module.stop() + + +def _make_post_step_module(config: MujocoSimModuleConfig) -> MujocoSimModule: + """Module with just enough state for _publish_shm_and_lcm().""" + module = MujocoSimModule() + module.config = config + module._root_base_qpos_adr = None + module._imu_quat_slice = None + module._imu_base_qpos_slice = None + module._imu_gyro_slice = None + module._imu_accel_slice = None + + class _FakeShm: + def signal_ready(self, *, num_joints: int, arm_joints: int) -> None: + pass + + def signal_stop(self) -> None: + pass + + def cleanup(self) -> None: + pass + + module._shm = _FakeShm() + return module + + +def test_gt_poses_not_published_when_disabled() -> None: + module = _make_post_step_module(MujocoSimModuleConfig(dof=2)) + try: + assert module.config.publish_ground_truth is False + published: list[Any] = [] + module.gt_object_poses.subscribe(published.append) + + module._publish_shm_and_lcm(_FakeEngine) + + assert published == [] + assert module._gt_bodies == [] + finally: + module.stop() + + +def test_gt_poses_published_and_throttled_when_enabled() -> None: + module = _make_post_step_module( + MujocoSimModuleConfig(dof=2, publish_ground_truth=True, ground_truth_hz=20.0) + ) + try: + module._gt_bodies = [("box_0", 0)] + published: list[Any] = [] + module.gt_object_poses.subscribe(published.append) + + module._publish_shm_and_lcm(_FakeEngine) + + assert len(published) == 1 + pose = published[0] + assert pose.frame_id == "box_0" + assert pose.position.to_numpy() == pytest.approx([0.0, 0.0, 0.75]) + assert ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) == pytest.approx((0.0, 0.0, 0.0, 1.0)) + + # Second step inside the 50ms throttle window is dropped. + module._publish_shm_and_lcm(_FakeEngine) + assert len(published) == 1 + + # Once the interval elapses, the next step publishes again. + module._gt_last_publish_monotonic -= 1.0 + module._publish_shm_and_lcm(_FakeEngine) + assert len(published) == 2 + finally: + module.stop() + + +@pytest.mark.mujoco +def test_resolve_gt_bodies_excludes_robot_root(tmp_path: Path) -> None: + scene_xml = tmp_path / "scene.xml" + robot_xml = tmp_path / "robot.xml" + _write_scene_xml(scene_xml) + _write_robot_xml(robot_xml) + + chair_001 = _scene_entity("chair_001") + chair_001["initial_pose"]["x"] = -1.0 # type: ignore[index] + + module = MujocoSimModule( + scene_xml=scene_xml, + robot_mjcf=robot_xml, + scene_entities=[_scene_entity("chair_000"), chair_001], + ) + try: + model = module._compose_model() + + bodies = _resolve_gt_bodies(model, root_qpos_adr=0) + + assert [name for name, _ in bodies] == ["entity:chair_000", "entity:chair_001"] + for _, qpos_adr in bodies: + assert qpos_adr > 0 + assert _resolve_gt_bodies(model, root_qpos_adr=None) == [ + ("base", 0), + *bodies, + ] + finally: + module.stop() From 8482f9c7fa2b434254be4ae7a2cc6e284fcb4769 Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Mon, 7 Sep 2026 15:43:09 +0800 Subject: [PATCH 2/3] feat(evals): ground-truth store, predicate library, xarm7 tabletop suite MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Part 2 of 2 for issue #3594, stacked on the MujocoSimModule GT stream. - GTRecorder: Recorder subclass subscribing /gt_object_poses over LCM into its own per-case db — ground truth stays out of the agent's recording. The eval runner deploys it in-process for cases declaring ground_truth=True and passes --mujoco-publish-ground-truth (new GlobalConfig flag) so any mujoco blueprint emits GT. - EvalRunner.gt_store() + two-arg GTScore scorers: score(store, gt) alongside the existing score(store); sample() wires the GT store in when the scorer asks for it. - dimos/evals/predicates.py: spatial predicates over the GT store (inside_region, lifted, ...); grasped() is a placeholder until the GT stream carries contact data. - suites/xarm7_tabletop.py: regression suite scoring pick/place against GT object poses. --- dimos/core/global_config.py | 3 + dimos/evals/gt_recorder.py | 41 +++++ dimos/evals/predicates.py | 130 ++++++++++++++++ dimos/evals/runner.py | 87 ++++++++++- dimos/evals/suites/xarm7_tabletop.py | 69 +++++++++ dimos/evals/test_evals.py | 54 ++++++- dimos/evals/test_mem2_wiring.py | 68 ++++++++ dimos/evals/test_predicates.py | 145 ++++++++++++++++++ dimos/evals/types.py | 34 +++- dimos/simulation/engines/mujoco_sim_module.py | 3 +- 10 files changed, 622 insertions(+), 12 deletions(-) create mode 100644 dimos/evals/gt_recorder.py create mode 100644 dimos/evals/predicates.py create mode 100644 dimos/evals/suites/xarm7_tabletop.py create mode 100644 dimos/evals/test_predicates.py diff --git a/dimos/core/global_config.py b/dimos/core/global_config.py index ff7b39edf2..68229ec0c3 100644 --- a/dimos/core/global_config.py +++ b/dimos/core/global_config.py @@ -99,6 +99,9 @@ class GlobalConfig(BaseSettings): mujoco_global_map_from_pointcloud: str | None = None mujoco_start_pos: str = "-1.0, 1.0" mujoco_steps_per_frame: int = 7 + # Global on-switch for MujocoSimModule's GT object pose stream — eval runs + # need it on blueprints whose module config doesn't set publish_ground_truth. + mujoco_publish_ground_truth: bool = False scene_package: str | None = None robot_model: str | None = None robot_id: str | None = None diff --git a/dimos/evals/gt_recorder.py b/dimos/evals/gt_recorder.py new file mode 100644 index 0000000000..a191ffe273 --- /dev/null +++ b/dimos/evals/gt_recorder.py @@ -0,0 +1,41 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""GT recorder: the simulator's privileged object poses, recorded for scoring. + +Subscribes ``/gt_object_poses`` (MujocoSimModule ``publish_ground_truth``) +over LCM and records it to its own db, separate from the agent-visible +recording — ground truth must never leak into the agent's memory. The eval +runner deploys this per case that declares ``ground_truth=True``; scorers +read it back through ``EvalRunner.gt_store()`` and +:mod:`dimos.evals.predicates`. +""" + +from __future__ import annotations + +from dimos.core.stream import In +from dimos.memory.module import Recorder +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + + +class GTRecorder(Recorder): + """Records the GT object pose stream to ``db_path``. + + GT poses are already world-frame (frame_id = body name, no tf anchor), + so deploy with ``poseless_streams=[predicates.GT_STREAM]`` and + ``record_tf=False``. The stream keeps the port name — the wiring layer + names topics after it. + """ + + gt_object_poses: In[PoseStamped] diff --git a/dimos/evals/predicates.py b/dimos/evals/predicates.py new file mode 100644 index 0000000000..32eb781484 --- /dev/null +++ b/dimos/evals/predicates.py @@ -0,0 +1,130 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Ground-truth predicates: physical outcomes from simulator truth, not agent memory. + +Interactive cases that declare ``ground_truth=True`` get a second store in +their score callable — the GT recorder's db of world poses the sim publishes +for every free-joint scene body (MujocoSimModule ``publish_ground_truth``). +Predicates here turn a role's pose history into a 0.0/1.0 outcome that +composes with the usual scorers and aggregates:: + + ROLES = {"cup": "cup", "apple": "apple"} + + def score(store: Store, gt: Store) -> float: + picked = lifted(gt, ROLES, "cup", min_delta=0.05) + collateral = displaced(gt, ROLES, "apple", threshold=0.10) + return picked * (1.0 - collateral) + + InteractiveEval(..., score=score, ground_truth=True, aggregate=final) + +``roles`` maps the case's semantic names to GT body names (the pose's +``frame_id``). All predicates raise LookupError while the GT stream has no +data for the role — the runner's sampler treats LookupError as "not yet, +keep waiting". +""" + +from __future__ import annotations + +from collections.abc import Mapping +from typing import TYPE_CHECKING + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped + +if TYPE_CHECKING: + from dimos.memory.store.base import Store + +GT_STREAM = "gt_object_poses" +"""Stream (and module port) the simulator's ground-truth poses flow on.""" + +# Body up-axis alignment below which a pose counts as toppled (cos 60°). +_UP_DOT_MIN = 0.5 + + +def gt_poses( + gt: Store, roles: Mapping[str, str], role: str, *, stream: str = GT_STREAM +) -> list[PoseStamped]: + """Time-ordered world poses of the body *role* maps to. + + The GT stream multiplexes every scene body on one stream; ``frame_id`` + carries the body name. + """ + body = roles[role] + poses = [obs.data for obs in gt.streams[stream] if obs.data.frame_id == body] + if not poses: + raise LookupError(f"no GT poses for role {role!r} (body {body!r}) on stream {stream!r}") + return poses + + +def lifted(gt: Store, roles: Mapping[str, str], role: str, *, min_delta: float) -> float: + """1.0 once the body's z rises ``min_delta`` above its episode-start z.""" + poses = gt_poses(gt, roles, role) + gain = max(p.position.z for p in poses) - poses[0].position.z + return float(gain > min_delta) + + +def displaced(gt: Store, roles: Mapping[str, str], role: str, *, threshold: float) -> float: + """1.0 once the body's xy distance from its episode-start xy exceeds + ``threshold`` — the knock-down/knock-aside detector.""" + poses = gt_poses(gt, roles, role) + x0, y0 = poses[0].position.x, poses[0].position.y + farthest = max(((p.position.x - x0) ** 2 + (p.position.y - y0) ** 2) ** 0.5 for p in poses) + return float(farthest > threshold) + + +def knocked_over(gt: Store, roles: Mapping[str, str], role: str) -> float: + """1.0 once the body's up axis tips more than 60° off world-up.""" + poses = gt_poses(gt, roles, role) + for p in poses: + up_z = p.orientation.to_rotation_matrix()[2, 2] + if up_z < _UP_DOT_MIN: + return 1.0 + return 0.0 + + +def near(gt: Store, roles: Mapping[str, str], role_a: str, role_b: str, *, dist: float) -> float: + """1.0 when the two bodies' latest positions are within ``dist`` (3D).""" + a, b = gt_poses(gt, roles, role_a)[-1], gt_poses(gt, roles, role_b)[-1] + return float((a.position - b.position).length() <= dist) + + +def contained( + gt: Store, + roles: Mapping[str, str], + role_a: str, + role_b: str, + *, + xy_tol: float = 0.1, +) -> float: + """1.0 when a sits inside b: within ``xy_tol`` of b's xy and above b's z. + + Pose-derived approximation — GT carries body poses, not shape extents, + so b's "footprint" is the tolerance disc around its center and its "top" + is its origin height. Size ``xy_tol`` to the container, not the content. + """ + a, b = gt_poses(gt, roles, role_a)[-1], gt_poses(gt, roles, role_b)[-1] + dx, dy = a.position.x - b.position.x, a.position.y - b.position.y + return float((dx**2 + dy**2) ** 0.5 <= xy_tol and a.position.z > b.position.z) + + +def grasped(gt: Store, roles: Mapping[str, str], role: str) -> float: + """Whether the gripper holds the object — placeholder. + + Not decidable from poses alone: a pose-only heuristic (object tracks the + end effector) also fires on pushes and drags. Blocked on the contact / + gripper-force channel from issue #3594 phase 2. + """ + raise NotImplementedError( + "grasped() needs the contact channel (issue #3594 phase 2); poses alone can't tell a grasp from a push" + ) diff --git a/dimos/evals/runner.py b/dimos/evals/runner.py index 59761dc92f..17dab077a1 100644 --- a/dimos/evals/runner.py +++ b/dimos/evals/runner.py @@ -22,23 +22,33 @@ from __future__ import annotations -from collections.abc import Callable, Mapping, Sequence +from collections.abc import Mapping, Sequence from dataclasses import asdict, dataclass, replace import json from pathlib import Path import subprocess import time -from typing import TYPE_CHECKING, Any +from typing import TYPE_CHECKING, Any, cast from dimos.constants import STATE_DIR from dimos.core.resource import CompositeResource -from dimos.evals.types import EvalCase, EvalResult, InteractiveEval, ResponseT, Suite +from dimos.evals.types import ( + EvalCase, + EvalResult, + GTScore, + InteractiveEval, + ResponseT, + Score, + Suite, + uses_gt_store, +) from dimos.protocol.service.spec import BaseConfig, Configurable from dimos.utils.logging_config import setup_logger if TYPE_CHECKING: from langchain_core.language_models.chat_models import BaseChatModel + from dimos.core.coordination.module_coordinator import ModuleCoordinator from dimos.e2e_tests.dim_sim_client import DimSimClient from dimos.e2e_tests.dimos_cli_call import DimosCliCall from dimos.memory.store.base import Store @@ -104,6 +114,8 @@ def __init__(self, **kwargs: Any) -> None: self._proc: DimosCliCall | None = None self._sim: DimSimClient | None = None self._run_dir: Path | None = None + self._gt_db: Path | None = None # current case's GT db, when ground_truth + self._gt_coordinator: ModuleCoordinator | None = None # -- run lifecycle ----------------------------------------------------------- @@ -206,6 +218,14 @@ def live_store(self) -> Store: return SqliteStore(path=self.config.live_db, must_exist=True) + def gt_store(self) -> Store: + """Store over the per-case GT db the GT recorder writes (ground_truth cases).""" + from dimos.memory.store.sqlite import SqliteStore + + if self._gt_db is None: + raise RuntimeError("gt_store() needs a case with ground_truth=True under run()") + return SqliteStore(path=str(self._gt_db), must_exist=True) + def encode(self, stream: Stream[Any, Any]) -> list[dict[str, Any]]: """mem2 Stream -> model-legible content blocks (the surface under test). @@ -345,10 +365,17 @@ def setup_env(self, case: InteractiveEval) -> None: proc = DimosCliCall() proc.simulator = case.simulator - proc.global_args = ["--dimsim-scene", case.scene] + # --dimsim-scene only exists for the dimsim launcher; mujoco sims + # name their scene in the blueprint itself. + if case.simulator == "dimsim": + proc.global_args = ["--dimsim-scene", case.scene] + if case.ground_truth: + proc.global_args.append("--mujoco-publish-ground-truth") proc.demo_args = ["run", *case.blueprint.split()] proc.start() self._proc = proc + if case.ground_truth: + self._start_gt_recorder(case) if not self._wait_mcp(self.config.launch_timeout_s): raise RuntimeError(f"MCP at {self.mcp_url} not ready — is dimos up?") if case.setup is not _no_setup: @@ -359,6 +386,29 @@ def setup_env(self, case: InteractiveEval) -> None: self._sim = sim case.setup(sim) + def _start_gt_recorder(self, case: InteractiveEval) -> None: + """Run a GT recorder blueprint in-process: subscribe the sim's GT + stream over LCM, record it to /.gt.db. + + The blueprint has no local Out for the recorder's In port — the + coordinator gives unmatched ports their ``/`` LCM topic, which + is exactly what the sim publishes on. + """ + from dimos.core.coordination.blueprints import autoconnect + from dimos.core.coordination.module_coordinator import ModuleCoordinator + from dimos.evals.gt_recorder import GTRecorder + from dimos.evals.predicates import GT_STREAM + + self._gt_db = self.run_dir / f"{case.id}.gt.db" + blueprint = autoconnect( + GTRecorder.blueprint( + db_path=self._gt_db, + record_tf=False, + poseless_streams=[GT_STREAM], + ) + ) + self._gt_coordinator = ModuleCoordinator.build(blueprint) + def teardown_env(self) -> None: """Per-case cleanup — the runner owns env lifecycle, cases just declare it.""" if self._sim is not None: @@ -367,6 +417,10 @@ def teardown_env(self) -> None: if self._proc is not None: self._proc.stop() self._proc = None + if self._gt_coordinator is not None: + self._gt_coordinator.stop() + self._gt_coordinator = None + self._gt_db = None def check_env(self, case: InteractiveEval) -> None: if self.config.attach or not case.simulator: @@ -396,18 +450,28 @@ def instruct(self, text: str) -> None: transport.stop() def sample( - self, score: Callable[[Store], float], interval_s: float, timeout_s: float + self, score: Score | GTScore, interval_s: float, timeout_s: float ) -> list[tuple[float, float]]: """Score the live Recorder store on an interval — the mem2 analogue of - lcm_spy.wait_until_odom_position, but it returns a graded series.""" + lcm_spy.wait_until_odom_position, but it returns a graded series. + + Two-arg scores additionally receive the GT store (the case declared + ``ground_truth=True``, so ``_start_gt_recorder`` ran in setup_env). + """ deadline = time.monotonic() + timeout_s t0 = time.monotonic() series: list[tuple[float, float]] = [] store = self._wait_live_store(deadline) + gt: Store | None = None try: + if uses_gt_store(score): + gt = self._wait_gt_store(deadline) while time.monotonic() < deadline: try: - value = score(store) + if gt is not None: + value = cast("GTScore", score)(store, gt) + else: + value = cast("Score", score)(store) except LookupError: value = None # stream not written yet — keep waiting if value is not None: @@ -417,6 +481,8 @@ def sample( time.sleep(interval_s) finally: store.stop() + if gt is not None: + gt.stop() return series def _wait_live_store(self, deadline: float) -> Store: @@ -425,6 +491,13 @@ def _wait_live_store(self, deadline: float) -> Store: time.sleep(1.0) return self.live_store() + def _wait_gt_store(self, deadline: float) -> Store: + """GT db appears once the GT recorder module starts — poll like the live db.""" + assert self._gt_db is not None, "two-arg score but no GT recorder — ground_truth=True?" + while not self._gt_db.exists() and time.monotonic() < deadline: + time.sleep(1.0) + return self.gt_store() + def _git_sha() -> str: try: diff --git a/dimos/evals/suites/xarm7_tabletop.py b/dimos/evals/suites/xarm7_tabletop.py new file mode 100644 index 0000000000..c8d561db2f --- /dev/null +++ b/dimos/evals/suites/xarm7_tabletop.py @@ -0,0 +1,69 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""xArm7 tabletop regression — the canonical case from issue #3594. + +"pick up the cup" on ``xarm-perception-sim-agent``: the grasp skill can +report success while the approach knocks the other objects off the table. +Agent-visible memory scores that a pass; the GT oracle fails the episode — +the cup must actually rise, and nothing else may move. + +The scene (LFS ``xarm7/scene.xml``) has three free-joint tabletop bodies — +``apple``, ``orange``, ``cup`` — which is what MujocoSimModule's +``publish_ground_truth`` stream (enabled here via the runner passing +``--mujoco-publish-ground-truth``) publishes world poses for. +""" + +from __future__ import annotations + +from typing import TYPE_CHECKING + +from dimos.evals.predicates import displaced, lifted +from dimos.evals.scorers import final +from dimos.evals.types import InteractiveEval, Suite + +if TYPE_CHECKING: + from dimos.memory.store.base import Store + +# Semantic role -> GT body name (the pose's frame_id in the GT stream). +ROLES = {"cup": "cup", "apple": "apple", "orange": "orange"} + +_LIFT_MIN = 0.05 # 5 cm — a held cup, not a nudge +_COLLATERAL_THRESHOLD = 0.10 # 10 cm xy — anything past this was struck + + +def _pick_without_collateral(store: Store, gt: Store) -> float: + """Full credit only when the cup lifted and every bystander stayed put.""" + picked = lifted(gt, ROLES, "cup", min_delta=_LIFT_MIN) + collateral = max( + displaced(gt, ROLES, bystander, threshold=_COLLATERAL_THRESHOLD) + for bystander in ("apple", "orange") + ) + return picked * (1.0 - collateral) + + +pick_up_cup = InteractiveEval( + id="xarm7_pick_up_cup_gt", + inputs="pick up the cup", + score=_pick_without_collateral, + aggregate=final, + ground_truth=True, + blueprint="xarm-perception-sim-agent", + simulator="mujoco", + interval_s=2.0, + timeout_s=300.0, + tags=frozenset({"manipulation", "gt"}), +) + +SUITE: Suite = [pick_up_cup] diff --git a/dimos/evals/test_evals.py b/dimos/evals/test_evals.py index ba0b2ed16e..57c64e83d2 100644 --- a/dimos/evals/test_evals.py +++ b/dimos/evals/test_evals.py @@ -89,6 +89,9 @@ def open_dataset(self, name: str) -> Any: def live_store(self) -> Any: raise NotImplementedError + def gt_store(self) -> Any: + raise NotImplementedError + def encode(self, stream: Any) -> list[dict[str, Any]]: return [{"type": "text", "text": f"{len(list(stream))} observations"}] @@ -253,6 +256,53 @@ def test_interactive_no_samples_is_error() -> None: assert "no samples" in case.evaluate(FakeRig()).error +# -- ground truth --------------------------------------------------------------------- + + +def test_uses_gt_store_arity() -> None: + from dimos.evals.types import uses_gt_store + + assert not uses_gt_store(lambda s: 1.0) + assert uses_gt_store(lambda s, gt: 1.0) + + def variadic(*args: Any) -> float: + return 1.0 + + assert uses_gt_store(variadic) + + +def test_two_arg_score_requires_ground_truth() -> None: + case = InteractiveEval(id="gt", inputs="x", score=lambda s, gt: 1.0, simulator="") + with pytest.raises(RuntimeError, match="ground_truth=True"): + case.preflight(FakeRig()) + + +def test_interactive_gt_dispatch() -> None: + case = InteractiveEval( + id="gt", + inputs="pick up the cup", + score=lambda s, gt: 1.0, + simulator="", + ground_truth=True, + ) + rig = FakeRig(series=[(0.0, 0.5), (1.0, 1.0)]) + result = case.evaluate(rig) + assert result.score == 1.0 # final aggregate + assert rig.calls == ["setup_env", "instruct:pick up the cup"] + + +def test_gt_recorder_blueprint_wires_in_port_without_local_out() -> None: + """The GT recorder blueprint has no Out matching its In — autoconnect must + still expose the port so the coordinator wires the /gt_object_poses topic.""" + pytest.importorskip("torch") # dimos.memory.module deps in minimal envs + from dimos.core.coordination.blueprints import autoconnect + from dimos.evals.gt_recorder import GTRecorder + + bp = autoconnect(GTRecorder.blueprint(db_path="gt.db")) + streams = [(s.name, s.direction) for atom in bp.blueprints for s in atom.streams] + assert ("gt_object_poses", "in") in streams + + # -- preflight ---------------------------------------------------------------------- @@ -355,9 +405,9 @@ def test_runner_encode_budget(dataset: str, tmp_path: Path) -> None: def test_suites_importable() -> None: """Suite modules construct without data or network (lambdas stay lazy).""" - from dimos.evals.suites import dimsim_house, examples, go2_smoke, go2_vqa + from dimos.evals.suites import dimsim_house, examples, go2_smoke, go2_vqa, xarm7_tabletop - for module in (examples, go2_smoke, go2_vqa, dimsim_house): + for module in (examples, go2_smoke, go2_vqa, dimsim_house, xarm7_tabletop): assert module.SUITE, module.__name__ diff --git a/dimos/evals/test_mem2_wiring.py b/dimos/evals/test_mem2_wiring.py index 07003f9db5..655c2ab055 100644 --- a/dimos/evals/test_mem2_wiring.py +++ b/dimos/evals/test_mem2_wiring.py @@ -206,3 +206,71 @@ def instruct(self, text: str) -> None: assert scores[-1] >= 0.99, "last sample sees the robot arrive (live data flowed)" assert r.score >= 0.99 # aggregate=final assert scores == sorted(scores), "monotonic approach must be visible in the series" + + +def test_interactive_two_arg_score_samples_gt_store(tmp_path: Path) -> None: + """A two-arg score receives the case's GT store alongside the live store, + and sees fresh GT observations as the (stubbed) recorder writes them.""" + from dimos.evals.predicates import GT_STREAM, lifted + + live_db = tmp_path / "live.db" + gt_db = tmp_path / "case.gt.db" + live_store = _open_store(live_db) + live_store.stream("odom", PoseStamped).append(_pose(0.0, 0.0), ts=time.time()) + gt_store = _open_store(gt_db) + gt_stream = gt_store.stream(GT_STREAM, PoseStamped) + roles = {"cup": "cup"} + stop = threading.Event() + + def writer() -> None: + # the cup rises off the table mid-episode + for i in range(1, 26): + if stop.is_set(): + return + pose = PoseStamped( + position=make_vector3(0.5, 0.0, 0.02 * i), + orientation=Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="cup", + ) + gt_stream.append(pose, ts=time.time()) + time.sleep(0.15) + + thread = threading.Thread(target=writer) + + class NoEnvRunner(EvalRunner): + def check_env(self, case: InteractiveEval) -> None: + pass + + def setup_env(self, case: InteractiveEval) -> None: + self._gt_db = gt_db # normally the GT recorder's db_path + thread.start() + + def instruct(self, text: str) -> None: + pass + + case = InteractiveEval( + id="gt_wiring", + inputs="pick up the cup", + score=lambda s, gt: lifted(gt, roles, "cup", min_delta=0.10), + aggregate=final, + ground_truth=True, + interval_s=0.1, + timeout_s=10.0, + simulator="", + ) + + runner = NoEnvRunner(live_db=str(live_db), out_dir=tmp_path / "evals") + try: + results = runner.run([case]) + finally: + stop.set() + if thread.ident is not None: + thread.join(timeout=5.0) + live_store.stop() + gt_store.stop() + + r = results[0] + assert not r.error, r.error + assert r.score == 1.0, "sampler must observe the cup crossing the lift threshold" + scores = [s for _, s in r.series] + assert 0.0 in scores, "early samples see the cup still on the table (GT data flowed)" diff --git a/dimos/evals/test_predicates.py b/dimos/evals/test_predicates.py new file mode 100644 index 0000000000..58f93938f4 --- /dev/null +++ b/dimos/evals/test_predicates.py @@ -0,0 +1,145 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Predicate library tests: a real SqliteStore, hand-built GT pose series.""" + +from __future__ import annotations + +from pathlib import Path +from typing import Any + +import pytest + +from dimos.evals.predicates import ( + GT_STREAM, + contained, + displaced, + grasped, + gt_poses, + knocked_over, + lifted, + near, +) +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.geometry_msgs.Quaternion import Quaternion +from dimos.msgs.geometry_msgs.Vector3 import Vector3, make_vector3 + +ROLES = {"cup": "cup", "bowl": "bowl"} + + +def _pose(x: float, y: float, z: float, orientation: Quaternion | None = None) -> PoseStamped: + return PoseStamped( + position=make_vector3(x, y, z), + orientation=orientation or Quaternion(0.0, 0.0, 0.0, 1.0), + frame_id="world", + ) + + +def _gt_store(tmp_path: Path, series: dict[str, list[PoseStamped]]) -> Any: + """A GT db with each body's pose series on the GT stream (frame_id = body).""" + from dimos.memory.store.sqlite import SqliteStore + + tmp_path.mkdir(parents=True, exist_ok=True) + try: + store = SqliteStore(path=str(tmp_path / "gt.db")) + except Exception as e: # pragma: no cover — sqlite-vec unavailable platforms + pytest.skip(f"SqliteStore unavailable: {e}") + stream = store.stream(GT_STREAM, PoseStamped) + for body, poses in series.items(): + for i, pose in enumerate(poses): + pose.frame_id = body + stream.append(pose, ts=1000.0 + i) + return store + + +def test_gt_poses_filters_by_role(tmp_path: Path) -> None: + store = _gt_store(tmp_path, {"cup": [_pose(0.5, 0.0, 0.1)], "bowl": [_pose(0.2, 0.1, 0.05)]}) + try: + poses = gt_poses(store, ROLES, "cup") + assert len(poses) == 1 and poses[0].position.x == 0.5 + finally: + store.stop() + + +def test_gt_poses_raises_until_data(tmp_path: Path) -> None: + store = _gt_store(tmp_path, {}) + try: + with pytest.raises(LookupError): # stream missing entirely + gt_poses(store, ROLES, "cup") + store.stream(GT_STREAM, PoseStamped).append(_pose(0.0, 0.0, 0.0), ts=1000.0) + with pytest.raises(LookupError): # stream live, but nothing for this body + gt_poses(store, ROLES, "cup") + finally: + store.stop() + + +def test_lifted_and_displaced(tmp_path: Path) -> None: + rising = [_pose(0.5, 0.0, 0.1), _pose(0.5, 0.0, 0.2)] + slid = [_pose(0.2, 0.1, 0.05), _pose(0.45, 0.1, 0.05)] + store = _gt_store(tmp_path, {"cup": rising, "bowl": slid}) + try: + assert lifted(store, ROLES, "cup", min_delta=0.05) == 1.0 + assert lifted(store, ROLES, "cup", min_delta=0.5) == 0.0 + assert displaced(store, ROLES, "cup", threshold=0.1) == 0.0 # straight up, no xy + assert displaced(store, ROLES, "bowl", threshold=0.1) == 1.0 + finally: + store.stop() + + +def test_knocked_over(tmp_path: Path) -> None: + on_side = Quaternion.from_euler(Vector3(1.5708, 0.0, 0.0)) # rolled 90° + store = _gt_store(tmp_path, {"cup": [_pose(0.5, 0.0, 0.1), _pose(0.5, 0.0, 0.05, on_side)]}) + try: + assert knocked_over(store, ROLES, "cup") == 1.0 + finally: + store.stop() + + store = _gt_store(tmp_path / "upright", {"cup": [_pose(0.5, 0.0, 0.1)]}) + try: + assert knocked_over(store, ROLES, "cup") == 0.0 + finally: + store.stop() + + +def test_near_and_contained(tmp_path: Path) -> None: + store = _gt_store( + tmp_path, + {"cup": [_pose(0.24, 0.0, 0.2)], "bowl": [_pose(0.2, 0.0, 0.05)]}, + ) + try: + assert near(store, ROLES, "cup", "bowl", dist=0.2) == 1.0 + assert near(store, ROLES, "cup", "bowl", dist=0.1) == 0.0 + # inside the bowl's tolerance disc, above its origin -> in + assert contained(store, ROLES, "cup", "bowl", xy_tol=0.1) == 1.0 + assert contained(store, ROLES, "cup", "bowl", xy_tol=0.01) == 0.0 + finally: + store.stop() + + below = _gt_store( + tmp_path / "below", + {"cup": [_pose(0.24, 0.0, 0.01)], "bowl": [_pose(0.2, 0.0, 0.05)]}, + ) + try: + assert contained(below, ROLES, "cup", "bowl", xy_tol=0.1) == 0.0 # under, not in + finally: + below.stop() + + +def test_grasped_is_a_phase2_placeholder(tmp_path: Path) -> None: + store = _gt_store(tmp_path, {"cup": [_pose(0.5, 0.0, 0.1)]}) + try: + with pytest.raises(NotImplementedError, match="contact"): + grasped(store, ROLES, "cup") + finally: + store.stop() diff --git a/dimos/evals/types.py b/dimos/evals/types.py index 0f9e32b601..f5ee40ea79 100644 --- a/dimos/evals/types.py +++ b/dimos/evals/types.py @@ -34,6 +34,7 @@ from abc import ABC, abstractmethod from collections.abc import Callable, Mapping, Sequence from dataclasses import dataclass, field +import inspect from typing import TYPE_CHECKING, Any, Generic, Protocol, TypeVar from pydantic import BaseModel @@ -55,6 +56,27 @@ lambda s: s.streams.odom.range_time(0, 600) """ +Score = Callable[["Store"], float] +"""Interactive scorer over the agent-visible live store (see InteractiveEval).""" + +GTScore = Callable[["Store", "Store"], float] +"""Interactive scorer that also reads the ground-truth store (second arg).""" + + +def uses_gt_store(score: Score | GTScore) -> bool: + """True when *score* takes the ground-truth store as a second argument.""" + try: + params = list(inspect.signature(score).parameters.values()) + except (TypeError, ValueError): # builtins without an introspectable signature + return False + positional = [ + p + for p in params + if p.kind in (inspect.Parameter.POSITIONAL_ONLY, inspect.Parameter.POSITIONAL_OR_KEYWORD) + ] + var_positional = any(p.kind is inspect.Parameter.VAR_POSITIONAL for p in params) + return len(positional) >= 2 or var_positional + @dataclass(frozen=True, kw_only=True) class EvalResult: @@ -80,6 +102,7 @@ def mcp_url(self) -> str: ... def open_dataset(self, name: str) -> Store: ... def live_store(self) -> Store: ... + def gt_store(self) -> Store: ... def encode(self, stream: Stream[Any, Any]) -> list[dict[str, Any]]: ... def ask(self, context: Sequence[dict[str, Any]], question: str) -> str: ... def ask_structured( @@ -95,7 +118,7 @@ def setup_env(self, case: InteractiveEval) -> None: ... def check_env(self, case: InteractiveEval) -> None: ... def instruct(self, text: str) -> None: ... def sample( - self, score: Callable[[Store], float], interval_s: float, timeout_s: float + self, score: Score | GTScore, interval_s: float, timeout_s: float ) -> list[tuple[float, float]]: ... @@ -176,13 +199,18 @@ class InteractiveEval(EvalCase): """Actions feed back into observations. The case names its environment so the eval is reproducible; the runner only decides attach-vs-launch.""" - score: Callable[[Store], float] # sampled every interval_s against live mem2 + # Sampled every interval_s against live mem2. Two-arg form also receives + # the ground-truth store (requires ground_truth=True). + score: Score | GTScore aggregate: Callable[[Sequence[float]], float] = final interval_s: float = 1.0 timeout_s: float = 300.0 blueprint: str = "unitree-go2-agentic" simulator: str = "dimsim" # "" = attach to a running dimos / real robot scene: str = "apartment" # --dimsim-scene name (ScenePackage name later) + # True: record the simulator's ground-truth object poses into a per-case + # gt db the score callable can read (see dimos.evals.predicates). + ground_truth: bool = False setup: Callable[[DimSimClient], None] = _no_setup def evaluate(self, rig: EvalRig) -> EvalResult: @@ -202,6 +230,8 @@ def evaluate(self, rig: EvalRig) -> EvalResult: def preflight(self, rig: EvalRig) -> None: rig.check_env(self) + if uses_gt_store(self.score) and not self.ground_truth: + raise RuntimeError(f"{self.id}: score reads the GT store — set ground_truth=True") Suite = Sequence[EvalCase] diff --git a/dimos/simulation/engines/mujoco_sim_module.py b/dimos/simulation/engines/mujoco_sim_module.py index e90dde801b..88da935377 100644 --- a/dimos/simulation/engines/mujoco_sim_module.py +++ b/dimos/simulation/engines/mujoco_sim_module.py @@ -279,6 +279,7 @@ class MujocoSimModuleConfig(ModuleConfig, DepthCameraConfig): # Opt-in privileged ground truth for eval scoring: world poses of all # free-joint scene bodies (robot root excluded, odom covers it). Never # consumed by the agent. Off = zero overhead, zero behavior change. + # The global --mujoco-publish-ground-truth flag enables this too. publish_ground_truth: bool = False ground_truth_hz: float = 20.0 headless: bool = False @@ -590,7 +591,7 @@ def add_camera( self._imu_base_qpos_slice = None self._root_spawn_clearance_z = self._compute_root_spawn_clearance_z() - if self.config.publish_ground_truth: + if self.config.publish_ground_truth or self.config.g.mujoco_publish_ground_truth: self._gt_bodies = _resolve_gt_bodies(self._engine.model, self._root_base_qpos_adr) logger.info( "MujocoSimModule: ground-truth publishing enabled", From 3143ca461cb948e8d20957fe04fdd4ddc1a71c63 Mon Sep 17 00:00:00 2001 From: Jerrybery Date: Thu, 10 Sep 2026 13:32:20 +0800 Subject: [PATCH 3/3] chore(robot): register GTRecorder in all_blueprints registry --- dimos/robot/all_blueprints.py | 1 + 1 file changed, 1 insertion(+) diff --git a/dimos/robot/all_blueprints.py b/dimos/robot/all_blueprints.py index c92c90c349..3ce6bbd2f3 100644 --- a/dimos/robot/all_blueprints.py +++ b/dimos/robot/all_blueprints.py @@ -241,6 +241,7 @@ "grasp-gen-x-module": "dimos.manipulation.grasping.grasp_gen_x.GraspGenXModule", "grasping-module": "dimos.manipulation.grasping.grasping.GraspingModule", "gstreamer-camera-module": "dimos.hardware.sensors.camera.gstreamer.gstreamer_camera.GstreamerCameraModule", + "gt-recorder": "dimos.evals.gt_recorder.GTRecorder", "hand-teleop-module": "dimos.teleop.quest.quest_extensions.HandTeleopModule", "heuristic-grasp-module": "dimos.manipulation.grasping.heuristic_grasp.HeuristicGraspModule", "hosted-stats-module": "dimos.teleop.hosted.hosted_stats.HostedStatsModule",