diff --git a/CMakeLists.txt b/CMakeLists.txt
index 461a9b6..0235da1 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/README.md b/README.md
index 151a1b0..354a691 100644
--- a/README.md
+++ b/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
diff --git a/include/mujoco_ros2_simulation/data.hpp b/include/mujoco_ros2_simulation/data.hpp
index 02fa65a..818f532 100644
--- a/include/mujoco_ros2_simulation/data.hpp
+++ b/include/mujoco_ros2_simulation/data.hpp
@@ -21,6 +21,7 @@
 
 #include <Eigen/Core>
 #include <Eigen/Geometry>
+#include "control_toolbox/pid_ros.hpp"
 
 #include <string>
 #include <vector>
@@ -28,6 +29,14 @@
 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>
diff --git a/package.xml b/package.xml
index 6ad03ab..1d70d7c 100644
--- a/package.xml
+++ b/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>
diff --git a/pixi.toml b/pixi.toml
index 7482a4e..8924b9a 100644
--- a/pixi.toml
+++ b/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 = "*"
diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp
index 6440b55..e1a686a 100644
--- a/src/mujoco_system_interface.cpp
+++ b/src/mujoco_system_interface.cpp
@@ -26,6 +26,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <cstring>
+#include <filesystem>
 #include <future>
 #include <iostream>
 #include <memory>
@@ -35,10 +36,16 @@
 #include <string>
 #include <thread>
 
+#include <tinyxml2.h>
+#include <std_msgs/msg/string.hpp>
+#include <unordered_map>
+
 #include <hardware_interface/types/hardware_interface_type_values.hpp>
 #include <pluginlib/class_list_macros.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include "control_toolbox/pid.hpp"
+
 #define MUJOCO_PLUGIN_DIR "mujoco_plugin"
 
 using namespace std::chrono_literals;
@@ -50,6 +57,9 @@ const int kErrorLength = 1024;           // load error string length
 
 using Seconds = std::chrono::duration<double>;
 
+/// \brief vector with the current actuator for each joint
+std::unordered_map<std::string, std::string> actuator_map;
+
 namespace mujoco_ros2_simulation
 {
 namespace mj = ::mujoco;
@@ -178,71 +188,125 @@ const char* Diverged(int disableflags, const mjData* d)
   return nullptr;
 }
 
-mjModel* LoadModel(const char* file, mj::Simulate& sim)
+mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node)
 {
+  mjModel* mnew = 0;
+
+  // Try to get the mujoco model from URDF
+
   // this copy is needed so that the mju::strlen call below compiles
   char filename[mj::Simulate::kMaxFilenameLength];
   mju::strcpy_arr(filename, file);
 
-  // make sure filename is not empty
-  if (!filename[0])
+  // load model from path if the filename is not empty
+  if (filename[0])
   {
-    return nullptr;
-  }
+    // load and compile
+    char loadError[kErrorLength] = "";
+    auto load_start = mj::Simulate::Clock::now();
+    if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb",
+                                                       mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4))
+    {
+      mnew = mj_loadModel(filename, nullptr);
+      if (!mnew)
+      {
+        mju::strcpy_arr(loadError, "could not load binary model");
+      }
+    }
+    else
+    {
+      mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength);
+
+      // remove trailing newline character from loadError
+      if (loadError[0])
+      {
+        int error_length = mju::strlen_arr(loadError);
+        if (loadError[error_length - 1] == '\n')
+        {
+          loadError[error_length - 1] = '\0';
+        }
+      }
+    }
+    auto load_interval = mj::Simulate::Clock::now() - load_start;
+    double load_seconds = Seconds(load_interval).count();
 
-  // load and compile
-  char loadError[kErrorLength] = "";
-  mjModel* mnew = 0;
-  auto load_start = mj::Simulate::Clock::now();
-  if (mju::strlen_arr(filename) > 4 && !std::strncmp(filename + mju::strlen_arr(filename) - 4, ".mjb",
-                                                     mju::sizeof_arr(filename) - mju::strlen_arr(filename) + 4))
-  {
-    mnew = mj_loadModel(filename, nullptr);
     if (!mnew)
     {
-      mju::strcpy_arr(loadError, "could not load binary model");
+      std::printf("%s\n", loadError);
+      mju::strcpy_arr(sim.load_error, loadError);
+      return nullptr;
     }
-  }
-  else
-  {
-    mnew = mj_loadXML(filename, nullptr, loadError, kErrorLength);
 
-    // remove trailing newline character from loadError
+    // compiler warning: print and pause
     if (loadError[0])
     {
-      int error_length = mju::strlen_arr(loadError);
-      if (loadError[error_length - 1] == '\n')
-      {
-        loadError[error_length - 1] = '\0';
-      }
+      // mj_forward() below will print the warning message
+      std::printf("Model compiled, but simulation warning (paused):\n  %s\n", loadError);
+      sim.run = 0;
+    }
+
+    // if no error and load took more than 1/4 seconds, report load time
+    else if (load_seconds > 0.25)
+    {
+      mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds);
     }
