eholum-nasa · May 27, 2026 · May 27, 2026 · May 27, 2026 · May 27, 2026 · May 27, 2026
diff --git a/doc/.gitignore b/doc/.gitignore
@@ -1,3 +1,4 @@
 _build/
 .pixi/
 mujoco_ros2_control**
+mujoco_extensions**
diff --git a/doc/index.rst b/doc/index.rst
@@ -154,4 +154,5 @@ Further Documentation
    Modeling Tips <../mujoco_ros2_control/docs/modeling_tips>
    Tutorials <../mujoco_ros2_control_demos/doc/tutorials>
    Plugins <../mujoco_ros2_control_plugins/doc/plugins>
+   MuJoCo Extensions <../mujoco_extensions/doc/mujoco_extensions>
    Developers Guide <development>
diff --git a/doc/pixi.toml b/doc/pixi.toml
@@ -19,7 +19,8 @@ sphinx-autobuild = ">=2024,<2026"
 setup = """
 ln -sfn ../mujoco_ros2_control mujoco_ros2_control && \
 ln -sfn ../mujoco_ros2_control_demos mujoco_ros2_control_demos && \
-ln -sfn ../mujoco_ros2_control_plugins mujoco_ros2_control_plugins
+ln -sfn ../mujoco_ros2_control_plugins mujoco_ros2_control_plugins && \
+ln -sfn ../mujoco_extensions mujoco_extensions
 """
 build = { cmd = "sphinx-build -b html . _build/html", depends-on = ["setup"] }
 clean = "rm -rf _build mujoco_ros2_control*"

