diff --git a/data/.lfs/go2_china_office_indoor.db.tar.gz b/data/.lfs/go2_china_office_indoor.db.tar.gz new file mode 100644 index 0000000000..03926f3fde --- /dev/null +++ b/data/.lfs/go2_china_office_indoor.db.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:86602c4d1df5510835c132cc7929009fc7372cb1d06c4172522de082fb892e2d +size 1718197739 diff --git a/data/.lfs/go2_china_office_indoor.mcap.tar.gz b/data/.lfs/go2_china_office_indoor.mcap.tar.gz new file mode 100644 index 0000000000..587e5af3c9 --- /dev/null +++ b/data/.lfs/go2_china_office_indoor.mcap.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:527d4ded7fdecc0423cba6a7e50d17148f280dfb5a988b1e74f40d896369e516 +size 1670777611 diff --git a/data/.lfs/go2_china_office_indoor_bestz.txt.tar.gz b/data/.lfs/go2_china_office_indoor_bestz.txt.tar.gz new file mode 100644 index 0000000000..7a77d63620 --- /dev/null +++ b/data/.lfs/go2_china_office_indoor_bestz.txt.tar.gz @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:56aeb9d72ab31c5c451c8b721e4a7be1d076dcc356ff36b8d750a314a8907fea +size 4813355 diff --git a/dimos/mapping/utils/cli/map.py b/dimos/mapping/utils/cli/map.py index f53f69923a..f375388e4e 100644 --- a/dimos/mapping/utils/cli/map.py +++ b/dimos/mapping/utils/cli/map.py @@ -164,6 +164,7 @@ def _progress(obs: Observation[Any]) -> None: def _log_reconstruction( *, voxel: float, + cubes: bool = False, global_map: PointCloud2 | None, path: list[tuple[float, float, float]], pgo_map: PointCloud2 | None, @@ -178,8 +179,12 @@ def _log_reconstruction( from dimos.msgs.geometry_msgs.Transform import Transform rr.send_blueprint(rrb.Blueprint(rrb.Spatial3DView(origin="world"))) + # Cubes tile the grid at full voxel size; dots (spheres) stay at half. + map_kw: dict[str, Any] = ( + {"voxel_size": voxel, "mode": "boxes"} if cubes else {"voxel_size": voxel / 2} + ) if global_map is not None: - rr.log("world/raw_map/pointcloud", global_map.to_rerun(voxel_size=voxel / 2), static=True) + rr.log("world/raw_map/pointcloud", global_map.to_rerun(**map_kw), static=True) if path: rr.log( "world/raw_map/path", @@ -187,11 +192,11 @@ def _log_reconstruction( static=True, ) if pgo_map is not None: - rr.log("world/pgo_map/pointcloud", pgo_map.to_rerun(voxel_size=voxel / 2), static=True) + rr.log("world/pgo_map/pointcloud", pgo_map.to_rerun(**map_kw), static=True) if full_pgo_map is not None: rr.log( "world/full_pgo_map/pointcloud", - full_pgo_map.to_rerun(voxel_size=voxel / 2), + full_pgo_map.to_rerun(**map_kw), static=True, ) if pgo_path: @@ -292,6 +297,9 @@ def main( None, "--duration", help="Use only N seconds from --seek (default: to the end)" ), voxel: float = typer.Option(0.05, "--voxel", help="Voxel size for the rebuild"), + cubes: bool = typer.Option( + False, "--cubes", help="Render the map as voxel-sized cubes instead of dots" + ), device: str = typer.Option( "CUDA:0", "--device", help="Open3D compute device (e.g. CUDA:0, CPU:0)" ), @@ -523,6 +531,7 @@ def main( rr.save(str(out)) _log_reconstruction( voxel=voxel, + cubes=cubes, global_map=global_map, path=path, pgo_map=pgo_map, diff --git a/dimos/mapping/utils/cli/replay.py b/dimos/mapping/utils/cli/replay.py index 79ef56c848..d11e9588da 100644 --- a/dimos/mapping/utils/cli/replay.py +++ b/dimos/mapping/utils/cli/replay.py @@ -164,8 +164,10 @@ def main( "--voxel", help="Voxel grid resolution (m) for --map/--map-final; rendering follows the same size", ), - point_mode: str = typer.Option( - "spheres", "--point-mode", help="Render mode: 'spheres', 'boxes', or 'points'" + cube: bool = typer.Option( + False, + "--cube", + help="Render the accumulated --map/--map-final as voxel-sized cubes instead of spheres", ), camera_hz: float = typer.Option( 0.0, @@ -201,6 +203,7 @@ def main( "--map-emit-every", help="Emit accumulated map every N frames (0 = only at end); --map only", ), + no_image: bool = typer.Option(False, "--no-image", help="Skip logging color_image frames"), ) -> None: """Dump a recording to .rrd (lidar clouds + camera frames) and open it in rerun.""" from dimos.mapping.utils.cli.summary import _stream_payload_types @@ -232,9 +235,10 @@ def main( # Static pinhole on the camera entity; per-frame Transform3D goes on the # same entity. Image is the child so it projects through the pinhole. - pinhole = cam_info.to_rerun() - assert not isinstance(pinhole, list) - rr.log("world/camera", pinhole, static=True) + if not no_image: + pinhole = cam_info.to_rerun() + assert not isinstance(pinhole, list) + rr.log("world/camera", pinhole, static=True) # Static axis triads as children of each moving Transform3D, so the # transforms are actually visible in the 3D view. @@ -257,10 +261,10 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: has_livox = "fastlio_lidar" in store.streams livox = clipped("fastlio_lidar", PointCloud2) if has_livox else None - # ---- per-frame raw clouds ---- - _log_clouds(" lidar", lidar, "world/lidar", voxel, point_mode) + # ---- per-frame raw clouds (always spheres) ---- + _log_clouds(" lidar", lidar, "world/lidar", voxel, "spheres") if livox is not None: - _log_clouds("fastlio_lidar", livox, "world/fastlio_lidar", voxel, point_mode) + _log_clouds("fastlio_lidar", livox, "world/fastlio_lidar", voxel, "spheres") # ---- accumulated voxel maps over the selected PointCloud2 streams ---- # --map logs a growing map per stream; --map-final logs one static map @@ -268,6 +272,10 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: # voxel (good for forward-facing lidar like the Go2 L1); off by default. if map or map_final: grid_kwargs = {"voxel_size": voxel, "device": map_device, "show_startup_log": False} + # --cube tiles the grid at the true voxel size; spheres render + # smaller so the gaps between them read as transparency. + map_mode = "boxes" if cube else "spheres" + map_render = voxel if cube else voxel / 4 for name in map_sources: src = clipped(name, PointCloud2) if not src.exists(): @@ -283,8 +291,8 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: ) ), f"world/{name}_voxels", - voxel / 4, # render smaller than the grid → gaps read as transparency - point_mode, + map_render, + map_mode, total=max(1, src.count() // max(map_emit_every, 1)), ) if map_final: @@ -296,7 +304,7 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: ).last() rr.log( f"world/{name}_map", - final.data.to_rerun(voxel_size=voxel / 4, mode=point_mode), + final.data.to_rerun(voxel_size=map_render, mode=map_mode), static=True, ) @@ -349,23 +357,24 @@ def clipped(name: str, ptype: type[Any]) -> Stream[Any]: ) # ---- pass 2: camera pose + image per color_image ---- - cam_pipeline = ( - color_image.transform(throttle(1.0 / camera_hz)) if camera_hz > 0 else color_image - ) - n_img = cam_pipeline.count() - cb = _progress(n_img, " color_image") - for img_obs in cam_pipeline: - cb(img_obs) - rr.set_time(TIMELINE, timestamp=img_obs.ts) - if img_obs.pose_tuple is not None: - x, y, z, qx, qy, qz, qw = img_obs.pose_tuple - rr.log( - "world/camera", - rr.Transform3D( - translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]) - ), - ) - rr.log("world/camera/image", img_obs.data.to_rerun()) + if not no_image: + cam_pipeline = ( + color_image.transform(throttle(1.0 / camera_hz)) if camera_hz > 0 else color_image + ) + n_img = cam_pipeline.count() + cb = _progress(n_img, " color_image") + for img_obs in cam_pipeline: + cb(img_obs) + rr.set_time(TIMELINE, timestamp=img_obs.ts) + if img_obs.pose_tuple is not None: + x, y, z, qx, qy, qz, qw = img_obs.pose_tuple + rr.log( + "world/camera", + rr.Transform3D( + translation=[x, y, z], quaternion=rr.Quaternion(xyzw=[qx, qy, qz, qw]) + ), + ) + rr.log("world/camera/image", img_obs.data.to_rerun()) print(f"wrote {out}") if no_gui: diff --git a/dimos/mapping/utils/cli/replay_marker.py b/dimos/mapping/utils/cli/replay_marker.py index c9766dab6d..ed95537313 100644 --- a/dimos/mapping/utils/cli/replay_marker.py +++ b/dimos/mapping/utils/cli/replay_marker.py @@ -56,6 +56,11 @@ def main( None, "--duration", help="Use only N seconds from --seek (default: to the end)" ), marker_size: float = typer.Option(0.1, "--marker-size", help="Marker edge length (m)"), + camera_info: Path | None = typer.Option( + None, + "--camera-info", + help="YAML calibration file for detection; defaults to Go2 builtin", + ), marker_max_speed: float = typer.Option( 0.5, "--marker-max-speed", help="Detection speed gate (m/s); 0 disables" ), @@ -73,6 +78,7 @@ def main( from dimos.memory2.store.sqlite import SqliteStore from dimos.memory2.transform import QualityWindow, SpeedLimit from dimos.memory2.vis.color import Color + from dimos.msgs.sensor_msgs.CameraInfo import CameraInfo from dimos.msgs.sensor_msgs.Image import Image from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 from dimos.perception.fiducial.marker_transformer import DetectMarkers @@ -82,7 +88,7 @@ def main( db_path = resolve_named_path(dataset, ".db") if out is None: out = Path.cwd() / f"{db_path.stem}.rrd" - cam_info = _camera_info_static() + cam_info = CameraInfo.from_yaml(str(camera_info)) if camera_info else _camera_info_static() rr.init("dimos markers", recording_id=db_path.stem) rr.save(str(out)) diff --git a/dimos/robot/unitree/go2/front_camera_1080.yaml b/dimos/robot/unitree/go2/front_camera_1080.yaml new file mode 100644 index 0000000000..1213cb20b0 --- /dev/null +++ b/dimos/robot/unitree/go2/front_camera_1080.yaml @@ -0,0 +1,54 @@ +image_width: 1920 +image_height: 1080 +camera_name: go2_front_camera_1080p +distortion_model: equidistant +camera_matrix: + rows: 3 + cols: 3 + data: + - 1196.2134247297394 + - 0.0 + - 965.3028251731779 + - 0.0 + - 1194.7308169154975 + - 523.917540801463 + - 0.0 + - 0.0 + - 1.0 +distortion_coefficients: + rows: 1 + cols: 4 + data: + - -0.07309428880537933 + - -0.02341140740909078 + - -0.0069305931780026956 + - 0.009238684474464793 +rectification_matrix: + rows: 3 + cols: 3 + data: + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 + - 0.0 + - 0.0 + - 1.0 +projection_matrix: + rows: 3 + cols: 4 + data: + - 1196.2134247297394 + - 0.0 + - 965.3028251731779 + - 0.0 + - 0.0 + - 1194.7308169154975 + - 523.917540801463 + - 0.0 + - 0.0 + - 0.0 + - 1.0 + - 0.0 diff --git a/dimos/robot/unitree/go2/mcap/README.md b/dimos/robot/unitree/go2/mcap/README.md new file mode 100644 index 0000000000..ca814915be --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/README.md @@ -0,0 +1,83 @@ +# Go2 MCAP → dataset ingest + +Turn a Go2 onboard MCAP recording into a memory2 SQLite dataset (`.db`) that the +`dimos map` CLI reads. Lidar scans are deskewed into the **world frame** (Go2 +convention); two trajectories are emitted for comparison. + +## Build + +```sh +uv run python -m dimos.robot.unitree.go2.mcap.ingest \ + data/go2_china_office_indoor.mcap --out data/go2_china_office_indoor.db --seconds 60 +``` +Drop `--seconds` for the full recording. `mcap` must be installed (`uv pip install mcap`). +The best-z trajectory defaults to `_bestz.txt` (data dir, via data utils); +override with `--bestz`. + +## Streams + +| stream | type | content | +|---|---|---| +| `color_image` | Image | RGB, posed at `camera_optical` in world | +| `odom` | PoseStamped | Go2 leg-inertial odometry | +| `odom_bestz` | PoseStamped | leg xy/yaw + pitch-reconstructed z | +| `lidar` | PointCloud2 | per-scan world cloud (deskewed by `odom`) | +| `lidar_bestz` | PointCloud2 | per-scan world cloud (deskewed by `odom_bestz`) | +| `lidar_1s` / `lidar_bestz_1s` | PointCloud2 | 1 s accumulation per trajectory | + +## Validate + +```sh +dimos map summary data/go2_china_office_indoor.db +dimos map global data/go2_china_office_indoor.db --lidar lidar --voxel 0.1 +dimos map global data/go2_china_office_indoor.db --lidar lidar_bestz --voxel 0.1 # z-corrected +dimos map replay data/go2_china_office_indoor.db --duration 60 +``` + +Extrinsic (`extrinsics.py`): the L1 is mounted nearly upside-down; `EXT_R` is the +official `pitch=2.88` flip + `rotate_yaw_bias` (~-123°), ground-leveled. +`data/_bestz.txt` is the reconstructed trajectory (column 6 = z). + +## Full-length run (go2_china_office_indoor, 1157 s ≈ 19 min) + +Ingest: `python -m ...ingest data/go2_china_office_indoor.mcap` (no `--seconds`) +→ **3 min 26 s**, `data/go2_china_office_indoor.db` ≈ **2.3 GB**. + +``` +anchor=1780066187180359363 odom poses=173616 span=1157.1s +wrote 34724 odom poses (x2) +wrote 17776 lidar scans (x2) +wrote 1158 1s accumulations (x2) +wrote 5703 color_image frames +``` + +`dimos map summary data/go2_china_office_indoor.db`: + +``` +Stream("color_image"): 5703 items, 2026-05-29 14:49:47 — 15:09:04 (1156.8s) +Stream("lidar"): 17776 items, 2026-05-29 14:49:47 — 15:09:04 (1157.0s) +Stream("lidar_1s"): 1158 items, 2026-05-29 14:49:47 — 15:09:04 (1157.0s) +Stream("lidar_bestz"): 17776 items, 2026-05-29 14:49:47 — 15:09:04 (1157.0s) +Stream("lidar_bestz_1s"): 1158 items, 2026-05-29 14:49:47 — 15:09:04 (1157.0s) +Stream("odom"): 34724 items, 2026-05-29 14:49:47 — 15:09:04 (1157.1s) +Stream("odom_bestz"): 34724 items, 2026-05-29 14:49:47 — 15:09:04 (1157.1s) +``` + +`dimos map global … --lidar lidar_bestz` reconstructs the z-corrected world map +(GPU voxel grid, pose-dedup) and writes an `.rrd`. Notes: odom is downsampled to +~30 Hz; a few onboard JPEG frames log a harmless "Corrupt JPEG data" warning and +are skipped. + +# marker validation + +for debugging just camera + +`dimos map replay-marker go2_china_office_indoor --camera-info dimos/robot/unitree/go2/front_camera_1080.yaml --duration 30` + +for global map that includes markers + +`dimos map global go2_china_office_indoor --lidar lidar_bestz --voxel 0.1 --duration 60 --markers --camera-info dimos/robot/unitree/go2/front_camera_1080.yaml --image-pose odom_bestz` + +for replay of global map creation + +`dimos map replay go2_china_office_indoor --duration 100 --voxel 0.05 --map --cube --no-image` diff --git a/dimos/robot/unitree/go2/mcap/__init__.py b/dimos/robot/unitree/go2/mcap/__init__.py new file mode 100644 index 0000000000..a4a9ac6402 --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/__init__.py @@ -0,0 +1,5 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"). + +"""Go2 MCAP → memory2 SQLite dataset ingest (lidar / rgb / odom in world frame).""" diff --git a/dimos/robot/unitree/go2/mcap/extrinsics.py b/dimos/robot/unitree/go2/mcap/extrinsics.py new file mode 100644 index 0000000000..fc4ad4c49f --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/extrinsics.py @@ -0,0 +1,33 @@ +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"). + +"""Go2 L1 lidar extrinsic (base_link <- lidar) and camera mount. + +The L1 is mounted nearly upside-down: raw lidar +z points at the floor. The +official Unitree config (go2_l1_lidar.yaml) encodes this as base->lidar +``[0.28216, 0, -0.02467, roll=0, pitch=2.88, yaw=0]`` plus a separate +``rotate_yaw_bias`` (~-123 deg, calibrated to the front-leg position). EXT_R below +is that flip, leveled to the averaged ground normal over several stationary windows +(floor tilt ~1-2 deg, floor below the robot) and yawed so the map heading matches +the trajectory. Validated against the official "default imu reading" +[yaw -57.9, pitch -8.1, roll -167.3] (agrees to a few degrees). +""" + +import numpy as np + +# base_link <- lidar rotation (lidar points -> base frame: p_base = EXT_R @ p_lidar + EXT_T) +EXT_R = np.array( + [ + [0.504486, -0.843018, 0.186588], + [-0.853668, -0.519391, -0.038544], + [0.129405, -0.139840, -0.981682], + ], + dtype=np.float64, +) +EXT_T = np.array([0.28216, 0.0, -0.02467], dtype=np.float64) + +# base_link -> camera_optical (from dimos GO2 connection BASE_TO_OPTICAL): +# translate 0.3m forward, then rotate into the optical frame. +CAM_T = np.array([0.30, 0.0, 0.0], dtype=np.float64) +CAM_Q = np.array([-0.5, 0.5, -0.5, 0.5], dtype=np.float64) # xyzw diff --git a/dimos/robot/unitree/go2/mcap/go2_cdr.py b/dimos/robot/unitree/go2/mcap/go2_cdr.py new file mode 100644 index 0000000000..8e8487ff78 --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/go2_cdr.py @@ -0,0 +1,180 @@ +# 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. + +"""Minimal little-endian CDR (XCDR1) decoders for the Go2 MCAP channels. + +Hand-rolled so the converter needs no ROS/IDL deps — just the field layouts we +recorded (see go2-station/reference/harvest/idl + 20260529-07/08). Alignment is +body-relative (the 4-byte encapsulation header is skipped first). +""" + +import struct + +import numpy as np + + +class Cur: + def __init__(self, b): + self.b = b + self.p = 4 # skip 4-byte CDR encapsulation header + + def _al(self, n): + m = (self.p - 4) % n + if m: + self.p += n - m + + def u8(self): + v = self.b[self.p] + self.p += 1 + return v + + def u16(self): + self._al(2) + v = struct.unpack_from("= 32 else np.empty(0, _PC_DT) + return {"stamp_ns": stamp, "width": w, "point_step": ps, "arr": arr} + + +def decode_imu(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() + o = c.f64n(4) # orientation x,y,z,w + c.f64n(9) # orientation_covariance + av = c.f64n(3) # angular_velocity + c.f64n(9) + la = c.f64n(3) # linear_acceleration + return {"stamp_ns": stamp, "orientation": o, "ang_vel": av, "lin_acc": la} + + +def decode_odometry(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() + c.s() # header.stamp, frame_id, child_frame_id + pos = c.f64n(3) # pose.pose.position + quat = c.f64n(4) # pose.pose.orientation x,y,z,w + return {"stamp_ns": stamp, "position": pos, "orientation": quat} + + +def decode_sportmode(data): + c = Cur(data) + stamp = _stamp_ns(c) # TimeSpec + c.u32() # error_code + c.f32n(4) + c.f32n(3) + c.f32n(3) + c.f32n(3) + c.u8() # imu_state + temperature + mode = c.u8() + c.f32() + c.u8() + c.f32() # mode, progress, gait_type, foot_raise_height + pos = c.f32n(3) + body_h = c.f32() + vel = c.f32n(3) + yaw = c.f32() + return { + "stamp_ns": stamp, + "mode": mode, + "position": pos, + "body_height": body_h, + "velocity": vel, + "yaw_speed": yaw, + } + + +def decode_compressed_image(data): + c = Cur(data) + stamp = _stamp_ns(c) + c.s() # header.stamp, frame_id + fmt = c.s() + n = c.u32() + blob = bytes(data[c.p : c.p + n]) + return {"stamp_ns": stamp, "format": fmt, "data": blob} diff --git a/dimos/robot/unitree/go2/mcap/go2_china_office_indoor_annotation.json b/dimos/robot/unitree/go2/mcap/go2_china_office_indoor_annotation.json new file mode 100644 index 0000000000..640b87d99a --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/go2_china_office_indoor_annotation.json @@ -0,0 +1,38 @@ +{ + "levels": { "1F": -4.545, "2F": -1.045, "2.5F": 0.0, "3F": 2.455 }, + "phases": [ + {"level":"2.5F", "t0":0.0, "t1":29.8}, + {"name":"descend","from":"2.5F","to":"2F", "t0":29.8, "t1":39.8}, + {"level":"2F", "t0":39.8, "t1":200.8}, + {"name":"ascend", "from":"2F","to":"3F", "t0":200.8, "t1":247.8}, + {"level":"3F", "t0":247.8, "t1":317.8}, + {"name":"descend","from":"3F","to":"2F", "t0":317.8, "t1":346.8}, + {"level":"2F", "t0":346.8, "t1":417.8}, + {"name":"descend","from":"2F","to":"1F", "t0":417.8, "t1":456.8}, + {"level":"1F", "t0":456.8, "t1":703.8}, + {"name":"ascend", "from":"1F","to":"2F", "t0":703.8, "t1":751.8}, + {"level":"2F", "t0":751.8, "t1":902.8}, + {"name":"ascend", "from":"2F","to":"3F", "t0":902.8, "t1":948.8}, + {"level":"3F", "t0":948.8, "t1":978.8}, + {"name":"descend","from":"3F","to":"2F", "t0":978.8, "t1":1029.8}, + {"level":"2F", "t0":1029.8, "t1":1109.8}, + {"name":"ascend", "from":"2F","to":"2.5F","t0":1109.8,"t1":1120.8}, + {"level":"2.5F", "t0":1120.8, "t1":1157.1} + ], + "apriltags": [ + {"tag":"A", "floor":"2.5F", "t0":8.8, "t1":8.8}, + {"tag":"B", "floor":"2F", "t0":40.8, "t1":43.8}, + {"tag":"B", "floor":"2F", "t0":49.8, "t1":59.8}, + {"tag":"C", "floor":"2F", "t0":131.8, "t1":145.8}, + {"tag":"D", "floor":"3F", "t0":255.8, "t1":269.8}, + {"tag":"E", "floor":"2F", "t0":378.8, "t1":384.8}, + {"tag":"F", "floor":"1F", "t0":471.8, "t1":483.8}, + {"tag":"F", "floor":"1F", "t0":664.8, "t1":690.8}, + {"tag":"B", "floor":"2F", "t0":771.8, "t1":772.8}, + {"tag":"C", "floor":"2F", "t0":843.8, "t1":849.8}, + {"tag":"D", "floor":"3F", "t0":953.8, "t1":961.8}, + {"tag":"E", "floor":"2F", "t0":1050.8, "t1":1060.8}, + {"tag":"B", "floor":"2F", "t0":1083.8, "t1":1103.8}, + {"tag":"A", "floor":"2.5F", "t0":1150.8, "t1":1157.1} + ] +} diff --git a/dimos/robot/unitree/go2/mcap/ingest.py b/dimos/robot/unitree/go2/mcap/ingest.py new file mode 100644 index 0000000000..9049cc62de --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/ingest.py @@ -0,0 +1,287 @@ +# 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. + +# Copyright 2026 Dimensional Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"). + +"""Ingest a Go2 MCAP recording into a memory2 SQLite dataset (.db). + +Decodes the onboard DDS channels, deskews each lidar scan into the WORLD frame +(the convention the Go2 publishes and that ``dimos map`` assumes), and writes the +streams the mapping CLI reads. Two trajectories are emitted so you can compare +them: the raw leg-inertial ``odom`` and our reconstructed ``odom_bestz`` (leg +xy/yaw + pitch-recovered z, from ``_bestz.txt`` in the data dir). + +Streams written: + color_image Image RGB, posed at camera_optical in world + odom PoseStamped Go2 leg-inertial odometry + odom_bestz PoseStamped leg xy/yaw + reconstructed z + lidar PointCloud2 per-scan world cloud (deskewed by odom) + lidar_bestz PointCloud2 per-scan world cloud (deskewed by odom_bestz) + lidar_1s PointCloud2 1 s accumulation (odom) + lidar_bestz_1s PointCloud2 1 s accumulation (odom_bestz) + +Usage: + uv run python -m dimos.robot.unitree.go2.mcap.ingest \ + data/go2_china_office_indoor.mcap --out go2_china_office_indoor.db --seconds 60 +""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np +import typer + +from dimos.msgs.geometry_msgs.PoseStamped import PoseStamped +from dimos.msgs.sensor_msgs.Image import Image, ImageFormat +from dimos.msgs.sensor_msgs.PointCloud2 import PointCloud2 +from dimos.robot.unitree.go2.mcap import go2_cdr as cdr +from dimos.robot.unitree.go2.mcap.extrinsics import CAM_Q, CAM_T, EXT_R, EXT_T +from mcap.reader import make_reader + +CLOUD = "rt/utlidar/cloud" +ODOM = "rt/utlidar/robot_odom" +VIDEO = "rt/frontvideo" + + +# --- quaternion helpers (xyzw) ---------------------------------------------- +def _qrot(q: np.ndarray, v: np.ndarray) -> np.ndarray: + """Rotate v[n,3] by xyzw quaternions q[n,4].""" + xyz, w = q[:, :3], q[:, 3:4] + t = 2.0 * np.cross(xyz, v) + return v + w * t + np.cross(xyz, t) + + +def _qmul(a: np.ndarray, b: np.ndarray) -> np.ndarray: + """Hamilton product a⊗b for xyzw quats (broadcast over rows).""" + ax, ay, az, aw = a[..., 0], a[..., 1], a[..., 2], a[..., 3] + bx, by, bz, bw = b[..., 0], b[..., 1], b[..., 2], b[..., 3] + return np.stack( + [ + aw * bx + ax * bw + ay * bz - az * by, + aw * by - ax * bz + ay * bw + az * bx, + aw * bz + ax * by - ay * bx + az * bw, + aw * bw - ax * bx - ay * by - az * bz, + ], + axis=-1, + ) + + +def _interp(tt: np.ndarray, pos: np.ndarray, quat: np.ndarray, q: np.ndarray): + """LERP position + NLERP quaternion of a trajectory at query times q.""" + idx = np.clip(np.searchsorted(tt, q), 1, len(tt) - 1) + i0, i1 = idx - 1, idx + dt = tt[i1] - tt[i0] + f = np.clip((q - tt[i0]) / np.where(dt == 0, 1.0, dt), 0.0, 1.0)[:, None] + p = pos[i0] * (1 - f) + pos[i1] * f + q0, q1 = quat[i0], quat[i1] + q1 = np.where(((q0 * q1).sum(1) < 0)[:, None], -q1, q1) + qi = q0 * (1 - f) + q1 * f + return p, qi / np.linalg.norm(qi, axis=1, keepdims=True) + + +def _deskew(pts: np.ndarray, tpt: np.ndarray, tt, pos, quat) -> np.ndarray: + """Per-point deskew of lidar-frame pts (at times tpt) into the world frame.""" + p, q = _interp(tt, pos, quat, tpt) + pbase = pts @ EXT_R.T + EXT_T + return _qrot(q, pbase) + p + + +def _pose7(tt, pos, quat, t: float) -> tuple: + """Single interpolated (x,y,z,qx,qy,qz,qw) world pose at time t.""" + p, q = _interp(tt, pos, quat, np.array([t])) + return (*p[0], *q[0]) + + +def _load_odom(mcap: Path, seconds: float | None): + """First-cloud anchor + leg-odom trajectory (t_rel, pos, quat).""" + with open(mcap, "rb") as f: + anchor = next(m.publish_time for _s, _c, m in make_reader(f).iter_messages(topics=[CLOUD])) + t, pos, quat = [], [], [] + with open(mcap, "rb") as f: + for _s, _c, m in make_reader(f).iter_messages(topics=[ODOM]): + tr = (m.publish_time - anchor) / 1e9 + if tr < 0 or (seconds is not None and tr > seconds + 1): + continue + o = cdr.decode_odometry(m.data) + t.append(tr) + pos.append(o["position"]) + quat.append(o["orientation"]) + return anchor, np.array(t), np.array(pos, np.float64), np.array(quat, np.float64) + + +def main( + mcap: Path = typer.Argument(..., help="Go2 .mcap recording"), + out: Path = typer.Option(None, "--out", help="Output .db (default: .db)"), + seconds: float = typer.Option(None, "--seconds", help="Only first N seconds"), + odom_hz: float = typer.Option(30.0, "--odom-hz", help="Downsample odom streams"), + voxel: float = typer.Option(0.05, "--voxel", help="Voxel size for 1 s accumulation"), + bestz: Path = typer.Option( + None, "--bestz", help="Best-z trajectory file (default: _bestz.txt, via data utils)" + ), + rmin: float = typer.Option(0.4, "--rmin"), + rmax: float = typer.Option(30.0, "--rmax"), +) -> None: + """Build a world-frame Go2 dataset (lidar/rgb/two odoms) the dimos map CLI reads.""" + from dimos.memory2.store.sqlite import SqliteStore + from dimos.utils.data import resolve_named_path + + mcap = resolve_named_path(mcap, ".mcap") # local path, repo data dir, or LFS pull + out = out or mcap.with_suffix(".db") + for p in (out, out.with_suffix(".db-wal"), out.with_suffix(".db-shm")): + p.unlink(missing_ok=True) # overwrite: avoid appending to an old db + anchor, lt, lpos, lquat = _load_odom(mcap, seconds) + print(f"anchor={anchor} odom poses={len(lt)} span={lt[-1]:.1f}s") + + # odom_bestz = leg xy/yaw + reconstructed z, z interpolated onto lt. The best-z + # trajectory lives next to the mcap in the data dir (resolved via data utils). + bestz = bestz or resolve_named_path(mcap.stem + "_bestz.txt") + bz_file = np.loadtxt(bestz) + bz = np.interp(lt, bz_file[:, 0], bz_file[:, 6]) + bpos = np.column_stack([lpos[:, 0], lpos[:, 1], bz]) + + def acc_world(bins: dict, t_rel: float, w: np.ndarray) -> None: + bins.setdefault(int(t_rel), []).append(w) + + store = SqliteStore(path=str(out)) + with store: + s_img = store.stream("color_image", Image) + s_od = store.stream("odom", PoseStamped) + s_odz = store.stream("odom_bestz", PoseStamped) + s_li = store.stream("lidar", PointCloud2) + s_liz = store.stream("lidar_bestz", PointCloud2) + s_li1 = store.stream("lidar_1s", PointCloud2) + s_liz1 = store.stream("lidar_bestz_1s", PointCloud2) + + # ts is UNIQUE per stream; keep it strictly increasing (raw publish_times + # can repeat) so appends never collide. + _last: dict = {} + + def put(stream, payload, ts: float, pose) -> None: + last = _last.get(stream) + if last is not None and ts <= last: + ts = last + 1e-4 + _last[stream] = ts + stream.append(payload, ts=ts, pose=pose) + + # ---- odom + odom_bestz (downsampled) ---- + step = max(1, round((len(lt) / max(lt[-1], 1e-6)) / max(odom_hz, 1e-6))) + for i in range(0, len(lt), step): + ts = anchor / 1e9 + lt[i] + qx, qy, qz, qw = lquat[i] + put( + s_od, + PoseStamped( + ts=ts, frame_id="world", position=lpos[i].tolist(), orientation=[qx, qy, qz, qw] + ), + ts, + (*lpos[i], qx, qy, qz, qw), + ) + put( + s_odz, + PoseStamped( + ts=ts, frame_id="world", position=bpos[i].tolist(), orientation=[qx, qy, qz, qw] + ), + ts, + (*bpos[i], qx, qy, qz, qw), + ) + print(f"wrote {len(range(0, len(lt), step))} odom poses (x2)") + + # ---- lidar (world) per scan, deskewed by both trajectories + 1s bins ---- + bins_l: dict = {} + bins_z: dict = {} + nclouds = 0 + with open(mcap, "rb") as f: + for _s, _c, m in make_reader(f).iter_messages(topics=[CLOUD]): + tr = (m.publish_time - anchor) / 1e9 + if tr < 0 or (seconds is not None and tr > seconds): + continue + a = cdr.decode_pointcloud2(m.data)["arr"] + if len(a) == 0: + continue + xyz = np.stack([a["x"], a["y"], a["z"]], 1).astype(np.float64) + inten = a["intensity"].astype(np.float32) + rr = np.linalg.norm(xyz, axis=1) + keep = np.isfinite(xyz).all(1) & (rr > rmin) & (rr < rmax) + xyz, inten = xyz[keep], inten[keep] + if len(xyz) < 10: + continue + tpt = tr + a["time"].astype(np.float64)[keep] + ts = m.publish_time / 1e9 + wl = _deskew(xyz, tpt, lt, lpos, lquat) + wz = _deskew(xyz, tpt, lt, bpos, lquat) + put( + s_li, + PointCloud2.from_numpy(wl.astype(np.float32), "world", ts, inten), + ts, + _pose7(lt, lpos, lquat, tr), + ) + put( + s_liz, + PointCloud2.from_numpy(wz.astype(np.float32), "world", ts, inten), + ts, + _pose7(lt, bpos, lquat, tr), + ) + acc_world(bins_l, tr, wl.astype(np.float32)) + acc_world(bins_z, tr, wz.astype(np.float32)) + nclouds += 1 + print(f"wrote {nclouds} lidar scans (x2)") + + # ---- 1 s accumulations (voxel-downsampled), posed at bin center ---- + def flush(bins: dict, pos, stream) -> None: + for sec, chunks in sorted(bins.items()): + ts = anchor / 1e9 + sec + 0.5 + pc = PointCloud2.from_numpy(np.concatenate(chunks), "world", ts) + pc = pc.voxel_downsample(voxel) + put(stream, pc, ts, _pose7(lt, pos, lquat, sec + 0.5)) + + flush(bins_l, lpos, s_li1) + flush(bins_z, bpos, s_liz1) + print(f"wrote {len(bins_l)} 1s accumulations (x2)") + + # ---- color_image, posed at camera_optical in world ---- + import cv2 + + nimg = 0 + with open(mcap, "rb") as f: + for _s, _c, m in make_reader(f).iter_messages(topics=[VIDEO]): + tr = (m.publish_time - anchor) / 1e9 + if tr < 0 or (seconds is not None and tr > seconds): + continue + img = cdr.decode_compressed_image(m.data) + bgr = cv2.imdecode(np.frombuffer(img["data"], np.uint8), cv2.IMREAD_COLOR) + if bgr is None: + continue + ts = m.publish_time / 1e9 + bp, bq = _interp(lt, lpos, lquat, np.array([tr])) + cam_p = bp[0] + _qrot(bq, CAM_T[None])[0] + cam_q = _qmul(bq[0], CAM_Q) + put( + s_img, + Image.from_numpy(bgr, ImageFormat.BGR, "camera_optical", ts), + ts, + (*cam_p, *cam_q), + ) + nimg += 1 + print(f"wrote {nimg} color_image frames") + + print( + f"\nwrote {out}\n dimos map summary {out}\n dimos map global {out} --lidar lidar --voxel 0.1" + ) + + +if __name__ == "__main__": + typer.run(main) diff --git a/dimos/robot/unitree/go2/mcap/vendor/config.yaml b/dimos/robot/unitree/go2/mcap/vendor/config.yaml new file mode 100644 index 0000000000..bf11d56458 --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/vendor/config.yaml @@ -0,0 +1,11 @@ +dds: + domain: 0 + network_interface: eth0 + config_file: "/unitree/etc/cyclonedds_noshm.xml" +common: + topic_client_cmd: rt/utlidar/client_command + topic_server_log: rt/utlidar/server_log + +lidar_search_list: [go2_l2, go2_l1] +lidar_search_time: 5 +lidar_search_cycle_max: 10 \ No newline at end of file diff --git a/dimos/robot/unitree/go2/mcap/vendor/go2_l1_lidar.yaml b/dimos/robot/unitree/go2/mcap/vendor/go2_l1_lidar.yaml new file mode 100644 index 0000000000..adbb2e9c13 --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/vendor/go2_l1_lidar.yaml @@ -0,0 +1,116 @@ +#################################################### +# lidar sdconfiguration +#################################################### +dds: + domain_id: 0 + # config_file_path: "/unitree/etc/cyclonedds.xml" + config_file_path: "/home/mlb/cyclonedds.xml" + +enable_debug: false + +lidar_state: + version: "1.0.0.38" + min_dis: 0.02 + srp_max: 6111 # unit: us, normal rotation speed = 10800 r/min, period = 0.0055556s + crp_max: 4294967295 # unit: us, normal rotation speed = 660 r/min, period = 0.090909s + dirty_percentage_max: 100 # dirty percentage max allowed + cloud_latency_max: 30 # unit: second + +lidar_switch_default: true + +mapping: + stop_when_dance: false + stop_mode: [5, 7, 8, 10, 11, 12, 13, 14] + # 0. idle, default stand + # 1. balanceStand + # 2. pose + # 3. locomotion + # 4. reserve + # 5. lieDown + # 6. jointLock + # 7. damping + # 8. recoveryStand + # 9. reserve + # 10. sit + # 11. frontFlip + # 12. frontJump + # 13. frontPounce + # 14. wallow + +serial: + # port: "/dev/ttyACM0" # default + port: "/dev/GBUNICH6" + baudrate: 2000000 + +calibration: + range_scale_factor: 0.001 # 0.001 + range_bias: 0 # -0.0065 + range_max: 50 + range_min: 0.0 # 0.4 + rotate_yaw_bias: -123 # unit: degree; default: 57-180=-123; + # Note: you can calibrate `rotate_yaw_bias` according to the front-leg position + rotate_axis_bias_correction: false + rotate_axis_z: [0, 0, 1] + rotate_axis_y: [0, 1, 0] + +lidar: + frame: "utlidar" + single_scan_topic: "rt/utlidar/scan" + multi_scan_topic: "rt/utlidar/cloud" + multi_scan_num: 12 + +imu: + enable: true + frame: "utlidar_imu" + topic: "rt/utlidar/imu" + imu2lidar: [0, 0, 0, 3.1415926536, 0, 0] # x, y, z, roll, pitch, yaw # default imu reading: [yaw=-57.8994, pitch=-8.0973, roll=-167.3450] + + +baselink: + frame: "base_link" + # baselink2lidar: [0.304, 0, -0.03, 0, 2.88, 0] # default: [0.304, 0, 0.02, 0, 2.88, 0] + baselink2lidar: [0.28216, 0, -0.02467, 0, 2.88, 0] + +world_frame: + frame: "odom" + +truncation: + enable: true + method: 0 + polar_azimuth_list: [[0, 2, -0.7, 0.7]] # range_to_eliminate: [polarMin, plarMax, azimuthMin, azimuthMax] + box: [-100, 0.04,-0.25,0.25,-100,100] # [xmin, xmax, ymin, ymax, zmin, zmax] + box2: [-100, -0.15,-0.37,0.37,-100,100] + +spherical_rectangle: + length_margin: 0.2 + half_width: 0.25 + enable_leg_removal: true + +outliers_removal: + enable: false + radius: 0.2 + min_num_of_points: 2 + +pointcloud_deskew: + enable: true + odometry_topic: "rt/utlidar/robot_odom" + +sport_mode: + state_topic: "rt/sportmodestate" + frequency: 250 + enable_system_stamp: false + +ros2: + odom_topic: "rt/utlidar/robot_odom" + odom_frame: "odom" + base_link_frame: "base_link" + topic_foot_position: "rt/utlidar/foot_position" + pose_topic: "rt/utlidar/robot_pose" + pose_pub_period: 0.05 + +robot_config: + b1x: 0.1881 + b1y: 0.04675 + b2: 0.08 + b3: 0.213 + b4: 0.213 \ No newline at end of file diff --git a/dimos/robot/unitree/go2/mcap/vendor/go2_l1_preprocess.yaml b/dimos/robot/unitree/go2/mcap/vendor/go2_l1_preprocess.yaml new file mode 100644 index 0000000000..6d251e76da --- /dev/null +++ b/dimos/robot/unitree/go2/mcap/vendor/go2_l1_preprocess.yaml @@ -0,0 +1,100 @@ +dds: + domain: 0 + network_interface: eno1 + +debug: + enable: false + +sub_pointcloud0: + enable: true + topic: "rt/utlidar/cloud" + point_type: "unilidar" + frame_cloud: "utlidar_lidar" + frame_parent: "base_link" + # T_parent2cloud: [0.28216, 0, -0.02467, -2.92072, -0.141324, -1.01053] # for L2 down + T_parent2cloud: [0.28216, 0, -0.02467, 0, 2.88, 0] + range_min: 0 + range_max: 100 + +# 订阅机器人状态 +sub_sportmodestate: + topic: "rt/sportmodestate" + enable_system_stamp: false + period: 0.004 + +sub_lowstate: + topic: "rt/lowstate" + enable_system_stamp: false + +# 发布机器人里程计 +pub_robot_odometry: + enable: true + frequency: 200 + topic: "rt/utlidar/robot_odom" + frame: "odom" + +# 发布机器人里程计的位姿 +pub_robot_pose: + topic: "rt/utlidar/robot_pose" + period: 0.05 + +# 发布机身坐标系下的原始点云 +pub_pointcloud_fusion: + enable: true + frequency: 15 + topic: "rt/utlidar/cloud_base" + frame: "base_link" + +# 发布世界坐标系下的去畸变点云 +pub_pointcloud_deskewed: + enable: true + topic: "rt/utlidar/cloud_deskewed" + frame: "base_link" + +robot: + type: "go2" + world_frame: "odom" + base_frame: "base_link" + + +########################################################################## + +voxel_height_mapping: + topic_mapping_cmd: "rt/utlidar/mapping_cmd" + stop_when_dance: false + stop_mode: [5, 7, 8, 10, 11, 12, 13, 14] + # 0. idle, default stand + # 1. balanceStand + # 2. pose + # 3. locomotion + # 4. reserve + # 5. lieDown + # 6. jointLock + # 7. damping + # 8. recoveryStand + # 9. reserve + # 10. sit + # 11. frontFlip + # 12. frontJump + # 13. frontPounce + # 14. wallow + +truncation: + enable: true # true for L1 + box: [-100, 0.32,-0.25,0.25,-100,100] # [xmin, xmax, ymin, ymax, zmin, zmax] + box2: [-100, 0.17,-0.37,0.37,-100,100] + +link_remove: + enable: false + +cloud_deskew: + enable: true + +cloud_filter: + enable: false + radius: 0.2 + min_num_of_points: 2 + +cloud_downsample: + enable: true + min_dis: 0.02 \ No newline at end of file