diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt
index bb956517..6fcf8071 100644
--- a/mujoco_ros2_control/CMakeLists.txt
+++ b/mujoco_ros2_control/CMakeLists.txt
@@ -16,7 +16,9 @@ find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(Threads REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(transmission_interface REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(backward_ros REQUIRED)
 
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
@@ -166,6 +168,7 @@ set_target_properties(mujoco_ros2_control PROPERTIES
 target_link_libraries(mujoco_ros2_control
   ${MUJOCO_LIB}
   ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
   mujoco::libsimulate
   controller_manager::controller_manager
   hardware_interface::hardware_interface
@@ -176,6 +179,7 @@ target_link_libraries(mujoco_ros2_control
   control_toolbox::control_toolbox
   ${nav_msgs_TARGETS}
   ${sensor_msgs_TARGETS}
+  tf2_ros::tf2_ros
   transmission_interface::transmission_interface
   Eigen3::Eigen
   Threads::Threads
diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp
index b17bb4e8..76217fe7 100644
--- a/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp
+++ b/mujoco_ros2_control/include/mujoco_ros2_control/data.hpp
@@ -89,12 +89,18 @@ struct InterfaceData
  * @param has_pos_pid Boolean flag indicating if a position PID controller is configured.
  * @param has_vel_pid Boolean flag indicating if a velocity PID controller is configured.
  */
+// Custom interface names for kp/kd gains (matching ros2_control pattern)
+constexpr char HW_IF_KP[] = "kp";
+constexpr char HW_IF_KD[] = "kd";
+
 struct MuJoCoActuatorData
 {
   std::string joint_name = "";
   InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
   InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
   InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
+  InterfaceData kp_interface{ HW_IF_KP };
+  InterfaceData kd_interface{ HW_IF_KD };
   std::shared_ptr<control_toolbox::PidROS> pos_pid{ nullptr };
   std::shared_ptr<control_toolbox::PidROS> vel_pid{ nullptr };
   ActuatorType actuator_type{ ActuatorType::UNKNOWN };
@@ -110,6 +116,7 @@ struct MuJoCoActuatorData
   bool is_velocity_pid_control_enabled{ false };
   bool is_velocity_control_enabled{ false };
   bool is_effort_control_enabled{ false };
+  bool is_impedance_control_enabled{ false };  // Uses commanded kp/kd for PD control
   bool has_pos_pid{ false };
   bool has_vel_pid{ false };
 
@@ -118,6 +125,7 @@ struct MuJoCoActuatorData
     position_interface.transmission_passthrough_ = position_interface.state_;
     velocity_interface.transmission_passthrough_ = velocity_interface.state_;
     effort_interface.transmission_passthrough_ = effort_interface.state_;
+    // kp/kd don't have state, only command
   }
 
   void copy_command_from_transmission()
@@ -125,6 +133,7 @@ struct MuJoCoActuatorData
     position_interface.command_ = position_interface.transmission_passthrough_;
     velocity_interface.command_ = velocity_interface.transmission_passthrough_;
     effort_interface.command_ = effort_interface.transmission_passthrough_;
+    // kp/kd commands are set directly, not through transmissions
   }
 
   void copy_command_to_state()
@@ -155,6 +164,8 @@ struct URDFJointData
   InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
   InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
   InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
+  InterfaceData kp_interface{ HW_IF_KP };
+  InterfaceData kd_interface{ HW_IF_KD };
 
   std::vector<std::string> command_interfaces = {};
 
@@ -165,6 +176,7 @@ struct URDFJointData
   bool is_position_control_enabled{ false };
   bool is_velocity_control_enabled{ false };
   bool is_effort_control_enabled{ false };
+  bool is_impedance_control_enabled{ false };  // Uses commanded kp/kd for PD control
 
   void copy_state_from_transmission()
   {
diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp
index 140571d0..b9cc5e09 100644
--- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp
+++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp
@@ -116,12 +116,7 @@ class MujocoLidar
   // LaserScan publishing rate in Hz
   double lidar_publish_rate_;
 
-  // Rendering options for the cameras, currently hard coded to defaults
-  mjvOption mjv_opt_;
-  mjvScene mjv_scn_;
-  mjrContext mjr_con_;
-
-  // Containers for ladar data and ROS constructs
+  // Containers for lidar data and ROS constructs
   std::vector<LidarData> lidar_sensors_;
 
   // Lidar processing thread
diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp
index b850a318..cd6f8503 100644
--- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp
+++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp
@@ -30,14 +30,17 @@
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/macros.hpp>
+#include <tf2_ros/transform_broadcaster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 #include <realtime_tools/realtime_publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include <mujoco/mujoco.h>
 
@@ -243,6 +246,13 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
    */
   void set_initial_pose();
 
+  /**
+   * @brief Reset the simulation to initial state.
+   *
+   * Resets velocities, accelerations, and time to zero, then restores initial positions.
+   */
+  void reset_simulation();
+
   /**
    * @brief Spins the physics simulation for the Simulate Application
    */
@@ -295,6 +305,14 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_publisher_;
   realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_realtime_publisher_;
 
+  // Accumulated time offset to keep published clock monotonic across sim resets.
+  // mj_resetData sets mj_data_->time to 0; we add this offset so /clock never
+  // goes backward.
+  double clock_offset_{0.0};
+
+  // Reset service to reset simulation to initial state
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
+
   // Actuators state publisher
   std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> actuator_state_publisher_ = nullptr;
   realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>::SharedPtr actuator_state_realtime_publisher_ =
@@ -306,6 +324,11 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>::SharedPtr floating_base_realtime_publisher_ = nullptr;
   nav_msgs::msg::Odometry floating_base_msg_;
 
+  // Floating base TF broadcaster
+  std::unique_ptr<tf2_ros::TransformBroadcaster> floating_base_tf_broadcaster_ = nullptr;
+  geometry_msgs::msg::TransformStamped floating_base_tf_msg_;
+  bool publish_floating_base_tf_ = true;
+
   // Free joint data
   int free_joint_id_ = -1;
   int free_joint_qpos_adr_ = -1;
diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp
index c88f9df4..6670414e 100644
--- a/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp
+++ b/mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp
@@ -30,10 +30,9 @@ namespace mujoco_ros2_control
 inline std::optional<hardware_interface::ComponentInfo>
 get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, const std::string& name)
 {
-  for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++)
+  for (const auto& sensor : hardware_info.sensors)
   {
-    const auto& sensor = hardware_info.sensors.at(sensor_index);
-    if (hardware_info.sensors.at(sensor_index).name == name)
+    if (sensor.name == name)
     {
       return sensor;
     }
diff --git a/mujoco_ros2_control/package.xml b/mujoco_ros2_control/package.xml
index 99af6e32..37749019 100644
--- a/mujoco_ros2_control/package.xml
+++ b/mujoco_ros2_control/package.xml
@@ -16,12 +16,14 @@
   <depend>controller_manager</depend>
   <depend>hardware_interface</depend>
   <depend>nav_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>libglfw3-dev</depend>
   <depend>pluginlib</depend>
   <depend>python3-pykdl</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>transmission_interface</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
diff --git a/mujoco_ros2_control/src/mujoco_system_interface.cpp b/mujoco_ros2_control/src/mujoco_system_interface.cpp
index 5504f67c..1d44477b 100644
--- a/mujoco_ros2_control/src/mujoco_system_interface.cpp
+++ b/mujoco_ros2_control/src/mujoco_system_interface.cpp
@@ -22,6 +22,7 @@
 
 #include <cerrno>
 #include <chrono>
+#include <cmath>
 #include <cstdint>
 #include <cstdio>
 #include <cstdlib>
@@ -30,13 +31,16 @@
 #include <future>
 #include <iostream>
 #include <limits>
+#include <map>
 #include <memory>
 #include <mutex>
 #include <new>
 #include <regex>
+#include <set>
 #include <stdexcept>
 #include <string>
 #include <thread>
+#include <unistd.h>
 
 #include <tinyxml2.h>
 #include <std_msgs/msg/string.hpp>
@@ -69,7 +73,8 @@ namespace
 std::optional<std::string> get_hardware_parameter(const hardware_interface::HardwareInfo& hardware_info,
                                                   const std::string& key)
 {
-  if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end())
+  auto it = hardware_info.hardware_parameters.find(key);
+  if (it != hardware_info.hardware_parameters.end())
   {
     return it->second;
   }
@@ -79,11 +84,7 @@ std::optional<std::string> get_hardware_parameter(const hardware_interface::Hard
 std::string get_hardware_parameter_or(const hardware_interface::HardwareInfo& hardware_info, const std::string& key,
                                       const std::string& default_value)
 {
-  if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end())
-  {
-    return it->second;
-  }
-  return default_value;
+  return get_hardware_parameter(hardware_info, key).value_or(default_value);
 }
 }  // namespace
 namespace mujoco_ros2_control
@@ -211,17 +212,10 @@ class HeadlessAdapter : public mj::PlatformUIAdapter
   }
 };
 
-// Clamps v to the lo or high value
-double clamp(double v, double lo, double hi)
-{
-  return (v < lo) ? lo : (hi < v) ? hi : v;
-}
-
 // return the path to the directory containing the current executable
 // used to determine the location of auto-loaded plugin libraries
 std::string getExecutableDir()
 {
-  constexpr char kPathSep = '/';
   const char* path = "/proc/self/exe";
 
   std::string real_path = [&]() -> std::string {
@@ -268,24 +262,15 @@ std::string getExecutableDir()
     return "";
   }
 
-  for (std::size_t i = real_path.size() - 1; i > 0; --i)
-  {
-    if (real_path.c_str()[i] == kPathSep)
-    {
-      return real_path.substr(0, i);
-    }
-  }
-
-  // don't scan through the entire file system's root
-  return "";
+  return std::filesystem::path(real_path).parent_path().string();
 }
 
 // scan for libraries in the plugin directory to load additional plugins
 void scanPluginLibraries()
 {
   // check and print plugins that are linked directly into the executable
-  int nplugin = mjp_pluginCount();
-  if (nplugin)
+  const int nplugin = mjp_pluginCount();
+  if (nplugin > 0)
   {
     std::printf("Built-in plugins:\n");
     for (int i = 0; i < nplugin; ++i)
@@ -294,8 +279,6 @@ void scanPluginLibraries()
     }
   }
 
-  const std::string sep = "/";
-
   // try to open the ${EXECDIR}/MUJOCO_PLUGIN_DIR directory
   // ${EXECDIR} is the directory containing the simulate binary itself
   // MUJOCO_PLUGIN_DIR is the MUJOCO_PLUGIN_DIR preprocessor macro
@@ -305,7 +288,7 @@ void scanPluginLibraries()
     return;
   }
 
-  const std::string plugin_dir = getExecutableDir() + sep + MUJOCO_PLUGIN_DIR;
+  const std::string plugin_dir = executable_dir + "/" + MUJOCO_PLUGIN_DIR;
   mj_loadAllPluginLibraries(
       plugin_dir.c_str(), +[](const char* filename, int first, int count) {
         std::printf("Plugins registered by library '%s':\n", filename);
@@ -865,6 +848,17 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
   clock_realtime_publisher_ =
       std::make_shared<realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>>(clock_publisher_);
 
+  // Create reset service
+  reset_service_ = mujoco_node_->create_service<std_srvs::srv::Trigger>(
+      "/mujoco/reset",
+      [this](const std_srvs::srv::Trigger::Request::SharedPtr /*request*/,
+             std_srvs::srv::Trigger::Response::SharedPtr response) {
+        reset_simulation();
+        response->success = true;
+        response->message = "Simulation reset to initial state";
+        RCLCPP_INFO(get_logger(), "Simulation reset to initial state");
+      });
+
   actuator_state_publisher_ =
       mujoco_node_->create_publisher<sensor_msgs::msg::JointState>("/mujoco_actuators_states", 100);
   actuator_state_realtime_publisher_ =
@@ -928,7 +922,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
     floating_base_realtime_publisher_ =
         std::make_shared<realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>>(floating_base_publisher_);
 
-    floating_base_msg_.header.frame_id = "odom";  // TODO: Make configurable
+    // Get frame names from parameters
+    std::string odom_frame = get_hardware_parameter_or(get_hardware_info(), "odom_frame", "odom");
+    floating_base_msg_.header.frame_id = odom_frame;
     // Set child frame as the root link of the robot as the body attached to the free joint
     floating_base_msg_.child_frame_id =
         std::string(mj_id2name(mj_model_, mjtObj::mjOBJ_BODY, mj_model_->jnt_bodyid[free_joint_id_]));
@@ -938,6 +934,18 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
         "Publishing floating base odometry using the free joint : '%s' attached to the body '%s' on topic: '%s'",
         mj_id2name(mj_model_, mjtObj::mjOBJ_JOINT, free_joint_id_), floating_base_msg_.child_frame_id.c_str(),
         odom_topic_name.c_str());
+
+    // TF broadcaster for floating base
+    publish_floating_base_tf_ =
+        get_hardware_parameter_or(get_hardware_info(), "publish_floating_base_tf", "true") == "true";
+    if (publish_floating_base_tf_)
+    {
+      floating_base_tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(mujoco_node_);
+      floating_base_tf_msg_.header.frame_id = odom_frame;
+      floating_base_tf_msg_.child_frame_id = floating_base_msg_.child_frame_id;
+      RCLCPP_INFO(get_logger(), "Publishing floating base TF: %s -> %s", odom_frame.c_str(),
+                  floating_base_tf_msg_.child_frame_id.c_str());
+    }
   }
 
   // Pull joint and sensor information
@@ -1193,6 +1201,14 @@ std::vector<hardware_interface::CommandInterface> MujocoSystemInterface::export_
         {
           new_command_interfaces.emplace_back(joint.name, command_if.name, &joint.effort_interface.command_);
         }
+        else if (command_if.name == HW_IF_KP)
+        {
+          new_command_interfaces.emplace_back(joint.name, HW_IF_KP, &joint.kp_interface.command_);
+        }
+        else if (command_if.name == HW_IF_KD)
+        {
+          new_command_interfaces.emplace_back(joint.name, HW_IF_KD, &joint.kd_interface.command_);
+        }
       }
     }
   }