-  }
-  auto load_interval = mj::Simulate::Clock::now() - load_start;
-  double load_seconds = Seconds(load_interval).count();
 
-  if (!mnew)
-  {
-    std::printf("%s\n", loadError);
     mju::strcpy_arr(sim.load_error, loadError);
-    return nullptr;
+    return mnew;
   }
 
-  // compiler warning: print and pause
-  if (loadError[0])
-  {
-    // mj_forward() below will print the warning message
-    std::printf("Model compiled, but simulation warning (paused):\n  %s\n", loadError);
-    sim.run = 0;
-  }
+  // Try to get the mujoco model from topic
+
+  std::string robot_description;
 
-  // if no error and load took more than 1/4 seconds, report load time
-  else if (load_seconds > 0.25)
+  rclcpp::QoS qos_profile(rclcpp::QoSInitialization::from_rmw(rmw_qos_profile_default));
+  qos_profile.reliable().transient_local().keep_last(1);
+  // Try to get mujoco_model via topic
+  auto mujoco_model_sub = node->create_subscription<std_msgs::msg::String>(
+      "/mujoco_robot_description", qos_profile, [&](const std_msgs::msg::String::SharedPtr msg) {
+        if (!msg->data.empty() && robot_description.empty())
+          robot_description = msg->data;
+      });
+
+  const auto start = node->now();
+  const auto timeout = rclcpp::Duration::from_seconds(5.0);
+  auto last_print = start;
+
+  while (robot_description.empty() && rclcpp::ok())
   {
-    mju::sprintf_arr(loadError, "Model loaded in %.2g seconds", load_seconds);
+    if (node->now() - last_print >= rclcpp::Duration::from_seconds(1.0))
+    {
+      RCLCPP_INFO(node->get_logger(), "Waiting for /mujoco_robot_description...");
+      last_print = node->now();
+    }
+
+    if (node->now() - start > timeout)
+    {
+      RCLCPP_WARN(node->get_logger(), "Timeout waiting for robot_description_mujoco topic.");
+      break;
+    }
+    rclcpp::sleep_for(std::chrono::milliseconds(200));
   }
+  if (!robot_description.empty())
+  {
+    // Load Mujoco model
+    char error[1000] = "Could not load XML model";
 
-  mju::strcpy_arr(sim.load_error, loadError);
+    mjSpec* spec = nullptr;
+    spec = mj_parseXMLString(robot_description.c_str(), nullptr, error, 1000);
+    mnew = mj_compile(spec, nullptr);
 
+    if (!mnew)
+    {
+      const char* myerr = mjs_getError(spec);
+      mj_deleteSpec(spec);
+      RCLCPP_INFO(node->get_logger(), "Error %s", myerr);
+      RCLCPP_FATAL(node->get_logger(), "Failed to compile MuJoCo model: %s", error);
+    }
+    mj_deleteSpec(spec);
+    RCLCPP_INFO(node->get_logger(), "Model body count: %d", mnew->nbody);
+    RCLCPP_INFO(node->get_logger(), "Model geom count: %d", mnew->ngeom);
+  }
   return mnew;
 }
 
@@ -321,12 +385,14 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
   const auto model_path_maybe = get_parameter("mujoco_model");
   if (!model_path_maybe.has_value())
   {
-    RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Missing 'mujoco_model' in <hardware_parameters>.");
-    return hardware_interface::CallbackReturn::ERROR;
+    RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Parameter 'mujoco_model' not found in URDF.");
+    model_path_.clear();
+  }
+  else
+  {
+    model_path_ = model_path_maybe.value();
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_);
   }
-  model_path_ = model_path_maybe.value();
-
-  RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_);
 
   // Pull the initial speed factor from the hardware parameters, if present
   sim_speed_factor_ = std::stod(get_parameter("sim_speed_factor").value_or("-1"));
@@ -388,7 +454,38 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
 
   // Load the model and data prior to hw registration and starting the physics thread
   sim_->LoadMessage(model_path_.c_str());
