mujoco_lidar 0.3.0__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.3.0/LICENSE +23 -0
- mujoco_lidar-0.3.0/PKG-INFO +175 -0
- mujoco_lidar-0.3.0/README.md +141 -0
- mujoco_lidar-0.3.0/pyproject.toml +78 -0
- mujoco_lidar-0.3.0/setup.cfg +4 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/__init__.py +43 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/lidar_wrapper.py +205 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/mj_lidar_utils.py +459 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_gen.py +278 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_gen_livox_ti.py +133 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/HAP.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/avia.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/horizon.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/mid360.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/mid40.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/mid70.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar/scan_mode/tele.npy +0 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar.egg-info/PKG-INFO +175 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar.egg-info/SOURCES.txt +27 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar.egg-info/dependency_links.txt +1 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar.egg-info/requires.txt +22 -0
- mujoco_lidar-0.3.0/src/mujoco_lidar.egg-info/top_level.txt +1 -0
- mujoco_lidar-0.3.0/tests/test_core.py +20 -0
- mujoco_lidar-0.3.0/tests/test_cpu_backend.py +14 -0
- mujoco_lidar-0.3.0/tests/test_integration.py +17 -0
- mujoco_lidar-0.3.0/tests/test_raytracing.py +22 -0
- mujoco_lidar-0.3.0/tests/test_scan_gen.py +13 -0
- mujoco_lidar-0.3.0/tests/test_scan_patterns.py +27 -0
- mujoco_lidar-0.3.0/tests/test_wrapper.py +21 -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,175 @@
|
|
|
1
|
+
Metadata-Version: 2.4
|
|
2
|
+
Name: mujoco_lidar
|
|
3
|
+
Version: 0.3.0
|
|
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.10
|
|
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
|
+
Provides-Extra: dev
|
|
27
|
+
Requires-Dist: pytest>=7.0; extra == "dev"
|
|
28
|
+
Requires-Dist: pytest-cov>=4.0; extra == "dev"
|
|
29
|
+
Requires-Dist: ruff>=0.3.0; extra == "dev"
|
|
30
|
+
Provides-Extra: examples
|
|
31
|
+
Requires-Dist: onnxruntime>=1.19.2; extra == "examples"
|
|
32
|
+
Requires-Dist: etils[epath]>=1.13.0; extra == "examples"
|
|
33
|
+
Dynamic: license-file
|
|
34
|
+
|
|
35
|
+
# MuJoCo-LiDAR: High-Performance LiDAR Simulation
|
|
36
|
+
|
|
37
|
+
[](https://pypi.org/project/mujoco-lidar/)
|
|
38
|
+
[](https://pypi.org/project/mujoco-lidar/)
|
|
39
|
+
|
|
40
|
+
High-performance LiDAR simulation for MuJoCo with CPU, Taichi, and JAX backends.
|
|
41
|
+
|
|
42
|
+
<p align="center">
|
|
43
|
+
<img src="./assets/go2.png" width="49%" />
|
|
44
|
+
<img src="./assets/g1.png" width="49%" />
|
|
45
|
+
</p>
|
|
46
|
+
<p align="center">
|
|
47
|
+
<img src="./assets/g1_native.png" width="32%" />
|
|
48
|
+
<img src="./assets/go2_native.png" width="32%" />
|
|
49
|
+
<img src="./assets/lidar_rviz.png" width="33%" />
|
|
50
|
+
</p>
|
|
51
|
+
|
|
52
|
+
[中文文档](README_zh.md) | [Installation](docs/en/INSTALLATION.md) | [Usage Guide](docs/en/USAGE.md) | [Development](docs/en/DEVELOPMENT.md) | [Contributing](CONTRIBUTING.md)
|
|
53
|
+
|
|
54
|
+
## Features
|
|
55
|
+
|
|
56
|
+
- **Multi-Backend Support**:
|
|
57
|
+
- **CPU**: MuJoCo native `mj_multiRay`, no GPU required
|
|
58
|
+
- **Taichi**: GPU parallel computing, supports Mesh and Hfield
|
|
59
|
+
- **JAX**: GPU + MJX integration, batch simulation support
|
|
60
|
+
- **High Performance**: 1M+ rays/sec on GPU, real-time BVH construction
|
|
61
|
+
- **Multiple LiDAR Models**: Velodyne (HDL-64E, VLP-32C), Livox (mid360, avia), Ouster (OS-128), custom patterns
|
|
62
|
+
- **ROS Integration**: Ready-to-use ROS1/ROS2 examples
|
|
63
|
+
|
|
64
|
+
## Quick Start
|
|
65
|
+
|
|
66
|
+
### Installation
|
|
67
|
+
|
|
68
|
+
**From PyPI:**
|
|
69
|
+
|
|
70
|
+
```bash
|
|
71
|
+
# Basic (CPU only)
|
|
72
|
+
uv add mujoco-lidar
|
|
73
|
+
|
|
74
|
+
# With Taichi backend (GPU)
|
|
75
|
+
uv add "mujoco-lidar[taichi]"
|
|
76
|
+
|
|
77
|
+
# With JAX backend (GPU + batch)
|
|
78
|
+
uv add "mujoco-lidar[jax]"
|
|
79
|
+
```
|
|
80
|
+
|
|
81
|
+
**From Source:**
|
|
82
|
+
|
|
83
|
+
```bash
|
|
84
|
+
git clone https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
85
|
+
cd MuJoCo-LiDAR
|
|
86
|
+
|
|
87
|
+
uv sync --extra dev # CPU only
|
|
88
|
+
uv sync --extra dev --extra taichi # with Taichi backend
|
|
89
|
+
uv sync --extra dev --extra taichi --extra jax # all backends
|
|
90
|
+
```
|
|
91
|
+
|
|
92
|
+
See [Installation Guide](docs/en/INSTALLATION.md) for details.
|
|
93
|
+
|
|
94
|
+
### Basic Usage
|
|
95
|
+
|
|
96
|
+
```python
|
|
97
|
+
import mujoco
|
|
98
|
+
from mujoco_lidar import MjLidarWrapper, scan_gen
|
|
99
|
+
|
|
100
|
+
# Load model
|
|
101
|
+
model = mujoco.MjModel.from_xml_path("scene.xml")
|
|
102
|
+
data = mujoco.MjData(model)
|
|
103
|
+
|
|
104
|
+
# Create LiDAR
|
|
105
|
+
lidar = MjLidarWrapper(
|
|
106
|
+
model,
|
|
107
|
+
site_name="lidar_site",
|
|
108
|
+
backend="cpu", # or "taichi", "jax"
|
|
109
|
+
cutoff_dist=50.0
|
|
110
|
+
)
|
|
111
|
+
|
|
112
|
+
# Generate scan pattern
|
|
113
|
+
theta, phi = scan_gen.generate_HDL64()
|
|
114
|
+
|
|
115
|
+
# Trace rays
|
|
116
|
+
ranges = lidar.trace_rays(data, theta, phi)
|
|
117
|
+
```
|
|
118
|
+
|
|
119
|
+
See [Usage Guide](docs/en/USAGE.md) for more examples.
|
|
120
|
+
|
|
121
|
+
## Performance
|
|
122
|
+
|
|
123
|
+
| Backend | Rays/sec | Hardware | Batch Support |
|
|
124
|
+
|---------|----------|----------|---------------|
|
|
125
|
+
| CPU | ~9M | Native | No |
|
|
126
|
+
| Taichi | ~62M | GPU | Yes |
|
|
127
|
+
| JAX | ~231M | GPU | Yes |
|
|
128
|
+
|
|
129
|
+
Run benchmarks: `make benchmark`
|
|
130
|
+
|
|
131
|
+
## Documentation
|
|
132
|
+
|
|
133
|
+
- [Installation Guide](docs/en/INSTALLATION.md) - Detailed installation instructions
|
|
134
|
+
- [Usage Guide](docs/en/USAGE.md) - Examples and tutorials
|
|
135
|
+
- [API Reference](docs/en/API.md) - Complete API documentation
|
|
136
|
+
- [Development Guide](docs/en/DEVELOPMENT.md) - Contributing and testing
|
|
137
|
+
- [Project Structure](docs/en/PROJECT_STRUCTURE.md) - Codebase organization
|
|
138
|
+
|
|
139
|
+
## Examples
|
|
140
|
+
|
|
141
|
+
- [examples/example_native.py](examples/example_native.py) - CPU backend
|
|
142
|
+
- [examples/example_taichi.py](examples/example_taichi.py) - Taichi backend
|
|
143
|
+
- [examples/lidar_vis_ros2.py](examples/lidar_vis_ros2.py) - ROS2 integration
|
|
144
|
+
- [examples/unitree_go2_ros2.py](examples/unitree_go2_ros2.py) - Unitree Go2 robot
|
|
145
|
+
|
|
146
|
+
## Development
|
|
147
|
+
|
|
148
|
+
```bash
|
|
149
|
+
git clone https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
150
|
+
cd MuJoCo-LiDAR
|
|
151
|
+
uv sync --extra dev
|
|
152
|
+
|
|
153
|
+
make test # Run tests
|
|
154
|
+
make lint # Check code quality
|
|
155
|
+
make benchmark # Run performance tests
|
|
156
|
+
```
|
|
157
|
+
|
|
158
|
+
See [CONTRIBUTING.md](CONTRIBUTING.md) for contribution guidelines.
|
|
159
|
+
|
|
160
|
+
## License
|
|
161
|
+
|
|
162
|
+
MIT License - see [LICENSE](LICENSE) for details.
|
|
163
|
+
|
|
164
|
+
## Citation
|
|
165
|
+
|
|
166
|
+
If you use this project in your research, please cite:
|
|
167
|
+
|
|
168
|
+
```bibtex
|
|
169
|
+
@software{mujoco_lidar,
|
|
170
|
+
title = {MuJoCo-LiDAR: High-Performance LiDAR Simulation},
|
|
171
|
+
author = {Yufei Jia},
|
|
172
|
+
year = {2024},
|
|
173
|
+
url = {https://github.com/TATP-233/MuJoCo-LiDAR}
|
|
174
|
+
}
|
|
175
|
+
```
|
|
@@ -0,0 +1,141 @@
|
|
|
1
|
+
# MuJoCo-LiDAR: High-Performance LiDAR Simulation
|
|
2
|
+
|
|
3
|
+
[](https://pypi.org/project/mujoco-lidar/)
|
|
4
|
+
[](https://pypi.org/project/mujoco-lidar/)
|
|
5
|
+
|
|
6
|
+
High-performance LiDAR simulation for MuJoCo with CPU, Taichi, and JAX backends.
|
|
7
|
+
|
|
8
|
+
<p align="center">
|
|
9
|
+
<img src="./assets/go2.png" width="49%" />
|
|
10
|
+
<img src="./assets/g1.png" width="49%" />
|
|
11
|
+
</p>
|
|
12
|
+
<p align="center">
|
|
13
|
+
<img src="./assets/g1_native.png" width="32%" />
|
|
14
|
+
<img src="./assets/go2_native.png" width="32%" />
|
|
15
|
+
<img src="./assets/lidar_rviz.png" width="33%" />
|
|
16
|
+
</p>
|
|
17
|
+
|
|
18
|
+
[中文文档](README_zh.md) | [Installation](docs/en/INSTALLATION.md) | [Usage Guide](docs/en/USAGE.md) | [Development](docs/en/DEVELOPMENT.md) | [Contributing](CONTRIBUTING.md)
|
|
19
|
+
|
|
20
|
+
## Features
|
|
21
|
+
|
|
22
|
+
- **Multi-Backend Support**:
|
|
23
|
+
- **CPU**: MuJoCo native `mj_multiRay`, no GPU required
|
|
24
|
+
- **Taichi**: GPU parallel computing, supports Mesh and Hfield
|
|
25
|
+
- **JAX**: GPU + MJX integration, batch simulation support
|
|
26
|
+
- **High Performance**: 1M+ rays/sec on GPU, real-time BVH construction
|
|
27
|
+
- **Multiple LiDAR Models**: Velodyne (HDL-64E, VLP-32C), Livox (mid360, avia), Ouster (OS-128), custom patterns
|
|
28
|
+
- **ROS Integration**: Ready-to-use ROS1/ROS2 examples
|
|
29
|
+
|
|
30
|
+
## Quick Start
|
|
31
|
+
|
|
32
|
+
### Installation
|
|
33
|
+
|
|
34
|
+
**From PyPI:**
|
|
35
|
+
|
|
36
|
+
```bash
|
|
37
|
+
# Basic (CPU only)
|
|
38
|
+
uv add mujoco-lidar
|
|
39
|
+
|
|
40
|
+
# With Taichi backend (GPU)
|
|
41
|
+
uv add "mujoco-lidar[taichi]"
|
|
42
|
+
|
|
43
|
+
# With JAX backend (GPU + batch)
|
|
44
|
+
uv add "mujoco-lidar[jax]"
|
|
45
|
+
```
|
|
46
|
+
|
|
47
|
+
**From Source:**
|
|
48
|
+
|
|
49
|
+
```bash
|
|
50
|
+
git clone https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
51
|
+
cd MuJoCo-LiDAR
|
|
52
|
+
|
|
53
|
+
uv sync --extra dev # CPU only
|
|
54
|
+
uv sync --extra dev --extra taichi # with Taichi backend
|
|
55
|
+
uv sync --extra dev --extra taichi --extra jax # all backends
|
|
56
|
+
```
|
|
57
|
+
|
|
58
|
+
See [Installation Guide](docs/en/INSTALLATION.md) for details.
|
|
59
|
+
|
|
60
|
+
### Basic Usage
|
|
61
|
+
|
|
62
|
+
```python
|
|
63
|
+
import mujoco
|
|
64
|
+
from mujoco_lidar import MjLidarWrapper, scan_gen
|
|
65
|
+
|
|
66
|
+
# Load model
|
|
67
|
+
model = mujoco.MjModel.from_xml_path("scene.xml")
|
|
68
|
+
data = mujoco.MjData(model)
|
|
69
|
+
|
|
70
|
+
# Create LiDAR
|
|
71
|
+
lidar = MjLidarWrapper(
|
|
72
|
+
model,
|
|
73
|
+
site_name="lidar_site",
|
|
74
|
+
backend="cpu", # or "taichi", "jax"
|
|
75
|
+
cutoff_dist=50.0
|
|
76
|
+
)
|
|
77
|
+
|
|
78
|
+
# Generate scan pattern
|
|
79
|
+
theta, phi = scan_gen.generate_HDL64()
|
|
80
|
+
|
|
81
|
+
# Trace rays
|
|
82
|
+
ranges = lidar.trace_rays(data, theta, phi)
|
|
83
|
+
```
|
|
84
|
+
|
|
85
|
+
See [Usage Guide](docs/en/USAGE.md) for more examples.
|
|
86
|
+
|
|
87
|
+
## Performance
|
|
88
|
+
|
|
89
|
+
| Backend | Rays/sec | Hardware | Batch Support |
|
|
90
|
+
|---------|----------|----------|---------------|
|
|
91
|
+
| CPU | ~9M | Native | No |
|
|
92
|
+
| Taichi | ~62M | GPU | Yes |
|
|
93
|
+
| JAX | ~231M | GPU | Yes |
|
|
94
|
+
|
|
95
|
+
Run benchmarks: `make benchmark`
|
|
96
|
+
|
|
97
|
+
## Documentation
|
|
98
|
+
|
|
99
|
+
- [Installation Guide](docs/en/INSTALLATION.md) - Detailed installation instructions
|
|
100
|
+
- [Usage Guide](docs/en/USAGE.md) - Examples and tutorials
|
|
101
|
+
- [API Reference](docs/en/API.md) - Complete API documentation
|
|
102
|
+
- [Development Guide](docs/en/DEVELOPMENT.md) - Contributing and testing
|
|
103
|
+
- [Project Structure](docs/en/PROJECT_STRUCTURE.md) - Codebase organization
|
|
104
|
+
|
|
105
|
+
## Examples
|
|
106
|
+
|
|
107
|
+
- [examples/example_native.py](examples/example_native.py) - CPU backend
|
|
108
|
+
- [examples/example_taichi.py](examples/example_taichi.py) - Taichi backend
|
|
109
|
+
- [examples/lidar_vis_ros2.py](examples/lidar_vis_ros2.py) - ROS2 integration
|
|
110
|
+
- [examples/unitree_go2_ros2.py](examples/unitree_go2_ros2.py) - Unitree Go2 robot
|
|
111
|
+
|
|
112
|
+
## Development
|
|
113
|
+
|
|
114
|
+
```bash
|
|
115
|
+
git clone https://github.com/TATP-233/MuJoCo-LiDAR.git
|
|
116
|
+
cd MuJoCo-LiDAR
|
|
117
|
+
uv sync --extra dev
|
|
118
|
+
|
|
119
|
+
make test # Run tests
|
|
120
|
+
make lint # Check code quality
|
|
121
|
+
make benchmark # Run performance tests
|
|
122
|
+
```
|
|
123
|
+
|
|
124
|
+
See [CONTRIBUTING.md](CONTRIBUTING.md) for contribution guidelines.
|
|
125
|
+
|
|
126
|
+
## License
|
|
127
|
+
|
|
128
|
+
MIT License - see [LICENSE](LICENSE) for details.
|
|
129
|
+
|
|
130
|
+
## Citation
|
|
131
|
+
|
|
132
|
+
If you use this project in your research, please cite:
|
|
133
|
+
|
|
134
|
+
```bibtex
|
|
135
|
+
@software{mujoco_lidar,
|
|
136
|
+
title = {MuJoCo-LiDAR: High-Performance LiDAR Simulation},
|
|
137
|
+
author = {Yufei Jia},
|
|
138
|
+
year = {2024},
|
|
139
|
+
url = {https://github.com/TATP-233/MuJoCo-LiDAR}
|
|
140
|
+
}
|
|
141
|
+
```
|
|
@@ -0,0 +1,78 @@
|
|
|
1
|
+
[build-system]
|
|
2
|
+
requires = ["setuptools>=61.0", "wheel"]
|
|
3
|
+
build-backend = "setuptools.build_meta"
|
|
4
|
+
|
|
5
|
+
[project]
|
|
6
|
+
name = "mujoco_lidar"
|
|
7
|
+
dynamic = ["version"]
|
|
8
|
+
authors = [
|
|
9
|
+
{name = "Yufei Jia", email = "jyf23@mails.tsinghua.edu.cn"}
|
|
10
|
+
]
|
|
11
|
+
description = "A LiDAR sensor designed for MuJoCo"
|
|
12
|
+
readme = "README.md"
|
|
13
|
+
license = {text = "MIT"}
|
|
14
|
+
requires-python = ">=3.10"
|
|
15
|
+
classifiers = [
|
|
16
|
+
"Programming Language :: Python :: 3",
|
|
17
|
+
"License :: OSI Approved :: MIT License",
|
|
18
|
+
"Operating System :: OS Independent",
|
|
19
|
+
]
|
|
20
|
+
dependencies = [
|
|
21
|
+
"numpy >= 1.20.0",
|
|
22
|
+
"mujoco >= 3.2.0",
|
|
23
|
+
"scipy",
|
|
24
|
+
"pynput",
|
|
25
|
+
"matplotlib",
|
|
26
|
+
"zhplot",
|
|
27
|
+
]
|
|
28
|
+
|
|
29
|
+
[project.optional-dependencies]
|
|
30
|
+
# Taichi backend with all dependencies (taichi, tibvh, and utilities)
|
|
31
|
+
taichi = [
|
|
32
|
+
"taichi >= 1.6.0",
|
|
33
|
+
"tibvh >= 0.1.2",
|
|
34
|
+
]
|
|
35
|
+
# JAX backend
|
|
36
|
+
jax = [
|
|
37
|
+
"jax[cuda12]",
|
|
38
|
+
]
|
|
39
|
+
dev = [
|
|
40
|
+
"pytest>=7.0",
|
|
41
|
+
"pytest-cov>=4.0",
|
|
42
|
+
"ruff>=0.3.0",
|
|
43
|
+
]
|
|
44
|
+
examples = [
|
|
45
|
+
"onnxruntime>=1.19.2",
|
|
46
|
+
"etils[epath]>=1.13.0",
|
|
47
|
+
]
|
|
48
|
+
|
|
49
|
+
[project.urls]
|
|
50
|
+
Homepage = "https://github.com/TATP-233/MuJoCo-LiDAR.git"
|
|
51
|
+
Repository = "https://github.com/TATP-233/MuJoCo-LiDAR.git"
|
|
52
|
+
|
|
53
|
+
[tool.setuptools]
|
|
54
|
+
packages = ["mujoco_lidar"]
|
|
55
|
+
package-dir = {"" = "src"}
|
|
56
|
+
|
|
57
|
+
[tool.setuptools.dynamic]
|
|
58
|
+
version = {attr = "mujoco_lidar.__version__"}
|
|
59
|
+
|
|
60
|
+
[tool.setuptools.package-data]
|
|
61
|
+
"mujoco_lidar" = ["scan_mode/*.npy"]
|
|
62
|
+
|
|
63
|
+
[tool.ruff]
|
|
64
|
+
line-length = 100
|
|
65
|
+
target-version = "py310"
|
|
66
|
+
exclude = ["examples/"]
|
|
67
|
+
|
|
68
|
+
[tool.ruff.lint]
|
|
69
|
+
select = ["E", "F", "I", "UP", "B", "SIM"]
|
|
70
|
+
ignore = ["E501"]
|
|
71
|
+
|
|
72
|
+
[tool.ruff.format]
|
|
73
|
+
quote-style = "double"
|
|
74
|
+
|
|
75
|
+
[tool.pytest.ini_options]
|
|
76
|
+
testpaths = ["tests"]
|
|
77
|
+
python_files = "test_*.py"
|
|
78
|
+
addopts = "-v"
|
|
@@ -0,0 +1,43 @@
|
|
|
1
|
+
# Lazy import to avoid loading dependencies when not needed
|
|
2
|
+
# Import the wrapper class directly
|
|
3
|
+
from typing import Any
|
|
4
|
+
|
|
5
|
+
from mujoco_lidar.lidar_wrapper import MjLidarWrapper
|
|
6
|
+
|
|
7
|
+
__version__ = "0.3.0"
|
|
8
|
+
|
|
9
|
+
__all__ = [
|
|
10
|
+
"MjLidarWrapper",
|
|
11
|
+
# Scan generation functions (imported lazily via __getattr__)
|
|
12
|
+
"LivoxGeneratorTi",
|
|
13
|
+
"LivoxGenerator", # From scan_gen_livox (requires taichi)
|
|
14
|
+
"generate_grid_scan_pattern",
|
|
15
|
+
"create_lidar_single_line",
|
|
16
|
+
"generate_HDL64", # From scan_gen (no taichi needed)
|
|
17
|
+
"generate_vlp32",
|
|
18
|
+
"generate_os128",
|
|
19
|
+
"generate_airy96",
|
|
20
|
+
]
|
|
21
|
+
|
|
22
|
+
|
|
23
|
+
def __getattr__(name: str) -> Any:
|
|
24
|
+
"""Lazy import for scan generation functions."""
|
|
25
|
+
# LivoxGeneratorTi requires taichi - import from scan_gen_livox_ti
|
|
26
|
+
if name == "LivoxGeneratorTi":
|
|
27
|
+
from mujoco_lidar.scan_gen_livox_ti import LivoxGeneratorTi
|
|
28
|
+
|
|
29
|
+
return LivoxGeneratorTi
|
|
30
|
+
# Other scan functions don't require taichi - import from scan_gen
|
|
31
|
+
elif name in [
|
|
32
|
+
"LivoxGenerator",
|
|
33
|
+
"generate_grid_scan_pattern",
|
|
34
|
+
"create_lidar_single_line",
|
|
35
|
+
"generate_HDL64",
|
|
36
|
+
"generate_vlp32",
|
|
37
|
+
"generate_os128",
|
|
38
|
+
"generate_airy96",
|
|
39
|
+
]:
|
|
40
|
+
from mujoco_lidar import scan_gen
|
|
41
|
+
|
|
42
|
+
return getattr(scan_gen, name)
|
|
43
|
+
raise AttributeError(f"module '{__name__}' has no attribute '{name}'")
|
|
@@ -0,0 +1,205 @@
|
|
|
1
|
+
from typing import Any
|
|
2
|
+
|
|
3
|
+
import mujoco
|
|
4
|
+
import numpy as np
|
|
5
|
+
|
|
6
|
+
|
|
7
|
+
class MjLidarWrapper:
|
|
8
|
+
"""
|
|
9
|
+
MuJoCo LiDAR wrapper supporting CPU, Taichi, and JAX backends.
|
|
10
|
+
|
|
11
|
+
Args:
|
|
12
|
+
mj_model: MuJoCo model object
|
|
13
|
+
site_name: Name of the LiDAR site in the model
|
|
14
|
+
backend: 'cpu', 'taichi', or 'jax' (default: 'taichi')
|
|
15
|
+
cutoff_dist: Maximum ray distance in meters (default: 100.0)
|
|
16
|
+
args: Backend-specific arguments (see docs/en/API.md)
|
|
17
|
+
"""
|
|
18
|
+
|
|
19
|
+
def __init__(
|
|
20
|
+
self,
|
|
21
|
+
mj_model: mujoco.MjModel,
|
|
22
|
+
site_name: str,
|
|
23
|
+
backend: str = "taichi",
|
|
24
|
+
cutoff_dist: float = 100.0,
|
|
25
|
+
args: dict[str, Any] = None,
|
|
26
|
+
):
|
|
27
|
+
if args is None:
|
|
28
|
+
args = {}
|
|
29
|
+
if backend == "gpu":
|
|
30
|
+
backend = "taichi"
|
|
31
|
+
self.backend = backend
|
|
32
|
+
self.mj_model = mj_model
|
|
33
|
+
self.cutoff_dist = cutoff_dist
|
|
34
|
+
self.args = args
|
|
35
|
+
|
|
36
|
+
if backend == "taichi":
|
|
37
|
+
self._init_taichi_backend()
|
|
38
|
+
elif backend == "jax":
|
|
39
|
+
self._init_jax_backend()
|
|
40
|
+
elif backend == "cpu":
|
|
41
|
+
self._init_cpu_backend()
|
|
42
|
+
else:
|
|
43
|
+
raise ValueError(
|
|
44
|
+
f"Unsupported backend: {backend}, choose from 'cpu', 'taichi', or 'jax'"
|
|
45
|
+
)
|
|
46
|
+
|
|
47
|
+
self.site_name = site_name
|
|
48
|
+
self._sensor_pose = np.eye(4, dtype=np.float32)
|
|
49
|
+
self._local_rays: np.ndarray | None = None
|
|
50
|
+
self._distances: np.ndarray | None = None
|
|
51
|
+
|
|
52
|
+
def _init_taichi_backend(self) -> None:
|
|
53
|
+
"""Initialize Taichi backend"""
|
|
54
|
+
try:
|
|
55
|
+
# Lazy import: only import when Taichi backend is selected
|
|
56
|
+
import taichi as ti
|
|
57
|
+
|
|
58
|
+
from mujoco_lidar.core_ti.mjlidar_ti import MjLidarTi
|
|
59
|
+
|
|
60
|
+
# Initialize Taichi if not already done
|
|
61
|
+
if not hasattr(ti, "_is_initialized") or not ti._is_initialized:
|
|
62
|
+
ti.init(arch=ti.gpu, **self.args.get("ti_init_args", {}))
|
|
63
|
+
|
|
64
|
+
# Create Taichi backend instance
|
|
65
|
+
geomgroup = self.args.get("geomgroup", None)
|
|
66
|
+
bodyexclude = self.args.get("bodyexclude", -1)
|
|
67
|
+
max_candidates = self.args.get("max_candidates", 64)
|
|
68
|
+
self._backend_instance = MjLidarTi(
|
|
69
|
+
self.mj_model,
|
|
70
|
+
cutoff_dist=self.cutoff_dist,
|
|
71
|
+
geomgroup=geomgroup,
|
|
72
|
+
bodyexclude=bodyexclude,
|
|
73
|
+
max_candidates=max_candidates,
|
|
74
|
+
)
|
|
75
|
+
|
|
76
|
+
except ImportError as e:
|
|
77
|
+
raise ImportError(
|
|
78
|
+
f"Failed to import Taichi backend dependencies. "
|
|
79
|
+
f'Please install taichi: uv add "mujoco-lidar[taichi]"\n'
|
|
80
|
+
f"Error: {e}"
|
|
81
|
+
) from e
|
|
82
|
+
|
|
83
|
+
def _init_jax_backend(self) -> None:
|
|
84
|
+
"""Initialize JAX backend"""
|
|
85
|
+
try:
|
|
86
|
+
from mujoco_lidar.core_jax.mjlidar_jax import MjLidarJax
|
|
87
|
+
|
|
88
|
+
geomgroup = self.args.get("geomgroup", None)
|
|
89
|
+
bodyexclude = self.args.get("bodyexclude", -1)
|
|
90
|
+
|
|
91
|
+
# Pass mj_model directly. MjLidarJax will extract what it needs.
|
|
92
|
+
self._backend_instance = MjLidarJax(
|
|
93
|
+
self.mj_model,
|
|
94
|
+
geom_ids=self.args.get("geom_ids"),
|
|
95
|
+
geomgroup=geomgroup,
|
|
96
|
+
bodyexclude=bodyexclude,
|
|
97
|
+
)
|
|
98
|
+
|
|
99
|
+
except ImportError as e:
|
|
100
|
+
raise ImportError(f"Failed to import JAX backend dependencies.\nError: {e}") from e
|
|
101
|
+
|
|
102
|
+
def _init_cpu_backend(self) -> None:
|
|
103
|
+
"""Initialize CPU backend"""
|
|
104
|
+
try:
|
|
105
|
+
from mujoco_lidar.core_cpu.mjlidar_cpu import MjLidarCPU
|
|
106
|
+
|
|
107
|
+
geomgroup = self.args.get("geomgroup", None)
|
|
108
|
+
bodyexclude = self.args.get("bodyexclude", -1)
|
|
109
|
+
self._backend_instance = MjLidarCPU(
|
|
110
|
+
self.mj_model,
|
|
111
|
+
cutoff_dist=self.cutoff_dist,
|
|
112
|
+
geomgroup=geomgroup,
|
|
113
|
+
bodyexclude=bodyexclude,
|
|
114
|
+
)
|
|
115
|
+
|
|
116
|
+
except ImportError as e:
|
|
117
|
+
raise ImportError(f"Failed to import CPU backend dependencies.\nError: {e}") from e
|
|
118
|
+
|
|
119
|
+
@property
|
|
120
|
+
def sensor_position(self) -> np.ndarray:
|
|
121
|
+
return self._sensor_pose[:3, 3].copy()
|
|
122
|
+
|
|
123
|
+
@property
|
|
124
|
+
def sensor_rotation(self) -> np.ndarray:
|
|
125
|
+
return self._sensor_pose[:3, :3].copy()
|
|
126
|
+
|
|
127
|
+
def update_sensor_pose(self, mj_data: mujoco.MjData, site_name: str) -> None:
|
|
128
|
+
# For CPU/Taichi/JAX backend, mj_data is mujoco.MjData
|
|
129
|
+
if self.backend in ["cpu", "taichi", "jax"]:
|
|
130
|
+
self._sensor_pose[:3, :3] = mj_data.site(site_name).xmat.reshape(3, 3)
|
|
131
|
+
self._sensor_pose[:3, 3] = mj_data.site(site_name).xpos
|
|
132
|
+
|
|
133
|
+
def trace_rays(
|
|
134
|
+
self,
|
|
135
|
+
mj_data: mujoco.MjData,
|
|
136
|
+
ray_theta: np.ndarray,
|
|
137
|
+
ray_phi: np.ndarray,
|
|
138
|
+
site_name: str | None = None,
|
|
139
|
+
) -> np.ndarray:
|
|
140
|
+
"""
|
|
141
|
+
Trace rays.
|
|
142
|
+
For JAX backend, mj_data can be mujoco.MjData.
|
|
143
|
+
"""
|
|
144
|
+
target_site = self.site_name if site_name is None else site_name
|
|
145
|
+
|
|
146
|
+
if self.backend == "jax":
|
|
147
|
+
# Update sensor pose for consistency
|
|
148
|
+
self.update_sensor_pose(mj_data, target_site)
|
|
149
|
+
|
|
150
|
+
# Use JITed trace_rays from backend instance
|
|
151
|
+
# This handles ray generation, transformation and rendering in one JIT call
|
|
152
|
+
self._distances, self._local_rays = self._backend_instance.trace_rays(
|
|
153
|
+
mj_data.geom_xpos,
|
|
154
|
+
mj_data.geom_xmat,
|
|
155
|
+
mj_data.site(target_site).xpos,
|
|
156
|
+
mj_data.site(target_site).xmat.reshape(3, 3),
|
|
157
|
+
ray_theta,
|
|
158
|
+
ray_phi,
|
|
159
|
+
)
|
|
160
|
+
|
|
161
|
+
return self._distances
|
|
162
|
+
|
|
163
|
+
elif self.backend == "taichi":
|
|
164
|
+
# Taichi Backend
|
|
165
|
+
self.update_sensor_pose(mj_data, target_site)
|
|
166
|
+
self._backend_instance.update(mj_data)
|
|
167
|
+
|
|
168
|
+
import taichi as ti
|
|
169
|
+
|
|
170
|
+
# Convert numpy arrays to Taichi ndarrays if necessary
|
|
171
|
+
if isinstance(ray_theta, np.ndarray):
|
|
172
|
+
theta_ti = ti.ndarray(dtype=ti.f32, shape=ray_theta.shape[0])
|
|
173
|
+
theta_ti.from_numpy(ray_theta.astype(np.float32))
|
|
174
|
+
else:
|
|
175
|
+
theta_ti = ray_theta
|
|
176
|
+
|
|
177
|
+
if isinstance(ray_phi, np.ndarray):
|
|
178
|
+
phi_ti = ti.ndarray(dtype=ti.f32, shape=ray_phi.shape[0])
|
|
179
|
+
phi_ti.from_numpy(ray_phi.astype(np.float32))
|
|
180
|
+
else:
|
|
181
|
+
phi_ti = ray_phi
|
|
182
|
+
|
|
183
|
+
self._backend_instance.trace_rays(self._sensor_pose, theta_ti, phi_ti)
|
|
184
|
+
return self._backend_instance.get_distances()
|
|
185
|
+
|
|
186
|
+
else:
|
|
187
|
+
# CPU Backend
|
|
188
|
+
self.update_sensor_pose(mj_data, target_site)
|
|
189
|
+
self._backend_instance.update(mj_data)
|
|
190
|
+
self._backend_instance.trace_rays(self._sensor_pose, ray_theta, ray_phi)
|
|
191
|
+
return self._backend_instance.get_distances()
|
|
192
|
+
|
|
193
|
+
def get_hit_points(self) -> np.ndarray:
|
|
194
|
+
if self.backend == "jax":
|
|
195
|
+
if self._distances is None or self._local_rays is None:
|
|
196
|
+
return np.zeros((0, 3), dtype=np.float32)
|
|
197
|
+
return np.asarray(self._distances[:, np.newaxis] * self._local_rays)
|
|
198
|
+
return self._backend_instance.get_hit_points()
|
|
199
|
+
|
|
200
|
+
def get_distances(self) -> np.ndarray:
|
|
201
|
+
if self.backend == "jax":
|
|
202
|
+
if self._distances is None:
|
|
203
|
+
return np.zeros(0, dtype=np.float32)
|
|
204
|
+
return np.asarray(self._distances)
|
|
205
|
+
return self._backend_instance.get_distances()
|