never-merge: SimpleLidar — onboard L1 lidar re-expressed as the Mid-360

dimos/hardware/sensors/lidar/pointlio/mount_correction.py

@@ -52,7 +52,7 @@ def _transform_to_matrix(transform: Transform) -> np.ndarray:
     return matrix
 
 
-def _base_to_frame_matrix(loader: UrdfLoader, leaf_frame: str) -> np.ndarray:
+def base_to_frame_matrix(loader: UrdfLoader, leaf_frame: str) -> np.ndarray:
     """Compose the fixed-joint chain from the model root down to ``leaf_frame``."""
     static_transforms = loader.static_transforms
     chain: list[Transform] = []
@@ -73,9 +73,9 @@ def mount_correction_matrix(
     sensor_frame: str = "mid360_link",
 ) -> list[float]:
     """Row-major 4x4 (16 floats) mapping the ``original`` mount to the ``new`` one."""
-    original = _base_to_frame_matrix(
+    original = base_to_frame_matrix(
         UrdfLoader(name="original", model_path=original_static_tf), sensor_frame
     )
-    new = _base_to_frame_matrix(UrdfLoader(name="new", model_path=new_static_tf), sensor_frame)
+    new = base_to_frame_matrix(UrdfLoader(name="new", model_path=new_static_tf), sensor_frame)
     correction = np.linalg.inv(new) @ original
     return [float(value) for value in correction.flatten()]

dimos/robot/all_blueprints.py

@@ -117,10 +117,12 @@
     "unitree-go2-detection": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_detection:unitree_go2_detection",
     "unitree-go2-fleet": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_fleet:unitree_go2_fleet",
     "unitree-go2-keyboard-teleop": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_keyboard_teleop:unitree_go2_keyboard_teleop",
+    "unitree-go2-l1-lidar": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_l1_lidar:unitree_go2_l1_lidar",
     "unitree-go2-markers": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_markers",
     "unitree-go2-memory": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_memory",
     "unitree-go2-mid360-record": "dimos.robot.unitree.go2.blueprints.basic.unitree_go2_mid360_record:unitree_go2_mid360_record",
     "unitree-go2-nav-3d": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d",
+    "unitree-go2-nav-3d-rotated": "dimos.robot.unitree.go2.blueprints.navigation.unitree_go2_nav_3d:unitree_go2_nav_3d_rotated",
     "unitree-go2-relocalization": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2:unitree_go2_relocalization",
     "unitree-go2-ros": "dimos.robot.unitree.go2.blueprints.smart.unitree_go2_ros:unitree_go2_ros",
     "unitree-go2-security": "dimos.robot.unitree.go2.blueprints.agentic.unitree_go2_security:unitree_go2_security",
@@ -241,6 +243,7 @@
     "rerun-web-socket-server": "dimos.visualization.rerun.websocket_server.RerunWebSocketServer",
     "security-module": "dimos.experimental.security_demo.security_module.SecurityModule",
     "semantic-search": "dimos.memory2.module.SemanticSearch",
+    "simple-lidar": "dimos.robot.unitree.go2.simple_lidar.SimpleLidar",
     "simple-phone-teleop": "dimos.teleop.phone.phone_extensions.SimplePhoneTeleop",
     "simple-planner": "dimos.navigation.cmu_nav.modules.simple_planner.simple_planner.SimplePlanner",
     "spatial-memory": "dimos.perception.spatial_perception.SpatialMemory",

dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record_combined.py

@@ -0,0 +1,133 @@
+#!/usr/bin/env python3
+# 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.
+
+"""Drive-and-record blueprint for the Go2 + Mid-360 rig.
+
+Pygame WASD teleop drives the dog while Point-LIO odom+lidar, the Go2's lidar/odom,
+and the front camera are recorded into a memory2 db. The Go2/Mid-360 mount frames are
+published continuously onto tf so they're captured in the recording. Raw Livox capture
+is opt-in: set ``RECORD_PCAP=1`` to also record a .pcap of the Mid-360 UDP stream.
+
+The lidar IP comes from ``DIMOS_MID360_LIDAR_IP`` (shared by the Mid-360 / pcap
+capture and Point-LIO). Run it for a timestamped ``recordings/`` folder::
+
+    export DIMOS_MID360_LIDAR_IP=192.168.1.171
+    uv run python dimos/robot/unitree/go2/blueprints/basic/unitree_go2_mid360_record_combined.py
+"""
+
+from datetime import datetime
+import os
+from pathlib import Path
+
+import numpy as np
+
+from dimos.core.coordination.blueprints import autoconnect
+from dimos.core.coordination.module_coordinator import ModuleCoordinator
+from dimos.core.global_config import global_config
+from dimos.hardware.sensors.lidar.livox.module import Mid360
+from dimos.hardware.sensors.lidar.pointlio.module import PointLio
+from dimos.hardware.sensors.lidar.pointlio.mount_correction import base_to_frame_matrix
+from dimos.hardware.sensors.lidar.virtual_mid360.recorder import Mid360PcapRecorder
+from dimos.navigation.cmu_nav.frames import FRAME_ODOM
+from dimos.navigation.movement_manager.movement_manager import MovementManager
+from dimos.robot.unitree.go2.config import mid360_urdf_path
+from dimos.robot.unitree.go2.go2_mid360_recorder import Go2Mid360Recorder
+from dimos.robot.unitree.go2.go2_mid360_static_transforms import Go2Mid360StaticTf
+from dimos.robot.unitree.go2.simple_lidar import SimpleLidar
+from dimos.robot.unitree.keyboard_teleop import KeyboardTeleop
+from dimos.robot.urdf_loader import UrdfLoader
+from dimos.utils.logging_config import set_run_log_dir, setup_logger
+
+logger = setup_logger()
+
+# Opt-in raw-Livox pcap capture (default off). Set RECORD_PCAP=1 to include it.
+_RECORD_PCAP = os.getenv("RECORD_PCAP", "").lower() in ("1", "true", "yes", "on")
+
+_TELEOP_LINEAR_SPEED = 0.3
+_TELEOP_ANGULAR_SPEED = 0.6
+
+MID360_FRAME = "mid360_link"
+
+# SimpleLidar lands the Go2 lidar in base_link; re-express it in the Mid-360 frame
+# (inv(base_link -> mid360_link) from the mount URDF) so it lines up with Point-LIO's
+# cloud on the shared topic.
+_base_to_mid360 = base_to_frame_matrix(
+    UrdfLoader(name="go2_mid360_record", model_path=mid360_urdf_path), MID360_FRAME
+)
+_lidar_in_mid360 = [float(value) for value in np.linalg.inv(_base_to_mid360).flatten()]
+
+
+def _default_recording_dir() -> Path:
+    # Local time, with the machine's actual zone abbreviation (not a hardcoded PST).
+    now = datetime.now().astimezone()
+    stamp = (
+        now.strftime("%Y-%m-%d") + "_" + now.strftime("%I-%M%p").lower() + "-" + now.strftime("%Z")
+    )
+    return Path("recordings") / stamp
+
+
+unitree_go2_mid360_record_combined = autoconnect(
+    MovementManager.blueprint(),
+    # hack: pretend the go2 lidar is an extension of the mid360 lidar (transform it into that frame)
+    SimpleLidar.blueprint(transform=_lidar_in_mid360, output_frame=MID360_FRAME).remappings(
+        [
+            (SimpleLidar, "odom", "go2_odom"),
+            (SimpleLidar, "lidar", "pointlio_lidar"),
+        ]
+    ),
+    Mid360.blueprint().remappings(
+        [
+            (Mid360, "lidar", "livox_lidar"),
+            (Mid360, "imu", "livox_imu"),
+        ]
+    ),
+    PointLio.blueprint(frame_mapping={FRAME_ODOM: "world"}).remappings(
+        [
+            (PointLio, "lidar", "pointlio_lidar"),
+            (PointLio, "odometry", "pointlio_odometry"),
+        ]
+    ),
+    Go2Mid360Recorder.blueprint(),
+    # Continuously republishes the rig's mount frames onto tf (no latched static tf).
+    Go2Mid360StaticTf.blueprint(),
+    # Pygame keyboard teleop (WASD drive + Q/E strafe). Its cmd_vel feeds
+    # MovementManager's tele_cmd_vel.
+    KeyboardTeleop.blueprint(
+        linear_speed=_TELEOP_LINEAR_SPEED, angular_speed=_TELEOP_ANGULAR_SPEED
+    ).remappings(
+        [
+            (KeyboardTeleop, "cmd_vel", "tele_cmd_vel"),
+        ]
+    ),
+).global_config(n_workers=12, robot_model="unitree_go2")
+
+# Opt-in: also capture a raw .pcap of the Mid-360 UDP stream (RECORD_PCAP=1).
+if _RECORD_PCAP:
+    unitree_go2_mid360_record_combined = autoconnect(
+        unitree_go2_mid360_record_combined,
+        Mid360PcapRecorder.blueprint(),
+    )
+
+
+if __name__ == "__main__":
+    recording_dir = _default_recording_dir().resolve()
+    recording_dir.mkdir(parents=True, exist_ok=True)
+    set_run_log_dir(recording_dir)
+    global_config.obstacle_avoidance = False
+    coordinator = ModuleCoordinator.build(
+        unitree_go2_mid360_record_combined,
+        {Go2Mid360Recorder.name: {"db_path": str(recording_dir / "mem2.db")}},
+    )
+    coordinator.loop()

dimos/robot/unitree/go2/blueprints/navigation/unitree_go2_l1_lidar.py

@@ -0,0 +1,50 @@
+#!/usr/bin/env python3
+# 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.
+
+"""Go2 onboard (L1) lidar re-expressed in the Mid-360 frame.
+
+Sibling to ``unitree_go2_nav_3d``. Instead of the physical Mid-360 + Point-LIO,
+this runs ``SimpleLidar``: it pulls the Go2's onboard lidar over WebRTC, undoes
+the onboard world transform back to ``base_link``, then applies the static
+``base_link -> mid360_link`` transform (inverted) so the cloud looks like it was
+captured by the Mid-360. Downstream consumers calibrated for the Mid-360 mount
+therefore see the L1 lidar in the frame they expect.
+"""
+
+import numpy as np
+
+from dimos.core.coordination.blueprints import autoconnect
+from dimos.core.global_config import global_config
+from dimos.hardware.sensors.lidar.pointlio.mount_correction import base_to_frame_matrix
+from dimos.robot.unitree.go2.blueprints.basic.unitree_go2_basic import rerun_config
+from dimos.robot.unitree.go2.config import mid360_urdf_path
+from dimos.robot.unitree.go2.simple_lidar import SimpleLidar
+from dimos.robot.urdf_loader import UrdfLoader
+from dimos.visualization.vis_module import vis_module
+
+MID360_FRAME = "mid360_link"
+
+# The Go2 lidar lands in base_link after SimpleLidar undoes the onboard transform.
+# Re-express it in the Mid-360 frame: inv(base_link -> mid360_link) maps base_link
+# coordinates into mid360_link, and the cloud is stamped mid360_link to match.
+_base_to_mid360 = base_to_frame_matrix(
+    UrdfLoader(name="go2_mid360", model_path=mid360_urdf_path), MID360_FRAME
+)
+_lidar_in_mid360 = [float(value) for value in np.linalg.inv(_base_to_mid360).flatten()]
+
+unitree_go2_l1_lidar = autoconnect(
+    vis_module(viewer_backend=global_config.viewer, rerun_config=rerun_config),
+    SimpleLidar.blueprint(transform=_lidar_in_mid360, output_frame=MID360_FRAME),
+).global_config(n_workers=4, robot_model="unitree_go2")