-  mj_model_ = LoadModel(model_path_.c_str(), *sim_);
+
+  // Construct and start the ROS node spinning
+  /// The PIDs config file
+  const auto pids_config_file = get_parameter("pids_config_file");
+  rclcpp::NodeOptions node_options;
+  node_options.append_parameter_override("use_sim_time", rclcpp::ParameterValue(true));
+  node_options.allow_undeclared_parameters(true);
+  node_options.automatically_declare_parameters_from_overrides(true);
+  if (pids_config_file.has_value())
+  {
+    // Check if the file exists
+    const std::filesystem::path path_to_file(pids_config_file.value());
+    if (!std::filesystem::exists(path_to_file))
+    {
+      RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "PID config file '%s' does not exist!",
+                   pids_config_file->c_str());
+      return hardware_interface::CallbackReturn::ERROR;
+    }
+    RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"),
+                       "Loading PID config from file: " << pids_config_file.value());
+    auto node_options_arguments = node_options.arguments();
+    node_options_arguments.push_back(RCL_ROS_ARGS_FLAG);
+    node_options_arguments.push_back(RCL_PARAM_FILE_FLAG);
+    node_options_arguments.push_back(pids_config_file.value());
+    node_options.arguments(node_options_arguments);
+  }
+  executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
+  mujoco_node_ = std::make_shared<rclcpp::Node>("mujoco_node", node_options);
+  executor_->add_node(mujoco_node_);
+  executor_thread_ = std::thread([this]() { executor_->spin(); });
+
+  mj_model_ = LoadModel(model_path_.c_str(), *sim_, mujoco_node_);
   if (!mj_model_)
   {
     RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Mujoco failed to load '%s'", model_path_.c_str());
@@ -411,12 +508,6 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
   register_sensors(get_hardware_info());
   set_initial_pose();
 
-  // Construct and start the ROS node spinning
-  executor_ = std::make_unique<rclcpp::executors::MultiThreadedExecutor>();
-  mujoco_node_ = std::make_shared<rclcpp::Node>("mujoco_node");
-  executor_->add_node(mujoco_node_);
-  executor_thread_ = std::thread([this]() { executor_->spin(); });
-
   // Time publisher will be pushed from the physics_thread_
   clock_publisher_ = mujoco_node_->create_publisher<rosgraph_msgs::msg::Clock>("/clock", 1);
 
@@ -611,15 +702,18 @@ std::vector<hardware_interface::CommandInterface> MujocoSystemInterface::export_
       {
         if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos)
         {
-          new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command);
+          if (joint.is_position_control_enabled || joint.is_position_pid_control_enabled)
+            new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command);
         }
         else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos)
         {
-          new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command);
+          if (joint.is_velocity_control_enabled || joint.is_velocity_pid_control_enabled)
+            new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_VELOCITY, &joint.velocity_command);
         }
         else if (command_if.name == hardware_interface::HW_IF_EFFORT)
         {
-          new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command);
+          if (joint.is_effort_control_enabled)
+            new_command_interfaces.emplace_back(joint.name, hardware_interface::HW_IF_EFFORT, &joint.effort_command);
         }
       }
     }
@@ -685,16 +779,32 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
       joint_it->is_position_control_enabled = false;
       joint_it->is_velocity_control_enabled = false;
       joint_it->is_effort_control_enabled = false;
+      joint_it->is_position_pid_control_enabled = false;
+      joint_it->is_velocity_pid_control_enabled = false;
 
       if (interface_type == hardware_interface::HW_IF_POSITION)
       {
-        joint_it->is_position_control_enabled = true;
+        if (joint_it->actuator_type == ActuatorType::POSITION)
+        {
+          joint_it->is_position_control_enabled = true;
+        }
+        else if (joint_it->has_pos_pid)
+        {
+          joint_it->is_position_pid_control_enabled = true;
+        }
         RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"),
                     "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str());
       }
       else if (interface_type == hardware_interface::HW_IF_VELOCITY)
       {
-        joint_it->is_velocity_control_enabled = true;
+        if (joint_it->actuator_type == ActuatorType::VELOCITY)
+        {
+          joint_it->is_velocity_control_enabled = true;
+        }
+        else if (joint_it->has_vel_pid)
+        {
+          joint_it->is_velocity_pid_control_enabled = true;
+        }
         RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"),
                     "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str());
       }
@@ -710,10 +820,12 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
       if (interface_type == hardware_interface::HW_IF_POSITION)
       {
         joint_it->is_position_control_enabled = false;
+        joint_it->is_position_pid_control_enabled = false;
       }
       else if (interface_type == hardware_interface::HW_IF_VELOCITY)
       {
         joint_it->is_velocity_control_enabled = false;
+        joint_it->is_velocity_pid_control_enabled = false;
       }
       else if (interface_type == hardware_interface::HW_IF_EFFORT)
       {
@@ -783,7 +895,7 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time&
 }
 
 hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time& /*time*/,
