From 9ddb880a80e6afb01eeb290aa22814b9e2d6de9f Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Tue, 22 Apr 2025 14:39:09 -0400 Subject: [PATCH 01/11] Update control and state interfaces for non PID actuators --- mujoco_ros2_control/src/mujoco_system.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index 95f879d..f7a4746 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -42,7 +42,7 @@ hardware_interface::return_type MujocoSystem::read( { joint_state.position = mj_data_->qpos[joint_state.mj_pos_adr]; joint_state.velocity = mj_data_->qvel[joint_state.mj_vel_adr]; - joint_state.effort = mj_data_->qfrc_applied[joint_state.mj_vel_adr]; + joint_state.effort = mj_data_->actuator_force[joint_state.mj_vel_adr]; } // IMU Sensor data @@ -114,7 +114,7 @@ hardware_interface::return_type MujocoSystem::write( } else { - mj_data_->qpos[joint_state.mj_pos_adr] = joint_state.position_command; + mj_data_->ctrl[joint_state.mj_pos_adr] = joint_state.position_command; } } @@ -129,7 +129,7 @@ hardware_interface::return_type MujocoSystem::write( } else { - mj_data_->qvel[joint_state.mj_vel_adr] = joint_state.velocity_command; + mj_data_->ctrl[joint_state.mj_vel_adr] = joint_state.velocity_command; } } From fddc58fbe37ad6190ed551f614261d4643233fad Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Fri, 2 May 2025 16:19:42 -0400 Subject: [PATCH 02/11] Find the matching actuator for a joint --- .../mujoco_ros2_control/mujoco_system.hpp | 1 + mujoco_ros2_control/src/mujoco_system.cpp | 24 +++++++++++++++++-- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp index 6060222..e0179c1 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp @@ -82,6 +82,7 @@ class MujocoSystem : public MujocoSystemInterface int mj_joint_type; int mj_pos_adr; int mj_vel_adr; + int mj_actuator_id; }; template diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index f7a4746..5a16ec8 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -114,7 +114,7 @@ hardware_interface::return_type MujocoSystem::write( } else { - mj_data_->ctrl[joint_state.mj_pos_adr] = joint_state.position_command; + mj_data_->ctrl[joint_state.mj_actuator_id] = joint_state.position_command; } } @@ -129,7 +129,7 @@ hardware_interface::return_type MujocoSystem::write( } else { - mj_data_->ctrl[joint_state.mj_vel_adr] = joint_state.velocity_command; + mj_data_->ctrl[joint_state.mj_actuator_id] = joint_state.velocity_command; } } @@ -184,12 +184,32 @@ void MujocoSystem::register_joints( continue; } + // Try to locate the matching actuator id for the joint, if available + int mujoco_actuator_id = -1; + for (int i = 0; i < mj_model_->nu; ++i) + { + // If it is the correct type and matches the joint id, we're done + if ( + mj_model_->actuator_trntype[i] == mjTRN_JOINT && + mj_model_->actuator_trnid[2 * i] == mujoco_joint_id) + { + mujoco_actuator_id = i; + break; + } + } + if (mujoco_actuator_id == -1) + { + RCLCPP_ERROR_STREAM(logger_, "No actuator found for joint: " << joint.name); + continue; + } + // save information in joint_states_ variable JointState joint_state; joint_state.name = joint.name; joint_state.mj_joint_type = mj_model_->jnt_type[mujoco_joint_id]; joint_state.mj_pos_adr = mj_model_->jnt_qposadr[mujoco_joint_id]; joint_state.mj_vel_adr = mj_model_->jnt_dofadr[mujoco_joint_id]; + joint_state.mj_actuator_id = mujoco_actuator_id; joint_states_.at(joint_index) = joint_state; JointState &last_joint_state = joint_states_.at(joint_index); From 7605fe757f9a7597d3cb6604520d42857798f180 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Fri, 2 May 2025 16:40:56 -0400 Subject: [PATCH 03/11] Add all joints even if they don't have actuators (mimics) --- mujoco_ros2_control/src/mujoco_system.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index 5a16ec8..91f4eba 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -199,8 +199,8 @@ void MujocoSystem::register_joints( } if (mujoco_actuator_id == -1) { - RCLCPP_ERROR_STREAM(logger_, "No actuator found for joint: " << joint.name); - continue; + // This isn't a failure the joint just won't be controllable + RCLCPP_WARN_STREAM(logger_, "No actuator found for joint: " << joint.name); } // save information in joint_states_ variable From a4bd9e3ae638c111fcd79e889920eee2da698396 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Sun, 4 May 2025 14:11:36 +0900 Subject: [PATCH 04/11] Update actuator in camera demo --- mujoco_ros2_control_demos/mujoco_models/test_camera.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_camera.xml b/mujoco_ros2_control_demos/mujoco_models/test_camera.xml index 8f8226b..93fa4b1 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_camera.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_camera.xml @@ -24,7 +24,7 @@ - + From 1c025a89efabdd51796d0299e4cfce873c70dcf2 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Sun, 4 May 2025 14:46:36 +0900 Subject: [PATCH 05/11] Update cart position and velocity demos --- .../examples/example_position.cpp | 10 +++++----- .../examples/example_velocity.cpp | 4 ++-- .../launch/cart_example_position.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- .../mujoco_models/test_cart_position.xml | 6 ++++++ .../mujoco_models/test_cart_velocity.xml | 6 ++++++ 6 files changed, 21 insertions(+), 9 deletions(-) create mode 100644 mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml create mode 100644 mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml diff --git a/mujoco_ros2_control_demos/examples/example_position.cpp b/mujoco_ros2_control_demos/examples/example_position.cpp index 8d2d4c0..4367e95 100644 --- a/mujoco_ros2_control_demos/examples/example_position.cpp +++ b/mujoco_ros2_control_demos/examples/example_position.cpp @@ -114,17 +114,17 @@ int main(int argc, char * argv[]) point.positions[0] = 0.0; trajectory_msgs::msg::JointTrajectoryPoint point2; - point2.time_from_start = rclcpp::Duration::from_seconds(1.0); + point2.time_from_start = rclcpp::Duration::from_seconds(5.0); point2.positions.resize(joint_names.size()); - point2.positions[0] = -1.0; + point2.positions[0] = -5.0; trajectory_msgs::msg::JointTrajectoryPoint point3; - point3.time_from_start = rclcpp::Duration::from_seconds(2.0); + point3.time_from_start = rclcpp::Duration::from_seconds(10.0); point3.positions.resize(joint_names.size()); - point3.positions[0] = 1.0; + point3.positions[0] = 5.0; trajectory_msgs::msg::JointTrajectoryPoint point4; - point4.time_from_start = rclcpp::Duration::from_seconds(3.0); + point4.time_from_start = rclcpp::Duration::from_seconds(15.0); point4.positions.resize(joint_names.size()); point4.positions[0] = 0.0; diff --git a/mujoco_ros2_control_demos/examples/example_velocity.cpp b/mujoco_ros2_control_demos/examples/example_velocity.cpp index 6607691..49bafb5 100644 --- a/mujoco_ros2_control_demos/examples/example_velocity.cpp +++ b/mujoco_ros2_control_demos/examples/example_velocity.cpp @@ -41,11 +41,11 @@ int main(int argc, char * argv[]) publisher->publish(commands); std::this_thread::sleep_for(1s); - commands.data[0] = 1; + commands.data[0] = 0.5; publisher->publish(commands); std::this_thread::sleep_for(1s); - commands.data[0] = -1; + commands.data[0] = -0.5; publisher->publish(commands); std::this_thread::sleep_for(1s); diff --git a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py index dd4393c..719619c 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): parameters=[ robot_description, controller_config_file, - {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} + {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_position.xml')} ] ) diff --git a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py index cf716b4..dec4e5b 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): parameters=[ robot_description, controller_config_file, - {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} + {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_velocity.xml')} ] ) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml b/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml new file mode 100644 index 0000000..eaab0ac --- /dev/null +++ b/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml b/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml new file mode 100644 index 0000000..10f308e --- /dev/null +++ b/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml @@ -0,0 +1,6 @@ + + + + + + From 84c0cb368d9a051d5aae02476296f5024ab4be87 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Sun, 4 May 2025 14:58:54 +0900 Subject: [PATCH 06/11] Update diff drive example --- mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml | 4 ++++ mujoco_ros2_control_demos/package.xml | 1 + 2 files changed, 5 insertions(+) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index 503eac5..648682d 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -31,4 +31,8 @@ + + + + diff --git a/mujoco_ros2_control_demos/package.xml b/mujoco_ros2_control_demos/package.xml index de98133..810b23a 100644 --- a/mujoco_ros2_control_demos/package.xml +++ b/mujoco_ros2_control_demos/package.xml @@ -25,6 +25,7 @@ imu_sensor_broadcaster position_controllers + diff_drive_controller ament_lint_auto ament_cmake_clang_format From d30e6042da386b2de7dd0af29a2d8bdaaf19f90b Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 28 May 2025 10:46:50 -0400 Subject: [PATCH 07/11] Simplify cart example --- .../launch/cart_example_position.launch.py | 2 +- .../launch/cart_example_velocity.launch.py | 2 +- mujoco_ros2_control_demos/mujoco_models/test_cart.xml | 4 ++++ .../mujoco_models/test_cart_position.xml | 6 ------ .../mujoco_models/test_cart_velocity.xml | 6 ------ 5 files changed, 6 insertions(+), 14 deletions(-) delete mode 100644 mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml delete mode 100644 mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml diff --git a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py index 719619c..dd4393c 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py @@ -33,7 +33,7 @@ def generate_launch_description(): parameters=[ robot_description, controller_config_file, - {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_position.xml')} + {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] ) diff --git a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py index dec4e5b..cf716b4 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -32,7 +32,7 @@ def generate_launch_description(): parameters=[ robot_description, controller_config_file, - {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart_velocity.xml')} + {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] ) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_cart.xml b/mujoco_ros2_control_demos/mujoco_models/test_cart.xml index 3de0c67..5a6ed91 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_cart.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_cart.xml @@ -25,4 +25,8 @@ + + + + diff --git a/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml b/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml deleted file mode 100644 index eaab0ac..0000000 --- a/mujoco_ros2_control_demos/mujoco_models/test_cart_position.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - diff --git a/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml b/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml deleted file mode 100644 index 10f308e..0000000 --- a/mujoco_ros2_control_demos/mujoco_models/test_cart_velocity.xml +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - From cad0e85c08814de800b5f5401894c2cf5d079e29 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 28 May 2025 11:06:58 -0400 Subject: [PATCH 08/11] Fix time warnings on camera launch from rviz --- .../launch/camera_example.launch.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/mujoco_ros2_control_demos/launch/camera_example.launch.py b/mujoco_ros2_control_demos/launch/camera_example.launch.py index 04f77f1..f272113 100644 --- a/mujoco_ros2_control_demos/launch/camera_example.launch.py +++ b/mujoco_ros2_control_demos/launch/camera_example.launch.py @@ -53,13 +53,13 @@ def generate_launch_description(): ) load_joint_state_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time', 'joint_state_broadcaster'], output='screen' ) load_position_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', '--use-sim-time', 'position_controller'], output='screen' ) @@ -74,9 +74,11 @@ def generate_launch_description(): ) return LaunchDescription([ + node_mujoco_ros2_control, + node_robot_state_publisher, RegisterEventHandler( event_handler=OnProcessStart( - target_action=node_mujoco_ros2_control, + target_action=node_robot_state_publisher, on_start=[load_joint_state_controller], ) ), @@ -86,7 +88,10 @@ def generate_launch_description(): on_exit=[load_position_controller], ) ), - node_mujoco_ros2_control, - node_robot_state_publisher, - rviz_node, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=load_position_controller, + on_exit=[rviz_node], + ) + ), ]) From b5209e74c9555003539a61af78d989d0a48c6c5a Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 28 May 2025 11:16:42 -0400 Subject: [PATCH 09/11] Update gripper model example --- .../mujoco_models/test_gripper_mimic_joint.xml | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml b/mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml index bdcf535..75b6728 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_gripper_mimic_joint.xml @@ -25,4 +25,16 @@ + + + + + + + + + + + + From 5c54b633f6576294c0052c51088802e32406736b Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Wed, 28 May 2025 11:23:43 -0400 Subject: [PATCH 10/11] Update tricycle drive example, add controller deps --- mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml | 2 +- .../mujoco_models/test_tricycle_drive.xml | 4 ++++ mujoco_ros2_control_demos/package.xml | 2 ++ 3 files changed, 7 insertions(+), 1 deletion(-) diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index 648682d..7f12124 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -34,5 +34,5 @@ - + diff --git a/mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml index 611be07..c1841ae 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_tricycle_drive.xml @@ -40,4 +40,8 @@ + + + + diff --git a/mujoco_ros2_control_demos/package.xml b/mujoco_ros2_control_demos/package.xml index 810b23a..fdaf151 100644 --- a/mujoco_ros2_control_demos/package.xml +++ b/mujoco_ros2_control_demos/package.xml @@ -26,6 +26,8 @@ position_controllers diff_drive_controller + tricycle_controller + tricycle_steering_controller ament_lint_auto ament_cmake_clang_format From 90cf2c90ed03d0cc58bb9d8d8470a3f33d5655c2 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Tue, 17 Jun 2025 10:19:50 -0400 Subject: [PATCH 11/11] Add documentation for actuation models --- doc/index.rst | 42 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/doc/index.rst b/doc/index.rst index 0584bdb..6d6180f 100755 --- a/doc/index.rst +++ b/doc/index.rst @@ -81,6 +81,48 @@ An overview is available through the "camera" demo. For additional information refer to the ``mujoco_ros2_control_demos/mujoco_models`` for examples. +Controller Options +------------------ +The drivers support two modes of control, either wrapping Mujoco's actuation interface or wrapped PID control of joint interfaces. + +Mujoco's built-in controllers essentially provide PD control of the actuator's interface. +For more information refer to the documentation in their `actuation model `_ docs. +To use their controllers, actuators must be added and tuned directly in the MJCF XML, as in many of the demo examples. +No additional configuration is needed from the drivers side, as commands will be sent directly to the actuator's `ctrl` data. +For example, + +.. code-block:: XML + + + + + + +To use the PID wrapper, users can omit the actuators from their MJCF descriptions, and instead specify gains directly in the ros2_control xacro. +An example is provided in the `examples repo `_. +In this case, we note that the `command_interface` _must_ be suffixed by `_pid`, + +.. code-block:: XML + + + + 3000 + 1 + 100 + 10000 + + + + + + + ${initial_positions['panda_joint1']} + + + 0.0 + + + Specify the location of Mujoco models and the controller configuration file ---------------------------------------------------------------------------- You need to pass parameters for paths as shown in the following example.