@@ -1225,18 +1241,35 @@ hardware_interface::return_type
 MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string>& start_interfaces,
                                                    const std::vector<std::string>& stop_interfaces)
 {
-  auto update_joint_interface = [this](const std::string& interface_name, bool enabled) {
-    size_t delimiter_pos = interface_name.find('/');
-    if (delimiter_pos == std::string::npos)
+  // Track which interfaces are being started for each joint to determine control mode
+  std::map<std::string, std::set<std::string>> joint_starting_interfaces;
+  std::map<std::string, std::set<std::string>> joint_stopping_interfaces;
+
+  // Parse interfaces into per-joint sets
+  for (const auto& interface : start_interfaces)
+  {
+    size_t delimiter_pos = interface.find('/');
+    if (delimiter_pos != std::string::npos)
     {
-      RCLCPP_ERROR(get_logger(), "Invalid interface name format: %s", interface_name.c_str());
-      return;
+      std::string joint_name = interface.substr(0, delimiter_pos);
+      std::string interface_type = interface.substr(delimiter_pos + 1);
+      joint_starting_interfaces[joint_name].insert(interface_type);
     }
+  }
+  for (const auto& interface : stop_interfaces)
+  {
+    size_t delimiter_pos = interface.find('/');
+    if (delimiter_pos != std::string::npos)
+    {
+      std::string joint_name = interface.substr(0, delimiter_pos);
+      std::string interface_type = interface.substr(delimiter_pos + 1);
+      joint_stopping_interfaces[joint_name].insert(interface_type);
+    }
+  }
 
-    std::string joint_name = interface_name.substr(0, delimiter_pos);
-    std::string interface_type = interface_name.substr(delimiter_pos + 1);
-
-    // Find the MuJoCoActuatorData in the vector
+  // Process each joint's interface changes
+  auto process_joint = [this](const std::string& joint_name, const std::set<std::string>& starting,
+                              const std::set<std::string>& stopping) {
     auto joint_it = std::find_if(urdf_joint_data_.begin(), urdf_joint_data_.end(),
                                  [&joint_name](const URDFJointData& joint) { return joint.name == joint_name; });
 
@@ -1249,7 +1282,7 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
     const auto actuator_name = get_joint_actuator_name(joint_name, get_hardware_info(), mj_model_);
 
     auto actuator_it = std::find_if(mujoco_actuator_data_.begin(), mujoco_actuator_data_.end(),
-                                    [&actuator_name, this](const MuJoCoActuatorData& actuator) {
+                                    [&actuator_name](const MuJoCoActuatorData& actuator) {
                                       return actuator.joint_name == actuator_name;
                                     });
 
@@ -1264,78 +1297,111 @@ MujocoSystemInterface::perform_command_mode_switch(const std::vector<std::string
       return;
     }
 
-    if (enabled)
+    // Process stopping interfaces first
+    for (const auto& interface_type : stopping)
     {
-      // Only one type of control mode can be active at a time, so stop everything first then enable the
-      // requested command interface.
+      if (interface_type == hardware_interface::HW_IF_POSITION)
+      {
+        actuator_it->is_position_control_enabled = false;
+        actuator_it->is_position_pid_control_enabled = false;
+        joint_it->is_position_control_enabled = false;
+      }
+      else if (interface_type == hardware_interface::HW_IF_VELOCITY)
+      {
+        actuator_it->is_velocity_control_enabled = false;
+        actuator_it->is_velocity_pid_control_enabled = false;
+        joint_it->is_velocity_control_enabled = false;
+      }
+      else if (interface_type == hardware_interface::HW_IF_EFFORT ||
+               interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE)
+      {
+        actuator_it->is_effort_control_enabled = false;
+        joint_it->is_effort_control_enabled = false;
+      }
+      else if (interface_type == HW_IF_KP || interface_type == HW_IF_KD)
+      {
+        // When kp or kd is stopped, disable impedance control
+        actuator_it->is_impedance_control_enabled = false;
+        joint_it->is_impedance_control_enabled = false;
+      }
+      RCLCPP_INFO(get_logger(), "Joint %s: %s control disabled", joint_name.c_str(), interface_type.c_str());
+    }
+
+    // Process starting interfaces
+    if (!starting.empty())
+    {
+      // Check if we're starting impedance control (kp or kd with position)
+      bool has_kp = starting.count(HW_IF_KP) > 0;
+      bool has_kd = starting.count(HW_IF_KD) > 0;
+      bool has_position = starting.count(hardware_interface::HW_IF_POSITION) > 0;
+      bool has_velocity = starting.count(hardware_interface::HW_IF_VELOCITY) > 0;
+      bool has_effort = starting.count(hardware_interface::HW_IF_EFFORT) > 0 ||
+                        starting.count(hardware_interface::HW_IF_TORQUE) > 0 ||
+                        starting.count(hardware_interface::HW_IF_FORCE) > 0;
 
+      // Disable all control modes first
       joint_it->is_position_control_enabled = false;
       joint_it->is_velocity_control_enabled = false;
       joint_it->is_effort_control_enabled = false;
+      joint_it->is_impedance_control_enabled = false;
 
       actuator_it->is_position_control_enabled = false;
       actuator_it->is_velocity_control_enabled = false;
       actuator_it->is_effort_control_enabled = false;
       actuator_it->is_position_pid_control_enabled = false;
       actuator_it->is_velocity_pid_control_enabled = false;
+      actuator_it->is_impedance_control_enabled = false;
 
-      if (interface_type == hardware_interface::HW_IF_POSITION)
+      // Determine control mode based on combination of interfaces
+      if ((has_kp || has_kd) && has_position)
       {
-        actuator_it->is_position_control_enabled = (actuator_it->pos_pid == nullptr);
-        actuator_it->is_position_pid_control_enabled = (actuator_it->pos_pid != nullptr);
-        joint_it->is_position_control_enabled = true;
-        RCLCPP_INFO(get_logger(), "Joint %s: position control enabled (velocity, effort disabled)", joint_name.c_str());
-      }
-      else if (interface_type == hardware_interface::HW_IF_VELOCITY)
-      {
-        actuator_it->is_velocity_control_enabled = (actuator_it->vel_pid == nullptr);
-        actuator_it->is_velocity_pid_control_enabled = (actuator_it->vel_pid != nullptr);
-        joint_it->is_velocity_control_enabled = true;
-        RCLCPP_INFO(get_logger(), "Joint %s: velocity control enabled (position, effort disabled)", joint_name.c_str());
+        // Impedance control: effort = effort_ff + kp * (pos_cmd - pos) + kd * (vel_cmd - vel)
+        actuator_it->is_impedance_control_enabled = true;
+        joint_it->is_impedance_control_enabled = true;
+        joint_it->is_position_control_enabled = has_position;
+        joint_it->is_velocity_control_enabled = has_velocity;
+        joint_it->is_effort_control_enabled = has_effort;  // Track effort for feed-forward
+        RCLCPP_INFO(get_logger(), "Joint %s: impedance control enabled (kp/kd with position%s%s)",
+                    joint_name.c_str(), has_velocity ? "/velocity" : "", has_effort ? "/effort_ff" : "");
       }
-      else if (interface_type == hardware_interface::HW_IF_EFFORT ||
-               interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE)
+      else if (has_effort)
       {
         actuator_it->is_effort_control_enabled = true;
         joint_it->is_effort_control_enabled = true;
-        RCLCPP_INFO(get_logger(), "Joint %s: %s control enabled (position, velocity disabled)", joint_name.c_str(),
-                    interface_type.c_str());
-      }
-    }
-    else
-    {
-      if (interface_type == hardware_interface::HW_IF_POSITION)
-      {
-        actuator_it->is_position_control_enabled = false;
-        actuator_it->is_position_pid_control_enabled = false;
-        joint_it->is_position_control_enabled = false;
+        RCLCPP_INFO(get_logger(), "Joint %s: effort control enabled", joint_name.c_str());
       }
-      else if (interface_type == hardware_interface::HW_IF_VELOCITY)
+      else if (has_velocity)
       {
-        actuator_it->is_velocity_control_enabled = false;
-        actuator_it->is_velocity_pid_control_enabled = false;
-        joint_it->is_velocity_control_enabled = false;
+        actuator_it->is_velocity_control_enabled = (actuator_it->vel_pid == nullptr);
+        actuator_it->is_velocity_pid_control_enabled = (actuator_it->vel_pid != nullptr);
+        joint_it->is_velocity_control_enabled = true;
+        RCLCPP_INFO(get_logger(), "Joint %s: velocity control enabled", joint_name.c_str());
       }
-      else if (interface_type == hardware_interface::HW_IF_EFFORT ||
-               interface_type == hardware_interface::HW_IF_TORQUE || interface_type == hardware_interface::HW_IF_FORCE)
+      else if (has_position)
       {
-        actuator_it->is_effort_control_enabled = false;
-        joint_it->is_effort_control_enabled = false;
+        actuator_it->is_position_control_enabled = (actuator_it->pos_pid == nullptr);
+        actuator_it->is_position_pid_control_enabled = (actuator_it->pos_pid != nullptr);
+        joint_it->is_position_control_enabled = true;
+        RCLCPP_INFO(get_logger(), "Joint %s: position control enabled", joint_name.c_str());
       }
-      RCLCPP_INFO(get_logger(), "Joint %s: %s control disabled", joint_name.c_str(), interface_type.c_str());
     }
   };
 
-  // Disable stopped interfaces
-  for (const auto& interface : stop_interfaces)
+  // Get all joints that need processing
+  std::set<std::string> all_joints;
+  for (const auto& [joint, _] : joint_starting_interfaces)
+  {
+    all_joints.insert(joint);
+  }
+  for (const auto& [joint, _] : joint_stopping_interfaces)
   {
-    update_joint_interface(interface, false);
+    all_joints.insert(joint);
   }
 
-  // Enable started interfaces
-  for (const auto& interface : start_interfaces)
+  // Process each joint
+  for (const auto& joint_name : all_joints)
   {
-    update_joint_interface(interface, true);
+    process_joint(joint_name, joint_starting_interfaces[joint_name], joint_stopping_interfaces[joint_name]);
   }
 
   return hardware_interface::return_type::OK;
@@ -1398,7 +1464,7 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time&
     data.torque.data.z() = -mj_data_control_->sensordata[data.torque.mj_sensor_index + 2];
   }
 
-  // Publish Odometry
+  // Publish Odometry and TF
   if (free_joint_id_ != -1 && floating_base_realtime_publisher_)
   {
     floating_base_msg_.header.stamp = time;
@@ -1429,6 +1495,17 @@ hardware_interface::return_type MujocoSystemInterface::read(const rclcpp::Time&
 #else
     floating_base_realtime_publisher_->try_publish(floating_base_msg_);
 #endif
+
+    // Publish TF
+    if (publish_floating_base_tf_ && floating_base_tf_broadcaster_)
+    {
+      floating_base_tf_msg_.header.stamp = time;
+      floating_base_tf_msg_.transform.translation.x = floating_base_msg_.pose.pose.position.x;
+      floating_base_tf_msg_.transform.translation.y = floating_base_msg_.pose.pose.position.y;
+      floating_base_tf_msg_.transform.translation.z = floating_base_msg_.pose.pose.position.z;
+      floating_base_tf_msg_.transform.rotation = floating_base_msg_.pose.pose.orientation;
+      floating_base_tf_broadcaster_->sendTransform(floating_base_tf_msg_);
+    }
   }
 
   return hardware_interface::return_type::OK;
@@ -1470,7 +1547,40 @@ hardware_interface::return_type MujocoSystemInterface::write(const rclcpp::Time&
     {
       continue;
     }
-    if (actuator.is_position_control_enabled)
+    if (actuator.is_impedance_control_enabled)
+    {
+      // Impedance control: effort = effort_ff + kp * (pos_cmd - pos) + kd * (vel_cmd - vel)
+      // Uses commanded kp/kd values and feed-forward effort
+      double kp = actuator.kp_interface.command_;
+      double kd = actuator.kd_interface.command_;
+      double effort_ff = actuator.effort_interface.command_;
+
+      // Handle NaN values - default to 0 if not set
+      if (std::isnan(kp))
+      {
+        kp = 0.0;
+      }
+      if (std::isnan(kd))
+      {
+        kd = 0.0;
+      }
+      if (std::isnan(effort_ff))
+      {
+        effort_ff = 0.0;  // No feed-forward torque if not set
+      }
+
+      double pos_error = actuator.position_interface.command_ - mj_data_->qpos[actuator.mj_pos_adr];
+      double vel_cmd = actuator.velocity_interface.command_;
+      if (std::isnan(vel_cmd))
+      {
+        vel_cmd = 0.0;  // Default to zero velocity target
+      }
+      double vel_error = vel_cmd - mj_data_->qvel[actuator.mj_vel_adr];
+
+      double effort = effort_ff + kp * pos_error + kd * vel_error;
+      mj_data_control_->ctrl[actuator.mj_actuator_id] = effort;
+    }
+    else if (actuator.is_position_control_enabled)
     {
       mj_data_control_->ctrl[actuator.mj_actuator_id] = actuator.position_interface.command_;
     }
@@ -1550,6 +1660,9 @@ void MujocoSystemInterface::joint_command_to_actuator_command()
         actuator_interface.position_interface.command_ = joint.position_interface.command_;
         actuator_interface.velocity_interface.command_ = joint.velocity_interface.command_;
         actuator_interface.effort_interface.command_ = joint.effort_interface.command_;
+        // Also copy kp/kd commands for impedance control
+        actuator_interface.kp_interface.command_ = joint.kp_interface.command_;
+        actuator_interface.kd_interface.command_ = joint.kd_interface.command_;
       }
     });
   }
