306 lines
11 KiB
Markdown
306 lines
11 KiB
Markdown
# G1 SLAM Stack
|
||
|
||
Production SLAM for **Unitree G1 Edu** humanoid + **Livox MID-360** head-mounted LiDAR.
|
||
|
||
KISS-ICP scan matching, voxel persistence filtering, geometric place recognition, pose-graph loop closure, submap mapping with atomic checkpointing, and a Nav2-compatible map export pipeline. Runs as a multi-process worker with a PyQt commander UI.
|
||
|
||
---
|
||
|
||
## Hardware
|
||
|
||
| Item | Spec |
|
||
|---|---|
|
||
| Robot | Unitree G1 Edu (~1.32 m, ~0.30 m stride) |
|
||
| LiDAR | Livox MID-360 (head-mounted, z ≈ 1.30 m) |
|
||
| LiDAR rate | 10 Hz, ~200 K pts/s, 360° H-FoV, -7° to +52° V-FoV |
|
||
| LiDAR range | 0.10 m – 40 m (10% reflectivity) |
|
||
| Compute | Workstation (mapping/UI) + Jetson Orin NX (deploy target) |
|
||
|
||
---
|
||
|
||
## Requirements
|
||
|
||
### System
|
||
|
||
- Linux x86_64 (workstation) or aarch64 (Jetson Orin NX)
|
||
- Python ≥ 3.10
|
||
- A wired NIC (Gigabit) on the LiDAR network — typically `enp3s0`
|
||
|
||
### Native libraries
|
||
|
||
- **Livox-SDK2** — C++ SDK from Livox. The repo ships with `Livox-SDK2/` as a submodule. Build and install:
|
||
```bash
|
||
cd Livox-SDK2
|
||
mkdir -p build && cd build
|
||
cmake .. && make -j
|
||
sudo make install
|
||
```
|
||
This places `liblivox_lidar_sdk_*` under `/usr/local/lib/`.
|
||
|
||
### Python packages
|
||
|
||
```bash
|
||
pip install numpy scipy open3d kiss-icp PyQt6 pyqtgraph
|
||
```
|
||
|
||
| Package | Why |
|
||
|---|---|
|
||
| `numpy` | Point-cloud math throughout |
|
||
| `scipy` | KD-trees in cleanup / loop closure |
|
||
| `open3d` | Point-cloud I/O (.ply read/write) |
|
||
| `kiss-icp` | Scan-to-scan ICP backend |
|
||
| `PyQt6` | Commander GUI |
|
||
| `pyqtgraph` | OpenGL viewport in the GUI |
|
||
|
||
The Livox driver wrapper (`livox2_python.py`) uses `ctypes` against the installed `liblivox_lidar_sdk_*` — no separate Python binding required.
|
||
|
||
### Network
|
||
|
||
LiDAR ships at `192.168.123.120`. The workstation NIC must be on the same `/24`:
|
||
|
||
```bash
|
||
sudo ip addr add 192.168.123.222/24 dev enp3s0
|
||
sudo ip link set enp3s0 up
|
||
ping 192.168.123.120 # verify
|
||
```
|
||
|
||
UDP ports 56100–56501 must not be in use (close LivoxViewer 2 / other SLAM instances first).
|
||
|
||
---
|
||
|
||
## How to run
|
||
|
||
### 1. Verify the LiDAR is online
|
||
|
||
```bash
|
||
ping 192.168.123.120
|
||
```
|
||
|
||
If no reply: check cable, NIC IP (`ip addr show enp3s0`), and that no other software is bound to the LiDAR's ports.
|
||
|
||
### 2. Sanity-check the config
|
||
|
||
Open `SLAM_Config.json` and `mid360_config.json`. Confirm:
|
||
- `network.default_host_ip` matches your NIC IP
|
||
- `mid360_config.json` host IPs match
|
||
- `extrinsic_parameter.z` matches the actual LiDAR mount height
|
||
|
||
### 3. Launch the GUI
|
||
|
||
```bash
|
||
cd /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar
|
||
python3 SLAM_GUI.py
|
||
```
|
||
|
||
The **SLAM Commander** window opens.
|
||
|
||
### 4. Map a new place
|
||
|
||
1. Click **CONNECT** — the worker spins up and connects to the LiDAR.
|
||
2. Click **MAP NEW PLACE** — sets profile to `MAP_NEW` (long decay, fine voxels).
|
||
3. Click **START** — mapping begins; points appear in the viewport.
|
||
4. Walk the robot through the space.
|
||
5. Click **STOP** — if `SAVE ARMED` is checked, the map saves to `DataMap/<name>.ply`.
|
||
|
||
Use **AUTOSAVE** for long sessions (saves every N seconds).
|
||
|
||
### 5. Localize / navigate in an existing map
|
||
|
||
1. **CONNECT**.
|
||
2. **NAVIGATE IN MAPPED PLACE** — sets profile to `LOCALIZE_MAP`.
|
||
3. **LOAD MAP** — pick a `.ply` from `DataMap/`.
|
||
4. **START** — the system runs ICP against the loaded map. The status bar shows `TRACKING` once aligned.
|
||
5. Use the mission/waypoint controls to drive goals.
|
||
|
||
If the robot starts in an unknown spot inside the map, global relocalization runs automatically: session-memory cache → place recognition → brute-force anchor × yaw search.
|
||
|
||
### 6. Override the config path
|
||
|
||
```bash
|
||
SLAM_CONFIG=/path/to/custom.json python3 SLAM_GUI.py
|
||
```
|
||
|
||
### 7. Headless replay / regression
|
||
|
||
```bash
|
||
python3 SLAM_Replay.py --recording <path>.lvx2
|
||
```
|
||
|
||
---
|
||
|
||
## File map
|
||
|
||
| File | Purpose |
|
||
|---|---|
|
||
| `SLAM_GUI.py` | PyQt commander UI |
|
||
| `SLAM_engine.py` | Config dataclasses, worker bootstrap, `EngineConfig` |
|
||
| `SLAM_worker.py` | Main SLAM loop: ingest, ICP, filter, map, checkpoint, save, navigate |
|
||
| `SLAM_LocalizationService.py` | `odom`/`map`/`ref` frame transforms |
|
||
| `SLAM_LoopClosure.py` | Keyframe loop closure with SE3-slerp error distribution |
|
||
| `SLAM_PlaceRecognition.py` | Geometric anchor descriptors for global relocalization |
|
||
| `SLAM_StateMachine.py` | TRACKING / DEGRADED / LOST / RECOVERY |
|
||
| `SLAM_Submap.py` | Local + global submap mapper, atomic `.pkl` checkpointing |
|
||
| `SLAM_Filter.py` | Voxel persistence filter + indoor map quality filter |
|
||
| `SLAM_MAP.py` | Stable-map layer, `.ply` export via Open3D |
|
||
| `SLAM_Navigation.py` | Nav2-compatible YAML/PGM export, A* planner |
|
||
| `SLAM_NavRuntime.py` | Live cost-map for runtime planning |
|
||
| `SLAM_Mission.py` | Waypoint missions |
|
||
| `SLAM_Fusion.py` | LiDAR + IMU + (wheel/vision) pose fusion |
|
||
| `SLAM_Safety.py` | E-stop / stale-localization watchdog |
|
||
| `SLAM_Session.py` | Session memory (cached `slam→ref` alignments per map) |
|
||
| `SLAM_Replay.py` | Offline replay / regression harness |
|
||
| `SLAM_Validation.py` | Startup self-check |
|
||
| `SLAM_Diagnostics.py` | Crash logging, performance snapshots |
|
||
| `SLAM_Transforms.py` | SE3 utilities, pose deltas, slerp |
|
||
| `livox2_python.py` | Livox-SDK2 ctypes wrapper, with tag-byte noise filter |
|
||
| `SLAM_Config.json` | Single source of truth for tuning |
|
||
| `mid360_config.json` | Livox driver config (IPs, ports, extrinsics) |
|
||
|
||
---
|
||
|
||
## Configuration reference
|
||
|
||
### Network — IPs & Interfaces
|
||
|
||
| Source | Setting | Value |
|
||
|---|---|---|
|
||
| `SLAM_Config.json` | `network.default_interface` | `enp3s0` |
|
||
| `SLAM_Config.json` | `network.default_host_ip` | `192.168.123.222` (workstation) |
|
||
| `mid360_config.json` | `lidar_configs[0].ip` | `192.168.123.120` (LiDAR — Unitree default) |
|
||
| `mid360_config.json` | `host_net_info.*_ip` | `192.168.123.222` (matches workstation) |
|
||
|
||
> Reference (Unitree docs): default LiDAR IP `192.168.123.120`, host on `192.168.123.x/24`, gateway `192.168.123.1`.
|
||
|
||
### LiDAR UDP ports — `mid360_config.json`
|
||
|
||
| Direction | Channel | LiDAR side | Host side |
|
||
|---|---|---|---|
|
||
| Control | `cmd_data` | 56100 | 56101 |
|
||
| Push messages | `push_msg` | 56200 | 56201 |
|
||
| Point cloud | `point_data` | 56300 | 56301 |
|
||
| IMU data | `imu_data` | 56400 | 56401 |
|
||
| Logs | `log_data` | 56500 | 56501 |
|
||
|
||
### LiDAR extrinsics — `mid360_config.json` (head mount, G1)
|
||
|
||
| Param | Value | Note |
|
||
|---|---|---|
|
||
| `roll` | 0.0° | |
|
||
| `pitch` | 0.0° | head mount, level (Unitree default) |
|
||
| `yaw` | 0.0° | |
|
||
| `x` | 0.0 m | |
|
||
| `y` | 0.0 m | |
|
||
| `z` | 1.30 m | LiDAR atop head, ≈ floor + 1.30 m |
|
||
| `pcl_data_type` | 1 | Cartesian High |
|
||
| `pattern_mode` | 0 | non-repetitive |
|
||
|
||
### SLAM core tuning — `SLAM_Config.json` highlights
|
||
|
||
| Section | Key | Value | Purpose |
|
||
|---|---|---|---|
|
||
| **slam** | `slam_voxel_size` | 0.12 m | ICP voxel |
|
||
| | `max_range` | 50.0 m | LiDAR clip |
|
||
| **filter** | `voxel_size` | 0.20 m | persistence base |
|
||
| | `hits_threshold` | 4 | (overridden by profiles) |
|
||
| | `persistence.decay_seconds` | 3.0 s | (overridden by profiles) |
|
||
| | `persistence.max_voxels` | 2,000,000 | mem cap |
|
||
| **map** | `display_voxel` / `save_voxel` | 0.08 / 0.05 m | GUI / .ply resolution |
|
||
| | `min_points_to_save` | 550 | guard |
|
||
| **map_quality** | `near_min_range_m` | 0.15 m | hardware min is 0.10 |
|
||
| | `body_exclusion` | x[-0.20, 0.25] · y[±0.25] · z[-1.40, -0.10] | head-mount G1 body box |
|
||
| **map_cleanup** | `keep_largest_n` / `period_sec` | 2 / 6.0 s | islands removed every 6 s |
|
||
| **submap_mapping** | `local_voxel_m` / `global_voxel_m` | 0.08 / 0.14 m | |
|
||
| | `checkpoint_interval_sec` | 60.0 s | atomic .pkl save |
|
||
| **navigation_export** | `z_min_m` / `z_max_m` | 0.05 / 1.40 m | floor+5 cm to 1.40 m |
|
||
| | `resolution_m` | 0.05 m | Nav2 grid |
|
||
| **state_machine** | `min_good_to_recover` | 3 | RECOVERY → TRACKING |
|
||
| **mission** | `waypoint_tolerance_m` | 0.55 m | G1 ~0.30 m stride margin |
|
||
| **safety** | `stop_radius_m` | 0.50 m | |
|
||
| | `stale_localization_sec` | 1.5 s | |
|
||
| **fusion** | `enabled` | `true` | LiDAR + IMU pose fusion |
|
||
| | `imu_weight` / `lidar_weight` | 0.25 / 1.0 | |
|
||
| **runtime** | `publish_hz` | 12.0 | GUI/status update rate |
|
||
| **livox** | `tag_filter` | `true` | Unitree-recommended noise drop |
|
||
| **loop_closure** | `enabled` | `false` | optimizer is real, but disabled |
|
||
|
||
### Stability profiles — hardcoded in `SLAM_worker.py:1957-1961`
|
||
|
||
| Profile | hit_threshold | decay_seconds | voxel_size | density |
|
||
|---|---|---|---|---|
|
||
| `MAP_NEW` | 2 | 1800.0 | 0.10 | MEDIUM |
|
||
| `LOCALIZE_MAP` | 3 | 45.0 | 0.20 | MEDIUM |
|
||
| `LIVE_NAV_MAP` | 2 | 18.0 | 0.20 | MEDIUM |
|
||
| `LIVE_NAV_NO_MAP` | 2 | 14.0 | 0.18 | HIGH |
|
||
| `QUICK_DEMO` | 2 | 8.0 | 0.20 | HIGH |
|
||
| `BALANCED` | (from config) | (from config) | (from config) | MEDIUM |
|
||
|
||
### File locations
|
||
|
||
```
|
||
G1_Lootah/Lidar/
|
||
├── SLAM_Config.json # main config (single source of truth)
|
||
├── mid360_config.json # Livox driver (read by SDK2)
|
||
└── DataMap/ # maps + submap_checkpoint.pkl + session memory
|
||
```
|
||
|
||
Override path: `SLAM_CONFIG=/path/to/other.json`.
|
||
|
||
---
|
||
|
||
## Workflows
|
||
|
||
| Workflow | Profile | When to use |
|
||
|---|---|---|
|
||
| Map a new place | `MAP_NEW` | Building a fresh map. Long decay (30 min), 0.10 m voxels. |
|
||
| Navigate in mapped place | `LOCALIZE_MAP` | Localize against an existing `.ply`. |
|
||
| Live nav with map | `LIVE_NAV_MAP` | Navigation + slow map updates. |
|
||
| Live nav (no map) | `LIVE_NAV_NO_MAP` | Pure obstacle-avoidance, no persistent map. |
|
||
| Quick demo | `QUICK_DEMO` | Short-window mapping for demos. |
|
||
|
||
The current profile shows in the GUI status bar.
|
||
|
||
---
|
||
|
||
## Outputs
|
||
|
||
```
|
||
DataMap/
|
||
├── *.ply # saved point cloud maps
|
||
├── *.yaml + *.pgm # Nav2-compatible map_server bundles
|
||
├── submap_checkpoint.pkl # atomic submap snapshot (auto-restored on next start)
|
||
└── SLAM_session_memory.json # cached relocalization alignments per map
|
||
```
|
||
|
||
---
|
||
|
||
## Frame conventions
|
||
|
||
- World z = 0 is floor.
|
||
- LiDAR origin at sensor frame's z = 0; extrinsic places it at body-frame z ≈ 1.30 m.
|
||
- Body exclusion box in **sensor frame**: z = -1.40 (floor) to z = -0.10 (just below LiDAR).
|
||
- ICP yields `T_world_lidar`; localization service maintains `odom_to_map` and `odom_to_ref` separately.
|
||
|
||
---
|
||
|
||
## Deploy
|
||
|
||
Edit on the **workstation only**. Push to Jetson via `scp`:
|
||
|
||
```bash
|
||
scp -r /home/zedx/Robotics_workspace/yslootahtech/G1_Lootah/Lidar/ \
|
||
unitree@<jetson-ip>:~/G1_Lootah/
|
||
```
|
||
|
||
Do not edit on the Jetson directly.
|
||
|
||
---
|
||
|
||
## Common issues
|
||
|
||
- **Connect fails / `Address already in use`** — another Livox app holds ports 56101–56501. Close LivoxViewer 2 / other SLAM instances.
|
||
- **No map / empty viewport** — verify `ping 192.168.123.120` works, NIC IP matches `default_host_ip`, and the LiDAR LED is solid.
|
||
- **Map ignores furniture** — head-mount + -7° lower FoV creates a blind cone underneath; close-floor area is unscannable < ~10 m. Move the robot closer or accept the limitation.
|
||
- **Stray points in saved map** — cleanup pass runs every 6 s and once more on save; if islands persist, lower `map_cleanup.keep_largest_n`.
|
||
- **Loop closure off** — set `loop_closure.enabled: true` in config; the optimizer (`SLAM_LoopClosure._optimize`) is fully wired.
|
||
- **`SDKINIT failed`** — Livox-SDK2 not installed; rebuild `Livox-SDK2/` and `sudo make install`.
|