Support for different control modes with mujoco by defining PIDs

CMakeLists.txt

@@ -11,6 +11,7 @@ find_package(ament_cmake REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(glfw3 REQUIRED)
+find_package(control_toolbox REQUIRED)
 find_package(hardware_interface REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -158,6 +159,7 @@ target_link_libraries(mujoco_ros2_simulation
   pluginlib::pluginlib
   rclcpp::rclcpp
   rclcpp_lifecycle::rclcpp_lifecycle
+  control_toolbox::control_toolbox
   sensor_msgs::sensor_msgs_library
   Eigen3::Eigen
   Threads::Threads

README.md

@@ -51,6 +51,11 @@ Just specify the plugin and point to a valid MJCF on launch:
       <plugin>mujoco_ros2_simulation/MujocoSystemInterface</plugin>
       <param name="mujoco_model">$(find my_description)/description/scene.xml</param>
 
+      <!--
+       Optional parameter to load the PIDs that can be used with the actuators loaded with the MuJoCo model. The velocity actuator supports position mode with the PID gains and the rest of the actuation models support both position and velocity mode provided the corresponding PID gains. The gains should be in ROS parameters format and is loaded by the control_toolbox::PidROS class
+        -->
+      <param name="pids_config_file">$(find my_description)/config/pids.yaml</param>
+
       <!--
        Optional parameter to override the speed scaling parameters from the Simulate App window
        and just attempt to run at whatever the desired rate is here. This allows users to run the simulation

include/mujoco_ros2_simulation/data.hpp

@@ -21,13 +21,22 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include "control_toolbox/pid_ros.hpp"
 
 #include <string>
 #include <vector>
 
 namespace mujoco_ros2_simulation
 {
 
+enum class ActuatorType
+{
+  MOTOR,
+  POSITION,
+  VELOCITY,
+  CUSTOM
+};
+
 /**
  * Wrapper for mujoco actuators and relevant ROS HW interface data.
  */
@@ -37,6 +46,9 @@ struct JointState
   double position;
   double velocity;
   double effort;
+  std::shared_ptr<control_toolbox::PidROS> pos_pid{ nullptr };
+  std::shared_ptr<control_toolbox::PidROS> vel_pid{ nullptr };
+  ActuatorType actuator_type;
   double position_command;
   double velocity_command;
   double effort_command;
@@ -51,8 +63,12 @@ struct JointState
   // Booleans record whether or not we should be writing commands to these interfaces
   // based on if they have been claimed.
   bool is_position_control_enabled{ false };
+  bool is_position_pid_control_enabled{ false };
+  bool is_velocity_pid_control_enabled{ false };
   bool is_velocity_control_enabled{ false };
   bool is_effort_control_enabled{ false };
+  bool has_pos_pid{ false };
+  bool has_vel_pid{ false };
 };
 
 template <typename T>

package.xml

@@ -17,6 +17,7 @@
   <depend>rclcpp</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>sensor_msgs</depend>
+  <depend>control_toolbox</depend>
 
   <test_depend>joint_state_broadcaster</test_depend>
   <test_depend>position_controllers</test_depend>

pixi.toml

@@ -49,6 +49,7 @@ ros-jazzy-ros-base = "*"
 ros-jazzy-ament-cmake = "*"
 ros-jazzy-rclcpp = "*"
 ros-jazzy-controller-manager = "*"
+ros-jazzy-control-toolbox = "*"
 ros-jazzy-hardware-interface = "*"
 ros-jazzy-joint-state-broadcaster = "*"
 ros-jazzy-position-controllers = "*"