@@ -1794,6 +1907,37 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
       joint.command_interfaces.clear();
     }
 
+    // For joints with actuators, ensure all standard command interfaces are available
+    // This is needed because ros2_control's URDF parser may filter out non-standard interfaces
+    if (actuator_exists && !joint.command_interfaces.empty())
+    {
+      auto has_interface = [&joint](const std::string& name) {
+        return std::any_of(joint.command_interfaces.begin(), joint.command_interfaces.end(),
+                           [&name](const hardware_interface::InterfaceInfo& ci) { return ci.name == name; });
+      };
+
+      // Add velocity interface if not present
+      if (!has_interface(hardware_interface::HW_IF_VELOCITY))
+      {
+        joint.command_interfaces.push_back({ hardware_interface::HW_IF_VELOCITY });
+      }
+      // Add effort interface if not present
+      if (!has_interface(hardware_interface::HW_IF_EFFORT))
+      {
+        joint.command_interfaces.push_back({ hardware_interface::HW_IF_EFFORT });
+      }
+      // Add kp interface for impedance control
+      if (!has_interface(HW_IF_KP))
+      {
+        joint.command_interfaces.push_back({ HW_IF_KP });
+      }
+      // Add kd interface for impedance control
+      if (!has_interface(HW_IF_KD))
+      {
+        joint.command_interfaces.push_back({ HW_IF_KD });
+      }
+    }
+
     // Add to the joint hw information map
     joint_hw_info_.insert(std::make_pair(joint.name, joint));
 