-                                                             const rclcpp::Duration& /*period*/)
+                                                             const rclcpp::Duration& period)
 {
   // Update mimic joints
   for (auto& joint_state : joint_states_)
@@ -813,10 +925,20 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time&
     {
       mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.position_command;
     }
+    else if (joint_state.is_position_pid_control_enabled)
+    {
+      double error = joint_state.position_command - mj_data_->qpos[joint_state.mj_pos_adr];
+      mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = joint_state.pos_pid->compute_command(error, period);
+    }
     else if (joint_state.is_velocity_control_enabled)
     {
       mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.velocity_command;
     }
+    else if (joint_state.is_velocity_pid_control_enabled)
+    {
+      double error = joint_state.velocity_command - mj_data_->qvel[joint_state.mj_vel_adr];
+      mj_data_control_->qfrc_applied[joint_state.mj_vel_adr] = joint_state.vel_pid->compute_command(error, period);
+    }
     else if (joint_state.is_effort_control_enabled)
     {
       mj_data_control_->ctrl[joint_state.mj_actuator_id] = joint_state.effort_command;
@@ -857,12 +979,6 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn
     mujoco_actuator_id = mujoco_actuator_id == -1 ? mj_name2id(mj_model_, mjtObj::mjOBJ_ACTUATOR, joint.name.c_str()) :
                                                     mujoco_actuator_id;
 
-    if (mujoco_actuator_id == -1)
-    {
-      // This isn't a failure the joint just won't be controllable
-      RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "No actuator found for joint: " << joint.name);
-    }
-
     // Add to the joint hw information map
     joint_hw_info_.insert(std::make_pair(joint.name, joint));
 
@@ -932,27 +1048,158 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn
       }
     }
 
+    if (mujoco_actuator_id == -1)
+    {
+      // This isn't a failure the joint just won't be controllable
+      RCLCPP_WARN_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "No actuator found for joint: " << joint.name);
+      continue;
+    }
+
+    int biastype = mj_model_->actuator_biastype[mujoco_actuator_id];
+    const int NBias = 10;
+    const mjtNum* biasprm = mj_model_->actuator_biasprm + mujoco_actuator_id * NBias;
+
+    if (biastype == mjBIAS_NONE)
+    {
+      last_joint_state.actuator_type = ActuatorType::MOTOR;
+    }
+    else if (biastype == mjBIAS_AFFINE && biasprm[1] != 0)
+    {
+      last_joint_state.actuator_type = ActuatorType::POSITION;
+    }
+    else if (biastype == mjBIAS_AFFINE && biasprm[1] == 0 && biasprm[2] != 0)
+    {
+      last_joint_state.actuator_type = ActuatorType::VELOCITY;
+    }
+    else
+    {
+      last_joint_state.actuator_type = ActuatorType::CUSTOM;
+      RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"),
+                  "Custom MuJoCo actuator for the joint : %s , using all command interfaces", joint.name.c_str());
+    }
+
     // command interfaces
     // overwrite joint limit with min/max value
+    bool has_command_interfaces = false;
     for (const auto& command_if : joint.command_interfaces)
     {
+      has_command_interfaces = true;
       // If available, always default to position control at the start
       if (command_if.name.find(hardware_interface::HW_IF_POSITION) != std::string::npos)
       {
-        last_joint_state.is_position_control_enabled = true;
-        last_joint_state.position_command = last_joint_state.position;
+        if (last_joint_state.actuator_type == ActuatorType::VELOCITY ||
+            last_joint_state.actuator_type == ActuatorType::MOTOR ||
+            last_joint_state.actuator_type == ActuatorType::CUSTOM)
+        {
+          last_joint_state.pos_pid =
+              std::make_shared<control_toolbox::PidROS>(mujoco_node_, "pid_gains.position." + joint.name, "", false);
+          last_joint_state.pos_pid->initialize_from_ros_parameters();
+          const auto gains = last_joint_state.pos_pid->get_gains();
+          last_joint_state.has_pos_pid =
+              std::isfinite(gains.p_gain_) && std::isfinite(gains.i_gain_) && std::isfinite(gains.d_gain_);
+          if (last_joint_state.has_pos_pid)
+          {
+            last_joint_state.is_position_pid_control_enabled = true;
+            last_joint_state.position_command = last_joint_state.position;
+            const auto gains = last_joint_state.pos_pid->get_gains();
+
+            RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"),
+                        "Position control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, "
+                        "Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s",
+                        joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max,
+                        gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_,
+                        gains.antiwindup_strat_.to_string().c_str());
+          }
+          else
+          {
+            RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"),
+                         "Position command interface for the joint : %s is not supported with velocity or motor "
+                         "actuator without defining the PIDs",
+                         joint.name.c_str());
+            continue;
+          }
+        }
+        else if (last_joint_state.actuator_type == ActuatorType::POSITION)
+        {
+          last_joint_state.is_position_control_enabled = true;
+          last_joint_state.position_command = last_joint_state.position;
+        }
       }
       else if (command_if.name.find(hardware_interface::HW_IF_VELOCITY) != std::string::npos)
       {
-        last_joint_state.is_velocity_control_enabled = true;
-        last_joint_state.velocity_command = last_joint_state.velocity;
+        if (last_joint_state.actuator_type == ActuatorType::POSITION)
+        {
+          RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"),
+                       "Velocity command interface for the joint : %s is not supported with position actuator",
+                       joint.name.c_str());
+          continue;
+        }
+        if (last_joint_state.actuator_type == ActuatorType::VELOCITY)
+        {
+          last_joint_state.is_velocity_control_enabled = true;
+          last_joint_state.velocity_command = last_joint_state.velocity;
+        }
+        else if (last_joint_state.actuator_type == ActuatorType::MOTOR ||
+                 last_joint_state.actuator_type == ActuatorType::CUSTOM)
+        {
+          last_joint_state.vel_pid =
+              std::make_shared<control_toolbox::PidROS>(mujoco_node_, "pid_gains.velocity." + joint.name, "", false);
+          last_joint_state.vel_pid->initialize_from_ros_parameters();
+          const auto gains = last_joint_state.vel_pid->get_gains();
+          last_joint_state.has_vel_pid =
+              std::isfinite(gains.p_gain_) && std::isfinite(gains.i_gain_) && std::isfinite(gains.d_gain_);
+
+          if (last_joint_state.has_vel_pid)
+          {
+            last_joint_state.is_velocity_pid_control_enabled = true;
+            const auto gains = last_joint_state.vel_pid->get_gains();
+            last_joint_state.velocity_command = last_joint_state.velocity;
+            RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"),
+                        "Velocity control PID gains for joint %s : P=%.4f, I=%.4f, D=%.4f, Imax=%.4f, Imin=%.4f, "
+                        "Umin=%.4f, Umax=%.4f, antiwindup_strategy=%s",
+                        joint.name.c_str(), gains.p_gain_, gains.i_gain_, gains.d_gain_, gains.antiwindup_strat_.i_max,
+                        gains.antiwindup_strat_.i_min, gains.u_min_, gains.u_max_,
+                        gains.antiwindup_strat_.to_string().c_str());
+          }
+          else
+          {
+            RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"),
+                         "Velocity command interface for the joint : %s is not supported with motor or custom actuator "
+                         "without defining the PIDs",
+                         joint.name.c_str());
+            continue;
+          }
+        }
       }
       else if (command_if.name.find(hardware_interface::HW_IF_EFFORT) != std::string::npos)
       {
-        last_joint_state.is_effort_control_enabled = true;
-        last_joint_state.effort_command = last_joint_state.effort;
+        if (last_joint_state.actuator_type == ActuatorType::POSITION ||
+            last_joint_state.actuator_type == ActuatorType::VELOCITY)
+        {
+          RCLCPP_ERROR(rclcpp::get_logger("MujocoSystemInterface"),
+                       "Effort command interface for the joint : %s is not supported with position or velocity "
+                       "actuator. Skipping it.",
+                       joint.name.c_str());
+          continue;
+        }
+        else
+        {
+          last_joint_state.is_effort_control_enabled = true;
+          last_joint_state.effort_command = last_joint_state.effort;
+        }
       }
     }
