├── resources ├── mujoco_logo.png └── scene.xml ├── .dockerignore ├── .gitattributes ├── test ├── config │ ├── start_positions.xml │ ├── controllers.yaml │ ├── mujoco_pid.yaml │ └── test_robot.rviz ├── CMakeLists.txt ├── test_resources │ ├── scene_info.xml │ ├── scene.xml │ ├── test_pid │ │ ├── scene_pid.xml │ │ └── test_robot_pid.xml │ ├── test_inputs.xml │ ├── test_robot.xml │ └── test_robot.urdf ├── src │ ├── robot_launch_mjcf_generation_from_robot_description_test.py │ ├── robot_launch_mjcf_generation_test.py │ ├── robot_launch_pid_test.py │ └── robot_launch_test.py ├── test_plugin.cpp └── launch │ └── test_robot.launch.py ├── .gitignore ├── colcon_defaults.yaml ├── mujoco_system_interface_plugin.xml ├── docker ├── entrypoint.sh └── Dockerfile ├── .github └── workflows │ ├── format.yml │ └── ci.yaml ├── package.xml ├── include └── mujoco_ros2_control │ ├── utils.hpp │ ├── data.hpp │ ├── mujoco_lidar.hpp │ ├── mujoco_cameras.hpp │ └── mujoco_system_interface.hpp ├── docker-compose.yml ├── pixi.toml ├── .clang-format ├── .pre-commit-config.yaml ├── docs ├── DEVELOPMENT.md └── TOOLS.md ├── scripts └── find_missing_inertias.py ├── src ├── mujoco_ros2_control_node.cpp ├── mujoco_cameras.cpp └── mujoco_lidar.cpp ├── CMakeLists.txt ├── LICENSE └── README.md /resources/mujoco_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-controls/mujoco_ros2_control/HEAD/resources/mujoco_logo.png -------------------------------------------------------------------------------- /.dockerignore: -------------------------------------------------------------------------------- 1 | .git/ 2 | .github 3 | .gitignore 4 | 5 | docker/Dockerfile 6 | docker-compose.yaml 7 | 8 | README.md 9 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # SCM syntax highlighting & preventing 3-way merges 2 | pixi.lock merge=binary linguist-language=YAML linguist-generated=true 3 | -------------------------------------------------------------------------------- /test/config/start_positions.xml: -------------------------------------------------------------------------------- 1 | 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Python 2 | *.pyc 3 | 4 | # Mujoco logs 5 | MUJOCO_LOG.TXT 6 | 7 | # pixi environments 8 | .pixi/* 9 | !.pixi/config.toml 10 | 11 | # Build output 12 | build/* 13 | install/* 14 | log/* 15 | 16 | # Cloned mujoco src 17 | mujoco/* 18 | -------------------------------------------------------------------------------- /colcon_defaults.yaml: -------------------------------------------------------------------------------- 1 | build: 2 | symlink-install: true 3 | mixin: 4 | - build-testing-on 5 | - compile-commands 6 | - mold 7 | - ninja 8 | - rel-with-deb-info 9 | - sccache 10 | event-handlers: 11 | - console_direct+ 12 | -------------------------------------------------------------------------------- /test/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 4 | 5 | joint_state_broadcaster: 6 | type: joint_state_broadcaster/JointStateBroadcaster 7 | 8 | position_controller: 9 | type: position_controllers/JointGroupPositionController 10 | 11 | position_controller: 12 | ros__parameters: 13 | joints: 14 | - joint1 15 | - joint2 16 | -------------------------------------------------------------------------------- /mujoco_system_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | Hardware interface that wraps a MuJoCo model simulation, based on upstream simulate libraries. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /docker/entrypoint.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # shellcheck disable=SC1090 4 | source "/opt/ros/${ROS_DISTRO}/setup.bash" 5 | 6 | if [ -f "${ROS_WS}/install/setup.bash" ]; then 7 | echo "Sourcing ${ROS_WS}/install/setup.bash" 8 | # shellcheck disable=SC1091 9 | source "${ROS_WS}/install/setup.bash" 10 | else 11 | echo "The ${ROS_WS} workspace is not yet built." 12 | echo "To build:" 13 | echo " cd ${ROS_WS}" 14 | echo " colcon build" 15 | fi 16 | 17 | # Execute the command passed into this entrypoint 18 | exec "$@" 19 | -------------------------------------------------------------------------------- /.github/workflows/format.yml: -------------------------------------------------------------------------------- 1 | name: format 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | branches: 8 | - humble 9 | - jazzy 10 | - '*-devel' 11 | pull_request: 12 | branches: 13 | - '*' 14 | workflow_dispatch: 15 | 16 | # Do not run multiple jobs for the same branch 17 | concurrency: 18 | group: ${{ github.workflow }}-${{ github.ref }} 19 | cancel-in-progress: true 20 | 21 | jobs: 22 | pre-commit: 23 | name: pre-commit 24 | runs-on: ubuntu-latest 25 | steps: 26 | - uses: actions/checkout@v4 27 | - uses: actions/setup-python@v5 28 | - uses: pre-commit/action@v3.0.1 29 | -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_gtest REQUIRED) 2 | 3 | install( 4 | DIRECTORY launch test_resources config 5 | DESTINATION share/${PROJECT_NAME} 6 | ) 7 | 8 | ament_add_gtest(test_plugin test_plugin.cpp) 9 | target_link_libraries(test_plugin 10 | mujoco_ros2_control 11 | rclcpp::rclcpp 12 | ) 13 | 14 | find_package(launch_testing_ament_cmake REQUIRED) 15 | 16 | add_launch_test(src/robot_launch_test.py 17 | TIMEOUT 50 18 | ) 19 | 20 | add_launch_test(src/robot_launch_pid_test.py 21 | TIMEOUT 50 22 | ) 23 | 24 | add_launch_test(src/robot_launch_mjcf_generation_test.py 25 | TIMEOUT 50 26 | ) 27 | 28 | add_launch_test(src/robot_launch_mjcf_generation_from_robot_description_test.py 29 | TIMEOUT 50 30 | ) 31 | -------------------------------------------------------------------------------- /test/test_resources/scene_info.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /test/test_resources/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /resources/scene.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /test/test_resources/test_pid/scene_pid.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /test/config/mujoco_pid.yaml: -------------------------------------------------------------------------------- 1 | /**: 2 | ros__parameters: 3 | pid_gains: 4 | position: 5 | joint1: 6 | p: 100.0 7 | i: 0.0 8 | d: 30.0 9 | u_clamp_max: 100.0 10 | u_clamp_min: -100.0 11 | i_clamp_max: 10.0 12 | i_clamp_min: -10.0 13 | joint2: 14 | p: 100.0 15 | i: 0.0 16 | d: 30.0 17 | u_clamp_max: 100.0 18 | u_clamp_min: -100.0 19 | i_clamp_max: 10.0 20 | i_clamp_min: -10.0 21 | pid_gains: 22 | velocity: 23 | joint1: 24 | p: 30.0 25 | i: 0.1 26 | d: 1.0 27 | u_clamp_max: 100.0 28 | u_clamp_min: -100.0 29 | i_clamp_max: 10.0 30 | i_clamp_min: -10.0 31 | joint2: 32 | p: 30.0 33 | i: 0.1 34 | d: 1.0 35 | u_clamp_max: 100.0 36 | u_clamp_min: -100.0 37 | i_clamp_max: 10.0 38 | i_clamp_min: -10.0 39 | -------------------------------------------------------------------------------- /test/src/robot_launch_mjcf_generation_from_robot_description_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2025 PAL Robotics S.L. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import sys 18 | import os 19 | import pytest 20 | 21 | 22 | sys.path.append(os.path.dirname(__file__)) # noqa: E402 23 | 24 | from robot_launch_test import generate_test_description_common # noqa: E402 25 | from robot_launch_pid_test import TestFixtureHardwareInterfacesCheck # noqa: F401, E402 26 | from robot_launch_test import TestFixture, TestMJCFGenerationFromURDF # noqa: F401, E402 27 | 28 | 29 | @pytest.mark.rostest 30 | def generate_test_description(): 31 | return generate_test_description_common(use_pid="true", use_mjcf_from_topic="true") 32 | -------------------------------------------------------------------------------- /test/src/robot_launch_mjcf_generation_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2025 PAL Robotics S.L. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import sys 18 | import os 19 | import pytest 20 | 21 | 22 | sys.path.append(os.path.dirname(__file__)) # noqa: E402 23 | 24 | from robot_launch_test import generate_test_description_common # noqa: E402 25 | from robot_launch_test import TestFixture # noqa: F401, E402 26 | from robot_launch_test import TestFixtureHardwareInterfacesCheck # noqa: F401, E402 27 | from robot_launch_test import TestMJCFGenerationFromURDF # noqa: F401, E402 28 | 29 | 30 | @pytest.mark.rostest 31 | def generate_test_description(): 32 | return generate_test_description_common(use_pid="false", use_mjcf_from_topic="true") 33 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | mujoco_ros2_control 5 | 0.0.0 6 | ros2_control wrapper for the Mujoco Simulate application 7 | Nathan Dunkelberger 8 | Erik Holum 9 | Apache License, Version 2.0 10 | 11 | ament_cmake 12 | 13 | controller_manager 14 | hardware_interface 15 | libglfw3-dev 16 | pluginlib 17 | rclcpp 18 | rclcpp_lifecycle 19 | sensor_msgs 20 | control_toolbox 21 | 22 | joint_state_broadcaster 23 | position_controllers 24 | rviz2 25 | urdfdom 26 | urdfdom_py 27 | xacro 28 | 29 | python3-mujoco-pip 30 | python3-trimesh-pip 31 | python3-obj2mjcf-pip 32 | 33 | 34 | ament_cmake 35 | 36 | 37 | -------------------------------------------------------------------------------- /include/mujoco_ros2_control/utils.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | 24 | namespace mujoco_ros2_control 25 | { 26 | 27 | /** 28 | * @brief Returns the sensor's component info for the provided sensor name, if it exists. 29 | */ 30 | inline std::optional 31 | get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, const std::string& name) 32 | { 33 | for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++) 34 | { 35 | const auto& sensor = hardware_info.sensors.at(sensor_index); 36 | if (hardware_info.sensors.at(sensor_index).name == name) 37 | { 38 | return sensor; 39 | } 40 | } 41 | return std::nullopt; 42 | } 43 | 44 | } // namespace mujoco_ros2_control 45 | -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | services: 2 | dev: 3 | build: 4 | context: . 5 | dockerfile: docker/Dockerfile 6 | target: mujoco_ros 7 | tags: 8 | - mujoco_ros:${ROS_DISTRO:-jazzy} 9 | args: 10 | - ROS_DISTRO=${ROS_DISTRO:-jazzy} 11 | - CPU_ARCH=${CPU_ARCH:-x86_64} 12 | # Ensures signals are actually passed and reaped in the container for shutdowns 13 | # https://docs.docker.com/compose/compose-file/compose-file-v3/#init 14 | init: true 15 | stdin_open: true 16 | tty: true 17 | network_mode: host 18 | # Allows graphical programs in the container. 19 | environment: 20 | - DISPLAY 21 | - QT_X11_NO_MITSHM=1 22 | # Pass the ROS_DOMAIN_ID from the host, if available, otherwise we default to 1 to avoid spamming networks 23 | - ROS_DOMAIN_ID=${ROS_DOMAIN_ID:-1} 24 | # Shared graphics between a host and a container are difficult. Users with GPUs may need to uncomment the 25 | # lines below to enable GPU rendering in the container. This connfiguration does not work across all 26 | # environments, and users may need to tweak their individual setups. 27 | # - NVIDIA_DRIVER_CAPABILITIES=all 28 | # deploy: 29 | # resources: 30 | # reservations: 31 | # devices: 32 | # - driver: nvidia 33 | # count: 1 # alternatively, use `count: all` for all GPUs 34 | # capabilities: [ gpu ] 35 | volumes: 36 | # Mount the source code. 37 | - ./:/opt/mujoco/ws/src/mujoco_ros2_control:rw 38 | # Allows graphical programs in the container. 39 | - x11_sock:/tmp/.X11-unix 40 | - /tmp/.X11-unix:/tmp/.X11-unix:rw 41 | - ${XAUTHORITY:-$HOME/.Xauthority}:/root/.Xauthority 42 | command: sleep infinity 43 | -------------------------------------------------------------------------------- /pixi.toml: -------------------------------------------------------------------------------- 1 | [workspace] 2 | authors = ["Erik Holum "] 3 | channels = ["conda-forge", "https://prefix.dev/robostack-jazzy"] 4 | name = "mujoco_ros2_control" 5 | platforms = ["linux-aarch64", "linux-64"] 6 | version = "0.1.0" 7 | 8 | [activation.env] 9 | CMAKE_INSTALL_MODE = "SYMLINK" 10 | COLCON_DEFAULT_OUTPUT_STYLE = "catkin_tools" 11 | 12 | # Ensures the linker knows where to look for conda managed packages 13 | LIBRARY_PATH = "${CONDA_PREFIX}/lib:${LIBRARY_PATH}" 14 | LD_LIBRARY_PATH = "${CONDA_PREFIX}/lib:${LD_LIBRARY_PATH}" 15 | LDFLAGS = "-L${CONDA_PREFIX}/lib -Wl,-rpath,${CONDA_PREFIX}/lib" 16 | CXXFLAGS = "-fuse-ld=mold" 17 | 18 | # The default location for mujoco 19 | MUJOCO_DIR="${PIXI_PROJECT_ROOT}/mujoco/mujoco-3.4.0" 20 | 21 | [tasks] 22 | # Setup tasks 23 | setup-colcon = { cmd = """ 24 | colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml && \ 25 | colcon mixin update default 26 | """ } 27 | 28 | # Build tasks 29 | build = "colcon build" 30 | clean = "rm -rf build install log" 31 | 32 | # Test tasks 33 | test = "colcon test" 34 | test-result = "colcon test-result --verbose" 35 | 36 | [dependencies] 37 | colcon-common-extensions = ">=0.3.0,<0.4" 38 | colcon-mixin = ">=0.2.3,<0.3" 39 | cmake = ">=4.1.2,<5" 40 | ninja = ">=1.13.1,<2" 41 | sccache = ">=0.11.0,<0.12" 42 | mold = ">=2.40.2,<3" 43 | libcap = ">=2.76,<3" 44 | glfw = ">=3.4,<4" 45 | filelock = ">=3.20.0,<4" 46 | trimesh = ">=4.10.1" 47 | pip = "*" 48 | 49 | # ROS dependencies 50 | ros-jazzy-ros-base = "*" 51 | ros-jazzy-ament-cmake = "*" 52 | ros-jazzy-rclcpp = "*" 53 | ros-jazzy-controller-manager = "*" 54 | ros-jazzy-control-toolbox = "*" 55 | ros-jazzy-hardware-interface = "*" 56 | ros-jazzy-joint-state-broadcaster = "*" 57 | ros-jazzy-position-controllers = "*" 58 | ros-jazzy-sensor-msgs = "*" 59 | ros-jazzy-urdfdom = "*" 60 | ros-jazzy-urdfdom-py = "*" 61 | ros-jazzy-rviz2 = "*" 62 | 63 | [pypi-dependencies] 64 | xacro = ">=2.1.1, <3" 65 | obj2mjcf = ">=0.0.25" 66 | mujoco = "==3.4.0" 67 | -------------------------------------------------------------------------------- /test/test_plugin.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | class MujocoSystemInterfaceLoadingTest : public ::testing::Test 27 | { 28 | protected: 29 | void SetUp() override 30 | { 31 | if (!rclcpp::ok()) 32 | { 33 | rclcpp::init(0, nullptr); 34 | } 35 | plugin_loader_ = std::make_unique>( 36 | "hardware_interface", "hardware_interface::SystemInterface"); 37 | } 38 | 39 | void TearDown() override 40 | { 41 | plugin_loader_.reset(); 42 | rclcpp::shutdown(); 43 | } 44 | 45 | std::unique_ptr> plugin_loader_; 46 | }; 47 | 48 | TEST_F(MujocoSystemInterfaceLoadingTest, test_load_plugin) 49 | { 50 | const std::string class_name = "mujoco_ros2_control/MujocoSystemInterface"; 51 | 52 | // Pluginlib should find the class 53 | ASSERT_TRUE(plugin_loader_->isClassAvailable(class_name)); 54 | 55 | // Construct a sim interface and verify it is not null 56 | std::shared_ptr mujoco_sim = plugin_loader_->createSharedInstance(class_name); 57 | EXPECT_NE(mujoco_sim, nullptr); 58 | } 59 | 60 | int main(int argc, char** argv) 61 | { 62 | ::testing::InitGoogleTest(&argc, argv); 63 | int result = RUN_ALL_TESTS(); 64 | return result; 65 | } 66 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | MaxEmptyLinesToKeep: 1 5 | 6 | # Allow us to adhere to https://google.github.io/styleguide/cppguide.html#Names_and_Order_of_Includes 7 | IncludeBlocks: Preserve 8 | SortIncludes: true 9 | 10 | Standard: Auto 11 | IndentWidth: 2 12 | TabWidth: 2 13 | UseTab: Never 14 | AccessModifierOffset: -2 15 | ConstructorInitializerIndentWidth: 2 16 | NamespaceIndentation: None 17 | ContinuationIndentWidth: 4 18 | IndentCaseLabels: true 19 | IndentFunctionDeclarationAfterType: false 20 | 21 | AlignEscapedNewlinesLeft: false 22 | AlignTrailingComments: true 23 | 24 | AllowAllParametersOfDeclarationOnNextLine: false 25 | ExperimentalAutoDetectBinPacking: false 26 | ObjCSpaceBeforeProtocolList: true 27 | Cpp11BracedListStyle: false 28 | 29 | AllowShortBlocksOnASingleLine: true 30 | AllowShortIfStatementsOnASingleLine: false 31 | AllowShortLoopsOnASingleLine: false 32 | AllowShortFunctionsOnASingleLine: None 33 | AllowShortCaseLabelsOnASingleLine: false 34 | 35 | AlwaysBreakTemplateDeclarations: true 36 | AlwaysBreakBeforeMultilineStrings: false 37 | BreakBeforeBinaryOperators: false 38 | BreakBeforeTernaryOperators: false 39 | BreakConstructorInitializersBeforeComma: true 40 | 41 | BinPackParameters: true 42 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 43 | DerivePointerBinding: false 44 | PointerBindsToType: true 45 | 46 | PenaltyExcessCharacter: 50 47 | PenaltyBreakBeforeFirstCallParameter: 30 48 | PenaltyBreakComment: 1000 49 | PenaltyBreakFirstLessLess: 10 50 | PenaltyBreakString: 100 51 | PenaltyReturnTypeOnItsOwnLine: 50 52 | 53 | SpacesBeforeTrailingComments: 2 54 | SpacesInParentheses: false 55 | SpacesInAngles: false 56 | SpaceInEmptyParentheses: false 57 | SpacesInCStyleCastParentheses: false 58 | SpaceAfterCStyleCast: false 59 | SpaceAfterControlStatementKeyword: true 60 | SpaceBeforeAssignmentOperators: true 61 | 62 | # Configure each individual brace in BraceWrapping 63 | BreakBeforeBraces: Custom 64 | 65 | # Control of individual brace wrapping cases 66 | BraceWrapping: 67 | AfterCaseLabel: true 68 | AfterClass: true 69 | AfterControlStatement: true 70 | AfterEnum: true 71 | AfterFunction: true 72 | AfterNamespace: true 73 | AfterStruct: true 74 | AfterUnion: true 75 | BeforeCatch: true 76 | BeforeElse: true 77 | IndentBraces: false 78 | -------------------------------------------------------------------------------- /test/test_resources/test_inputs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | 15 | repos: 16 | # Standard hooks 17 | - repo: https://github.com/pre-commit/pre-commit-hooks 18 | rev: v5.0.0 19 | hooks: 20 | - id: check-added-large-files 21 | exclude: 'pixi\.lock' 22 | - id: check-ast 23 | - id: check-case-conflict 24 | - id: check-docstring-first 25 | - id: check-merge-conflict 26 | - id: check-symlinks 27 | - id: check-xml 28 | - id: check-yaml 29 | # Allow for yaml constructors such as "!degrees" 30 | args: ["--unsafe"] 31 | - id: debug-statements 32 | - id: end-of-file-fixer 33 | exclude: (\.(svg|stl|dae))$ 34 | - id: mixed-line-ending 35 | - id: trailing-whitespace 36 | - id: fix-byte-order-marker 37 | 38 | # markdownlint hook 39 | - repo: https://github.com/igorshubovych/markdownlint-cli 40 | rev: v0.42.0 41 | hooks: 42 | - id: markdownlint 43 | # Disable max line length check for markdown 44 | args: ["--disable", "~MD013"] 45 | 46 | # shellcheck hook 47 | - repo: https://github.com/shellcheck-py/shellcheck-py 48 | rev: v0.10.0.1 49 | hooks: 50 | - id: shellcheck 51 | 52 | # Python hooks 53 | - repo: https://github.com/asottile/pyupgrade 54 | rev: v3.17.0 55 | hooks: 56 | - id: pyupgrade 57 | args: [--py36-plus] 58 | 59 | - repo: https://github.com/psf/black 60 | rev: 24.10.0 61 | hooks: 62 | - id: black 63 | args: ["--line-length=120"] 64 | 65 | - repo: https://github.com/pycqa/flake8 66 | rev: 7.1.1 67 | hooks: 68 | - id: flake8 69 | args: ['--max-line-length=120'] 70 | 71 | # CPP hooks 72 | - repo: https://github.com/pre-commit/mirrors-clang-format 73 | rev: v19.1.1 74 | hooks: 75 | - id: clang-format 76 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|m|proto|vert)$ 77 | # Copied in source so ignore it for now 78 | exclude: 'src/mujoco_ros2_control_node\.cpp' 79 | 80 | # TODO: Add ament checks for ROS packages, if required 81 | 82 | # Spellcheck in comments and docs 83 | - repo: https://github.com/codespell-project/codespell 84 | rev: v2.3.0 85 | hooks: 86 | - id: codespell 87 | exclude: '\.(svg|stl|dae)$|.*pixi.*' 88 | -------------------------------------------------------------------------------- /docs/DEVELOPMENT.md: -------------------------------------------------------------------------------- 1 | # Developers Guide 2 | 3 | This package can be built "normally" in a colcon workspace on any compatible system. 4 | However, we also include two workflows that enable developers to work in a completely isolated system environment. 5 | This ensure consistency with the supported workflows, and obviates the need to install any specific ROS, apt, or pip dependencies locally. 6 | 7 | ## Pixi Development Workflow 8 | 9 | A [pixi](https://pixi.sh/latest/installation/) and [robostack](https://robostack.github.io) workflow is also provided. 10 | The environment is currently only compatible with Jazzy. 11 | 12 | To run ensure pixi is installed. 13 | Then, 14 | 15 | ```bash 16 | # Setup the build environment 17 | pixi run setup-colcon 18 | 19 | # Build the package 20 | pixi run build 21 | 22 | # Run tests 23 | pixi run test 24 | ``` 25 | 26 | pixi also provides an interactive shell that sources the installed package environment. 27 | 28 | ```bash 29 | # Launch an interactive shell environment and run things as usual 30 | pixi shell 31 | 32 | # Build things as normal 33 | colcon build 34 | 35 | # And source and launch the test application 36 | source install/setup.bash 37 | ros2 launch mujoco_ros2_control test_robot.launch.py 38 | ``` 39 | 40 | For more information on pixi and ROS refer to the documentation or this excellent [blog post](https://jafarabdi.github.io/blog/2025/ros2-pixi-dev/). 41 | 42 | ## Docker Development Workflow 43 | 44 | This project includes a [compose](./../docker-compose.yml) and [Dockerfile](./../docker/Dockerfile) for development and testing in an isolated environment. 45 | 46 | > [!NOTE] 47 | > You may need to give docker access to xhost with `xhost +local:docker` to ensure the container has access to the host UI. 48 | 49 | For users on arm64 machines, be sure to specify the `CPU_ARCH` variable in your environment when building. 50 | 51 | ```bash 52 | docker compose build 53 | ``` 54 | 55 | The service can be started with: 56 | 57 | ```bash 58 | # Start the service in one shell (or start detached) 59 | docker compose up 60 | 61 | # Connect to it in another 62 | docker compose exec dev bash 63 | ``` 64 | 65 | This will launch a container with the source code mounted in a colcon workspace. 66 | From there the source can be modified, built, tested, or otherwise used as normal. 67 | For example, launch the included test scene with, 68 | 69 | ```bash 70 | # Evaluate using the included mujoco simulate application 71 | ${MUJOCO_DIR}/bin/simulate ${ROS_WS}/src/mujoco_ros2_control/test/test_resources/scene.xml 72 | 73 | # Or launch the test ROS control interface 74 | ros2 launch mujoco_ros2_control test_robot.launch.py 75 | ``` 76 | 77 | > [!NOTE] 78 | > Rendering contexts in containers can be tricky. 79 | 80 | Users may need to tweak the compose file to support their specific host OS or GPUs. 81 | For more information refer to the comments in the compose file. 82 | -------------------------------------------------------------------------------- /.github/workflows/ci.yaml: -------------------------------------------------------------------------------- 1 | name: build_and_test 2 | 3 | on: 4 | push: 5 | tags: 6 | - '*' 7 | branches: 8 | - main 9 | - humble 10 | - jazzy 11 | - '*-devel' 12 | pull_request: 13 | types: 14 | - opened 15 | - synchronize 16 | - reopened 17 | - ready_for_review 18 | branches: 19 | - '*' 20 | workflow_dispatch: 21 | 22 | # Do not run multiple jobs for the same branch 23 | concurrency: 24 | group: ${{ github.workflow }}-${{ github.ref }} 25 | cancel-in-progress: true 26 | 27 | jobs: 28 | build_and_test: 29 | timeout-minutes: 15 30 | strategy: 31 | matrix: 32 | ros_distro: [jazzy, kilted, rolling] 33 | runs-on: ubuntu-latest 34 | steps: 35 | - name: Checkout code 36 | uses: actions/checkout@v4 37 | - name: Set up Docker Buildx 38 | uses: docker/setup-buildx-action@v3 39 | - name: Set image tag name 40 | id: set_tag 41 | run: | 42 | github_ref_name="${{ github.head_ref || github.ref_name }}" 43 | image_tag=$(echo "$github_ref_name" | sed 's/\//-/g') 44 | echo "tag=${image_tag}" >> $GITHUB_OUTPUT 45 | - name: Build and push 46 | uses: docker/build-push-action@v5 47 | with: 48 | file: docker/Dockerfile 49 | tags: mujoco_ros2_control:${{ steps.set_tag.outputs.tag }} 50 | target: mujoco_ros 51 | build-args: | 52 | ROS_DISTRO=${{ matrix.ros_distro }} 53 | push: false 54 | load: true 55 | cache-from: type=gha,scope=ros-${{ matrix.ros_distro }} 56 | cache-to: type=gha,mode=max,scope=ros-${{ matrix.ros_distro }} 57 | - name: Run tests 58 | run: | 59 | docker run --rm \ 60 | mujoco_ros2_control:${{ steps.set_tag.outputs.tag }} \ 61 | bash -c " 62 | colcon test && 63 | colcon test-result --verbose 64 | " 65 | 66 | pixi_jazzy_build_and_test: 67 | timeout-minutes: 15 68 | env: 69 | SCCACHE_DIR: ${{ github.workspace }}/.sccache 70 | runs-on: ubuntu-latest 71 | steps: 72 | - name: Check out repository 73 | uses: actions/checkout@v4 74 | - name: Cache build directories and sccache 75 | uses: actions/cache@v4 76 | with: 77 | path: | 78 | ${{ env.SCCACHE_DIR }} 79 | key: ${{ runner.os }}-sccache-${{ github.head_ref || github.ref_name }} 80 | restore-keys: | 81 | ${{ runner.os }}-sccache-main 82 | ${{ runner.os }}-sccache- 83 | - uses: prefix-dev/setup-pixi@v0.8.14 84 | with: 85 | cache: true 86 | cache-write: ${{ github.event_name == 'push' && github.ref_name == 'main' }} 87 | frozen: true 88 | manifest-path: pixi.toml 89 | - name: Build and test 90 | run: | 91 | pixi run setup-colcon 92 | pixi run build 93 | pixi run test 94 | pixi run test-result 95 | -------------------------------------------------------------------------------- /test/test_resources/test_pid/test_robot_pid.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 52 | -------------------------------------------------------------------------------- /test/test_resources/test_robot.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 52 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=jazzy 2 | 3 | # This layer grabs package manifests from the src directory for preserving rosdep installs. 4 | # This can significantly speed up rebuilds for the base package when src contents have changed. 5 | FROM alpine:latest AS package-manifests 6 | 7 | # Copy in the src directory, then remove everything that isn't a manifest or an ignore file. 8 | COPY . /src/ 9 | RUN find /src -type f ! -name "package.xml" ! -name "COLCON_IGNORE" -delete && \ 10 | find /src -type d -empty -delete 11 | 12 | FROM ros:${ROS_DISTRO} AS mujoco_ros 13 | 14 | SHELL ["/bin/bash", "-o", "pipefail", "-c"] 15 | 16 | # Defaults for the MuJoCo install, CPU_ARCH may be set to aarch64 for arm cpus 17 | ARG MUJOCO_VERSION=3.4.0 18 | ARG CPU_ARCH=x86_64 19 | 20 | ARG DEBIAN_FRONTEND=noninteractive 21 | 22 | # Configure base dependencies 23 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 24 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 25 | apt-get update && \ 26 | apt-get install -y \ 27 | gdb \ 28 | libglfw3-dev \ 29 | libvirglrenderer1 \ 30 | libgl1-mesa-dri \ 31 | libglx-mesa0 \ 32 | libxi6 \ 33 | libxkbcommon0 \ 34 | mesa-utils \ 35 | python3-pip \ 36 | tmux \ 37 | wget \ 38 | xterm \ 39 | vim 40 | 41 | # Install extra python dependencies 42 | ENV PIP_BREAK_SYSTEM_PACKAGES=1 43 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 44 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 45 | pip install --ignore-installed mujoco obj2mjcf 46 | 47 | # There's no build for arm64 on linux, so just ignore failures here if that's the case 48 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 49 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 50 | pip install bpy==4.0.0 --extra-index-url https://download.blender.org/pypi/ || true 51 | 52 | # Configure and install MuJoCo 53 | ENV MUJOCO_VERSION=${MUJOCO_VERSION} 54 | ENV MUJOCO_DIR="/opt/mujoco/mujoco-${MUJOCO_VERSION}" 55 | ENV MUJOCO_INSTALL_DIR=${MUJOCO_DIR} 56 | RUN mkdir -p ${MUJOCO_DIR} 57 | 58 | RUN wget https://github.com/google-deepmind/mujoco/releases/download/${MUJOCO_VERSION}/mujoco-${MUJOCO_VERSION}-linux-${CPU_ARCH}.tar.gz 59 | RUN mkdir -p /home/mujoco 60 | RUN tar -xzf "mujoco-${MUJOCO_VERSION}-linux-${CPU_ARCH}.tar.gz" -C $(dirname "${MUJOCO_DIR}") 61 | 62 | # Setup a ROS workspace 63 | ENV ROS_WS="/opt/mujoco/ws" 64 | RUN mkdir -p ${ROS_WS}/src/mujoco_ros2_control 65 | WORKDIR ${ROS_WS} 66 | 67 | # Copy package manifests for installing rosdeps 68 | COPY --from=package-manifests /src/ ${ROS_WS}/src/mujoco_ros2_control 69 | 70 | RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ 71 | --mount=type=cache,target=/var/lib/apt,sharing=locked \ 72 | source /opt/ros/${ROS_DISTRO}/setup.bash && \ 73 | apt-get update && \ 74 | rosdep update && \ 75 | rosdep install -iry --from-paths src/ 76 | 77 | # Copy remaining source 78 | COPY . ${ROS_WS}/src/mujoco_ros2_control/ 79 | 80 | RUN source /opt/ros/${ROS_DISTRO}/setup.bash \ 81 | && colcon build \ 82 | --symlink-install \ 83 | --event-handlers console_direct+ \ 84 | --cmake-args "-DBUILD_TESTING=ON -DCMAKE_BUILD_TYPE=RelWithDebInfo" 85 | 86 | COPY docker/entrypoint.sh /entrypoint.sh 87 | RUN echo "source /entrypoint.sh" >> ~/.bashrc 88 | ENTRYPOINT ["/entrypoint.sh"] 89 | -------------------------------------------------------------------------------- /include/mujoco_ros2_control/data.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include "control_toolbox/pid_ros.hpp" 25 | 26 | #include 27 | #include 28 | 29 | namespace mujoco_ros2_control 30 | { 31 | 32 | /** 33 | * Maps to MuJoCo actuator types: 34 | * - MOTOR for MuJoCo motor actuator 35 | * - POSITION for MuJoCo position actuator 36 | * - VELOCITY for MuJoCo velocity actuator 37 | * - CUSTOM for MuJoCo general actuator or other types 38 | * 39 | * \note the MuJoCo types are as per the MuJoCo documentation: 40 | * https://mujoco.readthedocs.io/en/latest/XMLreference.html#actuator 41 | */ 42 | 43 | enum class ActuatorType 44 | { 45 | UNKNOWN, 46 | MOTOR, 47 | POSITION, 48 | VELOCITY, 49 | CUSTOM 50 | }; 51 | 52 | /** 53 | * Wrapper for mujoco actuators and relevant ROS HW interface data. 54 | */ 55 | struct JointState 56 | { 57 | std::string name; 58 | double position; 59 | double velocity; 60 | double effort; 61 | std::shared_ptr pos_pid{ nullptr }; 62 | std::shared_ptr vel_pid{ nullptr }; 63 | ActuatorType actuator_type{ ActuatorType::UNKNOWN }; 64 | double position_command; 65 | double velocity_command; 66 | double effort_command; 67 | bool is_mimic{ false }; 68 | int mimicked_joint_index; 69 | double mimic_multiplier; 70 | int mj_joint_type; 71 | int mj_pos_adr; 72 | int mj_vel_adr; 73 | int mj_actuator_id; 74 | 75 | // Booleans record whether or not we should be writing commands to these interfaces 76 | // based on if they have been claimed. 77 | bool is_position_control_enabled{ false }; 78 | bool is_position_pid_control_enabled{ false }; 79 | bool is_velocity_pid_control_enabled{ false }; 80 | bool is_velocity_control_enabled{ false }; 81 | bool is_effort_control_enabled{ false }; 82 | bool has_pos_pid{ false }; 83 | bool has_vel_pid{ false }; 84 | }; 85 | 86 | template 87 | struct SensorData 88 | { 89 | std::string name; 90 | T data; 91 | int mj_sensor_index; 92 | }; 93 | 94 | struct FTSensorData 95 | { 96 | std::string name; 97 | SensorData force; 98 | SensorData torque; 99 | }; 100 | 101 | struct IMUSensorData 102 | { 103 | std::string name; 104 | SensorData> orientation; 105 | SensorData angular_velocity; 106 | SensorData linear_acceleration; 107 | 108 | // These are currently unused but added to support controllers that require them. 109 | std::vector orientation_covariance; 110 | std::vector angular_velocity_covariance; 111 | std::vector linear_acceleration_covariance; 112 | }; 113 | 114 | } // namespace mujoco_ros2_control 115 | -------------------------------------------------------------------------------- /test/src/robot_launch_pid_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2025 PAL Robotics S.L. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import sys 18 | import os 19 | import rclpy 20 | import pytest 21 | import unittest 22 | 23 | from controller_manager_msgs.srv import ListHardwareInterfaces 24 | 25 | sys.path.append(os.path.dirname(__file__)) # noqa: E402 26 | 27 | from robot_launch_test import generate_test_description_common # noqa: E402 28 | from robot_launch_test import TestFixture # noqa: F401, E402 29 | 30 | 31 | @pytest.mark.rostest 32 | def generate_test_description(): 33 | return generate_test_description_common(use_pid="true") 34 | 35 | 36 | class TestFixtureHardwareInterfacesCheck(unittest.TestCase): 37 | 38 | @classmethod 39 | def setUpClass(cls): 40 | rclpy.init() 41 | 42 | @classmethod 43 | def tearDownClass(cls): 44 | rclpy.shutdown() 45 | 46 | def setUp(self): 47 | self.node = rclpy.create_node("test_node") 48 | 49 | def tearDown(self): 50 | self.node.destroy_node() 51 | 52 | def test_available_hardware_interfaces(self): 53 | # Call /controller_manager/list_hardware_interfaces service and check the response 54 | client = self.node.create_client(ListHardwareInterfaces, "/controller_manager/list_hardware_interfaces") 55 | if not client.wait_for_service(timeout_sec=10.0): 56 | self.fail("Service /controller_manager/list_hardware_interfaces not available") 57 | 58 | request = ListHardwareInterfaces.Request() 59 | future = client.call_async(request) 60 | rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) 61 | if future.result() is None: 62 | self.fail("Service call to /controller_manager/list_hardware_interfaces failed") 63 | response = future.result() 64 | 65 | # available state interfaces 66 | available_state_interfaces_names = [iface.name for iface in response.state_interfaces] 67 | assert ( 68 | len(available_state_interfaces_names) == 8 69 | ), f"Expected 8 state interfaces, got {len(available_state_interfaces_names)}" 70 | expected_state_interfaces = [ 71 | "joint1/position", 72 | "joint1/velocity", 73 | "joint1/effort", 74 | "joint1/torque", 75 | "joint2/position", 76 | "joint2/velocity", 77 | "joint2/effort", 78 | "joint2/torque", 79 | ] 80 | assert set(available_state_interfaces_names) == set( 81 | expected_state_interfaces 82 | ), f"State interfaces do not match expected. Got: {available_state_interfaces_names}" 83 | 84 | # available command interfaces 85 | available_command_interfaces_names = [iface.name for iface in response.command_interfaces] 86 | assert ( 87 | len(available_command_interfaces_names) == 4 88 | ), f"Expected 4 command interfaces, got {len(available_command_interfaces_names)}" 89 | expected_command_interfaces = ["joint1/position", "joint1/velocity", "joint2/position", "joint2/velocity"] 90 | assert set(available_command_interfaces_names) == set( 91 | expected_command_interfaces 92 | ), f"Command interfaces do not match expected. Got: {available_command_interfaces_names}" 93 | 94 | self.node.get_logger().info("Available hardware interfaces check passed.") 95 | -------------------------------------------------------------------------------- /include/mujoco_ros2_control/mujoco_lidar.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | namespace mujoco_ros2_control 36 | { 37 | 38 | struct LidarData 39 | { 40 | std::string name; 41 | std::string frame_name; 42 | int num_rangefinders; 43 | double min_angle; 44 | double max_angle; 45 | double angle_increment; 46 | double range_min; 47 | double range_max; 48 | 49 | // Maps the index of the rangefinder to the index of the mujoco rangefinder's data. 50 | // E.g. lidar-034 -> sensor_indexes[34] will contain index of that rangefinder in mj_data_->sensordata 51 | std::vector sensor_indexes; 52 | 53 | // For message publishing 54 | std::string laserscan_topic; 55 | sensor_msgs::msg::LaserScan laser_scan_msg; 56 | rclcpp::Publisher::SharedPtr scan_pub; 57 | }; 58 | 59 | /** 60 | * @brief Wraps Mujoco rangefinder sensors for publishing ROS 2 LaserScan messages. 61 | */ 62 | class MujocoLidar 63 | { 64 | public: 65 | /** 66 | * @brief Constructs a new MujocoLidar wrapper object. 67 | * 68 | * @param node Will be used to construct laserscan publishers 69 | * @param sim_mutex Provides synchronized access to the mujoco_data object for grabbing rangefinder data 70 | * @param mujoco_data Mujoco data for the simulation 71 | * @param mujoco_model Mujoco model for the simulation 72 | * @param lidar_publish_rate The rate to publish all camera images, for now all images are published at the same rate. 73 | */ 74 | explicit MujocoLidar(rclcpp::Node::SharedPtr& node, std::recursive_mutex* sim_mutex, mjData* mujoco_data, 75 | mjModel* mujoco_model, double lidar_publish_rate); 76 | 77 | /** 78 | * @brief Starts the lidar processing thread in the background. 79 | */ 80 | void init(); 81 | 82 | /** 83 | * @brief Stops the lidar processing thread and closes the relevant objects, call before shutdown. 84 | */ 85 | void close(); 86 | 87 | /** 88 | * @brief Parses lidar information from the mujoco model. 89 | * 90 | * Returns true if successful, false otherwise. 91 | */ 92 | bool register_lidar(const hardware_interface::HardwareInfo& hardware_info); 93 | 94 | private: 95 | /** 96 | * @brief Start publishing loop. 97 | */ 98 | void update_loop(); 99 | 100 | /** 101 | * @brief Updates the camera images and publishes info, images, and depth maps. 102 | */ 103 | void update(); 104 | 105 | rclcpp::Node::SharedPtr node_; 106 | 107 | // Ensures locked access to simulation data for rendering. 108 | std::recursive_mutex* sim_mutex_{ nullptr }; 109 | 110 | mjData* mj_data_; 111 | mjModel* mj_model_; 112 | 113 | // Vector container to copy sensordata out of mj_data_ 114 | std::vector mj_lidar_data_; 115 | 116 | // LaserScan publishing rate in Hz 117 | double lidar_publish_rate_; 118 | 119 | // Rendering options for the cameras, currently hard coded to defaults 120 | mjvOption mjv_opt_; 121 | mjvScene mjv_scn_; 122 | mjrContext mjr_con_; 123 | 124 | // Containers for ladar data and ROS constructs 125 | std::vector lidar_sensors_; 126 | 127 | // Lidar processing thread 128 | std::thread rendering_thread_; 129 | std::atomic_bool publish_lidar_; 130 | }; 131 | 132 | } // namespace mujoco_ros2_control 133 | -------------------------------------------------------------------------------- /include/mujoco_ros2_control/mujoco_cameras.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #pragma once 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | namespace mujoco_ros2_control 38 | { 39 | 40 | struct CameraData 41 | { 42 | mjvCamera mjv_cam; 43 | mjrRect viewport; 44 | 45 | std::string name; 46 | std::string frame_name; 47 | std::string info_topic; 48 | std::string image_topic; 49 | std::string depth_topic; 50 | 51 | uint32_t width; 52 | uint32_t height; 53 | 54 | std::vector image_buffer; 55 | std::vector depth_buffer; 56 | std::vector depth_buffer_flipped; 57 | 58 | sensor_msgs::msg::Image image; 59 | sensor_msgs::msg::Image depth_image; 60 | sensor_msgs::msg::CameraInfo camera_info; 61 | 62 | rclcpp::Publisher::SharedPtr image_pub; 63 | rclcpp::Publisher::SharedPtr depth_image_pub; 64 | rclcpp::Publisher::SharedPtr camera_info_pub; 65 | }; 66 | 67 | /** 68 | * @brief Wraps Mujoco cameras for publishing ROS 2 RGB-D streams. 69 | */ 70 | class MujocoCameras 71 | { 72 | public: 73 | /** 74 | * @brief Constructs a new MujocoCameras wrapper object. 75 | * 76 | * @param node Will be used to construct image publishers 77 | * @param sim_mutex Provides synchronized access to the mujoco_data object for rendering 78 | * @param mujoco_data Mujoco data for the simulation 79 | * @param mujoco_model Mujoco model for the simulation 80 | * @param camera_publish_rate The rate to publish all camera images, for now all images are published at the same rate. 81 | */ 82 | explicit MujocoCameras(rclcpp::Node::SharedPtr& node, std::recursive_mutex* sim_mutex, mjData* mujoco_data, 83 | mjModel* mujoco_model, double camera_publish_rate); 84 | 85 | /** 86 | * @brief Starts the image processing thread in the background. 87 | * 88 | * The background thread will initialize its own offscreen glfw context for rendering images that is 89 | * separate from the Simulate applications, as the context must be in the running thread. 90 | */ 91 | void init(); 92 | 93 | /** 94 | * @brief Stops the camera processing thread and closes the relevant objects, call before shutdown. 95 | */ 96 | void close(); 97 | 98 | /** 99 | * @brief Parses camera information from the mujoco model. 100 | */ 101 | void register_cameras(const hardware_interface::HardwareInfo& hardware_info); 102 | 103 | private: 104 | /** 105 | * @brief Initializes the rendering context and starts processing. 106 | */ 107 | void update_loop(); 108 | 109 | /** 110 | * @brief Updates the camera images and publishes info, images, and depth maps. 111 | */ 112 | void update(); 113 | 114 | rclcpp::Node::SharedPtr node_; 115 | 116 | // Ensures locked access to simulation data for rendering. 117 | std::recursive_mutex* sim_mutex_{ nullptr }; 118 | 119 | mjData* mj_data_; 120 | mjModel* mj_model_; 121 | mjData* mj_camera_data_; 122 | 123 | // Image publishing rate 124 | double camera_publish_rate_; 125 | 126 | // Rendering options for the cameras, currently hard coded to defaults 127 | mjvOption mjv_opt_; 128 | mjvScene mjv_scn_; 129 | mjrContext mjr_con_; 130 | 131 | // Containers for camera data and ROS constructs 132 | std::vector cameras_; 133 | 134 | // Camera processing thread 135 | std::thread rendering_thread_; 136 | std::atomic_bool publish_images_; 137 | }; 138 | 139 | } // namespace mujoco_ros2_control 140 | -------------------------------------------------------------------------------- /docs/TOOLS.md: -------------------------------------------------------------------------------- 1 | # Mujoco To ROS Tools 2 | 3 | 4 | > **WARNING**: These tools are highly experimental. 5 | Expect things to be broken. 6 | 7 | As Mujoco does not ingest URDFs, we have written a helper tool for converting URDF to MJCF to assist with converting a robot description to an MJCF. 8 | 9 | As noted in the warning above, but reiterating here, these tools are highly experimental. 10 | They are intended to be used for assistance and getting started, but do not expect things to work for all possible inputs, nor to work immediately out of the box. 11 | 12 | ## Usage 13 | 14 | The current tool that is available is `make_mjcf_from_robot_description`, which is runnable with: 15 | 16 | ```bash 17 | ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py 18 | ``` 19 | 20 | By default, the tool will pull a URDF from the `/robot_description` topic. 21 | However, this is configurable at execution time. 22 | A complete list of options is available from the argument parser: 23 | 24 | ```bash 25 | $ ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py --help 26 | usage: make_mjcf_from_robot_description.py [-h] [-u URDF] [-r ROBOT_DESCRIPTION] [-m MUJOCO_INPUTS] [-o OUTPUT] [-c] [-f] 27 | 28 | Convert a full URDF to MJCF for use in Mujoco 29 | 30 | options: 31 | -h, --help show this help message and exit 32 | -u URDF, --urdf URDF Optionally pass an existing URDF file 33 | -r ROBOT_DESCRIPTION, --robot_description ROBOT_DESCRIPTION 34 | Optionally pass the robot description string 35 | -m MUJOCO_INPUTS, --mujoco_inputs MUJOCO_INPUTS 36 | Optionally specify a defaults xml for default settings, actuators, options, and additional sensors 37 | -o OUTPUT, --output OUTPUT 38 | Generated output path 39 | -c, --convert_stl_to_obj 40 | If we should convert .stls to .objs 41 | -f, --add_free_joint Adds a free joint as the base link for mobile robots 42 | ``` 43 | 44 | A sample URDF and inputs file are provided in [test_robot.urdf](../test/test_resources/test_robot.urdf) and [test_inputs.xml](../test/test_resources/test_inputs.xml). 45 | 46 | To convert the URDF, run the following from the repo root 47 | 48 | ```bash 49 | ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py -u test/test_resources/test_robot.urdf -m test/test_resources/test_inputs.xml -o /tmp/output/ 50 | ``` 51 | 52 | The `/tmp/output/` directory will contain all necessary assets and MJCF files that can be copied into the relevant locations in a config package. 53 | They can also be adjusted as needed after the fact. 54 | 55 | ```bash 56 | "${MUJOCO_DIR}"/bin/simulate /tmp/output/scene.xml 57 | ``` 58 | 59 | Of note, the test robot has a good chunk of supported functionality, and we recommend using it as a guide. 60 | 61 | ## Conversion of CLR 62 | 63 | We generally recommend using the `view_robot.launch.py` from description packages to run a conversion for one of our robots. 64 | Though it can be as long as there is an active `/robot_description` topic with a fully processed URDF. 65 | For example, to convert the ChonkUR robot, we can use the inputs from the `clr_mujoco_config` package: 66 | 67 | ```bash 68 | # Publish the robot description 69 | ros2 launch clr_description view_robot.launch.py 70 | 71 | # Then process the urdf, note you MUST break down the .objs by passing `-c`. 72 | ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py -c -m $(ros2 pkg prefix clr_mujoco_config --share)/description/mujoco_inputs.xml -o /tmp/clr_output/ 73 | ``` 74 | 75 | Like above, this will dump all necessary data to the `/tmp/clr_output/` directory, which can then be copied or modified as needed. 76 | 77 | To view the results of the conversion, you can use Mujoco's `simulate` tool. 78 | This will bringup the standard Mujoco user panel with the converted Chonkur: 79 | 80 | ```bash 81 | "${MUJOCO_DIR}"/bin/simulate /tmp/clr_output/scene.xml 82 | ``` 83 | 84 | ## Notes 85 | 86 | **NOTE** This has some heave non-ROS dependencies that could probably be cleaned up: 87 | 88 | * bpy - blender python api 89 | * mujoco python api 90 | * xml.etree (not sure if this is already available) 91 | * xml.dom (not sure if this is already available) 92 | 93 | A rough outline of the automated process to convert a URDF: 94 | 95 | * reads a robot descriptiong URDF 96 | * add in mujoco tag that provides necessary info for conversion 97 | * replace package names from `package://` to absolute filepaths 98 | * read absolute filepaths of all meshes and convert either dae or stl to obj using blender python api 99 | * put all of these meshes into an `assets/` folder under `mjcf_data/` relative to current working dir 100 | * modify filepaths again in urdf to point to `assets/` folder 101 | * (NOTE) blender dae conversion is kind of broken, need to get this to do better orientation 102 | * publish the new formatted robot description xml file that can be used for conversion 103 | * convert the new robot description urdf file 104 | * run the mujoco conversion tool to get the mjcf version 105 | * copy in a default scene.xml file which gives some better camera and scene info 106 | * add remaining sites and items 107 | -------------------------------------------------------------------------------- /scripts/find_missing_inertias.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) 2025, United States Government, as represented by the 4 | # Administrator of the National Aeronautics and Space Administration. 5 | # 6 | # All rights reserved. 7 | # 8 | # This software is licensed under the Apache License, Version 2.0 9 | # (the "License"); you may not use this file except in compliance with the 10 | # License. You may obtain a copy of the License at 11 | # 12 | # http://www.apache.org/licenses/LICENSE-2.0 13 | # 14 | # Unless required by applicable law or agreed to in writing, software 15 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 16 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 17 | # License for the specific language governing permissions and limitations 18 | # under the License. 19 | 20 | import argparse 21 | import xml.etree.ElementTree as ET 22 | import sys 23 | from collections import deque, defaultdict 24 | 25 | # Prints a list of all moveable links in an URDF that are missing inertial data. 26 | # Mujoco requires that all moveable bodies have a non-zero inertia and mass, which boils 27 | # down to having at least one statically attached object to every non-fixed joint's children. 28 | # This is a little helper script to identify joints/links that are missing required data 29 | # because it can be really annoying to identify what is missing when converting URDFs, 30 | # and the errors from Mujoco's XML loader do not give info. 31 | 32 | 33 | def parse_urdf(urdf_fp): 34 | """ 35 | Parse the URDF and return a list of joints { name, parent, child, type } 36 | """ 37 | try: 38 | tree = ET.parse(urdf_fp) 39 | except ET.ParseError as e: 40 | sys.exit(f"Failed to parse URDF: {e}") 41 | root = tree.getroot() 42 | joints = [] 43 | for j in root.findall("joint"): 44 | joints.append( 45 | { 46 | "name": j.get("name", ""), 47 | "parent": j.find("parent").get("link"), 48 | "child": j.find("child").get("link"), 49 | "type": j.get("type", "fixed"), 50 | } 51 | ) 52 | return root, joints 53 | 54 | 55 | def collect_valid_inertial_links(root): 56 | """ 57 | Gets a list of all the joints in the URDF that have valid inertial properties. 58 | """ 59 | inertia_attrs = {"ixx", "ixy", "ixz", "iyy", "iyz", "izz"} 60 | valid = set() 61 | for link in root.findall("link"): 62 | name = link.get("name") 63 | inertial = link.find("inertial") 64 | if inertial is None: 65 | continue 66 | mass = inertial.find("mass") 67 | if mass is None or "value" not in mass.attrib: 68 | continue 69 | inertia = inertial.find("inertia") 70 | if inertia is None or not inertia_attrs.issubset(inertia.attrib): 71 | continue 72 | valid.add(name) 73 | return valid 74 | 75 | 76 | def has_static_inertial_descendant(start_link, child_map, valid_links): 77 | """ 78 | Traverse over statically connected descedents and make sure at least one of them is in the 79 | list of links with inertia data. 80 | """ 81 | q = deque([start_link]) 82 | seen = {start_link} 83 | while q: 84 | link = q.popleft() 85 | # if this link itself has valid inertial, accept 86 | if link in valid_links: 87 | return True, [] 88 | # traverse only fixed‐type joints 89 | for child, jtype, _ in child_map.get(link, ()): 90 | if jtype == "fixed" and child not in seen: 91 | seen.add(child) 92 | q.append(child) 93 | return False, seen 94 | 95 | 96 | def check_urdf(urdf_file): 97 | root, joints = parse_urdf(urdf_file) 98 | valid_links = collect_valid_inertial_links(root) 99 | 100 | # Make a list of all joints 101 | child_map = defaultdict(list) 102 | for j in joints: 103 | child_map[j["parent"]].append((j["child"], j["type"], j["name"])) 104 | 105 | invalid_joints = [] 106 | for j in joints: 107 | if j["type"] == "fixed": 108 | continue 109 | # for a moving joint, check its child link's static subtree 110 | valid, links = has_static_inertial_descendant(j["child"], child_map, valid_links) 111 | if not valid: 112 | invalid_joints.append((j["name"], links)) 113 | 114 | if invalid_joints: 115 | for joint, links in invalid_joints: 116 | print(f"Invalid joint: {joint}") 117 | print(" links:") 118 | links_str = {", ".join(links)} 119 | print(f" {links_str}") 120 | sys.exit(1) 121 | else: 122 | print("All moving joints have at least one static descendant with valid mass & inertia.") 123 | sys.exit(0) 124 | 125 | 126 | if __name__ == "__main__": 127 | parser = argparse.ArgumentParser( 128 | description="For each moving joint, report if its static-descendant subtree lacks any valid mass+inertia link." 129 | ) 130 | parser.add_argument("urdf_file", type=argparse.FileType("r"), help="Path to your URDF file") 131 | args = parser.parse_args() 132 | check_urdf(args.urdf_file) 133 | -------------------------------------------------------------------------------- /test/launch/test_robot.launch.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # 3 | # Copyright (c) 2025, United States Government, as represented by the 4 | # Administrator of the National Aeronautics and Space Administration. 5 | # 6 | # All rights reserved. 7 | # 8 | # This software is licensed under the Apache License, Version 2.0 9 | # (the "License"); you may not use this file except in compliance with the 10 | # License. You may obtain a copy of the License at 11 | # 12 | # http://www.apache.org/licenses/LICENSE-2.0 13 | # 14 | # Unless required by applicable law or agreed to in writing, software 15 | # distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 16 | # WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 17 | # License for the specific language governing permissions and limitations 18 | # under the License. 19 | 20 | from launch import LaunchDescription 21 | from launch.actions import DeclareLaunchArgument 22 | from launch.substitutions import ( 23 | Command, 24 | FindExecutable, 25 | LaunchConfiguration, 26 | PathJoinSubstitution, 27 | ) 28 | from launch_ros.actions import Node 29 | from launch.conditions import IfCondition, UnlessCondition 30 | from launch_ros.parameter_descriptions import ParameterValue, ParameterFile 31 | from launch_ros.substitutions import FindPackageShare 32 | 33 | 34 | def generate_launch_description(): 35 | 36 | # Refer https://github.com/ros-controls/mujoco_ros2_control?tab=readme-ov-file#joints 37 | use_pid = DeclareLaunchArgument( 38 | "use_pid", default_value="false", description="If we should use PID control to enable other control modes" 39 | ) 40 | 41 | headless = DeclareLaunchArgument("headless", default_value="false", description="Run in headless mode") 42 | 43 | use_mjcf_from_topic = DeclareLaunchArgument( 44 | "use_mjcf_from_topic", 45 | default_value="false", 46 | description="When set to true, the MJCF is generated at runtime from URDF", 47 | ) 48 | 49 | robot_description_content = Command( 50 | [ 51 | PathJoinSubstitution([FindExecutable(name="xacro")]), 52 | " ", 53 | PathJoinSubstitution( 54 | [ 55 | FindPackageShare("mujoco_ros2_control"), 56 | "test_resources", 57 | "test_robot.urdf", 58 | ] 59 | ), 60 | " ", 61 | "use_pid:=", 62 | LaunchConfiguration("use_pid"), 63 | " ", 64 | "headless:=", 65 | LaunchConfiguration("headless"), 66 | " ", 67 | "use_mjcf_from_topic:=", 68 | LaunchConfiguration("use_mjcf_from_topic"), 69 | ] 70 | ) 71 | 72 | robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)} 73 | 74 | converter_arguments_no_pid = [ 75 | "--robot_description", 76 | robot_description_content, 77 | "--m", 78 | PathJoinSubstitution( 79 | [ 80 | FindPackageShare("mujoco_ros2_control"), 81 | "test_resources", 82 | "test_inputs.xml", 83 | ] 84 | ), 85 | "--scene", 86 | PathJoinSubstitution( 87 | [ 88 | FindPackageShare("mujoco_ros2_control"), 89 | "test_resources", 90 | "scene_info.xml", 91 | ] 92 | ), 93 | "--publish_topic", 94 | "/mujoco_robot_description", 95 | ] 96 | 97 | converter_arguments_pid = [ 98 | "--publish_topic", 99 | "/mujoco_robot_description", 100 | ] 101 | 102 | converter_node_no_pid = Node( 103 | package="mujoco_ros2_control", 104 | executable="make_mjcf_from_robot_description.py", 105 | output="both", 106 | emulate_tty=True, 107 | arguments=converter_arguments_no_pid, 108 | condition=UnlessCondition(LaunchConfiguration("use_pid")), 109 | ) 110 | 111 | converter_node_pid = Node( 112 | package="mujoco_ros2_control", 113 | executable="make_mjcf_from_robot_description.py", 114 | output="both", 115 | emulate_tty=True, 116 | arguments=converter_arguments_pid, 117 | condition=IfCondition(LaunchConfiguration("use_pid")), 118 | ) 119 | 120 | controller_parameters = ParameterFile( 121 | PathJoinSubstitution([FindPackageShare("mujoco_ros2_control"), "config", "controllers.yaml"]), 122 | ) 123 | 124 | robot_state_publisher_node = Node( 125 | package="robot_state_publisher", 126 | executable="robot_state_publisher", 127 | output="both", 128 | parameters=[ 129 | robot_description, 130 | {"use_sim_time": True}, 131 | ], 132 | ) 133 | 134 | control_node = Node( 135 | package="mujoco_ros2_control", 136 | executable="ros2_control_node", 137 | output="both", 138 | parameters=[ 139 | {"use_sim_time": True}, 140 | controller_parameters, 141 | ], 142 | ) 143 | 144 | spawn_joint_state_broadcaster = Node( 145 | package="controller_manager", 146 | executable="spawner", 147 | name="spawn_joint_state_broadcaster", 148 | arguments=[ 149 | "joint_state_broadcaster", 150 | ], 151 | output="both", 152 | ) 153 | 154 | spawn_position_controller = Node( 155 | package="controller_manager", 156 | executable="spawner", 157 | name="spawn_position_controller", 158 | arguments=[ 159 | "position_controller", 160 | ], 161 | output="both", 162 | ) 163 | 164 | return LaunchDescription( 165 | [ 166 | use_pid, 167 | headless, 168 | use_mjcf_from_topic, 169 | robot_state_publisher_node, 170 | control_node, 171 | spawn_joint_state_broadcaster, 172 | spawn_position_controller, 173 | converter_node_pid, 174 | converter_node_no_pid, 175 | ] 176 | ) 177 | -------------------------------------------------------------------------------- /src/mujoco_ros2_control_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 ROS2-Control Development Team 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | using namespace std::chrono_literals; 26 | 27 | namespace 28 | { 29 | // Reference: https://man7.org/linux/man-pages/man2/sched_setparam.2.html 30 | // This value is used when configuring the main loop to use SCHED_FIFO scheduling 31 | // We use a midpoint RT priority to allow maximum flexibility to users 32 | int const kSchedPriority = 50; 33 | 34 | } // namespace 35 | 36 | int main(int argc, char ** argv) 37 | { 38 | rclcpp::init(argc, argv); 39 | 40 | std::shared_ptr executor = 41 | std::make_shared(); 42 | std::string manager_node_name = "controller_manager"; 43 | 44 | rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); 45 | std::vector node_arguments = cm_node_options.arguments(); 46 | for (int i = 1; i < argc; ++i) 47 | { 48 | if (node_arguments.empty() && std::string(argv[i]) != "--ros-args") 49 | { 50 | // A simple way to reject non ros args 51 | continue; 52 | } 53 | node_arguments.push_back(argv[i]); 54 | } 55 | cm_node_options.arguments(node_arguments); 56 | 57 | auto cm = std::make_shared( 58 | executor, manager_node_name, "", cm_node_options); 59 | 60 | const bool use_sim_time = cm->get_parameter_or("use_sim_time", false); 61 | 62 | const bool has_realtime = realtime_tools::has_realtime_kernel(); 63 | const bool lock_memory = cm->get_parameter_or("lock_memory", has_realtime); 64 | if (lock_memory) 65 | { 66 | const auto lock_result = realtime_tools::lock_memory(); 67 | if (!lock_result.first) 68 | { 69 | RCLCPP_WARN(cm->get_logger(), "Unable to lock the memory: '%s'", lock_result.second.c_str()); 70 | } 71 | } 72 | 73 | RCLCPP_INFO(cm->get_logger(), "update rate is %d Hz", cm->get_update_rate()); 74 | const bool manage_overruns = cm->get_parameter_or("overruns.manage", true); 75 | RCLCPP_INFO( 76 | cm->get_logger(), "Overruns handling is : %s", manage_overruns ? "enabled" : "disabled"); 77 | const int thread_priority = cm->get_parameter_or("thread_priority", kSchedPriority); 78 | RCLCPP_INFO( 79 | cm->get_logger(), "Spawning %s RT thread with scheduler priority: %d", cm->get_name(), 80 | thread_priority); 81 | 82 | std::thread cm_thread( 83 | [cm, thread_priority, use_sim_time, manage_overruns]() 84 | { 85 | rclcpp::Parameter cpu_affinity_param; 86 | if (cm->get_parameter("cpu_affinity", cpu_affinity_param)) 87 | { 88 | std::vector cpus = {}; 89 | if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) 90 | { 91 | cpus = {static_cast(cpu_affinity_param.as_int())}; 92 | } 93 | else if (cpu_affinity_param.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY) 94 | { 95 | const auto cpu_affinity_param_array = cpu_affinity_param.as_integer_array(); 96 | std::for_each( 97 | cpu_affinity_param_array.begin(), cpu_affinity_param_array.end(), 98 | [&cpus](int cpu) { cpus.push_back(static_cast(cpu)); }); 99 | } 100 | const auto affinity_result = realtime_tools::set_current_thread_affinity(cpus); 101 | if (!affinity_result.first) 102 | { 103 | RCLCPP_WARN( 104 | cm->get_logger(), "Unable to set the CPU affinity : '%s'", 105 | affinity_result.second.c_str()); 106 | } 107 | } 108 | 109 | if (!realtime_tools::configure_sched_fifo(thread_priority)) 110 | { 111 | RCLCPP_WARN( 112 | cm->get_logger(), 113 | "Could not enable FIFO RT scheduling policy: with error number <%i>(%s). See " 114 | "[https://control.ros.org/master/doc/ros2_control/controller_manager/doc/userdoc.html] " 115 | "for details on how to enable realtime scheduling.", 116 | errno, strerror(errno)); 117 | } 118 | else 119 | { 120 | RCLCPP_INFO( 121 | cm->get_logger(), "Successful set up FIFO RT scheduling policy with priority %i.", 122 | thread_priority); 123 | } 124 | 125 | // CHANGED FROM UPSTREAM 126 | // We must wait for the clock to be running, which requires the simulation to have started so that the 127 | // physics loop will publish to the clock topic. To account for that, we must spin the executor 128 | // and the controller manager node to ensure the MuJoCo hardware interface is constructed and launched. 129 | // So instead, we just wait in the control loop so that the hardware interface can still start and run. 130 | // 131 | // TODO: Potentially remove this node depending on what comes out of the upstream PR: 132 | // https://github.com/ros-controls/ros2_control/pull/2654 133 | cm->get_clock()->wait_until_started(); 134 | cm->get_clock()->sleep_for(rclcpp::Duration::from_seconds(1.0 / cm->get_update_rate())); 135 | 136 | // for calculating sleep time 137 | auto const period = std::chrono::nanoseconds(1'000'000'000 / cm->get_update_rate()); 138 | 139 | // for calculating the measured period of the loop 140 | rclcpp::Time previous_time = cm->get_trigger_clock()->now(); 141 | std::this_thread::sleep_for(period); 142 | 143 | std::chrono::steady_clock::time_point next_iteration_time{std::chrono::steady_clock::now()}; 144 | 145 | while (rclcpp::ok()) 146 | { 147 | // calculate measured period 148 | auto const current_time = cm->get_trigger_clock()->now(); 149 | auto const measured_period = current_time - previous_time; 150 | previous_time = current_time; 151 | 152 | // execute update loop 153 | cm->read(cm->get_trigger_clock()->now(), measured_period); 154 | cm->update(cm->get_trigger_clock()->now(), measured_period); 155 | cm->write(cm->get_trigger_clock()->now(), measured_period); 156 | 157 | // wait until we hit the end of the period 158 | if (use_sim_time) 159 | { 160 | cm->get_clock()->sleep_until(current_time + period); 161 | } 162 | else 163 | { 164 | next_iteration_time += period; 165 | const auto time_now = std::chrono::steady_clock::now(); 166 | if (manage_overruns && next_iteration_time < time_now) 167 | { 168 | const double time_diff = 169 | static_cast( 170 | std::chrono::duration_cast(time_now - next_iteration_time) 171 | .count()) / 172 | 1.e6; 173 | const double cm_period = 1.e3 / static_cast(cm->get_update_rate()); 174 | const int overrun_count = static_cast(std::ceil(time_diff / cm_period)); 175 | RCLCPP_WARN_THROTTLE( 176 | cm->get_logger(), *cm->get_clock(), 1000, 177 | "Overrun detected! The controller manager missed its desired rate of %d Hz. The loop " 178 | "took %f ms (missed cycles : %d).", 179 | cm->get_update_rate(), time_diff + cm_period, overrun_count + 1); 180 | next_iteration_time += (overrun_count * period); 181 | } 182 | std::this_thread::sleep_until(next_iteration_time); 183 | } 184 | } 185 | }); 186 | 187 | executor->add_node(cm); 188 | executor->spin(); 189 | cm_thread.join(); 190 | rclcpp::shutdown(); 191 | return 0; 192 | } 193 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.16) 2 | 3 | project(mujoco_ros2_control) 4 | 5 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 6 | add_compile_options(-Wall -Wextra -Wpedantic) 7 | endif() 8 | 9 | # find dependencies 10 | find_package(ament_cmake REQUIRED) 11 | find_package(controller_manager REQUIRED) 12 | find_package(Eigen3 REQUIRED) 13 | find_package(glfw3 REQUIRED) 14 | find_package(control_toolbox REQUIRED) 15 | find_package(hardware_interface REQUIRED) 16 | find_package(pluginlib REQUIRED) 17 | find_package(rclcpp REQUIRED) 18 | find_package(rclcpp_lifecycle REQUIRED) 19 | find_package(Threads REQUIRED) 20 | 21 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 22 | 23 | # Attempt to find an install of mujoco, either the library itself, from the environment, or 24 | # as a last resort download the required tar file manually and unzip it into the workspace. 25 | find_package(mujoco QUIET) 26 | if(mujoco_FOUND) 27 | message(STATUS "Mujoco build from source has been found") 28 | set(MUJOCO_LIB mujoco::mujoco) 29 | set(MUJOCO_INCLUDE_DIR ${MUJOCO_INCLUDE_DIR}) 30 | set(MUJOCO_SIMULATE_DIR ${MUJOCO_INCLUDE_DIR}/../simulate) 31 | elseif(DEFINED ENV{MUJOCO_INSTALL_DIR}) 32 | message(STATUS "Mujoco build from source has not been found. Attempting to find the binary in $ENV{MUJOCO_INSTALL_DIR} instead.") 33 | set(MUJOCO_DIR $ENV{MUJOCO_INSTALL_DIR}) 34 | find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib) 35 | if(NOT MUJOCO_LIB) 36 | message(FATAL_ERROR "Failed to find binary in $ENV{MUJOCO_DIR}") 37 | endif() 38 | set(MUJOCO_INCLUDE_DIR $ENV{MUJOCO_DIR}/include) 39 | set(MUJOCO_SIMULATE_DIR $ENV{MUJOCO_DIR}/simulate) 40 | else() 41 | include(FetchContent) 42 | 43 | # Detect architecture 44 | set(MUJOCO_VERSION "3.4.0") 45 | if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64|AMD64") 46 | set(CPU_ARCH "x86_64") 47 | elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") 48 | set(CPU_ARCH "aarch64") 49 | else() 50 | message(FATAL_ERROR "Unsupported architecture: ${CMAKE_SYSTEM_PROCESSOR}") 51 | endif() 52 | 53 | message(STATUS "No MuJoCo installation found. Downloading MuJoCo ${MUJOCO_VERSION} for ${CPU_ARCH}.") 54 | 55 | # Download the archive and put it next to the source directory 56 | set(MUJOCO_DOWNLOAD_DIR ${CMAKE_CURRENT_SOURCE_DIR}/mujoco) 57 | set(MUJOCO_EXTRACT_DIR ${MUJOCO_DOWNLOAD_DIR}/mujoco-${MUJOCO_VERSION}) 58 | set(FETCHCONTENT_UPDATES_DISCONNECTED ON) 59 | FetchContent_Declare( 60 | mujoco_download 61 | URL https://github.com/google-deepmind/mujoco/releases/download/${MUJOCO_VERSION}/mujoco-${MUJOCO_VERSION}-linux-${CPU_ARCH}.tar.gz 62 | SOURCE_DIR ${MUJOCO_EXTRACT_DIR} 63 | DOWNLOAD_EXTRACT_TIMESTAMP True 64 | ) 65 | FetchContent_MakeAvailable(mujoco_download) 66 | 67 | set(MUJOCO_DIR ${MUJOCO_EXTRACT_DIR}) 68 | message(STATUS "MuJoCo downloaded to: ${MUJOCO_DIR}") 69 | 70 | # Find the library in the downloaded location 71 | find_library(MUJOCO_LIB mujoco HINTS ${MUJOCO_DIR}/lib NO_DEFAULT_PATH) 72 | if(NOT MUJOCO_LIB) 73 | message(FATAL_ERROR "Failed to find MuJoCo library in ${MUJOCO_DIR}/lib") 74 | endif() 75 | 76 | set(MUJOCO_INCLUDE_DIR ${MUJOCO_DIR}/include) 77 | set(MUJOCO_SIMULATE_DIR ${MUJOCO_DIR}/simulate) 78 | 79 | message(STATUS "Installing ${MUJOCO_LIB} to the install directory...") 80 | install( 81 | DIRECTORY ${MUJOCO_DIR}/lib/ 82 | DESTINATION lib 83 | FILES_MATCHING PATTERN "libmujoco.so*" 84 | ) 85 | endif() 86 | 87 | # Fetch lodepng dependency. 88 | if(NOT TARGET lodepng) 89 | include(FetchContent) 90 | FetchContent_Declare( 91 | lodepng 92 | GIT_REPOSITORY https://github.com/lvandeve/lodepng.git 93 | GIT_TAG ${MUJOCO_DEP_VERSION_lodepng} 94 | ) 95 | 96 | FetchContent_GetProperties(lodepng) 97 | if(NOT lodepng_POPULATED) 98 | FetchContent_Populate(lodepng) 99 | # This is not a CMake project. 100 | set(LODEPNG_SRCS ${lodepng_SOURCE_DIR}/lodepng.cpp) 101 | set(LODEPNG_HEADERS ${lodepng_SOURCE_DIR}/lodepng.h) 102 | add_library(lodepng STATIC ${LODEPNG_HEADERS} ${LODEPNG_SRCS}) 103 | set_target_properties(lodepng PROPERTIES POSITION_INDEPENDENT_CODE ON) 104 | target_compile_options(lodepng PRIVATE ${MUJOCO_MACOS_COMPILE_OPTIONS}) 105 | target_link_options(lodepng PRIVATE ${MUJOCO_MACOS_LINK_OPTIONS}) 106 | target_include_directories(lodepng PUBLIC ${lodepng_SOURCE_DIR}) 107 | endif() 108 | endif() 109 | 110 | # We don't care about warnings from the external sources 111 | set(MUJOCO_SILENCE_COMPILER_WARNINGS 112 | -Wno-missing-field-initializers 113 | -Wno-unused-parameter 114 | -Wno-sign-compare 115 | -Wno-psabi 116 | ) 117 | 118 | # Build Mujoco's simulate application and make it available for linking 119 | add_library(platform_ui_adapter OBJECT) 120 | set_target_properties(platform_ui_adapter PROPERTIES 121 | POSITION_INDEPENDENT_CODE ON 122 | ) 123 | target_sources( 124 | platform_ui_adapter 125 | PUBLIC ${MUJOCO_SIMULATE_DIR}/glfw_adapter.h ${MUJOCO_SIMULATE_DIR}/glfw_dispatch.h ${MUJOCO_SIMULATE_DIR}/platform_ui_adapter.h 126 | PRIVATE ${MUJOCO_SIMULATE_DIR}/glfw_adapter.cc ${MUJOCO_SIMULATE_DIR}/glfw_dispatch.cc ${MUJOCO_SIMULATE_DIR}/platform_ui_adapter.cc 127 | ) 128 | target_include_directories( 129 | platform_ui_adapter PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR} 130 | $ 131 | ) 132 | target_link_libraries(platform_ui_adapter PUBLIC ${MUJOCO_LIB}) 133 | target_compile_options(platform_ui_adapter PRIVATE ${MUJOCO_SILENCE_COMPILER_WARNINGS}) 134 | add_library(mujoco::platform_ui_adapter ALIAS platform_ui_adapter) 135 | 136 | add_library(libsimulate STATIC $) 137 | add_library(mujoco::libsimulate ALIAS libsimulate) 138 | set_target_properties(libsimulate PROPERTIES 139 | OUTPUT_NAME simulate 140 | POSITION_INDEPENDENT_CODE ON 141 | ) 142 | target_sources( 143 | libsimulate 144 | PUBLIC ${MUJOCO_SIMULATE_DIR}/simulate.h 145 | PRIVATE ${MUJOCO_SIMULATE_DIR}/simulate.cc ${MUJOCO_SIMULATE_DIR}/array_safety.h 146 | ) 147 | target_include_directories(libsimulate PUBLIC ${MUJOCO_SIMULATE_DIR} ${MUJOCO_INCLUDE_DIR}) 148 | target_link_libraries(libsimulate PUBLIC lodepng mujoco::platform_ui_adapter ${MUJOCO_LIB}) 149 | target_compile_options(libsimulate PRIVATE ${MUJOCO_SILENCE_COMPILER_WARNINGS}) 150 | 151 | # Mujoco ROS 2 control system interface (this package) 152 | add_library(mujoco_ros2_control SHARED 153 | src/mujoco_system_interface.cpp 154 | src/mujoco_cameras.cpp 155 | src/mujoco_lidar.cpp 156 | ) 157 | # Ensure MuJoCo library can be found at runtime regardless of how it was installed above 158 | set_target_properties(mujoco_ros2_control PROPERTIES 159 | INSTALL_RPATH "\$ORIGIN/../lib;${CMAKE_INSTALL_PREFIX}/lib" 160 | ) 161 | target_link_libraries(mujoco_ros2_control 162 | ${MUJOCO_LIB} 163 | ${sensor_msgs_TARGETS} 164 | mujoco::libsimulate 165 | controller_manager::controller_manager 166 | controller_manager::controller_manager_parameters 167 | hardware_interface::hardware_interface 168 | hardware_interface::mock_components 169 | pluginlib::pluginlib 170 | rclcpp::rclcpp 171 | rclcpp_lifecycle::rclcpp_lifecycle 172 | control_toolbox::control_toolbox 173 | sensor_msgs::sensor_msgs_library 174 | Eigen3::Eigen 175 | Threads::Threads 176 | lodepng 177 | glfw 178 | ) 179 | target_include_directories(mujoco_ros2_control PUBLIC 180 | $ 181 | $ 182 | ) 183 | # Mark these as system packages to prevent compiler warnings 184 | target_include_directories(mujoco_ros2_control SYSTEM PUBLIC 185 | ${MUJOCO_INCLUDE_DIR} 186 | ${MUJOCO_SIMULATE_DIR} 187 | ) 188 | install(TARGETS mujoco_ros2_control 189 | LIBRARY DESTINATION lib/${PROJECT_NAME} 190 | ) 191 | 192 | # Install the ROS 2 control node 193 | add_executable(ros2_control_node src/mujoco_ros2_control_node.cpp) 194 | target_link_libraries(ros2_control_node PUBLIC 195 | controller_manager::controller_manager 196 | ) 197 | install( 198 | TARGETS ros2_control_node 199 | RUNTIME DESTINATION lib/${PROJECT_NAME} 200 | ) 201 | 202 | # Install python tools 203 | install(PROGRAMS 204 | scripts/find_missing_inertias.py 205 | scripts/make_mjcf_from_robot_description.py 206 | DESTINATION lib/${PROJECT_NAME} 207 | ) 208 | 209 | install( 210 | DIRECTORY resources scripts 211 | DESTINATION share/${PROJECT_NAME} 212 | ) 213 | 214 | pluginlib_export_plugin_description_file(hardware_interface mujoco_system_interface_plugin.xml) 215 | 216 | if(BUILD_TESTING) 217 | add_subdirectory(test) 218 | endif() 219 | 220 | ament_package() 221 | -------------------------------------------------------------------------------- /test/src/robot_launch_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | # Copyright 2025 PAL Robotics S.L. 4 | # 5 | # Licensed under the Apache License, Version 2.0 (the "License"); 6 | # you may not use this file except in compliance with the License. 7 | # You may obtain a copy of the License at 8 | # 9 | # http://www.apache.org/licenses/LICENSE-2.0 10 | # 11 | # Unless required by applicable law or agreed to in writing, software 12 | # distributed under the License is distributed on an "AS IS" BASIS, 13 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | # See the License for the specific language governing permissions and 15 | # limitations under the License. 16 | 17 | import os 18 | import time 19 | import unittest 20 | 21 | from ament_index_python.packages import get_package_share_directory 22 | from controller_manager.test_utils import check_controllers_running, check_if_js_published, check_node_running 23 | from launch import LaunchDescription 24 | from launch.actions import IncludeLaunchDescription 25 | from launch.launch_description_sources import PythonLaunchDescriptionSource 26 | from launch_testing.actions import ReadyToTest 27 | from launch_testing.util import KeepAliveProc 28 | from launch_testing_ros import WaitForTopics 29 | import pytest 30 | import rclpy 31 | from rclpy.qos import QoSProfile, DurabilityPolicy 32 | from rosgraph_msgs.msg import Clock 33 | from std_msgs.msg import Float64MultiArray, String 34 | from sensor_msgs.msg import JointState, Image, CameraInfo 35 | from controller_manager_msgs.srv import ListHardwareInterfaces 36 | 37 | 38 | # This function specifies the processes to be run for our test 39 | def generate_test_description_common(use_pid="false", use_mjcf_from_topic="false"): 40 | # This is necessary to get unbuffered output from the process under test 41 | proc_env = os.environ.copy() 42 | proc_env["PYTHONUNBUFFERED"] = "1" 43 | os.environ["USE_MJCF_FROM_TOPIC"] = use_mjcf_from_topic 44 | 45 | launch_include = IncludeLaunchDescription( 46 | PythonLaunchDescriptionSource( 47 | os.path.join( 48 | get_package_share_directory("mujoco_ros2_control"), 49 | "launch/test_robot.launch.py", 50 | ) 51 | ), 52 | launch_arguments={"headless": "true", "use_pid": use_pid, "use_mjcf_from_topic": use_mjcf_from_topic}.items(), 53 | ) 54 | 55 | return LaunchDescription([launch_include, KeepAliveProc(), ReadyToTest()]) 56 | 57 | 58 | @pytest.mark.rostest 59 | def generate_test_description(): 60 | return generate_test_description_common(use_pid="false") 61 | 62 | 63 | class TestFixture(unittest.TestCase): 64 | 65 | @classmethod 66 | def setUpClass(cls): 67 | rclpy.init() 68 | 69 | @classmethod 70 | def tearDownClass(cls): 71 | rclpy.shutdown() 72 | 73 | def setUp(self): 74 | self.node = rclpy.create_node("test_node") 75 | 76 | def tearDown(self): 77 | self.node.destroy_node() 78 | 79 | def test_node_start(self, proc_output): 80 | check_node_running(self.node, "robot_state_publisher") 81 | 82 | def test_clock(self): 83 | topic_list = [("/clock", Clock)] 84 | with WaitForTopics(topic_list, timeout=10.0): 85 | print("/clock is receiving messages!") 86 | 87 | def test_check_if_msgs_published(self): 88 | check_if_js_published( 89 | "/joint_states", 90 | [ 91 | "joint1", 92 | "joint2", 93 | ], 94 | ) 95 | 96 | def test_arm(self): 97 | 98 | # Check if the controllers are running 99 | cnames = ["position_controller", "joint_state_broadcaster"] 100 | check_controllers_running(self.node, cnames) 101 | 102 | # Create a publisher to send commands to the position controller 103 | pub = self.node.create_publisher(Float64MultiArray, "/position_controller/commands", 10) 104 | 105 | # Wait for subscriber to connect 106 | end_time = time.time() + 10 107 | while time.time() < end_time: 108 | if pub.get_subscription_count() > 0: 109 | break 110 | time.sleep(0.1) 111 | 112 | self.assertGreater(pub.get_subscription_count(), 0, "Controller did not subscribe to commands") 113 | 114 | msg = Float64MultiArray() 115 | msg.data = [0.5, -0.5] 116 | pub.publish(msg) 117 | 118 | # Allow some time for the message to be processed 119 | time.sleep(2.0) 120 | 121 | # Now, check that the joint states have been updated accordingly 122 | joint_state_topic = "/joint_states" 123 | wait_for_topics = WaitForTopics([(joint_state_topic, JointState)], timeout=20.0) 124 | assert wait_for_topics.wait(), f"Topic '{joint_state_topic}' not found!" 125 | msgs = wait_for_topics.received_messages(joint_state_topic) 126 | msg = msgs[0] 127 | assert len(msg.name) >= 2, "Joint states message doesn't have 2 joints" 128 | joint1_index = msg.name.index("joint1") 129 | joint2_index = msg.name.index("joint2") 130 | self.assertAlmostEqual( 131 | msg.position[joint1_index], 0.5, delta=0.05, msg="joint1 did not reach the commanded position" 132 | ) 133 | self.assertAlmostEqual( 134 | msg.position[joint2_index], -0.5, delta=0.05, msg="joint2 did not reach the commanded position" 135 | ) 136 | wait_for_topics.shutdown() 137 | 138 | # Runs the tests when the DISPLAY is set 139 | @unittest.skipIf(os.environ.get("DISPLAY", "") == "", "Skipping camera tests in headless mode.") 140 | def test_camera_topics(self): 141 | topic_list = [ 142 | ("/camera/color/image_raw", Image), 143 | ("/camera/color/camera_info", CameraInfo), 144 | ("/camera/aligned_depth_to_color/image_raw", Image), 145 | ] 146 | wait_for_topics = WaitForTopics(topic_list, timeout=5.0) 147 | assert wait_for_topics.wait(), "Not all camera topics were received in time!" 148 | assert wait_for_topics.topics_not_received() == set(), "Some topics were not received!" 149 | assert wait_for_topics.topics_received() == {t[0] for t in topic_list}, "Not all topics were received!" 150 | wait_for_topics.shutdown() 151 | 152 | 153 | class TestFixtureHardwareInterfacesCheck(unittest.TestCase): 154 | 155 | @classmethod 156 | def setUpClass(cls): 157 | rclpy.init() 158 | 159 | @classmethod 160 | def tearDownClass(cls): 161 | rclpy.shutdown() 162 | 163 | def setUp(self): 164 | self.node = rclpy.create_node("test_node") 165 | 166 | def tearDown(self): 167 | self.node.destroy_node() 168 | 169 | def test_available_hardware_interfaces(self): 170 | # Call /controller_manager/list_hardware_interfaces service and check the response 171 | client = self.node.create_client(ListHardwareInterfaces, "/controller_manager/list_hardware_interfaces") 172 | if not client.wait_for_service(timeout_sec=10.0): 173 | self.fail("Service /controller_manager/list_hardware_interfaces not available") 174 | 175 | request = ListHardwareInterfaces.Request() 176 | future = client.call_async(request) 177 | rclpy.spin_until_future_complete(self.node, future, timeout_sec=10.0) 178 | if future.result() is None: 179 | self.fail("Service call to /controller_manager/list_hardware_interfaces failed") 180 | response = future.result() 181 | 182 | # available state interfaces 183 | available_state_interfaces_names = [iface.name for iface in response.state_interfaces] 184 | assert ( 185 | len(available_state_interfaces_names) == 8 186 | ), f"Expected 8 state interfaces, got {len(available_state_interfaces_names)}" 187 | expected_state_interfaces = [ 188 | "joint1/position", 189 | "joint1/velocity", 190 | "joint1/effort", 191 | "joint1/torque", 192 | "joint2/position", 193 | "joint2/velocity", 194 | "joint2/effort", 195 | "joint2/torque", 196 | ] 197 | assert set(available_state_interfaces_names) == set( 198 | expected_state_interfaces 199 | ), f"State interfaces do not match expected. Got: {available_state_interfaces_names}" 200 | 201 | # available command interfaces 202 | available_command_interfaces_names = [iface.name for iface in response.command_interfaces] 203 | assert ( 204 | len(available_command_interfaces_names) == 2 205 | ), f"Expected 2 command interfaces, got {len(available_command_interfaces_names)}" 206 | expected_command_interfaces = ["joint1/position", "joint2/position"] 207 | assert set(available_command_interfaces_names) == set( 208 | expected_command_interfaces 209 | ), f"Command interfaces do not match expected. Got: {available_command_interfaces_names}" 210 | 211 | self.node.get_logger().info("Available hardware interfaces check passed.") 212 | 213 | 214 | class TestMJCFGenerationFromURDF(unittest.TestCase): 215 | 216 | @classmethod 217 | def setUpClass(cls): 218 | if os.environ.get("USE_MJCF_FROM_TOPIC") != "true": 219 | raise unittest.SkipTest("Skipping MJCF generation tests because use_mjcf_from_topic is not true") 220 | rclpy.init() 221 | 222 | @classmethod 223 | def tearDownClass(cls): 224 | rclpy.shutdown() 225 | 226 | def setUp(self): 227 | self.node = rclpy.create_node("test_node") 228 | 229 | def tearDown(self): 230 | self.node.destroy_node() 231 | 232 | def test_check_for_mujoco_robot_description_topic(self): 233 | # Create a QoS profile for transient_local topics 234 | qos_profile = QoSProfile(depth=1, durability=DurabilityPolicy.TRANSIENT_LOCAL) 235 | 236 | received_msgs = [] 237 | 238 | def callback(msg): 239 | received_msgs.append(msg) 240 | 241 | sub = self.node.create_subscription(String, "/mujoco_robot_description", callback, qos_profile) 242 | 243 | end_time = time.time() + 15.0 244 | while time.time() < end_time: 245 | rclpy.spin_once(self.node, timeout_sec=0.1) 246 | if received_msgs: 247 | break 248 | 249 | assert received_msgs, "The MuJoCo robot description topic is not published" 250 | msg = received_msgs[0] 251 | assert " 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | // Pull in the Simulate class and PhysicsThread/RenderLoop declarations: 42 | #include "glfw_adapter.h" // for mj::GlfwAdapter 43 | #include "simulate.h" // must be on your include path, handled by CMake 44 | 45 | #include "mujoco_ros2_control/data.hpp" 46 | #include "mujoco_ros2_control/mujoco_cameras.hpp" 47 | #include "mujoco_ros2_control/mujoco_lidar.hpp" 48 | 49 | namespace mujoco_ros2_control 50 | { 51 | class MujocoSystemInterface : public hardware_interface::SystemInterface 52 | { 53 | public: 54 | /** 55 | * @brief ros2_control SystemInterface to wrap Mujocos Simulate application. 56 | * 57 | * Supports Actuators, Force Torque/IMU Sensors, and RGB-D camera, and Lidar Sensors in ROS 2 simulations. 58 | * For more information on configuration refer to the docs, check the comment strings below, and refer to 59 | * the example in the test folder. 60 | */ 61 | MujocoSystemInterface(); 62 | ~MujocoSystemInterface() override; 63 | 64 | hardware_interface::CallbackReturn 65 | on_init(const hardware_interface::HardwareComponentInterfaceParams& params) override; 66 | std::vector export_state_interfaces() override; 67 | std::vector export_command_interfaces() override; 68 | 69 | hardware_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 70 | hardware_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 71 | 72 | hardware_interface::return_type perform_command_mode_switch(const std::vector& start_interfaces, 73 | const std::vector& stop_interfaces) override; 74 | 75 | hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; 76 | hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; 77 | 78 | /** 79 | * @brief Returns a copy of the MuJoCo model. 80 | * 81 | * This method locks the simulation mutex to ensure thread safety. 82 | * @param dest Pointer to an mjModel structure where the copy will be stored. The pointer will be allocated if it is nullptr. 83 | */ 84 | void get_model(mjModel*& dest); 85 | 86 | /** 87 | * @brief Returns a copy of the current MuJoCo data. 88 | * 89 | * This method locks the simulation mutex to ensure thread safety. 90 | * @param dest Pointer to an mjData structure where the copy will be stored. The pointer will be allocated if it is nullptr. 91 | */ 92 | void get_data(mjData*& dest); 93 | 94 | /** 95 | * @brief Sets the MuJoCo data to the provided value. 96 | * 97 | * This method locks the simulation mutex to ensure thread safety. 98 | * @param mj_data Pointer to an mjData structure containing the new data. 99 | */ 100 | void set_data(mjData* mj_data); 101 | 102 | private: 103 | /** 104 | * @brief Loads actuator information into the HW interface. 105 | * 106 | * Will pull joint/actuator information from the provided HardwareInfo, and map it to the appropriate 107 | * actuator in the sim's mujoco data. The data wrappers will be used as control/state interfaces for 108 | * the HW interface. 109 | */ 110 | void register_joints(const hardware_interface::HardwareInfo& info); 111 | 112 | /** 113 | * @brief Constructs all sensor data containers for the interface 114 | * 115 | * Pulls sensors (FTS and IMUs) out of the HardwareInfo and uses it to map relevant data containers 116 | * in the ros2_control interface. There are expectations on the naming of sensors in both the MJCF and 117 | * the ros2_control xacro, as Mujoco does not have direct support for either of these sensors. 118 | * 119 | * For a FTS named , we add both a `force` and `torque` sensor to the MJCF as: 120 | * 121 | * 122 | * 123 | * 124 | * 125 | * 126 | * In the ROS 2 control xacro, these must be mapped to a state interface called `_fts`, so, 127 | * 128 | * 129 | * 130 | * 131 | * 132 | * 133 | * 134 | * 135 | * fts_sensor 136 | * 137 | * 138 | * The HW interface will map the state interfaces accordingly. Similarly for an IMU named , we 139 | * must add three separate sensors to the MJCF, update the site/obj accordingly: 140 | * 141 | * 142 | * 143 | * 144 | * 145 | * 146 | * 147 | * These can be mapped with the following xacro (note the `_imu` suffix): 148 | * 149 | * 150 | * 151 | * 152 | * 153 | * 154 | * 155 | * 156 | * 157 | * 158 | * 159 | * 160 | * 161 | */ 162 | void register_sensors(const hardware_interface::HardwareInfo& info); 163 | 164 | /** 165 | * @brief Sets the initial simulation conditions (pos, vel, ctrl) values from provided filepath. 166 | * 167 | * @param override_start_position_file filepath that contains starting positions 168 | * @return success of reading the file and setting the positions 169 | */ 170 | bool set_override_start_positions(const std::string& override_start_position_file); 171 | 172 | /** 173 | * @brief Set the initial pose for all actuators if provided in the URDF. 174 | */ 175 | void set_initial_pose(); 176 | 177 | /** 178 | * @brief Spins the physics simulation for the Simulate Application 179 | */ 180 | void PhysicsLoop(); 181 | 182 | /** 183 | * @brief Publishes the Simulate Application's timestamp to the /clock topic 184 | * 185 | * This enables pausing and restarting of the simulation through the application window. 186 | */ 187 | void publish_clock(); 188 | 189 | rclcpp::Logger get_logger() const; 190 | 191 | // System information 192 | std::string model_path_; 193 | 194 | // MuJoCo data pointers 195 | mjModel* mj_model_{ nullptr }; 196 | mjData* mj_data_{ nullptr }; 197 | 198 | // Data container for control data 199 | mjData* mj_data_control_{ nullptr }; 200 | 201 | // For rendering 202 | mjvCamera cam_; 203 | mjvOption opt_; 204 | mjvPerturb pert_; 205 | 206 | // Logger 207 | rclcpp::Logger logger_ = rclcpp::get_logger("MujocoSystemInterface"); 208 | 209 | // Speed scaling parameter. if set to >0 then we ignore the value set in the simulate app and instead 210 | // attempt to loop at whatever this is set to. If this is <0, then we use the value from the app. 211 | double sim_speed_factor_; 212 | 213 | // Primary simulate object 214 | std::unique_ptr sim_; 215 | 216 | // Threads for rendering physics, the UI simulation, and the ROS node 217 | std::thread physics_thread_; 218 | std::thread ui_thread_; 219 | 220 | // Provides access to ROS interfaces for elements that require it 221 | std::shared_ptr mujoco_node_; 222 | std::unique_ptr executor_; 223 | std::thread executor_thread_; 224 | 225 | // Primary clock publisher for the world 226 | std::shared_ptr> clock_publisher_; 227 | realtime_tools::RealtimePublisher::SharedPtr clock_realtime_publisher_; 228 | 229 | // Containers for RGB-D cameras 230 | std::unique_ptr cameras_; 231 | 232 | // Containers for LIDAR sensors 233 | std::unique_ptr lidar_sensors_; 234 | 235 | // Mutex used inside simulate.h for protecting model/data, we keep a reference 236 | // here to protect access to shared data. 237 | // TODO: It would be far better to put all relevant data into a single container with accessors 238 | // in a common location rather than passing around the raw pointer to the mutex, but it would 239 | // require more work to pull it out of simulate.h. 240 | std::recursive_mutex* sim_mutex_{ nullptr }; 241 | 242 | // Data containers for the HW interface 243 | std::unordered_map joint_hw_info_; 244 | std::unordered_map sensors_hw_info_; 245 | 246 | // Data containers for the HW interface 247 | std::vector joint_states_; 248 | std::vector ft_sensor_data_; 249 | std::vector imu_sensor_data_; 250 | }; 251 | 252 | } // namespace mujoco_ros2_control 253 | -------------------------------------------------------------------------------- /test/config/test_robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Status1 8 | - /TF1/Tree1 9 | Splitter Ratio: 0.5 10 | Tree Height: 248 11 | - Class: rviz_common/Selection 12 | Name: Selection 13 | - Class: rviz_common/Tool Properties 14 | Expanded: 15 | - /2D Goal Pose1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz_common/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz_common/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: LaserScan 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Alpha: 0.5 33 | Cell Size: 1 34 | Class: rviz_default_plugins/Grid 35 | Color: 160; 160; 164 36 | Enabled: true 37 | Line Style: 38 | Line Width: 0.029999999329447746 39 | Value: Lines 40 | Name: Grid 41 | Normal Cell Count: 0 42 | Offset: 43 | X: 0 44 | Y: 0 45 | Z: 0 46 | Plane: XY 47 | Plane Cell Count: 10 48 | Reference Frame: 49 | Value: true 50 | - Alpha: 1 51 | Class: rviz_default_plugins/RobotModel 52 | Collision Enabled: false 53 | Description File: "" 54 | Description Source: Topic 55 | Description Topic: 56 | Depth: 5 57 | Durability Policy: Volatile 58 | History Policy: Keep Last 59 | Reliability Policy: Reliable 60 | Value: /robot_description 61 | Enabled: true 62 | Links: 63 | All Links Enabled: true 64 | Expand Joint Details: false 65 | Expand Link Details: false 66 | Expand Tree: false 67 | Link Tree Style: Links in Alphabetic Order 68 | base: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | camera_color_frame: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | camera_color_mujoco_frame: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | camera_color_optical_frame: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | camera_link: 85 | Alpha: 1 86 | Show Axes: false 87 | Show Trail: false 88 | Value: true 89 | ee_link: 90 | Alpha: 1 91 | Show Axes: false 92 | Show Trail: false 93 | forearm: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | lidar_sensor_frame: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | upperarm: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | world: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Mass Properties: 112 | Inertia: false 113 | Mass: false 114 | Name: RobotModel 115 | TF Prefix: "" 116 | Update Interval: 0 117 | Value: true 118 | Visual Enabled: true 119 | - Class: rviz_default_plugins/Camera 120 | Enabled: true 121 | Far Plane Distance: 100 122 | Image Rendering: background and overlay 123 | Name: Camera 124 | Overlay Alpha: 0.5 125 | Topic: 126 | Depth: 5 127 | Durability Policy: Volatile 128 | History Policy: Keep Last 129 | Reliability Policy: Reliable 130 | Value: /camera/color/image_raw 131 | Value: true 132 | Visibility: 133 | Grid: true 134 | Image: true 135 | LaserScan: true 136 | RobotModel: true 137 | TF: true 138 | Value: true 139 | Zoom Factor: 1 140 | - Class: rviz_default_plugins/TF 141 | Enabled: true 142 | Frame Timeout: 15 143 | Frames: 144 | All Enabled: true 145 | base: 146 | Value: true 147 | camera_color_frame: 148 | Value: true 149 | camera_color_mujoco_frame: 150 | Value: true 151 | camera_color_optical_frame: 152 | Value: true 153 | camera_link: 154 | Value: true 155 | ee_link: 156 | Value: true 157 | forearm: 158 | Value: true 159 | lidar_sensor_frame: 160 | Value: true 161 | upperarm: 162 | Value: true 163 | world: 164 | Value: true 165 | Marker Scale: 1 166 | Name: TF 167 | Show Arrows: true 168 | Show Axes: true 169 | Show Names: false 170 | Tree: 171 | world: 172 | base: 173 | upperarm: 174 | forearm: 175 | ee_link: 176 | camera_link: 177 | camera_color_frame: 178 | camera_color_optical_frame: 179 | camera_color_mujoco_frame: 180 | {} 181 | lidar_sensor_frame: 182 | {} 183 | Update Interval: 0 184 | Value: true 185 | - Class: rviz_default_plugins/Image 186 | Enabled: true 187 | Max Value: 1 188 | Median window: 5 189 | Min Value: 0 190 | Name: Image 191 | Normalize Range: true 192 | Topic: 193 | Depth: 5 194 | Durability Policy: Volatile 195 | History Policy: Keep Last 196 | Reliability Policy: Reliable 197 | Value: /camera/color/image_raw 198 | Value: true 199 | - Alpha: 1 200 | Autocompute Intensity Bounds: true 201 | Autocompute Value Bounds: 202 | Max Value: 10 203 | Min Value: -10 204 | Value: true 205 | Axis: Z 206 | Channel Name: intensity 207 | Class: rviz_default_plugins/LaserScan 208 | Color: 255; 255; 255 209 | Color Transformer: Intensity 210 | Decay Time: 0 211 | Enabled: true 212 | Invert Rainbow: false 213 | Max Color: 255; 255; 255 214 | Max Intensity: 4096 215 | Min Color: 0; 0; 0 216 | Min Intensity: 0 217 | Name: LaserScan 218 | Position Transformer: XYZ 219 | Selectable: true 220 | Size (Pixels): 3 221 | Size (m): 0.029999999329447746 222 | Style: Spheres 223 | Topic: 224 | Depth: 5 225 | Durability Policy: Volatile 226 | Filter size: 10 227 | History Policy: Keep Last 228 | Reliability Policy: Reliable 229 | Value: /scan 230 | Use Fixed Frame: true 231 | Use rainbow: true 232 | Value: true 233 | Enabled: true 234 | Global Options: 235 | Background Color: 48; 48; 48 236 | Fixed Frame: world 237 | Frame Rate: 30 238 | Name: root 239 | Tools: 240 | - Class: rviz_default_plugins/Interact 241 | Hide Inactive Objects: true 242 | - Class: rviz_default_plugins/MoveCamera 243 | - Class: rviz_default_plugins/Select 244 | - Class: rviz_default_plugins/FocusCamera 245 | - Class: rviz_default_plugins/Measure 246 | Line color: 128; 128; 0 247 | - Class: rviz_default_plugins/SetInitialPose 248 | Covariance x: 0.25 249 | Covariance y: 0.25 250 | Covariance yaw: 0.06853891909122467 251 | Topic: 252 | Depth: 5 253 | Durability Policy: Volatile 254 | History Policy: Keep Last 255 | Reliability Policy: Reliable 256 | Value: /initialpose 257 | - Class: rviz_default_plugins/SetGoal 258 | Topic: 259 | Depth: 5 260 | Durability Policy: Volatile 261 | History Policy: Keep Last 262 | Reliability Policy: Reliable 263 | Value: /goal_pose 264 | - Class: rviz_default_plugins/PublishPoint 265 | Single click: true 266 | Topic: 267 | Depth: 5 268 | Durability Policy: Volatile 269 | History Policy: Keep Last 270 | Reliability Policy: Reliable 271 | Value: /clicked_point 272 | Transformation: 273 | Current: 274 | Class: rviz_default_plugins/TF 275 | Value: true 276 | Views: 277 | Current: 278 | Class: rviz_default_plugins/Orbit 279 | Distance: 4.176185131072998 280 | Enable Stereo Rendering: 281 | Stereo Eye Separation: 0.05999999865889549 282 | Stereo Focal Distance: 1 283 | Swap Stereo Eyes: false 284 | Value: false 285 | Focal Point: 286 | X: 1.2192846536636353 287 | Y: 0.7369985580444336 288 | Z: 0.008791889995336533 289 | Focal Shape Fixed Size: true 290 | Focal Shape Size: 0.05000000074505806 291 | Invert Z Axis: false 292 | Name: Current View 293 | Near Clip Distance: 0.009999999776482582 294 | Pitch: 0.7003991603851318 295 | Target Frame: 296 | Value: Orbit (rviz) 297 | Yaw: 0.5522100329399109 298 | Saved: ~ 299 | Window Geometry: 300 | Camera: 301 | collapsed: false 302 | Displays: 303 | collapsed: false 304 | Height: 846 305 | Hide Left Dock: false 306 | Hide Right Dock: true 307 | Image: 308 | collapsed: false 309 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000181000000c700fffffffb0000000c00430061006d00650072006101000001c2000000960000002800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065010000025e000000910000002800ffffff000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 310 | Selection: 311 | collapsed: false 312 | Time: 313 | collapsed: false 314 | Tool Properties: 315 | collapsed: false 316 | Views: 317 | collapsed: true 318 | Width: 1200 319 | X: 70 320 | Y: 44 321 | -------------------------------------------------------------------------------- /src/mujoco_cameras.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #include "mujoco_ros2_control/mujoco_cameras.hpp" 21 | #include "mujoco_ros2_control/utils.hpp" 22 | 23 | #include "sensor_msgs/image_encodings.hpp" 24 | 25 | using namespace std::chrono_literals; 26 | 27 | namespace mujoco_ros2_control 28 | { 29 | 30 | MujocoCameras::MujocoCameras(rclcpp::Node::SharedPtr& node, std::recursive_mutex* sim_mutex, mjData* mujoco_data, 31 | mjModel* mujoco_model, double camera_publish_rate) 32 | : node_(node) 33 | , sim_mutex_(sim_mutex) 34 | , mj_data_(mujoco_data) 35 | , mj_model_(mujoco_model) 36 | , camera_publish_rate_(camera_publish_rate) 37 | { 38 | } 39 | 40 | void MujocoCameras::register_cameras(const hardware_interface::HardwareInfo& hardware_info) 41 | { 42 | cameras_.resize(0); 43 | for (auto i = 0; i < mj_model_->ncam; ++i) 44 | { 45 | const char* cam_name = mj_model_->names + mj_model_->name_camadr[i]; 46 | const int* cam_resolution = mj_model_->cam_resolution + 2 * i; 47 | const mjtNum cam_fovy = mj_model_->cam_fovy[i]; 48 | 49 | // Construct CameraData wrapper and set defaults 50 | CameraData camera; 51 | camera.name = cam_name; 52 | camera.mjv_cam.type = mjCAMERA_FIXED; 53 | camera.mjv_cam.fixedcamid = i; 54 | camera.width = static_cast(cam_resolution[0]); 55 | camera.height = static_cast(cam_resolution[1]); 56 | camera.viewport = { 0, 0, cam_resolution[0], cam_resolution[1] }; 57 | 58 | // If the hardware_info has a camera of the same name then we pull parameters from there. 59 | const auto camera_info_maybe = get_sensor_from_info(hardware_info, cam_name); 60 | if (camera_info_maybe.has_value()) 61 | { 62 | const auto camera_info = camera_info_maybe.value(); 63 | camera.frame_name = camera_info.parameters.at("frame_name"); 64 | camera.info_topic = camera_info.parameters.at("info_topic"); 65 | camera.image_topic = camera_info.parameters.at("image_topic"); 66 | camera.depth_topic = camera_info.parameters.at("depth_topic"); 67 | } 68 | // Otherwise set default values for the frame and topics. 69 | else 70 | { 71 | camera.frame_name = camera.name + "_frame"; 72 | camera.info_topic = camera.name + "/camera_info"; 73 | camera.image_topic = camera.name + "/color"; 74 | camera.depth_topic = camera.name + "/depth"; 75 | } 76 | 77 | RCLCPP_INFO_STREAM(node_->get_logger(), "Adding camera: " << cam_name); 78 | RCLCPP_INFO_STREAM(node_->get_logger(), " frame_name: " << camera.frame_name); 79 | RCLCPP_INFO_STREAM(node_->get_logger(), " info_topic: " << camera.info_topic); 80 | RCLCPP_INFO_STREAM(node_->get_logger(), " image_topic: " << camera.image_topic); 81 | RCLCPP_INFO_STREAM(node_->get_logger(), " depth_topic: " << camera.depth_topic); 82 | 83 | // Configure publishers 84 | camera.camera_info_pub = node_->create_publisher(camera.info_topic, 1); 85 | camera.image_pub = node_->create_publisher(camera.image_topic, 1); 86 | camera.depth_image_pub = node_->create_publisher(camera.depth_topic, 1); 87 | 88 | // Setup containers for color image data 89 | camera.image.header.frame_id = camera.frame_name; 90 | 91 | const auto image_size = camera.width * camera.height * 3; 92 | camera.image_buffer.resize(image_size); 93 | camera.image.data.resize(image_size); 94 | camera.image.width = camera.width; 95 | camera.image.height = camera.height; 96 | camera.image.step = camera.width * 3; 97 | camera.image.encoding = sensor_msgs::image_encodings::RGB8; 98 | 99 | // Depth image data 100 | camera.depth_image.header.frame_id = camera.frame_name; 101 | camera.depth_buffer.resize(camera.width * camera.height); 102 | camera.depth_buffer_flipped.resize(camera.width * camera.height); 103 | camera.depth_image.data.resize(camera.width * camera.height * sizeof(float)); 104 | camera.depth_image.width = camera.width; 105 | camera.depth_image.height = camera.height; 106 | camera.depth_image.step = camera.width * sizeof(float); 107 | camera.depth_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; 108 | 109 | // Camera info 110 | camera.camera_info.header.frame_id = camera.frame_name; 111 | camera.camera_info.width = camera.width; 112 | camera.camera_info.height = camera.height; 113 | camera.camera_info.distortion_model = "plumb_bob"; 114 | camera.camera_info.k.fill(0.0); 115 | camera.camera_info.r.fill(0.0); 116 | camera.camera_info.p.fill(0.0); 117 | camera.camera_info.d.resize(5, 0.0); 118 | 119 | double focal_scaling = (1.0 / std::tan((cam_fovy * M_PI / 180.0) / 2.0)) * camera.height / 2.0; 120 | camera.camera_info.k[0] = camera.camera_info.p[0] = focal_scaling; 121 | camera.camera_info.k[2] = camera.camera_info.p[2] = static_cast(camera.width) / 2.0; 122 | camera.camera_info.k[4] = camera.camera_info.p[5] = focal_scaling; 123 | camera.camera_info.k[5] = camera.camera_info.p[6] = static_cast(camera.height) / 2.0; 124 | camera.camera_info.k[8] = camera.camera_info.p[10] = 1.0; 125 | 126 | // Add to list of cameras 127 | cameras_.push_back(camera); 128 | } 129 | } 130 | 131 | void MujocoCameras::init() 132 | { 133 | // Start the rendering thread process 134 | if (!glfwInit()) 135 | { 136 | RCLCPP_WARN(node_->get_logger(), "Failed to initialize GLFW. Disabling camera publishing."); 137 | publish_images_ = false; 138 | return; 139 | } 140 | publish_images_ = true; 141 | rendering_thread_ = std::thread(&MujocoCameras::update_loop, this); 142 | } 143 | 144 | void MujocoCameras::close() 145 | { 146 | publish_images_ = false; 147 | if (rendering_thread_.joinable()) 148 | { 149 | rendering_thread_.join(); 150 | } 151 | 152 | mjv_freeScene(&mjv_scn_); 153 | mjr_freeContext(&mjr_con_); 154 | } 155 | 156 | void MujocoCameras::update_loop() 157 | { 158 | // We create an offscreen context specific to this process for managing camera rendering. 159 | glfwWindowHint(GLFW_VISIBLE, GLFW_FALSE); 160 | GLFWwindow* window = glfwCreateWindow(1, 1, "", NULL, NULL); 161 | glfwMakeContextCurrent(window); 162 | 163 | // Initialization of the context and data structures has to happen in the rendering thread 164 | RCLCPP_INFO(node_->get_logger(), "Initializing rendering for cameras"); 165 | mjv_defaultOption(&mjv_opt_); 166 | mjv_defaultScene(&mjv_scn_); 167 | mjr_defaultContext(&mjr_con_); 168 | 169 | // Turn rangefinder rendering off so we don't get rays in camera images 170 | mjv_opt_.flags[mjtVisFlag::mjVIS_RANGEFINDER] = 0; 171 | 172 | // Initialize data for camera rendering 173 | mj_camera_data_ = mj_makeData(mj_model_); 174 | RCLCPP_INFO(node_->get_logger(), "Starting the camera rendering loop, publishing at %f Hz", camera_publish_rate_); 175 | 176 | // create scene and context 177 | mjv_makeScene(mj_model_, &mjv_scn_, 2000); 178 | mjr_makeContext(mj_model_, &mjr_con_, mjFONTSCALE_150); 179 | 180 | // Ensure the context will support the largest cameras 181 | int max_width = 1, max_height = 1; 182 | for (const auto& cam : cameras_) 183 | { 184 | max_width = std::max(max_width, static_cast(cam.width)); 185 | max_height = std::max(max_height, static_cast(cam.height)); 186 | } 187 | mjr_resizeOffscreen(max_width, max_height, &mjr_con_); 188 | RCLCPP_INFO(node_->get_logger(), "Resized offscreen buffer to %d x %d", max_width, max_height); 189 | 190 | // TODO: Support per-camera publish rates? 191 | rclcpp::Rate rate(camera_publish_rate_); 192 | while (rclcpp::ok() && publish_images_) 193 | { 194 | update(); 195 | rate.sleep(); 196 | } 197 | } 198 | 199 | void MujocoCameras::update() 200 | { 201 | // Rendering is done offscreen 202 | mjr_setBuffer(mjFB_OFFSCREEN, &mjr_con_); 203 | 204 | // Step 1: Lock the sim and copy data for use in all camera rendering. 205 | { 206 | std::unique_lock lock(*sim_mutex_); 207 | mjv_copyData(mj_camera_data_, mj_model_, mj_data_); 208 | } 209 | 210 | // Step 2: Render the scene and copy images to relevant camera data containers. 211 | for (auto& camera : cameras_) 212 | { 213 | // Render scene 214 | mjv_updateScene(mj_model_, mj_camera_data_, &mjv_opt_, NULL, &camera.mjv_cam, mjCAT_ALL, &mjv_scn_); 215 | mjr_render(camera.viewport, &mjv_scn_, &mjr_con_); 216 | 217 | // Copy image into relevant buffers 218 | mjr_readPixels(camera.image_buffer.data(), camera.depth_buffer.data(), camera.viewport, &mjr_con_); 219 | } 220 | 221 | // Step 3: Adjust the images and copy depth data. 222 | const float near = static_cast(mj_model_->vis.map.znear * mj_model_->stat.extent); 223 | const float far = static_cast(mj_model_->vis.map.zfar * mj_model_->stat.extent); 224 | const float depth_scale = 1.0f - near / far; 225 | for (auto& camera : cameras_) 226 | { 227 | // Fix non-linear projections in the depth image and flip the data. 228 | // https://github.com/google-deepmind/mujoco/blob/3.4.0/python/mujoco/renderer.py#L190 229 | for (uint32_t h = 0; h < camera.height; h++) 230 | { 231 | for (uint32_t w = 0; w < camera.width; w++) 232 | { 233 | auto idx = h * camera.width + w; 234 | auto idx_flipped = (camera.height - 1 - h) * camera.width + w; 235 | camera.depth_buffer[idx] = near / (1.0f - camera.depth_buffer[idx] * (depth_scale)); 236 | camera.depth_buffer_flipped[idx_flipped] = camera.depth_buffer[idx]; 237 | } 238 | } 239 | // Copy flipped data into the depth image message, floats -> unsigned chars 240 | std::memcpy(&camera.depth_image.data[0], camera.depth_buffer_flipped.data(), camera.depth_image.data.size()); 241 | 242 | // OpenGL's coordinate system's origin is in the bottom left, so we invert the images row-by-row 243 | auto row_size = camera.width * 3; 244 | for (uint32_t h = 0; h < camera.height; h++) 245 | { 246 | auto src_idx = h * row_size; 247 | auto dest_idx = (camera.height - 1 - h) * row_size; 248 | std::memcpy(&camera.image.data[dest_idx], &camera.image_buffer[src_idx], row_size); 249 | } 250 | } 251 | 252 | // Step 4: Publish the images. 253 | for (auto& camera : cameras_) 254 | { 255 | // Publish images and camera info 256 | const auto time = node_->now(); 257 | camera.image.header.stamp = time; 258 | camera.depth_image.header.stamp = time; 259 | camera.camera_info.header.stamp = time; 260 | 261 | camera.image_pub->publish(camera.image); 262 | camera.depth_image_pub->publish(camera.depth_image); 263 | camera.camera_info_pub->publish(camera.camera_info); 264 | } 265 | } 266 | 267 | } // namespace mujoco_ros2_control 268 | -------------------------------------------------------------------------------- /src/mujoco_lidar.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2025, United States Government, as represented by the 3 | * Administrator of the National Aeronautics and Space Administration. 4 | * 5 | * All rights reserved. 6 | * 7 | * This software is licensed under the Apache License, Version 2.0 8 | * (the "License"); you may not use this file except in compliance with the 9 | * License. You may obtain a copy of the License at 10 | * 11 | * http://www.apache.org/licenses/LICENSE-2.0 12 | * 13 | * Unless required by applicable law or agreed to in writing, software 14 | * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT 15 | * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the 16 | * License for the specific language governing permissions and limitations 17 | * under the License. 18 | */ 19 | 20 | #include "mujoco_ros2_control/mujoco_lidar.hpp" 21 | #include "mujoco_ros2_control/utils.hpp" 22 | 23 | namespace mujoco_ros2_control 24 | { 25 | 26 | /** 27 | * Given a sensor in the form -id, returns the name and id if possible. 28 | * Otherwise, return "" and -1 to indicate failure. 29 | */ 30 | std::pair parse_lidar_name(const std::string& sensor_name) 31 | { 32 | const auto split_idx = sensor_name.find_last_of("-"); 33 | if (split_idx == std::string::npos) 34 | { 35 | return { sensor_name, -1 }; 36 | } 37 | 38 | // Grab the lidar sensor name 39 | const auto lidar_name = sensor_name.substr(0, split_idx); 40 | 41 | // Grab the index of the sensor name, if possible 42 | const auto lidar_idx = sensor_name.substr(split_idx + 1); 43 | if (lidar_idx.empty() || !std::all_of(lidar_idx.begin(), lidar_idx.end(), ::isdigit)) 44 | { 45 | return { lidar_name, -1 }; 46 | } 47 | 48 | return { lidar_name, std::stoi(lidar_idx) }; 49 | } 50 | 51 | /** 52 | * Construct a LidarData object given a string sensor name and hardware_info object to parse. 53 | */ 54 | std::optional get_lidar_data(const hardware_interface::HardwareInfo& hardware_info, const std::string& name) 55 | { 56 | const auto sensor_info_maybe = get_sensor_from_info(hardware_info, name); 57 | if (!sensor_info_maybe.has_value()) 58 | { 59 | return std::nullopt; 60 | } 61 | const auto sensor_info = sensor_info_maybe.value(); 62 | 63 | auto get_parameter = [&](const std::string& key) -> std::optional { 64 | if (auto it = sensor_info.parameters.find(key); it != sensor_info.parameters.end()) 65 | { 66 | return it->second; 67 | } 68 | return std::nullopt; 69 | }; 70 | 71 | auto frame_name = get_parameter("frame_name"); 72 | auto min_angle = get_parameter("min_angle"); 73 | auto max_angle = get_parameter("max_angle"); 74 | auto angle_increment = get_parameter("angle_increment"); 75 | auto laserscan_topic = get_parameter("laserscan_topic"); 76 | auto range_min = get_parameter("range_min"); 77 | auto range_max = get_parameter("range_max"); 78 | 79 | // If any required parameters are missing fire off an error. 80 | if (!frame_name || !angle_increment || !min_angle || !max_angle) 81 | { 82 | return std::nullopt; 83 | } 84 | 85 | // Otherwise construct and return a new LidarData object 86 | LidarData lidar_sensor; 87 | lidar_sensor.name = name; 88 | lidar_sensor.frame_name = *frame_name; 89 | lidar_sensor.min_angle = std::stod(*min_angle); 90 | lidar_sensor.max_angle = std::stod(*max_angle); 91 | lidar_sensor.angle_increment = std::stod(*angle_increment); 92 | lidar_sensor.num_rangefinders = 93 | static_cast((lidar_sensor.max_angle - lidar_sensor.min_angle) / lidar_sensor.angle_increment) + 1; 94 | 95 | lidar_sensor.laserscan_topic = laserscan_topic.has_value() ? laserscan_topic.value() : "/scan"; 96 | lidar_sensor.range_min = range_min.has_value() ? std::stod(range_min.value()) : 0.0; 97 | lidar_sensor.range_max = range_max.has_value() ? std::stod(range_max.value()) : 1000.0; 98 | lidar_sensor.sensor_indexes.resize(lidar_sensor.num_rangefinders); 99 | 100 | // Configure the static parameters of the laserscan message 101 | lidar_sensor.laser_scan_msg.header.frame_id = lidar_sensor.frame_name; 102 | lidar_sensor.laser_scan_msg.time_increment = 0.0; // Does this matter? 103 | lidar_sensor.laser_scan_msg.angle_min = lidar_sensor.min_angle; 104 | lidar_sensor.laser_scan_msg.angle_max = lidar_sensor.max_angle; 105 | lidar_sensor.laser_scan_msg.angle_increment = lidar_sensor.angle_increment; 106 | lidar_sensor.laser_scan_msg.range_min = lidar_sensor.range_min; 107 | lidar_sensor.laser_scan_msg.range_max = lidar_sensor.range_max; 108 | lidar_sensor.laser_scan_msg.ranges.resize(lidar_sensor.num_rangefinders); 109 | lidar_sensor.laser_scan_msg.intensities.resize(0); 110 | 111 | return lidar_sensor; 112 | } 113 | 114 | MujocoLidar::MujocoLidar(rclcpp::Node::SharedPtr& node, std::recursive_mutex* sim_mutex, mjData* mujoco_data, 115 | mjModel* mujoco_model, double lidar_publish_rate) 116 | : node_(node) 117 | , sim_mutex_(sim_mutex) 118 | , mj_data_(mujoco_data) 119 | , mj_model_(mujoco_model) 120 | , lidar_publish_rate_(lidar_publish_rate) 121 | { 122 | } 123 | 124 | bool MujocoLidar::register_lidar(const hardware_interface::HardwareInfo& hardware_info) 125 | { 126 | lidar_sensors_.resize(0); 127 | 128 | // Iterate over sensors and identify the rangefinders, then attempt to match them to 129 | // a relevant LidarData object. 130 | for (int i = 0; i < mj_model_->nsensor; ++i) 131 | { 132 | // Skip non-rangefinder sensors. 133 | if (mj_model_->sensor_type[i] != mjtSensor::mjSENS_RANGEFINDER) 134 | { 135 | continue; 136 | } 137 | 138 | // Grab the name of the sensor, which is required. 139 | const auto sensor_name_maybe = mj_id2name(mj_model_, mjtObj::mjOBJ_SENSOR, i); 140 | if (sensor_name_maybe == nullptr) 141 | { 142 | RCLCPP_WARN_STREAM(node_->get_logger(), "Cannot find a name for lidar sensor at index: " << i << ", skipping!"); 143 | continue; 144 | } 145 | const std::string sensor_name(sensor_name_maybe); 146 | 147 | // If it is a rangefinder, we expect the name to be of the form `-###` 148 | // The number of integers after the hyphen varies depending on the replicate. 149 | const auto [lidar_name, idx] = parse_lidar_name(sensor_name); 150 | if (idx == -1) 151 | { 152 | RCLCPP_WARN_STREAM(node_->get_logger(), "Failed to parse lidar sensor name: " << sensor_name << ", skipping!"); 153 | continue; 154 | } 155 | 156 | // If we have seen this before, update the data, otherwise create a new one and add it to the list. 157 | auto lidar_it = std::find_if(lidar_sensors_.begin(), lidar_sensors_.end(), 158 | [&lidar_name](const LidarData& data) { return data.name == lidar_name; }); 159 | 160 | if (lidar_it == lidar_sensors_.end()) 161 | { 162 | auto new_data_maybe = get_lidar_data(hardware_info, lidar_name); 163 | if (!new_data_maybe.has_value()) 164 | { 165 | RCLCPP_ERROR_STREAM(node_->get_logger(), 166 | "Failed to parse required configuration from ros2_control xacro: " << lidar_name); 167 | return false; 168 | } 169 | 170 | // Setup remaining msg params and publisher for the sensor 171 | auto lidar = new_data_maybe.value(); 172 | lidar.scan_pub = node_->create_publisher(lidar.laserscan_topic, 1); 173 | 174 | // We may someday want to compute this on the fly, but since everything is fixed this should be fine for now. 175 | lidar.laser_scan_msg.scan_time = 1.0 / lidar_publish_rate_; 176 | 177 | // Note that we have added the sensor 178 | RCLCPP_INFO_STREAM(node_->get_logger(), "Adding lidar sensor: " << lidar.name << ", idx: " << idx); 179 | RCLCPP_INFO_STREAM(node_->get_logger(), " frame_name: " << lidar.frame_name); 180 | RCLCPP_INFO_STREAM(node_->get_logger(), " num_rangefinders: " << lidar.num_rangefinders); 181 | RCLCPP_INFO_STREAM(node_->get_logger(), " min_angle: " << lidar.min_angle); 182 | RCLCPP_INFO_STREAM(node_->get_logger(), " max_angle: " << lidar.max_angle); 183 | RCLCPP_INFO_STREAM(node_->get_logger(), " angle_increment: " << lidar.angle_increment); 184 | RCLCPP_INFO_STREAM(node_->get_logger(), " range_min: " << lidar.range_min); 185 | RCLCPP_INFO_STREAM(node_->get_logger(), " range_max: " << lidar.range_max); 186 | 187 | // Add it to relevant containers 188 | lidar_sensors_.push_back(std::move(lidar)); 189 | 190 | // For updating the index below 191 | lidar_it = lidar_sensors_.end() - 1; 192 | } 193 | 194 | // Add this range to the sensor finders data array. There's technically no guarantee that the data is 195 | // sequential so we're just tracking this directly. 196 | lidar_it->sensor_indexes[idx] = mj_model_->sensor_adr[i]; 197 | } 198 | 199 | // TODO: Verify that everything actually got filled in correctly... 200 | for (const auto& lidar : lidar_sensors_) 201 | { 202 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "Lidar Sensor: " << lidar.name); 203 | for (size_t j = 0; j < lidar.sensor_indexes.size(); ++j) 204 | { 205 | RCLCPP_DEBUG_STREAM(node_->get_logger(), " sensor_indexes[" << j << "] = " << lidar.sensor_indexes[j]); 206 | } 207 | } 208 | 209 | return true; 210 | } 211 | 212 | void MujocoLidar::init() 213 | { 214 | // Start the rendering thread process 215 | publish_lidar_ = true; 216 | rendering_thread_ = std::thread(&MujocoLidar::update_loop, this); 217 | } 218 | 219 | void MujocoLidar::close() 220 | { 221 | publish_lidar_ = false; 222 | if (rendering_thread_.joinable()) 223 | { 224 | rendering_thread_.join(); 225 | } 226 | } 227 | 228 | void MujocoLidar::update_loop() 229 | { 230 | // Setup container for lidar data. 231 | mj_lidar_data_.resize(mj_model_->nsensordata); 232 | 233 | RCLCPP_INFO_STREAM(node_->get_logger(), 234 | "Starting the lidar processing loop, publishing at " << lidar_publish_rate_ << " Hz"); 235 | 236 | rclcpp::Rate rate(lidar_publish_rate_); 237 | while (rclcpp::ok() && publish_lidar_) 238 | { 239 | update(); 240 | rate.sleep(); 241 | } 242 | } 243 | 244 | void MujocoLidar::update() 245 | { 246 | // Step 1: Lock the sim and copy only the sensordata 247 | { 248 | std::unique_lock lock(*sim_mutex_); 249 | std::memcpy(mj_lidar_data_.data(), mj_data_->sensordata, mj_lidar_data_.size() * sizeof(mjtNum)); 250 | } 251 | 252 | // Step 2: Copy sensor information for lidar to the relevant containers, filtering as needed 253 | // TODO: This could be more efficient if we made assumptions about sensor data for a specific lidar 254 | // sensor being contiguous in sensordata. However, we haven't noted any issues with this 255 | // as is, and it is objectively more flexible. Leaving it for now. 256 | for (auto& lidar : lidar_sensors_) 257 | { 258 | RCLCPP_DEBUG_STREAM(node_->get_logger(), "Lidar Sensor: " << lidar.name); 259 | for (size_t idx = 0; idx < lidar.sensor_indexes.size(); ++idx) 260 | { 261 | const auto& i = lidar.sensor_indexes[idx]; 262 | auto range = mj_lidar_data_[i]; 263 | lidar.laser_scan_msg.ranges[idx] = range; 264 | RCLCPP_DEBUG_STREAM(node_->get_logger(), " sensor_indexes[" << idx << "] = " << lidar.sensor_indexes[idx] 265 | << " - " << mj_lidar_data_[i]); 266 | } 267 | 268 | // Apply range limits to the copied data 269 | std::transform(lidar.laser_scan_msg.ranges.begin(), lidar.laser_scan_msg.ranges.end(), 270 | lidar.laser_scan_msg.ranges.begin(), 271 | [&](auto range) { return (range < lidar.range_min || range > lidar.range_max) ? -1.0 : range; }); 272 | } 273 | 274 | // Step 3: Publish messages 275 | for (auto& lidar : lidar_sensors_) 276 | { 277 | lidar.laser_scan_msg.header.stamp = node_->now(); 278 | lidar.scan_pub->publish(lidar.laser_scan_msg); 279 | } 280 | } 281 | 282 | } // namespace mujoco_ros2_control 283 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [2025] United States Government, as represented by the 190 | Administrator of the National Aeronautics and Space Administration. 191 | All rights reserved. 192 | 193 | Licensed under the Apache License, Version 2.0 (the "License"); 194 | you may not use this file except in compliance with the License. 195 | You may obtain a copy of the License at 196 | 197 | http://www.apache.org/licenses/LICENSE-2.0 198 | 199 | Unless required by applicable law or agreed to in writing, software 200 | distributed under the License is distributed on an "AS IS" BASIS, 201 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 202 | See the License for the specific language governing permissions and 203 | limitations under the License. 204 | -------------------------------------------------------------------------------- /test/test_resources/test_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | mujoco_ros2_control/MujocoSystemInterface 215 | 216 | 217 | 218 | $(find mujoco_ros2_control)/config/mujoco_pid.yaml 219 | 220 | 221 | 222 | 223 | $(find mujoco_ros2_control)/test_resources/test_pid/scene_pid.xml 224 | 225 | 226 | 227 | $(find mujoco_ros2_control)/test_resources/scene.xml 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 3.0 236 | 237 | 238 | $(arg headless) 239 | 240 | 241 | 6.0 242 | 243 | 244 | 1.0 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 0.0 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 0.0 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | camera_color_mujoco_frame 279 | /camera/color/camera_info 280 | /camera/color/image_raw 281 | /camera/aligned_depth_to_color/image_raw 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | lidar_sensor_frame 290 | 0.025 291 | -0.3 292 | 0.3 293 | 0.05 294 | 10 295 | /scan 296 | 297 | 298 | 299 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MuJoCo ros2_control Simulation 2 | 3 | This package contains a ros2_control system interface for the [MuJoCo Simulator](https://mujoco.readthedocs.io/en/3.4.0/overview.html). 4 | It was originally written for simulating robot hardware in NASA Johnson's [iMETRO facility](https://ntrs.nasa.gov/citations/20230015485). 5 | 6 | The system interface wraps MuJoCo's [Simulate App](https://github.com/google-deepmind/mujoco/tree/3.4.0/simulate) to provide included functionality. 7 | Because the app is not bundled as a library, we compile it directly from a local install of MuJoCo. 8 | 9 | Parts of this library are also based on the MoveIt [mujoco_ros2_control](https://github.com/moveit/mujoco_ros2_control) package. 10 | 11 | ## Installation 12 | 13 | This interface has only been tested against ROS 2 jazzy and MuJoCo `3.4.0`. 14 | It should also be compatible with kilted and rolling, but we do not actively maintain those. 15 | We assume all required ROS dependencies have been installed either manually or with `rosdep`. 16 | 17 | For configuring MuJoCo, the included [CMakeLists.txt](./CMakeLists.txt) will download and install the tarfile automatically. 18 | As long as users have a good network connection there should not be an issue. 19 | 20 | However, a local install of MuJoCo can be used to build the application by setting the following environment variables, 21 | 22 | ```bash 23 | # The tested version 24 | MUJOCO_VERSION=3.4.0 25 | 26 | # Wherever it was installed and extracted on your machine 27 | MUJOCO_INSTALL_DIR=/opt/mujoco/mujoco-3.4.0 28 | ``` 29 | 30 | From there the library can be compiled with `colcon build ...`, as normal. 31 | 32 | ## URDF Model Conversion 33 | 34 | Mujoco does not support the full feature set of xacro/URDFs in the ROS 2 ecosystem. 35 | As such, users are required to convert any existing robot description files to an MJCF format. 36 | This includes adding actuators, sensors, and cameras as needed to the MJCF XML. 37 | 38 | We have built a *highly experimental tool to automate URDF conversion. 39 | For more information refer to the [documentation](./docs/TOOLS.md). 40 | 41 | ## Hardware Interface Setup 42 | 43 | ### Plugin 44 | 45 | This application is shipped as a ros2_control hardware interface, and can be configured as such. 46 | Just specify the plugin and point to a valid MJCF on launch: 47 | 48 | ```xml 49 | 50 | 51 | mujoco_ros2_control/MujocoSystemInterface 52 | $(find my_description)/description/scene.xml 53 | 54 | 60 | $(find my_description)/config/pids.yaml 61 | 62 | 68 | 5.0 69 | 70 | 77 | $(find my_description)/config/start_positions.xml 78 | 79 | 84 | 6.0 85 | 86 | 90 | 10.0 91 | 92 | 96 | false 97 | 98 | ... 99 | ``` 100 | 101 | Due to compatibility issues, we use a [slightly modified ROS 2 control node](./src/mujoco_ros2_control_node.cpp). 102 | It is the same executable and parameters as the upstream, but requires updating the launchfile: 103 | 104 | ```python 105 | control_node = Node( 106 | # Specify the control node from this package! 107 | package="mujoco_ros2_control", 108 | executable="ros2_control_node", 109 | output="both", 110 | parameters=[ 111 | {"use_sim_time": True}, 112 | controller_parameters, 113 | ], 114 | ) 115 | ``` 116 | 117 | > [!NOTE] 118 | > We can remove the the ROS 2 control node after the next ros2_control upstream release, 119 | > as the simulation requires [this PR](https://github.com/ros-controls/ros2_control/pull/2654) to run. 120 | > The hardware interface _should_ then be compatible with `humble`, `jazzy`, and `kilted`. 121 | 122 | ### Joints 123 | 124 | Joints in the ros2_control interface are mapped to actuators defined in the MJCF. 125 | The system supports different joint control modes based on the actuator type and available command interfaces. 126 | 127 | We rely on MuJoCo's PD-level ctrl input for direct position, velocity, or effort control. 128 | For velocity, motor, or custom actuators, a position or velocity PID is created if specified using ROS parameters to enable accurate control. 129 | Incompatible actuator-interface combinations trigger an error. 130 | 131 | Refer to Mujoco's [actuation model](https://mujoco.readthedocs.io/en/stable/computation/index.html#geactuation) for more information. 132 | 133 | Of note, only one type of MuJoCo actuator per-joint can be controllable at a time, and the type CANNOT be switched during runtime (i.e., switching from position to motor actuator is not supported). 134 | However, the active command interface can be switched dynamically, allowing control to shift between position, velocity, or effort as supported by the actuator type. 135 | Users are required to manually adjust actuator types and command interfaces to ensure that they are compatible. 136 | 137 | For example a position controlled joint on the mujoco 138 | 139 | ```xml 140 | 141 | 142 | 143 | ``` 144 | 145 | Could map to the following hardware interface: 146 | 147 | ```xml 148 | 149 | 150 | 151 | 152 | 0.0 153 | 154 | 155 | 156 | 157 | ``` 158 | 159 | **Supported modes between MuJoCo actuators and ros2_control command interfaces:** 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 |

		MuJoCo Actuators
		position	velocity	motor, general, etc