@@ -1827,16 +1971,8 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
       }
     }
 
-    auto get_initial_value = [this](const hardware_interface::InterfaceInfo& interface_info) {
-      if (!interface_info.initial_value.empty())
-      {
-        double value = std::stod(interface_info.initial_value);
-        return value;
-      }
-      else
-      {
-        return 0.0;
-      }
+    auto get_initial_value = [](const hardware_interface::InterfaceInfo& interface_info) -> double {
+      return interface_info.initial_value.empty() ? 0.0 : std::stod(interface_info.initial_value);
     };
 
     // Set initial values to joint interfaces if they are set in the info
@@ -1925,6 +2061,7 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
       {
         // Velocity command interface:
         // Direct control for velocity actuators; velocity PID required for motor or custom actuators.
+        // Exception: if kp/kd interfaces are present, velocity is used for impedance control (no PID needed).
         RCLCPP_ERROR_EXPRESSION(get_logger(), actuator_it->actuator_type == ActuatorType::POSITION,
                                 "Velocity command interface for the joint : %s is not supported with position actuator",
                                 actuator_name.c_str());
@@ -1936,6 +2073,11 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
         }
         else if (actuator_it->actuator_type == ActuatorType::MOTOR || actuator_it->actuator_type == ActuatorType::CUSTOM)
         {
+          // Check if kp/kd interfaces are present (indicating impedance control mode)
+          bool has_impedance_interfaces = std::any_of(
+              command_interface_names.begin(), command_interface_names.end(),
+              [](const std::string& name) { return name == HW_IF_KP || name == HW_IF_KD; });
+
           if (actuator_it->has_vel_pid)
           {
             actuator_it->is_velocity_control_enabled = false;
@@ -1951,6 +2093,13 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
                         gains.antiwindup_strat_.to_string().c_str());
 #endif
           }
+          else if (has_impedance_interfaces)
+          {
+            // Velocity interface will be used for impedance control - no PID needed
+            RCLCPP_DEBUG(get_logger(),
+                         "Velocity command interface for joint '%s' will be used for impedance control",
+                         actuator_name.c_str());
+          }
           else
           {
             RCLCPP_ERROR(get_logger(),
@@ -1978,6 +2127,14 @@ void MujocoSystemInterface::register_urdf_joints(const hardware_interface::Hardw
           actuator_it->is_effort_control_enabled = true;
         }
       }
+      else if (command_if == HW_IF_KP || command_if == HW_IF_KD)
+      {
+        // kp/kd interfaces are used for impedance control.
+        // They are handled in perform_command_mode_switch() and write().
+        // Just acknowledge them here - no special registration needed.
+        RCLCPP_DEBUG(get_logger(), "Registered %s command interface for joint '%s' (for impedance control)",
+                     command_if.c_str(), joint.name.c_str());
+      }
       else
       {
         RCLCPP_WARN(get_logger(), "Unsupported command interface '%s' for joint '%s'. Skipping it!", command_if.c_str(),
@@ -2419,6 +2576,26 @@ void MujocoSystemInterface::set_initial_pose()
   mj_copyData(mj_data_control_, mj_model_, mj_data_);
 }
 
+void MujocoSystemInterface::reset_simulation()
+{
+  const std::unique_lock<std::recursive_mutex> lock(*sim_mutex_);
+
+  // Accumulate current sim time so the published /clock stays monotonic.
+  // mj_resetData will set mj_data_->time to 0, but publish_clock() adds
+  // clock_offset_ so consumers never see a backward jump.
+  clock_offset_ += mj_data_->time;
+
+  // Reset MuJoCo data to initial state - this resets qpos to qpos0 (model defaults),
+  // qvel to zero, time to zero, and clears all other state.
+  mj_resetData(mj_model_, mj_data_);
+
+  // Forward pass to update derived quantities
+  mj_forward(mj_model_, mj_data_);
+
+  // Copy into the control data for reads
+  mj_copyData(mj_data_control_, mj_model_, mj_data_);
+}
+
 // simulate in background thread (while rendering in main thread)
 void MujocoSystemInterface::PhysicsLoop()
 {
@@ -2586,7 +2763,7 @@ void MujocoSystemInterface::PhysicsLoop()
 
 void MujocoSystemInterface::publish_clock()
 {
-  auto sim_time = mj_data_->time;
+  const double sim_time = mj_data_->time + clock_offset_;
   int32_t sim_time_sec = static_cast<int32_t>(std::floor(sim_time));
   uint32_t sim_time_nanosec = static_cast<uint32_t>((sim_time - sim_time_sec) * 1e9);
   rclcpp::Time sim_time_ros(sim_time_sec, sim_time_nanosec, RCL_ROS_TIME);
diff --git a/mujoco_ros2_control/test/src/robot_launch_pid_test.py b/mujoco_ros2_control/test/src/robot_launch_pid_test.py
index 41731958..52028947 100644
--- a/mujoco_ros2_control/test/src/robot_launch_pid_test.py
+++ b/mujoco_ros2_control/test/src/robot_launch_pid_test.py
@@ -94,9 +94,19 @@ def test_available_hardware_interfaces(self):
         expected_command_interfaces = [
             "joint1/position",
             "joint1/velocity",
+            "joint1/effort",
+            "joint1/kp",
+            "joint1/kd",
             "joint2/position",
             "joint2/velocity",
+            "joint2/effort",
+            "joint2/kp",
+            "joint2/kd",
             "gripper_left_finger_joint/position",
+            "gripper_left_finger_joint/velocity",
+            "gripper_left_finger_joint/effort",
+            "gripper_left_finger_joint/kp",
+            "gripper_left_finger_joint/kd",
         ]
         assert len(available_command_interfaces_names) == len(expected_command_interfaces), (
             f"Expected {len(expected_command_interfaces)} command interfaces, "
diff --git a/mujoco_ros2_control/test/src/robot_launch_test.py b/mujoco_ros2_control/test/src/robot_launch_test.py
index aff67197..efd4fdc4 100644
--- a/mujoco_ros2_control/test/src/robot_launch_test.py
+++ b/mujoco_ros2_control/test/src/robot_launch_test.py
@@ -360,7 +360,23 @@ def test_available_hardware_interfaces(self):
 
         # available command interfaces
         available_command_interfaces_names = [iface.name for iface in response.command_interfaces]
-        expected_command_interfaces = ["joint1/position", "joint2/position", "gripper_left_finger_joint/position"]
+        expected_command_interfaces = [
+            "joint1/position",
+            "joint1/velocity",
+            "joint1/effort",
+            "joint1/kp",
+            "joint1/kd",
+            "joint2/position",
+            "joint2/velocity",
+            "joint2/effort",
+            "joint2/kp",
+            "joint2/kd",
+            "gripper_left_finger_joint/position",
+            "gripper_left_finger_joint/velocity",
+            "gripper_left_finger_joint/effort",
+            "gripper_left_finger_joint/kp",
+            "gripper_left_finger_joint/kd",
+        ]
 
         assert len(available_command_interfaces_names) == len(expected_command_interfaces), (
             f"Expected {len(expected_command_interfaces)} command interfaces, "