+    if (!has_command_interfaces)
+    {
+      RCLCPP_INFO(rclcpp::get_logger("MujocoSystemInterface"), "Joint : %s is a passive joint", joint.name.c_str());
+    }
+    else if (!last_joint_state.is_position_control_enabled && !last_joint_state.is_velocity_control_enabled &&
+             !last_joint_state.is_effort_control_enabled && !last_joint_state.is_position_pid_control_enabled &&
+             !last_joint_state.is_velocity_pid_control_enabled)
+    {
+      throw std::runtime_error(std::string("Joint '") + joint.name +
+                               "' has an unsupported command interface for the specified MuJoCo actuator");
+    }
   }
 }
 
@@ -1124,6 +1371,7 @@ void MujocoSystemInterface::PhysicsLoop()
 
             // Copy data to the control
             mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, mj_model_->nu);
+            mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, mj_model_->nu);
             // run single step, let next iteration deal with timing
             mj_step(mj_model_, mj_data_);
 
@@ -1162,6 +1410,7 @@ void MujocoSystemInterface::PhysicsLoop()
 
               // Copy data to the control
               mju_copy(mj_data_->ctrl, mj_data_control_->ctrl, mj_model_->nu);
+              mju_copy(mj_data_->qfrc_applied, mj_data_control_->qfrc_applied, mj_model_->nu);
               // call mj_step
               mj_step(mj_model_, mj_data_);
 