ros2 control command interfaces	position	Native support	Supported using PIDs	Supported using PIDs
	velocity	Not supported	Native support	Supported using PIDs
	effort	Not supported	Not supported	Native support

197 | 198 | > [!NOTE] 199 | > The `torque` and `force` command/state interfaces are semantically equivalent to `effort`, and map to the same underlying data in the sim. 200 | 201 | Switching actuator/control types on the fly is an [open issue](#13). 202 | 203 | ### Sensors 204 | 205 | The hardware interfaces supports force-torque sensors (FTS) and inertial measurement units (IMUs). 206 | Mujoco does not support modeling complete FTS and IMUs out of the box, so we combine supported MJCF constructs to map to a ros2_control sensor. 207 | The types and other parameters can be specified in the ros2_control xacro, as noted below. 208 | 209 | For FTS, we model the `force` and `torque` sensors individually in the MJFC. 210 | For a sensor named `fts_sensor`, we suffix each entry accordingly as `fts_sensor_force` and `fts_sensor_torque`. 211 | For example, in the MJCF, 212 | 213 | ```xml 214 | 215 | 216 | 217 | 218 | ``` 219 | 220 | In the corresponding ros2_control xacro, this becomes a single sensor: 221 | 222 | ```xml 223 | 224 | fts 225 | 226 | fts_sensor 227 | 228 | _force 229 | 230 | _torque 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | ``` 239 | 240 | Similarly, for an IMU, we simulate a `framequat`, `gyro`, and `accelerometer` as a single IMU. 241 | 242 | ```xml 243 | 244 | 245 | 246 | 247 | 248 | ``` 249 | 250 | Which then map to the corresponding ros2_control sensor: 251 | 252 | ```xml 253 | 254 | imu 255 | 256 | imu_sensor 257 | 258 | _quat 259 | 260 | _gyro 261 | 262 | _accel 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | ``` 275 | 276 | These sensor state interfaces can then be used out of the box with the standard broadcasters. 277 | 278 | ### Cameras 279 | 280 | Any `camera` included in the MJCF will automatically have its RGB-D images and info published to ROS topics. 281 | Currently all images are published at a fixed 5hz rate. 282 | 283 | Cameras must include a string ``, which sets defaults for the frame and topic names. 284 | By default, the ROS 2 wrapper assumes the camera is attached to a frame named `_frame`. 285 | Additionally camera_info, color, and depth images will be published to topics called `/camera_info`, `/color`, and `/depth`, respectively. 286 | Also note that MuJuCo's conventions for cameras are different than ROS's, and which must be accounted for. 287 | Refer to the documentation for more information. 288 | 289 | For example, 290 | 291 | ```xml 292 | 293 | ``` 294 | 295 | Will publish the following topics: 296 | 297 | ```bash 298 | $ ros2 topic info /wrist_mounted_camera/camera_info 299 | Type: sensor_msgs/msg/CameraInfo 300 | $ ros2 topic info /wrist_mounted_camera/color 301 | Type: sensor_msgs/msg/Image 302 | $ ros2 topic info /wrist_mounted_camera/depth 303 | Type: sensor_msgs/msg/Image 304 | ``` 305 | 306 | The frame and topic names are also configurable from the ros2_control xacro. 307 | Default parameters can be overridden with: 308 | 309 | ```xml 310 | 311 | 312 | wrist_mounted_camera_mujoco_frame 313 | /wrist_mounted_camera/color/camera_info 314 | /wrist_mounted_camera/color/image_raw 315 | /wrist_mounted_camera/aligned_depth_to_color/image_raw 316 | 317 | ``` 318 | 319 | ### Lidar 320 | 321 | MuJoCo does not include native support for lidar sensors. 322 | However, this package offers a ROS 2-like lidar implementation by wrapping sets of [rangefinders](https://mujoco.readthedocs.io/en/stable/XMLreference.html#sensor-rangefinder) together. 323 | 324 | MuJoCo rangefinders measure the distance to the nearest surface along the positive `Z` axis of the sensor site. 325 | The ROS 2 lidar wrapper uses the standard defined in [LaserScan](https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/LaserScan.msg#L10) messages. 326 | In particular, the first rangefinder's `Z` axis (e.g. `rf-00`) must align with the ROS 2 lidar sensor's positive `X` axis. 327 | 328 | In the MJCF, use the `replicate` tag along with a `-` separator to add N sites to attach sensors to. 329 | For example, the following will add 12 sites named `rf-00` to `rf-11` each at a 0.025 radian offset from each other: 330 | 331 | ```xml 332 | 333 | 334 | 335 | ``` 336 | 337 | Then a set of rangefinders can be attached to each site with: 338 | 339 | ```xml 340 | 341 | 342 | 343 | 344 | ``` 345 | 346 | The lidar sensor is then configurable through ROS 2 control xacro with: 347 | 348 | ```xml 349 | 350 | 351 | 352 | 353 | 354 | 355 | lidar_sensor_frame 356 | 0.025 357 | 12 358 | 0.05 359 | 10 360 | /scan 361 | 362 | 363 | ``` 364 | 365 | ## Test Robot System 366 | 367 | While examples are limited, we maintain a functional example 2-dof robot system in the [test examples](./test/test_resources/test_robot.urdf) space. 368 | We generally recommend looking there for examples and recommended workflows. 369 | 370 | For now, built the drivers with testing enabled, then the test robot system can be launched with: 371 | 372 | ```bash 373 | # Brings up the hardware drivers and mujoco interface, along with a single position controller 374 | ros2 launch mujoco_ros2_control test_robot.launch.py 375 | 376 | # Or optionally include the PID controller as mentioned above 377 | ros2 launch mujoco_ros2_control test_robot.launch.py use_pid:=true 378 | 379 | # Launch an rviz2 window with the provided configuration 380 | rviz2 -d $(ros2 pkg prefix --share mujoco_ros2_control)/config/test_robot.rviz 381 | ``` 382 | 383 | From there, command joints to move with, 384 | 385 | ```bash 386 | ros2 topic pub /position_controller/commands std_msgs/msg/Float64MultiArray "data: [-0.25, 0.75]" --once 387 | ``` 388 | 389 | > [!TIP] 390 | > UI panels can be toggled with `Tab` or `Shift+Tab`. 391 | > All standard MuJoCo keyboard shortcuts are available. 392 | > To see a short list, press `F1`. 393 | 394 | ## Development 395 | 396 | More information is provided in the [developers guide](./docs/DEVELOPMENT.md) document. 397 | --------------------------------------------------------------------------------