diff --git a/.github/workflows/docs.yaml b/.github/workflows/docs.yaml index d8384292..d6665069 100644 --- a/.github/workflows/docs.yaml +++ b/.github/workflows/docs.yaml @@ -21,7 +21,7 @@ jobs: runs-on: ubuntu-latest steps: - uses: actions/checkout@v7 - - uses: prefix-dev/setup-pixi@v0.10.1 + - uses: prefix-dev/setup-pixi@v0.10.2 with: manifest-path: doc/pixi.toml - name: Build docs diff --git a/.github/workflows/rolling-check-docs.yml b/.github/workflows/rolling-check-docs.yml new file mode 100644 index 00000000..dfa6273f --- /dev/null +++ b/.github/workflows/rolling-check-docs.yml @@ -0,0 +1,29 @@ +name: Check Docs - Rolling + +on: + workflow_dispatch: + pull_request: + branches: + - main + paths: + - '**.rst' + - '**.md' + - '**.jpg' + - '**.jpeg' + - '**.png' + - '**.svg' + - '**.yml' + - '**.yaml' + - '!.github/**' # exclude yaml files in .github directory + - '.github/workflows/rolling-check-docs.yml' + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + check-docs: + name: Check Docs + uses: ros-controls/control.ros.org/.github/workflows/reusable-sphinx-check-single-version.yml@rolling + with: + MUJOCO_ROS2_CONTROL_PR: ${{ github.ref }} diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 5f0e5979..63bac63c 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -85,3 +85,4 @@ repos: hooks: - id: codespell exclude: '\.(svg|stl|dae)$|.*pixi.*' + args: ["--ignore-words-list=assertin"] diff --git a/doc/development.rst b/doc/development.rst index ee8021ca..0c728a35 100644 --- a/doc/development.rst +++ b/doc/development.rst @@ -45,7 +45,7 @@ pixi also provides an interactive shell that sources the installed package envir ros2 launch mujoco_ros2_control_demos demo.launch.py For more information on pixi and ROS refer to the documentation or this excellent -`blog post `_. +`blog post `_. Docker Development Workflow ---------------------------- diff --git a/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst new file mode 100644 index 00000000..509df98b --- /dev/null +++ b/mujoco_extensions/mujoco_3d_lidar/CHANGELOG.rst @@ -0,0 +1,29 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package mujoco_3d_lidar +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2026-09-07) +------------------ + +0.1.1 (2026-08-25) +------------------ +* Fix compilation on MuJoCo 3.12.0 (`#290 `_) +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + +0.1.0 (2026-08-18) +------------------ +* Make CI tests more robust (`#239 `_) +* Add Plugin for the 3D Lidar Extension (`#207 `_) +* Update Pixi packages with pixi.lock (`#223 `_) +* Add a 3-D Lidar Extension (`#205 `_) +* Contributors: Erik Holum, Sai Kishor Kothakota + +0.0.3 (2026-05-01) +------------------ + +0.0.2 (2026-03-17) +------------------ + +0.0.1 (2026-02-24) +------------------ diff --git a/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h b/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h index e6f48101..7ccaf757 100644 --- a/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h +++ b/mujoco_extensions/mujoco_3d_lidar/include/mujoco_3d_lidar/3dlidar.h @@ -29,7 +29,12 @@ #include #include +#include // pulls in whichever exists +#if mjVERSION_HEADER >= 3009000 +#include +#else #include +#endif #include namespace mujoco::plugin::lidar diff --git a/mujoco_extensions/mujoco_3d_lidar/package.xml b/mujoco_extensions/mujoco_3d_lidar/package.xml index a6ae874a..dcb44ee2 100644 --- a/mujoco_extensions/mujoco_3d_lidar/package.xml +++ b/mujoco_extensions/mujoco_3d_lidar/package.xml @@ -2,7 +2,7 @@ mujoco_3d_lidar - 0.0.3 + 0.1.2 Plugin for mujoco to use raycasters to simulate lidar Jodi Graf diff --git a/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp b/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp index c5e30d33..53221994 100644 --- a/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp +++ b/mujoco_extensions/mujoco_3d_lidar/src/3dlidar.cpp @@ -30,7 +30,12 @@ #include #include #include +#include // pulls in whichever exists +#if mjVERSION_HEADER >= 3009000 +#include +#else #include +#endif #include #include diff --git a/mujoco_ros2_control/CHANGELOG.rst b/mujoco_ros2_control/CHANGELOG.rst index 316bc98a..1d238104 100644 --- a/mujoco_ros2_control/CHANGELOG.rst +++ b/mujoco_ros2_control/CHANGELOG.rst @@ -2,6 +2,63 @@ Changelog for package mujoco_ros2_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2026-09-07) +------------------ +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Re-add conditional tinyxml2_vendor dependency (`#293 `_) +* Remove deprecated tinyxml2_vendor dependency (`#286 `_) +* Shut down ROS when the MuJoCo UI closes (`#285 `_) +* Contributors: Bilal Gill, Dylan Gallagher, Erik Holum, Sai Kishor Kothakota + +0.1.1 (2026-08-25) +------------------ +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + +0.1.0 (2026-08-18) +------------------ +* Add a pre-step callback to the simulation (`#282 `_) +* Fix control mode selection (`#281 `_) +* Support intvelocity actuators (`#271 `_) +* Add base twist plugin for commanding the mobile base (`#254 `_) +* Refine reset world overrides functionality (`#263 `_) +* Support joint overrides in reset world service (`#256 `_) +* Support multiple sets of state interfaces from multiple sensor tags with same name (`#258 `_) +* [Fix] Don't overwrite transmission-driven joint states in the direct-copy fallback (`#259 `_) +* [doc] Fix undefined label errors (`#260 `_) +* Add support for Magnetometer sensor (`#257 `_) +* [Feature] modify elements of geom attribute (`#244 `_) +* Add missing dependencies (`#250 `_) +* [CI] Fix dependency declarations for bloom (`#226 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) +* Add service to set the free joint body state (`#233 `_) +* Fix links in interface docs, use auto label generator like the upstream (`#238 `_) +* Fix state passed into pose sensors (`#231 `_) +* Fix controller manager rate collapse by decoupling control and physics locks (`#224 `_) +* Use updated ament_index_cpp interfaces, re-add rolling (`#229 `_) +* Add a 3-D Lidar Extension (`#205 `_) +* Add support for "site" transmission type (`#154 `_) +* Add support for pose sensors (`#220 `_) +* Fix deprecations in ament_index_cpp (`#145 `_) +* Fix race condition on sim-display overlay text (`#222 `_) +* Display the sim real time factor on the native viewer (`#189 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) +* Add 'lyrical' to ros_distro matrix in CI workflow (`#213 `_) +* Fix scipy for openblas missing dependency (`#212 `_) +* Isolate flakey functional tests and update pixi (`#204 `_) +* Support/cameras rendering/headless (`#197 `_) +* Fix glfw initialization on headless run (`#200 `_) +* Add changes for mujoco vendor bump (`#202 `_) +* Control and plugin data input cleanup (`#191 `_) +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Refactor the core mujoco simulation to be in its own container (`#175 `_) +* Handle topic model loading failures with nullptr instead of exit (`#183 `_) +* Improve LiDAR validation in URDF-to-MJCF conversion (`#182 `_) +* Fix Sphinx xref_missing reference (`#186 `_) +* Handle duplicate mesh file names in the conversion tool (`#171 `_) +* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Julia Jia, Lang Qinglin, Marq Rasmussen, Ortisa, Sai Kishor Kothakota, Sebastian Castro, Tianlin Zhang, Daniel Costanzi, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Add more useful plugins for the mujoco_ros2_control (`#165 `_) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index fe8f2e85..d929f555 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -311,6 +311,7 @@ ament_export_dependencies( mujoco_ros2_control_plugins pluginlib control_toolbox + tf2_ros Eigen3 mujoco_vendor ) diff --git a/mujoco_ros2_control/docs/hardware_interface.rst b/mujoco_ros2_control/docs/hardware_interface.rst index 05670881..9a2d0e54 100644 --- a/mujoco_ros2_control/docs/hardware_interface.rst +++ b/mujoco_ros2_control/docs/hardware_interface.rst @@ -115,20 +115,28 @@ Maps to the following ``ros2_control`` hardware interface: * - Command Interface - MuJoCo ``position`` - MuJoCo ``velocity`` + - MuJoCo ``intvelocity`` - MuJoCo ``motor``, ``general``, etc. * - **position** - Native support - Supported using PIDs - Supported using PIDs + - Supported using PIDs * - **velocity** - Not supported - Native support + - Native support - Supported using PIDs * - **effort** + - Not supported - Not supported - Not supported - Native support +MuJoCo's ``intvelocity`` actuator integrates its ``ctrl`` input into an internal position setpoint. The input itself has +velocity semantics, so it maps natively to a ros2_control ``velocity`` command interface without changing the actuator's +integrated-velocity dynamics. + .. note:: The ``torque`` and ``force`` command/state interfaces are semantically equivalent to ``effort`` and map to the same underlying data in the sim. @@ -170,13 +178,15 @@ MuJoCo does not model complete FTS and IMUs natively, so we combine supported MJ Force-Torque Sensors ~~~~~~~~~~~~~~~~~~~~ -Model ``force`` and ``torque`` sensors separately in the MJCF, suffixed with ``_force`` and ``_torque``: +Model ``force`` and ``torque`` sensors separately in the MJCF, suffixed with ``_force`` and ``_torque``. +Each optionally takes a ``noise`` attribute (standard deviation, in N / N*m) to add zero-mean Gaussian noise +to its readings, see :ref:`Sensor Noise ` below: .. code-block:: xml - - + + Map them to a single ``ros2_control`` sensor: @@ -201,14 +211,16 @@ Map them to a single ``ros2_control`` sensor: IMU ~~~ -Simulate a ``framequat``, ``gyro``, and ``accelerometer`` as a single IMU: +Simulate a ``framequat``, ``gyro``, and ``accelerometer`` as a single IMU. Each optionally takes a ``noise`` +attribute (standard deviation) to add zero-mean Gaussian noise to its readings, see +:ref:`Sensor Noise ` below: .. code-block:: xml - - - + + + Map to the corresponding ``ros2_control`` sensor: @@ -240,12 +252,13 @@ These sensor state interfaces work out of the box with the standard ROS 2 broadc Magnetometer ~~~~~~~~~~~~ -Model a MuJoCo ``magnetometer`` sensor in the MJCF: +Model a MuJoCo ``magnetometer`` sensor in the MJCF. It optionally takes a ``noise`` attribute (standard +deviation) to add zero-mean Gaussian noise to its readings, see :ref:`Sensor Noise ` below: .. code-block:: xml - + Map it to the corresponding ``ros2_control`` sensor: @@ -261,6 +274,53 @@ Map it to the corresponding ``ros2_control`` sensor: +.. _sensor_noise: + +Sensor Noise +~~~~~~~~~~~~ + +Every MJCF sensor element used above (``force``, ``torque``, ``framequat``, ``gyro``, ``accelerometer``, +``framepos``, ``magnetometer``) accepts MuJoCo's native ``noise`` attribute: the standard deviation, in the +sensor's native units, of the noise added to its reading. It defaults to ``0`` (no noise). Noise magnitude is +entirely driven by the MJCF, e.g.: + +.. code-block:: xml + + + + + + +.. note:: + + MuJoCo compiles ``noise`` into ``mjModel::sensor_noise`` but does not apply it itself (``mj_step`` leaves + ``sensordata`` noise-free) — it's normally left for tools like the ``simulate`` GUI to apply for display. + This hardware interface applies it on every ``read()``, using its own RNG per ``ros2_control`` sensor + (seeded from ``std::random_device``, so the noise sequence differs between runs; there is no seed + parameter). + +MJCF has no concept of noise *shape* though — ``noise`` only ever means "standard deviation of zero-mean +noise". Choose the shape with the ``ros2_control`` ``noise_distribution`` parameter, applying to every field +of that sensor: + +.. code-block:: xml + + + fts + fts_sensor + + gaussian + ... + + +``uniform`` draws from ``[-stddev*sqrt(3), stddev*sqrt(3)]``, so its actual standard deviation still matches +the configured MJCF ``noise`` value — switching distributions doesn't change the noise magnitude, only its +shape (bounded vs. unbounded tails). + +For an IMU, the configured ``noise`` values are also surfaced as the diagonal of that field's (previously +always-zero) covariance state interfaces (e.g. ``orientation_covariance.0``), for controllers that consume a +covariance estimate. + .. warning:: Cameras and lidar sensors are no longer supported in the base interface, they are now provided as ``mujoco_ros2_control_plugins``. diff --git a/mujoco_ros2_control/docs/tools.rst b/mujoco_ros2_control/docs/tools.rst index fc2315f5..1d27cdcf 100644 --- a/mujoco_ros2_control/docs/tools.rst +++ b/mujoco_ros2_control/docs/tools.rst @@ -128,11 +128,48 @@ Rough outline of the automated conversion process Duplicate mesh files will have an ``_N`` appended to them to avoid conflicts in the output ``assets`` folder. For instance, if running multiple types of UR robots in one sim, there will be multiple ``shoulder.dae`` files. -- Reads absolute filepaths of all meshes and converts either ``.dae`` or ``.stl`` to ``.obj`` using trimesh. - - - Puts all meshes into an ``assets/`` folder under ``mjcf_data/`` relative to current working dir. - - Modifies filepaths again in URDF to point to the ``assets/`` folder. - - Decomposes large meshes into multiple components to ensure convex hulls. +- Reads absolute filepaths of all meshes (from both ```` and ```` tags) and + materializes them into an ``assets/`` folder under ``mjcf_data/`` relative to the current + working dir, rewriting the URDF mesh paths to point there. + +- Handles visual and collision geometry independently: + + - **Visual** geometry is used purely for rendering (the ``visual`` class). Visual meshes are + plain references - ``.stl``/``.obj`` are kept as-is and only ``.dae`` is converted to ``.obj`` + (MuJoCo cannot load DAE). They live under ``assets/visual/`` and are **never decomposed**; their + color is applied as the geom's ``rgba``. + - **Collision** geometry drives physics (the ``collision`` class). Collision *primitives* + (box/sphere/cylinder/capsule) are used directly. Collision *meshes* are also used directly + (as a single whole-mesh collision geom) **unless** their link is named in a ``decompose_mesh`` + input, in which case they are convex-decomposed with obj2mjcf (coacd) under + ``assets/decomposed/`` at the requested threshold. So decomposition is opt-in per link. + - When a link defines no ````, one is synthesized from its ```` so the visual + mesh is reused as the collision shape. This synthesis happens at the URDF level, so it stays + correct per link even when MuJoCo fuses fixed-jointed bodies together. A mesh shared by a visual + and a collision is converted once and reused for both (and, if that link requests decomposition, + the whole mesh renders while its decomposed pieces collide). + - Both kinds of geom get explicit attributes written directly onto them, so visual/collision + separation does not depend on a user-supplied ```` block: + + - **Visual** geoms (``class="visual"``) are render-only: they never collide and keep + their own material or ``rgba``. + - **Collision** geoms (``class="collision"``), i.e. primitives and whole meshes, do the + colliding and are tinted with the ``bright_orange`` material so they are easy to spot in + the viewer by toggling their group. The material is added to the ```` automatically + when not already defined (an existing definition, e.g. from ``mujoco_inputs``, is left + untouched). + - **Decomposed collision** pieces (``class="decomposed_collision"``) collide like any other + collision geom but live in their own viewer group, so the convex decomposition can be + toggled independently of the other collisions. They are not tinted: they keep obj2mjcf's + per-piece colors so the individual convex hulls stay distinguishable. + +- Handles visual and collision geometry independently: + + - The ```` geometry is used purely for rendering (the ``visual`` class). + - The ```` geometry drives physics (the ``collision`` class). When a link defines no + ````, a collision is synthesized from its ```` so the visual mesh is reused + as the collision shape (the previous behaviour). This synthesis happens at the URDF level, so it + stays correct per link even when MuJoCo fuses fixed-jointed bodies together. - Publishes the new formatted robot description XML file that can be used for conversion. - Converts the new robot description URDF file. diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp index 3244cd06..46c63890 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp @@ -31,6 +31,7 @@ #include #include +#include #include #include @@ -41,7 +42,7 @@ namespace mujoco_ros2_control * Maps to MuJoCo actuator types: * - MOTOR for MuJoCo motor actuator * - POSITION for MuJoCo position actuator - * - VELOCITY for MuJoCo velocity actuator + * - VELOCITY for MuJoCo velocity and integrated-velocity actuators * - CUSTOM for MuJoCo general actuator or other types * * \note the MuJoCo types are as per the MuJoCo documentation: @@ -227,13 +228,59 @@ struct SensorData int mj_sensor_index; }; +/** + * Selects the shape of the noise sampled by `add_sensor_noise()`. Chosen per ros2_control sensor via the + * `noise_distribution` parameter ("gaussian" (default) or "uniform"); MJCF's `noise` attribute has no + * equivalent concept, since MuJoCo only ever means "standard deviation of a zero-mean Gaussian" by it. + */ +enum class NoiseDistribution +{ + kGaussian, + kUniform, +}; + +/** + * Per-sensor noise shape + RNG, shared by every noise-bearing field of a sensor. Bundled together since a + * sensor's fields are always sampled with the same distribution and draw from the same RNG stream. + */ +struct NoiseState +{ + NoiseDistribution distribution = NoiseDistribution::kGaussian; + std::mt19937 rng{ std::random_device{}() }; +}; + +/** + * @param force_noise_stddev Standard deviation (N) of zero-mean Gaussian noise added to `force` on read; taken + * from the underlying MJCF `` sensor's `noise` attribute (0 by default, which disables it). + * @param torque_noise_stddev Standard deviation (N*m) of zero-mean Gaussian noise added to `torque` on read; + * taken from the underlying MJCF `` sensor's `noise` attribute (0 by default, which disables it). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * both noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic across + * runs). + */ struct FTSensorData { std::string name; SensorData force; SensorData torque; + + double force_noise_stddev = 0.0; + double torque_noise_stddev = 0.0; + NoiseState noise; }; +/** + * @param orientation_noise_stddev Standard deviation of zero-mean Gaussian noise added to `orientation` on + * read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param angular_velocity_noise_stddev Standard deviation (rad/s) of zero-mean Gaussian noise added to + * `angular_velocity` on read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param linear_acceleration_noise_stddev Standard deviation (m/s^2) of zero-mean Gaussian noise added to + * `linear_acceleration` on read; taken from the underlying MJCF `` sensor's `noise` attribute + * (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * all three noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic + * across runs). + */ struct IMUSensorData { std::string name; @@ -241,23 +288,51 @@ struct IMUSensorData SensorData angular_velocity; SensorData linear_acceleration; - // These are currently unused but added to support controllers that require them. + double orientation_noise_stddev = 0.0; + double angular_velocity_noise_stddev = 0.0; + double linear_acceleration_noise_stddev = 0.0; + NoiseState noise; + + // Diagonal-only (independent per-axis noise) covariance, derived from the *_noise_stddev fields above at + // registration time. Left at all-zero (as before noise support existed) when noise is not configured. std::vector orientation_covariance; std::vector angular_velocity_covariance; std::vector linear_acceleration_covariance; }; +/** + * @param position_noise_stddev Standard deviation (m) of zero-mean Gaussian noise added to `position` on read; + * taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param orientation_noise_stddev Standard deviation of zero-mean Gaussian noise added to `orientation` on + * read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG shared by + * both noise fields above; the RNG is seeded from `std::random_device` (noise is non-deterministic across + * runs). + */ struct SitePoseData { std::string name; SensorData position; SensorData orientation; + + double position_noise_stddev = 0.0; + double orientation_noise_stddev = 0.0; + NoiseState noise; }; +/** + * @param magnetic_field_noise_stddev Standard deviation of zero-mean Gaussian noise added to `magnetic_field` + * on read; taken from the underlying MJCF `` sensor's `noise` attribute (0 by default). + * @param noise Shape (ros2_control `noise_distribution` parameter, defaults to Gaussian) and RNG, seeded from + * `std::random_device` (noise is non-deterministic across runs). + */ struct MagnetometerSensorData { std::string name; SensorData magnetic_field; + + double magnetic_field_noise_stddev = 0.0; + NoiseState noise; }; } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp index 3209f26c..db832e44 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_simulation.hpp @@ -87,18 +87,21 @@ namespace mujoco_ros2_control * * `apply_control_data(...)` will copy control inputs from the provided mjData into staging * buffers that the physics loop applies to `mj_data_` immediately before each step. - * Specifically, it stages `ctrl`, `qfrc_applied`, and `xfrc_applied`. Cartesian forces from - * `xfrc_applied` compete with inputs from Simulate's drag function, so they are resolved - * separately. This only takes the control staging mutex and never blocks on physics stepping. - * It also stages `qvel`: the system interface NaN-fills `qvel` before every plugin's - * `update()` runs, so any DOF a plugin leaves untouched is NaN here and any DOF it writes a - * finite value into is a requested hard velocity override (see - * `MuJoCoROS2ControlPluginBase::update()`'s doc comment). Non-NaN entries are applied as a - * direct `qvel` write, bypassing the normal dynamics for that DOF. + * Specifically, it stages `ctrl` and `qfrc_applied`. Both of these values should come from + * the interfaces consuming this class. This This only takes the control staging mutex + * and never blocks on physics stepping. * * `overwrite_physics_data(...)` will completely replace the data for the sim. Should be used * with extreme caution. * + * `set_pre_step_callback(...)` registers a callback which will be triggered in the Physics + * Loop. It will be called immediately before `mj_step()`, providing direct access to + * `mj_data_` immediately before progressing the simulation. This gives consumers direct + * access to access and modify the data immediately before integration. Users should be + * extremely careful with this callback, as it exposes "god like" powers to the simulation + * environment and can break, slow down, or otherwise damage the simulation. This should also + * be used with caution. + * * Thread safety is still somewhat messy, as callers are provided with a simulation mutex that * locks the model and data while the actual mujoco engine moves the sim forward. Callers * need to be wary of locking that mutex external to this class, as it can have significant @@ -130,6 +133,15 @@ class MujocoSimulation using KeyCallback = std::function; using VisualizationCallback = std::function; + /** + * @brief Callback function type the `set_pre_step_callback` hook. + * + * Called before `mjStep` in the physics loop, use with care. + * + * @param data The data from the physics simulation, under thread lock. + */ + using PreStepCallback = std::function; + /** * @brief Construct a new Mujoco Simulation object. This is a no-op until initialization. */ @@ -179,6 +191,14 @@ class MujocoSimulation /** Queue physics steps and wait for completion. */ SimulationStepResult request_simulation_steps(uint32_t steps, std::chrono::milliseconds timeout); + /** + * @brief Register the callback run before every `mj_step()`. + * + * See the class documentation for more information. Any function called here will hold + * the simulation mutex and block the physics loop. Use with care. + */ + void set_pre_step_callback(PreStepCallback callback); + /** * @brief Start the physics thread. Must be called after load_model(). */ @@ -314,6 +334,8 @@ class MujocoSimulation * and applied under the same lock as `ctrl`, so a step cannot observe a new setpoint with * old coefficients (or vice versa). This does not lock the sim mutex and never waits on * stepping. + * Anything else that requires direct access to simulation data can use + * `set_pre_step_callback`. */ void apply_control_data(mjData* control_data, const std::vector& actuator_parameter_updates = {}); @@ -356,6 +378,9 @@ class MujocoSimulation /** Compose viewer and plugin Cartesian forces without applying other staged inputs. */ void compose_cartesian_forces(); + /** Run the pre-step callback and record the resulting xfrc_applied for viewer-drag detection. */ + void run_pre_step_callback(); + /** * @brief Publishes the per-step control state from `mj_data_`. * @@ -571,6 +596,8 @@ class MujocoSimulation // Threads for rendering physics and the UI simulation std::thread physics_thread_; std::thread ui_thread_; + // Distinguishes a programmatic RenderLoop wakeup from the user closing its window. + std::atomic explicit_shutdown_requested_{ false }; // Primary clock publisher for the world std::shared_ptr> clock_publisher_; @@ -622,6 +649,9 @@ class MujocoSimulation VisualizationCallback visualization_callback_; mjvScene plugin_visualization_scene_{}; bool plugin_visualization_scene_initialized_{ false }; + + // Callback run immediately before every mj_step(), defaults to nothing. + PreStepCallback pre_step_callback_{ [](mjData* /*data*/) {} }; }; } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp index 0870962e..0dadebfb 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp @@ -246,6 +246,17 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface * * * + * + * Each underlying MJCF sensor element (`force`, `torque`, `framequat`, `gyro`, `accelerometer`, `framepos`, + * `magnetometer`) may set its own `noise` attribute (standard deviation, in the sensor's native units). + * MuJoCo compiles this into `mjModel::sensor_noise` but does not apply it itself, so `read()` adds it on + * every cycle, using an RNG seeded from `std::random_device` (so noise differs between runs). Setting + * `noise="0"` (the MJCF default) keeps a sensor noise-free. + * + * The noise *magnitude* thus comes entirely from the MJCF; MJCF has no concept of distribution shape + * though, so a ros2_control `` ("gaussian" (default) or "uniform"), + * applying to every field of that sensor, selects it. Uniform noise is drawn from + * `[-stddev*sqrt(3), stddev*sqrt(3)]`, so its actual standard deviation still matches the MJCF value. */ void register_sensors(const hardware_interface::HardwareInfo& info); diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp index c88f9df4..022c9cbe 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp @@ -19,7 +19,17 @@ #pragma once +#include +#include #include +#include + +#include "mujoco_ros2_control/data.hpp" + +#include +#include +#include +#include namespace mujoco_ros2_control { @@ -41,4 +51,144 @@ get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, cons return std::nullopt; } +/** + * @brief Returns the value of a sensor-level `` (from the sensor's own ComponentInfo), or a default + * value if the sensor has no such parameter set. + */ +inline std::string get_sensor_parameter_or(const hardware_interface::ComponentInfo& sensor, const std::string& key, + const std::string& default_value) +{ + if (auto it = sensor.parameters.find(key); it != sensor.parameters.end()) + { + return it->second; + } + return default_value; +} + +/** + * @brief Reads the sensor's `noise_distribution` parameter ("gaussian" (default) or "uniform"). + * A value that is neither (including a typo) logs a warning and is treated as Gaussian; an absent + * parameter is treated as Gaussian silently. + */ +inline NoiseDistribution get_noise_distribution(const hardware_interface::ComponentInfo& sensor) +{ + const std::string value = get_sensor_parameter_or(sensor, "noise_distribution", "gaussian"); + if (value != "gaussian" && value != "uniform") + { + RCLCPP_WARN(rclcpp::get_logger("mujoco_ros2_control"), + "Only noise distributions of 'gaussian' or 'uniform' are allowed, but you selected '%s'. " + "Defaulting to 'gaussian'.", + value.c_str()); + } + return value == "uniform" ? NoiseDistribution::kUniform : NoiseDistribution::kGaussian; +} + +/** + * @brief Logs, at INFO level, the noise distribution and standard deviation configured for each of a + * sensor's noise-bearing components (e.g. an IMU's `orientation`, `angular_velocity`, and + * `linear_acceleration` fields), identified by their underlying MJCF sensor names. Called once per sensor + * from register_sensors() so the console output documents every sensor's active noise configuration. + */ +inline void log_sensor_components_noise(const rclcpp::Logger& logger, NoiseDistribution distribution, + std::initializer_list> components) +{ + const char* distribution_name = (distribution == NoiseDistribution::kUniform ? "uniform" : "gaussian"); + for (const auto& [component_name, noise_stddev] : components) + { + RCLCPP_INFO(logger, "Sensor component '%s' is using '%s' noise with stddev: %f", component_name.c_str(), + distribution_name, noise_stddev); + } +} + +/** + * @brief Sets a flat, row-major 3x3 covariance's diagonal to `stddev * stddev`, leaving off-diagonal terms + * untouched (0 for a freshly-`resize()`d vector, matching the "independent per-axis noise" assumption). + */ +inline void set_diagonal_covariance(std::vector& covariance, double stddev) +{ + assert(covariance.size() == 9 && "covariance must be a flat 3x3 (9-element) matrix"); + covariance[0] = covariance[4] = covariance[8] = stddev * stddev; +} + +namespace detail +{ + +/** + * @brief Fills an N-vector with independent zero-mean noise samples of the given standard deviation and + * shape, constructing the underlying distribution once and drawing all N samples from it. + * + * Uniform noise is drawn from `[-stddev*sqrt(3), stddev*sqrt(3)]` (the range of a uniform distribution + * whose own standard deviation is `stddev`), so switching a sensor's `noise_distribution` doesn't change + * the magnitude implied by its MJCF `noise` value. + */ +template +inline Eigen::Matrix sample_noise(double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + Eigen::Matrix samples; + if (distribution == NoiseDistribution::kUniform) + { + const double bound = stddev * std::sqrt(3.0); + std::uniform_real_distribution dist(-bound, bound); + for (int i = 0; i < N; ++i) + { + samples[i] = dist(rng); + } + } + else + { + std::normal_distribution dist(0.0, stddev); + for (int i = 0; i < N; ++i) + { + samples[i] = dist(rng); + } + } + return samples; +} + +} // namespace detail + +/** + * @brief Adds zero-mean noise, independently sampled per axis, to a 3D vector in place. + * No-op if `stddev` is not positive, so a disabled (default) sensor pays no sampling cost. + */ +inline void add_sensor_noise(Eigen::Vector3d& value, double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + if (stddev <= 0.0) + { + return; + } + value += detail::sample_noise<3>(stddev, distribution, rng); +} + +/** + * @brief Adds zero-mean noise to a quaternion's coefficients, then renormalizes. + * + * This is a small-angle approximation of orientation noise: accurate for the small stddev values noise + * configuration is expected to use, but not a proper noise model on SO(3) for large values. + * No-op if `stddev` is not positive. + */ +inline void add_sensor_noise(Eigen::Quaterniond& value, double stddev, NoiseDistribution distribution, std::mt19937& rng) +{ + if (stddev <= 0.0) + { + return; + } + value.coeffs() += detail::sample_noise<4>(stddev, distribution, rng); + value.normalize(); +} + +/** + * @brief Convenience overloads taking a sensor's bundled NoiseState instead of a separate + * distribution/rng pair. + */ +inline void add_sensor_noise(Eigen::Vector3d& value, double stddev, NoiseState& noise) +{ + add_sensor_noise(value, stddev, noise.distribution, noise.rng); +} + +inline void add_sensor_noise(Eigen::Quaterniond& value, double stddev, NoiseState& noise) +{ + add_sensor_noise(value, stddev, noise.distribution, noise.rng); +} + } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/mujoco_ros2_control/__init__.py b/mujoco_ros2_control/mujoco_ros2_control/__init__.py index d37a0669..b9b0d691 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/__init__.py +++ b/mujoco_ros2_control/mujoco_ros2_control/__init__.py @@ -15,6 +15,7 @@ from .urdf_to_mujoco_utils import ( add_mujoco_info, remove_tag, + add_missing_collisions, extract_mesh_info, replace_package_names, get_images_from_dae, @@ -23,6 +24,11 @@ update_obj_assets, update_non_obj_assets, add_mujoco_inputs, + ensure_default_classes, + ensure_collision_material, + decompose_threshold, + mesh_home, + resolve_pregenerated, get_processed_mujoco_inputs, parse_inputs_xml, parse_scene_xml, @@ -42,11 +48,15 @@ write_mujoco_scene, DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, + DEFAULT_DECOMPOSE_THRESHOLD, + COLLISION_MATERIAL_NAME, ) __all__ = [ "add_mujoco_info", "remove_tag", + "add_missing_collisions", "extract_mesh_info", "replace_package_names", "get_images_from_dae", @@ -55,6 +65,11 @@ "update_obj_assets", "update_non_obj_assets", "add_mujoco_inputs", + "ensure_default_classes", + "ensure_collision_material", + "decompose_threshold", + "mesh_home", + "resolve_pregenerated", "get_processed_mujoco_inputs", "parse_inputs_xml", "parse_scene_xml", @@ -74,4 +89,7 @@ "write_mujoco_scene", DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, + DEFAULT_DECOMPOSE_THRESHOLD, + COLLISION_MATERIAL_NAME, ] diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index bde93636..b6200d52 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -17,6 +17,7 @@ # License for the specific language governing permissions and limitations # under the License +import hashlib import os import pathlib import re @@ -35,6 +36,34 @@ # Hardcoded relative paths for MuJoCo asset outputs DECOMPOSED_PATH_NAME = "decomposed" COMPOSED_PATH_NAME = "full" +# Plain visual meshes (rendering only) live here; they are referenced as-is and never +# decomposed. Collision meshes go through DECOMPOSED_PATH_NAME (obj2mjcf --decompose). +VISUAL_PATH_NAME = "visual" + +# coacd threshold used when a collision mesh is not named in a decompose_mesh input. +# Matches obj2mjcf's coacd default. +DEFAULT_DECOMPOSE_THRESHOLD = "0.05" + +# Explicit attributes written onto classified geoms so visual/collision separation does not +# depend on the user supplying a block via mujoco_inputs. +# +# - Visuals are render-only: they never collide and keep their own material/rgba. +# - Whole-mesh and primitive collisions do the colliding and are tinted +# COLLISION_MATERIAL_NAME so the collision shape can be inspected in the viewer. +# - Decomposed collision pieces collide the same way but live in their own viewer group, +# so the convex decomposition can be toggled independently of the other collisions, +# and are NOT tinted: they keep obj2mjcf's per-piece rgba so the hulls stay +# distinguishable from one another. +# The groups match the default classes injected by ensure_default_classes. +COLLISION_MATERIAL_NAME = "bright_orange" +VISUAL_GEOM_ATTRS = {"contype": "0", "conaffinity": "0", "group": "2", "density": "0"} +COLLISION_GEOM_ATTRS = { + "group": "3", + "contype": "1", + "conaffinity": "1", + "material": COLLISION_MATERIAL_NAME, +} +DECOMPOSED_COLLISION_GEOM_ATTRS = {"group": "4", "contype": "1", "conaffinity": "1"} def add_mujoco_info(raw_xml, output_filepath, publish_topic, fuse=True): @@ -84,11 +113,53 @@ def remove_tag(xml_string, tag_to_remove): return xmldoc.toprettyxml() +def add_missing_collisions(xml_string): + """ + Ensures every link that can be rendered can also collide, while respecting any + collision geometry the URDF author already provided. + + For each that has at least one but no , a + is synthesized for every visual by copying that visual's and . + Links that already declare a collision are left untouched (the authored collision + drives physics), and links without any visual are left untouched. + + Establishing this fallback at the URDF level - before MuJoCo conversion and any + static-body fusion - keeps the visual->collision fallback correct per link: the + visual meshes are used purely for rendering while the collision geometry (authored + when present, copied from the visual otherwise) is used for physics. + + :param xml_string: the URDF as a string + :returns: the URDF string with synthesized collisions added where they were missing + """ + dom = minidom.parseString(xml_string) + + for link in dom.getElementsByTagName("link"): + visuals = [c for c in link.childNodes if c.nodeType == c.ELEMENT_NODE and c.tagName == "visual"] + collisions = [c for c in link.childNodes if c.nodeType == c.ELEMENT_NODE and c.tagName == "collision"] + + # Respect authored collisions and skip links with nothing to render. + if collisions or not visuals: + continue + + for visual in visuals: + collision = dom.createElement("collision") + # Copy the geometry and origin (collisions carry no material in URDF). + for child in visual.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName in ("geometry", "origin"): + collision.appendChild(child.cloneNode(deep=True)) + link.appendChild(collision) + + return dom.toprettyxml() + + def extract_mesh_info(raw_xml, asset_dir, decompose_dict): """ - Builds a dictionary of all unique visual meshes in the URDF and rewrites the - raw_xml so every mesh filename points at a disambiguated path. There are some - gotchas: + Builds a dictionary of all unique meshes in the URDF (from both and + tags) and rewrites the raw_xml so every mesh filename points at a + disambiguated path. Visuals are processed first so their color wins; + a collision that reuses a visual's mesh file shares that asset, while a distinct + collision mesh gets its own entry (defaulting to white, as collisions carry no + material). There are some gotchas: - Different URDF meshes can share a filename stem, so colliding stems get an "__N" suffix so each resolves to its own asset directory. For example, if @@ -127,101 +198,229 @@ def resolve_color(visual): return tuple(ref.color.rgba) return (1.0, 1.0, 1.0, 1.0) + # Map each source mesh file to the entry (stem) it produced, so a collision that + # reuses a visual's mesh (the fallback case) shares the single converted source + # instead of duplicating it. Visuals are processed first so their material color + # wins; a shared entry is then flagged for both visual and collision use, and the + # downstream pipeline emits a whole visual mesh and decomposed collision pieces from + # that one source. + uri_to_stem = {} + + # Content-addressed lookup so a collision mesh that is a byte-identical copy of an + # already-cataloged file (e.g., collision/ holds copies of visual/) collapses into + # that entry instead of becoming a disambiguated duplicate. + content_to_stem = {} + file_digests = {} + + def file_digest(path): + if path not in file_digests: + try: + with open(path, "rb") as f: + file_digests[path] = hashlib.md5(f.read()).hexdigest() + except OSError: + file_digests[path] = None + return file_digests[path] + + def mark_usage(stem_key, is_collision): + entry = mesh_info_dict[stem_key] + if is_collision: + entry["used_as_collision"] = True + else: + entry["used_as_visual"] = True + + def process_geometry(element, is_collision): + nonlocal raw_xml + geom = element.geometry + if not (geom and hasattr(geom, "filename")): + return + + uri = geom.filename # full URI + + # A collision reusing a visual's mesh file just records the extra usage on the + # shared entry (one converted source, reused for both classes). + if is_collision and uri in uri_to_stem: + mark_usage(uri_to_stem[uri], is_collision) + return + + scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" + + # A collision mesh byte-identical to an already-cataloged file at the same scale + # also collapses into that entry: identical content means identical converted + # assets and decomposition. Rewrite the URDF so both references share one file + # and MuJoCo sees a single asset. + digest = file_digest(uri) + if is_collision and digest is not None: + shared_stem = content_to_stem.get((digest, scale)) + if shared_stem is not None: + shared = mesh_info_dict[shared_stem] + shared_ref = shared["new_filepath"] if shared["source_stem"] != shared_stem else shared["filename"] + raw_xml = raw_xml.replace(uri, shared_ref) + uri_to_stem.setdefault(uri, shared_stem) + mark_usage(shared_stem, is_collision) + return + + original_stem = pathlib.Path(uri).stem # remember original for disambiguation + stem = original_stem + counter = 0 + while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: + counter += 1 + stem = f"{original_stem}__{counter}" + stem_to_original_uri[stem] = uri + + # Collision elements carry no , so they default to white. + rgba = (1.0, 1.0, 1.0, 1.0) if is_collision else resolve_color(element) + + mesh_dict_value = { + "filename": uri, + "scale": scale, + "color": rgba, + } + + # check to see if the values we are trying to add already exist + existing_identifier = None + for key, value in mesh_info_dict.items(): + if mesh_dict_value.items() <= value.items(): + existing_identifier = key + break + + # an identical mesh already exists: reuse it and just record this usage + if existing_identifier is not None: + uri_to_stem.setdefault(uri, existing_identifier) + mark_usage(existing_identifier, is_collision) + return + + # the (possibly disambiguated) reference the URDF will carry for this entry + path_obj = pathlib.Path(uri) + new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) + + # add the unique name to the dictionary. + # Whether an entry can reuse a cached asset depends on where it will live + # (visual/full/decomposed), which depends on usage flags that are + # only complete after both passes - see resolve_pregenerated below. + mesh_info_dict[stem] = { + "is_pre_generated": False, + "filename": uri, + "scale": scale, + "color": rgba, + "new_filepath": new_filepath, + "source_stem": original_stem, + # which classes reference this source; a shared source carries both + "used_as_visual": not is_collision, + "used_as_collision": is_collision, + } + uri_to_stem.setdefault(uri, stem) + if digest is not None: + content_to_stem.setdefault((digest, scale), stem) + + # if we changed the identifier, make sure we update it in the underlying file + if stem != original_stem: + raw_xml = raw_xml.replace(uri, new_filepath) + + # First pass: visual meshes (their material color wins). Second pass: collision + # meshes, which only add genuinely distinct meshes thanks to processed_uris. for link in robot.links: for vis in link.visuals: - geom = vis.geometry - if not (geom and hasattr(geom, "filename")): - continue + process_geometry(vis, is_collision=False) + for link in robot.links: + for col in link.collisions: + process_geometry(col, is_collision=True) - uri = geom.filename # full URI - original_stem = pathlib.Path(uri).stem # NEW: remember original for disambiguation - stem = original_stem - counter = 0 - while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: - counter += 1 - stem = f"{original_stem}__{counter}" - stem_to_original_uri[stem] = uri - - # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original - is_pre_generated = False - new_uri = uri # default fallback - - if asset_dir: - if original_stem in decompose_dict: - # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches - mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" - settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" - - if os.path.exists(mesh_file) and os.path.exists(settings_file): - try: - with open(settings_file) as f: - data = json.load(f) - used_threshold = float(data.get(f"{stem}")) - except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: - print(f"Warning: could not read thresholds for {stem}: {e}") - used_threshold = None - # Use existing decomposed object only if it has the same threshold, otherwise regenerate it. - if used_threshold is not None and math.isclose( - used_threshold, float(decompose_dict[original_stem]), rel_tol=1e-9 - ): - new_uri = mesh_file - is_pre_generated = True - raw_xml = raw_xml.replace(geom.filename, new_uri) - else: - print( - f"Existing decomposed obj for {stem} has different threshold {used_threshold} " - f"than required {decompose_dict[original_stem]}. Regenerating..." - ) - else: - # Composed mesh: check if a pre-generated OBJ exists - mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" - - if os.path.exists(mesh_file): - new_uri = mesh_file - is_pre_generated = True - raw_xml = raw_xml.replace(geom.filename, new_uri) - - scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" - rgba = resolve_color(vis) - - mesh_dict_value = { - "is_pre_generated": is_pre_generated, - "filename": new_uri, - "scale": scale, - "color": rgba, - } - - # check to see if the values we are trying to add already exist - existing_identifier = None - for key, value in mesh_info_dict.items(): - if mesh_dict_value.items() <= value.items(): - existing_identifier = key - break + # Phase 2: with usage flags final, probe the pregenerated cache at each entry's home + raw_xml = resolve_pregenerated(mesh_info_dict, raw_xml, asset_dir, decompose_dict) - # if the values we want to add are not in the dictionary yet, add them - if existing_identifier is None: - # get the name of the new file so that we can reference it later, but grab correct - # pre generated asset if it exists - path_obj = pathlib.Path(new_uri) - if is_pre_generated: - new_filepath = str(path_obj.parent.parent / stem / (stem + path_obj.suffix)) - else: - new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) + return mesh_info_dict, raw_xml - # add the unique name to the dictionary - mesh_info_dict[stem] = { - "is_pre_generated": is_pre_generated, - "filename": new_uri, - "scale": scale, - "color": rgba, - "new_filepath": new_filepath, - } - # if we changed the identifier, make sure we update it in the underlying file - if stem != pathlib.Path(new_uri).stem: - raw_xml = raw_xml.replace(new_uri, new_filepath) +def decompose_threshold(mesh_name, mesh_item, decompose_dict): + """ + Returns the coacd threshold for a mesh entry that should be convex-decomposed, or + None when it should not be. Only collision-used meshes decompose, and only when + named in a decompose_mesh input - matched by entry name or, for entries renamed by + stem disambiguation, by the source file's stem. + """ + if not mesh_item.get("used_as_collision", False): + return None + return decompose_dict.get(mesh_name, decompose_dict.get(mesh_item.get("source_stem"))) - return mesh_info_dict, raw_xml + +def mesh_home(mesh_name, mesh_item, decompose_dict): + """ + The single source of truth for where a mesh entry lives in the assets tree: + + - DECOMPOSED_PATH_NAME for meshes that decompose + - COMPOSED_PATH_NAME for other collision-used meshes + - VISUAL_PATH_NAME for render-only meshes. + + The cache probe, conversion routing, obj2mjcf runs, and the pregenerated copy-in + must all agree on this, so they all call it. + """ + if decompose_threshold(mesh_name, mesh_item, decompose_dict) is not None: + return DECOMPOSED_PATH_NAME + if mesh_item.get("used_as_collision", False): + return COMPOSED_PATH_NAME + return VISUAL_PATH_NAME + + +def resolve_pregenerated(mesh_info_dict, raw_xml, asset_dir, decompose_dict): + """ + Probes the pregenerated asset cache (--asset_dir) for every catalog entry and, on a + hit, points the entry and the URDF at the cached file by ABSOLUTE path. The MuJoCo + compile then reads the cache directly; copy_pre_generated_meshes later brings the + cached assets into the output tree and post-processing relativizes the references. + + Runs after both catalog passes because an entry's home (visual/full/decomposed) + depends on its final usage flags. A decomposed home is only accepted when the coacd + threshold recorded in the cache's metadata.json matches the requested one. + """ + if not asset_dir: + return raw_xml + + metadata = {} + metadata_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" + if os.path.exists(metadata_file): + try: + with open(metadata_file) as f: + metadata = json.load(f) + except (PermissionError, json.JSONDecodeError) as e: + print(f"Warning: could not read decomposition metadata: {e}") + + for mesh_name, mesh_item in mesh_info_dict.items(): + home = mesh_home(mesh_name, mesh_item, decompose_dict) + + if home == DECOMPOSED_PATH_NAME: + threshold = decompose_threshold(mesh_name, mesh_item, decompose_dict) + candidates = [f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/{mesh_name}.obj"] + used_threshold = metadata.get(mesh_name) + if used_threshold is not None and not math.isclose(float(used_threshold), float(threshold), rel_tol=1e-9): + print( + f"Existing decomposed obj for {mesh_name} has different threshold " + f"{used_threshold} than required {threshold}. Regenerating..." + ) + continue + if used_threshold is None: + # no metadata record: the cached folder (if any) is unverifiable + continue + else: + candidates = [f"{asset_dir}/{home}/{mesh_name}{ext}" for ext in (".obj", ".stl")] + + # the string the URDF currently carries for this entry: the raw source URI, or + # the disambiguated path substituted when the stem was renamed (new_filepath is + # pathlib-normalized, so it only matches the document in the renamed case) + if mesh_item.get("source_stem") != mesh_name: + current_ref = mesh_item["new_filepath"] + else: + current_ref = mesh_item["filename"] + + for candidate in candidates: + if not os.path.exists(candidate): + continue + raw_xml = raw_xml.replace(current_ref, candidate) + mesh_item["filename"] = candidate + mesh_item["new_filepath"] = candidate + mesh_item["is_pre_generated"] = True + break + + return raw_xml def replace_package_names(xml_data): @@ -353,6 +552,24 @@ def set_up_axis_to_z_up(dae_file_path): def update_obj_assets(dom, output_filepath, mesh_info_dict): + """ + Expands every obj2mjcf-processed mesh into the final model. + + Two kinds of processed meshes exist: + + - Decomposed collision meshes (under DECOMPOSED_PATH_NAME): their collision geoms + are replaced with the convex pieces in the ``decomposed_collision`` class, keeping + obj2mjcf's per-piece colors so the hulls stay distinguishable when their viewer + group is toggled on. The whole ```` asset is kept while a visual still + references it. + - Plain meshes (under COMPOSED_PATH_NAME): the whole-mesh asset is swapped for + obj2mjcf's per-material submeshes, materials, and textures. + + In both cases obj2mjcf's own classification drives the geom split: its + class="visual" sub-geoms (carrying materials) replace URDF visuals, its collision + sub-geoms replace URDF collisions. This is what lets visual meshes render with + their real materials instead of a flat rgba. + """ # Find the element asset = dom.getElementsByTagName("asset") @@ -362,113 +579,219 @@ def update_obj_assets(dom, output_filepath, mesh_info_dict): print("No assets in URDF, skipping conversions...") return dom - # Find the element - worldbody = dom.getElementsByTagName("worldbody") - worldbody_element = worldbody[0] - worldbody_geoms = worldbody_element.getElementsByTagName("geom") - - # get all of the mesh tags in the asset element + worldbody_element = dom.getElementsByTagName("worldbody")[0] asset_element = asset[0] - meshes = asset_element.getElementsByTagName("mesh") - # obj - full_decomposed_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}" - full_composed_path = f"{output_filepath}assets/{COMPOSED_PATH_NAME}" - decomposed_dirs = [ - name for name in os.listdir(full_decomposed_path) if os.path.isdir(os.path.join(full_decomposed_path, name)) - ] - composed_dirs = [ - name for name in os.listdir(full_composed_path) if os.path.isdir(os.path.join(full_composed_path, name)) - ] + # obj2mjcf emits its meshes WITHOUT a name attribute, so MuJoCo derives the name from + # the file stem. We dedupe on that "effective name" (explicit name, else file stem): + # this skips obj2mjcf's whole/visual mesh when it collides with the kept whole mesh, + # while still letting every uniquely-named piece through. + def effective_mesh_name(m): + name = m.getAttribute("name") + return name if name else pathlib.Path(m.getAttribute("file")).stem + + existing_mesh_names = {effective_mesh_name(m) for m in asset_element.getElementsByTagName("mesh")} + existing_material_names = {m.getAttribute("name") for m in asset_element.getElementsByTagName("material")} + existing_texture_names = {t.getAttribute("name") for t in asset_element.getElementsByTagName("texture")} + + def merge_sub_assets(sub_asset_element, path_prefix, scale): + """Appends the obj2mjcf file's meshes/materials/textures into the model's assets, + de-duplicated by name, with file paths rewritten relative to the assets dir.""" + for sub_mesh in sub_asset_element.getElementsByTagName("mesh"): + eff = effective_mesh_name(sub_mesh) + if eff in existing_mesh_names: + continue + sub_mesh.setAttribute("file", f"{path_prefix}/{sub_mesh.getAttribute('file')}") + if scale: + sub_mesh.setAttribute("scale", scale) + asset_element.appendChild(sub_mesh) + existing_mesh_names.add(eff) + for sub_material in sub_asset_element.getElementsByTagName("material"): + name = sub_material.getAttribute("name") + if name in existing_material_names: + continue + asset_element.appendChild(sub_material) + existing_material_names.add(name) + for sub_texture in sub_asset_element.getElementsByTagName("texture"): + if sub_texture.hasAttribute("file"): + name = sub_texture.getAttribute("name") + if name in existing_texture_names: + continue + sub_texture.setAttribute("file", f"{path_prefix}/{sub_texture.getAttribute('file')}") + asset_element.appendChild(sub_texture) + existing_texture_names.add(name) + + def replace_geoms(mesh_name, sub_geoms, collision_class=None): + """ + Replaces every geom referencing mesh_name with the sub-geoms matching its role: + obj2mjcf's class="visual" sub-geoms for a URDF visual (which carries a contype marker + from the import), the rest for a URDF collision. + + Every clone gets the explicit visual/collision attributes, the same way + update_non_obj_assets classifies the geoms MuJoCo imported directly. + + When ``collision_class`` is given the collision sub-geoms are decomposed pieces: they + are routed into that class and get DECOMPOSED_COLLISION_GEOM_ATTRS (own viewer group, + no tint) instead of COLLISION_GEOM_ATTRS. + """ + for geom_element in list(worldbody_element.getElementsByTagName("geom")): + if geom_element.getAttribute("mesh") != mesh_name: + continue + is_visual = geom_element.hasAttribute("contype") + pos = geom_element.getAttribute("pos") + quat = geom_element.getAttribute("quat") + parent = geom_element.parentNode + parent.removeChild(geom_element) + for sub_geom in sub_geoms: + # match obj2mjcf's own classification to the role of the replaced geom + if (sub_geom.getAttribute("class") == "visual") != is_visual: + continue + sub_geom_local = sub_geom.cloneNode(False) + if pos: + sub_geom_local.setAttribute("pos", pos) + if quat: + sub_geom_local.setAttribute("quat", quat) + if is_visual: + attributes = VISUAL_GEOM_ATTRS + elif collision_class: + sub_geom_local.setAttribute("class", collision_class) + attributes = DECOMPOSED_COLLISION_GEOM_ATTRS + else: + attributes = COLLISION_GEOM_ATTRS + # a geom rgba would override the tint material + if sub_geom_local.hasAttribute("rgba"): + sub_geom_local.removeAttribute("rgba") + for attribute, value in attributes.items(): + sub_geom_local.setAttribute(attribute, value) + parent.appendChild(sub_geom_local) + + def expand_sibling_collisions(mesh_name, source_file, collision_class, collision_sub_geoms): + """ + Handles MuJoCo's auto-renamed scaled siblings of ``mesh_name`` (same source file, + e.g. a mirrored link's mesh re-emitted as "leg1" with scale="1 -1 1"): they have no + entry of their own in mesh_info_dict, so the main loop skips them entirely. Their + collision geoms must still be decomposed - each convex piece gets its own + sibling-scaled copy (name "__") instead of leaving the sibling as a + single whole-mesh collider. The whole sibling asset is dropped afterwards since + nothing but its own (now-replaced) collision geom referenced it. + """ + for sibling in list(asset_element.getElementsByTagName("mesh")): + sibling_name = sibling.getAttribute("name") + if not sibling_name or sibling_name == mesh_name or sibling_name in mesh_info_dict: + continue + if sibling.getAttribute("file") != source_file: + continue + sibling_scale = sibling.getAttribute("scale") - for mesh in meshes: + for geom_element in list(worldbody_element.getElementsByTagName("geom")): + if geom_element.getAttribute("mesh") != sibling_name or geom_element.hasAttribute("contype"): + continue + pos = geom_element.getAttribute("pos") + quat = geom_element.getAttribute("quat") + parent = geom_element.parentNode + parent.removeChild(geom_element) + for sub_geom in collision_sub_geoms: + piece_name = sub_geom.getAttribute("mesh") + scaled_name = f"{piece_name}__{sibling_name}" + if scaled_name not in existing_mesh_names: + piece_mesh = next( + m + for m in asset_element.getElementsByTagName("mesh") + if effective_mesh_name(m) == piece_name + ) + scaled_mesh = dom.createElement("mesh") + scaled_mesh.setAttribute("name", scaled_name) + scaled_mesh.setAttribute("file", piece_mesh.getAttribute("file")) + if sibling_scale: + scaled_mesh.setAttribute("scale", sibling_scale) + asset_element.appendChild(scaled_mesh) + existing_mesh_names.add(scaled_name) + sub_geom_local = sub_geom.cloneNode(False) + sub_geom_local.setAttribute("mesh", scaled_name) + if pos: + sub_geom_local.setAttribute("pos", pos) + if quat: + sub_geom_local.setAttribute("quat", quat) + sub_geom_local.setAttribute("class", collision_class) + for attribute, value in DECOMPOSED_COLLISION_GEOM_ATTRS.items(): + sub_geom_local.setAttribute(attribute, value) + parent.appendChild(sub_geom_local) + + asset_element.removeChild(sibling) + existing_mesh_names.discard(sibling_name) + + for mesh in list(asset_element.getElementsByTagName("mesh")): mesh_name = mesh.getAttribute("name") - # This should definitely be there, otherwise something is horribly wrong - scale = mesh_info_dict[mesh_name]["scale"] + # skip assets with no entry of their own, e.g. MuJoCo's auto-renamed scaled + # sibling of a mesh referenced at two different scales + if mesh_name not in mesh_info_dict: + continue - mesh_path = "" - if mesh_name in decomposed_dirs: - composed_type = DECOMPOSED_PATH_NAME - mesh_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/{mesh_name}.xml" - elif mesh_name in composed_dirs: - composed_type = COMPOSED_PATH_NAME - mesh_path = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}.xml" + source_file = mesh.getAttribute("file") + decomposed_prefix = f"{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" + plain_prefixes = (f"{COMPOSED_PATH_NAME}/{mesh_name}", f"{VISUAL_PATH_NAME}/{mesh_name}") + if os.path.exists(f"{output_filepath}assets/{decomposed_prefix}/{mesh_name}.xml"): + path_prefix = decomposed_prefix + collision_class = "decomposed_collision" + # keep the whole asset while a visual still resolves against it + keep_whole_mesh = mesh_info_dict[mesh_name].get("used_as_visual", False) + else: + # plain mesh: composed dir (collision-used) or visual dir (render-only) + path_prefix = next( + (p for p in plain_prefixes if os.path.exists(f"{output_filepath}assets/{p}/{mesh_name}.xml")), None + ) + if path_prefix is None: + # not processed by obj2mjcf (e.g. an unconverted plain .stl reference) + continue + collision_class = None + # the per-material submeshes fully replace the whole-mesh asset + keep_whole_mesh = False - if mesh_path: - sub_dom = minidom.parse(mesh_path) - # Find the element - sub_asset = sub_dom.getElementsByTagName("asset") - sub_asset_element = sub_asset[0] + sub_dom = minidom.parse(f"{output_filepath}assets/{path_prefix}/{mesh_name}.xml") + sub_asset_element = sub_dom.getElementsByTagName("asset")[0] - # remove the old mesh element that is not separated + if not keep_whole_mesh: asset_element.removeChild(mesh) + existing_mesh_names.discard(mesh_name) - # bring in the new elements - sub_meshes = sub_asset_element.getElementsByTagName("mesh") - for sub_mesh in sub_meshes: - sub_mesh_file = sub_mesh.getAttribute("file") - if composed_type == DECOMPOSED_PATH_NAME: - sub_mesh.setAttribute("file", f"{composed_type}/{mesh_name}/{mesh_name}/{sub_mesh_file}") - else: - sub_mesh.setAttribute("file", f"{composed_type}/{mesh_name}/{sub_mesh_file}") - if scale: - sub_mesh.setAttribute("scale", scale) - asset_element.appendChild(sub_mesh) - - # bring in the materials - sub_materials = sub_asset_element.getElementsByTagName("material") - for sub_material in sub_materials: - asset_element.appendChild(sub_material) - - # bring in the textures, and modify filepath to properly reference filepaths - sub_textures = sub_asset_element.getElementsByTagName("texture") - for sub_texture in sub_textures: - if sub_texture.hasAttribute("file"): - sub_texture_file = sub_texture.getAttribute("file") - if composed_type == DECOMPOSED_PATH_NAME: - sub_texture.setAttribute("file", f"{composed_type}/{mesh_name}/{mesh_name}/{sub_texture_file}") - else: - sub_texture.setAttribute("file", f"{composed_type}/{mesh_name}/{sub_texture_file}") - asset_element.appendChild(sub_texture) - - sub_body = sub_dom.getElementsByTagName("body") - sub_body = sub_body[0] - - # change the geoms - body = sub_dom.getElementsByTagName("body") - body_element = body[0] - sub_geoms = body_element.getElementsByTagName("geom") - for geom_element in worldbody_geoms: - if geom_element.getAttribute("mesh") == mesh_name: - pos = geom_element.getAttribute("pos") - quat = geom_element.getAttribute("quat") - - parent = geom_element.parentNode - parent.removeChild(geom_element) - for sub_geom in sub_geoms: - sub_geom_local = sub_geom.cloneNode(False) - sub_geom_local.setAttribute("pos", pos) - sub_geom_local.setAttribute("quat", quat) - parent.appendChild(sub_geom_local) + merge_sub_assets(sub_asset_element, path_prefix, mesh_info_dict[mesh_name]["scale"]) + sub_geoms = sub_dom.getElementsByTagName("body")[0].getElementsByTagName("geom") + replace_geoms(mesh_name, sub_geoms, collision_class) + + if collision_class: + collision_sub_geoms = [g for g in sub_geoms if g.getAttribute("class") != "visual"] + expand_sibling_collisions(mesh_name, source_file, collision_class, collision_sub_geoms) return dom -def update_non_obj_assets(dom, output_filepath): +def update_non_obj_assets(dom, output_filepath, mesh_info_dict=None): """ - We want to take the group 1 objects that get created, and turn them into the equivalent - but both in group 2 and in group 3. That means taking something like this - - and turning it into this - - + Classifies the geoms that MuJoCo imported from the URDF into the "visual" and + "collision" default classes. Because the source URDF now always carries both a + and a per renderable link (the latter synthesized from the + visual when missing, see add_missing_collisions), MuJoCo emits a separate geom for + each, and we simply tag them rather than duplicating a single geom. - To do this, we need to add in class visual, and class collision to them, keep the rgba on the visual one, and - get rid of the other components (type, contype, conaffinity, group, density) + When ``mesh_info_dict`` is provided, a visual mesh geom is also given an ``rgba`` from + the mesh's URDF color, so plain (non-obj2mjcf) visual meshes keep their solid color. - We can tell that we need to modify it because it will have a contype attribute attached to it (not the best way - but I guess it works for now) + MuJoCo imports a URDF as a geom that still carries its raw import + attributes, e.g. + + and a as a plain collidable geom with no contype. So: + + - A geom WITH a contype attribute is a visual: it becomes + + keeping its rgba but dropping the raw import attributes (contype, conaffinity, + group, density). + - A geom WITHOUT a contype attribute is a collision: it becomes + + and its rgba (if any) is dropped since collisions are not rendered. + + Geoms that already carry a class attribute (e.g. those expanded by + update_obj_assets) are left untouched. """ # Find the element @@ -478,39 +801,116 @@ def update_non_obj_assets(dom, output_filepath): # get all of the geom elements in the worldbody element worldbody_geoms = worldbody_element.getElementsByTagName("geom") - # elements to remove - remove_attributes = ["contype", "conaffinity", "group", "density"] - for geom in worldbody_geoms: - if not geom.hasAttribute("contype"): - pass + # already classified upstream (e.g. obj-decomposed meshes); leave as is + if geom.hasAttribute("class"): + continue + + # Ensure a type: the saved model omits type="sphere" (MuJoCo's default). + if not geom.hasAttribute("type"): + geom.setAttribute("type", "sphere") + + if geom.hasAttribute("contype"): + # visual geom: keep rgba, set explicit render attributes (contype=0 -> no collide) + geom.setAttribute("class", "visual") + for attribute, value in VISUAL_GEOM_ATTRS.items(): + geom.setAttribute(attribute, value) + # apply the URDF color so plain visual meshes (no obj2mjcf material) render + if mesh_info_dict: + mesh_name = geom.getAttribute("mesh") + if mesh_name in mesh_info_dict and "color" in mesh_info_dict[mesh_name]: + rgba = mesh_info_dict[mesh_name]["color"] + geom.setAttribute("rgba", " ".join(str(v) for v in rgba)) else: - collision_geom = geom.cloneNode(False) + # collision geom: ensure a type, drop rgba (not rendered), set explicit collision + # attributes and tint it so the collision shape is visible in the viewer + geom.setAttribute("class", "collision") + if geom.hasAttribute("rgba"): + geom.removeAttribute("rgba") + for attribute, value in COLLISION_GEOM_ATTRS.items(): + geom.setAttribute(attribute, value) - # if there is no type associated, make the type sphere explicitly - if not collision_geom.hasAttribute("type"): - collision_geom.setAttribute("type", "sphere") + return dom - # set to collision class - collision_geom.setAttribute("class", "collision") - for attribute in remove_attributes: - if collision_geom.hasAttribute(attribute): - collision_geom.removeAttribute(attribute) - # most of the components are the same between collision and visual, so just copy it - visual_geom = collision_geom.cloneNode(False) - visual_geom.setAttribute("class", "visual") +def ensure_collision_material(dom): + """ + Ensures the ```` named COLLISION_MATERIAL_NAME exists in an ```` of + ``dom`` whenever a geom references it, since MuJoCo fails to load a model referencing an + undefined material. + + Collision geoms are tinted with this material so the collision geometry can be inspected + in the MuJoCo viewer. If the material is already defined (e.g. supplied via mujoco_inputs) + it is left untouched; otherwise a default orange material is created. Creates the + ```` element if the document has none. Meant to run after add_mujoco_inputs so a + user-defined material is seen before a default one is added. + """ + if not any(g.getAttribute("material") == COLLISION_MATERIAL_NAME for g in dom.getElementsByTagName("geom")): + return dom - # remove rgba from collision geom bc it isn't necessary - if geom.hasAttribute("rgba"): - collision_geom.removeAttribute("rgba") + assets = dom.getElementsByTagName("asset") + if assets: + asset = assets[0] + for material in asset.getElementsByTagName("material"): + if material.getAttribute("name") == COLLISION_MATERIAL_NAME: + return dom + else: + asset = dom.createElement("asset") + dom.documentElement.appendChild(asset) - # get the parent of the geom node, and remove the old element - parent = geom.parentNode - parent.removeChild(geom) - # add the new collision and visual specific elements - parent.appendChild(collision_geom) - parent.appendChild(visual_geom) + material = dom.createElement("material") + material.setAttribute("name", COLLISION_MATERIAL_NAME) + material.setAttribute("rgba", "1 0.5 0 1") + asset.appendChild(material) + return dom + + +def ensure_default_classes(dom): + """ + Ensures the default geom classes the converter tags geoms with (visual, collision, + decomposed_collision) exist in the final MJCF. Classes the user already defined via + their mujoco inputs are left untouched; only the missing ones are injected, into a + top-level block. This keeps the generated model self-contained without + requiring every robot config to declare the classes itself. + """ + required = { + # render-only geoms: never collide + "visual": { + "group": VISUAL_GEOM_ATTRS["group"], + "type": "mesh", + "contype": VISUAL_GEOM_ATTRS["contype"], + "conaffinity": VISUAL_GEOM_ATTRS["conaffinity"], + }, + # authored / whole-mesh / primitive collision geoms + "collision": {"group": COLLISION_GEOM_ATTRS["group"], "type": "mesh"}, + # obj2mjcf convex decomposition pieces: same physics as collision, but their own + # viewer group so the decomposition can be toggled on for inspection + "decomposed_collision": {"group": DECOMPOSED_COLLISION_GEOM_ATTRS["group"], "type": "mesh"}, + } + existing = {d.getAttribute("class") for d in dom.getElementsByTagName("default")} + missing = {name: attrs for name, attrs in required.items() if name not in existing} + if not missing: + return dom + + # Reuse the first top-level block if one exists, otherwise create one. + root = dom.documentElement + top_default = None + for child in root.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName == "default": + top_default = child + break + if top_default is None: + top_default = dom.createElement("default") + root.appendChild(top_default) + + for name, attrs in missing.items(): + class_default = dom.createElement("default") + class_default.setAttribute("class", name) + geom = dom.createElement("geom") + for attr, value in attrs.items(): + geom.setAttribute(attr, value) + class_default.appendChild(geom) + top_default.appendChild(class_default) return dom @@ -1166,7 +1566,11 @@ def add_modifiers(dom, modify_element_dict): def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): """ - Copies pre-generated mesh folders into the final MJCF assets structure. + Copies every cache-resolved entry's assets into the output tree, at the same home + mesh_home dictates everywhere else, so the output stays self-contained and usable as + a future --asset_dir. Decomposed entries copy their obj2mjcf pieces folder (and merge + their threshold into metadata.json); plain entries copy the mesh file plus its + obj2mjcf per-mesh folder (materials and mjcf) when present. """ thresholds_file = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/metadata.json" thresholds_data = {} @@ -1177,22 +1581,27 @@ def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): except json.JSONDecodeError: thresholds_data = {} - for mesh_name in mesh_info_dict: - mesh_item = mesh_info_dict[mesh_name] - filename = os.path.basename(mesh_item["filename"]) - filename_no_ext = os.path.splitext(filename)[0] - full_path = mesh_item["filename"] - mesh_dir = os.path.dirname(os.path.splitext(full_path)[0]) - - if mesh_item["is_pre_generated"]: - if filename_no_ext in decompose_dict: - threshold = decompose_dict[filename_no_ext] - dst_base = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" - thresholds_data[filename_no_ext] = float(threshold) - else: - dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" + for mesh_name, mesh_item in mesh_info_dict.items(): + if not mesh_item["is_pre_generated"]: + continue + + # after resolve_pregenerated, filename is the absolute path into the cache + cache_path = mesh_item["filename"] + home = mesh_home(mesh_name, mesh_item, decompose_dict) - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + if home == DECOMPOSED_PATH_NAME: + # cache layout: .../decomposed///.obj plus the pieces + src_dir = os.path.dirname(cache_path) + dst_dir = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}/" + shutil.copytree(src_dir, dst_dir, dirs_exist_ok=True) + thresholds_data[mesh_name] = float(decompose_threshold(mesh_name, mesh_item, decompose_dict)) + else: + dst_dir = f"{output_filepath}assets/{home}" + os.makedirs(dst_dir, exist_ok=True) + shutil.copy2(cache_path, os.path.join(dst_dir, os.path.basename(cache_path))) + src_folder = os.path.join(os.path.dirname(cache_path), mesh_name) + if os.path.isdir(src_folder): + shutil.copytree(src_folder, os.path.join(dst_dir, mesh_name), dirs_exist_ok=True) if thresholds_data: with open(thresholds_file, "w") as f: diff --git a/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch index 0830b442..6d144fce 100644 --- a/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch +++ b/mujoco_ros2_control/patches/mujoco_simulate_managed_user_scn.patch @@ -56,7 +56,7 @@ } else { mjopt_prev_ = m_->opt; opt_prev_ = opt; -@@ -2864,6 +2906,19 @@ void Simulate::RenderLoop() { +@@ -2864,6 +2906,20 @@ void Simulate::RenderLoop() { // update scene, doing a full sync if in fully managed mode if (!is_passive_) { Sync(); @@ -65,7 +65,8 @@ + int nusergeom = user_scn_geoms_.size(); + int ngeom = std::min(nusergeom, this->scn.maxgeom - this->scn.ngeom); + if (ngeom < nusergeom) { -+ mj_warning(this->d_, mjWARN_VGEOMFULL, this->scn.maxgeom); ++ mju_warning("Pre-allocated visual geom buffer is full. Increase maxgeom above %d.", ++ this->scn.maxgeom); + } + std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), + ngeom * sizeof(mjvGeom)); @@ -76,7 +77,7 @@ } else if (m_passive_ && d_passive_) { // the user has called Sync() in their code mjv_updateScene(m_passive_, d_passive_, -@@ -2877,6 +2932,9 @@ void Simulate::RenderLoop() { +@@ -2877,6 +2933,9 @@ void Simulate::RenderLoop() { } std::memcpy(this->scn.geoms + this->scn.ngeom, user_scn_geoms_.data(), ngeom * sizeof(mjvGeom)); diff --git a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py index b8da413b..3a941f0b 100755 --- a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py +++ b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py @@ -31,7 +31,6 @@ from xml.dom import minidom import mujoco_ros2_control as mrc -import pathlib def extract_rgba(visual): @@ -67,221 +66,233 @@ def extract_rgba(visual): return np.array([0.7, 0.7, 0.7, 1.0]) -def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, decompose_dict): - # keep track of files we have already processed so we don't do it again - converted_filenames = [] +def _export_stl_to_obj(full_filepath, output_path, mesh_item, mesh_name): + """Convert an STL source mesh to an OBJ at output_path, baking in the URDF color.""" + mesh = trimesh.load(full_filepath) + if "color" in mesh_item: + # trimesh expects an rgba; make a material so the color survives the export + rgba = mesh_item["color"] + mtl_name = "mtl_" + mesh_name + material = trimesh.visual.material.SimpleMaterial( + name=mtl_name, diffuse=rgba, glossiness=1000, specular=[0.2, 0.2, 0.2] + ) + mesh.visual = trimesh.visual.TextureVisuals(material=material) + mesh.export(output_path, include_color=True, mtl_name=mtl_name) + else: + mesh.export(output_path) + + +def _export_dae_to_obj(full_filepath, output_path, mesh_name): + """Convert a Collada (.dae) source mesh to an OBJ at output_path, preserving its + materials/textures (MuJoCo cannot load DAE directly, so conversion is required).""" + # keep track of the image files that we need to copy from the dae + image_files = mrc.get_images_from_dae(full_filepath) + copied_image_files = [] + + # set z axis to up in the dae file because that is how MuJoCo expects it + z_up_dae_txt = mrc.set_up_axis_to_z_up(full_filepath) + + # make a temporary file rather than overwriting the old one + temp_file = tempfile.NamedTemporaryFile(suffix=".dae", mode="w+", delete=False) + temp_filepath = temp_file.name + temp_folder = os.path.dirname(temp_filepath) + try: + temp_file.write(z_up_dae_txt) + temp_file.close() + + for image_file in image_files: + shutil.copy2(image_file, temp_folder) + copied_image_files.append(f"{temp_folder}/{os.path.basename(image_file)}") + + scene = trimesh.load(temp_filepath, force=trimesh.scene) + + # The default glossiness was way too high, so reassign default glossiness and + # specular but keep the rgba values. Skipped when there are image files so we + # don't accidentally overwrite textures. + if not image_files: + for geom in scene.geometry.values(): + visual = geom.visual + rgba = extract_rgba(visual) + new_mat = trimesh.visual.material.SimpleMaterial( + diffuse=rgba[:3], + alpha=rgba[3], + glossiness=1000, + specular=[0.2, 0.2, 0.2], + ) + visual.material = new_mat + + # give the material a unique name so that it can be properly referenced + mtl_modifier = f"{mesh_name}" + mtl_name = "mtl_" + mtl_modifier + mtl_filepath = os.path.dirname(output_path) + f"/{mtl_name}" + scene.export(output_path, include_color=True, mtl_name=mtl_name) + mrc.rename_material_textures(dir_path=os.path.dirname(output_path), modifier=mtl_modifier) + + # make the material names unique (material_0 -> material_{mesh_name}_0) so the + # per-mesh .obj/.mtl pair don't collide across meshes + if os.path.exists(mtl_filepath): + for filepath in [mtl_filepath, output_path]: + with open(filepath) as f: + data = f.read() + data = data.replace("material_", f"material_{mtl_modifier}_") + with open(filepath, "w") as f: + f.write(data) + finally: + if os.path.exists(temp_filepath): + os.remove(temp_filepath) + + for copied_image_file in copied_image_files: + if os.path.exists(copied_image_file): + os.remove(copied_image_file) + +def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, decompose_dict): + """ + Materializes each URDF mesh into the assets folder and rewrites xml_data to point at + it. Visual and collision meshes are routed differently: + + - Visual-only meshes are plain references for rendering: kept as-is (.stl/.obj copied, + .dae converted to .obj) under ``VISUAL_PATH_NAME``. They are never decomposed and + never run through obj2mjcf; their color is applied later as the geom's rgba. + - Collision meshes (and the collision side of a shared mesh) are converted to a whole + .obj under ``DECOMPOSED_PATH_NAME//`` so run_obj2mjcf can convex-decompose them + for the collision class. + - A mesh used by both a visual and a collision (the shared / fallback case) is + converted once: the single whole .obj backs the visual geom directly, and its + decomposed pieces back the collision geoms - no duplicate copy. + """ # clean assets directory and remake required paths if os.path.exists(f"{directory}assets/"): shutil.rmtree(f"{directory}assets/") + os.makedirs(f"{directory}assets/{mrc.VISUAL_PATH_NAME}", exist_ok=True) os.makedirs(f"{directory}assets/{mrc.COMPOSED_PATH_NAME}", exist_ok=True) os.makedirs(f"{directory}assets/{mrc.DECOMPOSED_PATH_NAME}", exist_ok=True) for mesh_name in mesh_info_dict: mesh_item = mesh_info_dict[mesh_name] filename = os.path.basename(mesh_item["filename"]) - filename_no_ext = os.path.splitext(filename)[0] - filename_ext = os.path.splitext(filename)[1] + filename_ext = os.path.splitext(filename)[1].lower() full_filepath = mesh_item["filename"] new_filepath = mesh_item["new_filepath"] - if full_filepath in converted_filenames: - pass - - print(f"processing {full_filepath} (mesh_name={mesh_name})") - - # if we want to decompose the mesh, put it in decomposed filepath, otherwise put it in full - # Note: we use the mesh_name as the the unique rather than filename_no_ext to avoid collisions - # for commonly named elements like on UR robots. - if mesh_name in decompose_dict or filename_no_ext in decompose_dict: - assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}/" - os.makedirs(f"{directory}assets/{assets_relative_filepath}", exist_ok=True) + used_as_collision = mesh_item.get("used_as_collision", False) + home = mrc.mesh_home(mesh_name, mesh_item, decompose_dict) + decompose_this = home == mrc.DECOMPOSED_PATH_NAME + + # Cache-resolved entries need no conversion: the URDF already references the + # cached file by absolute path, so the MuJoCo compile reads the cache directly + # and copy_pre_generated_meshes brings its assets into the output tree. + if mesh_item["is_pre_generated"]: + print(f"reusing pre-generated asset for {mesh_name} ({full_filepath})") + continue + + print( + f"processing {full_filepath} (mesh_name={mesh_name}, collision={used_as_collision}, " + f"decompose={decompose_this})" + ) + + if decompose_this: + # whole .obj in the decomposed dir so obj2mjcf --decompose can run on it; the + # visual side of a shared mesh reuses that same whole .obj + os.makedirs(f"{directory}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", exist_ok=True) + assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" + force_obj = True else: - assets_relative_filepath = f"{mrc.COMPOSED_PATH_NAME}/" - assets_relative_filepath += mesh_name + # plain reference, used directly: collision-used meshes go to the composed dir + # so obj2mjcf can process them (real materials for the shared visual, a plain + # whole-mesh geom for the collision); visual-only meshes are plain render + # references in the visual dir + assets_relative_filepath = f"{home}/{mesh_name}" + force_obj = False output_path = f"{directory}assets/{assets_relative_filepath}.obj" - # Import the .stl or .dae file - if filename_ext.lower() == ".stl": - if filename_no_ext in decompose_dict and not convert_stl_to_obj: - raise ValueError("The --convert_stl_to_obj argument must be specified to decompose .stl mesh") - if convert_stl_to_obj: - # Load mesh using trimesh - mesh = trimesh.load(full_filepath) - - # bring in file color from urdf - if "color" in mesh_item: - # trimesh expects 0-255 uint8 for colors - rgba = mesh_item["color"] - # make a material for the rgba values and export it - mtl_name = "mtl_" + mesh_name - material = trimesh.visual.material.SimpleMaterial( - name=mtl_name, diffuse=rgba, glossiness=1000, specular=[0.2, 0.2, 0.2] - ) # RGBA - mesh.visual = trimesh.visual.TextureVisuals(material=material) - - # Export to OBJ - mesh.export(output_path, include_color=True, mtl_name=mtl_name) + if filename_ext == ".stl": + if convert_stl_to_obj or force_obj: + # collision meshes always need an .obj (obj2mjcf input); visuals only when asked + _export_stl_to_obj(full_filepath, output_path, mesh_item, mesh_name) xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: + # plain visual reference: MuJoCo loads .stl directly, no conversion needed shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.stl") xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.stl") - pass - elif filename_ext.lower() == ".obj": - # If import .obj files from URDF - if not mesh_item["is_pre_generated"]: - shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") - # If the .obj depends on a mtl should copy also this - old_directory = os.path.dirname(mesh_item["filename"]) - if os.path.exists(old_directory + "/material.mtl"): - final_path = os.path.dirname(assets_relative_filepath) - shutil.copy2(old_directory + "/material.mtl", f"{directory}assets/{final_path}/material.mtl") - pass - # objs are ok as is - elif filename_ext.lower() == ".dae": - # keep track of the image files that we need to copy from the dae - image_files = mrc.get_images_from_dae(full_filepath) - # keep track of the files that were copied to tmp directory to delete - copied_image_files = [] - - # set z axis to up in the dae file because that is how MuJoCo expects it - z_up_dae_txt = mrc.set_up_axis_to_z_up(full_filepath) - - # make a temporary file rather than overwriting the old one - temp_file = tempfile.NamedTemporaryFile(suffix=".dae", mode="w+", delete=False) - temp_filepath = temp_file.name - temp_folder = os.path.dirname(temp_filepath) - try: - temp_file.write(z_up_dae_txt) - temp_file.close() - - # copy relevant images into the temporary directory - for image_file in image_files: - shutil.copy2(image_file, temp_folder) - copied_image_files.append(f"{temp_folder}/{os.path.basename(image_file)}") - - # Load scene using trimesh - scene = trimesh.load(temp_filepath, force=trimesh.scene) - - # The default glossiness was way too high, so here we iterate over each visual - # element, assign default glossiness and specular but keep the rgba values. - # We also ignore this process if there are any image files so that we don't - # accidentally overwrite those. Technically we could have cases where daes have - # both images, and materials but I think the logic gets exceedingly complex to - # handle that. - if not image_files: - for geom in scene.geometry.values(): - visual = geom.visual - rgba = extract_rgba(visual) - new_mat = trimesh.visual.material.SimpleMaterial( - diffuse=rgba[:3], - alpha=rgba[3], - glossiness=1000, - specular=[0.2, 0.2, 0.2], - ) - visual.material = new_mat - - # give the material a unique name so that it can be properly referenced - mtl_modifier = f"{mesh_name}" - mtl_name = "mtl_" + mtl_modifier - mtl_filepath = os.path.dirname(output_path) + f"/{mtl_name}" - scene.export(output_path, include_color=True, mtl_name=mtl_name) - # rename textures - mrc.rename_material_textures(dir_path=os.path.dirname(output_path), modifier=mtl_modifier) - - # we need to modify the material names to not all be material_X so they don't conflict - # all of the objs will have a line that looks like - # usemtl material_0 - # and all of the mtl files will have a line that looks like - # newmtl material_0 - # We will modify both of them to be like so - # usemtl material_{mesh_name}_0 - # newmtl material_{mesh_name}_0 - - if os.path.exists(mtl_filepath): - for filepath in [mtl_filepath, output_path]: - with open(filepath) as f: - data = f.read() - data = data.replace("material_", f"material_{mtl_modifier}_") - with open(filepath, "w") as f: - f.write(data) - finally: - if os.path.exists(temp_filepath): - os.remove(temp_filepath) - - # get rid of copied image files in the temporary file directory - for copied_image_file in copied_image_files: - if os.path.exists(copied_image_file): - os.remove(copied_image_file) - + elif filename_ext == ".obj": + shutil.copy2(full_filepath, output_path) + # if the .obj depends on a mtl, copy that too + old_directory = os.path.dirname(mesh_item["filename"]) + if os.path.exists(old_directory + "/material.mtl"): + final_path = os.path.dirname(assets_relative_filepath) + shutil.copy2(old_directory + "/material.mtl", f"{directory}assets/{final_path}/material.mtl") + xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") + elif filename_ext == ".dae": + _export_dae_to_obj(full_filepath, output_path, mesh_name) xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: - print(f"Can't convert {full_filepath} \n\tOnly stl and dae file extensions are supported at the moment") + print(f"Can't convert {full_filepath} \n\tOnly stl, obj and dae file extensions are supported") print(f"extension: {filename_ext}") - pass - - # keep track of what we have been working on - converted_filenames.append(full_filepath) return xml_data def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): - # remove the folders in the asset directory so that we are clean to run obj2mjcf - with os.scandir(f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}") as entries: - for entry in entries: - if entry.is_dir(): - shutil.rmtree(entry.path) - - # remove the folders in the asset directory so that we are clean to run obj2mjcf + """ + Convex-decomposes the collision meshes that were explicitly requested via a + decompose_mesh input (obj2mjcf --decompose, at the requested coacd threshold). Visual + meshes and collision meshes used directly are plain references and are NOT processed + here. + """ + # clean any pre-existing decomposed sub-folders so the run starts fresh top_level_path = f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}" for item in os.listdir(top_level_path): first_level_path = os.path.join(top_level_path, item) if os.path.isdir(first_level_path): - # Now check inside this first-level directory for sub_item in os.listdir(first_level_path): second_level_path = os.path.join(first_level_path, sub_item) if os.path.isdir(second_level_path): shutil.rmtree(second_level_path) - # run obj2mjcf to generate folders of processed objs - cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}", "--save-mjcf"] - subprocess.run(cmd) - composed_base_path = f"{output_filepath}assets/{mrc.COMPOSED_PATH_NAME}" - for file in os.listdir(composed_base_path): - if file.lower().endswith(".obj"): - mesh_name = pathlib.Path(file).stem - mesh_subfolder = os.path.join(composed_base_path, mesh_name) - - target_path = os.path.join(mesh_subfolder, file) - source_path = os.path.join(composed_base_path, file) - - try: - if not os.path.exists(target_path): + # run obj2mjcf (without decomposition) over the composed AND visual dirs so every + # plain mesh gets per-material submeshes and MJCF materials/textures; visual geoms + # then render with real materials instead of a flat rgba, in both modes. + for plain_dir in (mrc.COMPOSED_PATH_NAME, mrc.VISUAL_PATH_NAME): + base_path = f"{output_filepath}assets/{plain_dir}" + if not os.path.isdir(base_path): + continue + cmd = ["obj2mjcf", "--obj-dir", base_path, "--save-mjcf"] + subprocess.run(cmd) + # obj2mjcf moves nothing into its per-mesh folder for single-material meshes; make + # sure the source obj is available inside each folder so the mjcf's file refs resolve + for file in os.listdir(base_path): + if file.lower().endswith(".obj"): + mesh_name = os.path.splitext(file)[0] + target_path = os.path.join(base_path, mesh_name, file) + source_path = os.path.join(base_path, file) + if os.path.isdir(os.path.dirname(target_path)) and not os.path.exists(target_path): shutil.copy2(source_path, target_path) - except Exception as e: - print(f"Error while running the obj2mjcf command for {mesh_name}:{e}") - raise - thresholds_file = os.path.join(f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}", "metadata.json") + thresholds_file = os.path.join(top_level_path, "metadata.json") thresholds_data = {} - # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components - for mesh_name, threshold in decompose_dict.items(): - mesh_item = mesh_info_dict[mesh_name] - if not mesh_item["is_pre_generated"]: - cmd = [ - "obj2mjcf", - "--obj-dir", - f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", - "--save-mjcf", - "--decompose", - "--coacd-args.threshold", - threshold, - ] - subprocess.run(cmd) - - thresholds_data[mesh_name] = float(threshold) + # decompose only the collision meshes named in a decompose_mesh input, per the same + # routing decision (mesh_home) that placed them in the decomposed dir + for mesh_name, mesh_item in mesh_info_dict.items(): + if mrc.mesh_home(mesh_name, mesh_item, decompose_dict) != mrc.DECOMPOSED_PATH_NAME: + continue + if mesh_item["is_pre_generated"]: + continue + + threshold = str(mrc.decompose_threshold(mesh_name, mesh_item, decompose_dict)) + cmd = [ + "obj2mjcf", + "--obj-dir", + f"{output_filepath}assets/{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}", + "--save-mjcf", + "--decompose", + "--coacd-args.threshold", + threshold, + ] + subprocess.run(cmd) + + thresholds_data[mesh_name] = float(threshold) with open(thresholds_file, "w") as f: json.dump(thresholds_data, f, indent=4) @@ -321,11 +332,17 @@ def fix_mujoco_description( # Update and add the new fixed assets dom = mrc.update_obj_assets(dom, output_filepath, mesh_info_dict) - dom = mrc.update_non_obj_assets(dom, output_filepath) + dom = mrc.update_non_obj_assets(dom, output_filepath, mesh_info_dict) # Add the MuJoCo input elements dom = mrc.add_mujoco_inputs(dom, raw_inputs, scene_inputs) + # Inject any default geom classes the user's mujoco inputs did not define themselves + dom = mrc.ensure_default_classes(dom) + + # Same for the material tinting the collision geoms + dom = mrc.ensure_collision_material(dom) + # Add links as sites dom = mrc.add_links_as_sites(urdf, dom, request_add_free_joint) @@ -395,6 +412,13 @@ def main(args=None): default=True, help="Allows MuJoCo to merge static bodies. Use --no-fuse to prevent merging.", ) + parser.add_argument( + "--use_collision_tags", + action="store_true", + help="Use the URDF's authored geometry for the MuJoCo collision geoms. " + "Without this flag, authored collisions are ignored and all collision geometry is " + "derived from the tags instead (the legacy behavior).", + ) parser.add_argument( "-a", "--asset_dir", @@ -484,9 +508,13 @@ def main(args=None): # Add required MuJoCo tags to the starting URDF xml_data = mrc.add_mujoco_info(urdf, output_filepath, parsed_args.publish_topic, parsed_args.fuse) - # get rid of collision data, assuming the visual data is much better resolution. - # not sure if this is the best move... - xml_data = mrc.remove_tag(xml_data, "collision") + # Unless --use_collision_tags is given, drop the URDF's authored collision geometry + # first so every link falls back to a collision synthesized from its visuals (the + # legacy behavior, and the default). + if not parsed_args.use_collision_tags: + xml_data = mrc.remove_tag(xml_data, "collision") + + xml_data = mrc.add_missing_collisions(xml_data) xml_data = mrc.replace_package_names(xml_data) mesh_info_dict, xml_data = mrc.extract_mesh_info(xml_data, parsed_args.asset_dir, decompose_dict) diff --git a/mujoco_ros2_control/src/mujoco_simulation.cpp b/mujoco_ros2_control/src/mujoco_simulation.cpp index bdde472a..62ad1913 100644 --- a/mujoco_ros2_control/src/mujoco_simulation.cpp +++ b/mujoco_ros2_control/src/mujoco_simulation.cpp @@ -20,6 +20,7 @@ #include "mujoco_ros2_control/mujoco_simulation.hpp" #include "array_safety.h" #include "mujoco_ros2_control/sim_display_text.hpp" +#include "render_loop_exit.hpp" #include #include @@ -34,7 +35,6 @@ #include #include #include -#include #include #include #include @@ -550,6 +550,7 @@ MujocoSimulation::~MujocoSimulation() bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::string& model_path, const std::string& mujoco_model_topic, double sim_speed_factor, bool headless) { + explicit_shutdown_requested_.store(false); node_ = node; model_path_ = model_path; mujoco_model_topic_ = mujoco_model_topic; @@ -646,6 +647,12 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin // Blocks until terminated RCLCPP_INFO(get_logger(), "Starting the MuJoCo rendering thread..."); sim_->RenderLoop(); + + if (detail::handle_render_loop_exit(sim_->exitrequest, explicit_shutdown_requested_, + node_->get_node_base_interface()->get_context())) + { + RCLCPP_INFO(get_logger(), "MuJoCo rendering window closed; shut down its ROS context."); + } }); } @@ -748,6 +755,19 @@ bool MujocoSimulation::initialize(rclcpp::Node::SharedPtr node, const std::strin refresh_data_snapshot(); publish_control_state(); } + + // if there is an id set in the global settings, use that as the initial fixed camera + if (mj_model_->vis.global.cameraid >= 0 && mj_model_->vis.global.cameraid < mj_model_->ncam) + { + sim_->cam.fixedcamid = mj_model_->vis.global.cameraid; + sim_->cam.type = mjCAMERA_FIXED; + } + + // otherwise use default free camera + else + { + mjv_defaultFreeCamera(mj_model_, &sim_->cam); + } } if (!headless_) { @@ -848,6 +868,17 @@ MujocoSimulation::SimulationStepResult MujocoSimulation::request_simulation_step return SimulationStepResult::Completed; } +void MujocoSimulation::set_pre_step_callback(PreStepCallback callback) +{ + // Don't drop in a nullptr if requested, just pass an empty function. + PreStepCallback next = callback ? std::move(callback) : PreStepCallback([](mjData* /*data*/) {}); + + // Plugins may be registered after the physics thread is already running, so the loop + // could otherwise be reading pre_step_callback_ while we assign to it. + const std::unique_lock lock(*sim_mutex_); + pre_step_callback_ = std::move(next); +} + void MujocoSimulation::start_physics_thread() { // Disable the rangefinder flag at startup so that we don't get the yellow lines. @@ -895,6 +926,8 @@ void MujocoSimulation::start_physics_thread() void MujocoSimulation::shutdown() { + explicit_shutdown_requested_.store(true); + // If sim_ is created and running, clean shut it down if (sim_) { @@ -949,9 +982,12 @@ void MujocoSimulation::reset_world_state(bool fill_initial_state, std::fill(mj_data_->qfrc_applied, mj_data_->qfrc_applied + mj_model_->nv, 0.0); std::fill(mj_data_->xfrc_applied, mj_data_->xfrc_applied + 6 * mj_model_->nbody, 0.0); + // Restore equality-constraint activations to their MJCF defaults + std::copy(mj_model_->eq_active0, mj_model_->eq_active0 + mj_model_->neq, mj_data_->eq_active); + { - // Clear staged control inputs and plugin contributions so stale commands from before the - // reset are not re-applied on the next step. + // Clear staged control inputs so stale commands from before the reset are not re-applied + // on the next step. const std::lock_guard staging_lock(control_staging_mutex_); control_inputs_staged_ = false; std::fill(ctrl_staged_.begin(), ctrl_staged_.end(), 0.0); @@ -1550,6 +1586,16 @@ void MujocoSimulation::apply_staged_control_inputs() compose_cartesian_forces(); } +void MujocoSimulation::run_pre_step_callback() +{ + pre_step_callback_(mj_data_); + + // Pre-step callbacks may write xfrc_applied directly. Record the result so the next outer + // iteration only treats render-thread changes as viewer drag forces. + const std::lock_guard lock(control_staging_mutex_); + mju_copy(xfrc_last_written_.data(), mj_data_->xfrc_applied, 6 * static_cast(mj_model_->nbody)); +} + void MujocoSimulation::compose_cartesian_forces() { const std::lock_guard lock(control_staging_mutex_); @@ -1693,6 +1739,7 @@ void MujocoSimulation::physics_loop() sim_->speed_changed = false; apply_staged_control_inputs(); + run_pre_step_callback(); // run single step, let next iteration deal with timing mj_step(mj_model_, mj_data_); @@ -1744,6 +1791,7 @@ void MujocoSimulation::physics_loop() sim_->InjectNoise(-1); #endif apply_staged_control_inputs(); + run_pre_step_callback(); // call mj_step mj_step(mj_model_, mj_data_); @@ -1797,6 +1845,7 @@ void MujocoSimulation::physics_loop() if (pending_steps_.load() > 0) { apply_staged_control_inputs(); + run_pre_step_callback(); mj_step(mj_model_, mj_data_); publish_control_state(); publish_clock(); diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp index 012205d9..559157b7 100644 --- a/mujoco_ros2_control/src/mujoco_system_interface.cpp +++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp @@ -22,6 +22,7 @@ */ #include "mujoco_ros2_control/mujoco_system_interface.hpp" +#include "mujoco_ros2_control/utils.hpp" #include #include @@ -115,13 +116,22 @@ void add_items(std::vector& vector, const std::vector& items) ActuatorType getActuatorType(const mjModel* mj_model, int mujoco_actuator_id) { - // Returns the MuJoCo actuator type based on the actuator's bias settings. + // Returns the MuJoCo actuator type based on the compiled actuator settings. ActuatorType actuator_type = ActuatorType::UNKNOWN; - int biastype = mj_model->actuator_biastype[mujoco_actuator_id]; - const int NBias = 10; - const mjtNum* biasprm = mj_model->actuator_biasprm + mujoco_actuator_id * NBias; + const int dyntype = mj_model->actuator_dyntype[mujoco_actuator_id]; + const int gaintype = mj_model->actuator_gaintype[mujoco_actuator_id]; + const int biastype = mj_model->actuator_biastype[mujoco_actuator_id]; + const mjtNum* gainprm = mj_model->actuator_gainprm + mujoco_actuator_id * mjNGAIN; + const mjtNum* biasprm = mj_model->actuator_biasprm + mujoco_actuator_id * mjNBIAS; - if (biastype == mjBIAS_NONE) + // MuJoCo compiles an intvelocity shortcut into a fixed-gain, affine-bias actuator whose control is integrated into + // a position setpoint. Although its feedback terms resemble a position actuator, ctrl has velocity semantics. + if (dyntype == mjDYN_INTEGRATOR && gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && biasprm[0] == 0 && + biasprm[1] == -gainprm[0]) + { + actuator_type = ActuatorType::VELOCITY; + } + else if (biastype == mjBIAS_NONE) { actuator_type = ActuatorType::MOTOR; } @@ -267,6 +277,13 @@ MujocoSystemInterface::MujocoSystemInterface() = default; MujocoSystemInterface::~MujocoSystemInterface() { + // We don't know what plugins are doing with the mj_data pointer, so be + // sure to kill callback so that nothing can access it on destruction. + if (simulation_) + { + simulation_->set_pre_step_callback(nullptr); + } + // Stop plugins if (simulation_) { @@ -519,7 +536,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf get_hardware_parameter_or(get_hardware_info(), "publish_floating_base_tf", "true") == "true"; if (publish_floating_base_tf_) { - floating_base_tf_broadcaster_ = std::make_unique(get_node()); + floating_base_tf_broadcaster_ = std::make_unique(*get_node()); floating_base_tf_msg_.header.frame_id = odom_frame; floating_base_tf_msg_.child_frame_id = floating_base_msg_.child_frame_id; RCLCPP_INFO(get_logger(), "Publishing floating base TF: %s -> %s", odom_frame.c_str(), @@ -1067,6 +1084,10 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.linear_acceleration.data.x() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index]; data.linear_acceleration.data.y() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index + 1]; data.linear_acceleration.data.z() = control_state_.sensordata[data.linear_acceleration.mj_sensor_index + 2]; + + add_sensor_noise(data.orientation.data, data.orientation_noise_stddev, data.noise); + add_sensor_noise(data.angular_velocity.data, data.angular_velocity_noise_stddev, data.noise); + add_sensor_noise(data.linear_acceleration.data, data.linear_acceleration_noise_stddev, data.noise); } // FT Sensor data @@ -1079,6 +1100,9 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.torque.data.x() = -control_state_.sensordata[data.torque.mj_sensor_index]; data.torque.data.y() = -control_state_.sensordata[data.torque.mj_sensor_index + 1]; data.torque.data.z() = -control_state_.sensordata[data.torque.mj_sensor_index + 2]; + + add_sensor_noise(data.force.data, data.force_noise_stddev, data.noise); + add_sensor_noise(data.torque.data, data.torque_noise_stddev, data.noise); } // pose sensor data @@ -1092,6 +1116,9 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.orientation.data.x() = control_state_.sensordata[data.orientation.mj_sensor_index + 1]; data.orientation.data.y() = control_state_.sensordata[data.orientation.mj_sensor_index + 2]; data.orientation.data.z() = control_state_.sensordata[data.orientation.mj_sensor_index + 3]; + + add_sensor_noise(data.position.data, data.position_noise_stddev, data.noise); + add_sensor_noise(data.orientation.data, data.orientation_noise_stddev, data.noise); } // Magnetometer sensor data @@ -1100,6 +1127,8 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time& data.magnetic_field.data.x() = control_state_.sensordata[data.magnetic_field.mj_sensor_index]; data.magnetic_field.data.y() = control_state_.sensordata[data.magnetic_field.mj_sensor_index + 1]; data.magnetic_field.data.z() = control_state_.sensordata[data.magnetic_field.mj_sensor_index + 2]; + + add_sensor_noise(data.magnetic_field.data, data.magnetic_field_noise_stddev, data.noise); } // Publish Odometry @@ -1492,20 +1521,14 @@ bool MujocoSystemInterface::register_mujoco_actuators() return std::isfinite(gains.p_gain_) && std::isfinite(gains.i_gain_) && std::isfinite(gains.d_gain_); }; - if (actuator_data.actuator_type == ActuatorType::POSITION) - { - actuator_data.is_position_control_enabled = true; - } - else if (actuator_data.actuator_type == ActuatorType::VELOCITY) + if (actuator_data.actuator_type == ActuatorType::VELOCITY) { actuator_data.has_pos_pid = initialize_position_pids(); - actuator_data.is_velocity_control_enabled = true; } else if (actuator_data.actuator_type == ActuatorType::MOTOR || actuator_data.actuator_type == ActuatorType::CUSTOM) { actuator_data.has_pos_pid = initialize_position_pids(); actuator_data.has_vel_pid = initialize_velocity_pids(); - actuator_data.is_effort_control_enabled = true; } RCLCPP_DEBUG(get_logger(), "Successfully registered actuator '%s'", act_name); } @@ -1813,10 +1836,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } else if (!has_impedance_interfaces) { - RCLCPP_ERROR(get_logger(), - "Position command interface for the joint : %s is not supported with velocity or motor " - "actuator without defining the PIDs", - actuator_name.c_str()); + throw std::runtime_error("Position command interface for the joint : " + actuator_name + + " is not supported with motor or custom actuator without defining the PIDs"); } } } @@ -1854,11 +1875,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw } else if (!has_impedance_interfaces) { - RCLCPP_ERROR( - get_logger(), - "Velocity command interface for the joint : %s is not supported with motor or custom actuator " - "without defining the PIDs", - actuator_name.c_str()); + throw std::runtime_error("Velocity command interface for the joint : " + actuator_name + + " is not supported with motor or custom actuator without defining the PIDs"); } } } @@ -2182,6 +2200,15 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.force.mj_sensor_index = simulation_->model()->sensor_adr[force_sensor_id]; sensor_data.torque.mj_sensor_index = simulation_->model()->sensor_adr[torque_sensor_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.force_noise_stddev = simulation_->model()->sensor_noise[force_sensor_id]; + sensor_data.torque_noise_stddev = simulation_->model()->sensor_noise[torque_sensor_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.force.name, sensor_data.force_noise_stddev }, + { sensor_data.torque.name, sensor_data.torque_noise_stddev } }); + ft_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_IMU) @@ -2223,6 +2250,25 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.angular_velocity.mj_sensor_index = simulation_->model()->sensor_adr[gyro_id]; sensor_data.linear_acceleration.mj_sensor_index = simulation_->model()->sensor_adr[accel_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.orientation_noise_stddev = simulation_->model()->sensor_noise[quat_id]; + sensor_data.angular_velocity_noise_stddev = simulation_->model()->sensor_noise[gyro_id]; + sensor_data.linear_acceleration_noise_stddev = simulation_->model()->sensor_noise[accel_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + + log_sensor_components_noise( + get_logger(), sensor_data.noise.distribution, + { { sensor_data.orientation.name, sensor_data.orientation_noise_stddev }, + { sensor_data.angular_velocity.name, sensor_data.angular_velocity_noise_stddev }, + { sensor_data.linear_acceleration.name, sensor_data.linear_acceleration_noise_stddev } }); + + // Surface the configured noise as diagonal covariance for consumers that expect an uncertainty + // estimate (off-diagonal terms stay 0, i.e. axes are assumed independent). Stays all-zero, as before + // noise support existed, when the corresponding *_noise_stddev is left at its 0 default. + set_diagonal_covariance(sensor_data.orientation_covariance, sensor_data.orientation_noise_stddev); + set_diagonal_covariance(sensor_data.angular_velocity_covariance, sensor_data.angular_velocity_noise_stddev); + set_diagonal_covariance(sensor_data.linear_acceleration_covariance, sensor_data.linear_acceleration_noise_stddev); + imu_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_POSE) @@ -2254,6 +2300,15 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.position.mj_sensor_index = simulation_->model()->sensor_adr[pos_id]; sensor_data.orientation.mj_sensor_index = simulation_->model()->sensor_adr[quat_id]; + // Noise magnitude sourced from the MJCF sensors' own `noise` attribute; see register_sensors() doc. + sensor_data.position_noise_stddev = simulation_->model()->sensor_noise[pos_id]; + sensor_data.orientation_noise_stddev = simulation_->model()->sensor_noise[quat_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.position.name, sensor_data.position_noise_stddev }, + { sensor_data.orientation.name, sensor_data.orientation_noise_stddev } }); + pose_sensor_data_.push_back(sensor_data); } else if (mujoco_type == MUJOCO_TYPE_MAGNETOMETER) @@ -2274,6 +2329,13 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI sensor_data.magnetic_field.mj_sensor_index = simulation_->model()->sensor_adr[magnetometer_id]; + // Noise magnitude sourced from the MJCF sensor's own `noise` attribute; see register_sensors() doc. + sensor_data.magnetic_field_noise_stddev = simulation_->model()->sensor_noise[magnetometer_id]; + sensor_data.noise.distribution = get_noise_distribution(sensor); + + log_sensor_components_noise(get_logger(), sensor_data.noise.distribution, + { { sensor_data.magnetic_field.name, sensor_data.magnetic_field_noise_stddev } }); + magnetometer_sensor_data_.push_back(sensor_data); } else @@ -2451,9 +2513,12 @@ void MujocoSystemInterface::reset_simulation_state(bool /*fill_initial_state*/) joint.effort_interface.command_ = 0.0; } + // Notify plugins: they own runtime state that the world reset has invalidated. + // eq_active has already been restored to MJCF defaults. for (auto& plugin : plugin_instances_) { plugin->reset(); + plugin->on_reset(simulation_->data()); } } @@ -2566,6 +2631,14 @@ void MujocoSystemInterface::load_mujoco_plugins() { RCLCPP_ERROR(get_logger(), "Failed to create plugin loader: %s", ex.what()); } + + // Connect plugin pre-step callbacks directly to the physics simulation. + simulation_->set_pre_step_callback([this](mjData* data) { + for (auto& plugin : plugin_instances_) + { + plugin->pre_step(data); + } + }); } /// @@ -2609,20 +2682,6 @@ bool MujocoSystemInterface::auto_register_plugin_if_needed(const std::string& pl return true; } -inline std::optional -get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, const std::string& name) -{ - for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++) - { - const auto& sensor = hardware_info.sensors.at(sensor_index); - if (hardware_info.sensors.at(sensor_index).name == name) - { - return sensor; - } - } - return std::nullopt; -} - void MujocoSystemInterface::load_legacy_cameras(const std::vector& plugins_ns) { // For now we are manually checking for cameras in the sim, then loading the CameraPlugin diff --git a/mujoco_ros2_control/src/render_loop_exit.hpp b/mujoco_ros2_control/src/render_loop_exit.hpp new file mode 100644 index 00000000..3adecbd7 --- /dev/null +++ b/mujoco_ros2_control/src/render_loop_exit.hpp @@ -0,0 +1,45 @@ +/** + * Copyright (c) 2026, Dylan Gallagher. + * + * 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. + */ + +#ifndef MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ +#define MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ + +#include +#include + +#include + +namespace mujoco_ros2_control::detail +{ + +inline bool handle_render_loop_exit(std::atomic& exit_request, + const std::atomic& explicit_shutdown_requested, + const std::shared_ptr& context) +{ + // MuJoCo sets exitrequest when the window closes, so it cannot distinguish a user close from + // MujocoSimulation::shutdown(). The owner marks the latter before waking RenderLoop(). + exit_request.store(1); + if (explicit_shutdown_requested.load()) + { + return false; + } + + return context && context->shutdown("MuJoCo rendering window closed"); +} + +} // namespace mujoco_ros2_control::detail + +#endif // MUJOCO_ROS2_CONTROL__RENDER_LOOP_EXIT_HPP_ diff --git a/mujoco_ros2_control/tests/CMakeLists.txt b/mujoco_ros2_control/tests/CMakeLists.txt index 2914d7de..7760f4d7 100644 --- a/mujoco_ros2_control/tests/CMakeLists.txt +++ b/mujoco_ros2_control/tests/CMakeLists.txt @@ -6,6 +6,7 @@ target_link_libraries(test_mujoco_simulation mujoco_ros2_control rclcpp::rclcpp ) +target_include_directories(test_mujoco_simulation PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/../src) ament_add_gtest(test_mujoco_system_interface test_mujoco_system_interface.cpp) target_link_libraries(test_mujoco_system_interface diff --git a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp index 752c1a6d..6edf959c 100644 --- a/mujoco_ros2_control/tests/test_mujoco_simulation.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_simulation.cpp @@ -20,7 +20,6 @@ #include #include - #include #include #include @@ -35,11 +34,14 @@ #include #include +#include "render_loop_exit.hpp" + namespace { // Basic model for executing unit tests: a hinge joint with an actuator, plus two free-floating -// bodies ("free_object", "free_object_2") for exercising the free-joint state service. +// bodies ("free_object", "free_object_2") for exercising the free-joint state service, plus an +// inactive weld equality ("test_weld") for exercising the eq_active restore on world reset. // nu=1, nq=15 (1 hinge + 7 + 7 free joint), nv=13 (1 hinge + 6 + 6 free joint), nbody=4 // (world + pendulum + free_object + free_object_2) constexpr const char* kTestModel = R"( @@ -65,6 +67,10 @@ constexpr const char* kTestModel = R"( + + + + @@ -83,6 +89,35 @@ void write_test_model() constexpr double TEST_TOLERANCE = 1e-9; } // namespace +TEST(RenderLoopExitHandler, RequestsSimulationExitAndShutsOwningContext) +{ + auto context = std::make_shared(); + context->init(0, nullptr); + std::atomic exit_request{ 1 }; + std::atomic explicit_shutdown_requested{ false }; + + ASSERT_TRUE(rclcpp::ok(context)); + EXPECT_TRUE(mujoco_ros2_control::detail::handle_render_loop_exit(exit_request, explicit_shutdown_requested, context)); + + EXPECT_EQ(exit_request.load(), 1); + EXPECT_FALSE(rclcpp::ok(context)); +} + +TEST(RenderLoopExitHandler, PreservesContextAfterExplicitShutdown) +{ + auto context = std::make_shared(); + context->init(0, nullptr); + std::atomic exit_request{ 1 }; + std::atomic explicit_shutdown_requested{ true }; + + ASSERT_TRUE(rclcpp::ok(context)); + EXPECT_FALSE(mujoco_ros2_control::detail::handle_render_loop_exit(exit_request, explicit_shutdown_requested, context)); + + EXPECT_EQ(exit_request.load(), 1); + EXPECT_TRUE(rclcpp::ok(context)); + context->shutdown("test cleanup"); +} + class MujocoSimulationTest : public ::testing::Test { protected: @@ -310,39 +345,34 @@ TEST_F(MujocoSimulationTest, RejectsInvalidActuatorParameterUpdateIds) ASSERT_NE(control, nullptr); EXPECT_THROW(sim_->apply_control_data(control, { { -1, {} } }), std::out_of_range); - EXPECT_THROW(sim_->apply_control_data(control, { { sim_->model()->nu, {} } }), std::out_of_range); + EXPECT_THROW(sim_->apply_control_data(control, { { static_cast(sim_->model()->nu), {} } }), std::out_of_range); EXPECT_THROW(sim_->set_actuator_parameters({ -1, {} }), std::out_of_range); - EXPECT_THROW(sim_->set_actuator_parameters({ sim_->model()->nu, {} }), std::out_of_range); + EXPECT_THROW(sim_->set_actuator_parameters({ static_cast(sim_->model()->nu), {} }), std::out_of_range); mj_deleteData(control); } -TEST_F(MujocoSimulationTest, XfrcAppliedTests) +TEST_F(MujocoSimulationTest, PreStepCallbackAppliesXfrcApplied) { ASSERT_TRUE(initialize_sim()); - // Simulate the read/write cycle in the ros2_control loop, making sure data - // is updated where expected - mjData* control = nullptr; - sim_->copy_physics_data(control); - ASSERT_NE(control, nullptr); - - // Zero xfrc_applied (like for plugins), and apply a force + // xfrc_applied is not staged through apply_control_data, instead we register a pre-step callback that + // writes directly into the live mjData immediately before every mj_step. const size_t body_id = 1; - mju_zero(control->xfrc_applied, 6 * sim_->model()->nbody); - control->xfrc_applied[body_id * 6 + 0] = 1.0; - control->xfrc_applied[body_id * 6 + 1] = 2.0; - control->xfrc_applied[body_id * 6 + 2] = 3.0; - - sim_->apply_control_data(control); + sim_->set_pre_step_callback([body_id](mjData* data) { + data->xfrc_applied[body_id * 6 + 0] = 1.0; + data->xfrc_applied[body_id * 6 + 1] = 2.0; + data->xfrc_applied[body_id * 6 + 2] = 3.0; + }); - // xfrc_applied should NOT be in mj_data_ directly, it goes through the triple - // plugin buffer and gets composed in the physics loop + // The callback has not run yet as it is only invoked from the physics loop. EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 0], 0.0); - EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 1], 0.0); - EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 2], 0.0); - mj_deleteData(control); + sim_->start_physics_thread(); + EXPECT_TRUE(wait_until([this, body_id]() { return sim_->data()->xfrc_applied[body_id * 6 + 0] == 1.0; })) + << "pre_step callback's xfrc_applied write was not applied by the physics loop"; + EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 1], 2.0); + EXPECT_DOUBLE_EQ(sim_->data()->xfrc_applied[body_id * 6 + 2], 3.0); } TEST_F(MujocoSimulationTest, PauseStepUnpause) @@ -512,6 +542,26 @@ TEST_F(MujocoSimulationTest, ResetWorldTest) << "Time should advance after unpausing post-reset"; } +TEST_F(MujocoSimulationTest, ResetWorldRestoresEqualityConstraintActivation) +{ + ASSERT_TRUE(initialize_sim()); + + const int eq_id = mj_name2id(sim_->model(), mjOBJ_EQUALITY, "test_weld"); + ASSERT_NE(eq_id, -1); + ASSERT_EQ(sim_->model()->eq_active0[eq_id], 0) << "test_weld must be authored inactive"; + ASSERT_EQ(sim_->data()->eq_active[eq_id], 0); + + sim_->capture_initial_state(); + + // Simulate a plugin activating the constraint at runtime (e.g. a vacuum gripper engaging + // a weld); a world reset must restore the activation to the MJCF-authored default. + sim_->data()->eq_active[eq_id] = 1; + sim_->reset_world_state(true); + + EXPECT_EQ(sim_->data()->eq_active[eq_id], 0) + << "eq_active should be restored to its MJCF default (eq_active0) on world reset"; +} + TEST_F(MujocoSimulationTest, ResetWorldJointStateOverrides) { ASSERT_TRUE(initialize_sim()); diff --git a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp index c45e4cf1..1aedd4f5 100644 --- a/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp +++ b/mujoco_ros2_control/tests/test_mujoco_system_interface.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include #include @@ -32,6 +34,7 @@ #include #include +#include #define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3) @@ -55,12 +58,32 @@ constexpr const char* kTestModel = R"( + )"; +constexpr const char* kIntVelocityTestModel = R"( + + +)"; + // Write to disk for testing const std::string kTestModelPath = "/tmp/test_mujoco_system_interface_model.xml"; +const std::string kIntVelocityTestModelPath = "/tmp/test_mujoco_system_interface_intvelocity_model.xml"; void write_test_model() { std::ofstream file(kTestModelPath); @@ -68,6 +91,57 @@ void write_test_model() file.close(); } +void write_intvelocity_test_model() +{ + std::ofstream file(kIntVelocityTestModelPath); + file << kIntVelocityTestModel; + file.close(); +} + +// Builds a ros2_control magnetometer ComponentInfo mapped to `mujoco_sensor_name`, with the standard +// magnetic_field.{x,y,z} state interfaces. `extra_params` are merged in on top of the required ones +// (e.g. "noise_distribution"). +hardware_interface::ComponentInfo +make_magnetometer_sensor(const std::string& name, const std::string& mujoco_sensor_name, + const std::unordered_map& extra_params = {}) +{ + hardware_interface::ComponentInfo sensor_info; + sensor_info.name = name; + sensor_info.parameters[mujoco_ros2_control::MUJOCO_TYPE_PARAM] = mujoco_ros2_control::MUJOCO_TYPE_MAGNETOMETER; + sensor_info.parameters[mujoco_ros2_control::MUJOCO_SENSOR_NAME_PARAM] = mujoco_sensor_name; + for (const auto& [key, value] : extra_params) + { + sensor_info.parameters[key] = value; + } + for (const auto* interface_name : { "magnetic_field.x", "magnetic_field.y", "magnetic_field.z" }) + { + hardware_interface::InterfaceInfo interface_info; + interface_info.name = interface_name; + sensor_info.state_interfaces.push_back(interface_info); + } + return sensor_info; +} + +struct NoiseStats +{ + double mean; + double stddev; +}; + +// Computes the sample mean and (population) standard deviation of `samples`. +NoiseStats compute_stats(const std::vector& samples) +{ + const double count = static_cast(samples.size()); + const double mean = std::accumulate(samples.begin(), samples.end(), 0.0) / count; + double variance = 0.0; + for (const double sample : samples) + { + variance += (sample - mean) * (sample - mean); + } + variance /= count; + return { mean, std::sqrt(variance) }; +} + } // namespace class MujocoSystemInterfaceTest : public ::testing::Test @@ -110,6 +184,10 @@ class MujocoSystemInterfaceTest : public ::testing::Test { std::filesystem::remove(kTestModelPath); } + if (std::filesystem::exists(kIntVelocityTestModelPath)) + { + std::filesystem::remove(kIntVelocityTestModelPath); + } } // Create the hardware info to initialize the interface with, in headless mode. @@ -151,9 +229,69 @@ class MujocoSystemInterfaceTest : public ::testing::Test return condition(); } + // Reads a state interface's current value, abstracting the ROS-distro-specific accessor. + static double get_value(const hardware_interface::StateInterface& state_interface) + { +#if ROS_DISTRO_HUMBLE + return state_interface.get_value(); +#else + return state_interface.get_optional().value(); +#endif + } + std::shared_ptr interface_; }; +TEST_F(MujocoSystemInterfaceTest, IntVelocityActuatorSupportsVelocityCommandInterface) +{ + write_intvelocity_test_model(); + auto hardware_info = create_hardware_info(); + hardware_info.hardware_parameters["mujoco_model"] = kIntVelocityTestModelPath; + + hardware_interface::ComponentInfo joint_info; + joint_info.name = "wheel_joint"; + + hardware_interface::InterfaceInfo command_interface; + command_interface.name = hardware_interface::HW_IF_VELOCITY; + joint_info.command_interfaces.push_back(command_interface); + + for (const auto* interface_name : + { hardware_interface::HW_IF_POSITION, hardware_interface::HW_IF_VELOCITY, hardware_interface::HW_IF_EFFORT }) + { + hardware_interface::InterfaceInfo state_interface; + state_interface.name = interface_name; + joint_info.state_interfaces.push_back(state_interface); + } + hardware_info.joints.push_back(joint_info); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + interface_->get_model(model); + ASSERT_NE(model, nullptr); + const int actuator_id = mj_name2id(model, mjOBJ_ACTUATOR, "wheel_joint"); + ASSERT_NE(actuator_id, -1); + EXPECT_EQ(model->actuator_dyntype[actuator_id], mjDYN_INTEGRATOR); + + auto command_interfaces = interface_->export_command_interfaces(); + ASSERT_EQ(command_interfaces.size(), 1u); + EXPECT_EQ(command_interfaces.front().get_name(), "wheel_joint/velocity"); + + constexpr double velocity_command = 2.0; + command_interfaces.front().set_value(velocity_command); + ASSERT_EQ(interface_->write(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)), + hardware_interface::return_type::OK); + + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_data(data); + return data != nullptr && std::abs(data->ctrl[actuator_id] - velocity_command) < 1e-9; + })) << "Velocity command was not written to the intvelocity actuator ctrl input"; + + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MujocoSystemInterfaceTest, PoseSensorStateInterfacesRead) { // Register a pose sensor mapped to the framepos/framequat sensors in the MJCF model. @@ -271,6 +409,239 @@ TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorStateInterfacesRead) EXPECT_NEAR(magnetic_field_z, data->sensordata[magnetometer_data_index + 2], tol); } +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseIsApplied) +{ + // Register the underlying MJCF magnetometer twice: once via the plain sensor (no `noise` attribute) as a + // noise-free baseline, and once via `magnetometer_sensor_noisy`, which sets noise="0.05" directly in the + // MJCF (see kTestModel). Both are read from the exact same control_state_ snapshot within a single read() + // call, so every reading's (noisy - raw) delta is a pure noise sample with no risk of racing a stepping sim. + constexpr double kNoiseStdDev = 0.05; // must match magnetometer_sensor_noisy's noise="0.05" in kTestModel + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_raw", "magnetometer_sensor")); + hardware_info.sensors.push_back( + make_magnetometer_sensor("magnetometer_sensor_noisy_iface", "magnetometer_sensor_noisy")); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; + std::vector noise_samples; + noise_samples.reserve(kSamples * 3); + for (int i = 0; i < kSamples; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + const double raw_value = get_value(state_interfaces[axis]); + const double noisy_value = get_value(state_interfaces[3 + axis]); + noise_samples.push_back(noisy_value - raw_value); + } + } + + const auto stats = compute_stats(noise_samples); + + // Generous bounds: tight enough to catch "noise not applied" (stddev ~ 0) or a badly mis-scaled + // implementation, loose enough to avoid flaking on the randomness itself. + EXPECT_NEAR(stats.mean, 0.0, 0.01); + EXPECT_NEAR(stats.stddev, kNoiseStdDev, kNoiseStdDev * 0.2); +} + +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseUsesIndependentRngPerSensor) +{ + // Two ros2_control sensors both mapped to the same noisy MJCF magnetometer (noise="0.05" in kTestModel, + // see magnetometer_sensor_noisy). Each should draw from its own independently-seeded RNG, so their noise + // sequences diverge even though the underlying clean signal (and configured noise stddev) is identical. + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_a", "magnetometer_sensor_noisy")); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_b", "magnetometer_sensor_noisy")); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + bool saw_divergence = false; + for (int i = 0; i < 20 && !saw_divergence; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + if (std::abs(get_value(state_interfaces[axis]) - get_value(state_interfaces[3 + axis])) > 1e-9) + { + saw_divergence = true; + break; + } + } + } + + EXPECT_TRUE(saw_divergence) << "Two sensors mapped to the same noisy MJCF sensor should not share an RNG " + "(their noise sequences should diverge)"; +} + +TEST_F(MujocoSystemInterfaceTest, MagnetometerSensorNoiseSupportsUniformDistribution) +{ + // magnetometer_sensor_noisy sets noise="0.05" in the MJCF; pairing it with a `noise_distribution=uniform` + // ros2_control sensor here confirms the distribution choice is honored end-to-end (bounded samples with + // stddev matching the MJCF value), not just in the add_sensor_noise() unit tests below. + constexpr double kNoiseStdDev = 0.05; // must match magnetometer_sensor_noisy's noise="0.05" in kTestModel + const double bound = kNoiseStdDev * std::sqrt(3.0); + auto hardware_info = create_hardware_info(); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_raw", "magnetometer_sensor")); + hardware_info.sensors.push_back(make_magnetometer_sensor("magnetometer_sensor_uniform", "magnetometer_sensor_noisy", + { { "noise_distribution", "uniform" } })); + + ASSERT_EQ(initialize_interface(hardware_info), hardware_interface::CallbackReturn::SUCCESS); + + mjModel* model = nullptr; + mjData* data = nullptr; + ASSERT_TRUE(wait_until([&]() { + interface_->get_model(model); + interface_->get_data(data); + return model != nullptr && data != nullptr && data->time > 0.0; + })) << "Simulation did not start stepping"; + + const auto state_interfaces = interface_->export_state_interfaces(); + ASSERT_EQ(state_interfaces.size(), 6u); + + constexpr int kSamples = 2000; + std::vector noise_samples; + noise_samples.reserve(kSamples * 3); + for (int i = 0; i < kSamples; ++i) + { + interface_->read(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.002)); + for (int axis = 0; axis < 3; ++axis) + { + const double raw_value = get_value(state_interfaces[axis]); + const double uniform_value = get_value(state_interfaces[3 + axis]); + const double sample = uniform_value - raw_value; + ASSERT_LE(sample, bound + 1e-9) << "Uniform noise must never exceed its scaled bound"; + ASSERT_GE(sample, -bound - 1e-9) << "Uniform noise must never exceed its scaled bound"; + noise_samples.push_back(sample); + } + } + + const auto stats = compute_stats(noise_samples); + + // The uniform range is scaled by sqrt(3) so its true stddev should still match the configured MJCF value. + EXPECT_NEAR(stats.mean, 0.0, 0.01); + EXPECT_NEAR(stats.stddev, kNoiseStdDev, kNoiseStdDev * 0.2); +} + +TEST(GetNoiseDistributionTest, DefaultsToGaussianWhenParamAbsent) +{ + hardware_interface::ComponentInfo sensor; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kGaussian); +} + +TEST(GetNoiseDistributionTest, ReturnsUniformWhenParamSetToUniform) +{ + hardware_interface::ComponentInfo sensor; + sensor.parameters["noise_distribution"] = "uniform"; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kUniform); +} + +TEST(GetNoiseDistributionTest, ReturnsGaussianWhenParamExplicitlySetToGaussian) +{ + hardware_interface::ComponentInfo sensor; + sensor.parameters["noise_distribution"] = "gaussian"; + EXPECT_EQ(mujoco_ros2_control::get_noise_distribution(sensor), mujoco_ros2_control::NoiseDistribution::kGaussian); +} + +TEST(AddSensorNoiseTest, GaussianZeroStdDevIsNoOp) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_DOUBLE_EQ(value.x(), 1.0); + EXPECT_DOUBLE_EQ(value.y(), 2.0); + EXPECT_DOUBLE_EQ(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, GaussianNonZeroStdDevChangesValue) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.1, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_NE(value.x(), 1.0); + EXPECT_NE(value.y(), 2.0); + EXPECT_NE(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, GaussianSameSeedProducesSameNoise) +{ + std::mt19937 rng_a(7); + std::mt19937 rng_b(7); + Eigen::Vector3d value_a(0.0, 0.0, 0.0); + Eigen::Vector3d value_b(0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value_a, 1.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng_a); + mujoco_ros2_control::add_sensor_noise(value_b, 1.0, mujoco_ros2_control::NoiseDistribution::kGaussian, rng_b); + EXPECT_TRUE(value_a.isApprox(value_b)); +} + +TEST(AddSensorNoiseTest, GaussianQuaternionRemainsNormalized) +{ + std::mt19937 rng(3); + Eigen::Quaterniond value(1.0, 0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, 0.2, mujoco_ros2_control::NoiseDistribution::kGaussian, rng); + EXPECT_NEAR(value.norm(), 1.0, 1e-9); +} + +TEST(AddSensorNoiseTest, UniformZeroStdDevIsNoOp) +{ + std::mt19937 rng(1); + Eigen::Vector3d value(1.0, 2.0, 3.0); + mujoco_ros2_control::add_sensor_noise(value, 0.0, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + EXPECT_DOUBLE_EQ(value.x(), 1.0); + EXPECT_DOUBLE_EQ(value.y(), 2.0); + EXPECT_DOUBLE_EQ(value.z(), 3.0); +} + +TEST(AddSensorNoiseTest, UniformQuaternionRemainsNormalized) +{ + std::mt19937 rng(3); + Eigen::Quaterniond value(1.0, 0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, 0.2, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + EXPECT_NEAR(value.norm(), 1.0, 1e-9); +} + +TEST(AddSensorNoiseTest, UniformStaysWithinScaledBounds) +{ + // Uniform noise is drawn from [-stddev*sqrt(3), stddev*sqrt(3)] so its true stddev matches `stddev`, + // unlike Gaussian noise, which has unbounded tails. This also distinguishes it from a mis-wired Gaussian. + constexpr double kStdDev = 0.1; + const double bound = kStdDev * std::sqrt(3.0); + std::mt19937 rng(11); + for (int i = 0; i < 5000; ++i) + { + Eigen::Vector3d value(0.0, 0.0, 0.0); + mujoco_ros2_control::add_sensor_noise(value, kStdDev, mujoco_ros2_control::NoiseDistribution::kUniform, rng); + ASSERT_LE(value.x(), bound); + ASSERT_GE(value.x(), -bound); + ASSERT_LE(value.y(), bound); + ASSERT_GE(value.y(), -bound); + ASSERT_LE(value.z(), bound); + ASSERT_GE(value.z(), -bound); + } +} + int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); diff --git a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py index e2d10239..44c63231 100644 --- a/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/tests/test_urdf_to_mujoco_utils.py @@ -37,6 +37,7 @@ get_processed_mujoco_inputs, DECOMPOSED_PATH_NAME, COMPOSED_PATH_NAME, + VISUAL_PATH_NAME, write_mujoco_scene, add_urdf_free_joint, get_xml_from_file, @@ -50,6 +51,12 @@ parse_scene_xml, extract_mesh_info, copy_pre_generated_meshes, + add_missing_collisions, + ensure_default_classes, + ensure_collision_material, + decompose_threshold, + mesh_home, + resolve_pregenerated, ) @@ -404,7 +411,220 @@ def test_update_obj_assets_no_matching_dirs(self): result_dom = update_obj_assets(dom, tmpdir + "/", mesh_info_dict) self.assertIsNotNone(result_dom) - def test_update_non_obj_assets_basic(self): + def _write_decomposed_mjcf(self, tmpdir, name): + # Mimic obj2mjcf --decompose --save-mjcf output: meshes carry NO name attribute + # (MuJoCo derives the name from the file stem), a visual geom referencing the whole + # mesh, and convex collision-piece geoms. Layout: decomposed///.xml + mesh_dir = os.path.join(tmpdir, "assets", DECOMPOSED_PATH_NAME, name, name) + os.makedirs(mesh_dir) + with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: + f.write( + "" + '' + '' + '' + "" + "" + '' + '' + '' + "".format(n=name) + ) + + def test_update_obj_assets_expands_collision_only(self): + # A collision-only mesh is decomposed: its (no-contype) geom expands into the + # convex pieces; obj2mjcf's nameless piece meshes are all kept (not collapsed to a + # single empty name) and its visual sub-geom is not turned into a collision geom. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "col_mesh") + xml_string = ( + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "col_mesh": {"scale": "1 1 1", "used_as_visual": False, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # convex pieces land in their own class (and viewer group) for inspection + self.assertRegex( + result_xml, r']*mesh="col_mesh_collision_0"[^>]*class="decomposed_collision"[^>]*>' + ) + # both convex piece mesh assets are present (not skipped as duplicate empty names) + self.assertIn("col_mesh_collision_0.obj", result_xml) + self.assertIn("col_mesh_collision_1.obj", result_xml) + # obj2mjcf's visual sub-geom (whole mesh) is not cloned as a collidable geom + self.assertNotRegex(result_xml, r']*mesh="col_mesh"[^>]*class="(decomposed_)?collision"[^>]*>') + + # decomposed collision pieces collide (explicit contype/conaffinity 1) but sit in + # their own viewer group (4), separate from whole-mesh/primitive collisions (3) ... + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*group="4"[^>]*>') + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*contype="1"[^>]*>') + self.assertRegex(result_xml, r']*mesh="col_mesh_collision_0"[^>]*conaffinity="1"[^>]*>') + # ... and are NOT tinted bright_orange - they keep obj2mjcf's own decomposition + # materials/rgba so the individual hulls stay distinguishable. + self.assertNotIn('material="bright_orange"', result_xml) + + def _write_plain_mjcf(self, tmpdir, name): + # Mimic obj2mjcf --save-mjcf output WITHOUT --decompose: per-material visual + # sub-geoms carrying materials, plus a whole-mesh collision sub-geom with an rgba + # that must not override the tint. Layout: full//.xml + mesh_dir = os.path.join(tmpdir, "assets", COMPOSED_PATH_NAME, name) + os.makedirs(mesh_dir) + with open(os.path.join(mesh_dir, f"{name}.xml"), "w") as f: + f.write( + "" + '' + '' + "" + "" + '' + '' + "".format(n=name) + ) + + def test_update_obj_assets_plain_mesh_tints_collision_keeps_visual_material(self): + # A shared mesh that is NOT decomposed: the whole-mesh collision sub-geom is a + # regular collision (group 3, tinted bright_orange, rgba dropped so the tint shows) + # while the visual sub-geom keeps its obj2mjcf material and gets the explicit + # render-only attributes. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_plain_mjcf(tmpdir, "plain") + xml_string = ( + '' + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "plain": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + geoms = { + g.getAttribute("class"): g + for g in minidom.parseString(result_xml).getElementsByTagName("geom") + if g.getAttribute("mesh") == "plain_0" + } + self.assertEqual(set(geoms), {"visual", "collision"}) + + visual = geoms["visual"] + self.assertEqual(visual.getAttribute("material"), "material_plain_0") + self.assertEqual(visual.getAttribute("contype"), "0") + self.assertEqual(visual.getAttribute("conaffinity"), "0") + self.assertEqual(visual.getAttribute("group"), "2") + self.assertEqual(visual.getAttribute("density"), "0") + + collision = geoms["collision"] + self.assertEqual(collision.getAttribute("material"), "bright_orange") + self.assertFalse(collision.hasAttribute("rgba")) + self.assertEqual(collision.getAttribute("contype"), "1") + self.assertEqual(collision.getAttribute("conaffinity"), "1") + self.assertEqual(collision.getAttribute("group"), "3") + + def test_update_obj_assets_visual_only_untouched(self): + # A visual-only mesh (not in the decomposed dir) is a plain reference: its geom + # and asset are left as-is (no expansion). + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, "assets", DECOMPOSED_PATH_NAME)) + xml_string = ( + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "vis_mesh": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": False}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # the single mesh asset and its geom are untouched (still one reference) + self.assertRegex(result_xml, r' asset and + # the visual geom are kept; only the collision (no-contype) geom is expanded. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "shared") + xml_string = ( + '' + '' + '' + '' + '' + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "shared": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # exactly one mesh resolves to the name "shared" - our kept whole mesh; obj2mjcf's + # nameless re-emit (file decomposed/shared/shared/shared.obj) is deduped away + self.assertEqual(result_xml.count('name="shared"'), 1) + self.assertNotIn("shared/shared/shared.obj", result_xml) + # the visual geom is replaced by obj2mjcf's render geom, still referencing the + # whole mesh (this is what carries the obj2mjcf material instead of a flat rgba), + # with explicit render-only attributes instead of the raw import ones + self.assertRegex(result_xml, r']*mesh="shared"[^>]*class="visual"[^>]*>') + self.assertNotRegex(result_xml, r']*mesh="shared"[^>]*rgba="1 0 0 1"[^>]*>') + self.assertNotRegex(result_xml, r']*mesh="shared"[^>]*group="1"[^>]*>') + self.assertRegex(result_xml, r']*mesh="shared"[^>]*contype="0"[^>]*>') + # the collision geom was expanded into decomposed pieces in their own class + self.assertRegex(result_xml, r']*mesh="shared_collision_0"[^>]*class="decomposed_collision"[^>]*>') + + def test_update_obj_assets_expands_scaled_sibling(self): + # A mirrored link makes MuJoCo emit a scaled sibling mesh (same file, scale="1 -1 1", + # auto-named e.g. "leg1") for the mirrored side. Its collision must also be decomposed, + # using scaled copies of the convex pieces - not left as a single whole mesh. + with tempfile.TemporaryDirectory() as tmpdir: + self._write_decomposed_mjcf(tmpdir, "leg") + xml_string = ( + '' + '' + '' + "" + '' + '' + '' + "" + '' + '' + '' + "" + "" + ) + dom = minidom.parseString(xml_string) + mesh_info_dict = { + "leg": {"scale": "1 1 1", "used_as_visual": True, "used_as_collision": True}, + } + result_xml = update_obj_assets(dom, tmpdir + "/", mesh_info_dict).toxml() + # the mirrored sibling's whole-mesh collision geom is gone ... + self.assertNotRegex(result_xml, r']*mesh="leg1"[^>]*class="collision"[^>]*>') + # ... replaced by scaled decomposed pieces referencing per-sibling piece meshes + self.assertRegex( + result_xml, r']*mesh="leg_collision_0__leg1"[^>]*class="decomposed_collision"[^>]*>' + ) + # a scaled piece mesh asset exists, carrying the sibling's mirror scale + self.assertRegex(result_xml, r']*scale="1 -1 1"[^>]*>') + # the whole sibling mesh asset is dropped (no visual references it) + self.assertNotRegex(result_xml, r']') + # the unscaled (left) side still expands as before + self.assertRegex(result_xml, r']*mesh="leg_collision_0"[^>]*class="decomposed_collision"[^>]*>') + + def test_update_non_obj_assets_visual_geom(self): + # A geom with contype is a MuJoCo-imported ; it is classified as visual and + # given explicit render attributes (contype=0 so it never collides) plus group 2. xml_string = """ @@ -416,45 +636,144 @@ def test_update_non_obj_assets_basic(self): dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - assert 'class="collision"' in result_xml assert 'class="visual"' in result_xml - assert "contype" not in result_xml - - def test_update_non_obj_assets_no_contype(self): + assert 'class="collision"' not in result_xml + # explicit visual attributes: visuals never collide and live in group 2 + assert 'contype="0"' in result_xml + assert 'conaffinity="0"' in result_xml + assert 'group="2"' in result_xml + assert 'density="0"' in result_xml + # visual keeps its rgba for rendering + assert 'rgba="0.2 0.2 0.2 1"' in result_xml + + def test_update_non_obj_assets_collision_geom(self): + # A geom without contype is a MuJoCo-imported ; it is classified as + # collision, its rgba is dropped, and it gets explicit collision attributes + # (group 3, contype/conaffinity 1) tinted bright_orange for inspection. xml_string = """ - + """ dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - assert " + # element when there is none) so the MJCF still loads. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertRegex(result_xml, r']*name="bright_orange"') + + def test_ensure_collision_material_noop_when_unreferenced(self): + # No geom uses the tint (e.g. a mesh-less or collision-less model): nothing is added. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertNotIn("bright_orange", result_xml) + self.assertNotIn(" + + + - - + + + +""" + dom = minidom.parseString(xml_string) + result_xml = ensure_collision_material(dom).toxml() + self.assertEqual(result_xml.count('name="bright_orange"'), 1) + # the user's definition is preserved, not overwritten + assert 'rgba="0.9 0.4 0.1 1"' in result_xml + + def test_update_non_obj_assets_visual_and_collision(self): + # Real case: a link with both a visual mesh and a collision mesh. The visual + # geom becomes class="visual" and the collision geom becomes class="collision"; + # no duplication occurs. + xml_string = """ + + + + + """ dom = minidom.parseString(xml_string) result_dom = update_non_obj_assets(dom, "/tmp/output/") result_xml = result_dom.toxml() - self.assertEqual(result_xml.count('class="collision"'), 2) - self.assertEqual(result_xml.count('class="visual"'), 2) - self.assertEqual(result_xml.count("contype"), 0) - self.assertEqual(result_xml.count("conaffinity"), 0) - self.assertEqual(result_xml.count('group="1"'), 0) - self.assertEqual(result_xml.count('density="0"'), 0) - self.assertRegex(result_xml, r']*mesh="mesh1"[^>]*class="visual"[^>]*>') - self.assertRegex(result_xml, r']*mesh="mesh2"[^>]*class="collision"[^>]*>') + self.assertEqual(result_xml.count('class="visual"'), 1) + self.assertEqual(result_xml.count('class="collision"'), 1) + # both geoms now carry an explicit contype (0 for visual, 1 for collision) + self.assertRegex(result_xml, r']*mesh="visual_mesh"[^>]*class="visual"[^>]*>') + self.assertRegex(result_xml, r']*mesh="collision_mesh"[^>]*class="collision"[^>]*>') + + def test_update_non_obj_assets_skips_classified(self): + # Geoms already classified (e.g. by update_obj_assets) are left untouched. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + result_dom = update_non_obj_assets(dom, "/tmp/output/") + result_xml = result_dom.toxml() + self.assertEqual(result_xml.count('class="visual"'), 1) + self.assertEqual(result_xml.count('class="collision"'), 0) + + def test_update_non_obj_assets_visual_rgba_from_mesh_info(self): + # When a mesh_info_dict is supplied, a plain visual mesh geom gets its rgba from + # the URDF color so it renders correctly without obj2mjcf materials. + xml_string = """ + + + + + + +""" + dom = minidom.parseString(xml_string) + mesh_info_dict = {"vis_mesh": {"color": (1.0, 0.0, 0.0, 1.0)}} + result_dom = update_non_obj_assets(dom, "/tmp/output/", mesh_info_dict) + result_xml = result_dom.toxml() + self.assertRegex(result_xml, r']*mesh="vis_mesh"[^>]*class="visual"[^>]*>') + assert 'rgba="1.0 0.0 0.0 1.0"' in result_xml def test_add_mujoco_inputs_both_none(self): xml_string = '' @@ -1783,6 +2102,86 @@ def test_parse_scene_xml_invalid_root(self): finally: os.unlink(invalid_path) + def test_add_missing_collisions_adds_from_visual(self): + # A link with a visual but no collision should get a synthesized collision + # that copies the visual's geometry and origin. + urdf = """ + + + + + + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 1) + col = collisions[0] + # geometry mesh is copied + col_meshes = col.getElementsByTagName("mesh") + self.assertEqual(len(col_meshes), 1) + self.assertEqual(col_meshes[0].getAttribute("filename"), "package://test_package/meshes/model.stl") + # origin is copied + col_origins = col.getElementsByTagName("origin") + self.assertEqual(len(col_origins), 1) + self.assertEqual(col_origins[0].getAttribute("xyz"), "0.5 0 0") + # collisions must not carry a material + self.assertEqual(len(col.getElementsByTagName("material")), 0) + + def test_add_missing_collisions_keeps_existing(self): + # A link that already defines a collision must be left untouched. + urdf = """ + + + + + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 1) + # the original sphere collision is preserved (not replaced by the visual box) + self.assertEqual(len(collisions[0].getElementsByTagName("sphere")), 1) + self.assertEqual(len(collisions[0].getElementsByTagName("box")), 0) + + def test_add_missing_collisions_no_visual(self): + # A link without any visual must not get a collision. + urdf = """ + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + self.assertEqual(len(link.getElementsByTagName("collision")), 0) + + def test_add_missing_collisions_multiple_visuals(self): + # Every visual on a collision-less link produces a matching collision. + urdf = """ + + + + + +""" + result = add_missing_collisions(urdf) + dom = minidom.parseString(result) + link = dom.getElementsByTagName("link")[0] + collisions = link.getElementsByTagName("collision") + self.assertEqual(len(collisions), 2) + def test_extract_mesh_info_basic(self): urdf = """ @@ -1801,6 +2200,9 @@ def test_extract_mesh_info_basic(self): self.assertEqual(result["model"]["scale"], "1.0 1.0 1.0") self.assertEqual(result["model"]["color"], (1.0, 1.0, 1.0, 1.0)) self.assertFalse(result["model"]["is_pre_generated"]) + # a visual-only mesh is flagged for visual use, not collision + self.assertTrue(result["model"]["used_as_visual"]) + self.assertFalse(result["model"]["used_as_collision"]) assert "package://test_package/meshes/model.dae" in updated_xml def test_extract_mesh_info_with_material_color(self): @@ -1859,15 +2261,77 @@ def test_extract_mesh_info_no_mesh_geometry(self): self.assertEqual(len(result), 0) self.assertEqual(updated_xml, urdf) - def test_extract_mesh_info_with_decompose_dict(self): + def test_extract_mesh_info_distinct_collision_mesh(self): + # A link with a visual mesh and a different collision mesh yields two entries. + # The collision entry has no material, so its color defaults to white. urdf = """ - + + + + + + + + +""" + result, updated_xml = extract_mesh_info(urdf, None, {}) + self.assertEqual(len(result), 2) + assert "visual_model" in result + assert "collision_model" in result + self.assertEqual(result["visual_model"]["color"], (1.0, 0.0, 0.0, 1.0)) + self.assertEqual(result["collision_model"]["color"], (1.0, 1.0, 1.0, 1.0)) + # distinct meshes are flagged for their respective single use + self.assertTrue(result["visual_model"]["used_as_visual"]) + self.assertFalse(result["visual_model"]["used_as_collision"]) + self.assertTrue(result["collision_model"]["used_as_collision"]) + self.assertFalse(result["collision_model"]["used_as_visual"]) + assert "package://test_package/meshes/collision_model.stl" in updated_xml + + def test_extract_mesh_info_shared_collision_mesh(self): + # When the collision references the same mesh file as the visual (the + # fallback case), they share a single entry - one converted source, reused - + # flagged for both uses, with the visual's material color winning. + urdf = """ + + + + + + + + + + + + + + +""" + result, _ = extract_mesh_info(urdf, None, {}) + self.assertEqual(len(result), 1) + assert "model" in result + self.assertEqual(result["model"]["color"], (1.0, 0.0, 0.0, 1.0)) + # one entry, reused for both visual and collision + self.assertTrue(result["model"]["used_as_visual"]) + self.assertTrue(result["model"]["used_as_collision"]) + + def test_extract_mesh_info_with_decompose_dict(self): + # decomposition applies to collision meshes, so the decompose pregen lookup is + # exercised through a mesh. + urdf = """ + + + + + + + """ with tempfile.TemporaryDirectory() as tmpdir: @@ -1900,11 +2364,11 @@ def test_extract_mesh_different_threshold(self): urdf = """ - + - + """ with tempfile.TemporaryDirectory() as tmpdir: @@ -1929,7 +2393,8 @@ def test_extract_mesh_different_threshold(self): self.assertEqual(urdf, updated_xml) - def test_extract_mesh_with_compose_dict(self): + def test_extract_mesh_visual_pre_generated(self): + # A visual mesh resolves to a plain pre-generated asset in the visual dir. urdf = """ @@ -1941,9 +2406,9 @@ def test_extract_mesh_with_compose_dict(self): """ with tempfile.TemporaryDirectory() as tmpdir: - composed_dir = os.path.join(tmpdir, COMPOSED_PATH_NAME, "complex_mesh") - os.makedirs(composed_dir) - mesh_file = os.path.join(composed_dir, "complex_mesh.obj") + visual_dir = os.path.join(tmpdir, VISUAL_PATH_NAME) + os.makedirs(visual_dir) + mesh_file = os.path.join(visual_dir, "complex_mesh.obj") with open(mesh_file, "w") as f: f.write("# OBJ file") @@ -1951,9 +2416,10 @@ def test_extract_mesh_with_compose_dict(self): self.assertEqual(len(result), 1) assert "complex_mesh" in result self.assertTrue(result["complex_mesh"]["is_pre_generated"]) + self.assertTrue(result["complex_mesh"]["used_as_visual"]) self.assertEqual(result["complex_mesh"]["scale"], "1.0 1.0 1.0") self.assertEqual(result["complex_mesh"]["color"], (1.0, 1.0, 1.0, 1.0)) - self.assertEqual(f"{composed_dir}/complex_mesh.obj", result["complex_mesh"]["filename"]) + self.assertEqual(f"{visual_dir}/complex_mesh.obj", result["complex_mesh"]["filename"]) # The updated_xml differs from urdf only for the new path assert f"{mesh_file}" in updated_xml @@ -1962,6 +2428,66 @@ def test_extract_mesh_with_compose_dict(self): ) self.assertEqual(urdf, reverted_xml) + def test_extract_mesh_collision_direct_pre_generated(self): + # A collision mesh NOT named in a decompose_mesh input is used directly: its + # pre-generated asset is the plain composed mesh, not a decomposed one. + urdf = """ + + + + + + + + +""" + with tempfile.TemporaryDirectory() as tmpdir: + composed_dir = os.path.join(tmpdir, COMPOSED_PATH_NAME) + os.makedirs(composed_dir) + mesh_file = os.path.join(composed_dir, "chunk.obj") + with open(mesh_file, "w") as f: + f.write("# OBJ file") + + # empty decompose_dict -> the collision mesh is used directly (not decomposed) + result, updated_xml = extract_mesh_info(urdf, tmpdir, {}) + self.assertEqual(len(result), 1) + assert "chunk" in result + self.assertTrue(result["chunk"]["is_pre_generated"]) + self.assertTrue(result["chunk"]["used_as_collision"]) + self.assertFalse(result["chunk"]["used_as_visual"]) + self.assertEqual(f"{composed_dir}/chunk.obj", result["chunk"]["filename"]) + + def test_copy_pre_generated_meshes_collision_direct(self): + # A collision mesh used directly (not in decompose_dict) is copied as a single + # file into the composed dir, not decomposed. + with tempfile.TemporaryDirectory() as tmpdir: + source_dir = os.path.join(tmpdir, "source", COMPOSED_PATH_NAME) + output_dir = os.path.join(tmpdir, "output") + "/" + os.makedirs(source_dir) + mesh_file = os.path.join(source_dir, "chunk.obj") + with open(mesh_file, "w") as f: + f.write("# DIRECT COLLISION OBJ") + + mesh_info_dict = { + "chunk": { + "is_pre_generated": True, + "filename": mesh_file, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, + } + } + + copy_pre_generated_meshes(output_dir, mesh_info_dict, {}) + + expected_dst = os.path.join(output_dir, "assets", COMPOSED_PATH_NAME, "chunk.obj") + self.assertTrue(os.path.exists(expected_dst)) + with open(expected_dst) as f: + self.assertEqual(f.read(), "# DIRECT COLLISION OBJ") + # not decomposed -> no metadata.json written + self.assertFalse(os.path.exists(os.path.join(output_dir, "assets", DECOMPOSED_PATH_NAME, "metadata.json"))) + def test_copy_pre_generated_meshes_decomposed(self): with tempfile.TemporaryDirectory() as tmpdir: source_dir = os.path.join(tmpdir, "source") @@ -1980,6 +2506,8 @@ def test_copy_pre_generated_meshes_decomposed(self): "filename": os.path.join(source_dir, "mesh_name", "mesh_name", "mesh_name.obj"), "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, } } decompose_dict = {"mesh_name": "0.05"} @@ -2025,6 +2553,8 @@ def test_copy_pre_generated_meshes_decomposed_not_empty_metadata(self): "filename": os.path.join(source_dir, "mesh_name", "mesh_name", "mesh_name.obj"), "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": False, + "used_as_collision": True, } } decompose_dict = {"mesh_name": "0.10"} @@ -2045,34 +2575,35 @@ def test_copy_pre_generated_meshes_decomposed_not_empty_metadata(self): self.assertEqual(updated_data["previous_mesh_name"], 0.05) self.assertEqual(updated_data["mesh_name"], 0.10) - def test_copy_pre_generated_meshes_composed(self): + def test_copy_pre_generated_meshes_visual(self): + # A plain visual mesh is copied as a single file into the visual dir. with tempfile.TemporaryDirectory() as tmpdir: - source_dir = os.path.join(tmpdir, "source") + source_dir = os.path.join(tmpdir, "source", VISUAL_PATH_NAME) output_dir = os.path.join(tmpdir, "output") + "/" os.makedirs(source_dir) - mesh_source_dir = os.path.join(source_dir, "mesh_name") - os.makedirs(mesh_source_dir) - mesh_file = os.path.join(mesh_source_dir, "mesh_name.obj") + mesh_file = os.path.join(source_dir, "mesh_name.obj") with open(mesh_file, "w") as f: - f.write("# COMPOSED OBJ content") + f.write("# VISUAL OBJ content") mesh_info_dict = { "mesh_name": { "is_pre_generated": True, - "filename": os.path.join(source_dir, "mesh_name", "mesh_name.obj"), + "filename": mesh_file, "scale": "1.0 1.0 1.0", "color": (1.0, 1.0, 1.0, 1.0), + "used_as_visual": True, + "used_as_collision": False, } } decompose_dict = {} copy_pre_generated_meshes(output_dir, mesh_info_dict, decompose_dict) - expected_dst = os.path.join(output_dir, "assets", COMPOSED_PATH_NAME, "mesh_name", "mesh_name.obj") + expected_dst = os.path.join(output_dir, "assets", VISUAL_PATH_NAME, "mesh_name.obj") self.assertTrue(os.path.exists(expected_dst)) with open(expected_dst) as f: - self.assertEqual(f.read(), "# COMPOSED OBJ content") + self.assertEqual(f.read(), "# VISUAL OBJ content") def test_copy_pre_generated_meshes_not_pre_generated(self): with tempfile.TemporaryDirectory() as tmpdir: @@ -2093,6 +2624,234 @@ def test_copy_pre_generated_meshes_not_pre_generated(self): assets_dir = os.path.join(output_dir, "assets") self.assertFalse(os.path.exists(assets_dir)) + def test_extract_mesh_info_identical_collision_collapses(self): + # A collision mesh that is a byte-identical copy of the visual mesh collapses + # into the visual entry and the URDF is rewritten to share one file. + with tempfile.TemporaryDirectory() as tmpdir: + visual_file = os.path.join(tmpdir, "visual", "part.stl") + collision_file = os.path.join(tmpdir, "collision", "part.stl") + for path in (visual_file, collision_file): + os.makedirs(os.path.dirname(path)) + with open(path, "wb") as f: + f.write(b"same bytes") + urdf = f""" + + + + + + + + + + + + + +""" + result, updated_xml = extract_mesh_info(urdf, None, {}) + self.assertEqual(list(result.keys()), ["part"]) + self.assertTrue(result["part"]["used_as_visual"]) + self.assertTrue(result["part"]["used_as_collision"]) + self.assertEqual(result["part"]["filename"], visual_file) + self.assertNotIn(collision_file, updated_xml) + self.assertEqual(updated_xml.count(visual_file), 2) + + def test_decompose_threshold_collision_named(self): + item = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(decompose_threshold("part", item, {"part": "0.05"}), "0.05") + + def test_decompose_threshold_visual_only(self): + # only collision-used meshes decompose, even when named + item = {"used_as_visual": True, "used_as_collision": False, "source_stem": "part"} + self.assertIsNone(decompose_threshold("part", item, {"part": "0.05"})) + + def test_decompose_threshold_not_named(self): + item = {"used_as_collision": True, "source_stem": "part"} + self.assertIsNone(decompose_threshold("part", item, {"other": "0.05"})) + + def test_decompose_threshold_by_source_stem(self): + # a disambiguated entry (part__1) still matches a decompose input naming its source + item = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(decompose_threshold("part__1", item, {"part": "0.02"}), "0.02") + + def test_mesh_home(self): + decompose_dict = {"part": "0.05"} + decomposed = {"used_as_collision": True, "source_stem": "part"} + self.assertEqual(mesh_home("part", decomposed, decompose_dict), DECOMPOSED_PATH_NAME) + plain_collision = {"used_as_collision": True, "source_stem": "chunk"} + self.assertEqual(mesh_home("chunk", plain_collision, decompose_dict), COMPOSED_PATH_NAME) + visual = {"used_as_visual": True, "used_as_collision": False, "source_stem": "part"} + self.assertEqual(mesh_home("part", visual, decompose_dict), VISUAL_PATH_NAME) + + def test_resolve_pregenerated_no_asset_dir(self): + uri = "package://pkg/meshes/part.stl" + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": True, + "used_as_collision": False, + } + } + raw_xml = f'' + self.assertEqual(resolve_pregenerated(mesh_info_dict, raw_xml, None, {}), raw_xml) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + def test_resolve_pregenerated_visual_hit(self): + uri = "package://pkg/meshes/part.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, VISUAL_PATH_NAME)) + cached = os.path.join(tmpdir, VISUAL_PATH_NAME, "part.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": True, + "used_as_collision": False, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertTrue(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(mesh_info_dict["part"]["filename"], cached) + self.assertEqual(mesh_info_dict["part"]["new_filepath"], cached) + self.assertEqual(raw_xml, f'') + + def test_resolve_pregenerated_probes_home_only(self): + # a collision-used mesh lives in the composed dir: a cached file in the visual dir + # is not a hit for it + uri = "package://pkg/meshes/part.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, VISUAL_PATH_NAME)) + with open(os.path.join(tmpdir, VISUAL_PATH_NAME, "part.obj"), "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part": { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(raw_xml, f'') + + def test_resolve_pregenerated_decomposed_requires_metadata(self): + uri = "package://pkg/meshes/part.stl" + entry = { + "is_pre_generated": False, + "filename": uri, + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": "package:/pkg/meshes/part.stl", + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + with tempfile.TemporaryDirectory() as tmpdir: + decomposed_dir = os.path.join(tmpdir, DECOMPOSED_PATH_NAME, "part", "part") + os.makedirs(decomposed_dir) + cached = os.path.join(decomposed_dir, "part.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + + # no metadata record: the cached folder is unverifiable, so no hit + mesh_info_dict = {"part": dict(entry)} + resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.05"}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + # matching threshold: hit + with open(os.path.join(tmpdir, DECOMPOSED_PATH_NAME, "metadata.json"), "w") as f: + json.dump({"part": 0.05}, f) + mesh_info_dict = {"part": dict(entry)} + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.05"}) + self.assertTrue(mesh_info_dict["part"]["is_pre_generated"]) + self.assertEqual(mesh_info_dict["part"]["filename"], cached) + self.assertEqual(raw_xml, f'') + + # different threshold: no hit + mesh_info_dict = {"part": dict(entry)} + resolve_pregenerated(mesh_info_dict, f'', tmpdir, {"part": "0.02"}) + self.assertFalse(mesh_info_dict["part"]["is_pre_generated"]) + + def test_resolve_pregenerated_renamed_entry(self): + # a stem-disambiguated entry carries new_filepath in the URDF, and that is what + # gets replaced by the cached path + renamed_ref = "package:/pkg/other/part__1.stl" + with tempfile.TemporaryDirectory() as tmpdir: + os.makedirs(os.path.join(tmpdir, COMPOSED_PATH_NAME)) + cached = os.path.join(tmpdir, COMPOSED_PATH_NAME, "part__1.obj") + with open(cached, "w") as f: + f.write("# OBJ file") + mesh_info_dict = { + "part__1": { + "is_pre_generated": False, + "filename": "package://pkg/other/part.stl", + "scale": "1.0 1.0 1.0", + "color": (1.0, 1.0, 1.0, 1.0), + "new_filepath": renamed_ref, + "source_stem": "part", + "used_as_visual": False, + "used_as_collision": True, + } + } + raw_xml = resolve_pregenerated(mesh_info_dict, f'', tmpdir, {}) + self.assertTrue(mesh_info_dict["part__1"]["is_pre_generated"]) + self.assertEqual(raw_xml, f'') + + def test_ensure_default_classes_adds_all(self): + dom = minidom.parseString("") + ensure_default_classes(dom) + top_defaults = [n for n in dom.documentElement.childNodes if n.nodeName == "default"] + self.assertEqual(len(top_defaults), 1) + classes = {d.getAttribute("class"): d for d in top_defaults[0].getElementsByTagName("default")} + self.assertEqual(set(classes), {"visual", "collision", "decomposed_collision"}) + visual_geom = classes["visual"].getElementsByTagName("geom")[0] + self.assertEqual(visual_geom.getAttribute("contype"), "0") + self.assertEqual(visual_geom.getAttribute("conaffinity"), "0") + self.assertEqual(visual_geom.getAttribute("group"), "2") + self.assertEqual(classes["collision"].getElementsByTagName("geom")[0].getAttribute("group"), "3") + self.assertEqual(classes["decomposed_collision"].getElementsByTagName("geom")[0].getAttribute("group"), "4") + + def test_ensure_default_classes_keeps_existing(self): + # a user-defined class is left untouched and only the missing ones are added into + # the existing top-level block + dom = minidom.parseString( + '' + ) + ensure_default_classes(dom) + top_defaults = [n for n in dom.documentElement.childNodes if n.nodeName == "default"] + self.assertEqual(len(top_defaults), 1) + classes = [d.getAttribute("class") for d in top_defaults[0].getElementsByTagName("default")] + self.assertEqual(sorted(classes), ["collision", "decomposed_collision", "visual"]) + self.assertIn('', dom.toxml()) + + def test_ensure_default_classes_noop_when_present(self): + xml = ( + "" + '' + '' + '' + "" + ) + dom = minidom.parseString(xml) + self.assertEqual(ensure_default_classes(dom).toxml(), minidom.parseString(xml).toxml()) + if __name__ == "__main__": unittest.main() diff --git a/mujoco_ros2_control_demos/CHANGELOG.rst b/mujoco_ros2_control_demos/CHANGELOG.rst index 433ed7b2..620bffe1 100644 --- a/mujoco_ros2_control_demos/CHANGELOG.rst +++ b/mujoco_ros2_control_demos/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package mujoco_ros2_control_demos ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2026-09-07) +------------------ + +0.1.1 (2026-08-25) +------------------ +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + +0.1.0 (2026-08-18) +------------------ +* Add base twist plugin for commanding the mobile base (`#254 `_) +* Add support for Magnetometer sensor (`#257 `_) +* Add missing dependencies (`#250 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) +* Add service to set the free joint body state (`#233 `_) +* Add pose sensor test to launch tests (`#232 `_) +* Fix state passed into pose sensors (`#231 `_) +* remove shebangs from launch files (`#227 `_) +* Add support for pose sensors (`#220 `_) +* Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Contributors: Christian Rauch, Daniel Costanzi, Erik Holum, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Switch to forward_command_controller dependency (`#173 `_) diff --git a/mujoco_ros2_control_msgs/CHANGELOG.rst b/mujoco_ros2_control_msgs/CHANGELOG.rst index 5ae1d37f..61c4bcbd 100644 --- a/mujoco_ros2_control_msgs/CHANGELOG.rst +++ b/mujoco_ros2_control_msgs/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package mujoco_ros2_control_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2026-09-07) +------------------ + +0.1.1 (2026-08-25) +------------------ + +0.1.0 (2026-08-18) +------------------ +* Refine reset world overrides functionality (`#263 `_) +* Support joint overrides in reset world service (`#256 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Add service to set the free joint body state (`#233 `_) +* Contributors: Sai Kishor Kothakota, Sebastian Castro + 0.0.3 (2026-05-01) ------------------ * Add more useful plugins for the mujoco_ros2_control (`#165 `_) diff --git a/mujoco_ros2_control_plugins/CHANGELOG.rst b/mujoco_ros2_control_plugins/CHANGELOG.rst index 9aa90ba7..1dde7e84 100644 --- a/mujoco_ros2_control_plugins/CHANGELOG.rst +++ b/mujoco_ros2_control_plugins/CHANGELOG.rst @@ -2,6 +2,37 @@ Changelog for package mujoco_ros2_control_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2026-09-07) +------------------ +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Fix plugin parameter namespaces (`#291 `_) +* Contributors: Bilal Gill, Sebastian Castro, Erik Holum + +0.1.1 (2026-08-25) +------------------ +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + +0.1.0 (2026-08-18) +------------------ +* Add a pre-step callback to the simulation (`#282 `_) +* Add base twist plugin for commanding the mobile base (`#254 `_) +* [doc] Fix undefined label errors (`#260 `_) +* Add missing documentation (`#249 `_) +* Add missing dependencies (`#250 `_) +* [CI] Fix dependency declarations for bloom (`#226 `_) +* MuJoCo free joints state publisher plugin (`#247 `_) +* Fix CameraPlugin default frame name to be consistent with docs. (`#242 `_) +* Make CI tests more robust (`#239 `_) +* Fix links in interface docs, use auto label generator like the upstream (`#238 `_) +* Add Plugin for the 3D Lidar Extension (`#207 `_) +* Fix plugin export so that downstream plugins don't fail (`#215 `_) +* Support polling, streaming, and disabled cameras in camera plugin (`#217 `_) +* Refactor Rangefinder base Lidar as a Plugin (`#214 `_) +* Refactor RGB-D Cameras to a Plugin (`#211 `_) +* Migrate documentation to rst format for control.ros.org (`#188 `_) +* Contributors: Christian Rauch, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa, pang-yann + 0.0.3 (2026-05-01) ------------------ * Add macro check using realtime_tools_MAJOR_VERSION (`#181 `_) diff --git a/mujoco_ros2_control_plugins/CMakeLists.txt b/mujoco_ros2_control_plugins/CMakeLists.txt index f5760bd8..255bd918 100644 --- a/mujoco_ros2_control_plugins/CMakeLists.txt +++ b/mujoco_ros2_control_plugins/CMakeLists.txt @@ -54,6 +54,7 @@ add_library(mujoco_ros2_control_plugins_impl SHARED src/external_wrench_plugin.cpp src/base_velocity_plugin.cpp src/free_joint_state_publisher_plugin.cpp + src/fts_grav_comp_plugin.cpp src/camera_plugin.cpp src/mujoco_3d_lidar_plugin.cpp # TODO: Remove once this has been fully deprecated @@ -187,6 +188,22 @@ if(BUILD_TESTING) "$" "LINKER:--pop-state" ) + + ament_add_gtest(test_fts_grav_comp_plugin + test/test_fts_grav_comp_plugin.cpp + ) + target_include_directories(test_fts_grav_comp_plugin PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/src + ) + target_link_libraries(test_fts_grav_comp_plugin + mujoco_ros2_control_plugins_impl + ${geometry_msgs_TARGETS} + ) + target_link_options(test_fts_grav_comp_plugin PUBLIC + "LINKER:--push-state,--no-as-needed" + "$" + "LINKER:--pop-state" + ) endif() # Install include directory diff --git a/mujoco_ros2_control_plugins/README.md b/mujoco_ros2_control_plugins/README.md index 0595b783..4c164bba 100644 --- a/mujoco_ros2_control_plugins/README.md +++ b/mujoco_ros2_control_plugins/README.md @@ -17,6 +17,7 @@ Full documentation is maintained in RST format: | `ExternalWrenchPlugin` | Applies external wrenches to MuJoCo bodies via a ROS 2 service | | `BaseVelocityPlugin` | Drives a mobile/floating-base robot from a `cmd_vel`-style topic via a direct free-joint velocity override, independent of wheel-ground contact | | `FreeJointStatePublisherPlugin` | Publishes the pose/velocity of free-joint bodies to a topic, in a selectable reference frame | +| `FtsGravCompPlugin` | Applies gravity compensation to a force torque sensor based on a specified mass and center of mass | | `Mujoco3dLidarPlugin` | Republishes MuJoCo lidar-extension sensor data as `LaserScan`/`PointCloud2` messages | | `RangefinderLidarPlugin` | **[Deprecated]** Wraps MuJoCo rangefinder sensors as `LaserScan` publishers | diff --git a/mujoco_ros2_control_plugins/doc/plugins.rst b/mujoco_ros2_control_plugins/doc/plugins.rst index 474be0fe..bb81b7b8 100644 --- a/mujoco_ros2_control_plugins/doc/plugins.rst +++ b/mujoco_ros2_control_plugins/doc/plugins.rst @@ -326,11 +326,9 @@ Each cycle, the commanded body-frame ``vx``/``vy`` is clamped to ``max_linear_ve direction) and rotated into the world frame using the body's current orientation, since a free joint's linear ``qvel`` is expressed in the world frame. The commanded yaw-rate is clamped to ``max_yaw_rate`` and used as-is, since a free joint's rotational ``qvel`` is already expressed in -the body-local frame. The result is written directly into ``data->qvel`` during ``update()``: -the core NaN-fills ``qvel`` before every plugin's ``update()`` runs, so writing a finite value -into an entry requests a hard velocity override there, applied by the core simulation as a direct -``qvel`` write immediately before the next physics step (see "Creating Your Own Plugin" below for -the full mechanism, which is not available through ``xfrc_applied`` alone). +the body-local frame. The result is written directly into ``data->qvel`` during ``pre_step()``, +which runs on the physics thread immediately before every ``mj_step``, which will happen per physics +step! A command that hasn't been refreshed within ``cmd_timeout`` seconds is treated as zero (safety stop) rather than left to coast on the last commanded velocity. @@ -495,6 +493,65 @@ FreeJointStatePublisher Parameters ros2 topic echo /mujoco_ros2_control_node/free_joint_state_publisher/free_joint_states +FtsGravCompPlugin +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Performs gravity compensation for a force torque sensor based on provided parameters. + +This is intended to mock hardware configurations that have a way to perform gravity compensation on the force torque sensor data based on the weight and center of gravity of your end effector. +One example of this is with the `Universal Robots Set Payload `_ command. + +FtsGravCompPlugin Parameters +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Note that each force torque sensor lives as a top level parameter underneath the base `mujoco_ros2_control_plugins/FtsGravCompPlugin` plugin. +That top level parameter should be named the same as what you see in the `mujoco_sensor_name` parameter in the ros2_control tag of your urdf as you might see like `{sensor_name}`. +If the top level parameter does not correspond to two sensors in the mujoco config named `_force` and `_torque`, the plugin will fail to load. + +Each force torque sensor name is treated as a namespace for the remainder of the parameters in the table below. +See the example configuration below for more details. + +.. list-table:: + :widths: 15 15 70 + :header-rows: 1 + + * - Parameter + - Type + - Description + * - ``frame_id`` + - ``string`` + - Name of the mujoco site that the center of mass is represented in. + * - ``CoG.pos`` + - ``double[]`` + - Position in meters w.r.t. the `frame_id` site of the center of gravity. + This vector should be size 3 representing x, y, and z. + * - ``CoG.mass`` + - ``double`` + - Mass to use for gravity compensation in kg. + + +**Example configuration** + +.. code-block:: yaml + + /**: + ros__parameters: + mujoco_plugins: + fts_grav_comp_plugin: + type: "mujoco_ros2_control_plugins/FtsGravCompPlugin" + # name of the sensor to modify. Note that mujoco sensors will look + # like 'fts_sensor_force' and 'fts_sensor_torque' + fts_sensor: + # mujoco site the CoG is represented in + frame_id: ft_sensor_site + # specifies the center of gravity w.r.t the 'frame_id' parameter + CoG: + pos: + - 0.1 # x in m + - 0.0 # y in m + - 0.0 # z in m + mass: 10.0 # mass in kg + .. _rangefinder_lidar_plugin: RangefinderLidarPlugin @@ -602,7 +659,13 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: { public: bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; + + // Override whichever of these you need -- all have a no-op default, see + // "Plugin Lifecycle" below for how they differ. void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; + void on_reset(mjData* /*data*/) + void cleanup() override; private: @@ -633,7 +696,17 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: void MyCustomPlugin::update(const mjModel* model, mjData* data) { - // Called every control loop iteration + // Called once per ros2_control write() cycle, on the control thread. + } + + void MyCustomPlugin::pre_step(mjData* data) + { + // Called on the physics thread, immediately before every mj_step. + } + + void on_reset(mjData* /*data*/) + { + // Called after a world reset (ResetWorld service or UI reset). } void MyCustomPlugin::cleanup() @@ -641,7 +714,7 @@ Create a header that inherits from ``MuJoCoROS2ControlPluginBase``: // Clean up resources } - } // namespace my_namespace + } // namespace my_namespace PLUGINLIB_EXPORT_CLASS( my_namespace::MyCustomPlugin, @@ -693,28 +766,28 @@ Plugin Lifecycle 1. **Initialization** (``init``): Called once when the plugin is loaded. Use this to read parameters and set up publishers, subscribers, and services. -2. **Update** (``update``): Called every simulation step at the **end of the** ``read`` **loop**, - before the controller update and ``write`` loops. Changes to ``mjData`` here are visible to - controllers and affect the next simulation step. This runs in a real-time thread — avoid - blocking operations. - - ``data->qvel`` here has one special property: it is **NaN-filled before every plugin's** - ``update()`` **runs this cycle**. Most plugins never touch it and can ignore this entirely. - A plugin that needs to *dictate* a free joint's velocity exactly (``BaseVelocityPlugin`` is - the example in this package) can write a finite value into any ``qvel`` entry to request a - hard velocity override there — the core simulation applies every non-NaN entry as a direct - ``qvel`` write immediately before the next ``mj_step``, bypassing the normal mass/contact - dynamics for that DOF. This exists because plugins only ever see a throwaway copy of - ``mjData`` — only ``ctrl``, ``qfrc_applied``, and ``xfrc_applied`` are otherwise copied back - into the real simulation state, so this NaN convention is the only way a plugin can - influence ``qvel`` at all. Leaving an entry ``NaN`` keeps its normal physics-driven value; - because of this, ``data->qvel`` cannot be read here for the body's actual velocity either, - since it isn't restored to a real value until after every plugin's ``update()`` has run. - See ``MuJoCoROS2ControlPluginBase::update()``'s doc comment in - ``mujoco_ros2_control_plugins_base.hpp`` for the authoritative reference. -3. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in +2. **Update** (``update``, optional): Called once per ``ros2_control`` ``write()`` cycle, on the + control thread. ``data`` is a recent snapshot, not the live simulation data. Use this for + anything that doesn't need to run on exactly one physics step: publishing sensor data, + servicing a trigger, etc. Most plugins in this package (``CameraPlugin``, the lidar plugins, + ``HeartbeatPublisherPlugin``, ``FreeJointStatePublisherPlugin``) use only this hook. +3. **Pre-step** (``pre_step``, optional): Called on the physics thread, immediately before every + ``mj_step`` -- including multiple times per outer iteration when the loop batches steps to + catch up. ``data`` is the live simulation data: read and write it directly, with no separate + command buffer, so an untouched entry keeps its last value. Use this for anything that must + hold for exactly one physics step, such as ``BaseVelocityPlugin``'s kinematic velocity + override. Runs with the simulation mutex held, so blocking here stalls the physics loop and + native viewer too. +4. **On Reset** (``on_reset``, optional): Called when the simulation is "reset", either from + the ResetWorld service from the UI. This lets plugins know that the state of the world has + been reset and they can take appropriate action. +5. **Cleanup** (``cleanup``): Called when shutting down. Release any resources acquired in ``init``. +Both hooks default to doing nothing, so implement whichever fits (or both, or neither). See +``MuJoCoROS2ControlPluginBase``'s class doc comment in ``mujoco_ros2_control_plugins_base.hpp`` +for the authoritative reference. + Building -------- diff --git a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp index 73bbfa30..3f0bf179 100644 --- a/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp +++ b/mujoco_ros2_control_plugins/include/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp @@ -26,9 +26,24 @@ namespace mujoco_ros2_control_plugins /** * @brief Base class for MuJoCo ROS 2 control plugins * - * This is an example base class that plugins can inherit from. - * Plugins can extend the functionality of mujoco_ros2_control - * by implementing custom behaviors. + * Plugins extend mujoco_ros2_control with custom behavior, e.g. publishing extra topics or + * applying external forces. + * + * Three optional hooks are available. Override whichever fits your use case: + * + * - `update()` runs on the ros2_control control thread, once per `write()` cycle. `data` is a + * recent snapshot, not the live `mj_data_`. Use this for anything that doesn't need to run on + * exactly one physics step, e.g. publishing sensor data or servicing a trigger. + * - `pre_step()` runs on the physics thread, immediately before every `mj_step()`, with direct + * access to the live `mj_data_`. Use this for anything that must hold for exactly one step, + * e.g. a hard kinematic override. There's no command buffer: an untouched entry keeps its + * last value, so releasing a command (e.g. an expired force) needs an explicit write. This + * should be used with caution! + * - `on_reset()` runs immediately after a world reset, with the live `mj_data_` in its + * post-reset state. Use this to clear plugin state that refers to the pre-reset world, + * e.g. a latched command. + * + * @note All hooks must avoid blocking or the control loop or simulation itself will be held up! */ class MuJoCoROS2ControlPluginBase { @@ -48,22 +63,33 @@ class MuJoCoROS2ControlPluginBase virtual bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) = 0; /** - * @brief Update the plugin (called every simulation step) + * @brief Called once per control cycle, on the control thread. Default does nothing. * @param model Pointer to the MuJoCo model - * @param data Pointer to the MuJoCo data - * @note This method will be called at the end of the mujoco_ros2_control read loop, before the update loop of - * controllers and the write loop. This means that changes to the data here will be visible to controllers and will - * affect the next simulation step. - * @note This method will be called in a real-time thread, so it should avoid blocking operations and should be - * efficient. - * @note data->qvel is NaN-filled before every plugin's update() runs this cycle. Write a finite value into any - * entry to request a hard velocity override on that DOF -- it bypasses the normal mass/contact dynamics for that - * DOF this step, overwriting mj_data_->qvel directly right before the next mj_step. Leave an entry NaN to keep its - * normal physics-driven value untouched. This is the only channel a plugin has to influence qvel: unlike - * ctrl/qfrc_applied/xfrc_applied, data->qvel here cannot be read for the body's actual velocity, since it is not - * restored to a real value until after every plugin's update() has run this cycle. + * @param data Pointer to a recent snapshot (not the live `mj_data_`) + */ + virtual void update(const mjModel* /*model*/, mjData* /*data*/) + { + } + + /** + * @brief Called immediately before every simulation step on the physics thread. Default does nothing. + * + * USE WITH CAUTION. + * + * @param data Pointer to the live MuJoCo data, can be modified as needed. + */ + virtual void pre_step(mjData* /*data*/) + { + } + + /** + * @brief Called after a world reset (ResetWorld service or UI reset). Default does nothing. + * + * @param data Pointer to the live MuJoCo data in its post-reset state. */ - virtual void update(const mjModel* model, mjData* data) = 0; + virtual void on_reset(mjData* /*data*/) + { + } /** * @brief Cleanup the plugin diff --git a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml index 6d24aaec..f4d760a7 100644 --- a/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml +++ b/mujoco_ros2_control_plugins/mujoco_ros2_control_plugins.xml @@ -40,6 +40,14 @@ unset. + + + Runs gravity compensation on the wrench found by the force torque sensor as specified in + the params file. + + diff --git a/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp b/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp index 8c140a75..d1db6e6c 100644 --- a/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/base_velocity_plugin.cpp @@ -128,7 +128,7 @@ void BaseVelocityPlugin::storeCommand(double vx, double vy, double wz) latest_cmd_ = { vx, vy, wz, node_->get_clock()->now() }; } -void BaseVelocityPlugin::update(const mjModel* /*model_arg*/, mjData* data) +void BaseVelocityPlugin::pre_step(mjData* data) { // Step 1 - refresh the cached command from the subscription callback without // blocking the real-time thread; if the lock is contended, keep using the last diff --git a/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp b/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp index 822c10b6..0e7efd51 100644 --- a/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp +++ b/mujoco_ros2_control_plugins/src/base_velocity_plugin.hpp @@ -31,9 +31,7 @@ namespace mujoco_ros2_control_plugins /** * @brief Drives a mobile/floating-base robot from a commanded planar body velocity * (vx, vy, yaw-rate) by writing a hard kinematic override of the base's free-joint - * velocity directly into data->qvel every cycle (see - * MuJoCoROS2ControlPluginBase::update()'s doc comment for the NaN convention this - * relies on). + * velocity directly into data->qvel every cycle. * * Wheel-terrain friction/slip modelling is often unreliable enough to make it a poor * foundation for testing navigation stacks. This plugin instead subscribes to a @@ -73,7 +71,7 @@ class BaseVelocityPlugin : public MuJoCoROS2ControlPluginBase ~BaseVelocityPlugin() override = default; bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; - void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; void cleanup() override; private: @@ -106,12 +104,12 @@ class BaseVelocityPlugin : public MuJoCoROS2ControlPluginBase }; // Latest command, written by the subscription callback (ROS executor thread) under - // cmd_mutex_. update() (real-time thread) copies it into cached_cmd_ via try_lock, so + // cmd_mutex_. pre_step() (physics thread) copies it into cached_cmd_ via try_lock, so // it never blocks on the subscription callback. std::mutex cmd_mutex_; CommandState latest_cmd_; - // Local copy of the command, only ever touched from update() (single real-time thread). + // Local copy of the command, only ever touched from pre_step() (physics thread). CommandState cached_cmd_; }; diff --git a/mujoco_ros2_control_plugins/src/camera_plugin.cpp b/mujoco_ros2_control_plugins/src/camera_plugin.cpp index 3a20e2f9..74741ffb 100644 --- a/mujoco_ros2_control_plugins/src/camera_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/camera_plugin.cpp @@ -152,14 +152,14 @@ void CameraPlugin::trigger_update() bool CameraPlugin::register_cameras() { - const std::string param_prefix = "mujoco_plugins.mujoco_camera_plugin."; - - if (!node_->has_parameter(param_prefix + "camera_publish_rate")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string camera_publish_rate_param = param_prefix + "camera_publish_rate"; + if (!node_->has_parameter(camera_publish_rate_param)) { - node_->declare_parameter(param_prefix + "camera_publish_rate", 5.0); + node_->declare_parameter(camera_publish_rate_param, 5.0); } - camera_publish_rate_ = node_->get_parameter(param_prefix + "camera_publish_rate").as_double(); + camera_publish_rate_ = node_->get_parameter(camera_publish_rate_param).as_double(); RCLCPP_INFO(node_->get_logger(), "Publishing camera data at rate %f per second.", camera_publish_rate_); cameras_.resize(0); diff --git a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp index 0671fda5..32814250 100644 --- a/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/external_wrench_plugin.cpp @@ -31,16 +31,19 @@ bool ExternalWrenchPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mod model_ = model; // Visualization parameters - if (!node_->has_parameter("force_arrow_scale")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string force_arrow_scale_param = param_prefix + "force_arrow_scale"; + if (!node_->has_parameter(force_arrow_scale_param)) { - node_->declare_parameter("force_arrow_scale", force_arrow_scale_); + node_->declare_parameter(force_arrow_scale_param, force_arrow_scale_); } - if (!node_->has_parameter("torque_arrow_scale")) + const std::string torque_arrow_scale_param = param_prefix + "torque_arrow_scale"; + if (!node_->has_parameter(torque_arrow_scale_param)) { - node_->declare_parameter("torque_arrow_scale", torque_arrow_scale_); + node_->declare_parameter(torque_arrow_scale_param, torque_arrow_scale_); } - force_arrow_scale_ = node_->get_parameter("force_arrow_scale").as_double(); - torque_arrow_scale_ = node_->get_parameter("torque_arrow_scale").as_double(); + force_arrow_scale_ = node_->get_parameter(force_arrow_scale_param).as_double(); + torque_arrow_scale_ = node_->get_parameter(torque_arrow_scale_param).as_double(); marker_pub_raw_ = node_->create_publisher("~/wrench_markers", rclcpp::SystemDefaultsQoS()); marker_pub_ = std::make_unique>(marker_pub_raw_); @@ -55,12 +58,10 @@ bool ExternalWrenchPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* mod return true; } -void ExternalWrenchPlugin::update(const mjModel* /*model_arg*/, mjData* data) +void ExternalWrenchPlugin::pre_step(mjData* data) { - // Step 0 - undo the xfrc_applied contributions written in the previous cycle - // so stale forces are never left in the array when all wrenches have expired. - // When the system interface also zeroes xfrc_applied before calling update(), - // this loop is a harmless no-op on already-zero values. + // Step 0 - undo the xfrc_applied contributions written last cycle, so an expired wrench is + // actually released instead of left in place. for (const int body_id : prev_written_body_ids_) { mjtNum* base = data->xfrc_applied + body_id * 6; diff --git a/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp b/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp index 848c39a4..f7cb0d3f 100644 --- a/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp +++ b/mujoco_ros2_control_plugins/src/external_wrench_plugin.hpp @@ -80,13 +80,10 @@ namespace mujoco_ros2_control_plugins * where R = data->xmat (body → world rotation) and * p_world = data->xpos + R * application_point_body. * - * At the start of every update() call the plugin zeroes the xfrc_applied slots - * it wrote during the previous cycle before re-accumulating the current active - * wrenches. This self-cleanup ensures the plugin leaves no stale contributions - * in xfrc_applied when all wrenches have expired. When the surrounding system - * interface also zeroes xfrc_applied before calling update() (as - * MujocoSystemInterface::read() does), the plugin's undo step is a harmless - * no-op on already-zero values. + * pre_step() writes directly into the live xfrc_applied, which holds its value across steps + * until written again. So at the start of every pre_step() call, the plugin zeroes the + * xfrc_applied slots it wrote last cycle before re-accumulating the active wrenches, that + * will release the expired wrenches. */ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase { @@ -95,7 +92,7 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase ~ExternalWrenchPlugin() override = default; bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; - void update(const mjModel* model, mjData* data) override; + void pre_step(mjData* data) override; void cleanup() override; /** @@ -135,7 +132,7 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase void handleApplyWrench(const ApplyExternalWrench::Request::SharedPtr request, ApplyExternalWrench::Response::SharedPtr response); - /// Publish the result of publish_markers() via the realtime publisher. Called from update(). + /// Publish the result of publish_markers() via the realtime publisher. Called from pre_step(). void publish_markers(); // ROS interfaces @@ -149,15 +146,15 @@ class ExternalWrenchPlugin : public MuJoCoROS2ControlPluginBase // Model pointer (const, valid for simulation lifetime) const mjModel* model_{ nullptr }; - // Pending queue written by service callback, drained in update() + // Pending queue written by service callback, drained in pre_step() std::mutex pending_mutex_; std::queue pending_wrenches_; - // Active wrenches — only modified inside update() + // Active wrenches — only modified inside pre_step() std::vector active_wrenches_; - // Body IDs whose xfrc_applied slots were written in the previous update() call. - // Zeroed at the start of the next update() before re-accumulating. + // Body IDs whose xfrc_applied slots were written in the previous pre_step() call. + // Zeroed at the start of the next pre_step() before re-accumulating. std::vector prev_written_body_ids_; // Marker visualization scaling diff --git a/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp b/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp index da98cd76..f7fff14f 100644 --- a/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/free_joint_state_publisher_plugin.cpp @@ -84,27 +84,32 @@ bool FreeJointStatePublisherPlugin::init(rclcpp::Node::SharedPtr node, const mjM model_ = model; logger_ = node_->get_logger().get_child(node->get_sub_namespace()); - if (!node_->has_parameter("frame_id")) + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string frame_id_param = param_prefix + "frame_id"; + if (!node_->has_parameter(frame_id_param)) { - node_->declare_parameter("frame_id", std::string("")); + node_->declare_parameter(frame_id_param, std::string("")); } - if (!node_->has_parameter("body_names")) + const std::string body_names_param = param_prefix + "body_names"; + if (!node_->has_parameter(body_names_param)) { - node_->declare_parameter("body_names", std::vector{}); + node_->declare_parameter(body_names_param, std::vector{}); } - if (!node_->has_parameter("topic")) + const std::string topic_param = param_prefix + "topic"; + if (!node_->has_parameter(topic_param)) { - node_->declare_parameter("topic", std::string("free_joint_states")); + node_->declare_parameter(topic_param, std::string("free_joint_states")); } - if (!node_->has_parameter("publish_rate")) + const std::string publish_rate_param = param_prefix + "publish_rate"; + if (!node_->has_parameter(publish_rate_param)) { - node_->declare_parameter("publish_rate", 50.0); + node_->declare_parameter(publish_rate_param, 50.0); } - const std::string frame_id = node_->get_parameter("frame_id").as_string(); - const std::vector body_names = node_->get_parameter("body_names").as_string_array(); - const std::string topic = node_->get_parameter("topic").as_string(); - const double publish_rate = node_->get_parameter("publish_rate").as_double(); + const std::string frame_id = node_->get_parameter(frame_id_param).as_string(); + const std::vector body_names = node_->get_parameter(body_names_param).as_string_array(); + const std::string topic = node_->get_parameter(topic_param).as_string(); + const double publish_rate = node_->get_parameter(publish_rate_param).as_double(); if (publish_rate <= 0.0) { diff --git a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp new file mode 100644 index 00000000..cc6975f7 --- /dev/null +++ b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.cpp @@ -0,0 +1,226 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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. + */ + +#include "fts_grav_comp_plugin.hpp" +#include + +namespace mujoco_ros2_control_plugins +{ + +std::vector FtsGravCompPlugin::get_sensor_names_from_parameters() +{ + const std::string param_namespace = "mujoco_plugins." + node_->get_sub_namespace(); + // List all parameters under mujoco_plugins. recursively. A finite depth is not + // used here because rclcpp's depth counting is off-by-one on Humble (fixed in Jazzy), so a + // fixed depth value does not behave consistently across distros. + auto param_names = + node_->list_parameters({ param_namespace }, rcl_interfaces::srv::ListParameters::Request::DEPTH_RECURSIVE).names; + + // Use a set to store unique sensor names + std::set sensor_names_set; + + const std::string prefix = param_namespace + "."; + + for (const auto& name : param_names) + { + // Remove the prefix + if (name.find(prefix) == 0) + { + std::string relative_path = name.substr(prefix.length()); + + // Extract the first part (sensor name) before the first dot + size_t dot_pos = relative_path.find('.'); + if (dot_pos != std::string::npos) + { + std::string sensor_name = relative_path.substr(0, dot_pos); + + // Skip "type" parameter + if (relative_path.compare(dot_pos + 1, std::string::npos, "frame_id") == 0) + { + sensor_names_set.insert(sensor_name); + } + } + } + } + + // Convert set to vector + std::vector sensor_names(sensor_names_set.begin(), sensor_names_set.end()); + + return sensor_names; +} + +bool FtsGravCompPlugin::register_fts(const mjModel* model) +{ + const auto sensor_names = get_sensor_names_from_parameters(); + + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; + const std::string force_suffix = "_force"; + const std::string torque_suffix = "_torque"; + + fts_.resize(0); + for (const auto& sensor_name : sensor_names) + { + const std::string sensor_param_prefix = param_prefix + sensor_name + "."; + const std::string sensor_name_force = sensor_name + force_suffix; + const std::string sensor_name_torque = sensor_name + torque_suffix; + + // get sensor ids and make sure they are valid + int sensor_id_force = mj_name2id(model, mjOBJ_SENSOR, sensor_name_force.c_str()); + if (sensor_id_force == -1) + { + RCLCPP_ERROR(logger_, "Force sensor name %s not found in the model.", sensor_name_force.c_str()); + return false; + } + int sensor_id_torque = mj_name2id(model, mjOBJ_SENSOR, sensor_name_torque.c_str()); + if (sensor_id_torque == -1) + { + RCLCPP_ERROR(logger_, "Torque sensor name %s not found in the model.", sensor_name_torque.c_str()); + return false; + } + + // grab the rest of the parameters that we need + const std::vector cog_pos_param = node_->get_parameter(sensor_param_prefix + "CoG.pos").as_double_array(); + const double cog_mass_param = node_->get_parameter(sensor_param_prefix + "CoG.mass").as_double(); + const std::string frame_id_param = node_->get_parameter(sensor_param_prefix + "frame_id").as_string(); + + // make sure that the force and torque sensors are of the right dimension + const int sensor_dim_force = model->sensor_dim[sensor_id_force]; + const int sensor_dim_torque = model->sensor_dim[sensor_id_torque]; + + if (cog_pos_param.size() != 3) + { + RCLCPP_ERROR(logger_, "CoG.pos should be of exactly size 3 representing x, y, z coordinates"); + return false; + } + + if ((sensor_dim_force != 3) || (sensor_dim_torque != 3)) + { + RCLCPP_ERROR(logger_, + "Force and Torque sensors must be size 3. Sensors '%s' and '%s' are size %d and %d respectively.", + sensor_name_force.c_str(), sensor_name_torque.c_str(), sensor_dim_force, sensor_dim_torque); + return false; + } + + int cog_site_id = mj_name2id(model, mjOBJ_SITE, frame_id_param.c_str()); + if (cog_site_id == -1) + { + RCLCPP_ERROR(logger_, "Param cog.frame_id '%s' does not exist as a site in the mjcf", frame_id_param.c_str()); + return false; + } + + FtsData fts_data; + fts_data.sensor_name = sensor_name; + fts_data.sensor_adr_force = model->sensor_adr[sensor_id_force]; + fts_data.sensor_adr_torque = model->sensor_adr[sensor_id_torque]; + fts_data.cog_site_id = cog_site_id; + fts_data.cog_mass = cog_mass_param; + fts_data.cog_pos[0] = cog_pos_param[0]; + fts_data.cog_pos[1] = cog_pos_param[1]; + fts_data.cog_pos[2] = cog_pos_param[2]; + fts_data.fts_site_id = model->sensor_objid[sensor_id_force]; + + RCLCPP_INFO(logger_, "Registered FTS under fts_grav_comp_plugin with"); + RCLCPP_INFO(logger_, "\tname: '%s'", fts_data.sensor_name.c_str()); + RCLCPP_INFO(logger_, "\tfts_site: '%s'", mj_id2name(model, mjOBJ_SITE, fts_data.fts_site_id)); + RCLCPP_INFO(logger_, "\tcog_mass: '%0.3f'", fts_data.cog_mass); + RCLCPP_INFO(logger_, "\tcog_pos: '%0.3f, %0.3f, %0.3f'", fts_data.cog_pos[0], fts_data.cog_pos[1], + fts_data.cog_pos[2]); + RCLCPP_INFO(logger_, "\tcog_site: '%s'", mj_id2name(model, mjOBJ_SITE, fts_data.cog_site_id)); + fts_.push_back(fts_data); + } + return true; +} + +bool FtsGravCompPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* /*data*/) +{ + node_ = node; + logger_ = node_->get_logger().get_child(node->get_sub_namespace()); + + if (!register_fts(model)) + { + RCLCPP_ERROR(node_->get_logger(), "Failed to register force torque sensors."); + return false; + } + if (fts_.empty()) + { + return true; + } + + return true; +} + +void FtsGravCompPlugin::update(const mjModel* model, mjData* data) +{ + // run gravity compensation for each force torque sensor + for (auto& fts : fts_) + { + // Get the FTS data locally + mjtNum* sensordata_force = data->sensordata + fts.sensor_adr_force; + mjtNum* sensordata_torque = data->sensordata + fts.sensor_adr_torque; + // pre-processed force and torque + mjtNum raw_force_in_fts_frame[3]; + mjtNum raw_torque_in_fts_frame[3]; + mju_copy3(raw_force_in_fts_frame, sensordata_force); + mju_copy3(raw_torque_in_fts_frame, sensordata_torque); + // gravity compensated force and torque + mjtNum comped_force_in_fts_frame[3]; + mjtNum comped_torque_in_fts_frame[3]; + + // Step 1 - transform cog pos to be about the fts frame + // part a: transform cog pos to world: pos_world = R_cog_frame * pos_in_cog_frame + pos_in_cog_frame + mjtNum cog_pos_world[3]; + mju_mulMatVec3(cog_pos_world, data->site_xmat + 9 * fts.cog_site_id, fts.cog_pos); + mju_addTo3(cog_pos_world, data->site_xpos + 3 * fts.cog_site_id); + + // part b: transform to FTS frame: pos_in_fts_frame = R_ft_frame^T * (pos_world - pos_ft_frame) + mjtNum pos_relative[3]; + mjtNum cog_pos_in_fts_frame[3]; + mju_sub3(pos_relative, cog_pos_world, data->site_xpos + 3 * fts.fts_site_id); + mju_mulMatTVec3(cog_pos_in_fts_frame, data->site_xmat + 9 * fts.fts_site_id, pos_relative); + + // Step 2 - get gravity into the FTS frame + mjtNum gravity_in_fts_frame[3]; + mjtNum weight_in_fts_frame[3]; + mjtNum weight_torque_in_fts_frame[3]; + mju_mulMatTVec3(gravity_in_fts_frame, data->site_xmat + 9 * fts.fts_site_id, model->opt.gravity); + mju_scl3(weight_in_fts_frame, gravity_in_fts_frame, fts.cog_mass); + + // Step 3 - subtract out forces and torques due to weight of the end effector + // subtract out weight of gravity from the wrench value (add bc mujoco has negative values for these) + mju_add3(comped_force_in_fts_frame, raw_force_in_fts_frame, weight_in_fts_frame); + // calculate torque, and subtract out torque from the torque value (add bc mujoco has negative values for these) + mju_cross(weight_torque_in_fts_frame, cog_pos_in_fts_frame, weight_in_fts_frame); + mju_add3(comped_torque_in_fts_frame, raw_torque_in_fts_frame, weight_torque_in_fts_frame); + + // copy the data back to the FTS data + mju_copy3(sensordata_force, comped_force_in_fts_frame); + mju_copy3(sensordata_torque, comped_torque_in_fts_frame); + } +} + +void FtsGravCompPlugin::cleanup() +{ + node_.reset(); +} + +} // namespace mujoco_ros2_control_plugins + +// Export the plugin +PLUGINLIB_EXPORT_CLASS(mujoco_ros2_control_plugins::FtsGravCompPlugin, + mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase) diff --git a/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp new file mode 100644 index 00000000..b432b98e --- /dev/null +++ b/mujoco_ros2_control_plugins/src/fts_grav_comp_plugin.hpp @@ -0,0 +1,105 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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. + */ + +#ifndef MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ +#define MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ + +#include +#include +#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" + +namespace mujoco_ros2_control_plugins +{ + +/** + * @brief Per-FTS bookkeeping - + * + * One instance exists per FTS registered by FtsGravCompPlugin::register_fts(). + */ +struct FtsData +{ + // sensor name of FTS. Mujoco should have sensors _force and _torque + std::string sensor_name; + // mujoco sensor address for the force and torque sensors + int sensor_adr_force{ -1 }; + int sensor_adr_torque{ -1 }; + // site id of the site used for center of gravity reference + int cog_site_id{ -1 }; + // array of doubles of the center of gravity w.r.t. cog_site_it in meters for [x, y, z] + mjtNum cog_pos[3]; + // mass that is being compensated in kg + mjtNum cog_mass; + // site id of the site used for the mujoco force and torque sensors + int fts_site_id{ -1 }; +}; + +/** + * @brief Plugin that runs gravity compensation on a force torque sensor + */ +class FtsGravCompPlugin : public MuJoCoROS2ControlPluginBase +{ +public: + FtsGravCompPlugin() = default; + ~FtsGravCompPlugin() override = default; + + /** + * @brief Parses the params file and returns a vector of unique sensor names + * + */ + std::vector get_sensor_names_from_parameters(); + + /** + * @brief Register force torque sensors from the model that match the params + * + */ + bool register_fts(const mjModel* model); + + /** + * @brief Initialize the plugin + */ + bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override; + + /** + * @brief Update the plugin (called every simulation step) + */ + void update(const mjModel* model, mjData* data) override; + + /** + * @brief Cleanup the plugin + */ + void cleanup() override; + + /** + * @brief getter for fts data used for testing + */ + std::vector get_fts_data() const + { + return fts_; + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp::Logger logger_ = rclcpp::get_logger("FtsGravCompPlugin"); + + std::vector fts_; +}; + +} // namespace mujoco_ros2_control_plugins + +#endif // MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_ diff --git a/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp b/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp index bc1db6ea..57863cf7 100644 --- a/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/mujoco_3d_lidar_plugin.cpp @@ -219,7 +219,7 @@ bool Mujoco3dLidarPlugin::register_sensor(const mjModel* model, int sensor_idx) // site name in the MJCF if not provided. Topics default to `/scan` and `/points` for 2-D and // 3-D scans, respectively. // Note that we must prefix our param declarations to ensure nesting works out properly. - const std::string param_prefix = "mujoco_plugins.mujoco_3d_lidar_plugin."; + const std::string param_prefix = "mujoco_plugins." + node_->get_sub_namespace() + "."; const auto site_name = mj_id2name(model, mjtObj::mjOBJ_SITE, model->sensor_objid[sensor_idx]); const std::string frame_name_param = param_prefix + lidar_config.name + ".frame_name"; if (!node_->has_parameter(frame_name_param)) diff --git a/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp b/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp index 71ea48c3..e43bb086 100644 --- a/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp +++ b/mujoco_ros2_control_plugins/src/rangefinder_lidar_plugin.cpp @@ -124,7 +124,7 @@ bool RangefinderLidarPlugin::init(rclcpp::Node::SharedPtr node, const mjModel* m bool RangefinderLidarPlugin::register_sensor(const std::string& lidar_name, const mjModel* /* model */) { - const std::string ns = "mujoco_plugins.rangefinder_lidar_plugin." + lidar_name + "."; + const std::string ns = "mujoco_plugins." + node_->get_sub_namespace() + "." + lidar_name + "."; // Check required parameters if (!node_->has_parameter(ns + "frame_name")) diff --git a/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp b/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp index 4c2efe54..a3acfde6 100644 --- a/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_base_velocity_plugin.cpp @@ -237,7 +237,7 @@ TEST_F(BaseVelocityPluginTest, NoCommandRequestsZeroVelocity) auto plugin = makeInitializedPlugin(); fillQvelNaN(); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_DOUBLE_EQ(qvel[0], 0.0); @@ -259,7 +259,7 @@ TEST_F(BaseVelocityPluginTest, CommandIsReportedAsOverride) fillQvelNaN(); publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.3); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); // Identity orientation => body-x == world-x. This is a direct assignment, not a servo, @@ -282,13 +282,13 @@ TEST_F(BaseVelocityPluginTest, StaleCommandRequestsZeroVelocity) publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); ASSERT_GT(qvel[0], 0.0) << "sanity: velocity requested while command is fresh"; // Wait past the timeout without publishing again. std::this_thread::sleep_for(std::chrono::milliseconds(400)); - plugin->update(model_, data_); + plugin->pre_step(data_); EXPECT_NEAR(qvel[0], 0.0, 1e-9) << "stale command should be treated as zero"; @@ -301,7 +301,7 @@ TEST_F(BaseVelocityPluginTest, LinearCommandIsClampedToMaxLinearVelocity) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/5.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 0.2, 1e-9); @@ -315,7 +315,7 @@ TEST_F(BaseVelocityPluginTest, YawCommandIsClampedToMaxYawRate) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/0.0, /*vy=*/0.0, /*wz=*/5.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[5], 0.3, 1e-9); @@ -330,7 +330,7 @@ TEST_F(BaseVelocityPluginTest, UnsetLimitsDoNotClamp) auto plugin = makeInitializedPlugin(); publishTwist("cmd_vel", /*vx=*/1000.0, /*vy=*/0.0, /*wz=*/1000.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 1000.0, 1e-6); @@ -355,7 +355,7 @@ TEST_F(BaseVelocityPluginTest, BodyFrameCommandIsRotatedIntoFreeJointFrame) mj_forward(model_, data_); publishTwist("cmd_vel", /*vx=*/1.0, /*vy=*/0.0, /*wz=*/0.0); - plugin->update(model_, data_); + plugin->pre_step(data_); const mjtNum* qvel = data_->qvel + dofAdr(); EXPECT_NEAR(qvel[0], 0.0, 1e-6) << "world-x should be ~0 after a 90 deg yaw"; diff --git a/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp b/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp index 0caab54f..e3947dfa 100644 --- a/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_camera_plugin.cpp @@ -257,8 +257,9 @@ TEST_F(CameraPluginTest, InitAndPublish) // Only polled cameras should expose a trigger service; streaming cameras should not. TEST_F(CameraPluginTest, OnlyPolledCamerasCreateTriggerService) { - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.stream_cam.policy", std::string("streaming")); - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.poll_cam.policy", std::string("polled")); + const std::string param_prefix = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "."; + plugin_node_->declare_parameter(param_prefix + "stream_cam.policy", std::string("streaming")); + plugin_node_->declare_parameter(param_prefix + "poll_cam.policy", std::string("polled")); load_model(R"( @@ -313,7 +314,8 @@ TEST_F(CameraPluginTest, PolledCameraPublishesOncePerTrigger) )"); ASSERT_EQ(model_->ncam, 1); - plugin_node_->declare_parameter("mujoco_plugins.mujoco_camera_plugin.poll_cam.policy", std::string("polled")); + const std::string param_prefix = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "."; + plugin_node_->declare_parameter(param_prefix + "poll_cam.policy", std::string("polled")); mujoco_ros2_control_plugins::CameraPlugin plugin; EXPECT_TRUE(plugin.init(plugin_node_, model_, data_, []() { return 0; })); diff --git a/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp b/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp index a26fa014..68d26e76 100644 --- a/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_external_wrench_plugin.cpp @@ -241,7 +241,7 @@ TEST_F(ExternalWrenchPluginTest, UpdateAppliesForceToXfrcApplied) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); // At least one xfrc_applied entry must carry a non-zero value. double total = 0.0; @@ -264,10 +264,10 @@ TEST_F(ExternalWrenchPluginTest, UpdateUndoesPreviousContributionOnNextCall) ASSERT_TRUE(resp->success); // First update: wrench applied, then immediately expired (zero duration). - plugin.update(model_, data_); + plugin.pre_step(data_); // Second update: plugin subtracts its saved contribution; no active wrenches remain. - plugin.update(model_, data_); + plugin.pre_step(data_); for (int i = 0; i < model_->nbody * 6; ++i) { @@ -295,7 +295,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) } // First update: both wrenches active, both expire (zero duration). - plugin.update(model_, data_); + plugin.pre_step(data_); double total_both = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -304,7 +304,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) } // Second update clears state. - plugin.update(model_, data_); + plugin.pre_step(data_); // Queue a single wrench of the same magnitude. { @@ -313,7 +313,7 @@ TEST_F(ExternalWrenchPluginTest, MultipleWrenchesAccumulateLinearly) ASSERT_TRUE(r->success); } - plugin.update(model_, data_); + plugin.pre_step(data_); double total_one = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -339,7 +339,7 @@ TEST_F(ExternalWrenchPluginTest, TorqueOnlyWrenchAppliesRotationalForce) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); double total = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -382,7 +382,7 @@ TEST_F(ExternalWrenchPluginTest, SingleCallWithMultipleWrenchesAppliesAll) ASSERT_NE(resp, nullptr); ASSERT_TRUE(resp->success); - plugin.update(model_, data_); + plugin.pre_step(data_); double total = 0.0; for (int i = 0; i < model_->nbody * 6; ++i) @@ -427,7 +427,7 @@ TEST_F(ExternalWrenchPluginTest, SingleCallRejectsIfAnyBodyNameInvalid) EXPECT_FALSE(resp->message.empty()); // No wrench should have been applied — xfrc_applied must remain zero. - plugin.update(model_, data_); + plugin.pre_step(data_); for (int i = 0; i < model_->nbody * 6; ++i) { EXPECT_DOUBLE_EQ(data_->xfrc_applied[i], 0.0) << "xfrc_applied[" << i << "] must be zero after rejected batch"; @@ -442,14 +442,14 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersForActiveForceWrench) ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); // Send a wrench with a 200 ms duration from a background thread so the - // sleeping service callback does not block update() in the main thread. + // sleeping service callback does not block pre_step() in the main thread. std::thread svc_thread([this]() { callServiceWithDuration("test_body", 0.2, /*fx=*/10.0); }); // Wait briefly for the wrench to reach the pending queue. std::this_thread::sleep_for(std::chrono::milliseconds(50)); - // Run update() while the wrench is still active. - plugin.update(model_, data_); + // Run pre_step() while the wrench is still active. + plugin.pre_step(data_); // Collect markers via the new aggregation API. MarkerArray markers; @@ -484,7 +484,7 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersContributesNothingWhenNoWrenchesA ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); // No service calls — no active wrenches. - plugin.update(model_, data_); + plugin.pre_step(data_); // publish_markers() must not append anything when there are no active wrenches. // Marker lifetime-based expiry (managed by the system interface) handles cleanup @@ -504,7 +504,7 @@ TEST_F(ExternalWrenchPluginTest, PublishMarkersAppendsToExistingArray) std::thread svc_thread([this]() { callServiceWithDuration("test_body", 0.2, /*fx=*/5.0, /*fy=*/0.0, /*fz=*/0.0); }); std::this_thread::sleep_for(std::chrono::milliseconds(50)); - plugin.update(model_, data_); + plugin.pre_step(data_); // Pre-populate the array with a sentinel marker (simulates another plugin's contribution). MarkerArray markers; diff --git a/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp b/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp index 40d49b03..838387cf 100644 --- a/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp +++ b/mujoco_ros2_control_plugins/test/test_free_joint_state_publisher_plugin.cpp @@ -152,11 +152,12 @@ class FreeJointStatePublisherPluginTest : public ::testing::Test template void setParam(const std::string& name, const T& value) { - if (!plugin_node_->has_parameter(name)) + const std::string resolved_name = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "." + name; + if (!plugin_node_->has_parameter(resolved_name)) { - plugin_node_->declare_parameter(name, rclcpp::ParameterValue(value)); + plugin_node_->declare_parameter(resolved_name, rclcpp::ParameterValue(value)); } - plugin_node_->set_parameter(rclcpp::Parameter(name, value)); + plugin_node_->set_parameter(rclcpp::Parameter(resolved_name, value)); } /// Subscribes to `topic` on plugin_node_ (matching the publisher's node) and spins until a diff --git a/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp b/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp new file mode 100644 index 00000000..78a314e2 --- /dev/null +++ b/mujoco_ros2_control_plugins/test/test_fts_grav_comp_plugin.cpp @@ -0,0 +1,372 @@ +/** + * Copyright (c) 2026, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * This software is 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. + */ + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "fts_grav_comp_plugin.hpp" + +namespace +{ +// A simple representative FTS testing suite +// Note here that the FTS is attached to the child body, which agrees with +// the standard that the FTS is attached to the child body +constexpr const char* kMjcf = R"( + + +)"; +} // namespace + +class FtsGravCompPluginTest : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestCase() + { + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } + } + + void SetUp() override + { + node_ = std::make_shared("fts_grav_comp_test_node"); + plugin_node_ = node_->create_sub_node("fts_grav_comp_plugin"); + + // Use two executor threads so a blocking service callback (sleeping for + // its wrench duration) does not prevent other callbacks (e.g. subscription + // delivery) from being dispatched. + executor_ = std::make_unique(); + executor_->add_node(node_); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + char error[1024] = { 0 }; + mjSpec* spec = mj_parseXMLString(kMjcf, nullptr, error, sizeof(error)); + ASSERT_NE(spec, nullptr) << error; + + model_ = mj_compile(spec, nullptr); + if (model_ == nullptr) + { + const char* ce = mjs_getError(spec); + mj_deleteSpec(spec); + FAIL() << (ce ? ce : "mj_compile failed"); + } + mj_deleteSpec(spec); + + data_ = mj_makeData(model_); + ASSERT_NE(data_, nullptr); + mj_forward(model_, data_); + } + + void TearDown() override + { + executor_->cancel(); + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } + executor_.reset(); + plugin_node_.reset(); + node_.reset(); + mj_deleteData(data_); + data_ = nullptr; + mj_deleteModel(model_); + model_ = nullptr; + } + + /// Pre-declares and sets a parameter on plugin_node_ before init(), matching how launch + /// parameter files set plugin parameters ahead of load_mujoco_plugins(). + template + void setParam(const std::string& name, const T& value) + { + const std::string resolved_name = "mujoco_plugins." + plugin_node_->get_sub_namespace() + "." + name; + if (!plugin_node_->has_parameter(resolved_name)) + { + plugin_node_->declare_parameter(resolved_name, rclcpp::ParameterValue(value)); + } + plugin_node_->set_parameter(rclcpp::Parameter(resolved_name, value)); + } + + /// sets parameters for the plugin + /// fts_grav_comp_plugin: + /// fts_sensor: + /// # frame the CoG is represented in + /// frame_id: ft_sensor_site + /// # specifies the center of gravity of the end effector + /// CoG: + /// pos: + /// - 0.1 # x + /// - 0.0 # y + /// - 0.0 # z + /// mass: 10.0 # mass + /// or_fts_sensor: + /// # frame the CoG is represented in + /// frame_id: offset_rotated_site + /// # specifies the center of gravity of the end effector + /// CoG: + /// pos: + /// - 0.0 # x + /// - 0.0 # y + /// - 0.05 # z + /// mass: 10.0 # mass + void set_plugin_params() + { + setParam("fts_sensor.frame_id", std::string("ft_sensor_site")); + setParam("fts_sensor.CoG.pos", std::vector{ 0.1, 0.0, 0.0 }); + setParam("fts_sensor.CoG.mass", double{ 10.0 }); + + setParam("or_fts_sensor.frame_id", std::string("offset_rotated_site")); + setParam("or_fts_sensor.CoG.pos", std::vector{ 0.0, 0.0, 0.05 }); + setParam("or_fts_sensor.CoG.mass", double{ 10.0 }); + } + + std::vector get_ft_data(mujoco_ros2_control_plugins::FtsData fts) + { + std::vector ft_data(6, 0); + const mjtNum* sensordata_force = data_->sensordata + fts.sensor_adr_force; + const mjtNum* sensordata_torque = data_->sensordata + fts.sensor_adr_torque; + // everything must be negated to handle the way force and torque sensors are set up + ft_data[0] = -sensordata_force[0]; // force x + ft_data[1] = -sensordata_force[1]; // force y + ft_data[2] = -sensordata_force[2]; // force z + ft_data[3] = -sensordata_torque[0]; // torque x + ft_data[4] = -sensordata_torque[1]; // torque y + ft_data[5] = -sensordata_torque[2]; // torque z + + return ft_data; + } + + mjModel* model_{ nullptr }; + mjData* data_{ nullptr }; + rclcpp::Node::SharedPtr node_; + rclcpp::Node::SharedPtr plugin_node_; + +private: + std::unique_ptr executor_; + std::thread spin_thread_; +}; + +// can properly load the MJCF and initialize the plugin +TEST_F(FtsGravCompPluginTest, InitSucceeds) +{ + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + EXPECT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.cleanup(); +} + +// reads in the plugin and verifies that the sensors loaded were correct +TEST_F(FtsGravCompPluginTest, SensorsLoaded) +{ + set_plugin_params(); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + + ASSERT_EQ(plugin.get_fts_data().size(), 2) << "Two sensors should have been loaded"; + EXPECT_EQ(plugin.get_fts_data()[0].sensor_name, "fts_sensor"); + EXPECT_EQ(plugin.get_fts_data()[1].sensor_name, "or_fts_sensor"); + plugin.cleanup(); +} + +// verifies that initialization fails if a site id is wrong +TEST_F(FtsGravCompPluginTest, FailOnNonExistentSite) +{ + set_plugin_params(); + setParam("fts_sensor.frame_id", std::string("non_existent_site")); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_FALSE(plugin.init(plugin_node_, model_, data_)); + + plugin.cleanup(); +} + +// verifies that initialization fails if the name of the sensor is wrong +TEST_F(FtsGravCompPluginTest, FailOnNonExistentSensor) +{ + set_plugin_params(); + setParam("nonexistent_fts_sensor.frame_id", std::string("ft_sensor_site")); + setParam("nonexistent_fts_sensor.CoG.pos", std::vector{ 0.1, 0.0, 0.0 }); + setParam("nonexistent_fts_sensor.CoG.mass", double{ 10.0 }); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_FALSE(plugin.init(plugin_node_, model_, data_)); + + plugin.cleanup(); +} + +// verifies that the final wrench data is 0 after gravity compensation +TEST_F(FtsGravCompPluginTest, GravityIsComped) +{ + set_plugin_params(); + + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // expected that the FT data should be 0 after gravity compensation + std::vector expected_data = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_0[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_0[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_0[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_0[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_0[5], expected_data[5], 1e-9); + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_1[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_1[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_1[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_1[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_1[5], expected_data[5], 1e-9); + + plugin.cleanup(); +} + +// verifies that the the same data sees the real weight after gravity compensation mass is 0 +TEST_F(FtsGravCompPluginTest, GravityIsNotComped) +{ + set_plugin_params(); + setParam("fts_sensor.CoG.mass", double{ 0.0 }); + setParam("or_fts_sensor.CoG.mass", double{ 0.0 }); + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // force FTS 0 (negative z direction) + // mass * gravity + double force_0 = 10.0 * -9.81; + // torque FTS 1 (positive y direction) + // mass * gravity * distance + double torque_0 = 10.0 * 9.81 * 0.1; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[1], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[2], force_0, 1e-9); + EXPECT_NEAR(real_data_0[3], 0.0, 1e-9); + EXPECT_NEAR(real_data_0[4], torque_0, 1e-9); + EXPECT_NEAR(real_data_0[5], 0.0, 1e-9); + + // force FTS 0 (positive x direction) + // mass * gravity + double force_1 = 10.0 * 9.81; + // torque FTS 1 (positive y direction) + // mass * gravity * distance + double torque_1 = 10.0 * 9.81 * 0.05; + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], force_1, 1e-9); + EXPECT_NEAR(real_data_1[1], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[2], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[3], 0.0, 1e-9); + EXPECT_NEAR(real_data_1[4], torque_1, 1e-9); + EXPECT_NEAR(real_data_1[5], 0.0, 1e-9); + + plugin.cleanup(); +} + +// verifies that the final wrench data is 0 after gravity compensation +TEST_F(FtsGravCompPluginTest, GravityIsCompedWithFrameIdDifferentFromSensorId) +{ + set_plugin_params(); + setParam("fts_sensor.frame_id", std::string("offset_site")); + setParam("fts_sensor.CoG.pos", std::vector{ 0.1, -0.1, 0.0 }); + setParam("or_fts_sensor.frame_id", std::string("offset_site")); + setParam("or_fts_sensor.CoG.pos", std::vector{ 0.1, -0.1, 0.0 }); + + mujoco_ros2_control_plugins::FtsGravCompPlugin plugin; + ASSERT_TRUE(plugin.init(plugin_node_, model_, data_)); + plugin.update(model_, data_); + + // expected that the FT data should be 0 after gravity compensation + std::vector expected_data = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // check the first FT sensor + std::vector real_data_0 = get_ft_data(plugin.get_fts_data()[0]); + EXPECT_NEAR(real_data_0[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_0[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_0[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_0[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_0[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_0[5], expected_data[5], 1e-9); + + // check the second FT sensor + std::vector real_data_1 = get_ft_data(plugin.get_fts_data()[1]); + EXPECT_NEAR(real_data_1[0], expected_data[0], 1e-9); + EXPECT_NEAR(real_data_1[1], expected_data[1], 1e-9); + EXPECT_NEAR(real_data_1[2], expected_data[2], 1e-9); + EXPECT_NEAR(real_data_1[3], expected_data[3], 1e-9); + EXPECT_NEAR(real_data_1[4], expected_data[4], 1e-9); + EXPECT_NEAR(real_data_1[5], expected_data[5], 1e-9); + + plugin.cleanup(); +} diff --git a/mujoco_ros2_control_tests/CHANGELOG.rst b/mujoco_ros2_control_tests/CHANGELOG.rst index 9e9d152d..e5502e16 100644 --- a/mujoco_ros2_control_tests/CHANGELOG.rst +++ b/mujoco_ros2_control_tests/CHANGELOG.rst @@ -2,6 +2,36 @@ Changelog for package mujoco_ros2_control_tests ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +0.1.2 (2026-09-07) +------------------ +* Add world_reset plugin hook and restore eq_active on world reset (`#294 `_) +* Contributors: Bilal Gill, Erik Holum + +0.1.1 (2026-08-25) +------------------ +* Fix compilation on MuJoCo 3.12.0 (`#290 `_) +* Fix the generate changelog of 0.1.0 (`#288 `_) +* Contributors: Sai Kishor Kothakota + +0.1.0 (2026-08-18) +------------------ +* Add support for Magnetometer sensor (`#257 `_) +* Add missing dependencies (`#250 `_) +* Fix transmissions integration working with multiple actuators (`#241 `_) +* Add service to set the free joint body state (`#233 `_) +* Add pose sensor test to launch tests (`#232 `_) +* Switch to isolated launch tests (`#234 `_) +* remove shebangs from launch files (`#227 `_) +* Add a 3-D Lidar Extension (`#205 `_) +* Add support for "site" transmission type (`#154 `_) +* Add support for pose sensors (`#220 `_) +* fixing plugin export so that downstream plugins don't fail (`#215 `_) +* Re-add descoped ResetWorld functional test (`#206 `_) +* Isolate flakey functional tests and update pixi (`#204 `_) +* Support/cameras rendering/headless (`#197 `_) +* Increase timeout for mjcf_generation* tests (`#201 `_) +* Contributors: Christian Rauch, Christoph Fröhlich, Erik Holum, Nathan Dunkelberger, Sai Kishor Kothakota, Sebastian Castro, msavchen-nasa + 0.0.3 (2026-05-01) ------------------ * Switch to forward_command_controller dependency (`#173 `_) diff --git a/mujoco_ros2_control_tests/CMakeLists.txt b/mujoco_ros2_control_tests/CMakeLists.txt index 04e767b0..b8343dc6 100644 --- a/mujoco_ros2_control_tests/CMakeLists.txt +++ b/mujoco_ros2_control_tests/CMakeLists.txt @@ -9,6 +9,9 @@ if(BUILD_TESTING) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) + find_package(std_msgs REQUIRED) + find_package(mujoco_ros2_control REQUIRED) + find_package(mujoco_ros2_control_msgs REQUIRED) find_package(mujoco_ros2_control_plugins REQUIRED) # Compile a test plugin alongside base plugins to make sure it works. @@ -16,6 +19,7 @@ if(BUILD_TESTING) target_link_libraries(test_plugin ${mujoco_ros2_control_plugins_TARGETS} pluginlib::pluginlib + ${std_msgs_TARGETS} ) install(TARGETS test_plugin @@ -26,6 +30,16 @@ if(BUILD_TESTING) mujoco_ros2_control_plugins src/test_plugin.xml ) + # End-to-end check that reset_world dispatches on_reset() to loaded plugins. + ament_add_gtest(test_world_reset_dispatch test/test_world_reset_dispatch.cpp) + target_link_libraries(test_world_reset_dispatch + ${mujoco_ros2_control_TARGETS} + ${mujoco_ros2_control_msgs_TARGETS} + ${std_msgs_TARGETS} + rclcpp::rclcpp + hardware_interface::hardware_interface + ) + install( DIRECTORY launch config test_resources DESTINATION share/${PROJECT_NAME} diff --git a/mujoco_ros2_control_tests/src/test_plugin.cpp b/mujoco_ros2_control_tests/src/test_plugin.cpp index aa910fd2..bbcfbaad 100644 --- a/mujoco_ros2_control_tests/src/test_plugin.cpp +++ b/mujoco_ros2_control_tests/src/test_plugin.cpp @@ -1,4 +1,5 @@ #include +#include #include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp" namespace external_plugin_test @@ -7,16 +8,30 @@ namespace external_plugin_test class TestPlugin : public mujoco_ros2_control_plugins::MuJoCoROS2ControlPluginBase { public: - bool init(rclcpp::Node::SharedPtr, const mjModel*, mjData*) override + bool init(rclcpp::Node::SharedPtr node, const mjModel*, mjData*) override { + // Latched so a test subscribing after a reset still observes the count. + world_reset_count_pub_ = + node->create_publisher("world_reset_count", rclcpp::QoS(1).transient_local()); return true; } void update(const mjModel*, mjData*) override { } + void on_reset(mjData*) override + { + std_msgs::msg::UInt32 msg; + msg.data = ++world_reset_count_; + world_reset_count_pub_->publish(msg); + } void cleanup() override { + world_reset_count_pub_.reset(); } + +private: + rclcpp::Publisher::SharedPtr world_reset_count_pub_; + uint32_t world_reset_count_{ 0 }; }; } // namespace external_plugin_test diff --git a/mujoco_ros2_control_tests/test/robot_launch_test.py b/mujoco_ros2_control_tests/test/robot_launch_test.py index 8295c3f7..c0ecb7ed 100644 --- a/mujoco_ros2_control_tests/test/robot_launch_test.py +++ b/mujoco_ros2_control_tests/test/robot_launch_test.py @@ -17,10 +17,11 @@ # limitations under the License. import os +import re import time import unittest -from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_prefix, get_package_share_directory from controller_manager.test_utils import check_controllers_running, check_if_js_published, check_node_running from launch import LaunchDescription from launch.actions import IncludeLaunchDescription @@ -40,6 +41,18 @@ from controller_manager_msgs.srv import ListHardwareInterfaces, SwitchController +def get_mujoco_version_header(): + """Return the mjVERSION_HEADER integer (e.g. 3012000 for MuJoCo 3.12.0) of the + MuJoCo build that mujoco_vendor actually installed/linked, read from its + installed mujoco.h. + """ + prefix = get_package_prefix("mujoco_vendor") + header_path = os.path.join(prefix, "opt", "mujoco_vendor", "include", "mujoco", "mujoco.h") + with open(header_path) as header_file: + match = re.search(r"#define\s+mjVERSION_HEADER\s+(\d+)", header_file.read()) + return int(match.group(1)) + + # This function specifies the processes to be run for our test def generate_test_description_common(use_pid="false", use_mjcf_from_topic="false", test_transmissions="false"): # This is necessary to get unbuffered output from the process under test @@ -248,10 +261,16 @@ def test_pose_interfaces_transform(self): self.skipTest("pose_broadcaster is only spawned in the basic robot configuration") # The settled contact pose differs between MuJoCo versions: the ROS binaries and pixi/conda environment - # ships different versions of libmujoco. Both are deterministic, so keep one exact expected pose per - # environment instead of a tolerance loose enough to span the gap between them. + # can ship different versions of libmujoco. Both are deterministic, so keep one exact expected pose per + # MuJoCo version instead of a tolerance loose enough to span the gap between them. # See https://github.com/pal-robotics/mujoco_vendor/issues/11 for more details. - if os.environ.get("PIXI_PROJECT_ROOT") or os.environ.get("CONDA_PREFIX"): + # MuJoCo 3.4.0 (mjVERSION_HEADER == 3004000) is the only version known to settle to the older pose below; + # every later version observed so far (including the 3.12.0 bump) settles to the newer one. + if ( + os.environ.get("PIXI_PROJECT_ROOT") + or os.environ.get("CONDA_PREFIX") + or get_mujoco_version_header() > 3004000 + ): expected_pose = { "pose/position.x": 1.8753, "pose/position.y": 0.0, diff --git a/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp b/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp new file mode 100644 index 00000000..d7d64265 --- /dev/null +++ b/mujoco_ros2_control_tests/test/test_world_reset_dispatch.cpp @@ -0,0 +1,190 @@ +// Copyright 2026 Bilal Gill +// +// 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. + +// End-to-end check of the on_reset plugin hook: the out-of-package TestPlugin is loaded +// through the `mujoco_plugins` parameter namespace and must have its on_reset() called +// when the reset_world service resets the simulation. + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#define ROS_DISTRO_HUMBLE (HARDWARE_INTERFACE_VERSION_MAJOR < 3) + +namespace +{ + +constexpr const char* kTestModel = R"( + + +)"; + +// Loads TestPlugin into the hardware node via the wildcard parameter file passed through the +// `pids_config_file` hardware parameter (the node auto-declares parameters from overrides). +constexpr const char* kPluginParams = R"(/**: + ros__parameters: + mujoco_plugins: + test_plugin: + type: "external_plugin_test/TestPlugin" +)"; + +const std::string kTestModelPath = "/tmp/test_world_reset_dispatch_model.xml"; +const std::string kPluginParamsPath = "/tmp/test_world_reset_dispatch_params.yaml"; + +void write_file(const std::string& path, const char* content) +{ + std::ofstream file(path); + file << content; +} + +} // namespace + +class WorldResetDispatchTest : public ::testing::Test +{ +protected: + static void SetUpTestSuite() + { + if (!rclcpp::ok()) + { + rclcpp::init(0, nullptr); + } + } + + static void TearDownTestSuite() + { + if (rclcpp::ok()) + { + rclcpp::shutdown(); + } + } + + void SetUp() override + { + write_file(kTestModelPath, kTestModel); + write_file(kPluginParamsPath, kPluginParams); + interface_ = std::make_shared(); + + node_ = std::make_shared("world_reset_dispatch_test_node"); + executor_ = std::make_unique(); + executor_->add_node(node_); + spin_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + if (executor_) + { + executor_->cancel(); + } + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } + if (interface_) + { + rclcpp_lifecycle::State inactive_state(0, "inactive"); + interface_->on_deactivate(inactive_state); + interface_.reset(); + } + node_.reset(); + executor_.reset(); + + std::filesystem::remove(kTestModelPath); + std::filesystem::remove(kPluginParamsPath); + } + + hardware_interface::CallbackReturn initialize_interface() + { + hardware_interface::HardwareInfo info; + info.name = "test_world_reset_dispatch"; + info.type = "system"; + info.hardware_parameters["mujoco_model"] = kTestModelPath; + info.hardware_parameters["meshdir"] = ""; + info.hardware_parameters["headless"] = "true"; + info.hardware_parameters["disable_rendering"] = "true"; + info.hardware_parameters["pids_config_file"] = kPluginParamsPath; +#if ROS_DISTRO_HUMBLE + return interface_->on_init(info); +#else + hardware_interface::HardwareComponentInterfaceParams params; + params.hardware_info = info; + return interface_->on_init(params); +#endif + } + + bool wait_until(std::function condition, std::chrono::milliseconds timeout = std::chrono::seconds(5), + std::chrono::milliseconds poll_interval = std::chrono::milliseconds(10)) + { + const auto deadline = std::chrono::steady_clock::now() + timeout; + while (std::chrono::steady_clock::now() < deadline) + { + if (condition()) + return true; + std::this_thread::sleep_for(poll_interval); + } + return condition(); + } + + std::shared_ptr interface_; + rclcpp::Node::SharedPtr node_; + std::unique_ptr executor_; + std::thread spin_thread_; +}; + +TEST_F(WorldResetDispatchTest, ResetWorldServiceDispatchesWorldResetToPlugins) +{ + ASSERT_EQ(initialize_interface(), hardware_interface::CallbackReturn::SUCCESS); + + // TestPlugin publishes its on_reset call count (latched) from its plugin sub-namespace. + std::atomic world_reset_count{ 0 }; + auto count_sub = node_->create_subscription( + "/test_plugin/world_reset_count", rclcpp::QoS(1).transient_local(), + [&world_reset_count](const std_msgs::msg::UInt32& msg) { world_reset_count = msg.data; }); + + auto reset_client = + node_->create_client("/mujoco_ros2_control_node/reset_world"); + ASSERT_TRUE(reset_client->wait_for_service(std::chrono::seconds(5))) << "reset_world service not found"; + + // No reset yet: the plugin must not have been notified. + EXPECT_EQ(world_reset_count.load(), 0u); + + auto reset_future = + reset_client->async_send_request(std::make_shared()); + ASSERT_EQ(reset_future.wait_for(std::chrono::seconds(5)), std::future_status::ready); + ASSERT_TRUE(reset_future.get()->success); + + EXPECT_TRUE(wait_until([&]() { return world_reset_count.load() == 1u; })) + << "on_reset() was not dispatched to the loaded plugin on reset_world"; +}