mujoco-lidar 0.2.5__tar.gz
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- mujoco_lidar-0.2.5/LICENSE +23 -0
- mujoco_lidar-0.2.5/PKG-INFO +689 -0
- mujoco_lidar-0.2.5/README.md +662 -0
- mujoco_lidar-0.2.5/mujoco_lidar/__init__.py +32 -0
- mujoco_lidar-0.2.5/mujoco_lidar/lidar_wrapper.py +240 -0
- mujoco_lidar-0.2.5/mujoco_lidar/mj_lidar_utils.py +457 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_gen.py +193 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_gen_livox_ti.py +89 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/HAP.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/avia.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/horizon.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/mid360.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/mid40.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/mid70.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar/scan_mode/tele.npy +0 -0
- mujoco_lidar-0.2.5/mujoco_lidar.egg-info/PKG-INFO +689 -0
- mujoco_lidar-0.2.5/mujoco_lidar.egg-info/SOURCES.txt +20 -0
- mujoco_lidar-0.2.5/mujoco_lidar.egg-info/dependency_links.txt +1 -0
- mujoco_lidar-0.2.5/mujoco_lidar.egg-info/requires.txt +13 -0
- mujoco_lidar-0.2.5/mujoco_lidar.egg-info/top_level.txt +1 -0
- mujoco_lidar-0.2.5/pyproject.toml +51 -0
- mujoco_lidar-0.2.5/setup.cfg +4 -0
|
@@ -0,0 +1,23 @@
|
|
|
1
|
+
MIT License
|
|
2
|
+
|
|
3
|
+
Copyright (c) 2025 Yufei Jia
|
|
4
|
+
|
|
5
|
+
Permission is hereby granted, free of charge, to any person obtaining a copy
|
|
6
|
+
of this software and associated documentation files (the "Software"), to deal
|
|
7
|
+
in the Software without restriction, including without limitation the rights
|
|
8
|
+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
|
|
9
|
+
copies of the Software, and to permit persons to whom the Software is
|
|
10
|
+
furnished to do so, subject to the following conditions:
|
|
11
|
+
|
|
12
|
+
The above copyright notice and this permission notice shall be included in all
|
|
13
|
+
copies or substantial portions of the Software.
|
|
14
|
+
|
|
15
|
+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
|
16
|
+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
|
17
|
+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
|
|
18
|
+
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
|
|
19
|
+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
|
|
20
|
+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
|
|
21
|
+
SOFTWARE.
|
|
22
|
+
|
|
23
|
+
|
|
@@ -0,0 +1,689 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: mujoco_lidar
|
|
3
|
+
Version: 0.2.5
|
|
4
|
+
Summary: A LiDAR sensor designed for MuJoCo
|
|
5
|
+
Author-email: Yufei Jia <jyf23@mails.tsinghua.edu.cn>
|
|
6
|
+
License: MIT
|
|
7
|
+
Project-URL: Homepage, https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
8
|
+
Project-URL: Repository, https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
9
|
+
Classifier: Programming Language :: Python :: 3
|
|
10
|
+
Classifier: License :: OSI Approved :: MIT License
|
|
11
|
+
Classifier: Operating System :: OS Independent
|
|
12
|
+
Requires-Python: >=3.8
|
|
13
|
+
Description-Content-Type: text/markdown
|
|
14
|
+
License-File: LICENSE
|
|
15
|
+
Requires-Dist: numpy>=1.20.0
|
|
16
|
+
Requires-Dist: mujoco>=3.2.0
|
|
17
|
+
Requires-Dist: scipy
|
|
18
|
+
Requires-Dist: pynput
|
|
19
|
+
Requires-Dist: matplotlib
|
|
20
|
+
Requires-Dist: zhplot
|
|
21
|
+
Provides-Extra: taichi
|
|
22
|
+
Requires-Dist: taichi>=1.6.0; extra == "taichi"
|
|
23
|
+
Requires-Dist: tibvh>=0.1.2; extra == "taichi"
|
|
24
|
+
Provides-Extra: jax
|
|
25
|
+
Requires-Dist: jax[cuda12]; extra == "jax"
|
|
26
|
+
Dynamic: license-file
|
|
27
|
+
|
|
28
|
+
# MuJoCo-LiDAR: High-Performance LiDAR Simulation Based on MuJoCo
|
|
29
|
+
|
|
30
|
+
A high-performance LiDAR simulation tool based on MuJoCo, supporting CPU, Taichi, and JAX backends.
|
|
31
|
+
|
|
32
|
+
<p align="center">
|
|
33
|
+
<img src="./assets/go2.png" width="49%" />
|
|
34
|
+
<img src="./assets/g1.png" width="49%" />
|
|
35
|
+
</p>
|
|
36
|
+
<p align="center">
|
|
37
|
+
<img src="./assets/g1_native.png" width="32%" />
|
|
38
|
+
<img src="./assets/go2_native.png" width="32%" />
|
|
39
|
+
<img src="./assets/lidar_rviz.png" width="33%" />
|
|
40
|
+
</p>
|
|
41
|
+
|
|
42
|
+
[中文文档](README_zh.md)
|
|
43
|
+
|
|
44
|
+
## 🌟 Features
|
|
45
|
+
|
|
46
|
+
- **Multi-Backend Support**:
|
|
47
|
+
- **CPU Backend**: Based on MuJoCo's native `mj_multiRay` function, no GPU required, simple and easy to use.
|
|
48
|
+
- **Taichi Backend**: Utilizes Taichi for efficient GPU parallel computing, supports mesh scenes with millions of faces and height fields.
|
|
49
|
+
- **JAX Backend**: Utilizes JAX for GPU parallel computing and MJX integration, supports Height Field (Hfield).
|
|
50
|
+
- **High Performance**: GPU-accelerated backends can generate 1 million+ rays in milliseconds.
|
|
51
|
+
- **Dynamic Scenes**: Supports real-time BVH construction (Taichi backend) for dynamic scenes.
|
|
52
|
+
- **Multiple LiDAR Models**: Supports various scanning patterns:
|
|
53
|
+
- Livox non-repetitive scanning modes: mid360, mid70, mid40, tele, avia
|
|
54
|
+
- Velodyne HDL-64E, VLP-32C
|
|
55
|
+
- Ouster OS-128
|
|
56
|
+
- Customizable grid scanning patterns
|
|
57
|
+
- **Accurate Physical Simulation**: Ray tracing for MuJoCo geometry types.
|
|
58
|
+
- **Flexible API**: Provides unified Wrapper interface.
|
|
59
|
+
- **ROS Integration**: Ready-to-use ROS1 and ROS2 examples.
|
|
60
|
+
|
|
61
|
+
## 🔧 Installation
|
|
62
|
+
|
|
63
|
+
### System Requirements
|
|
64
|
+
|
|
65
|
+
**Basic Dependencies:**
|
|
66
|
+
- Python >= 3.8
|
|
67
|
+
- MuJoCo >= 3.2.0
|
|
68
|
+
- NumPy >= 1.20.0
|
|
69
|
+
|
|
70
|
+
**Optional Backend Dependencies:**
|
|
71
|
+
- **Taichi**: `taichi >= 1.6.0`, `tibvh`
|
|
72
|
+
- **JAX**: `jax[cuda12]`
|
|
73
|
+
|
|
74
|
+
### Quick Installation
|
|
75
|
+
|
|
76
|
+
You can install MuJoCo-LiDAR via pip from PyPI:
|
|
77
|
+
|
|
78
|
+
```bash
|
|
79
|
+
# 1. Install basic dependencies (CPU backend)
|
|
80
|
+
pip install mujoco-lidar
|
|
81
|
+
|
|
82
|
+
# Verify installation
|
|
83
|
+
python -c "import mujoco_lidar; print(mujoco_lidar.__version__)"
|
|
84
|
+
# should print the installed version, e.g., "0.2.5"
|
|
85
|
+
|
|
86
|
+
# 2.(Optional) Install Taichi backend dependencies
|
|
87
|
+
pip install mujoco-lidar[taichi]
|
|
88
|
+
|
|
89
|
+
# Verify Taichi installation
|
|
90
|
+
python -c "import taichi as ti; ti.init(ti.gpu)"
|
|
91
|
+
# should print something like:
|
|
92
|
+
# [Taichi] version 1.7.3, llvm 15.0.4, commit 5ec301be, linux, python 3.10.16
|
|
93
|
+
# [Taichi] Starting on arch=cuda
|
|
94
|
+
|
|
95
|
+
# 3.(Optional) Install JAX backend dependencies
|
|
96
|
+
pip install mujoco-lidar[jax]
|
|
97
|
+
|
|
98
|
+
|
|
99
|
+
# Verify JAX installation
|
|
100
|
+
python -c "import jax; print(jax.default_backend())"
|
|
101
|
+
# should print "gpu"
|
|
102
|
+
```
|
|
103
|
+
|
|
104
|
+
From Source Code:
|
|
105
|
+
|
|
106
|
+
```bash
|
|
107
|
+
# Clone the repository
|
|
108
|
+
git clone https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
109
|
+
cd MuJoCo-LiDAR
|
|
110
|
+
|
|
111
|
+
# 1. Install basic dependencies (CPU backend)
|
|
112
|
+
pip install -e .
|
|
113
|
+
|
|
114
|
+
# 2.(Optional) Install Taichi backend dependencies
|
|
115
|
+
pip install -e ".[taichi]"
|
|
116
|
+
|
|
117
|
+
# Verify Taichi installation
|
|
118
|
+
python -c "import taichi as ti; ti.init(ti.gpu)"
|
|
119
|
+
# should print something like:
|
|
120
|
+
# [Taichi] version 1.7.3, llvm 15.0.4, commit 5ec301be, linux, python 3.10.16
|
|
121
|
+
# [Taichi] Starting on arch=cuda
|
|
122
|
+
|
|
123
|
+
# 3.(Optional) Install JAX backend dependencies
|
|
124
|
+
pip install -e ".[jax]"
|
|
125
|
+
|
|
126
|
+
# Verify JAX installation
|
|
127
|
+
python -c "import jax; print(jax.default_backend())"
|
|
128
|
+
# should print "gpu"
|
|
129
|
+
```
|
|
130
|
+
|
|
131
|
+
**Notice**:
|
|
132
|
+
- CPU backend does not require Taichi and TIBVH, works out-of-the-box.
|
|
133
|
+
- Taichi backend requires a properly configured NVIDIA GPU with CUDA or other Taichi-supported GPUs.
|
|
134
|
+
- Currently, only Taichi and JAX backend supports batch environments.
|
|
135
|
+
|
|
136
|
+
## 📚 Usage Examples
|
|
137
|
+
|
|
138
|
+
[ROS Integration](#-ros-integration) provides quick start examples for ROS1/2, and [Unitree Go2/G1](#-more-examples).
|
|
139
|
+
|
|
140
|
+
MuJoCo-LiDAR provides usage approaches and backend options:
|
|
141
|
+
|
|
142
|
+
### Backend Selection
|
|
143
|
+
|
|
144
|
+
1. **CPU Backend**:
|
|
145
|
+
- Advantages: No GPU required, fewer dependencies.
|
|
146
|
+
- Use Cases: Simple scenes, fewer rays (<10000).
|
|
147
|
+
- Performance: Uses MuJoCo's native `mj_multiRay`.
|
|
148
|
+
|
|
149
|
+
2. **Taichi Backend**:
|
|
150
|
+
- Advantages: High performance, supports complex Mesh and Hfield scenes.
|
|
151
|
+
- Use Cases: Complex scenes, large number of rays, Mesh or Hfield geometries.
|
|
152
|
+
- Performance: GPU parallel computing with BVH acceleration.
|
|
153
|
+
|
|
154
|
+
3. **JAX Backend**:
|
|
155
|
+
- Advantages: High performance, supports **Batch Simulation** (multiple environments in parallel).
|
|
156
|
+
- Use Cases: Research involving JAX/MJX, large-scale parallel simulation, supports Primitives and Height Fields (Hfield).
|
|
157
|
+
- Note: Does not support Mesh geometries currently.
|
|
158
|
+
|
|
159
|
+
### Approach: Using Wrapper (Recommended)
|
|
160
|
+
|
|
161
|
+
The Wrapper approach provides a unified interface that automatically handles CPU and GPU backend differences. This is the **recommended approach**.
|
|
162
|
+
|
|
163
|
+
#### Example 1: CPU Backend + Wrapper (Scene Defined via String)
|
|
164
|
+
|
|
165
|
+
```python
|
|
166
|
+
import time
|
|
167
|
+
import mujoco
|
|
168
|
+
import mujoco.viewer
|
|
169
|
+
|
|
170
|
+
from mujoco_lidar import MjLidarWrapper
|
|
171
|
+
from mujoco_lidar import scan_gen
|
|
172
|
+
|
|
173
|
+
# Define a simple MuJoCo scene
|
|
174
|
+
simple_demo_scene = """
|
|
175
|
+
<mujoco model="simple_demo">
|
|
176
|
+
<worldbody>
|
|
177
|
+
<!-- Ground + Four Walls -->
|
|
178
|
+
<geom name="ground" type="plane" size="5 5 0.1" pos="0 0 0" rgba="0.2 0.9 0.9 1"/>
|
|
179
|
+
<geom name="wall1" type="box" size="1e-3 3 1" pos=" 3 0 1" rgba="0.9 0.9 0.9 1"/>
|
|
180
|
+
<geom name="wall2" type="box" size="1e-3 3 1" pos="-3 0 1" rgba="0.9 0.9 0.9 1"/>
|
|
181
|
+
<geom name="wall3" type="box" size="3 1e-3 1" pos="0 3 1" rgba="0.9 0.9 0.9 1"/>
|
|
182
|
+
<geom name="wall4" type="box" size="3 1e-3 1" pos="0 -3 1" rgba="0.9 0.9 0.9 1"/>
|
|
183
|
+
|
|
184
|
+
<!-- Various Geometries -->
|
|
185
|
+
<geom name="box1" type="box" size="0.5 0.5 0.5" pos="2 0 0.5" euler="45 -45 0" rgba="1 0 0 1"/>
|
|
186
|
+
<geom name="sphere1" type="sphere" size="0.5" pos="0 2 0.5" rgba="0 1 0 1"/>
|
|
187
|
+
<geom name="cylinder1" type="cylinder" size="0.4 0.6" pos="0 -2 0.4" euler="0 90 0" rgba="0 0 1 1"/>
|
|
188
|
+
|
|
189
|
+
<!-- LiDAR Site -->
|
|
190
|
+
<body name="lidar_base" pos="0 0 1" quat="1 0 0 0" mocap="true">
|
|
191
|
+
<inertial pos="0 0 0" mass="1e-4" diaginertia="1e-9 1e-9 1e-9"/>
|
|
192
|
+
<site name="lidar_site" size="0.001" type='sphere'/>
|
|
193
|
+
<geom type="box" size="0.1 0.1 0.1" density="0" contype="0" conaffinity="0" rgba="0.3 0.6 0.3 0.2"/>
|
|
194
|
+
</body>
|
|
195
|
+
</worldbody>
|
|
196
|
+
</mujoco>
|
|
197
|
+
"""
|
|
198
|
+
|
|
199
|
+
# Create MuJoCo model and data
|
|
200
|
+
mj_model = mujoco.MjModel.from_xml_string(simple_demo_scene)
|
|
201
|
+
mj_data = mujoco.MjData(mj_model)
|
|
202
|
+
|
|
203
|
+
# Generate scan pattern
|
|
204
|
+
rays_theta, rays_phi = scan_gen.generate_grid_scan_pattern(num_ray_cols=64, num_ray_rows=16)
|
|
205
|
+
|
|
206
|
+
# Get body ID to exclude (avoid LiDAR detecting itself)
|
|
207
|
+
exclude_body_id = mj_model.body("lidar_base").id
|
|
208
|
+
|
|
209
|
+
# Create CPU backend LiDAR sensor
|
|
210
|
+
lidar = MjLidarWrapper(
|
|
211
|
+
mj_model,
|
|
212
|
+
site_name="lidar_site",
|
|
213
|
+
backend="cpu", # Use CPU backend
|
|
214
|
+
cutoff_dist=50.0, # Maximum detection distance of 50 meters
|
|
215
|
+
args={'bodyexclude': exclude_body_id} # CPU backend specific parameter: exclude body
|
|
216
|
+
)
|
|
217
|
+
|
|
218
|
+
# Use in simulation loop
|
|
219
|
+
with mujoco.viewer.launch_passive(mj_model, mj_data) as viewer:
|
|
220
|
+
while viewer.is_running():
|
|
221
|
+
mujoco.mj_step(mj_model, mj_data)
|
|
222
|
+
viewer.sync()
|
|
223
|
+
|
|
224
|
+
# Perform ray tracing (Wrapper automatically handles pose updates)
|
|
225
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
226
|
+
|
|
227
|
+
# Get point cloud data (in local coordinate system)
|
|
228
|
+
points = lidar.get_hit_points() # shape: (N, 3)
|
|
229
|
+
distances = lidar.get_distances() # shape: (N,)
|
|
230
|
+
|
|
231
|
+
time.sleep(1./60.)
|
|
232
|
+
```
|
|
233
|
+
|
|
234
|
+
#### Example 2: Taichi Backend + Wrapper (Loading from MJCF File)
|
|
235
|
+
|
|
236
|
+
```python
|
|
237
|
+
import mujoco
|
|
238
|
+
from mujoco_lidar import MjLidarWrapper, scan_gen
|
|
239
|
+
|
|
240
|
+
# Load MuJoCo model from file
|
|
241
|
+
mj_model = mujoco.MjModel.from_xml_path("path/to/your/model.xml")
|
|
242
|
+
mj_data = mujoco.MjData(mj_model)
|
|
243
|
+
|
|
244
|
+
# Generate scan pattern (using Velodyne HDL-64)
|
|
245
|
+
rays_theta, rays_phi = scan_gen.generate_HDL64()
|
|
246
|
+
|
|
247
|
+
# Create Taichi backend LiDAR sensor
|
|
248
|
+
lidar = MjLidarWrapper(
|
|
249
|
+
mj_model,
|
|
250
|
+
site_name="lidar_site",
|
|
251
|
+
backend="taichi", # Use Taichi backend
|
|
252
|
+
cutoff_dist=100.0,
|
|
253
|
+
args={
|
|
254
|
+
'max_candidates': 64, # Taichi backend specific parameter: BVH candidate nodes
|
|
255
|
+
'ti_init_args': {'device_memory_GB': 4.0} # Taichi initialization parameters
|
|
256
|
+
}
|
|
257
|
+
)
|
|
258
|
+
|
|
259
|
+
# Simulation loop
|
|
260
|
+
with mujoco.viewer.launch_passive(mj_model, mj_data) as viewer:
|
|
261
|
+
while viewer.is_running():
|
|
262
|
+
mujoco.mj_step(mj_model, mj_data)
|
|
263
|
+
|
|
264
|
+
# Taichi backend usage is the same as CPU
|
|
265
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
266
|
+
points = lidar.get_hit_points()
|
|
267
|
+
```
|
|
268
|
+
|
|
269
|
+
#### Wrapper Parameter Description
|
|
270
|
+
|
|
271
|
+
```python
|
|
272
|
+
MjLidarWrapper(
|
|
273
|
+
mj_model, # MuJoCo model
|
|
274
|
+
site_name, # LiDAR site name
|
|
275
|
+
backend="cpu", # "cpu" or "taichi"
|
|
276
|
+
cutoff_dist=100.0, # Maximum detection distance (meters)
|
|
277
|
+
args={} # Backend-specific parameters
|
|
278
|
+
)
|
|
279
|
+
|
|
280
|
+
# CPU backend parameters (args)
|
|
281
|
+
{
|
|
282
|
+
'geomgroup': None, # Geometry group filter (0-5, None means all)
|
|
283
|
+
'bodyexclude': -1 # Body ID to exclude (-1 means no exclusion)
|
|
284
|
+
}
|
|
285
|
+
|
|
286
|
+
# Taichi backend parameters (args)
|
|
287
|
+
{
|
|
288
|
+
'max_candidates': 32, # Maximum BVH candidate nodes (16-128)
|
|
289
|
+
'ti_init_args': {} # Taichi initialization parameters
|
|
290
|
+
}
|
|
291
|
+
```
|
|
292
|
+
|
|
293
|
+
### Approach 2: Using Core Directly (Advanced Users)
|
|
294
|
+
|
|
295
|
+
The Core approach provides direct access to low-level APIs, suitable for advanced users who need fine-grained control.
|
|
296
|
+
|
|
297
|
+
#### Example 3: CPU Core Approach
|
|
298
|
+
|
|
299
|
+
```python
|
|
300
|
+
import numpy as np
|
|
301
|
+
import mujoco
|
|
302
|
+
from mujoco_lidar.core_cpu.mjlidar_cpu import MjLidarCPU
|
|
303
|
+
from mujoco_lidar import scan_gen
|
|
304
|
+
|
|
305
|
+
# Create model
|
|
306
|
+
mj_model = mujoco.MjModel.from_xml_string(xml_string)
|
|
307
|
+
mj_data = mujoco.MjData(mj_model)
|
|
308
|
+
|
|
309
|
+
# Generate scan pattern
|
|
310
|
+
rays_theta, rays_phi = scan_gen.generate_grid_scan_pattern(64, 16)
|
|
311
|
+
|
|
312
|
+
# Create CPU core instance
|
|
313
|
+
lidar_cpu = MjLidarCPU(
|
|
314
|
+
mj_model,
|
|
315
|
+
cutoff_dist=50.0,
|
|
316
|
+
geomgroup=None, # Detect all geometry groups
|
|
317
|
+
bodyexclude=-1 # Don't exclude any body
|
|
318
|
+
)
|
|
319
|
+
|
|
320
|
+
# Simulation loop
|
|
321
|
+
with mujoco.viewer.launch_passive(mj_model, mj_data) as viewer:
|
|
322
|
+
while viewer.is_running():
|
|
323
|
+
mujoco.mj_step(mj_model, mj_data)
|
|
324
|
+
|
|
325
|
+
# Manually construct 4x4 pose matrix
|
|
326
|
+
pose_4x4 = np.eye(4, dtype=np.float32)
|
|
327
|
+
pose_4x4[:3, 3] = mj_data.site("lidar_site").xpos
|
|
328
|
+
pose_4x4[:3, :3] = mj_data.site("lidar_site").xmat.reshape(3, 3)
|
|
329
|
+
|
|
330
|
+
# Update data and perform ray tracing
|
|
331
|
+
lidar_cpu.update(mj_data)
|
|
332
|
+
lidar_cpu.trace_rays(pose_4x4, rays_theta, rays_phi)
|
|
333
|
+
|
|
334
|
+
# Get results
|
|
335
|
+
points = lidar_cpu.get_hit_points()
|
|
336
|
+
distances = lidar_cpu.get_distances()
|
|
337
|
+
```
|
|
338
|
+
|
|
339
|
+
#### Example 4: Taichi Core Approach
|
|
340
|
+
|
|
341
|
+
```python
|
|
342
|
+
import numpy as np
|
|
343
|
+
import mujoco
|
|
344
|
+
import taichi as ti
|
|
345
|
+
from mujoco_lidar.core_ti.mjlidar_ti import MjLidarTi
|
|
346
|
+
from mujoco_lidar import scan_gen_livox_ti
|
|
347
|
+
|
|
348
|
+
# Initialize Taichi (must be done before creating MjLidarTi)
|
|
349
|
+
ti.init(arch=ti.gpu, device_memory_GB=4.0)
|
|
350
|
+
|
|
351
|
+
# Create model
|
|
352
|
+
mj_model = mujoco.MjModel.from_xml_string(xml_string)
|
|
353
|
+
mj_data = mujoco.MjData(mj_model)
|
|
354
|
+
|
|
355
|
+
# Use Livox scan pattern (Taichi optimized version)
|
|
356
|
+
livox_gen = scan_gen_livox_ti.LivoxGeneratorTi("mid360")
|
|
357
|
+
|
|
358
|
+
# Create Taichi core instance
|
|
359
|
+
lidar_ti = MjLidarTi(
|
|
360
|
+
mj_model,
|
|
361
|
+
cutoff_dist=100.0,
|
|
362
|
+
max_candidates=64 # BVH candidate nodes
|
|
363
|
+
)
|
|
364
|
+
|
|
365
|
+
# Get ray angles in Taichi format
|
|
366
|
+
rays_theta_ti, rays_phi_ti = livox_gen.sample_ray_angles_ti()
|
|
367
|
+
|
|
368
|
+
# Simulation loop
|
|
369
|
+
with mujoco.viewer.launch_passive(mj_model, mj_data) as viewer:
|
|
370
|
+
while viewer.is_running():
|
|
371
|
+
mujoco.mj_step(mj_model, mj_data)
|
|
372
|
+
|
|
373
|
+
# Manually construct pose matrix
|
|
374
|
+
pose_4x4 = np.eye(4, dtype=np.float32)
|
|
375
|
+
pose_4x4[:3, 3] = mj_data.site("lidar_site").xpos
|
|
376
|
+
pose_4x4[:3, :3] = mj_data.site("lidar_site").xmat.reshape(3, 3)
|
|
377
|
+
|
|
378
|
+
# Update BVH and perform ray tracing
|
|
379
|
+
lidar_ti.update(mj_data)
|
|
380
|
+
lidar_ti.trace_rays(pose_4x4, rays_theta_ti, rays_phi_ti)
|
|
381
|
+
|
|
382
|
+
# For Livox, resample angles each time
|
|
383
|
+
rays_theta_ti, rays_phi_ti = livox_gen.sample_ray_angles_ti()
|
|
384
|
+
|
|
385
|
+
# Get results (copy from Taichi to CPU)
|
|
386
|
+
points = lidar_ti.get_hit_points() # Returns numpy array
|
|
387
|
+
distances = lidar_ti.get_distances()
|
|
388
|
+
```
|
|
389
|
+
|
|
390
|
+
#### Example 5: JAX Backend (Batch Processing)
|
|
391
|
+
|
|
392
|
+
Ideal for MJX or other JAX-based massive parallel simulation environments.
|
|
393
|
+
|
|
394
|
+
```python
|
|
395
|
+
import jax
|
|
396
|
+
import jax.numpy as jnp
|
|
397
|
+
from mujoco_lidar.core_jax import MjLidarJax
|
|
398
|
+
|
|
399
|
+
# Initialize JAX Lidar (using host model)
|
|
400
|
+
lidar = MjLidarJax(mj_model)
|
|
401
|
+
|
|
402
|
+
# Prepare batch data (e.g., from MJX state)
|
|
403
|
+
# batch_size = 4096
|
|
404
|
+
# geom_xpos: (B, Ngeom, 3)
|
|
405
|
+
# geom_xmat: (B, Ngeom, 3, 3)
|
|
406
|
+
# rays_origin: (B, 3)
|
|
407
|
+
# rays_direction: (B, Nrays, 3)
|
|
408
|
+
|
|
409
|
+
# Perform batch rendering
|
|
410
|
+
# Returns distances: (B, Nrays)
|
|
411
|
+
batch_distances = lidar.render_batch(
|
|
412
|
+
batch_geom_xpos,
|
|
413
|
+
batch_geom_xmat,
|
|
414
|
+
batch_rays_origin,
|
|
415
|
+
batch_rays_direction
|
|
416
|
+
)
|
|
417
|
+
```
|
|
418
|
+
|
|
419
|
+
## 🤖 ROS Integration
|
|
420
|
+
|
|
421
|
+
MuJoCo-LiDAR provides complete ROS1 and ROS2 integration examples, supporting point cloud publishing and scene visualization.
|
|
422
|
+
|
|
423
|
+
### ROS1 Example
|
|
424
|
+
|
|
425
|
+
ROS1 related dependencies need to be installed in advance
|
|
426
|
+
|
|
427
|
+
```bash
|
|
428
|
+
# First terminal: Start ROS core
|
|
429
|
+
roscore
|
|
430
|
+
|
|
431
|
+
# Second terminal: Run LiDAR simulation (using Taichi backend). RViz will be automatically launched.
|
|
432
|
+
python examples/lidar_vis_ros1_wrapper.py --lidar mid360 --rate 12
|
|
433
|
+
```
|
|
434
|
+
|
|
435
|
+
### ROS2 Examples
|
|
436
|
+
|
|
437
|
+
**Approach 1: Using Wrapper (Recommended)**
|
|
438
|
+
|
|
439
|
+
```bash
|
|
440
|
+
# Run LiDAR simulation. RViz2 will be automatically launched.
|
|
441
|
+
python examples/lidar_vis_ros2_wrapper.py --lidar mid360 --rate 12
|
|
442
|
+
```
|
|
443
|
+
|
|
444
|
+
**Approach 2: Using Core (Advanced)**
|
|
445
|
+
|
|
446
|
+
```bash
|
|
447
|
+
# Using low-level Taichi Core API. RViz2 will be automatically launched.
|
|
448
|
+
python examples/lidar_vis_ros2.py --lidar mid360 --rate 12
|
|
449
|
+
```
|
|
450
|
+
|
|
451
|
+
### ROS Example Command Line Arguments
|
|
452
|
+
|
|
453
|
+
Both ROS examples support the following command line arguments:
|
|
454
|
+
|
|
455
|
+
```bash
|
|
456
|
+
python examples/lidar_vis_ros2_wrapper.py [options]
|
|
457
|
+
|
|
458
|
+
Options:
|
|
459
|
+
--lidar MODEL Specify LiDAR model, available values:
|
|
460
|
+
- Livox series: avia, mid40, mid70, mid360, tele
|
|
461
|
+
- Velodyne series: HDL64, vlp32
|
|
462
|
+
- Ouster series: os128
|
|
463
|
+
- Custom: custom
|
|
464
|
+
Default: mid360
|
|
465
|
+
--verbose Show detailed output, including position, orientation, and performance statistics
|
|
466
|
+
--rate HZ Set point cloud publishing rate (Hz), default: 12
|
|
467
|
+
```
|
|
468
|
+
|
|
469
|
+
**Usage Examples:**
|
|
470
|
+
|
|
471
|
+
```bash
|
|
472
|
+
# Use HDL64 LiDAR, enable verbose output, set publishing rate to 10Hz
|
|
473
|
+
python examples/lidar_vis_ros2_wrapper.py --lidar HDL64 --verbose --rate 10
|
|
474
|
+
|
|
475
|
+
# Use Velodyne VLP-32, default rate
|
|
476
|
+
python examples/lidar_vis_ros2_wrapper.py --lidar vlp32
|
|
477
|
+
|
|
478
|
+
# Use custom scan pattern
|
|
479
|
+
python examples/lidar_vis_ros2_wrapper.py --lidar custom
|
|
480
|
+
|
|
481
|
+
```
|
|
482
|
+
|
|
483
|
+
### Keyboard Controls
|
|
484
|
+
|
|
485
|
+
In ROS examples, you can control the LiDAR's position and orientation using the keyboard:
|
|
486
|
+
|
|
487
|
+
**Movement Controls:**
|
|
488
|
+
- `W`: Move forward
|
|
489
|
+
- `S`: Move backward
|
|
490
|
+
- `A`: Move left
|
|
491
|
+
- `D`: Move right
|
|
492
|
+
- `Q`: Move up
|
|
493
|
+
- `E`: Move down
|
|
494
|
+
|
|
495
|
+
**Orientation Controls:**
|
|
496
|
+
- `↑`: Pitch up
|
|
497
|
+
- `↓`: Pitch down
|
|
498
|
+
- `←`: Yaw left
|
|
499
|
+
- `→`: Yaw right
|
|
500
|
+
|
|
501
|
+
**Other:**
|
|
502
|
+
- `ESC`: Exit program
|
|
503
|
+
|
|
504
|
+
### ROS Topics
|
|
505
|
+
|
|
506
|
+
Example programs publish the following ROS topics:
|
|
507
|
+
|
|
508
|
+
| Topic Name | Message Type | Description |
|
|
509
|
+
|-----------|-------------|-------------|
|
|
510
|
+
| `/lidar_points` | `sensor_msgs/PointCloud2` | LiDAR point cloud data |
|
|
511
|
+
| `/mujoco_scene` | `visualization_msgs/MarkerArray` | MuJoCo scene geometry visualization |
|
|
512
|
+
| `/tf` | `tf2_msgs/TFMessage` | LiDAR coordinate transforms |
|
|
513
|
+
|
|
514
|
+
### Wrapper vs Core in ROS
|
|
515
|
+
|
|
516
|
+
**`lidar_vis_ros2_wrapper.py` (Wrapper Approach)**:
|
|
517
|
+
- Uses `MjLidarWrapper` class
|
|
518
|
+
- Automatically handles data format conversion (numpy ↔ Taichi)
|
|
519
|
+
- More concise code, easier to maintain
|
|
520
|
+
- Suitable for most application scenarios
|
|
521
|
+
|
|
522
|
+
```python
|
|
523
|
+
from mujoco_lidar import MjLidarWrapper
|
|
524
|
+
|
|
525
|
+
# Create Wrapper instance
|
|
526
|
+
lidar = MjLidarWrapper(mj_model, site_name="lidar_site", backend="taichi")
|
|
527
|
+
|
|
528
|
+
# Simple call
|
|
529
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
530
|
+
points = lidar.get_hit_points() # Automatically returns numpy array
|
|
531
|
+
```
|
|
532
|
+
|
|
533
|
+
**`lidar_vis_ros2.py` (Core Approach)**:
|
|
534
|
+
- Directly uses `MjLidarTi` class
|
|
535
|
+
- Need to manually manage Taichi data format
|
|
536
|
+
- Need to manually construct 4x4 pose matrix
|
|
537
|
+
- More room for performance optimization, suitable for advanced users
|
|
538
|
+
|
|
539
|
+
```python
|
|
540
|
+
from mujoco_lidar.core_ti.mjlidar_ti import MjLidarTi
|
|
541
|
+
import taichi as ti
|
|
542
|
+
|
|
543
|
+
# Must initialize Taichi first
|
|
544
|
+
ti.init(arch=ti.gpu)
|
|
545
|
+
|
|
546
|
+
# Create Core instance
|
|
547
|
+
lidar = MjLidarTi(mj_model)
|
|
548
|
+
|
|
549
|
+
# Need Taichi ndarray format
|
|
550
|
+
rays_theta_ti = ti.ndarray(dtype=ti.f32, shape=n_rays)
|
|
551
|
+
rays_phi_ti = ti.ndarray(dtype=ti.f32, shape=n_rays)
|
|
552
|
+
rays_theta_ti.from_numpy(rays_theta)
|
|
553
|
+
rays_phi_ti.from_numpy(rays_phi)
|
|
554
|
+
|
|
555
|
+
# Manually construct pose matrix
|
|
556
|
+
pose_4x4 = np.eye(4, dtype=np.float32)
|
|
557
|
+
pose_4x4[:3, 3] = mj_data.site("lidar_site").xpos
|
|
558
|
+
pose_4x4[:3, :3] = mj_data.site("lidar_site").xmat.reshape(3, 3)
|
|
559
|
+
|
|
560
|
+
# Call
|
|
561
|
+
lidar.update(mj_data)
|
|
562
|
+
lidar.trace_rays(pose_4x4, rays_theta_ti, rays_phi_ti)
|
|
563
|
+
points = lidar.get_hit_points() # Copy from GPU to CPU
|
|
564
|
+
```
|
|
565
|
+
|
|
566
|
+
## 🤝 More Examples
|
|
567
|
+
|
|
568
|
+
We also provide ROS2 integration examples with Unitree Go2 quadruped robot and G1 humanoid robot.
|
|
569
|
+
|
|
570
|
+
```bash
|
|
571
|
+
# Install onnx runtime
|
|
572
|
+
pip install onnxruntime
|
|
573
|
+
|
|
574
|
+
# go2 example
|
|
575
|
+
python examples/unitree_go2_ros2.py --lidar mid360
|
|
576
|
+
# Choose other lidar, for example: --lidar airy
|
|
577
|
+
|
|
578
|
+
# g1 example
|
|
579
|
+
python examples/unitree_g1_ros2.py --lidar mid360
|
|
580
|
+
```
|
|
581
|
+
|
|
582
|
+
## ⚡ Performance Optimization and Best Practices
|
|
583
|
+
|
|
584
|
+
### 1. Reduce Ray Tracing Frequency
|
|
585
|
+
|
|
586
|
+
LiDAR doesn't need to run at the same frequency as physics simulation:
|
|
587
|
+
|
|
588
|
+
```python
|
|
589
|
+
lidar_rate = 10 # LiDAR at 10Hz
|
|
590
|
+
physics_rate = 60 # Physics simulation at 60Hz
|
|
591
|
+
step_cnt = 0
|
|
592
|
+
|
|
593
|
+
with mujoco.viewer.launch_passive(mj_model, mj_data) as viewer:
|
|
594
|
+
while viewer.is_running():
|
|
595
|
+
# High-frequency physics simulation
|
|
596
|
+
mujoco.mj_step(mj_model, mj_data)
|
|
597
|
+
step_cnt += 1
|
|
598
|
+
|
|
599
|
+
# Low-frequency LiDAR scanning
|
|
600
|
+
if step_cnt % (physics_rate // lidar_rate) == 0:
|
|
601
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
602
|
+
points = lidar.get_hit_points()
|
|
603
|
+
```
|
|
604
|
+
|
|
605
|
+
### 2. Reuse Ray Angle Arrays
|
|
606
|
+
|
|
607
|
+
For fixed scan patterns (non-Livox), generate angle arrays only once:
|
|
608
|
+
|
|
609
|
+
```python
|
|
610
|
+
# ✅ Correct: Generate once outside loop
|
|
611
|
+
rays_theta, rays_phi = scan_gen.generate_HDL64()
|
|
612
|
+
|
|
613
|
+
while True:
|
|
614
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
615
|
+
|
|
616
|
+
# ❌ Wrong: Regenerate every loop (wasteful)
|
|
617
|
+
while True:
|
|
618
|
+
rays_theta, rays_phi = scan_gen.generate_HDL64() # Unnecessary!
|
|
619
|
+
lidar.trace_rays(mj_data, rays_theta, rays_phi)
|
|
620
|
+
```
|
|
621
|
+
|
|
622
|
+
### 3. Use Taichi Arrays with Taichi Backend
|
|
623
|
+
|
|
624
|
+
When using Taichi Core approach, avoid frequent numpy↔Taichi conversions:
|
|
625
|
+
|
|
626
|
+
```python
|
|
627
|
+
import taichi as ti
|
|
628
|
+
|
|
629
|
+
# ✅ Correct: Use Taichi ndarray
|
|
630
|
+
rays_theta_ti = ti.ndarray(dtype=ti.f32, shape=n_rays)
|
|
631
|
+
rays_phi_ti = ti.ndarray(dtype=ti.f32, shape=n_rays)
|
|
632
|
+
rays_theta_ti.from_numpy(rays_theta) # Convert only once
|
|
633
|
+
rays_phi_ti.from_numpy(rays_phi)
|
|
634
|
+
|
|
635
|
+
while True:
|
|
636
|
+
lidar.trace_rays(pose_4x4, rays_theta_ti, rays_phi_ti) # Use directly
|
|
637
|
+
|
|
638
|
+
# ❌ Wrong: Convert every time (high overhead)
|
|
639
|
+
while True:
|
|
640
|
+
theta_ti = ti.ndarray(dtype=ti.f32, shape=n_rays)
|
|
641
|
+
theta_ti.from_numpy(rays_theta) # Frequent conversion!
|
|
642
|
+
# ...
|
|
643
|
+
```
|
|
644
|
+
|
|
645
|
+
### 4. Livox Scan Pattern Optimization
|
|
646
|
+
|
|
647
|
+
When using Taichi backend, for Livox non-repetitive scanning, use Taichi optimized version:
|
|
648
|
+
|
|
649
|
+
```python
|
|
650
|
+
from mujoco_lidar import scan_gen_livox_ti
|
|
651
|
+
import taichi as ti
|
|
652
|
+
|
|
653
|
+
ti.init(arch=ti.gpu)
|
|
654
|
+
|
|
655
|
+
# ✅ Taichi optimized version: Returns Taichi arrays directly, no conversion needed
|
|
656
|
+
livox_gen = scan_gen_livox_ti.LivoxGeneratorTi("mid360")
|
|
657
|
+
rays_theta_ti, rays_phi_ti = livox_gen.sample_ray_angles_ti()
|
|
658
|
+
|
|
659
|
+
# ❌ CPU version: Need numpy→Taichi conversion every time
|
|
660
|
+
livox_gen = scan_gen.LivoxGenerator("mid360")
|
|
661
|
+
rays_theta, rays_phi = livox_gen.sample_ray_angles()
|
|
662
|
+
# Still need to convert to Taichi format...
|
|
663
|
+
```
|
|
664
|
+
|
|
665
|
+
### 5. Properly Set Scene Complexity
|
|
666
|
+
|
|
667
|
+
- Remove geometries outside the field of view
|
|
668
|
+
- Use geomgroup to organize the scene
|
|
669
|
+
- Simplify geometry shapes of unimportant objects
|
|
670
|
+
- For mesh models, consider reducing face count
|
|
671
|
+
- When using height fields, it is recommended to use Taichi backend (rather than JAX backend) for better performance
|
|
672
|
+
|
|
673
|
+
## 📄 License
|
|
674
|
+
|
|
675
|
+
This project is licensed under the MIT License - see the [LICENSE](LICENSE) file for details
|
|
676
|
+
|
|
677
|
+
## 📜 Citation
|
|
678
|
+
|
|
679
|
+
If you find MuJoCo-LiDAR useful in your research, please consider citing our work:
|
|
680
|
+
|
|
681
|
+
```bibtex
|
|
682
|
+
@article{jia2025discoverse,
|
|
683
|
+
title={DISCOVERSE: Efficient Robot Simulation in Complex High-Fidelity Environments},
|
|
684
|
+
author={Yufei Jia and Guangyu Wang and Yuhang Dong and Junzhe Wu and Yupei Zeng and Haonan Lin and Zifan Wang and Haizhou Ge and Weibin Gu and Chuxuan Li and Ziming Wang and Yunjie Cheng and Wei Sui and Ruqi Huang and Guyue Zhou},
|
|
685
|
+
journal={arXiv preprint arXiv:2507.21981},
|
|
686
|
+
year={2025},
|
|
687
|
+
url={https://arxiv.org/abs/2507.21981}
|
|
688
|
+
}
|
|
689
|
+
```
|