├── .dockerignore ├── doc ├── images │ ├── diff_drive.gif │ └── cart_position.gif ├── moveit_doc.rst └── index.rst ├── .github ├── dependabot.yml └── workflows │ ├── stale.yml │ └── ci.yml ├── .gitignore ├── mujoco_ros2_control ├── mujoco_system_plugins.xml ├── package.xml ├── include │ └── mujoco_ros2_control │ │ ├── mujoco_system_interface.hpp │ │ ├── mujoco_ros2_control.hpp │ │ ├── mujoco_cameras.hpp │ │ ├── mujoco_rendering.hpp │ │ └── mujoco_system.hpp ├── CMakeLists.txt └── src │ ├── mujoco_ros2_control_node.cpp │ ├── mujoco_rendering.cpp │ ├── mujoco_cameras.cpp │ ├── mujoco_ros2_control.cpp │ └── mujoco_system.cpp ├── .clang-format ├── mujoco_ros2_control_demos ├── config │ ├── ft_broadcaster.yaml │ ├── camera_controller_position.yaml │ ├── cartpole_controller_velocity.yaml │ ├── gripper_controller.yaml │ ├── cartpole_controller_position.yaml │ ├── cartpole_controller_effort.yaml │ ├── diff_drive_controller.yaml │ └── tricycle_drive_controller.yaml ├── mujoco_models │ ├── test_vertical_cart.xml │ ├── test_cart.xml │ ├── test_ft_sensor.xml │ ├── test_gripper_mimic_joint.xml │ ├── test_camera.xml │ ├── test_diff_drive.xml │ └── test_tricycle_drive.xml ├── package.xml ├── examples │ ├── example_tricycle_drive.cpp │ ├── example_diff_drive.cpp │ ├── example_effort.cpp │ ├── example_velocity.cpp │ ├── example_gripper.cpp │ ├── example_camera.cpp │ └── example_position.cpp ├── urdf │ ├── test_vertical_cart_position_pid.xacro.urdf │ ├── test_vertical_cart_velocity_pid.xacro.urdf │ ├── test_cart_velocity.xacro.urdf │ ├── test_cart_position.xacro.urdf │ ├── test_camera.xacro.urdf │ ├── test_ft_sensor.xacro.urdf │ ├── test_gripper_mimic_joint.xacro.urdf │ ├── test_cart_effort.xacro.urdf │ ├── test_diff_drive.xacro.urdf │ └── test_tricycle_drive.xacro.urdf ├── CMakeLists.txt └── launch │ ├── ft_sensor_example.launch.py │ ├── gripper_mimic_joint_example.launch.py │ ├── cart_example_velocity.launch.py │ ├── tricycle_drive.launch.py │ ├── diff_drive.launch.py │ ├── cart_example_position.launch.py │ ├── vertical_cart_example_velocity.launch.py │ ├── vertical_cart_example_position_pid.launch.py │ ├── cart_example_effort.launch.py │ ├── camera_example.launch.py │ └── camera_demo.rviz ├── docker ├── run.sh ├── build.sh ├── Dockerfile └── RUNNING_IN_DOCKER.md ├── LICENSE ├── README.md ├── CONTRIBUTING.md └── .pre-commit-config.yaml /.dockerignore: -------------------------------------------------------------------------------- 1 | .git/ 2 | doc/ 3 | docker/ 4 | \.* 5 | 6 | LICENSE 7 | *.md 8 | -------------------------------------------------------------------------------- /doc/images/diff_drive.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/mujoco_ros2_control/HEAD/doc/images/diff_drive.gif -------------------------------------------------------------------------------- /doc/images/cart_position.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/moveit/mujoco_ros2_control/HEAD/doc/images/cart_position.gif -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: "github-actions" 4 | directory: "/" 5 | schedule: 6 | interval: "weekly" 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # ros2 build 2 | build/ 3 | install/ 4 | log/ 5 | 6 | # vscode 7 | **/.vscode 8 | **/.devcontainer 9 | 10 | # mujoco log 11 | **/MUJOCO_LOG.TXT 12 | -------------------------------------------------------------------------------- /mujoco_ros2_control/mujoco_system_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | MujocoSystem plugin 4 | 5 | 6 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Google 3 | 4 | ColumnLimit: 100 5 | AccessModifierOffset: -2 6 | AlignAfterOpenBracket: AlwaysBreak 7 | BreakBeforeBraces: Allman 8 | ContinuationIndentWidth: 2 9 | DerivePointerAlignment: false 10 | PointerAlignment: Right 11 | ReflowComments: true 12 | IncludeBlocks: Preserve 13 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/ft_broadcaster.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | force_torque_broadcaster: 6 | type: force_torque_sensor_broadcaster/ForceTorqueSensorBroadcaster 7 | 8 | force_torque_broadcaster: 9 | ros__parameters: 10 | frame_id: motor_fts 11 | sensor_name: motor_fts 12 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/camera_controller_position.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 | - camera_joint 15 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/cartpole_controller_velocity.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | velocity_controller: 6 | type: velocity_controllers/JointGroupVelocityController 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | velocity_controller: 12 | ros__parameters: 13 | joints: 14 | - slider_to_cart 15 | -------------------------------------------------------------------------------- /docker/run.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Default values, modify as needed, depending on build 4 | CONTAINER_NAME="mujoco_ros2_ws" 5 | IMAGE_NAME="mujoco_ros2" 6 | TAG="latest" 7 | 8 | docker run --rm \ 9 | -it \ 10 | --network host \ 11 | -e DISPLAY \ 12 | -e QT_X11_NO_MITSHM=1 \ 13 | -v /tmp/.X11-unix:/tmp/.X11-unix:ro \ 14 | --name ${CONTAINER_NAME} \ 15 | ${IMAGE_NAME}:${TAG} 16 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/gripper_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | gripper_controller: 6 | type: forward_command_controller/ForwardCommandController 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | gripper_controller: 12 | ros__parameters: 13 | joints: 14 | - right_finger_joint 15 | interface_name: position 16 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/cartpole_controller_position.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | joint_trajectory_controller: 6 | type: joint_trajectory_controller/JointTrajectoryController 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | joint_trajectory_controller: 12 | ros__parameters: 13 | joints: 14 | - slider_to_cart 15 | interface_name: position 16 | command_interfaces: 17 | - position 18 | state_interfaces: 19 | - position 20 | - velocity 21 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/cartpole_controller_effort.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | 5 | effort_controller: 6 | type: effort_controllers/JointGroupEffortController 7 | 8 | joint_state_broadcaster: 9 | type: joint_state_broadcaster/JointStateBroadcaster 10 | 11 | imu_sensor_broadcaster: 12 | type: imu_sensor_broadcaster/IMUSensorBroadcaster 13 | 14 | effort_controller: 15 | ros__parameters: 16 | joints: 17 | - slider_to_cart 18 | 19 | imu_sensor_broadcaster: 20 | ros__parameters: 21 | sensor_name: cart_imu 22 | frame_id: cart_imu 23 | -------------------------------------------------------------------------------- /docker/build.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Get the directory of this script (where the Dockerfile is located) 4 | SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" 5 | 6 | # Default values, modify as needed 7 | IMAGE_NAME="mujoco_ros2" 8 | ROS_DISTRO="humble" 9 | MUJOCO_VERSION="3.2.7" 10 | TAG="latest" 11 | 12 | # Grab CPU architecture to support ARM machines 13 | ARCH="$(uname -m)" 14 | CPU_ARCH="x86_64" 15 | if [[ "$ARCH" == "aarch64" || "$ARCH" == "arm64" ]]; then 16 | CPU_ARCH="aarch64" 17 | fi 18 | 19 | # Build the Docker image, context from the root directory 20 | docker build --build-arg ROS_DISTRO="${ROS_DISTRO}" \ 21 | --build-arg MUJOCO_VERSION="${MUJOCO_VERSION}" \ 22 | --build-arg CPU_ARCH="${CPU_ARCH}" \ 23 | -f "${SCRIPT_DIR}/Dockerfile" \ 24 | -t "${IMAGE_NAME}:${TAG}" "${SCRIPT_DIR}/.." 25 | -------------------------------------------------------------------------------- /.github/workflows/stale.yml: -------------------------------------------------------------------------------- 1 | name: 'Close stale issues and PR' 2 | on: 3 | schedule: 4 | - cron: '30 1 * * *' 5 | 6 | jobs: 7 | stale: 8 | runs-on: ubuntu-latest 9 | steps: 10 | - uses: actions/stale@v10 11 | with: 12 | stale-issue-message: 'This issue is stale because it has been open 60 days with no activity. Remove stale label or comment or this will be closed in 30 days.' 13 | stale-pr-message: 'This PR is stale because it has been open 60 days with no activity. Please ensure the PR is still relevant. If not, please close the PR.' 14 | close-issue-message: 'This issue was closed because it has been stalled for 30 days with no activity.' 15 | days-before-stale: 60 16 | days-before-close: 30 17 | days-before-pr-close: -1 18 | stale-issue-label: 'stale' 19 | stale-pr-label: 'stale' 20 | exempt-issue-labels: 'question,bug,documentation,enhancement,good first issue,help wanted' 21 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_vertical_cart.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 SangtaekLee 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_cart.xml: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | ARG ROS_DISTRO=humble 2 | 3 | FROM ros:${ROS_DISTRO} 4 | 5 | ARG MUJOCO_VERSION=3.2.7 6 | ARG CPU_ARCH=x86_64 7 | 8 | SHELL ["/bin/bash", "-o", "pipefail", "-c"] 9 | 10 | # Configure base dependencies 11 | RUN apt-get update && apt-get upgrade -y 12 | RUN apt-get install -y python3-pip wget libglfw3-dev 13 | RUN pip3 install xacro 14 | 15 | # Configure and install MuJoCo 16 | ENV MUJOCO_VERSION=${MUJOCO_VERSION} 17 | ENV MUJOCO_DIR="/opt/mujoco/mujoco-${MUJOCO_VERSION}" 18 | RUN mkdir -p ${MUJOCO_DIR} 19 | 20 | RUN wget https://github.com/google-deepmind/mujoco/releases/download/${MUJOCO_VERSION}/mujoco-${MUJOCO_VERSION}-linux-${CPU_ARCH}.tar.gz 21 | RUN mkdir -p /home/mujoco 22 | RUN tar -xzf "mujoco-${MUJOCO_VERSION}-linux-${CPU_ARCH}.tar.gz" -C $(dirname "${MUJOCO_DIR}") 23 | 24 | # Install existing src and dependencies 25 | ENV ROS_WS="/home/ros2_ws/" 26 | RUN mkdir -p ${ROS_WS}/src 27 | WORKDIR ${ROS_WS} 28 | COPY . src/ 29 | 30 | # Install ROS dependencies 31 | RUN rosdep update && rosdep install --from-paths src/ --ignore-src -y 32 | 33 | # Compile 34 | RUN . /opt/ros/${ROS_DISTRO}/setup.bash && \ 35 | colcon build --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DBUILD_TESTING=ON 36 | 37 | RUN echo 'source ${ROS_WS}/install/setup.bash' >> ~/.bashrc 38 | 39 | CMD ["/bin/bash"] 40 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | mujoco_ros2_control_demos 7 | 0.1.0 8 | mujoco_ros2_control_demos 9 | 10 | Sangtaek Lee 11 | 12 | MIT 13 | 14 | Sangtaek Lee 15 | 16 | ament_cmake 17 | 18 | rclcpp 19 | rclcpp_action 20 | std_msgs 21 | control_msgs 22 | joint_trajectory_controller 23 | velocity_controllers 24 | force_torque_sensor_broadcaster 25 | imu_sensor_broadcaster 26 | 27 | position_controllers 28 | 29 | ament_lint_auto 30 | ament_cmake_clang_format 31 | ament_cmake_cppcheck 32 | ament_cmake_cpplint 33 | ament_cmake_lint_cmake 34 | ament_cmake_copyright 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /mujoco_ros2_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | mujoco_ros2_control 7 | 0.1.0 8 | mujoco_ros2_control 9 | 10 | Sangtaek Lee 11 | 12 | MIT 13 | 14 | Sangtaek Lee 15 | 16 | ament_cmake 17 | 18 | rclcpp 19 | hardware_interface 20 | controller_manager 21 | joint_limits 22 | pluginlib 23 | urdf 24 | control_toolbox 25 | ros2controlcli 26 | joint_state_broadcaster 27 | effort_controllers 28 | 29 | ament_lint_auto 30 | ament_cmake_clang_format 31 | ament_cmake_cppcheck 32 | ament_cmake_cpplint 33 | ament_cmake_lint_cmake 34 | ament_cmake_copyright 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_ft_sensor.xml: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_camera.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 | -------------------------------------------------------------------------------- /docker/RUNNING_IN_DOCKER.md: -------------------------------------------------------------------------------- 1 | # Running in Docker 2 | 3 | We have provided a basic container to run the provided demonstrations in isolation. 4 | The [Dockerfile](Dockerfile) handles installing dependencies, MuJoCo, and building the packages in this environment. 5 | 6 | To build, clone the repo and run [the build script](./build.sh), e.g. from the repo root: 7 | 8 | ```bash 9 | ./docker/build.sh 10 | ``` 11 | 12 | This will create a local image (generally `mujoco_ros2:latest`): 13 | 14 | ```bash 15 | $ docker image ls 16 | REPOSITORY TAG IMAGE ID CREATED SIZE 17 | mujoco_ros2 latest 37874e0d663a 6 minutes ago 1.52GB 18 | ``` 19 | 20 | To launch the container use [the run script](./run.sh), which will drop you into the pre-compiled workspace. 21 | From here launch any of the demo launch scripts: 22 | 23 | ```bash 24 | $ ./docker/run.sh 25 | root@ubuntu-linux:/home/ros2_ws# ros2 launch mujoco_ros2_control_demos cart_example_position.launch.py 26 | ... 27 | ``` 28 | 29 | The run script includes the required variables to enable a Ubuntu host to pass through varibles to access the display. 30 | However, it may be required to give permissions prior to launching the container with: 31 | 32 | ```bash 33 | # Run on host prior to launching the container 34 | xhost +local:docker 35 | ``` 36 | 37 | To test the MuJoCo install and rendering: 38 | 39 | ```bash 40 | $ ./docker/run.sh 41 | root@ubuntu-linux:/home/ros2_ws# ${MUJOCO_DIR}/bin/simulate ${MUJOCO_DIR}/model/humanoid/humanoid.xml 42 | ``` 43 | -------------------------------------------------------------------------------- /.github/workflows/ci.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | on: 3 | workflow_dispatch: 4 | pull_request: 5 | merge_group: 6 | push: 7 | branches: 8 | - main 9 | 10 | env: 11 | ROS_DISTRO: humble 12 | 13 | jobs: 14 | pre-commit: 15 | runs-on: ubuntu-22.04 16 | steps: 17 | - name: Checkout code from current branch 18 | uses: actions/checkout@v5 19 | - name: Setup ROS 20 | uses: ros-tooling/setup-ros@v0.7 21 | - name: Install dependencies 22 | run: | 23 | sudo apt-get update 24 | sudo rosdep update --rosdistro ${{ env.ROS_DISTRO }} 25 | sudo rosdep install --from-paths . --ignore-src -y -t test --rosdistro ${{ env.ROS_DISTRO }} 26 | - name: Run pre-commit 27 | run: | 28 | pip3 install pre-commit 29 | source /opt/ros/${{ env.ROS_DISTRO }}/setup.bash 30 | pre-commit run --all-files 31 | 32 | build_and_test: 33 | needs: pre-commit 34 | runs-on: ubuntu-latest 35 | steps: 36 | - name: Checkout code from current branch 37 | uses: actions/checkout@v5 38 | - name: Set up Docker Buildx 39 | uses: docker/setup-buildx-action@v3 40 | - name: Build in Docker container 41 | uses: docker/build-push-action@v6 42 | with: 43 | context: . 44 | file: docker/Dockerfile 45 | push: false 46 | load: true 47 | tags: mujoco_ros2_control:latest 48 | - name: Test in Docker container 49 | run: docker run --entrypoint "/bin/bash" --rm mujoco_ros2_control:latest -c "colcon test && colcon test-result --verbose" 50 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_tricycle_drive.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 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 | 17 | #include 18 | 19 | #include 20 | 21 | using namespace std::chrono_literals; 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | std::shared_ptr node = 28 | std::make_shared("tricycle_drive_test_node"); 29 | 30 | auto publisher = node->create_publisher( 31 | "/tricycle_controller/cmd_vel", 10); 32 | 33 | RCLCPP_INFO(node->get_logger(), "node created"); 34 | 35 | geometry_msgs::msg::Twist command; 36 | 37 | command.linear.x = 0.2; 38 | command.linear.y = 0.0; 39 | command.linear.z = 0.0; 40 | 41 | command.angular.x = 0.0; 42 | command.angular.y = 0.0; 43 | command.angular.z = 0.1; 44 | 45 | while (1) { 46 | publisher->publish(command); 47 | std::this_thread::sleep_for(50ms); 48 | rclcpp::spin_some(node); 49 | } 50 | rclcpp::shutdown(); 51 | 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_diff_drive.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Open Source Robotics Foundation, Inc. 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 | 17 | #include 18 | 19 | #include 20 | 21 | using namespace std::chrono_literals; 22 | 23 | int main(int argc, char * argv[]) 24 | { 25 | rclcpp::init(argc, argv); 26 | 27 | std::shared_ptr node = 28 | std::make_shared("diff_drive_test_node"); 29 | 30 | auto publisher = node->create_publisher( 31 | "/diff_drive_base_controller/cmd_vel_unstamped", 10); 32 | 33 | RCLCPP_INFO(node->get_logger(), "node created"); 34 | 35 | geometry_msgs::msg::Twist command; 36 | 37 | command.linear.x = 0.2; 38 | command.linear.y = 0.0; 39 | command.linear.z = 0.0; 40 | 41 | command.angular.x = 0.0; 42 | command.angular.y = 0.0; 43 | command.angular.z = 0.1; 44 | 45 | while (1) { 46 | publisher->publish(command); 47 | std::this_thread::sleep_for(50ms); 48 | rclcpp::spin_some(node); 49 | } 50 | rclcpp::shutdown(); 51 | 52 | return 0; 53 | } 54 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # mujoco_ros2_control 2 | 3 | [![License](https://img.shields.io/badge/License-MIT-blue.svg)](LICENSE) 4 | 5 | ## Overview 6 | 7 | This repository contains a ROS2 control package for Mujoco simulation, offering the `MujocoSystem` plugin to integrate `ros2_control` with Mujoco. Additionally, it includes a node responsible for initializing the plugin, Mujoco rendering, and the simulation. 8 | 9 | ## Installation Guide 10 | 11 | Follow these steps to install and run the project locally. 12 | 13 | ### Prerequisites 14 | 15 | Make sure you have the following software installed if you are running on the local machine: 16 | 17 | - [ROS](https://docs.ros.org/) 18 | - [Mujoco](https://mujoco.org/) 19 | 20 | ### Package Install 21 | 22 | Before build this package configure environment variable for mujoco directory. 23 | 24 | ```bash 25 | export MUJOCO_DIR=/PATH/TO/MUJOCO/mujoco-3.x.x 26 | ``` 27 | 28 | You can now compile the package using the following commands. 29 | 30 | ```bash 31 | cd mujoco_ros2_control 32 | source /opt/ros/${ROS_DISTRO}/setup.bash 33 | colcon build 34 | ``` 35 | 36 | ## Usage 37 | 38 | See the [documentation](doc/index.rst) for usage. 39 | 40 | ## Docker 41 | 42 | A basic containerized workflow is provided to test this package in isolation. 43 | For more information refer to the [docker documentation](docker/RUNNING_IN_DOCKER.md). 44 | 45 | ## Future Work 46 | 47 | Here are several potential areas for future improvement: 48 | 49 | 1. **Sensors:** Implement IMU sensors, and range sensors. 50 | 51 | 2. **Loading Model From URDF:** Implement direct loading of models from URDF, eliminating the need to convert URDF files to XML. 52 | 53 | Feel free to suggest ideas for new features or improvements. 54 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contributing to mujoco_ros2_control 2 | 3 | Thank you for considering contributing to our project! We welcome contributions in all forms, from bug fixes and documentation improvements to new features. Please read our guidelines carefully and ensure you follow contribution guidelines throughout your participation. 4 | 5 | ## How to Contribute 6 | 7 | ### 1. Reporting Issues 8 | 9 | - **Check Existing Issues:** Before opening a new issue, please search through the existing issues to see if someone else has already reported it. 10 | - **Bug Reports:** If you find a bug, please open an issue with a detailed description and, if possible, steps to reproduce it. 11 | - **Feature Requests:** For new features or improvements, please open an issue to discuss the idea before starting work. 12 | 13 | ### 2. Submitting Pull Requests 14 | 15 | When you're ready to contribute code or documentation, please follow these steps: 16 | 17 | - Click the "Fork" button at the top right of the repository page to create your own copy. 18 | - Clone your forked repository. 19 | - Create a feature branch. 20 | - Implement your feature or bug fix. 21 | - **Before submitting your pull request, run the following commands to ensure everything is in order:** 22 | 23 | ```bash 24 | # Run Unit/Integration Tests 25 | colcon test 26 | ``` 27 | 28 | ```bash 29 | # Run Pre-Commit Hooks 30 | pre-commit run -a 31 | ``` 32 | 33 | - Commit and push your changes 34 | - Open a pull request 35 | 36 | ## License 37 | 38 | Any contribution that you make to this repository will 39 | be under the MIT license, as dictated by that 40 | [license](https://opensource.org/licenses/MIT). 41 | 42 | ## Thank You 43 | 44 | We appreciate your time and effort in contributing to our project. 45 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_effort.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Open Source Robotics Foundation, Inc. 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 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | #include "std_msgs/msg/float64_multi_array.hpp" 22 | 23 | std::shared_ptr node; 24 | 25 | int main(int argc, char * argv[]) 26 | { 27 | rclcpp::init(argc, argv); 28 | 29 | node = std::make_shared("effort_test_node"); 30 | 31 | auto publisher = node->create_publisher( 32 | "/effort_controller/commands", 10); 33 | 34 | RCLCPP_INFO(node->get_logger(), "node created"); 35 | 36 | std_msgs::msg::Float64MultiArray commands; 37 | 38 | using namespace std::chrono_literals; 39 | 40 | commands.data.push_back(0); 41 | publisher->publish(commands); 42 | std::this_thread::sleep_for(1s); 43 | 44 | commands.data[0] = 100; 45 | publisher->publish(commands); 46 | std::this_thread::sleep_for(1s); 47 | 48 | commands.data[0] = -200; 49 | publisher->publish(commands); 50 | std::this_thread::sleep_for(1s); 51 | 52 | commands.data[0] = 0; 53 | publisher->publish(commands); 54 | std::this_thread::sleep_for(1s); 55 | rclcpp::shutdown(); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_velocity.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Open Source Robotics Foundation, Inc. 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 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | #include "std_msgs/msg/float64_multi_array.hpp" 22 | 23 | std::shared_ptr node; 24 | 25 | int main(int argc, char * argv[]) 26 | { 27 | rclcpp::init(argc, argv); 28 | 29 | node = std::make_shared("velocity_test_node"); 30 | 31 | auto publisher = node->create_publisher( 32 | "/velocity_controller/commands", 10); 33 | 34 | RCLCPP_INFO(node->get_logger(), "node created"); 35 | 36 | std_msgs::msg::Float64MultiArray commands; 37 | 38 | using namespace std::chrono_literals; 39 | 40 | commands.data.push_back(0); 41 | publisher->publish(commands); 42 | std::this_thread::sleep_for(1s); 43 | 44 | commands.data[0] = 1; 45 | publisher->publish(commands); 46 | std::this_thread::sleep_for(1s); 47 | 48 | commands.data[0] = -1; 49 | publisher->publish(commands); 50 | std::this_thread::sleep_for(1s); 51 | 52 | commands.data[0] = 0; 53 | publisher->publish(commands); 54 | std::this_thread::sleep_for(1s); 55 | rclcpp::shutdown(); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_gripper.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Stogl Robotics Consulting UG (haftungsbeschränkt) 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 | 19 | #include "rclcpp/rclcpp.hpp" 20 | 21 | #include "std_msgs/msg/float64_multi_array.hpp" 22 | 23 | std::shared_ptr node; 24 | 25 | int main(int argc, char * argv[]) 26 | { 27 | rclcpp::init(argc, argv); 28 | 29 | node = std::make_shared("gripper_test_node"); 30 | 31 | auto publisher = node->create_publisher( 32 | "/gripper_controller/commands", 10); 33 | 34 | RCLCPP_INFO(node->get_logger(), "node created"); 35 | 36 | std_msgs::msg::Float64MultiArray commands; 37 | 38 | using namespace std::chrono_literals; 39 | 40 | commands.data.push_back(0); 41 | publisher->publish(commands); 42 | std::this_thread::sleep_for(1s); 43 | 44 | commands.data[0] = 0.38; 45 | publisher->publish(commands); 46 | std::this_thread::sleep_for(1s); 47 | 48 | commands.data[0] = 0.19; 49 | publisher->publish(commands); 50 | std::this_thread::sleep_for(1s); 51 | 52 | commands.data[0] = 0; 53 | publisher->publish(commands); 54 | std::this_thread::sleep_for(1s); 55 | rclcpp::shutdown(); 56 | 57 | return 0; 58 | } 59 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/diff_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 100 # Hz 4 | use_sim_time: true 5 | 6 | joint_state_broadcaster: 7 | type: joint_state_broadcaster/JointStateBroadcaster 8 | 9 | diff_drive_base_controller: 10 | type: diff_drive_controller/DiffDriveController 11 | 12 | diff_drive_base_controller: 13 | ros__parameters: 14 | left_wheel_names: ["left_wheel_joint"] 15 | right_wheel_names: ["right_wheel_joint"] 16 | 17 | wheel_separation: 1.25 18 | #wheels_per_side: 1 # actually 2, but both are controlled by 1 signal 19 | wheel_radius: 0.3 20 | 21 | wheel_separation_multiplier: 1.0 22 | left_wheel_radius_multiplier: 1.0 23 | right_wheel_radius_multiplier: 1.0 24 | 25 | publish_rate: 50.0 26 | odom_frame_id: odom 27 | base_frame_id: chassis 28 | pose_covariance_diagonal : [0.001, 0.001, 0.0, 0.0, 0.0, 0.01] 29 | twist_covariance_diagonal: [0.001, 0.0, 0.0, 0.0, 0.0, 0.01] 30 | 31 | open_loop: true 32 | enable_odom_tf: true 33 | 34 | cmd_vel_timeout: 0.5 35 | # publish_limited_velocity: true 36 | use_stamped_vel: false 37 | #velocity_rolling_window_size: 10 38 | 39 | # Velocity and acceleration limits 40 | # Whenever a min_* is unspecified, default to -max_* 41 | linear.x.has_velocity_limits: true 42 | linear.x.has_acceleration_limits: true 43 | linear.x.has_jerk_limits: false 44 | linear.x.max_velocity: 1.0 45 | linear.x.min_velocity: -1.0 46 | linear.x.max_acceleration: 1.0 47 | linear.x.max_jerk: 0.0 48 | linear.x.min_jerk: 0.0 49 | 50 | angular.z.has_velocity_limits: true 51 | angular.z.has_acceleration_limits: true 52 | angular.z.has_jerk_limits: false 53 | angular.z.max_velocity: 1.0 54 | angular.z.min_velocity: -1.0 55 | angular.z.max_acceleration: 1.0 56 | angular.z.min_acceleration: -1.0 57 | angular.z.max_jerk: 0.0 58 | angular.z.min_jerk: 0.0 59 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | repos: 2 | - repo: https://github.com/pre-commit/pre-commit-hooks 3 | rev: v4.6.0 4 | hooks: 5 | - id: check-added-large-files 6 | - id: check-case-conflict 7 | - id: check-docstring-first 8 | - id: check-merge-conflict 9 | - id: end-of-file-fixer 10 | - id: mixed-line-ending 11 | - id: trailing-whitespace 12 | exclude_types: [rst, markdown] 13 | 14 | - repo: local 15 | hooks: 16 | - id: ament_clang_format 17 | name: amend_clang_format 18 | entry: ament_clang_format 19 | language: system 20 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 21 | args: ["--config=.clang-format", "--reformat"] 22 | exclude: mujoco_ros2_control_demos/examples 23 | 24 | - repo: local 25 | hooks: 26 | - id: ament_cppcheck 27 | name: ament_cppcheck 28 | entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck 29 | language: system 30 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 31 | 32 | - repo: local 33 | hooks: 34 | - id: ament_cpplint 35 | name: ament_cpplint 36 | entry: ament_cpplint 37 | language: system 38 | files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ 39 | args: ["--linelength=100", "--filter=-whitespace/newline"] 40 | 41 | - repo: local 42 | hooks: 43 | - id: ament_lint_cmake 44 | name: ament_lint_cmake 45 | entry: ament_lint_cmake 46 | language: system 47 | files: CMakeLists\.txt$ 48 | 49 | - repo: local 50 | hooks: 51 | - id: ament_copyright 52 | name: ament_copyright 53 | entry: ament_copyright 54 | language: system 55 | exclude: '.*(conf\.py|launch\.py)$' 56 | 57 | - repo: https://github.com/igorshubovych/markdownlint-cli 58 | rev: v0.44.0 59 | hooks: 60 | - id: markdownlint 61 | # Ignore line length checks 62 | args: ["--disable", "MD013", "--"] 63 | -------------------------------------------------------------------------------- /mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #ifndef MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_INTERFACE_HPP_ 22 | #define MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_INTERFACE_HPP_ 23 | 24 | #include "hardware_interface/system_interface.hpp" 25 | #include "mujoco/mujoco.h" 26 | #include "rclcpp/rclcpp.hpp" 27 | #include "urdf/model.h" 28 | 29 | namespace mujoco_ros2_control 30 | { 31 | using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; 32 | 33 | class MujocoSystemInterface : public hardware_interface::SystemInterface 34 | { 35 | public: 36 | virtual bool init_sim( 37 | mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model, 38 | const hardware_interface::HardwareInfo &hardware_info) = 0; 39 | 40 | protected: 41 | }; 42 | } // namespace mujoco_ros2_control 43 | 44 | #endif // MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_INTERFACE_HPP_ 45 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.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 | mujoco_ros2_control/MujocoSystem 55 | 56 | 57 | 100 58 | 1 59 | 10 60 | 10000 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.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 | mujoco_ros2_control/MujocoSystem 55 | 56 | 57 | 10 58 | 10 59 | 0.1 60 | 10000 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mujoco_ros2_control_demos) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rclcpp_action REQUIRED) 18 | find_package(std_msgs REQUIRED) 19 | find_package(control_msgs REQUIRED) 20 | 21 | set(THIS_PACKAGE_DEPENDS 22 | ament_cmake 23 | rclcpp 24 | rclcpp_action 25 | std_msgs 26 | control_msgs 27 | ) 28 | 29 | install(DIRECTORY 30 | launch 31 | config 32 | urdf 33 | mujoco_models 34 | DESTINATION share/${PROJECT_NAME}/ 35 | ) 36 | 37 | add_executable(example_position examples/example_position.cpp) 38 | ament_target_dependencies(example_position ${THIS_PACKAGE_DEPENDS}) 39 | 40 | add_executable(example_velocity examples/example_velocity.cpp) 41 | ament_target_dependencies(example_velocity ${THIS_PACKAGE_DEPENDS}) 42 | 43 | add_executable(example_effort examples/example_effort.cpp) 44 | ament_target_dependencies(example_effort ${THIS_PACKAGE_DEPENDS}) 45 | 46 | add_executable(example_diff_drive examples/example_diff_drive.cpp) 47 | ament_target_dependencies(example_diff_drive ${THIS_PACKAGE_DEPENDS}) 48 | 49 | add_executable(example_tricycle_drive examples/example_tricycle_drive.cpp) 50 | ament_target_dependencies(example_tricycle_drive ${THIS_PACKAGE_DEPENDS}) 51 | 52 | add_executable(example_gripper examples/example_gripper.cpp) 53 | ament_target_dependencies(example_gripper ${THIS_PACKAGE_DEPENDS}) 54 | 55 | add_executable(example_camera examples/example_camera.cpp) 56 | ament_target_dependencies(example_camera ${THIS_PACKAGE_DEPENDS}) 57 | 58 | install(TARGETS 59 | example_position 60 | example_velocity 61 | example_effort 62 | example_diff_drive 63 | example_tricycle_drive 64 | example_gripper 65 | example_camera 66 | DESTINATION lib/${PROJECT_NAME} 67 | ) 68 | 69 | ament_package() 70 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/ft_sensor_example.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessStart, OnProcessExit 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_ft_sensor.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'ft_broadcaster.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_ft_sensor.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_ft_broadcaster = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'force_torque_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | return LaunchDescription([ 54 | RegisterEventHandler( 55 | event_handler=OnProcessStart( 56 | target_action=node_mujoco_ros2_control, 57 | on_start=[load_ft_broadcaster], 58 | ) 59 | ), 60 | 61 | node_mujoco_ros2_control, 62 | node_robot_state_publisher 63 | ]) 64 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_cart_velocity.xacro.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 | mujoco_ros2_control/MujocoSystem 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_cart_position.xacro.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 | mujoco_ros2_control/MujocoSystem 54 | 55 | 56 | 57 | 58 | 1.0 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 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 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/config/tricycle_drive_controller.yaml: -------------------------------------------------------------------------------- 1 | controller_manager: 2 | ros__parameters: 3 | update_rate: 50 # Hz 4 | use_sim_time: true 5 | 6 | tricycle_controller: 7 | type: tricycle_controller/TricycleController 8 | 9 | joint_state_broadcaster: 10 | type: joint_state_broadcaster/JointStateBroadcaster 11 | 12 | joint_state_broadcaster: 13 | ros__parameters: 14 | extra_joints: ["right_wheel_joint", "left_wheel_joint"] 15 | 16 | tricycle_controller: 17 | ros__parameters: 18 | # Model 19 | traction_joint_name: traction_joint # Name of traction joint in URDF 20 | steering_joint_name: steering_joint # Name of steering joint in URDF 21 | wheel_radius: 0.3 # Radius of front wheel 22 | wheelbase: 1.7 # Distance between center of back wheels and front wheel 23 | 24 | # Odometry 25 | odom_frame_id: odom 26 | base_frame_id: base_link 27 | open_loop: false # if True, uses cmd_vel instead of hardware interface feedback to compute odometry 28 | enable_odom_tf: true # If True, publishes odom<-base_link TF 29 | odom_only_twist: false # If True, publishes on /odom only linear.x and angular.z; Useful for computing odometry in another node, e.g robot_localization's ekf 30 | pose_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source 31 | twist_covariance_diagonal: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # Need to be set if fusing odom with other localization source 32 | velocity_rolling_window_size: 10 # Rolling window size of rcppmath::RollingMeanAccumulator applied on linear and angular speeds published on odom 33 | 34 | # Rate Limiting 35 | traction: # All values should be positive 36 | # min_velocity: 0.0 37 | # max_velocity: 1000.0 38 | # min_acceleration: 0.0 39 | max_acceleration: 5.0 40 | # min_deceleration: 0.0 41 | max_deceleration: 8.0 42 | # min_jerk: 0.0 43 | # max_jerk: 1000.0 44 | steering: 45 | min_position: -1.57 46 | max_position: 1.57 47 | # min_velocity: 0.0 48 | max_velocity: 1.0 49 | # min_acceleration: 0.0 50 | # max_acceleration: 1000.0 51 | 52 | # cmd_vel input 53 | cmd_vel_timeout: 500 # In milliseconds. Timeout to stop if no cmd_vel is received 54 | use_stamped_vel: false # Set to True if using TwistStamped. 55 | 56 | # Debug 57 | publish_ackermann_command: true # Publishes AckermannDrive. The speed does not comply to the msg definition, it the wheel angular speed in rad/s. 58 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_camera.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Erik Holum 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include 22 | #include 23 | 24 | #include "rclcpp/rclcpp.hpp" 25 | #include "std_msgs/msg/float64_multi_array.hpp" 26 | 27 | using namespace std::chrono_literals; 28 | 29 | class CameraJointPublisher : public rclcpp::Node 30 | { 31 | public: 32 | CameraJointPublisher() : Node("camera_joint_publisher") 33 | { 34 | publisher_ = 35 | this->create_publisher("/position_controller/commands", 10); 36 | start_time_ = this->now(); 37 | timer_ = this->create_wall_timer(100ms, std::bind(&CameraJointPublisher::timer_callback, this)); 38 | } 39 | 40 | private: 41 | void timer_callback() 42 | { 43 | double elapsed = (this->now() - start_time_).seconds(); 44 | double period = 3.0; 45 | double amplitude = 0.2; 46 | double angle = amplitude * std::sin(2 * M_PI * elapsed / period); 47 | 48 | std_msgs::msg::Float64MultiArray msg; 49 | msg.data.push_back(angle); 50 | 51 | publisher_->publish(msg); 52 | RCLCPP_INFO(this->get_logger(), "Publishing angle: %.3f", angle); 53 | } 54 | 55 | rclcpp::Publisher::SharedPtr publisher_; 56 | rclcpp::TimerBase::SharedPtr timer_; 57 | rclcpp::Time start_time_; 58 | }; 59 | 60 | int main(int argc, char *argv[]) 61 | { 62 | rclcpp::init(argc, argv); 63 | rclcpp::spin(std::make_shared()); 64 | rclcpp::shutdown(); 65 | return 0; 66 | } 67 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_camera.xacro.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 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | mujoco_ros2_control/MujocoSystem 74 | 75 | 76 | 77 | 78 | 0.0 79 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | from launch import LaunchDescription 5 | from launch.actions import ExecuteProcess, RegisterEventHandler 6 | from launch.event_handlers import OnProcessExit, OnProcessStart 7 | from launch_ros.actions import Node 8 | import xacro 9 | 10 | 11 | def generate_launch_description(): 12 | mujoco_ros2_control_demos_path = os.path.join( 13 | get_package_share_directory('mujoco_ros2_control_demos')) 14 | 15 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 16 | 'urdf', 17 | 'test_gripper_mimic_joint.xacro.urdf') 18 | 19 | doc = xacro.parse(open(xacro_file)) 20 | xacro.process_doc(doc) 21 | robot_description = {'robot_description': doc.toxml()} 22 | 23 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'gripper_controller.yaml') 24 | 25 | node_mujoco_ros2_control = Node( 26 | package='mujoco_ros2_control', 27 | executable='mujoco_ros2_control', 28 | output='screen', 29 | parameters=[ 30 | robot_description, 31 | controller_config_file, 32 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_gripper_mimic_joint.xml')} 33 | ] 34 | ) 35 | 36 | node_robot_state_publisher = Node( 37 | package='robot_state_publisher', 38 | executable='robot_state_publisher', 39 | output='screen', 40 | parameters=[robot_description] 41 | ) 42 | 43 | load_joint_state_controller = ExecuteProcess( 44 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 45 | 'joint_state_broadcaster'], 46 | output='screen' 47 | ) 48 | 49 | load_gripper_controller = ExecuteProcess( 50 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 51 | 'gripper_controller'], 52 | output='screen' 53 | ) 54 | 55 | return LaunchDescription([ 56 | RegisterEventHandler( 57 | event_handler=OnProcessStart( 58 | target_action=node_mujoco_ros2_control, 59 | on_start=[load_joint_state_controller], 60 | ) 61 | ), 62 | RegisterEventHandler( 63 | event_handler=OnProcessExit( 64 | target_action=load_joint_state_controller, 65 | on_exit=[load_gripper_controller], 66 | ) 67 | ), 68 | node_mujoco_ros2_control, 69 | node_robot_state_publisher 70 | ]) 71 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessExit, OnProcessStart 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_cart_velocity.xacro.urdf') 22 | doc = xacro.parse(open(xacro_file)) 23 | xacro.process_doc(doc) 24 | robot_description = {'robot_description': doc.toxml()} 25 | 26 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_velocity.yaml') 27 | 28 | node_mujoco_ros2_control = Node( 29 | package='mujoco_ros2_control', 30 | executable='mujoco_ros2_control', 31 | output='screen', 32 | parameters=[ 33 | robot_description, 34 | controller_config_file, 35 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} 36 | ] 37 | ) 38 | 39 | node_robot_state_publisher = Node( 40 | package='robot_state_publisher', 41 | executable='robot_state_publisher', 42 | output='screen', 43 | parameters=[robot_description] 44 | ) 45 | 46 | load_joint_state_controller = ExecuteProcess( 47 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 48 | 'joint_state_broadcaster'], 49 | output='screen' 50 | ) 51 | 52 | load_joint_trajectory_controller = ExecuteProcess( 53 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'velocity_controller'], 54 | output='screen' 55 | ) 56 | 57 | return LaunchDescription([ 58 | RegisterEventHandler( 59 | event_handler=OnProcessStart( 60 | target_action=node_mujoco_ros2_control, 61 | on_start=[load_joint_state_controller], 62 | ) 63 | ), 64 | RegisterEventHandler( 65 | event_handler=OnProcessExit( 66 | target_action=load_joint_state_controller, 67 | on_exit=[load_joint_trajectory_controller], 68 | ) 69 | ), 70 | node_mujoco_ros2_control, 71 | node_robot_state_publisher 72 | ]) 73 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/tricycle_drive.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessExit, OnProcessStart 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_tricycle_drive.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'tricycle_drive_controller.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_tricycle_drive.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_joint_state_controller = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'joint_state_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | load_tricycle_controller = ExecuteProcess( 54 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 55 | 'tricycle_controller'], 56 | output='screen' 57 | ) 58 | 59 | return LaunchDescription([ 60 | RegisterEventHandler( 61 | event_handler=OnProcessStart( 62 | target_action=node_mujoco_ros2_control, 63 | on_start=[load_joint_state_controller], 64 | ) 65 | ), 66 | RegisterEventHandler( 67 | event_handler=OnProcessExit( 68 | target_action=load_joint_state_controller, 69 | on_exit=[load_tricycle_controller], 70 | ) 71 | ), 72 | node_mujoco_ros2_control, 73 | node_robot_state_publisher 74 | ]) 75 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/diff_drive.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessExit, OnProcessStart 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_diff_drive.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'diff_drive_controller.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_diff_drive.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_joint_state_controller = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'joint_state_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | load_diff_drive_base_controller = ExecuteProcess( 54 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 55 | 'diff_drive_base_controller'], 56 | output='screen' 57 | ) 58 | 59 | return LaunchDescription([ 60 | RegisterEventHandler( 61 | event_handler=OnProcessStart( 62 | target_action=node_mujoco_ros2_control, 63 | on_start=[load_joint_state_controller], 64 | ) 65 | ), 66 | RegisterEventHandler( 67 | event_handler=OnProcessExit( 68 | target_action=load_joint_state_controller, 69 | on_exit=[load_diff_drive_base_controller], 70 | ) 71 | ), 72 | node_mujoco_ros2_control, 73 | node_robot_state_publisher 74 | ]) 75 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/cart_example_position.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessStart, OnProcessExit 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_cart_position.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_position.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_joint_state_controller = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'joint_state_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | load_joint_trajectory_controller = ExecuteProcess( 54 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 55 | 'joint_trajectory_controller'], 56 | output='screen' 57 | ) 58 | 59 | return LaunchDescription([ 60 | RegisterEventHandler( 61 | event_handler=OnProcessStart( 62 | target_action=node_mujoco_ros2_control, 63 | on_start=[load_joint_state_controller], 64 | ) 65 | ), 66 | RegisterEventHandler( 67 | event_handler=OnProcessExit( 68 | target_action=load_joint_state_controller, 69 | on_exit=[load_joint_trajectory_controller], 70 | ) 71 | ), 72 | node_mujoco_ros2_control, 73 | node_robot_state_publisher 74 | ]) 75 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/vertical_cart_example_velocity.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessExit, OnProcessStart 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_vertical_cart_velocity_pid.xacro.urdf') 22 | doc = xacro.parse(open(xacro_file)) 23 | xacro.process_doc(doc) 24 | robot_description = {'robot_description': doc.toxml()} 25 | 26 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_velocity.yaml') 27 | 28 | node_mujoco_ros2_control = Node( 29 | package='mujoco_ros2_control', 30 | executable='mujoco_ros2_control', 31 | output='screen', 32 | parameters=[ 33 | robot_description, 34 | controller_config_file, 35 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_vertical_cart.xml')} 36 | ] 37 | ) 38 | 39 | node_robot_state_publisher = Node( 40 | package='robot_state_publisher', 41 | executable='robot_state_publisher', 42 | output='screen', 43 | parameters=[robot_description] 44 | ) 45 | 46 | load_joint_state_controller = ExecuteProcess( 47 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 48 | 'joint_state_broadcaster'], 49 | output='screen' 50 | ) 51 | 52 | load_joint_trajectory_controller = ExecuteProcess( 53 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'velocity_controller'], 54 | output='screen' 55 | ) 56 | 57 | return LaunchDescription([ 58 | RegisterEventHandler( 59 | event_handler=OnProcessStart( 60 | target_action=node_mujoco_ros2_control, 61 | on_start=[load_joint_state_controller], 62 | ) 63 | ), 64 | RegisterEventHandler( 65 | event_handler=OnProcessExit( 66 | target_action=load_joint_state_controller, 67 | on_exit=[load_joint_trajectory_controller], 68 | ) 69 | ), 70 | node_mujoco_ros2_control, 71 | node_robot_state_publisher 72 | ]) 73 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessStart,OnProcessExit 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_vertical_cart_position_pid.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_position.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_vertical_cart.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_joint_state_controller = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'joint_state_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | load_joint_trajectory_controller = ExecuteProcess( 54 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 55 | 'joint_trajectory_controller'], 56 | output='screen' 57 | ) 58 | 59 | return LaunchDescription([ 60 | RegisterEventHandler( 61 | event_handler=OnProcessStart( 62 | target_action=node_mujoco_ros2_control, 63 | on_start=[load_joint_state_controller], 64 | ) 65 | ), 66 | RegisterEventHandler( 67 | event_handler=OnProcessExit( 68 | target_action=load_joint_state_controller, 69 | on_exit=[load_joint_trajectory_controller], 70 | ) 71 | ), 72 | node_mujoco_ros2_control, 73 | node_robot_state_publisher 74 | ]) 75 | -------------------------------------------------------------------------------- /mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #ifndef MUJOCO_ROS2_CONTROL__MUJOCO_ROS2_CONTROL_HPP_ 22 | #define MUJOCO_ROS2_CONTROL__MUJOCO_ROS2_CONTROL_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | #include "controller_manager/controller_manager.hpp" 28 | #include "pluginlib/class_loader.hpp" 29 | #include "rclcpp/rclcpp.hpp" 30 | #include "rosgraph_msgs/msg/clock.hpp" 31 | 32 | #include "mujoco/mujoco.h" 33 | 34 | #include "mujoco_ros2_control/mujoco_system.hpp" 35 | 36 | namespace mujoco_ros2_control 37 | { 38 | class MujocoRos2Control 39 | { 40 | public: 41 | MujocoRos2Control(rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data); 42 | ~MujocoRos2Control(); 43 | void init(); 44 | void update(); 45 | 46 | private: 47 | void publish_sim_time(rclcpp::Time sim_time); 48 | std::string get_robot_description(); 49 | rclcpp::Node::SharedPtr node_; 50 | mjModel *mj_model_; 51 | mjData *mj_data_; 52 | 53 | rclcpp::Logger logger_; 54 | std::shared_ptr> robot_hw_sim_loader_; 55 | 56 | std::shared_ptr controller_manager_; 57 | rclcpp::Executor::SharedPtr cm_executor_; 58 | std::thread cm_thread_; 59 | bool stop_cm_thread_; 60 | rclcpp::Duration control_period_; 61 | 62 | rclcpp::Time last_update_sim_time_ros_; 63 | rclcpp::Publisher::SharedPtr clock_publisher_; 64 | }; 65 | } // namespace mujoco_ros2_control 66 | 67 | #endif // MUJOCO_ROS2_CONTROL__MUJOCO_ROS2_CONTROL_HPP_ 68 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_ft_sensor.xacro.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 | mujoco_ros2_control/MujocoSystem 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | motor_fts 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /mujoco_ros2_control/include/mujoco_ros2_control/mujoco_cameras.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Erik Holum 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #pragma once 22 | 23 | #include 24 | #include 25 | 26 | #include "GLFW/glfw3.h" 27 | #include "mujoco/mujoco.h" 28 | #include "rclcpp/rclcpp.hpp" 29 | 30 | #include "rclcpp/node.hpp" 31 | #include "rclcpp/publisher.hpp" 32 | #include "sensor_msgs/msg/camera_info.hpp" 33 | #include "sensor_msgs/msg/image.hpp" 34 | 35 | namespace mujoco_ros2_control 36 | { 37 | 38 | struct CameraData 39 | { 40 | mjvCamera mjv_cam; 41 | mjrRect viewport; 42 | 43 | std::string name; 44 | std::string frame_name; 45 | uint32_t width; 46 | uint32_t height; 47 | 48 | std::vector image_buffer; 49 | std::vector depth_buffer; 50 | std::vector depth_buffer_flipped; 51 | 52 | sensor_msgs::msg::Image image; 53 | sensor_msgs::msg::Image depth_image; 54 | sensor_msgs::msg::CameraInfo camera_info; 55 | 56 | rclcpp::Publisher::SharedPtr image_pub; 57 | rclcpp::Publisher::SharedPtr depth_image_pub; 58 | rclcpp::Publisher::SharedPtr camera_info_pub; 59 | }; 60 | 61 | class MujocoCameras 62 | { 63 | public: 64 | explicit MujocoCameras(rclcpp::Node::SharedPtr &node); 65 | 66 | void init(mjModel *mujoco_model); 67 | void update(mjModel *mujoco_model, mjData *mujoco_data); 68 | void close(); 69 | 70 | private: 71 | void register_cameras(const mjModel *mujoco_model); 72 | 73 | rclcpp::Node::SharedPtr node_; 74 | 75 | // Rendering options for the cameras, currently hard coded to defaults 76 | mjvOption mjv_opt_; 77 | mjvScene mjv_scn_; 78 | mjrContext mjr_con_; 79 | 80 | // Containers for camera data and ROS constructs 81 | std::vector cameras_; 82 | }; 83 | 84 | } // namespace mujoco_ros2_control 85 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.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 | mujoco_ros2_control/MujocoSystem 72 | 73 | 74 | 75 | 76 | 0.15 77 | 78 | 79 | 80 | 81 | 82 | right_finger_joint 83 | 1 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/cart_example_effort.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | 3 | from ament_index_python.packages import get_package_share_directory 4 | 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessStart, OnProcessExit 9 | 10 | from launch_ros.actions import Node 11 | 12 | import xacro 13 | 14 | 15 | def generate_launch_description(): 16 | mujoco_ros2_control_demos_path = os.path.join( 17 | get_package_share_directory('mujoco_ros2_control_demos')) 18 | 19 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 20 | 'urdf', 21 | 'test_cart_effort.xacro.urdf') 22 | 23 | doc = xacro.parse(open(xacro_file)) 24 | xacro.process_doc(doc) 25 | robot_description = {'robot_description': doc.toxml()} 26 | 27 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_effort.yaml') 28 | 29 | node_mujoco_ros2_control = Node( 30 | package='mujoco_ros2_control', 31 | executable='mujoco_ros2_control', 32 | output='screen', 33 | parameters=[ 34 | robot_description, 35 | controller_config_file, 36 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} 37 | ] 38 | ) 39 | 40 | node_robot_state_publisher = Node( 41 | package='robot_state_publisher', 42 | executable='robot_state_publisher', 43 | output='screen', 44 | parameters=[robot_description] 45 | ) 46 | 47 | load_joint_state_broadcaster = ExecuteProcess( 48 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 49 | 'joint_state_broadcaster'], 50 | output='screen' 51 | ) 52 | 53 | load_imu_sensor_broadcaster = ExecuteProcess( 54 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 55 | 'imu_sensor_broadcaster'], 56 | output='screen' 57 | ) 58 | 59 | load_joint_trajectory_controller = ExecuteProcess( 60 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controller'], 61 | output='screen' 62 | ) 63 | 64 | return LaunchDescription([ 65 | RegisterEventHandler( 66 | event_handler=OnProcessStart( 67 | target_action=node_mujoco_ros2_control, 68 | on_start=[load_joint_state_broadcaster], 69 | ) 70 | ), 71 | RegisterEventHandler( 72 | event_handler=OnProcessStart( 73 | target_action=load_joint_state_broadcaster, 74 | on_start=[load_imu_sensor_broadcaster], 75 | ) 76 | ), 77 | RegisterEventHandler( 78 | event_handler=OnProcessExit( 79 | target_action=load_imu_sensor_broadcaster, 80 | on_exit=[load_joint_trajectory_controller], 81 | ) 82 | ), 83 | node_mujoco_ros2_control, 84 | node_robot_state_publisher 85 | ]) 86 | -------------------------------------------------------------------------------- /mujoco_ros2_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(mujoco_ros2_control) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 8 | endif() 9 | 10 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 11 | add_compile_options(-Wall -Wextra -Wpedantic) 12 | endif() 13 | 14 | # find dependencies 15 | find_package(ament_cmake REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(hardware_interface REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | find_package(controller_manager REQUIRED) 20 | find_package(joint_limits REQUIRED) 21 | find_package(urdf REQUIRED) 22 | find_package(glfw3 REQUIRED) 23 | find_package(Eigen3 REQUIRED) 24 | find_package(control_toolbox REQUIRED) 25 | 26 | set(THIS_PACKAGE_DEPENDS 27 | ament_cmake 28 | rclcpp 29 | hardware_interface 30 | pluginlib 31 | controller_manager 32 | joint_limits 33 | urdf 34 | glfw3 35 | control_toolbox 36 | ) 37 | set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) 38 | 39 | find_package(mujoco QUIET) 40 | if(mujoco_FOUND) 41 | message(STATUS "Mujoco build from source has been found") 42 | set(MUJOCO_LIB mujoco::mujoco) 43 | set(MUJOCO_INCLUDE_DIR ${MUJOCO_INCLUDE_DIR}) 44 | elseif(DEFINED ENV{MUJOCO_DIR}) 45 | message(STATUS "Mujoco build from source has not been found. Attempting to find the binary in $ENV{MUJOCO_DIR} instead.") 46 | find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib) 47 | if(NOT MUJOCO_LIB) 48 | message(FATAL_ERROR "Failed to find binary in $ENV{MUJOCO_DIR}") 49 | endif() 50 | set(MUJOCO_INCLUDE_DIR $ENV{MUJOCO_DIR}/include) 51 | else() 52 | message(FATAL_ERROR "Failed to find mujoco with find_package. 53 | Either build and install mujoco from source or set the MUJOCO_DIR environment variable to tell CMake where to find the binary install. ") 54 | endif() 55 | 56 | add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp) 57 | ament_target_dependencies(mujoco_system_plugins ${THIS_PACKAGE_DEPENDS}) 58 | target_link_libraries(mujoco_system_plugins ${MUJOCO_LIB}) 59 | target_include_directories(mujoco_system_plugins 60 | PUBLIC 61 | $ 62 | $ 63 | ${MUJOCO_INCLUDE_DIR} 64 | ${EIGEN3_INCLUDE_DIR}) 65 | 66 | install(TARGETS 67 | mujoco_system_plugins 68 | ARCHIVE DESTINATION lib 69 | LIBRARY DESTINATION lib 70 | RUNTIME DESTINATION bin 71 | ) 72 | 73 | # TODO: make it simple 74 | add_executable(mujoco_ros2_control 75 | src/mujoco_ros2_control_node.cpp 76 | src/mujoco_rendering.cpp 77 | src/mujoco_ros2_control.cpp 78 | src/mujoco_cameras.cpp 79 | ) 80 | ament_target_dependencies(mujoco_ros2_control ${THIS_PACKAGE_DEPENDS}) 81 | target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} glfw) 82 | target_include_directories(mujoco_ros2_control 83 | PUBLIC 84 | $ 85 | $ 86 | ${MUJOCO_INCLUDE_DIR} 87 | ${EIGEN3_INCLUDE_DIR}) 88 | 89 | install(TARGETS 90 | mujoco_ros2_control 91 | DESTINATION lib/${PROJECT_NAME}) 92 | 93 | pluginlib_export_plugin_description_file(mujoco_ros2_control mujoco_system_plugins.xml) 94 | ament_package() 95 | -------------------------------------------------------------------------------- /mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #ifndef MUJOCO_ROS2_CONTROL__MUJOCO_RENDERING_HPP_ 22 | #define MUJOCO_ROS2_CONTROL__MUJOCO_RENDERING_HPP_ 23 | 24 | #include 25 | #include 26 | 27 | #include "GLFW/glfw3.h" 28 | #include "mujoco/mujoco.h" 29 | #include "rclcpp/rclcpp.hpp" 30 | 31 | #include "rclcpp/node.hpp" 32 | 33 | namespace mujoco_ros2_control 34 | { 35 | 36 | class MujocoRendering 37 | { 38 | public: 39 | MujocoRendering(const MujocoRendering &obj) = delete; 40 | void operator=(const MujocoRendering &) = delete; 41 | 42 | static MujocoRendering *get_instance(); 43 | void init(mjModel *mujoco_model, mjData *mujoco_data); 44 | bool is_close_flag_raised(); 45 | void update(); 46 | void close(); 47 | 48 | private: 49 | MujocoRendering(); 50 | static void keyboard_callback(GLFWwindow *window, int key, int scancode, int act, int mods); 51 | static void mouse_button_callback(GLFWwindow *window, int button, int act, int mods); 52 | static void mouse_move_callback(GLFWwindow *window, double xpos, double ypos); 53 | static void scroll_callback(GLFWwindow *window, double xoffset, double yoffset); 54 | 55 | void keyboard_callback_impl(GLFWwindow *window, int key, int scancode, int act, int mods); 56 | void mouse_button_callback_impl(GLFWwindow *window, int button, int act, int mods); 57 | void mouse_move_callback_impl(GLFWwindow *window, double xpos, double ypos); 58 | void scroll_callback_impl(GLFWwindow *window, double xoffset, double yoffset); 59 | 60 | static MujocoRendering *instance_; 61 | 62 | mjModel *mj_model_; 63 | mjData *mj_data_; 64 | 65 | // Window and primary camera for the simulation's viewer 66 | GLFWwindow *window_; 67 | mjvCamera mjv_cam_; 68 | 69 | // Options for the rendering context and scene, all of these are hard coded to defaults. 70 | mjvOption mjv_opt_; 71 | mjvScene mjv_scn_; 72 | mjrContext mjr_con_; 73 | 74 | bool button_left_; 75 | bool button_middle_; 76 | bool button_right_; 77 | double lastx_; 78 | double lasty_; 79 | }; 80 | } // namespace mujoco_ros2_control 81 | 82 | #endif // MUJOCO_ROS2_CONTROL__MUJOCO_RENDERING_HPP_ 83 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/camera_example.launch.py: -------------------------------------------------------------------------------- 1 | import os 2 | import xacro 3 | 4 | from ament_index_python.packages import get_package_share_directory 5 | 6 | from launch import LaunchDescription 7 | from launch.actions import ExecuteProcess, RegisterEventHandler 8 | from launch.event_handlers import OnProcessStart, OnProcessExit 9 | 10 | from launch_ros.actions import Node 11 | 12 | 13 | def generate_launch_description(): 14 | 15 | mujoco_ros2_control_demos_path = os.path.join( 16 | get_package_share_directory('mujoco_ros2_control_demos')) 17 | xacro_file = os.path.join(mujoco_ros2_control_demos_path, 18 | 'urdf', 19 | 'test_camera.xacro.urdf') 20 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 21 | 'config', 22 | 'camera_controller_position.yaml') 23 | rviz_config_file = os.path.join(mujoco_ros2_control_demos_path, 24 | 'launch', 25 | 'camera_demo.rviz') 26 | 27 | doc = xacro.parse(open(xacro_file)) 28 | xacro.process_doc(doc) 29 | robot_description = {'robot_description': doc.toxml()} 30 | 31 | use_sim_time = {'use_sim_time': True} 32 | 33 | node_mujoco_ros2_control = Node( 34 | package='mujoco_ros2_control', 35 | executable='mujoco_ros2_control', 36 | output='screen', 37 | parameters=[ 38 | robot_description, 39 | controller_config_file, 40 | use_sim_time, 41 | {'mujoco_model_path' : os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_camera.xml')} 42 | ] 43 | ) 44 | 45 | node_robot_state_publisher = Node( 46 | package='robot_state_publisher', 47 | executable='robot_state_publisher', 48 | output='screen', 49 | parameters=[ 50 | use_sim_time, 51 | robot_description, 52 | ] 53 | ) 54 | 55 | load_joint_state_controller = ExecuteProcess( 56 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 57 | 'joint_state_broadcaster'], 58 | output='screen' 59 | ) 60 | 61 | load_position_controller = ExecuteProcess( 62 | cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 63 | 'position_controller'], 64 | output='screen' 65 | ) 66 | 67 | rviz_node = Node( 68 | package="rviz2", 69 | executable="rviz2", 70 | name="rviz2", 71 | output="screen", 72 | parameters=[use_sim_time], 73 | arguments=["-d", rviz_config_file], 74 | ) 75 | 76 | return LaunchDescription([ 77 | RegisterEventHandler( 78 | event_handler=OnProcessStart( 79 | target_action=node_mujoco_ros2_control, 80 | on_start=[load_joint_state_controller], 81 | ) 82 | ), 83 | RegisterEventHandler( 84 | event_handler=OnProcessExit( 85 | target_action=load_joint_state_controller, 86 | on_exit=[load_position_controller], 87 | ) 88 | ), 89 | node_mujoco_ros2_control, 90 | node_robot_state_publisher, 91 | rviz_node, 92 | ]) 93 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_cart_effort.xacro.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 | mujoco_ros2_control/MujocoSystem 67 | 68 | 69 | 70 | -1000 71 | 1000 72 | 73 | 74 | 1.0 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_diff_drive.xacro.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 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 | 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 | 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 | 132 | 133 | 134 | 135 | 136 | 137 | mujoco_ros2_control/MujocoSystem 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | -------------------------------------------------------------------------------- /mujoco_ros2_control/src/mujoco_ros2_control_node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include "mujoco/mujoco.h" 22 | #include "rclcpp/rclcpp.hpp" 23 | 24 | #include "mujoco_ros2_control/mujoco_cameras.hpp" 25 | #include "mujoco_ros2_control/mujoco_rendering.hpp" 26 | #include "mujoco_ros2_control/mujoco_ros2_control.hpp" 27 | 28 | // MuJoCo data structures 29 | mjModel *mujoco_model = nullptr; 30 | mjData *mujoco_data = nullptr; 31 | 32 | // main function 33 | int main(int argc, const char **argv) 34 | { 35 | rclcpp::init(argc, argv); 36 | std::shared_ptr node = rclcpp::Node::make_shared( 37 | "mujoco_ros2_control_node", 38 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 39 | 40 | RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node..."); 41 | auto model_path = node->get_parameter("mujoco_model_path").as_string(); 42 | 43 | // load and compile model 44 | char error[1000] = "Could not load binary model"; 45 | if ( 46 | std::strlen(model_path.c_str()) > 4 && 47 | !std::strcmp(model_path.c_str() + std::strlen(model_path.c_str()) - 4, ".mjb")) 48 | { 49 | mujoco_model = mj_loadModel(model_path.c_str(), 0); 50 | } 51 | else 52 | { 53 | mujoco_model = mj_loadXML(model_path.c_str(), 0, error, 1000); 54 | } 55 | if (!mujoco_model) 56 | { 57 | mju_error("Load model error: %s", error); 58 | } 59 | 60 | RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco model has been successfully loaded !"); 61 | // make data 62 | mujoco_data = mj_makeData(mujoco_model); 63 | 64 | // initialize mujoco control 65 | auto mujoco_control = mujoco_ros2_control::MujocoRos2Control(node, mujoco_model, mujoco_data); 66 | 67 | mujoco_control.init(); 68 | RCLCPP_INFO_STREAM( 69 | node->get_logger(), "Mujoco ros2 controller has been successfully initialized !"); 70 | 71 | // initialize mujoco visualization environment for rendering and cameras 72 | if (!glfwInit()) 73 | { 74 | mju_error("Could not initialize GLFW"); 75 | } 76 | auto rendering = mujoco_ros2_control::MujocoRendering::get_instance(); 77 | rendering->init(mujoco_model, mujoco_data); 78 | RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !"); 79 | 80 | auto cameras = std::make_unique(node); 81 | cameras->init(mujoco_model); 82 | 83 | // run main loop, target real-time simulation and 60 fps rendering with cameras around 6 hz 84 | mjtNum last_cam_update = mujoco_data->time; 85 | while (rclcpp::ok() && !rendering->is_close_flag_raised()) 86 | { 87 | // advance interactive simulation for 1/60 sec 88 | // Assuming MuJoCo can simulate faster than real-time, which it usually can, 89 | // this loop will finish on time for the next frame to be rendered at 60 fps. 90 | // Otherwise add a cpu timer and exit this loop when it is time to render. 91 | mjtNum simstart = mujoco_data->time; 92 | while (mujoco_data->time - simstart < 1.0 / 60.0) 93 | { 94 | mujoco_control.update(); 95 | } 96 | rendering->update(); 97 | 98 | // Updating cameras at ~6 Hz 99 | // TODO(eholum): Break control and rendering into separate processes 100 | if (simstart - last_cam_update > 1.0 / 6.0) 101 | { 102 | cameras->update(mujoco_model, mujoco_data); 103 | last_cam_update = simstart; 104 | } 105 | } 106 | 107 | rendering->close(); 108 | cameras->close(); 109 | 110 | // free MuJoCo model and data 111 | mj_deleteData(mujoco_data); 112 | mj_deleteModel(mujoco_model); 113 | 114 | return 1; 115 | } 116 | -------------------------------------------------------------------------------- /doc/moveit_doc.rst: -------------------------------------------------------------------------------- 1 | How To Setup mujoco_ros2_control 2 | ================================ 3 | 4 | Introduction 5 | ------------ 6 | 7 | This document provides guidance on how to setup and use ``mujoco_ros2_control`` package. 8 | This page will demonstrate the supported MuJoCo simulation features with a robot manipulator. 9 | However, this package is not specifically designed for manipulation and can be used for any types of robots. 10 | 11 | 12 | Prerequisites 13 | -------------- 14 | 15 | Running on the local machine 16 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 17 | 18 | If you want to run an application on the local machine, you need following dependencies installed on the machine. 19 | 20 | 1. `MuJoCo `_ 21 | 2. `ROS 2 humble `_ 22 | 3. `MoveIt 2 `_ 23 | 24 | 25 | Installation 26 | ------------ 27 | 28 | Running on the local machine 29 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 30 | 31 | 1. Create a workspace 32 | .. code-block:: bash 33 | 34 | mkdir -p ~/mujoco_ros_ws/src 35 | 36 | 2. Clone the repository. 37 | .. code-block:: bash 38 | 39 | cd ~/mujoco_ros_ws/src 40 | git clone https://github.com/sangteak601/mujoco_ros2_control.git 41 | 42 | 3. Set environment variable 43 | .. code-block:: bash 44 | 45 | export MUJOCO_DIR=/PATH/TO/MUJOCO/mujoco-3.x.x 46 | 47 | 4. Build the package 48 | .. code-block:: bash 49 | 50 | sudo apt update && rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y 51 | source /opt/ros/${ROS_DISTRO}/setup.bash 52 | cd ~/mujoco_ros_ws 53 | colcon build 54 | 55 | 56 | How to Setup mujoco_ros2_control 57 | -------------------------------- 58 | 59 | .. note:: Please refer to `this `_ for the details. 60 | 61 | Set ros2_control plugin in the URDF 62 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 63 | 64 | You can configure the `ros2_control` hardware system plugin for MuJoCo in the URDF file as follows: 65 | 66 | .. code-block:: XML 67 | 68 | 69 | 70 | mujoco_ros2_control/MujocoSystem 71 | 72 | 73 | 74 | You can also set parameters for the plugin such as pid gains, min/max effort and so on. 75 | To find examples of parameters, please see `urdf examples `_. 76 | 77 | Create MJCF(MuJoCo xml format) 78 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 79 | 80 | You need to convert the URDF model to a MJCF XML file. 81 | Make sure to use the **same name** for the ``link`` and ``joint``, which are mapped to the ``body`` and ``joint`` in MuJoCo. 82 | You can specify position limits in ```` in MJCF, and effort limits in URDF as shown in this 83 | `example `_ 84 | Velocity limits will not be applied at all. 85 | 86 | Any force torque sensors need to be mapped to separate force and torque sensors in the MJCF, since there is no support for combined sensors in MuJoCo. 87 | The name of each sensor should be sensor_name + _force and sensor_name + _torque. 88 | For example, if you have a force torque sensor called ``my_sensor``, you need to create ``my_sensor_force`` and ``my_sensor_torque`` in MJCF. 89 | 90 | Check `mujoco_models `_ for examples. 91 | 92 | Specify the path to MJCF and controller config 93 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 94 | 95 | You need to pass the path to MJCF as ``mujoco_model_path`` parameter to the node. 96 | You also need to pass controller configuration since ``mujoco_ros2_control`` is replacing ``ros2_control`` node. 97 | 98 | .. code-block:: Python 99 | 100 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_position.yaml') 101 | 102 | node_mujoco_ros2_control = Node( 103 | package='mujoco_ros2_control', 104 | executable='mujoco_ros2_control', 105 | output='screen', 106 | parameters=[ 107 | robot_description, 108 | controller_config_file, 109 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_position.xml')} 110 | ] 111 | ) 112 | 113 | 114 | Running the MoveIt Interactive Marker Demo with MuJoCo 115 | ------------------------------------------------------ 116 | 117 | .. note:: Please refer to `this `_ for running the demo. 118 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | mujoco_ros2_control 2 | ===================== 3 | These are the basic examples of using Mujoco with ROS2. 4 | 5 | .. image:: images/cart_position.gif 6 | :alt: Cart 7 | 8 | .. image:: images/diff_drive.gif 9 | :alt: DiffBot 10 | 11 | Usage 12 | ===================== 13 | ros2_control tag in URDF 14 | -------------------------- 15 | Use ``mujoco_ros2_control/MujocoSystem`` for plugin 16 | 17 | .. code-block:: xml 18 | 19 | 20 | 21 | mujoco_ros2_control/MujocoSystem 22 | 23 | 24 | 25 | -15 26 | 15 27 | 28 | 29 | 1.0 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 | motor_fts 56 | 57 | 58 | 59 | Convert URDF model to XML 60 | -------------------------- 61 | URDF models must be converted to `MJCF XML `_ files. 62 | Make sure to use the same name for the link and joint, which are mapped to the body and joint in Mujoco. 63 | You need to specify which is mapped to ``range`` in MJCF. 64 | For now, there is no way to specify velocity or acceleration limit. 65 | 66 | For an IMU sensor, we add ``framequat``, ``gyro``, and ``accelerometer`` sensors in the MJCF since there is no combined IMU in MuJoCo. 67 | The name of each sensor should be ``sensor_name`` + ``_quat``, ``sensor_name`` + ``_gyro``, and ``sensor_name`` + ``_accel`` respectively. 68 | Take note that the sensor must be suffixed with `_imu`, as in the sample ros2_control xml above. 69 | 70 | For a force torque sensor, we map both a force sensor and a torque sensor in the MJCF since there is no combined force torque sensor in MuJoCo. 71 | The name of each sensor should be ``sensor_name`` + ``_force`` and ``sensor_name`` + ``_torque``. 72 | For example, if you have a force torque sensor called ``my_sensor``, you need to create ``my_sensor_force`` and ``my_sensor_torque`` in MJCF. 73 | Take note that the sensor must be suffixed with `_fts`, as in the sample ros2_control xml above. 74 | 75 | The drivers additionally support simulated RGB-D cameras for publishing simulated color images and depth maps. 76 | Cameras must be given a name and be attached to a joint called ``_optical_frame``. 77 | The camera_info, color, and depth images will be published to topics called ``/camera_info``, 78 | ``/color``, and ``/depth``, repectively. 79 | Also note that MuJuCo's conventions for cameras are different than ROS's, and which must be accounted for. 80 | An overview is available through the "camera" demo. 81 | 82 | For additional information refer to the ``mujoco_ros2_control_demos/mujoco_models`` for examples. 83 | 84 | Specify the location of Mujoco models and the controller configuration file 85 | ---------------------------------------------------------------------------- 86 | You need to pass parameters for paths as shown in the following example. 87 | 88 | .. code-block:: python3 89 | 90 | controller_config_file = os.path.join(mujoco_ros2_control_demos_path, 'config', 'cartpole_controller_position.yaml') 91 | 92 | node_mujoco_ros2_control = Node( 93 | package='mujoco_ros2_control', 94 | executable='mujoco_ros2_control', 95 | output='screen', 96 | parameters=[ 97 | robot_description, 98 | controller_config_file, 99 | {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_position.xml')} 100 | ] 101 | ) 102 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/urdf/test_tricycle_drive.xacro.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 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | mujoco_ros2_control/MujocoSystem 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | -------------------------------------------------------------------------------- /mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #ifndef MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_HPP_ 22 | #define MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_HPP_ 23 | 24 | #include 25 | #include 26 | #include 27 | 28 | #include "control_toolbox/pid.hpp" 29 | #include "hardware_interface/types/hardware_interface_type_values.hpp" 30 | #include "joint_limits/joint_limits.hpp" 31 | #include "mujoco_ros2_control/mujoco_system_interface.hpp" 32 | 33 | namespace mujoco_ros2_control 34 | { 35 | constexpr char PARAM_KP[]{"_kp"}; 36 | constexpr char PARAM_KI[]{"_ki"}; 37 | constexpr char PARAM_KD[]{"_kd"}; 38 | constexpr char PARAM_I_MAX[]{"_i_max"}; 39 | constexpr char PARAM_I_MIN[]{"_i_min"}; 40 | 41 | class MujocoSystem : public MujocoSystemInterface 42 | { 43 | public: 44 | MujocoSystem(); 45 | std::vector export_state_interfaces() override; 46 | std::vector export_command_interfaces() override; 47 | 48 | hardware_interface::return_type read( 49 | const rclcpp::Time &time, const rclcpp::Duration &period) override; 50 | hardware_interface::return_type write( 51 | const rclcpp::Time &time, const rclcpp::Duration &period) override; 52 | 53 | bool init_sim( 54 | mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model, 55 | const hardware_interface::HardwareInfo &hardware_info) override; 56 | 57 | struct JointState 58 | { 59 | std::string name; 60 | double position; 61 | double velocity; 62 | double effort; 63 | double position_command; 64 | double velocity_command; 65 | double effort_command; 66 | double min_position_command; 67 | double max_position_command; 68 | double min_velocity_command; 69 | double max_velocity_command; 70 | double min_effort_command; 71 | double max_effort_command; 72 | control_toolbox::Pid position_pid; 73 | control_toolbox::Pid velocity_pid; 74 | bool is_position_control_enabled{false}; 75 | bool is_velocity_control_enabled{false}; 76 | bool is_effort_control_enabled{false}; 77 | bool is_pid_enabled{false}; 78 | joint_limits::JointLimits joint_limits; 79 | bool is_mimic{false}; 80 | int mimicked_joint_index; 81 | double mimic_multiplier; 82 | int mj_joint_type; 83 | int mj_pos_adr; 84 | int mj_vel_adr; 85 | }; 86 | 87 | template 88 | struct SensorData 89 | { 90 | std::string name; 91 | T data; 92 | int mj_sensor_index; 93 | }; 94 | 95 | struct FTSensorData 96 | { 97 | std::string name; 98 | SensorData force; 99 | SensorData torque; 100 | }; 101 | 102 | struct IMUSensorData 103 | { 104 | std::string name; 105 | SensorData> orientation; 106 | SensorData angular_velocity; 107 | SensorData linear_acceleration; 108 | }; 109 | 110 | private: 111 | void register_joints( 112 | const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info); 113 | void register_sensors( 114 | const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info); 115 | void set_initial_pose(); 116 | void get_joint_limits( 117 | urdf::JointConstSharedPtr urdf_joint, joint_limits::JointLimits &joint_limits); 118 | control_toolbox::Pid get_pid_gains( 119 | const hardware_interface::ComponentInfo &joint_info, std::string command_interface); 120 | double clamp(double v, double lo, double hi) { return (v < lo) ? lo : (hi < v) ? hi : v; } 121 | 122 | std::vector state_interfaces_; 123 | std::vector command_interfaces_; 124 | 125 | std::vector joint_states_; 126 | std::vector ft_sensor_data_; 127 | std::vector imu_sensor_data_; 128 | 129 | mjModel *mj_model_; 130 | mjData *mj_data_; 131 | 132 | rclcpp::Logger logger_; // TODO(sangteak601): delete? 133 | }; 134 | } // namespace mujoco_ros2_control 135 | 136 | #endif // MUJOCO_ROS2_CONTROL__MUJOCO_SYSTEM_HPP_ 137 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/examples/example_position.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2020 Open Source Robotics Foundation, Inc. 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 | 19 | #include "rclcpp/rclcpp.hpp" 20 | #include "rclcpp_action/rclcpp_action.hpp" 21 | 22 | #include "control_msgs/action/follow_joint_trajectory.hpp" 23 | 24 | std::shared_ptr node; 25 | bool common_goal_accepted = false; 26 | rclcpp_action::ResultCode common_resultcode = rclcpp_action::ResultCode::UNKNOWN; 27 | int common_action_result_code = control_msgs::action::FollowJointTrajectory_Result::SUCCESSFUL; 28 | 29 | void common_goal_response( 30 | rclcpp_action::ClientGoalHandle 31 | ::SharedPtr goal_handle) 32 | { 33 | RCLCPP_DEBUG( 34 | node->get_logger(), "common_goal_response time: %f", 35 | rclcpp::Clock().now().seconds()); 36 | if (!goal_handle) { 37 | common_goal_accepted = false; 38 | printf("Goal rejected\n"); 39 | } else { 40 | common_goal_accepted = true; 41 | printf("Goal accepted\n"); 42 | } 43 | } 44 | 45 | void common_result_response( 46 | const rclcpp_action::ClientGoalHandle 47 | ::WrappedResult & result) 48 | { 49 | printf("common_result_response time: %f\n", rclcpp::Clock().now().seconds()); 50 | common_resultcode = result.code; 51 | common_action_result_code = result.result->error_code; 52 | switch (result.code) { 53 | case rclcpp_action::ResultCode::SUCCEEDED: 54 | printf("SUCCEEDED result code\n"); 55 | break; 56 | case rclcpp_action::ResultCode::ABORTED: 57 | printf("Goal was aborted\n"); 58 | return; 59 | case rclcpp_action::ResultCode::CANCELED: 60 | printf("Goal was canceled\n"); 61 | return; 62 | default: 63 | printf("Unknown result code\n"); 64 | return; 65 | } 66 | } 67 | 68 | void common_feedback( 69 | rclcpp_action::ClientGoalHandle::SharedPtr, 70 | const std::shared_ptr feedback) 71 | { 72 | std::cout << "feedback->desired.positions :"; 73 | for (auto & x : feedback->desired.positions) { 74 | std::cout << x << "\t"; 75 | } 76 | std::cout << std::endl; 77 | std::cout << "feedback->desired.velocities :"; 78 | for (auto & x : feedback->desired.velocities) { 79 | std::cout << x << "\t"; 80 | } 81 | std::cout << std::endl; 82 | } 83 | 84 | int main(int argc, char * argv[]) 85 | { 86 | rclcpp::init(argc, argv); 87 | 88 | node = std::make_shared("trajectory_test_node"); 89 | 90 | std::cout << "node created" << std::endl; 91 | 92 | rclcpp_action::Client::SharedPtr action_client; 93 | action_client = rclcpp_action::create_client( 94 | node->get_node_base_interface(), 95 | node->get_node_graph_interface(), 96 | node->get_node_logging_interface(), 97 | node->get_node_waitables_interface(), 98 | "/joint_trajectory_controller/follow_joint_trajectory"); 99 | 100 | bool response = 101 | action_client->wait_for_action_server(std::chrono::seconds(1)); 102 | if (!response) { 103 | throw std::runtime_error("could not get action server"); 104 | } 105 | std::cout << "Created action server" << std::endl; 106 | 107 | std::vector joint_names = {"slider_to_cart"}; 108 | 109 | std::vector points; 110 | trajectory_msgs::msg::JointTrajectoryPoint point; 111 | point.time_from_start = rclcpp::Duration::from_seconds(0.0); // start asap 112 | point.positions.resize(joint_names.size()); 113 | 114 | point.positions[0] = 0.0; 115 | 116 | trajectory_msgs::msg::JointTrajectoryPoint point2; 117 | point2.time_from_start = rclcpp::Duration::from_seconds(1.0); 118 | point2.positions.resize(joint_names.size()); 119 | point2.positions[0] = -1.0; 120 | 121 | trajectory_msgs::msg::JointTrajectoryPoint point3; 122 | point3.time_from_start = rclcpp::Duration::from_seconds(2.0); 123 | point3.positions.resize(joint_names.size()); 124 | point3.positions[0] = 1.0; 125 | 126 | trajectory_msgs::msg::JointTrajectoryPoint point4; 127 | point4.time_from_start = rclcpp::Duration::from_seconds(3.0); 128 | point4.positions.resize(joint_names.size()); 129 | point4.positions[0] = 0.0; 130 | 131 | points.push_back(point); 132 | points.push_back(point2); 133 | points.push_back(point3); 134 | points.push_back(point4); 135 | 136 | rclcpp_action::Client::SendGoalOptions opt; 137 | opt.goal_response_callback = std::bind(common_goal_response, std::placeholders::_1); 138 | opt.result_callback = std::bind(common_result_response, std::placeholders::_1); 139 | opt.feedback_callback = std::bind(common_feedback, std::placeholders::_1, std::placeholders::_2); 140 | 141 | control_msgs::action::FollowJointTrajectory_Goal goal_msg; 142 | goal_msg.goal_time_tolerance = rclcpp::Duration::from_seconds(1.0); 143 | goal_msg.trajectory.joint_names = joint_names; 144 | goal_msg.trajectory.points = points; 145 | 146 | auto goal_handle_future = action_client->async_send_goal(goal_msg, opt); 147 | 148 | if (rclcpp::spin_until_future_complete(node, goal_handle_future) != 149 | rclcpp::FutureReturnCode::SUCCESS) 150 | { 151 | RCLCPP_ERROR(node->get_logger(), "send goal call failed :("); 152 | action_client.reset(); 153 | node.reset(); 154 | return 1; 155 | } 156 | RCLCPP_INFO(node->get_logger(), "send goal call ok :)"); 157 | 158 | rclcpp_action::ClientGoalHandle::SharedPtr 159 | goal_handle = goal_handle_future.get(); 160 | if (!goal_handle) { 161 | RCLCPP_ERROR(node->get_logger(), "Goal was rejected by server"); 162 | action_client.reset(); 163 | node.reset(); 164 | return 1; 165 | } 166 | RCLCPP_INFO(node->get_logger(), "Goal was accepted by server"); 167 | 168 | // Wait for the server to be done with the goal 169 | auto result_future = action_client->async_get_result(goal_handle); 170 | RCLCPP_INFO(node->get_logger(), "Waiting for result"); 171 | if (rclcpp::spin_until_future_complete(node, result_future) != 172 | rclcpp::FutureReturnCode::SUCCESS) 173 | { 174 | RCLCPP_ERROR(node->get_logger(), "get result call failed :("); 175 | return 1; 176 | } 177 | 178 | action_client.reset(); 179 | node.reset(); 180 | rclcpp::shutdown(); 181 | 182 | return 0; 183 | } 184 | -------------------------------------------------------------------------------- /mujoco_ros2_control/src/mujoco_rendering.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include "mujoco_ros2_control/mujoco_rendering.hpp" 22 | 23 | #include "sensor_msgs/image_encodings.hpp" 24 | 25 | namespace mujoco_ros2_control 26 | { 27 | MujocoRendering *MujocoRendering::instance_ = nullptr; 28 | 29 | MujocoRendering *MujocoRendering::get_instance() 30 | { 31 | if (instance_ == nullptr) 32 | { 33 | instance_ = new MujocoRendering(); 34 | } 35 | 36 | return instance_; 37 | } 38 | 39 | MujocoRendering::MujocoRendering() 40 | : mj_model_(nullptr), 41 | mj_data_(nullptr), 42 | button_left_(false), 43 | button_middle_(false), 44 | button_right_(false), 45 | lastx_(0.0), 46 | lasty_(0.0) 47 | { 48 | } 49 | 50 | void MujocoRendering::init(mjModel *mujoco_model, mjData *mujoco_data) 51 | { 52 | mj_model_ = mujoco_model; 53 | mj_data_ = mujoco_data; 54 | 55 | // create window, make OpenGL context current, request v-sync 56 | glfwWindowHint(GLFW_VISIBLE, GLFW_TRUE); 57 | glfwWindowHint(GLFW_DOUBLEBUFFER, GLFW_TRUE); 58 | window_ = glfwCreateWindow(1200, 900, "Demo", NULL, NULL); 59 | glfwMakeContextCurrent(window_); 60 | 61 | // initialize visualization data structures 62 | mjv_defaultCamera(&mjv_cam_); 63 | mjv_defaultOption(&mjv_opt_); 64 | mjv_defaultScene(&mjv_scn_); 65 | mjr_defaultContext(&mjr_con_); 66 | 67 | mjv_cam_.type = mjCAMERA_FREE; 68 | mjv_cam_.distance = 8.; 69 | 70 | // create scene and context 71 | mjv_makeScene(mj_model_, &mjv_scn_, 2000); 72 | mjr_makeContext(mj_model_, &mjr_con_, mjFONTSCALE_150); 73 | 74 | // install GLFW mouse and keyboard callbacks 75 | glfwSetKeyCallback(window_, &MujocoRendering::keyboard_callback); 76 | glfwSetCursorPosCallback(window_, &MujocoRendering::mouse_move_callback); 77 | glfwSetMouseButtonCallback(window_, &MujocoRendering::mouse_button_callback); 78 | glfwSetScrollCallback(window_, &MujocoRendering::scroll_callback); 79 | 80 | // This might cause tearing, but having RViz and the renderer both open can 81 | // wreak havoc on the rendering process. 82 | glfwSwapInterval(0); 83 | } 84 | 85 | bool MujocoRendering::is_close_flag_raised() { return glfwWindowShouldClose(window_); } 86 | 87 | void MujocoRendering::update() 88 | { 89 | // get framebuffer viewport 90 | mjrRect viewport = {0, 0, 0, 0}; 91 | glfwGetFramebufferSize(window_, &viewport.width, &viewport.height); 92 | glfwMakeContextCurrent(window_); 93 | 94 | // Reset the buffer 95 | mjr_setBuffer(mjFB_WINDOW, &mjr_con_); 96 | 97 | // update scene and render 98 | mjv_updateScene(mj_model_, mj_data_, &mjv_opt_, NULL, &mjv_cam_, mjCAT_ALL, &mjv_scn_); 99 | mjr_render(viewport, &mjv_scn_, &mjr_con_); 100 | 101 | // swap OpenGL buffers (blocking call due to v-sync) 102 | glfwSwapBuffers(window_); 103 | 104 | // process pending GUI events, call GLFW callbacks 105 | glfwPollEvents(); 106 | } 107 | 108 | void MujocoRendering::close() 109 | { 110 | // free visualization storage 111 | mjv_freeScene(&mjv_scn_); 112 | mjr_freeContext(&mjr_con_); 113 | glfwDestroyWindow(window_); 114 | 115 | // terminate GLFW (crashes with Linux NVidia drivers) 116 | #if defined(__APPLE__) || defined(_WIN32) 117 | glfwTerminate(); 118 | #endif 119 | } 120 | 121 | void MujocoRendering::keyboard_callback( 122 | GLFWwindow *window, int key, int scancode, int act, int mods) 123 | { 124 | get_instance()->keyboard_callback_impl(window, key, scancode, act, mods); 125 | } 126 | 127 | void MujocoRendering::mouse_button_callback(GLFWwindow *window, int button, int act, int mods) 128 | { 129 | get_instance()->mouse_button_callback_impl(window, button, act, mods); 130 | } 131 | 132 | void MujocoRendering::mouse_move_callback(GLFWwindow *window, double xpos, double ypos) 133 | { 134 | get_instance()->mouse_move_callback_impl(window, xpos, ypos); 135 | } 136 | 137 | void MujocoRendering::scroll_callback(GLFWwindow *window, double xoffset, double yoffset) 138 | { 139 | get_instance()->scroll_callback_impl(window, xoffset, yoffset); 140 | } 141 | 142 | void MujocoRendering::keyboard_callback_impl( 143 | GLFWwindow * /* window */, int key, int /* scancode */, int act, int /* mods */) 144 | { 145 | // backspace: reset simulation 146 | if (act == GLFW_PRESS && key == GLFW_KEY_BACKSPACE) 147 | { 148 | mj_resetData(mj_model_, mj_data_); 149 | mj_forward(mj_model_, mj_data_); 150 | } 151 | } 152 | 153 | void MujocoRendering::mouse_button_callback_impl( 154 | GLFWwindow *window, int /* button */, int /* act */, int /* mods */) 155 | { 156 | // update button state 157 | button_left_ = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS); 158 | button_middle_ = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_MIDDLE) == GLFW_PRESS); 159 | button_right_ = (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_RIGHT) == GLFW_PRESS); 160 | 161 | // update mouse position 162 | glfwGetCursorPos(window, &lastx_, &lasty_); 163 | } 164 | 165 | void MujocoRendering::mouse_move_callback_impl(GLFWwindow *window, double xpos, double ypos) 166 | { 167 | // no buttons down: nothing to do 168 | if (!button_left_ && !button_middle_ && !button_right_) 169 | { 170 | return; 171 | } 172 | 173 | // compute mouse displacement, save 174 | double dx = xpos - lastx_; 175 | double dy = ypos - lasty_; 176 | lastx_ = xpos; 177 | lasty_ = ypos; 178 | 179 | // get current window size 180 | int width, height; 181 | glfwGetWindowSize(window, &width, &height); 182 | 183 | // get shift key state 184 | bool mod_shift = 185 | (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS || 186 | glfwGetKey(window, GLFW_KEY_RIGHT_SHIFT) == GLFW_PRESS); 187 | 188 | // determine action based on mouse button 189 | mjtMouse action; 190 | if (button_right_) 191 | { 192 | action = mod_shift ? mjMOUSE_MOVE_H : mjMOUSE_MOVE_V; 193 | } 194 | else if (button_left_) 195 | { 196 | action = mod_shift ? mjMOUSE_ROTATE_H : mjMOUSE_ROTATE_V; 197 | } 198 | else 199 | { 200 | action = mjMOUSE_ZOOM; 201 | } 202 | 203 | // move camera 204 | mjv_moveCamera(mj_model_, action, dx / height, dy / height, &mjv_scn_, &mjv_cam_); 205 | } 206 | 207 | void MujocoRendering::scroll_callback_impl( 208 | GLFWwindow * /* window */, double /* xoffset */, double yoffset) 209 | { 210 | // emulate vertical mouse motion = 5% of window height 211 | mjv_moveCamera(mj_model_, mjMOUSE_ZOOM, 0, -0.05 * yoffset, &mjv_scn_, &mjv_cam_); 212 | } 213 | 214 | } // namespace mujoco_ros2_control 215 | -------------------------------------------------------------------------------- /mujoco_ros2_control/src/mujoco_cameras.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Erik Holum 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include "mujoco_ros2_control/mujoco_cameras.hpp" 22 | 23 | #include "sensor_msgs/image_encodings.hpp" 24 | 25 | namespace mujoco_ros2_control 26 | { 27 | 28 | MujocoCameras::MujocoCameras(rclcpp::Node::SharedPtr &node) : node_(node) {} 29 | 30 | void MujocoCameras::init(mjModel *mujoco_model) 31 | { 32 | // initialize visualization data structures 33 | mjv_defaultOption(&mjv_opt_); 34 | mjv_defaultScene(&mjv_scn_); 35 | mjr_defaultContext(&mjr_con_); 36 | 37 | // create scene and context 38 | mjv_makeScene(mujoco_model, &mjv_scn_, 2000); 39 | mjr_makeContext(mujoco_model, &mjr_con_, mjFONTSCALE_150); 40 | 41 | // Add user cameras 42 | register_cameras(mujoco_model); 43 | } 44 | 45 | void MujocoCameras::update(mjModel *mujoco_model, mjData *mujoco_data) 46 | { 47 | // Rendering is done offscreen 48 | mjr_setBuffer(mjFB_OFFSCREEN, &mjr_con_); 49 | 50 | for (auto &camera : cameras_) 51 | { 52 | // Render simple RGB data for all cameras 53 | mjv_updateScene( 54 | mujoco_model, mujoco_data, &mjv_opt_, NULL, &camera.mjv_cam, mjCAT_ALL, &mjv_scn_); 55 | mjr_render(camera.viewport, &mjv_scn_, &mjr_con_); 56 | 57 | // Copy image into relevant buffers 58 | mjr_readPixels( 59 | camera.image_buffer.data(), camera.depth_buffer.data(), camera.viewport, &mjr_con_); 60 | 61 | // Fix non-linear projections in the depth image and flip the data. 62 | // https://github.com/google-deepmind/mujoco/blob/3.2.7/python/mujoco/renderer.py#L190 63 | float near = static_cast(mujoco_model->vis.map.znear * mujoco_model->stat.extent); 64 | float far = static_cast(mujoco_model->vis.map.zfar * mujoco_model->stat.extent); 65 | for (uint32_t h = 0; h < camera.height; h++) 66 | { 67 | for (uint32_t w = 0; w < camera.width; w++) 68 | { 69 | auto idx = h * camera.width + w; 70 | auto idx_flipped = (camera.height - 1 - h) * camera.width + w; 71 | camera.depth_buffer[idx] = near / (1.0f - camera.depth_buffer[idx] * (1.0f - near / far)); 72 | camera.depth_buffer_flipped[idx_flipped] = camera.depth_buffer[idx]; 73 | } 74 | } 75 | // Copy flipped data into the depth image message, floats -> unsigned chars 76 | std::memcpy( 77 | &camera.depth_image.data[0], camera.depth_buffer_flipped.data(), 78 | camera.depth_image.data.size()); 79 | 80 | // OpenGL's coordinate system's origin is in the bottom left, so we invert the images row-by-row 81 | auto row_size = camera.width * 3; 82 | for (uint32_t h = 0; h < camera.height; h++) 83 | { 84 | auto src_idx = h * row_size; 85 | auto dest_idx = (camera.height - 1 - h) * row_size; 86 | std::memcpy(&camera.image.data[dest_idx], &camera.image_buffer[src_idx], row_size); 87 | } 88 | 89 | // Publish images and camera info 90 | auto time = node_->now(); 91 | camera.image.header.stamp = time; 92 | camera.depth_image.header.stamp = time; 93 | camera.camera_info.header.stamp = time; 94 | 95 | camera.image_pub->publish(camera.image); 96 | camera.depth_image_pub->publish(camera.depth_image); 97 | camera.camera_info_pub->publish(camera.camera_info); 98 | } 99 | } 100 | 101 | void MujocoCameras::close() 102 | { 103 | mjv_freeScene(&mjv_scn_); 104 | mjr_freeContext(&mjr_con_); 105 | } 106 | 107 | void MujocoCameras::register_cameras(const mjModel *mujoco_model) 108 | { 109 | cameras_.resize(0); 110 | for (auto i = 0; i < mujoco_model->ncam; ++i) 111 | { 112 | const char *cam_name = mujoco_model->names + mujoco_model->name_camadr[i]; 113 | const int *cam_resolution = mujoco_model->cam_resolution + 2 * i; 114 | const mjtNum cam_fovy = mujoco_model->cam_fovy[i]; 115 | 116 | // Construct CameraData wrapper and set defaults 117 | CameraData camera; 118 | camera.name = cam_name; 119 | camera.mjv_cam.type = mjCAMERA_FIXED; 120 | camera.mjv_cam.fixedcamid = i; 121 | camera.width = static_cast(cam_resolution[0]); 122 | camera.height = static_cast(cam_resolution[1]); 123 | camera.viewport = {0, 0, cam_resolution[0], cam_resolution[1]}; 124 | 125 | // TODO(eholum): Ensure that the camera is attached to the expected pose. 126 | // For now assume that's the case. 127 | camera.frame_name = camera.name + "_optical_frame"; 128 | 129 | // Configure publishers 130 | camera.image_pub = node_->create_publisher(camera.name + "/color", 1); 131 | camera.depth_image_pub = 132 | node_->create_publisher(camera.name + "/depth", 1); 133 | camera.camera_info_pub = 134 | node_->create_publisher(camera.name + "/camera_info", 1); 135 | 136 | // Setup contaienrs for color image data 137 | camera.image.header.frame_id = camera.frame_name; 138 | 139 | const auto image_size = camera.width * camera.height * 3; 140 | camera.image_buffer.resize(image_size); 141 | camera.image.data.resize(image_size); 142 | camera.image.width = camera.width; 143 | camera.image.height = camera.height; 144 | camera.image.step = camera.width * 3; 145 | camera.image.encoding = sensor_msgs::image_encodings::RGB8; 146 | 147 | // Depth image data 148 | camera.depth_image.header.frame_id = camera.frame_name; 149 | camera.depth_buffer.resize(camera.width * camera.height); 150 | camera.depth_buffer_flipped.resize(camera.width * camera.height); 151 | camera.depth_image.data.resize(camera.width * camera.height * sizeof(float)); 152 | camera.depth_image.width = camera.width; 153 | camera.depth_image.height = camera.height; 154 | camera.depth_image.step = camera.width * sizeof(float); 155 | camera.depth_image.encoding = sensor_msgs::image_encodings::TYPE_32FC1; 156 | 157 | // Camera info 158 | camera.camera_info.header.frame_id = camera.frame_name; 159 | camera.camera_info.width = camera.width; 160 | camera.camera_info.height = camera.height; 161 | camera.camera_info.distortion_model = "plumb_bob"; 162 | camera.camera_info.k.fill(0.0); 163 | camera.camera_info.r.fill(0.0); 164 | camera.camera_info.p.fill(0.0); 165 | camera.camera_info.d.resize(5, 0.0); 166 | 167 | double focal_scaling = (1.0 / std::tan((cam_fovy * M_PI / 180.0) / 2.0)) * camera.height / 2.0; 168 | camera.camera_info.k[0] = camera.camera_info.p[0] = focal_scaling; 169 | camera.camera_info.k[2] = camera.camera_info.p[2] = static_cast(camera.width) / 2.0; 170 | camera.camera_info.k[4] = camera.camera_info.p[5] = focal_scaling; 171 | camera.camera_info.k[5] = camera.camera_info.p[6] = static_cast(camera.height) / 2.0; 172 | camera.camera_info.k[8] = camera.camera_info.p[10] = 1.0; 173 | 174 | // Add to list of cameras 175 | cameras_.push_back(camera); 176 | } 177 | } 178 | 179 | } // namespace mujoco_ros2_control 180 | -------------------------------------------------------------------------------- /mujoco_ros2_control_demos/launch/camera_demo.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz_common/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Camera1 10 | - /Camera2 11 | - /TF1/Frames1 12 | Splitter Ratio: 0.5 13 | Tree Height: 629 14 | - Class: rviz_common/Selection 15 | Name: Selection 16 | - Class: rviz_common/Tool Properties 17 | Expanded: 18 | - /2D Goal Pose1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz_common/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz_common/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: DepthCloud 32 | Visualization Manager: 33 | Class: "" 34 | Displays: 35 | - Class: rviz_default_plugins/Camera 36 | Enabled: true 37 | Far Plane Distance: 100 38 | Image Rendering: background and overlay 39 | Name: Camera 40 | Overlay Alpha: 0.5 41 | Topic: 42 | Depth: 5 43 | Durability Policy: Volatile 44 | History Policy: Keep Last 45 | Reliability Policy: Reliable 46 | Value: /camera/color 47 | Value: true 48 | Visibility: 49 | Camera: true 50 | DepthCloud: true 51 | TF: true 52 | Value: true 53 | Zoom Factor: 1 54 | - Class: rviz_default_plugins/Camera 55 | Enabled: true 56 | Far Plane Distance: 100 57 | Image Rendering: background and overlay 58 | Name: Camera 59 | Overlay Alpha: 0.5 60 | Topic: 61 | Depth: 5 62 | Durability Policy: Volatile 63 | History Policy: Keep Last 64 | Reliability Policy: Reliable 65 | Value: /camera/depth 66 | Value: true 67 | Visibility: 68 | Camera: true 69 | DepthCloud: true 70 | TF: true 71 | Value: true 72 | Zoom Factor: 1 73 | - Class: rviz_default_plugins/TF 74 | Enabled: true 75 | Frame Timeout: 15 76 | Frames: 77 | All Enabled: false 78 | base_link: 79 | Value: false 80 | camera: 81 | Value: false 82 | camera_optical_frame: 83 | Value: true 84 | green_sphere: 85 | Value: true 86 | red_box: 87 | Value: true 88 | Marker Scale: 1 89 | Name: TF 90 | Show Arrows: true 91 | Show Axes: true 92 | Show Names: false 93 | Tree: 94 | base_link: 95 | camera: 96 | camera_optical_frame: 97 | {} 98 | green_sphere: 99 | {} 100 | red_box: 101 | {} 102 | Update Interval: 0 103 | Value: true 104 | - Alpha: 1 105 | Auto Size: 106 | Auto Size Factor: 1 107 | Value: true 108 | Autocompute Intensity Bounds: true 109 | Autocompute Value Bounds: 110 | Max Value: 10 111 | Min Value: -10 112 | Value: true 113 | Axis: Z 114 | Channel Name: intensity 115 | Class: rviz_default_plugins/DepthCloud 116 | Color: 255; 255; 255 117 | Color Image Topic: /camera/color 118 | Color Transformer: RGB8 119 | Color Transport Hint: raw 120 | Decay Time: 0 121 | Depth Map Topic: /camera/depth 122 | Depth Map Transport Hint: raw 123 | Enabled: true 124 | Invert Rainbow: false 125 | Max Color: 255; 255; 255 126 | Max Intensity: 4096 127 | Min Color: 0; 0; 0 128 | Min Intensity: 0 129 | Name: DepthCloud 130 | Occlusion Compensation: 131 | Occlusion Time-Out: 30 132 | Value: false 133 | Position Transformer: XYZ 134 | Queue Size: 5 135 | Reliability Policy: Best effort 136 | Selectable: true 137 | Size (Pixels): 3 138 | Style: Flat Squares 139 | Topic Filter: true 140 | Use Fixed Frame: true 141 | Use rainbow: true 142 | Value: true 143 | Enabled: true 144 | Global Options: 145 | Background Color: 48; 48; 48 146 | Fixed Frame: base_link 147 | Frame Rate: 30 148 | Name: root 149 | Tools: 150 | - Class: rviz_default_plugins/Interact 151 | Hide Inactive Objects: true 152 | - Class: rviz_default_plugins/MoveCamera 153 | - Class: rviz_default_plugins/Select 154 | - Class: rviz_default_plugins/FocusCamera 155 | - Class: rviz_default_plugins/Measure 156 | Line color: 128; 128; 0 157 | - Class: rviz_default_plugins/SetInitialPose 158 | Covariance x: 0.25 159 | Covariance y: 0.25 160 | Covariance yaw: 0.06853891909122467 161 | Topic: 162 | Depth: 5 163 | Durability Policy: Volatile 164 | History Policy: Keep Last 165 | Reliability Policy: Reliable 166 | Value: /initialpose 167 | - Class: rviz_default_plugins/SetGoal 168 | Topic: 169 | Depth: 5 170 | Durability Policy: Volatile 171 | History Policy: Keep Last 172 | Reliability Policy: Reliable 173 | Value: /goal_pose 174 | - Class: rviz_default_plugins/PublishPoint 175 | Single click: true 176 | Topic: 177 | Depth: 5 178 | Durability Policy: Volatile 179 | History Policy: Keep Last 180 | Reliability Policy: Reliable 181 | Value: /clicked_point 182 | Transformation: 183 | Current: 184 | Class: rviz_default_plugins/TF 185 | Value: true 186 | Views: 187 | Current: 188 | Class: rviz_default_plugins/Orbit 189 | Distance: 2.5519511699676514 190 | Enable Stereo Rendering: 191 | Stereo Eye Separation: 0.05999999865889549 192 | Stereo Focal Distance: 1 193 | Swap Stereo Eyes: false 194 | Value: false 195 | Focal Point: 196 | X: 0.15866273641586304 197 | Y: 0.23128746449947357 198 | Z: 0.28159964084625244 199 | Focal Shape Fixed Size: true 200 | Focal Shape Size: 0.05000000074505806 201 | Invert Z Axis: false 202 | Name: Current View 203 | Near Clip Distance: 0.009999999776482582 204 | Pitch: 0.4447966516017914 205 | Target Frame: 206 | Value: Orbit (rviz) 207 | Yaw: 4.253581523895264 208 | Saved: ~ 209 | Window Geometry: 210 | Camera: 211 | collapsed: false 212 | Displays: 213 | collapsed: false 214 | Height: 842 215 | Hide Left Dock: false 216 | Hide Right Dock: false 217 | QMainWindow State: 000000ff00000000fd00000004000000000000018e000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b0000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000025f000002b0fc0200000008fb0000000c00430061006d006500720061010000003b000001490000002800fffffffb0000000c00430061006d006500720061010000018a000001610000002800fffffffb0000000c00430061006d0065007200610100000169000001820000000000000000fb0000000c00430061006d00650072006101000001b9000001320000000000000000fb0000000a0049006d006100670065010000003b000002b00000000000000000fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a20000003efc0100000002fb0000000800540069006d00650100000000000005a20000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000001a9000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 218 | Selection: 219 | collapsed: false 220 | Time: 221 | collapsed: false 222 | Tool Properties: 223 | collapsed: false 224 | Views: 225 | collapsed: false 226 | Width: 1442 227 | X: 70 228 | Y: 27 229 | -------------------------------------------------------------------------------- /mujoco_ros2_control/src/mujoco_ros2_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include "hardware_interface/component_parser.hpp" 22 | #include "hardware_interface/resource_manager.hpp" 23 | #include "hardware_interface/system_interface.hpp" 24 | 25 | #include "mujoco_ros2_control/mujoco_ros2_control.hpp" 26 | 27 | namespace mujoco_ros2_control 28 | { 29 | MujocoRos2Control::MujocoRos2Control( 30 | rclcpp::Node::SharedPtr &node, mjModel *mujoco_model, mjData *mujoco_data) 31 | : node_(node), 32 | mj_model_(mujoco_model), 33 | mj_data_(mujoco_data), 34 | logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), 35 | control_period_(rclcpp::Duration(1, 0)), 36 | last_update_sim_time_ros_(0, 0, RCL_ROS_TIME) 37 | { 38 | } 39 | 40 | MujocoRos2Control::~MujocoRos2Control() 41 | { 42 | stop_cm_thread_ = true; 43 | cm_executor_->remove_node(controller_manager_); 44 | cm_executor_->cancel(); 45 | 46 | if (cm_thread_.joinable()) cm_thread_.join(); 47 | } 48 | 49 | std::string MujocoRos2Control::get_robot_description() 50 | { 51 | // Getting robot description from parameter first. If not set trying from topic 52 | std::string robot_description; 53 | 54 | auto node = std::make_shared( 55 | "robot_description_node", 56 | rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); 57 | 58 | if (node->has_parameter("robot_description")) 59 | { 60 | robot_description = node->get_parameter("robot_description").as_string(); 61 | return robot_description; 62 | } 63 | 64 | RCLCPP_WARN( 65 | logger_, 66 | "Failed to get robot_description from parameter. Will listen on the ~/robot_description " 67 | "topic..."); 68 | 69 | auto robot_description_sub = node->create_subscription( 70 | "robot_description", rclcpp::QoS(1).transient_local(), 71 | [&](const std_msgs::msg::String::SharedPtr msg) 72 | { 73 | if (!msg->data.empty() && robot_description.empty()) robot_description = msg->data; 74 | }); 75 | 76 | while (robot_description.empty() && rclcpp::ok()) 77 | { 78 | rclcpp::spin_some(node); 79 | RCLCPP_INFO(node->get_logger(), "Waiting for robot description message"); 80 | rclcpp::sleep_for(std::chrono::milliseconds(500)); 81 | } 82 | 83 | return robot_description; 84 | } 85 | 86 | void MujocoRos2Control::init() 87 | { 88 | clock_publisher_ = node_->create_publisher("/clock", 10); 89 | 90 | std::string urdf_string = this->get_robot_description(); 91 | 92 | // setup actuators and mechanism control node. 93 | std::vector control_hardware_info; 94 | try 95 | { 96 | control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string); 97 | } 98 | catch (const std::runtime_error &ex) 99 | { 100 | RCLCPP_ERROR_STREAM(logger_, "Error parsing URDF : " << ex.what()); 101 | return; 102 | } 103 | 104 | try 105 | { 106 | robot_hw_sim_loader_.reset(new pluginlib::ClassLoader( 107 | "mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface")); 108 | } 109 | catch (pluginlib::LibraryLoadException &ex) 110 | { 111 | RCLCPP_ERROR_STREAM(logger_, "Failed to create hardware interface loader: " << ex.what()); 112 | return; 113 | } 114 | 115 | std::unique_ptr resource_manager = 116 | std::make_unique(); 117 | 118 | try 119 | { 120 | resource_manager->load_urdf(urdf_string, false, false); 121 | } 122 | catch (...) 123 | { 124 | RCLCPP_ERROR(logger_, "Error while initializing URDF!"); 125 | } 126 | 127 | for (const auto &hardware : control_hardware_info) 128 | { 129 | std::string robot_hw_sim_type_str_ = hardware.hardware_class_type; 130 | std::unique_ptr mujoco_system; 131 | try 132 | { 133 | mujoco_system = std::unique_ptr( 134 | robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); 135 | } 136 | catch (pluginlib::PluginlibException &ex) 137 | { 138 | RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what()); 139 | continue; 140 | } 141 | 142 | urdf::Model urdf_model; 143 | urdf_model.initString(urdf_string); 144 | if (!mujoco_system->init_sim(mj_model_, mj_data_, urdf_model, hardware)) 145 | { 146 | RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface"); 147 | return; 148 | } 149 | 150 | resource_manager->import_component(std::move(mujoco_system), hardware); 151 | 152 | rclcpp_lifecycle::State state( 153 | lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, 154 | hardware_interface::lifecycle_state_names::ACTIVE); 155 | resource_manager->set_component_state(hardware.name, state); 156 | } 157 | 158 | // Create the controller manager 159 | RCLCPP_INFO(logger_, "Loading controller_manager"); 160 | cm_executor_ = std::make_shared(); 161 | controller_manager_ = std::make_shared( 162 | std::move(resource_manager), cm_executor_, "controller_manager", node_->get_namespace()); 163 | cm_executor_->add_node(controller_manager_); 164 | 165 | if (!controller_manager_->has_parameter("update_rate")) 166 | { 167 | RCLCPP_ERROR_STREAM(logger_, "controller manager doesn't have an update_rate parameter"); 168 | return; 169 | } 170 | 171 | auto update_rate = controller_manager_->get_parameter("update_rate").as_int(); 172 | control_period_ = rclcpp::Duration(std::chrono::duration_cast( 173 | std::chrono::duration(1.0 / static_cast(update_rate)))); 174 | 175 | // Force setting of use_sime_time parameter 176 | controller_manager_->set_parameter( 177 | rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true))); 178 | 179 | stop_cm_thread_ = false; 180 | auto spin = [this]() 181 | { 182 | while (rclcpp::ok() && !stop_cm_thread_) 183 | { 184 | cm_executor_->spin_once(); 185 | } 186 | }; 187 | cm_thread_ = std::thread(spin); 188 | } 189 | 190 | void MujocoRos2Control::update() 191 | { 192 | // Get the simulation time and period 193 | auto sim_time = mj_data_->time; 194 | int sim_time_sec = static_cast(sim_time); 195 | int sim_time_nanosec = static_cast((sim_time - sim_time_sec) * 1000000000); 196 | 197 | rclcpp::Time sim_time_ros(sim_time_sec, sim_time_nanosec, RCL_ROS_TIME); 198 | rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; 199 | 200 | publish_sim_time(sim_time_ros); 201 | 202 | mj_step1(mj_model_, mj_data_); 203 | 204 | if (sim_period >= control_period_) 205 | { 206 | controller_manager_->read(sim_time_ros, sim_period); 207 | controller_manager_->update(sim_time_ros, sim_period); 208 | last_update_sim_time_ros_ = sim_time_ros; 209 | } 210 | 211 | // use same time as for read and update call - this is how it is done in ros2_control_node 212 | controller_manager_->write(sim_time_ros, sim_period); 213 | 214 | mj_step2(mj_model_, mj_data_); 215 | } 216 | 217 | void MujocoRos2Control::publish_sim_time(rclcpp::Time sim_time) 218 | { 219 | // TODO(sangteak601) 220 | rosgraph_msgs::msg::Clock sim_time_msg; 221 | sim_time_msg.clock = sim_time; 222 | clock_publisher_->publish(sim_time_msg); 223 | } 224 | 225 | } // namespace mujoco_ros2_control 226 | -------------------------------------------------------------------------------- /mujoco_ros2_control/src/mujoco_system.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2025 Sangtaek Lee 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #include "mujoco_ros2_control/mujoco_system.hpp" 22 | 23 | namespace mujoco_ros2_control 24 | { 25 | MujocoSystem::MujocoSystem() : logger_(rclcpp::get_logger("")) {} 26 | 27 | std::vector MujocoSystem::export_state_interfaces() 28 | { 29 | return std::move(state_interfaces_); 30 | } 31 | 32 | std::vector MujocoSystem::export_command_interfaces() 33 | { 34 | return std::move(command_interfaces_); 35 | } 36 | 37 | hardware_interface::return_type MujocoSystem::read( 38 | const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) 39 | { 40 | // Joint states 41 | for (auto &joint_state : joint_states_) 42 | { 43 | joint_state.position = mj_data_->qpos[joint_state.mj_pos_adr]; 44 | joint_state.velocity = mj_data_->qvel[joint_state.mj_vel_adr]; 45 | joint_state.effort = mj_data_->qfrc_applied[joint_state.mj_vel_adr]; 46 | } 47 | 48 | // IMU Sensor data 49 | for (auto &data : imu_sensor_data_) 50 | { 51 | data.orientation.data.w() = mj_data_->sensordata[data.orientation.mj_sensor_index]; 52 | data.orientation.data.x() = mj_data_->sensordata[data.orientation.mj_sensor_index + 1]; 53 | data.orientation.data.y() = mj_data_->sensordata[data.orientation.mj_sensor_index + 2]; 54 | data.orientation.data.z() = mj_data_->sensordata[data.orientation.mj_sensor_index + 3]; 55 | 56 | data.angular_velocity.data.x() = mj_data_->sensordata[data.angular_velocity.mj_sensor_index]; 57 | data.angular_velocity.data.y() = 58 | mj_data_->sensordata[data.angular_velocity.mj_sensor_index + 1]; 59 | data.angular_velocity.data.z() = 60 | mj_data_->sensordata[data.angular_velocity.mj_sensor_index + 2]; 61 | 62 | data.linear_acceleration.data.x() = 63 | mj_data_->sensordata[data.linear_acceleration.mj_sensor_index]; 64 | data.linear_acceleration.data.y() = 65 | mj_data_->sensordata[data.linear_acceleration.mj_sensor_index + 1]; 66 | data.linear_acceleration.data.z() = 67 | mj_data_->sensordata[data.linear_acceleration.mj_sensor_index + 2]; 68 | } 69 | 70 | // FT Sensor data 71 | for (auto &data : ft_sensor_data_) 72 | { 73 | data.force.data.x() = -mj_data_->sensordata[data.force.mj_sensor_index]; 74 | data.force.data.y() = -mj_data_->sensordata[data.force.mj_sensor_index + 1]; 75 | data.force.data.z() = -mj_data_->sensordata[data.force.mj_sensor_index + 2]; 76 | 77 | data.torque.data.x() = -mj_data_->sensordata[data.torque.mj_sensor_index]; 78 | data.torque.data.y() = -mj_data_->sensordata[data.torque.mj_sensor_index + 1]; 79 | data.torque.data.z() = -mj_data_->sensordata[data.torque.mj_sensor_index + 2]; 80 | } 81 | 82 | return hardware_interface::return_type::OK; 83 | } 84 | 85 | hardware_interface::return_type MujocoSystem::write( 86 | const rclcpp::Time & /* time */, const rclcpp::Duration &period) 87 | { 88 | // update mimic joint 89 | for (auto &joint_state : joint_states_) 90 | { 91 | if (joint_state.is_mimic) 92 | { 93 | joint_state.position_command = 94 | joint_state.mimic_multiplier * 95 | joint_states_.at(joint_state.mimicked_joint_index).position_command; 96 | joint_state.velocity_command = 97 | joint_state.mimic_multiplier * 98 | joint_states_.at(joint_state.mimicked_joint_index).velocity_command; 99 | joint_state.effort_command = 100 | joint_state.mimic_multiplier * 101 | joint_states_.at(joint_state.mimicked_joint_index).effort_command; 102 | } 103 | } 104 | // Joint states 105 | for (auto &joint_state : joint_states_) 106 | { 107 | if (joint_state.is_position_control_enabled) 108 | { 109 | if (joint_state.is_pid_enabled) 110 | { 111 | double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr]; 112 | mj_data_->qfrc_applied[joint_state.mj_vel_adr] = 113 | joint_state.position_pid.computeCommand(error, period.nanoseconds()); 114 | } 115 | else 116 | { 117 | mj_data_->qpos[joint_state.mj_pos_adr] = joint_state.position_command; 118 | } 119 | } 120 | 121 | if (joint_state.is_velocity_control_enabled) 122 | { 123 | if (joint_state.is_pid_enabled) 124 | { 125 | double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr]; 126 | mj_data_->qfrc_applied[joint_state.mj_vel_adr] = 127 | joint_state.velocity_pid.computeCommand(error, period.nanoseconds()); 128 | ; 129 | } 130 | else 131 | { 132 | mj_data_->qvel[joint_state.mj_vel_adr] = joint_state.velocity_command; 133 | } 134 | } 135 | 136 | if (joint_state.is_effort_control_enabled) 137 | { 138 | double min_eff, max_eff; 139 | min_eff = joint_state.joint_limits.has_effort_limits 140 | ? -1 * joint_state.joint_limits.max_effort 141 | : std::numeric_limits::lowest(); 142 | min_eff = std::max(min_eff, joint_state.min_effort_command); 143 | 144 | max_eff = joint_state.joint_limits.has_effort_limits ? joint_state.joint_limits.max_effort 145 | : std::numeric_limits::max(); 146 | max_eff = std::min(max_eff, joint_state.max_effort_command); 147 | 148 | mj_data_->qfrc_applied[joint_state.mj_vel_adr] = 149 | clamp(joint_state.effort_command, min_eff, max_eff); 150 | } 151 | } 152 | return hardware_interface::return_type::OK; 153 | } 154 | 155 | bool MujocoSystem::init_sim( 156 | mjModel *mujoco_model, mjData *mujoco_data, const urdf::Model &urdf_model, 157 | const hardware_interface::HardwareInfo &hardware_info) 158 | { 159 | mj_model_ = mujoco_model; 160 | mj_data_ = mujoco_data; 161 | 162 | logger_ = rclcpp::get_logger("mujoco_system"); 163 | 164 | register_joints(urdf_model, hardware_info); 165 | register_sensors(urdf_model, hardware_info); 166 | 167 | set_initial_pose(); 168 | return true; 169 | } 170 | 171 | void MujocoSystem::register_joints( 172 | const urdf::Model &urdf_model, const hardware_interface::HardwareInfo &hardware_info) 173 | { 174 | joint_states_.resize(hardware_info.joints.size()); 175 | 176 | for (size_t joint_index = 0; joint_index < hardware_info.joints.size(); joint_index++) 177 | { 178 | auto joint = hardware_info.joints.at(joint_index); 179 | int mujoco_joint_id = mj_name2id(mj_model_, mjtObj::mjOBJ_JOINT, joint.name.c_str()); 180 | if (mujoco_joint_id == -1) 181 | { 182 | RCLCPP_ERROR_STREAM( 183 | logger_, "Failed to find joint in mujoco model, joint name: " << joint.name); 184 | continue; 185 | } 186 | 187 | // save information in joint_states_ variable 188 | JointState joint_state; 189 | joint_state.name = joint.name; 190 | joint_state.mj_joint_type = mj_model_->jnt_type[mujoco_joint_id]; 191 | joint_state.mj_pos_adr = mj_model_->jnt_qposadr[mujoco_joint_id]; 192 | joint_state.mj_vel_adr = mj_model_->jnt_dofadr[mujoco_joint_id]; 193 | 194 | joint_states_.at(joint_index) = joint_state; 195 | JointState &last_joint_state = joint_states_.at(joint_index); 196 | 197 | // get joint limit from urdf 198 | get_joint_limits(urdf_model.getJoint(last_joint_state.name), last_joint_state.joint_limits); 199 | 200 | // check if mimicked 201 | if (joint.parameters.find("mimic") != joint.parameters.end()) 202 | { 203 | const auto mimicked_joint = joint.parameters.at("mimic"); 204 | const auto mimicked_joint_it = std::find_if( 205 | hardware_info.joints.begin(), hardware_info.joints.end(), 206 | [&mimicked_joint](const hardware_interface::ComponentInfo &info) 207 | { return info.name == mimicked_joint; }); 208 | if (mimicked_joint_it == hardware_info.joints.end()) 209 | { 210 | throw std::runtime_error(std::string("Mimicked joint '") + mimicked_joint + "' not found"); 211 | } 212 | last_joint_state.is_mimic = true; 213 | last_joint_state.mimicked_joint_index = 214 | std::distance(hardware_info.joints.begin(), mimicked_joint_it); 215 | 216 | auto param_it = joint.parameters.find("multiplier"); 217 | if (param_it != joint.parameters.end()) 218 | { 219 | last_joint_state.mimic_multiplier = std::stod(joint.parameters.at("multiplier")); 220 | } 221 | else 222 | { 223 | last_joint_state.mimic_multiplier = 1.0; 224 | } 225 | } 226 | 227 | auto get_initial_value = [this](const hardware_interface::InterfaceInfo &interface_info) 228 | { 229 | if (!interface_info.initial_value.empty()) 230 | { 231 | double value = std::stod(interface_info.initial_value); 232 | return value; 233 | } 234 | else 235 | { 236 | return 0.0; 237 | } 238 | }; 239 | 240 | // state interfaces 241 | for (const auto &state_if : joint.state_interfaces) 242 | { 243 | if (state_if.name == hardware_interface::HW_IF_POSITION) 244 | { 245 | state_interfaces_.emplace_back( 246 | joint.name, hardware_interface::HW_IF_POSITION, &last_joint_state.position); 247 | last_joint_state.position = get_initial_value(state_if); 248 | } 249 | else if (state_if.name == hardware_interface::HW_IF_VELOCITY) 250 | { 251 | state_interfaces_.emplace_back( 252 | joint.name, hardware_interface::HW_IF_VELOCITY, &last_joint_state.velocity); 253 | last_joint_state.velocity = get_initial_value(state_if); 254 | } 255 | else if (state_if.name == hardware_interface::HW_IF_EFFORT) 256 | { 257 | state_interfaces_.emplace_back( 258 | joint.name, hardware_interface::HW_IF_EFFORT, &last_joint_state.effort); 259 | last_joint_state.effort = get_initial_value(state_if); 260 | } 261 | } 262 | 263 | auto get_min_value = [this](const hardware_interface::InterfaceInfo &interface_info) 264 | { 265 | if (!interface_info.min.empty()) 266 | { 267 | double value = std::stod(interface_info.min); 268 | return value; 269 | } 270 | else 271 | { 272 | return -1 * std::numeric_limits::max(); 273 | } 274 | }; 275 | 276 | auto get_max_value = [this](const hardware_interface::InterfaceInfo &interface_info) 277 | { 278 | if (!interface_info.max.empty()) 279 | { 280 | double value = std::stod(interface_info.max); 281 | return value; 282 | } 283 | else 284 | { 285 | return std::numeric_limits::max(); 286 | } 287 | }; 288 | 289 | // command interfaces 290 | // overwrite joint limit with min/max value 291 | for (const auto &command_if : joint.command_interfaces) 292 | { 293 | if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos) 294 | { 295 | command_interfaces_.emplace_back( 296 | joint.name, hardware_interface::HW_IF_POSITION, &last_joint_state.position_command); 297 | last_joint_state.is_position_control_enabled = true; 298 | last_joint_state.position_command = last_joint_state.position; 299 | // TODO(sangteak601): These are not used at all. Potentially can be removed. 300 | last_joint_state.min_position_command = get_min_value(command_if); 301 | last_joint_state.max_position_command = get_max_value(command_if); 302 | } 303 | else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos) 304 | { 305 | command_interfaces_.emplace_back( 306 | joint.name, hardware_interface::HW_IF_VELOCITY, &last_joint_state.velocity_command); 307 | last_joint_state.is_velocity_control_enabled = true; 308 | last_joint_state.velocity_command = last_joint_state.velocity; 309 | // TODO(sangteak601): These are not used at all. Potentially can be removed. 310 | last_joint_state.min_velocity_command = get_min_value(command_if); 311 | last_joint_state.max_velocity_command = get_max_value(command_if); 312 | } 313 | else if (command_if.name == hardware_interface::HW_IF_EFFORT) 314 | { 315 | command_interfaces_.emplace_back( 316 | joint.name, hardware_interface::HW_IF_EFFORT, &last_joint_state.effort_command); 317 | last_joint_state.is_effort_control_enabled = true; 318 | last_joint_state.effort_command = last_joint_state.effort; 319 | last_joint_state.min_effort_command = get_min_value(command_if); 320 | last_joint_state.max_effort_command = get_max_value(command_if); 321 | } 322 | 323 | if (command_if.name.find("_pid") != std::string::npos) 324 | { 325 | last_joint_state.is_pid_enabled = true; 326 | } 327 | } 328 | 329 | // Get PID gains, if needed 330 | if (last_joint_state.is_pid_enabled) 331 | { 332 | last_joint_state.position_pid = get_pid_gains(joint, hardware_interface::HW_IF_POSITION); 333 | last_joint_state.velocity_pid = get_pid_gains(joint, hardware_interface::HW_IF_VELOCITY); 334 | } 335 | } 336 | } 337 | 338 | void MujocoSystem::register_sensors( 339 | const urdf::Model & /* urdf_model */, const hardware_interface::HardwareInfo &hardware_info) 340 | { 341 | // Assuming force/torque sensor end with "_fts" in the name, 342 | // and IMU sensor end with "_imu" in the name 343 | for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++) 344 | { 345 | auto sensor = hardware_info.sensors.at(sensor_index); 346 | std::string sensor_name = sensor.name; 347 | sensor_name = sensor_name.substr(0, sensor_name.rfind('_')); 348 | 349 | if (sensor.name.find("_fts") != std::string::npos) 350 | { 351 | FTSensorData sensor_data; 352 | sensor_data.name = sensor_name; 353 | sensor_data.force.name = sensor_name + "_force"; 354 | sensor_data.torque.name = sensor_name + "_torque"; 355 | 356 | int force_sensor_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.force.name.c_str()); 357 | int torque_sensor_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.torque.name.c_str()); 358 | 359 | if (force_sensor_id == -1 || torque_sensor_id == -1) 360 | { 361 | RCLCPP_ERROR_STREAM( 362 | logger_, 363 | "Failed to find force/torque sensor in mujoco model, sensor name: " << sensor.name); 364 | continue; 365 | } 366 | 367 | sensor_data.force.mj_sensor_index = mj_model_->sensor_adr[force_sensor_id]; 368 | sensor_data.torque.mj_sensor_index = mj_model_->sensor_adr[torque_sensor_id]; 369 | 370 | ft_sensor_data_.push_back(sensor_data); 371 | auto &last_sensor_data = ft_sensor_data_.back(); 372 | 373 | for (const auto &state_if : sensor.state_interfaces) 374 | { 375 | if (state_if.name == "force.x") 376 | { 377 | state_interfaces_.emplace_back( 378 | sensor.name, state_if.name, &last_sensor_data.force.data.x()); 379 | } 380 | else if (state_if.name == "force.y") 381 | { 382 | state_interfaces_.emplace_back( 383 | sensor.name, state_if.name, &last_sensor_data.force.data.y()); 384 | } 385 | else if (state_if.name == "force.z") 386 | { 387 | state_interfaces_.emplace_back( 388 | sensor.name, state_if.name, &last_sensor_data.force.data.z()); 389 | } 390 | else if (state_if.name == "torque.x") 391 | { 392 | state_interfaces_.emplace_back( 393 | sensor.name, state_if.name, &last_sensor_data.torque.data.x()); 394 | } 395 | else if (state_if.name == "torque.y") 396 | { 397 | state_interfaces_.emplace_back( 398 | sensor.name, state_if.name, &last_sensor_data.torque.data.y()); 399 | } 400 | else if (state_if.name == "torque.z") 401 | { 402 | state_interfaces_.emplace_back( 403 | sensor.name, state_if.name, &last_sensor_data.torque.data.z()); 404 | } 405 | } 406 | } 407 | 408 | else if (sensor.name.find("_imu") != std::string::npos) 409 | { 410 | IMUSensorData sensor_data; 411 | sensor_data.name = sensor_name; 412 | sensor_data.orientation.name = sensor_name + "_quat"; 413 | sensor_data.angular_velocity.name = sensor_name + "_gyro"; 414 | sensor_data.linear_acceleration.name = sensor_name + "_accel"; 415 | 416 | int quat_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.orientation.name.c_str()); 417 | int gyro_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.angular_velocity.name.c_str()); 418 | int accel_id = 419 | mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.linear_acceleration.name.c_str()); 420 | 421 | if (quat_id == -1 || gyro_id == -1 || accel_id == -1) 422 | { 423 | RCLCPP_ERROR_STREAM( 424 | logger_, "Failed to find IMU sensor in mujoco model, sensor name: " << sensor.name); 425 | continue; 426 | } 427 | 428 | sensor_data.orientation.mj_sensor_index = mj_model_->sensor_adr[quat_id]; 429 | sensor_data.angular_velocity.mj_sensor_index = mj_model_->sensor_adr[gyro_id]; 430 | sensor_data.linear_acceleration.mj_sensor_index = mj_model_->sensor_adr[accel_id]; 431 | 432 | imu_sensor_data_.push_back(sensor_data); 433 | auto &last_sensor_data = imu_sensor_data_.back(); 434 | 435 | for (const auto &state_if : sensor.state_interfaces) 436 | { 437 | if (state_if.name == "orientation.x") 438 | { 439 | state_interfaces_.emplace_back( 440 | sensor.name, state_if.name, &last_sensor_data.orientation.data.x()); 441 | } 442 | else if (state_if.name == "orientation.y") 443 | { 444 | state_interfaces_.emplace_back( 445 | sensor.name, state_if.name, &last_sensor_data.orientation.data.y()); 446 | } 447 | else if (state_if.name == "orientation.z") 448 | { 449 | state_interfaces_.emplace_back( 450 | sensor.name, state_if.name, &last_sensor_data.orientation.data.z()); 451 | } 452 | else if (state_if.name == "orientation.w") 453 | { 454 | state_interfaces_.emplace_back( 455 | sensor.name, state_if.name, &last_sensor_data.orientation.data.w()); 456 | } 457 | else if (state_if.name == "angular_velocity.x") 458 | { 459 | state_interfaces_.emplace_back( 460 | sensor.name, state_if.name, &last_sensor_data.angular_velocity.data.x()); 461 | } 462 | else if (state_if.name == "angular_velocity.y") 463 | { 464 | state_interfaces_.emplace_back( 465 | sensor.name, state_if.name, &last_sensor_data.angular_velocity.data.y()); 466 | } 467 | else if (state_if.name == "angular_velocity.z") 468 | { 469 | state_interfaces_.emplace_back( 470 | sensor.name, state_if.name, &last_sensor_data.angular_velocity.data.z()); 471 | } 472 | else if (state_if.name == "linear_acceleration.x") 473 | { 474 | state_interfaces_.emplace_back( 475 | sensor.name, state_if.name, &last_sensor_data.linear_acceleration.data.x()); 476 | } 477 | else if (state_if.name == "linear_acceleration.y") 478 | { 479 | state_interfaces_.emplace_back( 480 | sensor.name, state_if.name, &last_sensor_data.linear_acceleration.data.y()); 481 | } 482 | else if (state_if.name == "linear_acceleration.z") 483 | { 484 | state_interfaces_.emplace_back( 485 | sensor.name, state_if.name, &last_sensor_data.linear_acceleration.data.z()); 486 | } 487 | } 488 | } 489 | } 490 | } 491 | 492 | void MujocoSystem::set_initial_pose() 493 | { 494 | for (auto &joint_state : joint_states_) 495 | { 496 | mj_data_->qpos[joint_state.mj_pos_adr] = joint_state.position; 497 | } 498 | } 499 | 500 | void MujocoSystem::get_joint_limits( 501 | urdf::JointConstSharedPtr urdf_joint, joint_limits::JointLimits &joint_limits) 502 | { 503 | if (urdf_joint->limits) 504 | { 505 | joint_limits.min_position = urdf_joint->limits->lower; 506 | joint_limits.max_position = urdf_joint->limits->upper; 507 | joint_limits.max_velocity = urdf_joint->limits->velocity; 508 | joint_limits.max_effort = urdf_joint->limits->effort; 509 | } 510 | } 511 | 512 | control_toolbox::Pid MujocoSystem::get_pid_gains( 513 | const hardware_interface::ComponentInfo &joint_info, std::string command_interface) 514 | { 515 | double kp, ki, kd, i_max, i_min; 516 | std::string key; 517 | key = command_interface + std::string(PARAM_KP); 518 | if (joint_info.parameters.find(key) != joint_info.parameters.end()) 519 | { 520 | kp = std::stod(joint_info.parameters.at(key)); 521 | } 522 | else 523 | { 524 | kp = 0.0; 525 | } 526 | 527 | key = command_interface + std::string(PARAM_KI); 528 | if (joint_info.parameters.find(key) != joint_info.parameters.end()) 529 | { 530 | ki = std::stod(joint_info.parameters.at(key)); 531 | } 532 | else 533 | { 534 | ki = 0.0; 535 | } 536 | 537 | key = command_interface + std::string(PARAM_KD); 538 | if (joint_info.parameters.find(key) != joint_info.parameters.end()) 539 | { 540 | kd = std::stod(joint_info.parameters.at(key)); 541 | } 542 | else 543 | { 544 | kd = 0.0; 545 | } 546 | 547 | bool enable_anti_windup = false; 548 | key = command_interface + std::string(PARAM_I_MAX); 549 | if (joint_info.parameters.find(key) != joint_info.parameters.end()) 550 | { 551 | i_max = std::stod(joint_info.parameters.at(key)); 552 | enable_anti_windup = true; 553 | } 554 | else 555 | { 556 | i_max = std::numeric_limits::max(); 557 | } 558 | 559 | key = command_interface + std::string(PARAM_I_MIN); 560 | if (joint_info.parameters.find(key) != joint_info.parameters.end()) 561 | { 562 | i_min = std::stod(joint_info.parameters.at(key)); 563 | enable_anti_windup = true; 564 | } 565 | else 566 | { 567 | i_min = std::numeric_limits::lowest(); 568 | } 569 | 570 | return control_toolbox::Pid(kp, ki, kd, i_max, i_min, enable_anti_windup); 571 | } 572 | } // namespace mujoco_ros2_control 573 | 574 | #include "pluginlib/class_list_macros.hpp" 575 | PLUGINLIB_EXPORT_CLASS( 576 | mujoco_ros2_control::MujocoSystem, mujoco_ros2_control::MujocoSystemInterface) 577 | --------------------------------------------------------------------------------