diff --git a/mujoco_extensions/doc/mujoco_extensions.rst b/mujoco_extensions/doc/mujoco_extensions.rst
@@ -0,0 +1,137 @@
+.. _custom_mujoco_extensions:
+
+Custom MuJoCo Extensions
+========================
+
+MuJoCo includes support for `custom extensions <https://mujoco.readthedocs.io/en/stable/programming/extension.html>`_ that can be loaded directly into the application at runtime.
+This gives developers the ability to interact with the simulation at a very low level, and to connect custom applications that work directly with the ``Simulate`` application.
+To build custom extensions, follow the guide in the upstream documentation.
+
+Adding Extensions
+-----------------
+
+``mujoco_ros2_control`` automatically discovers and loads MuJoCo plugin libraries at runtime using two mechanisms:
+
+1. **Vendor plugins**: Any ``.so`` files in the ``mujoco_plugin/`` directory next to the ``ros2_control_node`` executable (these come from ``mujoco_vendor``).
+2. **Ament resource index**: Any ROS 2 package that registers itself as a ``mujoco_plugins`` resource provider.
+
+To add a new MuJoCo plugin to the system, create a standard ament package that builds a shared library and register it with the ament resource index.
+The ``mujoco_ros2_control`` node will find and load it automatically at startup.
+
+CMakeLists.txt
+~~~~~~~~~~~~~~
+
+A minimal ``CMakeLists.txt`` for a MuJoCo plugin package:
+
+.. code-block:: cmake
+
+    cmake_minimum_required(VERSION 3.22)
+    project(my_mujoco_plugin)
+
+    find_package(ament_cmake REQUIRED)
+    find_package(mujoco_vendor REQUIRED)
+
+    add_library(my_plugin SHARED src/my_plugin.cpp src/register.cpp)
+    target_link_libraries(my_plugin PRIVATE mujoco::mujoco)
+
+    # Install the plugin .so into a known location
+    install(TARGETS my_plugin
+      LIBRARY DESTINATION lib/${PROJECT_NAME}/mujoco_plugin
+    )
+
+    # Register with the ament resource index so mujoco_ros2_control discovers it
+    ament_index_register_resource(mujoco_plugins
+      CONTENT "lib/${PROJECT_NAME}/mujoco_plugin")
+
+    ament_export_dependencies(mujoco_vendor)
+    ament_package()
+
+
+Using with ``simulate``
+~~~~~~~~~~~~~~~~~~~~~~~
+
+The standalone ``simulate`` application only loads plugins from the ``mujoco_plugin/`` directory next to its own binary.
+Users can attempt to install the plugin to that directory by adding a best-effort copy to ``CMakeLists.txt``:
+
+.. code-block:: cmake
+
+    find_program(mujoco_SIMULATE NAMES simulate mujoco-simulate)
+    if(mujoco_SIMULATE)
+      cmake_path(SET mujoco_BIN_PATH NORMALIZE ${mujoco_SIMULATE}/..)
+      install(CODE "
+        execute_process(
+          COMMAND cp -f \"\${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/mujoco_plugin/libmy_plugin.so\"
+                        \"${mujoco_BIN_PATH}/mujoco_plugin/\"
+          RESULT_VARIABLE _err)
+        if(_err)
+          message(WARNING \"Could not install plugin next to simulate! Manually copy it to ${mujoco_BIN_PATH}/mujoco_plugin/\")
+        endif()
+      ")
+    endif()
+
+If the copy fails due to permissions, the warning message will include the exact ``cp`` command to run manually (e.g., with ``sudo``).
+
+Supported Extensions
+--------------------
+
+3D Lidar (``mujoco.plugin.lidar``)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+This sensor uses ray casting to simulate lidar.
+
+A ``lidar`` sensor is associated with a site and finds the nearest collision points from the site along a set of vectors.
+The vectors are determined by the size and field of view parameters.
+
+- ``resolution``: Horizontal and vertical ray count (e.g., ``"360 1"`` for a 2D scanner, ``"24 10"`` for 3D).
+- ``azimuth_range``: Min and max horizontal angles in radians (e.g., ``"-3.14159 3.14159"``).
+- ``elevation_range``: Min and max vertical angles in radians. For 2D sensors, a single value is sufficient (e.g., ``"0.0"``).
+- ``max_range``: Maximum detection distance in meters.
+- ``min_range``: Minimum detection distance in meters (optional, defaults to 0).
+- ``update_rate``: Sensor update frequency in Hz (optional, defaults to every timestep).
+
+    * Setting an update can drastically save time when calling ``mj_step``.
+    * This may be removed in future versions after the `interval attribute <https://mujoco.readthedocs.io/en/stable/modeling.html#sensors>`_ is available for sensors.
+
+Note that rays that hit nothing or fall outside the min/max range return ``-1``.
+
+Example MJCF for a 2D lidar sensor:
+
+.. code-block:: xml
+
+   <extension>
+     <plugin plugin="mujoco.plugin.lidar">
+       <instance name="2d_lidar">
+         <config key="resolution" value="24 1"/>
+         <config key="azimuth_range" value="-0.3 0.3"/>
+         <config key="elevation_range" value="0.0"/>
+         <config key="max_range" value="10.0"/>
+         <config key="min_range" value="0.0"/>
+         <config key="update_rate" value="10.0"/>
+       </instance>
+     </plugin>
+   </extension>
+
+   <sensor>
+     <plugin name="2d_lidar" instance="2d_lidar" objtype="site" objname="lidar_sensor_frame"/>
+   </sensor>
+
+For a 3D lidar, increase the vertical resolution and specify an elevation range.
+
+.. code-block:: xml
+
+   <extension>
+     <plugin plugin="mujoco.plugin.lidar">
+       <instance name="3d_lidar">
+         <config key="resolution" value="24 50"/>
+         <config key="azimuth_range" value="-0.3 0.3"/>
+         <config key="elevation_range" value="-1.0 1.0"/>
+         <config key="max_range" value="10.0"/>
+         <config key="min_range" value="0.0"/>
+         <config key="update_rate" value="1.0"/>
+       </instance>
+     </plugin>
+   </extension>
+
+   <sensor>
+     <plugin name="3d_lidar" instance="3d_lidar" objtype="site" objname="3d_lidar_sensor_frame"/>
+   </sensor>
diff --git a/mujoco_extensions/mujoco_3d_lidar/.gitignore b/mujoco_extensions/mujoco_3d_lidar/.gitignore
@@ -0,0 +1,5 @@
+MUJOCO_LOG.TXT
+build
+log
+install
+.vscode
diff --git a/mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt b/mujoco_extensions/mujoco_3d_lidar/CMakeLists.txt
@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.22)
+project(mujoco_3d_lidar)
+
+find_package(ament_cmake QUIET)
+# Link MuJoCo via the vendor package
+find_package(mujoco_vendor REQUIRED)
+
+add_library(mujoco_3d_lidar SHARED)
+target_sources(mujoco_3d_lidar PRIVATE
+  src/register.cpp
+  src/3dlidar.cpp
+)
+target_link_libraries(mujoco_3d_lidar PRIVATE mujoco::mujoco)
+target_compile_options(mujoco_3d_lidar PRIVATE ${mujoco_SILENCE_COMPILER_WARNINGS})
+target_include_directories(mujoco_3d_lidar PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Install in the local package, and export
+install(TARGETS mujoco_3d_lidar
+  EXPORT export_${PROJECT_NAME}
+  LIBRARY DESTINATION lib/${PROJECT_NAME}
+)
+install(TARGETS mujoco_3d_lidar
+  LIBRARY DESTINATION lib/${PROJECT_NAME}/mujoco_plugin
+)
+install(DIRECTORY include/ DESTINATION include)
+
+# Duplicate the install to be next to the simulate application so this works there, as well.
+# This is best effort because it may not be writeable...
+# TODO: Have mujoco_vendor support a writeable plugin directory
+find_program(mujoco_SIMULATE NAMES simulate mujoco-simulate)
+if(mujoco_SIMULATE)
+  cmake_path(SET mujoco_BIN_PATH NORMALIZE ${mujoco_SIMULATE}/..)
+  install(CODE "
+    file(MAKE_DIRECTORY \"${mujoco_BIN_PATH}/mujoco_plugin\")
+    execute_process(
+      COMMAND cp -f \"\${CMAKE_INSTALL_PREFIX}/lib/${PROJECT_NAME}/mujoco_plugin/libmujoco_3d_lidar.so\"
+                    \"${mujoco_BIN_PATH}/mujoco_plugin/\"
+      RESULT_VARIABLE _err)
+    if(_err)
+      message(WARNING \"Could not install lidar plugin next to simulate! Manually copy it to ${mujoco_BIN_PATH}/mujoco_plugin/\")
+    endif()
+  ")
+else()
+  message(WARNING "NO SIMULATE FOUND! "
+    "To use the lidar plugin with simulate, manually copy libmujoco_3d_lidar.so "
+    "into .../mujoco/bin/mujoco_plugin/")
+endif()
+
+# Optionally export
+if(ament_cmake_FOUND)
+  # Register this package as a mujoco_plugin provider in the ament resource index.
+  # The marker file contains the relative path to the plugin directory.
+  ament_index_register_resource(mujoco_plugins CONTENT "lib/${PROJECT_NAME}/mujoco_plugin")
+
+  ament_export_targets(export_${PROJECT_NAME})
+  ament_export_dependencies(mujoco_vendor)
+  ament_package()
+endif()
diff --git a/mujoco_extensions/mujoco_3d_lidar/README.md b/mujoco_extensions/mujoco_3d_lidar/README.md
@@ -0,0 +1,100 @@
+# 3-D Lidar Extension for Mujoco
+
+## [Lidar](include/mujoco_3d_lidar/3dlidar.h)
+
+This sensor uses ray casting to simulate lidar.
+
+A `lidar` sensor is associated with a site and finds the nearest collision points from the site along a set of vectors.
+The vectors are determined by the size and field of view parameters.
+
+The sensor is parametrizable with the following:
+
+1. Horizontal resolution (size[0]). _positive integer_
+2. Vertical resolution (size[1]). _positive integer_
+3. Horizontal field-of-view (fov[0]). _positive float in (0, 2 $\pi$.] radians_
+4. Vertical field-of-view(fov[1]). _positive float in (0, $\pi$] radians_
+5. Maximum Range. _positive float greater than 0.0 (m)_
+6. Minimum Range. _positive float greater than 0.0 (m)_ (Optional, defaults to 0.0)
+7. Update Rate. _positive float greater than 0.0 (Hz)_ (Optional, defaults to 0.0)
+    * **Note**. This may be removed in future versions after the [interval attribute](https://mujoco.readthedocs.io/en/stable/modeling.html#sensors) is available for sensors.
+
+Field of view should always be in radians regardless of the compiler options.
+
+The horizontal fov is divided by the horizontal resolution (hn) to get hn azmuth angles.
+The vertical fov is divided by the vertical resolution (vn) to gt vn elevation angles.
+Each azmuth/elevation pair defines a vector.
+The distance result is the set of distances to collision from the site origin along that vector.
+
+These parameters are passed as extension config attributes:
+
+```xml
+<mujoco>
+  <extension>
+    <plugin plugin="mujoco.sensor.lidar"/>
+  </extension>
+  ...
+  <sensor>
+    <plugin name="lidar" plugin="mujoco.plugin.lidar" objtype="site" objname="lidar_sensor">
+      <config key="size" value="360 10"/>
+      <config key="fov" value="6.2832 0.7854"/>
+      <config key="max_range" value="13.0"/>
+      <config key="min_range" value="1.0"/>
+      <config key="update_rate" value="1.0"/>
+    </plugin>
+  </sensor>
+</mujoco>
+```
+
+Note the following:
+
+- The dimensionality of the sensor output is `size_x *size_y`.
+- `objtype="site" objname="lidar"` specify that the sensor is associated with a
+  site, and the name of the specific site.
+- Field-of-view angles are always in radians
+
+### Example model
+
+<img src="docs/lidar_45x1.jpg" width="600" height="400">
+Lidar with:
+ horizontal fov = 45 deg,
+ vertical fov = 0,
+ horizontal size = 15,
+ vertical size = 1
+
+<img src="docs/lidar_360x1.jpg" width="600" height="400">
+Lidar with:
+ horizontal fov = 360 deg,
+ vertical fov = 0,
+ horizontal size = 360,
+ vertical size = 1
+
+<img src="docs/lidar_360x45.jpg" width="600" height="400">
+Lidar with:
+ horizontal fov = 360 deg,
+ vertical fov = 45,
+ horizontal size = 360,
+ vertical size = 4
+
+See [lidar.xml](example/lidar.xml) to play with the model above.
+
+## Standalone Build
+
+This package is intended to be build alongside the `mujoco_ros2_control` packages.
+However, we maintain a standalone build in the event that users want to pull this out and use it separately.
+To compile, ensure `mujoco_vendor` is installed and available.
+
+From this package's root:
+
+```bash
+mkdir build
+cd build
+cmake ..
+make
+```
+
+This will create a mujoco_plugin directory alongside the mujoco binaries and install the lidar extension there.
+It can now be found.
+
+```bash
+simulate example/lidar.xml
+```
diff --git a/mujoco_extensions/mujoco_3d_lidar/docs/lidar_360x1.jpg b/mujoco_extensions/mujoco_3d_lidar/docs/lidar_360x1.jpg
diff --git a/mujoco_extensions/mujoco_3d_lidar/docs/lidar_360x45.jpg b/mujoco_extensions/mujoco_3d_lidar/docs/lidar_360x45.jpg
diff --git a/mujoco_extensions/mujoco_3d_lidar/docs/lidar_45x1.jpg b/mujoco_extensions/mujoco_3d_lidar/docs/lidar_45x1.jpg
diff --git a/mujoco_extensions/mujoco_3d_lidar/example/lidar.xml b/mujoco_extensions/mujoco_3d_lidar/example/lidar.xml
@@ -0,0 +1,27 @@
+<mujoco model="lidar_test1">
+    <include file="scene.xml" />
+
+    <extension>
+        <plugin plugin="mujoco.plugin.lidar">
+            <instance name="hokuyo">
+                <config key="resolution" value="24 10"/>
+                <config key="azimuth_range" value="-0.3 0.3"/>
+                <config key="elevation_range" value="0.0, 0.3"/>
+                <config key="max_range" value="10.0"/>
+                <config key="min_range" value="1.0"/>
+                <config key="update_rate" value="1.0"/>
+            </instance>
+        </plugin>
+    </extension>
+
+    <worldbody>
+        <body name="lidar_mount" pos="0 0 0.5">
+            <inertial mass="1.0" pos="0 0 0" diaginertia="0.001 0.001 0.001"></inertial>
+            <geom type="cylinder" size="0.05 0.05" mass="1" pos="0 0 0" />
+            <site name="lidar_sensor" pos="0 0 .15"/>
+        </body>
+    </worldbody>
+  <sensor>
+    <plugin name="front_lidar" instance="hokuyo" objtype="site" objname="lidar_sensor"/>
+  </sensor>
+</mujoco>
diff --git a/mujoco_extensions/mujoco_3d_lidar/example/scene.xml b/mujoco_extensions/mujoco_3d_lidar/example/scene.xml
@@ -0,0 +1,38 @@
+<mujoco>
+  <statistic meansize=".05" />
+
+  <visual>
+    <rgba haze="0.15 0.25 0.35 1" />
+    <quality shadowsize="4096" />
+    <map stiffness="700" shadowscale="0.5" fogstart="1" fogend="15" zfar="40" haze="1" />
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512"
+      height="512" />
+    <texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
+      width="512" height="512" mark="cross" markrgb=".8 .8 .8" />
+
+    <material name="matplane" reflectance="0.3" texture="texplane" texrepeat="10 10"
+      texuniform="true" />
+  </asset>
+
+  <worldbody>
+    <light diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 2.0" dir="0 0 -1" castshadow="false" />
+    <light directional="true" diffuse=".8 .8 .8" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1" />
+
+    <geom name="ground" type="plane" size="0 0 1" pos="0 0 0" quat="1 0 0 0" material="matplane" />
+    <body name="object1" pos="5 0 0.75" quat="0 0 0 1">
+      <joint type="free" />
+      <geom type="box" size="0.75 3.75 0.75" mass="1" pos="0 0 0" rgba="1 0 0 1" />
+    </body>
+    <body name="object2" pos="0 3.75 0.75" quat="0.3007058 0 0 0.953717">
+      <joint type="free" />
+      <geom type="box" size="0.75 3.75 0.75" mass="1" pos="0 0 0"  rgba="0 1 0 1"/>
+    </body>
+    <body name="object3" pos="0 -3 0.75" quat="0.3007058 0 0 0.953717">
+      <joint type="free" />
+      <geom type="box" size="3.75 0.75 0.75" mass="1" pos="0 0 0" rgba="0 0 1 1"/>
+    </body>
+  </worldbody>
+</mujoco>