diff --git a/test/config/mujoco_pid.yaml b/test/config/mujoco_pid.yaml
new file mode 100644
index 0000000..d89a8c4
--- /dev/null
+++ b/test/config/mujoco_pid.yaml
@@ -0,0 +1,30 @@
+/**:
+  ros__parameters:
+    pid_gains:
+      position:
+        joint1:
+          p: 1.0
+          i: 0.5
+          d: 0.05
+          u_clamp_max: 100.0
+          u_clamp_min: -100.0
+        joint2:
+          p: 1.0
+          i: 0.5
+          d: 0.05
+          u_clamp_max: 100.0
+          u_clamp_min: -100.0
+    pid_gains:
+      velocity:
+        joint1:
+          p: 1.0
+          i: 0.5
+          d: 0.05
+          u_clamp_max: 100.0
+          u_clamp_min: -100.0
+        joint2:
+          p: 1.0
+          i: 0.5
+          d: 0.05
+          u_clamp_max: 100.0
+          u_clamp_min: -100.0
diff --git a/test/launch/test_robot_pid.launch.py b/test/launch/test_robot_pid.launch.py
new file mode 100644
index 0000000..492b590
--- /dev/null
+++ b/test/launch/test_robot_pid.launch.py
@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+#
+# Copyright (c) 2025, PAL Robotics S.L.
+#
+# All rights reserved.
+#
+# This software is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+
+from launch import LaunchDescription
+from launch.substitutions import (
+    Command,
+    FindExecutable,
+    PathJoinSubstitution,
+)
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue, ParameterFile
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("mujoco_ros2_simulation"),
+                    "test_resources",
+                    "test_pid",
+                    "test_robot_pid.urdf",
+                ]
+            ),
+        ]
+    )
+
+    robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)}
+
+    controller_parameters_pid = ParameterFile(
+        PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers.yaml"]),
+    )
+
+    robot_state_publisher_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[
+            robot_description,
+            {"use_sim_time": True},
+        ],
+    )
+
+    control_node = Node(
+        package="mujoco_ros2_simulation",
+        executable="ros2_control_node",
+        output="both",
+        parameters=[
+            {"use_sim_time": True},
+            controller_parameters_pid,
+        ],
+        emulate_tty=True,
+    )
+
+    spawn_joint_state_broadcaster = Node(
+        package="controller_manager",
+        executable="spawner",
+        name="spawn_joint_state_broadcaster",
+        arguments=[
+            "joint_state_broadcaster",
+        ],
+        output="both",
+    )
+
+    spawn_position_controller = Node(
+        package="controller_manager",
+        executable="spawner",
+        name="spawn_position_controller",
+        arguments=[
+            "position_controller",
+        ],
+        output="both",
+    )
+
+    return LaunchDescription(
+        [
+            robot_state_publisher_node,
+            control_node,
+            spawn_joint_state_broadcaster,
+            spawn_position_controller,
+        ]
+    )
diff --git a/test/test_resources/test_pid/scene_pid.xml b/test/test_resources/test_pid/scene_pid.xml
new file mode 100644
index 0000000..2f62145
--- /dev/null
+++ b/test/test_resources/test_pid/scene_pid.xml
@@ -0,0 +1,22 @@
+<mujoco model="scene">
+  <include file="test_robot_pid.xml"/>
+
+  <statistic center="0.4 0 0.4" extent="1"/>
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0"/>
+    <rgba haze="0.15 0.25 0.35 1"/>
+    <global azimuth="120" elevation="-20"/>
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072"/>
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3"
+      markrgb="0.8 0.8 0.8" width="300" height="300"/>
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5"/>
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true"/>
+  </worldbody>
+</mujoco>
diff --git a/test/test_resources/test_pid/test_robot_pid.urdf b/test/test_resources/test_pid/test_robot_pid.urdf
new file mode 100644
index 0000000..be78798
--- /dev/null
+++ b/test/test_resources/test_pid/test_robot_pid.urdf
@@ -0,0 +1,192 @@
+<robot name="two_dof_robot">
+
+  <link name="world"/>
+
+  <link name="base"/>
+  <joint name="base_link" type="fixed">
+      <axis rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="world"/>
+      <child link="base"/>
+  </joint>
+
+  <link name="upperarm">
+      <inertial>
+          <origin xyz="0.5 0 0" rpy="0 0 0"/>
+          <mass value="27.0"/>
+          <inertia
+              ixx="0.0225" ixy="0.0"  ixz="0.0"
+              iyy="0.225"  iyz="0.0"  izz="0.225"/>
+      </inertial>
+      <visual>
+          <geometry>
+              <box size="1.0 0.1 0.1"/>
+          </geometry>
+          <origin rpy="0 0 0" xyz="0.5 0 0"/>
+          <material name="red">
+              <color rgba="1.0 0 0 1.0"/>
+          </material>
+      </visual>
+      <collision>
+          <geometry>
+              <box size="1.0 0.1 0.1"/>
+          </geometry>
+          <origin rpy="0 0 0" xyz="0.5 0 0"/>
+      </collision>
+  </link>
+  <joint name="joint1" type="revolute">
+      <axis rpy="0 0 0" xyz="0 0 1"/>
+      <parent link="base"/>
+      <child link="upperarm"/>
+      <limit lower="-3.14" upper="3.14" velocity="3.14" effort="1000" />
+  </joint>
+
+  <link name="forearm">
+      <inertial>
+          <origin xyz="0.5 0 0" rpy="0 0 0"/>
+          <mass value="27.0"/>
+          <inertia
+              ixx="0.0225" ixy="0.0"  ixz="0.0"
+              iyy="0.225"  iyz="0.0"  izz="0.225"/>
+      </inertial>
+      <visual>
+          <geometry>
+              <box size="1.0 0.1 0.1"/>
+          </geometry>
+          <origin rpy="0 0 0" xyz="0.5 0 0"/>
+          <material name="blue">
+              <color rgba="0 0 1.0 1.0"/>
+          </material>
+      </visual>
+      <collision>
+          <geometry>
+              <box size="1.0 0.1 0.1"/>
+          </geometry>
+          <origin rpy="0 0 0" xyz="0.5 0 0"/>
+      </collision>
+  </link>
+  <joint name="joint2" type="revolute">
+      <axis rpy="0 0 0" xyz="0 0 1"/>
+      <parent link="upperarm"/>
+      <child link="forearm"/>
+      <origin rpy="0 0 0" xyz="1 0 0"/>
+      <limit lower="-3.14" upper="3.14" velocity="3.14" effort="1000" />
+  </joint>
+
+  <link name="ee_link" />
+  <joint name="ee_joint" type="fixed">
+      <parent link="forearm"/>
+      <child link="ee_link"/>
+      <origin rpy="0 0 0" xyz="1 0 0"/>
+  </joint>
+
+  <link name="camera_link">
+    <visual>
+      <geometry>
+        <box size="0.1 0.1 0.1" />
+      </geometry>
+      <material name="dark_grey">
+        <color rgba="0.2 0.2 0.2 1.0"/>
+      </material>
+    </visual>
+  </link>
+  <joint name="camera_joint" type="fixed">
+    <parent link="ee_link"/>
+    <child link="camera_link"/>
+    <origin xyz="0.05 0 0.1" rpy="0 0 0"/>
+  </joint>
+
+  <link name="camera_color_frame"/>
+  <joint name="camera_color_joint" type="fixed">
+    <parent link="camera_link"/>
+    <child link="camera_color_frame"/>
+    <origin xyz="-0.05 0 0" rpy="0 0 0"/>
+  </joint>
+
+  <!-- The camera looks backwards along the axis of the forearm, following the standard ROS image conventions -->
+  <!-- https://github.com/ros2/common_interfaces/blob/rolling/sensor_msgs/msg/Image.msg#L7 -->
+  <link name="camera_color_optical_frame"/>
+  <joint name="camera_color_optical_joint" type="fixed">
+    <parent link="camera_color_frame"/>
+    <child link="camera_color_optical_frame"/>
+    <origin xyz="0 0 0" rpy="-1.5708 0 1.5708"/>
+  </joint>
+
+  <!-- For the sake of visibility, this is where the mujoco camera should end up -->
+  <link name="camera_color_mujoco_frame"/>
+  <joint name="camera_color_mujoco_joint" type="fixed">
+    <parent link="camera_color_optical_frame"/>
+    <child link="camera_color_mujoco_frame"/>
+    <origin xyz="0 0 0" rpy="3.14159 0 0"/>
+  </joint>
+
+  <!-- In the URDF, the lidar sensor's Z-axis should point "up" so that the sensor data sweeps -->
+  <!-- counter clockwise. We expect the X-axis to be in line with the first rangefinder's Z-axis. -->
+  <link name="lidar_sensor_frame" />
+  <joint name="lidar_sensor_link" type="fixed">
+      <parent link="world"/>
+      <child link="lidar_sensor_frame"/>
+      <origin rpy="0 0 -1.5708" xyz="1.0 2.0 0"/>
+  </joint>
+
+  <ros2_control name="MujocoSystem" type="system">
+    <hardware>
+      <plugin>mujoco_ros2_simulation/MujocoSystemInterface</plugin>
+      <param name="mujoco_model">$(find mujoco_ros2_simulation)/test_resources/test_pid/scene_pid.xml</param>
+
+      <param name="pids_config_file">$(find mujoco_ros2_simulation)/config/mujoco_pid.yaml</param>
+
+      <!-- Override the slowdown settings in the Simulate App and run at 3x speed -->
+      <param name="sim_speed_factor">3.0</param>
+
+      <!-- Publish camera images at 6 hz -->
+      <param name="camera_publish_rate">6.0</param>
+
+      <!-- Publish lidar scans at 10 hz -->
+      <param name="lidar_publish_rate">10.0</param>
+    </hardware>
+    <joint name="joint1">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+    <joint name="joint2">
+      <command_interface name="position"/>
+      <command_interface name="velocity"/>
+
+      <state_interface name="position">
+        <param name="initial_value">0.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+
+    <!-- For cameras, the sensor name _must_ match the camera name in the MJCF -->
+    <sensor name="camera">
+      <!-- Parameters will be used as is -->
+      <param name="frame_name">camera_color_mujoco_frame</param>
+      <param name="info_topic">/camera/color/camera_info</param>
+      <param name="image_topic">/camera/color/image_raw</param>
+      <param name="depth_topic">/camera/aligned_depth_to_color/image_raw</param>
+    </sensor>
+
+    <!-- Lidar sensors are matched to a set of rangefinder sensors in the MJCF, which should be -->
+    <!-- generated with the "<lidar>" inputs in the conversion script. The consiste of N rangefinders -->
+    <!-- and will generally be of the form "<sensor_name>-01". These parameters should line up with -->
+    <!-- those documented in the laserscan message. -->
+    <sensor name="lidar">
+      <param name="frame_name">lidar_sensor_frame</param>
+      <param name="angle_increment">0.025</param>
+      <param name="min_angle">-0.3</param>
+      <param name="max_angle">0.3</param>
+      <param name="range_min">0.05</param>
+      <param name="range_max">10</param>
+      <param name="laserscan_topic">/scan</param>
+    </sensor>
+  </ros2_control>
+
+</robot>
diff --git a/test/test_resources/test_pid/test_robot_pid.xml b/test/test_resources/test_pid/test_robot_pid.xml
new file mode 100644
index 0000000..52c9063
--- /dev/null
+++ b/test/test_resources/test_pid/test_robot_pid.xml
@@ -0,0 +1,51 @@
+<mujoco model="two_dof_robot">
+  <compiler angle="radian" meshdir="assets/"/>
+  <worldbody>
+    <body name="upperarm">
+      <inertial pos="0.5 0 0" quat="0.5 0.5 0.5 0.5" mass="27" diaginertia="0.225 0.225 0.0225"/>
+      <joint name="joint1" pos="0 0 0" axis="0 0 1" range="-3.14 3.14" actuatorfrcrange="-1000 1000" frictionloss="1.0" damping="2.0"/>
+      <body name="forearm" pos="1 0 0" gravcomp="1.0">
+        <inertial pos="0.5 0 0" quat="0.5 0.5 0.5 0.5" mass="27" diaginertia="0.225 0.225 0.0225"/>
+        <joint name="joint2" pos="0 0 0" axis="0 0 1" range="-3.14 3.14" actuatorfrcrange="-1000 1000"/>
+        <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" class="collision"/>
+        <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" rgba="0 0 1 1" class="visual"/>
+        <geom size="0.05 0.05 0.05" pos="1.05 0 0.1" type="box" class="collision"/>
+        <geom size="0.05 0.05 0.05" pos="1.05 0 0.1" type="box" rgba="0.2 0.2 0.2 1" class="visual"/>
+        <site name="forearm" pos="0 0 0" quat="1.0 0 0 0"/>
+        <site name="ee_link" pos="1.0 0 0" quat="1.0 0 0 0"/>
+        <site name="camera_link" pos="1.05 0 0.1" quat="1.0 0 0 0"/>
+        <site name="camera_color_frame" pos="1.0 0 0.1" quat="1.0 0 0 0"/>
+        <site name="camera_color_optical_frame" pos="1.0 0 0.1" quat="-0.49999816339744824 0.4999999999966268 0.5000018366025517 -0.4999999999966268"/>
+        <site name="camera_color_mujoco_frame" pos="1.0 0 0.1" quat="0.5000006633911983 0.4999974999995599 0.49999933659630164 0.5000024999995599"/>
+        <camera name="camera" fovy="58" mode="fixed" resolution="640 480" pos="1.0 0 0.1" quat="-0.4999999999966268 -0.49999816339744824 -0.4999999999966268 -0.5000018366025517"/>
+      </body>
+      <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" class="collision"/>
+      <geom size="0.5 0.05 0.05" pos="0.5 0 0" type="box" rgba="1 0 0 1" class="visual"/>
+      <site name="upperarm" pos="0 0 0" quat="1.0 0 0 0"/>
+    </body>
+    <site name="world" pos="0 0 0" quat="1.0 0 0 0"/>
+    <site name="base" pos="0 0 0" quat="1.0 0 0 0"/>
+    <site name="lidar_sensor_frame" pos="1.0 2.0 0" quat="0.7071054825112363 0 0 -0.7071080798594737"/>
+    <body name="lidar_sensor_frame_lidar_body" pos="1.0 2.0 0" quat="0.6991665401770936 0.6991665401770936 -0.10567000093116563 -0.10567000093116563">
+      <replicate site="lidar_sensor_frame" count="24" sep="-" offset="0 0 0" euler="0 0.025 0">
+        <site name="rf" pos="0.0 0.0 0.0" quat="0.0 0.0 0.0 1.0"/>
+      </replicate>
+    </body>
+  </worldbody>
+  <option integrator="implicitfast"/>
+  <default>
+    <default class="visual">
+      <geom group="2" type="mesh" contype="0" conaffinity="0"/>
+    </default>
+    <default class="collision">
+      <geom group="3" type="mesh"/>
+    </default>
+  </default>
+  <actuator>
+    <motor name="joint1_motor" joint="joint1" gear="1"/>
+    <motor name="joint2_motor" joint="joint2" gear="1"/>
+  </actuator>
+  <sensor>
+    <rangefinder name="lidar" site="rf"/>
+  </sensor>
+</mujoco>