diff --git a/ign_ros2_control/CMakeLists.txt b/ign_ros2_control/CMakeLists.txt
index f11c290f..caa232f3 100644
--- a/ign_ros2_control/CMakeLists.txt
+++ b/ign_ros2_control/CMakeLists.txt
@@ -20,10 +20,15 @@ find_package(ament_cmake REQUIRED)
 find_package(ament_index_cpp REQUIRED)
 find_package(controller_manager REQUIRED)
 find_package(hardware_interface REQUIRED)
+find_package(generate_parameter_library REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 
+generate_parameter_library(ign_ros2_control_parameters
+  src/ign_ros2_control_parameters.yaml
+)
+
 if("$ENV{IGNITION_VERSION}" STREQUAL "citadel")
   find_package(ignition-gazebo3 REQUIRED)
   set(IGN_GAZEBO_VER ${ignition-gazebo3_VERSION_MAJOR})
@@ -39,10 +44,30 @@ elseif("$ENV{IGNITION_VERSION}" STREQUAL "fortress")
   set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
   message(STATUS "Compiling against Ignition Fortress")
 
+  find_package(ignition-cmake2 REQUIRED)
+  find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+  set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+  add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
+  target_link_libraries(mimic-joint-system
+    PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+    PRIVATE ignition-gazebo6::ignition-gazebo6)
+  install(TARGETS mimic-joint-system
+          DESTINATION lib)
+
 else()
   find_package(ignition-gazebo6 REQUIRED)
   set(IGN_GAZEBO_VER ${ignition-gazebo6_VERSION_MAJOR})
   message(STATUS "Compiling against Ignition Fortress")
+
+  find_package(ignition-cmake2 REQUIRED)
+  find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+  set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+  add_library(mimic-joint-system SHARED src/MimicJointSystem.cc)
+  target_link_libraries(mimic-joint-system
+    ignition-gazebo${IGN_GAZEBO_VER}::core
+    ignition-plugin${IGN_PLUGIN_VER}::register)
+  install(TARGETS mimic-joint-system
+          DESTINATION lib)
 endif()
 
 find_package(ignition-plugin1 REQUIRED)
@@ -58,6 +83,7 @@ target_link_libraries(${PROJECT_NAME}-system
   ignition-gazebo${IGN_GAZEBO_VER}::core
   ignition-plugin${IGN_PLUGIN_VER}::register
 )
+target_link_libraries(${PROJECT_NAME}-system ign_ros2_control_parameters)
 ament_target_dependencies(${PROJECT_NAME}-system
   ament_index_cpp
   controller_manager
@@ -73,6 +99,8 @@ ament_target_dependencies(${PROJECT_NAME}-system
 add_library(ign_hardware_plugins SHARED
   src/ign_system.cpp
 )
+target_include_directories(ign_hardware_plugins PRIVATE include)
+target_link_libraries(ign_hardware_plugins ign_ros2_control_parameters)
 ament_target_dependencies(ign_hardware_plugins
   rclcpp_lifecycle
   hardware_interface
@@ -82,9 +110,14 @@ target_link_libraries(ign_hardware_plugins
   ignition-gazebo${IGN_GAZEBO_VER}::core
 )
 
+install(DIRECTORY include/
+  DESTINATION include
+)
+
 ## Install
 install(TARGETS
-  ign_hardware_plugins
+  ign_hardware_plugins ign_ros2_control_parameters
+  EXPORT export_ign_ros2_control
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin
@@ -98,7 +131,9 @@ endif()
 
 ament_export_include_directories(include)
 ament_export_libraries(${PROJECT_NAME} ign_hardware_plugins)
-
+ament_export_targets(
+  export_ign_ros2_control HAS_LIBRARY_TARGET
+)
 # Install directories
 install(TARGETS ${PROJECT_NAME}-system
   DESTINATION lib
diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
new file mode 100644
index 00000000..e9972e18
--- /dev/null
+++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
@@ -0,0 +1,104 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Lovro Ivanov lovro.ivanov@gmail.com
+ * \date    2023-03-15
+ *
+ */
+//----------------------------------------------------------------------
+
+#ifndef IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
+#define IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
+
+//! [header]
+#include <memory>
+
+#include <ignition/gazebo/System.hh>
+
+// This is an example to go in your urdf
+//! <plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
+//!     <joint_name>joint_name</joint_name>
+//!     <mimic_joint_name>mimic_joint_name</mimic_joint_name>
+//!     <multiplier>1.0</multiplier>
+//!     <offset>0.0</offset>
+//!     <joint_index>0</joint_index>
+//!     <mimic_joint_index>0</mimic_joint_index>
+//!     <p_gain>100.0</p_gain>
+//!     <i_gain>0.1</i_gain>
+//!     <d_gain>0.0</d_gain>
+//!     <i_max>5.0</i_max>
+//!     <i_min>-5.0</i_min>
+//!     <cmd_max>500.0</cmd_max>
+//!     <cmd_min>-500.0</cmd_min>
+//!     <cmd_offset>0.0</cmd_offset>
+//!     <dead_zone>0.001</dead_zone>
+//!     <use_velocity_commands>false</use_velocity_commands>
+//! </plugin>
+
+namespace ign_ros2_control
+{
+
+// Forward declaration
+  class MimicJointSystemPrivate;  // NOLINT
+
+  class MimicJointSystem:  // NOLINT
+// This class is a system.
+  public ignition::gazebo::System,  // NOLINT
+  public ignition::gazebo::ISystemConfigure,  // NOLINT
+// This class also implements the ISystemPreUpdate, ISystemUpdate,
+// and ISystemPostUpdate interfaces.
+  public ignition::gazebo::ISystemPreUpdate,  // NOLINT
+  public ignition::gazebo::ISystemUpdate,  // NOLINT
+  public ignition::gazebo::ISystemPostUpdate  // NOLINT
+  {
+public:
+    MimicJointSystem();
+
+    // Documentation inherited
+
+public:
+    void Configure(
+      const ignition::gazebo::Entity & _entity,
+      const std::shared_ptr < const sdf::Element > & _sdf,
+      ignition::gazebo::EntityComponentManager & _ecm,
+      ignition::gazebo::EventManager & _eventMgr) override;
+
+public:
+    void PreUpdate(
+      const ignition::gazebo::UpdateInfo & _info,
+      ignition::gazebo::EntityComponentManager & _ecgm) override;
+
+public:
+    void Update(
+      const ignition::gazebo::UpdateInfo & _info,
+      ignition::gazebo::EntityComponentManager & _ecm) override;
+
+public:
+    void PostUpdate(
+      const ignition::gazebo::UpdateInfo & _info,
+      const ignition::gazebo::EntityComponentManager & _ecm) override;
+
+private:
+    /// \brief Private data pointer
+
+private:
+    std::unique_ptr < MimicJointSystemPrivate > dataPtr;
+  };
+}  // namespace ign_ros2_control
+//! [header]
+
+#endif  // IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
diff --git a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp
index 08dfd847..1949f77f 100644
--- a/ign_ros2_control/include/ign_ros2_control/ign_system.hpp
+++ b/ign_ros2_control/include/ign_ros2_control/ign_system.hpp
@@ -22,9 +22,13 @@
 #include <vector>
 
 #include "ign_ros2_control/ign_system_interface.hpp"
+
+#include "rclcpp/executors/single_threaded_executor.hpp"
 #include "rclcpp_lifecycle/state.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
+#include "ign_ros2_control_parameters.hpp"
+
 namespace ign_ros2_control
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
@@ -88,6 +92,15 @@ class IgnitionSystem : public IgnitionSystemInterface
 
   /// \brief Private data class
   std::unique_ptr<IgnitionSystemPrivate> dataPtr;
+
+  // Parameters from ROS for ign_ros2_control
+  std::shared_ptr<ParamListener> param_listener_;
+  Params params_;
+
+  rclcpp::Node::SharedPtr param_node_;
+  std::thread spin_thread_;
+  std::atomic<bool> stop_spin_ = false;
+  rclcpp::executors::SingleThreadedExecutor::SharedPtr exec_;
 };
 
 }  // namespace ign_ros2_control
diff --git a/ign_ros2_control/package.xml b/ign_ros2_control/package.xml
index 90195285..7d31d748 100644
--- a/ign_ros2_control/package.xml
+++ b/ign_ros2_control/package.xml
@@ -22,6 +22,7 @@
   <depend>rclcpp_lifecycle</depend>
   <depend>hardware_interface</depend>
   <depend>controller_manager</depend>
+  <depend>generate_parameter_library</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc
new file mode 100644
index 00000000..ca3e9903
--- /dev/null
+++ b/ign_ros2_control/src/MimicJointSystem.cc
@@ -0,0 +1,402 @@
+// Copyright 2023 Open Source Robotics Foundation, Inc.
+//
+// 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.
+
+//----------------------------------------------------------------------
+/*!\file
+ *
+ * \author  Lovro Ivanov lovro.ivanov@gmail.com
+ * \date    2023-03-15
+ *
+ */
+//----------------------------------------------------------------------
+
+#include "ign_ros2_control/MimicJointSystem.hh"
+
+#include <string>
+#include <unordered_set>
+
+#include <ignition/common/Profiler.hh>
+#include <ignition/math/PID.hh>
+#include <ignition/plugin/Register.hh>
+
+#include "ignition/gazebo/components/JointForceCmd.hh"
+#include "ignition/gazebo/components/JointVelocityCmd.hh"
+#include "ignition/gazebo/components/JointPosition.hh"
+#include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/Name.hh"
+#include "ignition/gazebo/components/JointType.hh"
+
+#include "ignition/gazebo/Model.hh"
+
+
+class ign_ros2_control::MimicJointSystemPrivate
+{
+/// \brief Joint Entity
+
+public:
+  ignition::gazebo::Entity jointEntity {ignition::gazebo::kNullEntity};
+
+public:
+  ignition::gazebo::Entity mimicJointEntity {ignition::gazebo::kNullEntity};
+
+/// \brief Joint name
+
+public:
+  std::string jointName;
+
+public:
+  std::string mimicJointName;
+
+/// \brief Commanded joint position
+
+public:
+  double jointPosCmd {0.0};
+
+public:
+  double multiplier {1.0};
+
+public:
+  double offset {0.0};
+
+  /// \brief Model interface
+
+public:
+  ignition::gazebo::Model model {ignition::gazebo::kNullEntity};
+
+  /// \brief Position PID controller.
+
+public:
+  ignition::math::PID posPid;
+
+  /// \brief Joint index to be used.
+
+public:
+  unsigned int jointIndex = 0u;
+
+public:
+  unsigned int mimicJointIndex = 0u;
+
+public:
+  double deadZone = 1e-6;
+
+  // approach adopted from https://github.com/gazebosim/gz-sim/blob/330eaf2f135301e90096fe91897f052cdaa77013/src/systems/joint_position_controller/JointPositionController.cc#L69-L77
+  /// \brief Operation modes
+  enum OperationMode
+  {
+    /// \brief Use PID to achieve positional control
+    PID,
+    /// \brief Bypass PID completely. This means the joint will move to that
+    /// position bypassing the physics engine.
+    ABS
+  };
+
+  /// \brief Joint position mode
+
+public:
+  OperationMode mode = OperationMode::PID;
+};
+
+namespace ign_ros2_control
+{
+
+  MimicJointSystem::MimicJointSystem()
+  : dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ())
+  {
+  }
+
+  void MimicJointSystem::Configure(
+    const ignition::gazebo::Entity & _entity,
+    const std::shared_ptr < const sdf::Element > & _sdf,
+    ignition::gazebo::EntityComponentManager & _ecm,
+    ignition::gazebo::EventManager & /* _eventMgr*/)
+  {
+    // Make sure the controller is attached to a valid model
+    this->dataPtr->model = ignition::gazebo::Model(_entity);
+    if (!this->dataPtr->model.Valid(_ecm)) {
+      ignerr << "MimicJointSystem  Failed to initialize because [" <<
+        this->dataPtr->model.Name(_ecm)
+             << "] (Entity=" << _entity << ")] is not a model.. ";
+      return;
+    }
+
+    // Get params from SDF
+    this->dataPtr->jointName = _sdf->Get < std::string > ("joint_name");
+
+    if (this->dataPtr->jointName.empty()) {
+      ignerr << "MimicJointSystem found an empty joint_name parameter. "
+             << "Failed to initialize.";
+      return;
+    }
+
+    this->dataPtr->mimicJointName = _sdf->Get < std::string > ("mimic_joint_name");
+    if (this->dataPtr->mimicJointName.empty()) {
+      ignerr << "MimicJointSystem found an empty mimic_joint_name parameter. "
+             << "Failed to initialize.";
+      return;
+    }
+
+    if (_sdf->HasElement("multiplier")) {
+      this->dataPtr->multiplier = _sdf->Get < double > ("multiplier");
+    }
+
+    if (_sdf->HasElement("offset")) {
+      this->dataPtr->offset = _sdf->Get < double > ("offset");
+    }
+
+    if (_sdf->HasElement("joint_index")) {
+      this->dataPtr->jointIndex = _sdf->Get < unsigned int > ("joint_index");
+    }
+
+    if (_sdf->HasElement("mimic_joint_index")) {
+      this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index");
+    }
+
+    if (_sdf->HasElement("dead_zone")) {
+      this->dataPtr->deadZone = _sdf->Get < double > ("dead_zone");
+    }
+
+    // PID parameters
+    double p = 1;
+    double i = 0.1;
+    double d = 0.01;
+    double iMax = 1;
+    double iMin = -1;
+    double cmdMax = 1000;
+    double cmdMin = -1000;
+    double cmdOffset = 0;
+
+    if (_sdf->HasElement("p_gain")) {
+      p = _sdf->Get < double > ("p_gain");
+    }
+    if (_sdf->HasElement("i_gain")) {
+      i = _sdf->Get < double > ("i_gain");
+    }
+    if (_sdf->HasElement("d_gain")) {
+      d = _sdf->Get < double > ("d_gain");
+    }
+    if (_sdf->HasElement("i_max")) {
+      iMax = _sdf->Get < double > ("i_max");
+    }
+    if (_sdf->HasElement("i_min")) {
+      iMin = _sdf->Get < double > ("i_min");
+    }
+    if (_sdf->HasElement("cmd_max")) {
+      cmdMax = _sdf->Get < double > ("cmd_max");
+    }
+    if (_sdf->HasElement("cmd_min")) {
+      cmdMin = _sdf->Get < double > ("cmd_min");
+    }
+    if (_sdf->HasElement("cmd_offset")) {
+      cmdOffset = _sdf->Get < double > ("cmd_offset");
+    }
+    if (_sdf->HasElement("use_velocity_commands")) {
+      auto useVelocityCommands = _sdf->Get < bool > ("use_velocity_commands");
+      if (useVelocityCommands) {
+        this->dataPtr->mode = MimicJointSystemPrivate::OperationMode::ABS;
+      }
+    }
+
+    this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
+
+    if (_sdf->HasElement("initial_position")) {
+      this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position");
+    }
+
+    igndbg << "[MimicJointSystem] system parameters:\n";
+    igndbg << "p_gain: [" << p << "]\n";
+    igndbg << "i_gain: [" << i << "]\n";
+    igndbg << "d_gain: [" << d << "]\n";
+    igndbg << "i_max: [" << iMax << "]\n";
+    igndbg << "i_min: [" << iMin << "]\n";
+    igndbg << "cmd_max: [" << cmdMax << "]\n";
+    igndbg << "cmd_min: [" << cmdMin << "]\n";
+    igndbg << "cmd_offset: [" << cmdOffset << "]\n";
+    igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]\n";
+  }
+
+  void MimicJointSystem::PreUpdate(
+    const ignition::gazebo::UpdateInfo & _info,
+    ignition::gazebo::EntityComponentManager & _ecm)
+  {
+    if (_info.dt < std::chrono::steady_clock::duration::zero()) {
+      ignwarn << "Detected jump back in time [" << std::chrono::duration_cast <
+        std::chrono::seconds > (_info.dt).count()
+              << "s]. System may not work properly." << std::endl;
+    }
+
+    // If the joint hasn't been identified yet, look for it
+    if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity ||
+      this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity)
+    {
+      auto jointEntities =
+        _ecm.ChildrenByComponents(
+        this->dataPtr->model.Entity(),
+        ignition::gazebo::components::Joint());
+
+      // Iterate over all joints and verify whether they can be enabled or not
+      for (const auto & jointEntity : jointEntities) {
+        const auto jointName = _ecm.Component < ignition::gazebo::components::Name >
+          (jointEntity)->Data();
+        if (jointName == this->dataPtr->jointName) {
+          this->dataPtr->jointEntity = jointEntity;
+        } else if (jointName == this->dataPtr->mimicJointName) {
+          this->dataPtr->mimicJointEntity = jointEntity;
+        }
+      }
+    }
+
+    if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) {
+      return;
+    }
+
+    if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) {
+      return;
+    }
+
+    if (_info.paused) {
+      return;
+    }
+
+    auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition >
+      (this->dataPtr->jointEntity);
+    if (!jointPosComp) {
+      _ecm.CreateComponent(
+        this->dataPtr->jointEntity,
+        ignition::gazebo::components::JointPosition());
+    }
+
+    auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition >
+      (this->dataPtr->mimicJointEntity);
+    if (!mimicJointPosComp) {
+      _ecm.CreateComponent(
+        this->dataPtr->mimicJointEntity,
+        ignition::gazebo::components::JointPosition());
+    }
+
+    if (jointPosComp == nullptr || jointPosComp->Data().empty() || mimicJointPosComp == nullptr ||
+      mimicJointPosComp->Data().empty())
+    {
+      return;
+    }
+
+    // Sanity check: Make sure that the joint index is valid.
+    if (this->dataPtr->jointIndex >= jointPosComp->Data().size()) {
+      static std::unordered_set < ignition::gazebo::Entity > reported;
+      if (reported.find(this->dataPtr->jointEntity) == reported.end()) {
+        ignerr << "[MimicJointSystem]: Detected an invalid <joint_index> "
+               << "parameter. The index specified is [" << this->dataPtr->jointIndex <<
+          "] but joint ["
+               << this->dataPtr->jointName << "] only has [" << jointPosComp->Data().size() <<
+          "] index[es]. "
+               << "This controller will be ignored" << std::endl;
+        reported.insert(this->dataPtr->jointEntity);
+      }
+      return;
+    }
+
+    // Sanity check: Make sure that the mimic joint index is valid.
+    if (this->dataPtr->mimicJointIndex >= mimicJointPosComp->Data().size()) {
+      static std::unordered_set < ignition::gazebo::Entity > mimic_reported;
+      if (mimic_reported.find(this->dataPtr->mimicJointEntity) == mimic_reported.end()) {
+        ignerr << "[MimicJointSystem]: Detected an invalid <joint_index> "
+               << "parameter. The index specified is [" << this->dataPtr->mimicJointIndex <<
+          "] but joint ["
+               << this->dataPtr->mimicJointName << "] only has [" <<
+          mimicJointPosComp->Data().size() << "] index[es]. "
+               << "This controller will be ignored" << std::endl;
+        mimic_reported.insert(this->dataPtr->mimicJointEntity);
+      }
+      return;
+    }
+
+    // Get error in position
+    double error = jointPosComp->Data().at(this->dataPtr->jointIndex) -
+      (mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) * this->dataPtr->multiplier +
+      this->dataPtr->offset);
+
+    if (fabs(error) > this->dataPtr->deadZone) {
+      // Check if the mode is ABS
+      if (this->dataPtr->mode == MimicJointSystemPrivate::OperationMode::ABS) {
+        // Calculate target velcity
+        double targetVel = 0;
+
+        // Get time in seconds
+        auto dt = std::chrono::duration < double > (_info.dt).count();
+
+        // Get the maximum amount in m that this joint may move
+        auto maxMovement = this->dataPtr->posPid.CmdMax() * dt;
+
+        // Limit the maximum change to maxMovement
+        if (abs(error) > maxMovement) {
+          targetVel =
+            (error < 0) ? this->dataPtr->posPid.CmdMax() : -this->dataPtr->posPid.CmdMax();
+        } else {
+          targetVel = -error;
+        }
+
+        // Update velocity command.
+        auto vel = _ecm.Component < ignition::gazebo::components::JointVelocityCmd >
+          (this->dataPtr->jointEntity);
+
+        if (vel == nullptr) {
+          _ecm.CreateComponent(
+            this->dataPtr->jointEntity, ignition::gazebo::components::JointVelocityCmd(
+              {targetVel}));
+        } else {
+          *vel = ignition::gazebo::components::JointVelocityCmd({targetVel});
+        }
+        return;
+      }
+
+      // If the mode is not ABS, default mode is PID
+      // (e.g.) MimicJointSystemPrivate::OperationMode::PID
+
+      // Update force command.
+      double force = this->dataPtr->posPid.Update(error, _info.dt);
+
+      auto forceComp = _ecm.Component < ignition::gazebo::components::JointForceCmd >
+        (this->dataPtr->jointEntity);
+      if (forceComp == nullptr) {
+        _ecm.CreateComponent(
+          this->dataPtr->jointEntity, ignition::gazebo::components::JointForceCmd(
+            {force}));
+      } else {
+        *forceComp = ignition::gazebo::components::JointForceCmd({force});
+      }
+    }
+  }
+
+  void MimicJointSystem::Update(
+    const ignition::gazebo::UpdateInfo & /*_info*/,
+    ignition::gazebo::EntityComponentManager & /*_ecm*/)
+  {
+  }
+
+  void MimicJointSystem::PostUpdate(
+    const ignition::gazebo::UpdateInfo & /*_info*/,
+    const ignition::gazebo::EntityComponentManager & /*_ecm*/)
+  {
+  }
+}  // namespace ign_ros2_control
+
+//! [registerMimicJointSystem]
+
+IGNITION_ADD_PLUGIN(
+  ign_ros2_control::MimicJointSystem,
+  ignition::gazebo::System,
+  ign_ros2_control::MimicJointSystem::ISystemConfigure,
+  ign_ros2_control::MimicJointSystem::ISystemPreUpdate,
+  ign_ros2_control::MimicJointSystem::ISystemUpdate,
+  ign_ros2_control::MimicJointSystem::ISystemPostUpdate)
+//! [registerMimicJointSystem]
diff --git a/ign_ros2_control/src/ign_ros2_control_parameters.yaml b/ign_ros2_control/src/ign_ros2_control_parameters.yaml
new file mode 100644
index 00000000..61d4d86e
--- /dev/null
+++ b/ign_ros2_control/src/ign_ros2_control_parameters.yaml
@@ -0,0 +1,98 @@
+ign_ros2_control:
+  joints: {
+    type: string_array,
+    default_value: [],
+    description: "Names of joints used by the controller",
+    validation: {
+      unique<>: null,
+    }
+  }
+  mode:
+    __map_joints:
+      use_cascade_control: {
+        type: bool,
+        default_value: false,
+        description: "Defines if cascade control is ued in position control (outer position based loop and inner velocity based loop with force output)"
+      }
+  gains:
+    __map_joints:
+      p_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Proportional gain for PID"
+      }
+      i_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral gain for PID"
+      }
+      d_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Derivative gain for PID"
+      }
+      iMax_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral positive clamp."
+      }
+      iMin_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral negative clamp."
+      }
+      cmdMax_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Maximum value for the PID command."
+      }
+      cmdMin_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Maximum value for the PID command."
+      }
+      cmdOffset_pos: {
+        type: double,
+        default_value: 0.0,
+        description: "Offset value for the command which is added to the result of the PID controller."
+      }
+      p_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Proportional gain for PID"
+      }
+      i_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral gain for PID"
+      }
+      d_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Derivative gain for PID"
+      }
+      iMax_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral positive clamp."
+      }
+      iMin_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Integral negative clamp."
+      }
+      cmdMax_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Maximum value for the PID command."
+      }
+      cmdMin_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Maximum value for the PID command."
+      }
+      cmdOffset_vel: {
+        type: double,
+        default_value: 0.0,
+        description: "Offset value for the command which is added to the result of the PID controller."
+      }
diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp
index 9921523a..3e1e958b 100644
--- a/ign_ros2_control/src/ign_system.cpp
+++ b/ign_ros2_control/src/ign_system.cpp
@@ -16,6 +16,8 @@
 
 #include <ignition/msgs/imu.pb.h>
 
+#include <iostream>
+
 #include <limits>
 #include <map>
 #include <memory>
@@ -30,16 +32,25 @@
 #include <ignition/gazebo/components/JointPosition.hh>
 #include <ignition/gazebo/components/JointVelocity.hh>
 #include <ignition/gazebo/components/JointVelocityCmd.hh>
+#include <ignition/gazebo/components/JointVelocityReset.hh>
+
+#include <ignition/gazebo/components/JointAxis.hh>
+#include <ignition/gazebo/components/JointType.hh>
+#include <ignition/gazebo/components/Joint.hh>
+
 #include <ignition/gazebo/components/LinearAcceleration.hh>
 #include <ignition/gazebo/components/Name.hh>
 #include <ignition/gazebo/components/ParentEntity.hh>
 #include <ignition/gazebo/components/Pose.hh>
 #include <ignition/gazebo/components/Sensor.hh>
 
-#include <ignition/transport/Node.hh>
+// pid_pos stuff
+#include <ignition/math/PID.hh>
 
+#include <ignition/transport/Node.hh>
 
 #include <hardware_interface/hardware_info.hpp>
+#include <normApi.h>
 
 struct jointData
 {
@@ -67,6 +78,12 @@ struct jointData
   /// \brief handles to the joints from within Gazebo
   ignition::gazebo::Entity sim_joint;
 
+  /// \brief PID for position control
+  ignition::math::PID pid_pos;
+
+  /// \brief PID for velocity control
+  ignition::math::PID pid_vel;
+
   /// \brief Control method defined in the URDF for each joint.
   ign_ros2_control::IgnitionSystemInterface::ControlMethod joint_control_method;
 };
@@ -76,6 +93,7 @@ struct MimicJoint
   std::size_t joint_index;
   std::size_t mimicked_joint_index;
   double multiplier = 1.0;
+  double mimic_ff_force_scaling = 0.01;
   std::vector<std::string> interfaces_to_mimic;
 };
 
@@ -159,8 +177,7 @@ bool IgnitionSystem::initSim(
   rclcpp::Node::SharedPtr & model_nh,
   std::map<std::string, ignition::gazebo::Entity> & enableJoints,
   const hardware_interface::HardwareInfo & hardware_info,
-  ignition::gazebo::EntityComponentManager & _ecm,
-  int & update_rate)
+  ignition::gazebo::EntityComponentManager & _ecm, int & update_rate)
 {
   this->dataPtr = std::make_unique<IgnitionSystemPrivate>();
   this->dataPtr->last_update_sim_time_ros_ = rclcpp::Time();
@@ -177,13 +194,12 @@ bool IgnitionSystem::initSim(
 
   constexpr double default_gain = 0.1;
   if (!this->nh_->get_parameter_or(
-      "position_proportional_gain",
-      this->dataPtr->position_proportional_gain_, default_gain))
+      "position_proportional_gain", this->dataPtr->position_proportional_gain_,
+      default_gain))
   {
     RCLCPP_WARN_STREAM(
       this->nh_->get_logger(),
-      "The position_proportional_gain parameter was not defined, defaulting to: " <<
-        default_gain);
+      "The position_proportional_gain parameter was not defined, defaulting to: " << default_gain);
   }
 
   if (this->dataPtr->n_dof_ == 0) {
@@ -191,10 +207,21 @@ bool IgnitionSystem::initSim(
     return false;
   }
 
+  // parameters needed for joint control
+  std::vector<std::string> joint_names;
+  this->param_node_ =
+    rclcpp::Node::make_shared(
+    hardware_info.name,
+    rclcpp::NodeOptions().allow_undeclared_parameters(true));
+  std::vector<rclcpp::Parameter> param_vec;
+
   for (unsigned int j = 0; j < this->dataPtr->n_dof_; j++) {
     auto & joint_info = hardware_info.joints[j];
     std::string joint_name = this->dataPtr->joints_[j].name = joint_info.name;
 
+    // add as one of node parameters
+    joint_names.push_back(joint_name);
+
     ignition::gazebo::Entity simjoint = enableJoints[joint_name];
     this->dataPtr->joints_[j].sim_joint = simjoint;
 
@@ -222,6 +249,147 @@ bool IgnitionSystem::initSim(
       _ecm.CreateComponent(simjoint, ignition::gazebo::components::JointForce());
     }
 
+    const auto * jointAxis =
+      this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
+      this->dataPtr->joints_[
+        j].sim_joint);
+
+    bool use_cascade_control =
+      (hardware_info.joints[j].parameters.find("use_cascade_control") ==
+      hardware_info.joints[j].parameters.end()) ?
+      false :
+      [&]() {
+      if (hardware_info.joints[j].parameters.at("use_cascade_control") == "true" ||
+        hardware_info.joints[j].parameters.at("use_cascade_control") == "True")
+      {
+        return true;
+      } else {
+        return false;
+      }
+    } ();
+
+    param_vec.push_back(
+      rclcpp::Parameter{"mode." + joint_name + ".use_cascade_control",
+        use_cascade_control});
+
+    double upper = jointAxis->Data().Upper();
+    double lower = jointAxis->Data().Lower();
+    double max_velocity = jointAxis->Data().MaxVelocity();
+    double max_effort = jointAxis->Data().Effort();
+
+    double dummy_guess_p_pos = 10 * max_velocity / abs(upper - lower);
+
+    // PID parameters
+    double p_gain_pos =
+      (hardware_info.joints[j].parameters.find(
+        "p_pos") == hardware_info.joints[j].parameters.end()) ?
+      dummy_guess_p_pos :
+      stod(hardware_info.joints[j].parameters.at("p_pos"));
+    double i_gain_pos =
+      (hardware_info.joints[j].parameters.find(
+        "i_pos") == hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("i_pos"));
+    double d_gain_pos =
+      (hardware_info.joints[j].parameters.find(
+        "d_pos") == hardware_info.joints[j].parameters.end()) ?
+      dummy_guess_p_pos / 100.0 :
+      stod(hardware_info.joints[j].parameters.at("d_pos"));
+    // set integral max and min component to 50 percent of the max effort
+    double iMax_pos =
+      (hardware_info.joints[j].parameters.find("iMax_pos") ==
+      hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("iMax_pos"));
+    double iMin_pos =
+      (hardware_info.joints[j].parameters.find("iMin_pos") ==
+      hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("iMin_pos"));
+    double cmdMax_pos =
+      (hardware_info.joints[j].parameters.find("cmdMax_pos") ==
+      hardware_info.joints[j].parameters.end()) ?
+      max_velocity :
+      stod(hardware_info.joints[j].parameters.at("cmdMax_pos"));
+    double cmdMin_pos =
+      (hardware_info.joints[j].parameters.find("cmdMin_pos") ==
+      hardware_info.joints[j].parameters.end()) ?
+      -1.0 * max_velocity :
+      stod(hardware_info.joints[j].parameters.at("cmdMin_pos"));
+    double cmdOffset_pos =
+      (hardware_info.joints[j].parameters.find("cmdOffset_pos") ==
+      hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("cmdOffset_pos"));
+
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_pos", p_gain_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_pos", i_gain_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d_pos", d_gain_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax_pos", iMax_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin_pos", iMin_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax_pos", cmdMax_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin_pos", cmdMin_pos});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_pos", cmdOffset_pos});
+
+    this->dataPtr->joints_[j].pid_pos.Init(
+      p_gain_pos, i_gain_pos, d_gain_pos, iMax_pos, iMin_pos, cmdMax_pos,
+      cmdMin_pos, cmdOffset_pos);
+
+    double p_gain_vel =
+      (hardware_info.joints[j].parameters.find(
+        "p_vel") == hardware_info.joints[j].parameters.end()) ?
+      dummy_guess_p_pos / 100.0 :
+      stod(hardware_info.joints[j].parameters.at("p_vel"));
+    double i_gain_vel =
+      (hardware_info.joints[j].parameters.find(
+        "i_vel") == hardware_info.joints[j].parameters.end()) ?
+      dummy_guess_p_pos / 1000.0 :
+      stod(hardware_info.joints[j].parameters.at("i_vel"));
+    double d_gain_vel =
+      (hardware_info.joints[j].parameters.find(
+        "d_vel") == hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("d_vel"));
+    // set integral max and min component to 50 percent of the max effort
+    double iMax_vel =
+      (hardware_info.joints[j].parameters.find("iMax_vel") ==
+      hardware_info.joints[j].parameters.end()) ?
+      max_effort / 2.0 :
+      stod(hardware_info.joints[j].parameters.at("iMax_vel"));
+    double iMin_vel =
+      (hardware_info.joints[j].parameters.find("iMin_vel") ==
+      hardware_info.joints[j].parameters.end()) ?
+      -1.0 * max_effort / 2.0 :
+      stod(hardware_info.joints[j].parameters.at("iMin_vel"));
+    double cmdMax_vel =
+      (hardware_info.joints[j].parameters.find("cmdMax_vel") ==
+      hardware_info.joints[j].parameters.end()) ?
+      max_effort :
+      stod(hardware_info.joints[j].parameters.at("cmdMax_vel"));
+    double cmdMin_vel =
+      (hardware_info.joints[j].parameters.find("cmdMin_vel") ==
+      hardware_info.joints[j].parameters.end()) ?
+      -1.0 * max_effort :
+      stod(hardware_info.joints[j].parameters.at("cmdMin_vel"));
+    double cmdOffset_vel =
+      (hardware_info.joints[j].parameters.find("cmdOffset_vel") ==
+      hardware_info.joints[j].parameters.end()) ?
+      0.0 :
+      stod(hardware_info.joints[j].parameters.at("cmdOffset_vel"));
+
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".p_vel", p_gain_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".i_vel", i_gain_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".d_vel", d_gain_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMax_vel", iMax_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".iMin_vel", iMin_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMax_vel", cmdMax_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdMin_vel", cmdMin_vel});
+    param_vec.push_back(rclcpp::Parameter{"gains." + joint_name + ".cmdOffset_vel", cmdOffset_vel});
+
+    this->dataPtr->joints_[j].pid_vel.Init(
+      p_gain_vel, i_gain_vel, d_gain_vel, iMax_vel, iMin_vel, cmdMax_vel,
+      cmdMin_vel, cmdOffset_vel);
+
     // Accept this joint and continue configuration
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading joint: " << joint_name);
 
@@ -236,8 +404,7 @@ bool IgnitionSystem::initSim(
           return info.name == mimicked_joint;
         });
       if (mimicked_joint_it == hardware_info.joints.end()) {
-        throw std::runtime_error(
-                std::string("Mimicked joint '") + mimicked_joint + "' not found");
+        throw std::runtime_error(std::string("Mimicked joint '") + mimicked_joint + "' not found");
       }
 
       MimicJoint mimic_joint;
@@ -251,17 +418,30 @@ bool IgnitionSystem::initSim(
         mimic_joint.multiplier = 1.0;
       }
 
+      mimic_joint.mimic_ff_force_scaling =
+        (hardware_info.joints[j].parameters.find("mimic_ff_force_scaling") ==
+        hardware_info.joints[j].parameters.end()) ?
+        mimic_joint.mimic_ff_force_scaling :
+        stod(hardware_info.joints[j].parameters.at("mimic_ff_force_scaling"));
+
       // check joint info of mimicked joint
       auto & joint_info_mimicked = hardware_info.joints[mimic_joint.mimicked_joint_index];
-      const auto state_mimicked_interface = std::find_if(
+      const auto state_mimicked_interface =
+        std::find_if(
         joint_info_mimicked.state_interfaces.begin(), joint_info_mimicked.state_interfaces.end(),
         [&mimic_joint](const hardware_interface::InterfaceInfo & interface_info) {
           bool pos = interface_info.name == "position";
-          if (pos) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION);}
+          if (pos) {
+            mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_POSITION);
+          }
           bool vel = interface_info.name == "velocity";
-          if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY);}
+          if (vel) {
+            mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_VELOCITY);
+          }
           bool eff = interface_info.name == "effort";
-          if (vel) {mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT);}
+          if (vel) {
+            mimic_joint.interfaces_to_mimic.push_back(hardware_interface::HW_IF_EFFORT);
+          }
           return pos || vel || eff;
         });
       if (state_mimicked_interface == joint_info_mimicked.state_interfaces.end()) {
@@ -272,11 +452,10 @@ bool IgnitionSystem::initSim(
                 " ' to mimic");
       }
       RCLCPP_INFO_STREAM(
-        this->nh_->get_logger(),
-        "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint << "' with mutiplier: "
-                  << mimic_joint.multiplier);
+        this->nh_->get_logger(), "Joint '" << joint_name <<
+          "'is mimicking joint '" << mimicked_joint
+                                           << "' with mutiplier: " << mimic_joint.multiplier);
       this->dataPtr->mimic_joints_.push_back(mimic_joint);
-      suffix = "_mimic";
     }
 
     RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
@@ -300,8 +479,7 @@ bool IgnitionSystem::initSim(
       if (joint_info.state_interfaces[i].name == "position") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_POSITION,
+          joint_name + suffix, hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joints_[j].joint_position);
         initial_position = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_position = initial_position;
@@ -309,8 +487,7 @@ bool IgnitionSystem::initSim(
       if (joint_info.state_interfaces[i].name == "velocity") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_VELOCITY,
+          joint_name + suffix, hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joints_[j].joint_velocity);
         initial_velocity = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_velocity = initial_velocity;
@@ -318,8 +495,7 @@ bool IgnitionSystem::initSim(
       if (joint_info.state_interfaces[i].name == "effort") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
         this->dataPtr->state_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_EFFORT,
+          joint_name + suffix, hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joints_[j].joint_effort);
         initial_effort = get_initial_value(joint_info.state_interfaces[i]);
         this->dataPtr->joints_[j].joint_effort = initial_effort;
@@ -333,8 +509,7 @@ bool IgnitionSystem::initSim(
       if (joint_info.command_interfaces[i].name == "position") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t position");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_POSITION,
+          joint_name + suffix, hardware_interface::HW_IF_POSITION,
           &this->dataPtr->joints_[j].joint_position_cmd);
         if (!std::isnan(initial_position)) {
           this->dataPtr->joints_[j].joint_position_cmd = initial_position;
@@ -342,8 +517,7 @@ bool IgnitionSystem::initSim(
       } else if (joint_info.command_interfaces[i].name == "velocity") {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t velocity");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_VELOCITY,
+          joint_name + suffix, hardware_interface::HW_IF_VELOCITY,
           &this->dataPtr->joints_[j].joint_velocity_cmd);
         if (!std::isnan(initial_velocity)) {
           this->dataPtr->joints_[j].joint_velocity_cmd = initial_velocity;
@@ -352,8 +526,7 @@ bool IgnitionSystem::initSim(
         this->dataPtr->joints_[j].joint_control_method |= EFFORT;
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\t\t effort");
         this->dataPtr->command_interfaces_.emplace_back(
-          joint_name + suffix,
-          hardware_interface::HW_IF_EFFORT,
+          joint_name + suffix, hardware_interface::HW_IF_EFFORT,
           &this->dataPtr->joints_[j].joint_effort_cmd);
         if (!std::isnan(initial_effort)) {
           this->dataPtr->joints_[j].joint_effort_cmd = initial_effort;
@@ -369,13 +542,47 @@ bool IgnitionSystem::initSim(
     }
   }
 
+  rclcpp::Parameter joint_names_parameter("joints", joint_names);
+  if (!this->param_node_->has_parameter("joints")) {
+    this->param_node_->set_parameter(joint_names_parameter);
+  }
+  for (const auto & p : param_vec) {
+    if (!this->param_node_->has_parameter(p.get_name())) {
+      this->param_node_->set_parameter(p);
+    }
+  }
+
+  spin_thread_ = std::thread(
+    [this]() {
+      exec_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+      exec_->add_node(this->param_node_);
+
+      while (rclcpp::ok() && !stop_spin_) {
+        exec_->spin_once();
+      }
+      exec_->remove_node(this->param_node_);
+      exec_.reset();
+    });
+
+  try {
+    // Create the parameter listener and get the parameters
+    param_listener_ = std::make_shared<ParamListener>(this->param_node_);
+    params_ = param_listener_->get_params();
+  } catch (const std::exception & e) {
+    fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
+    return false;
+  }
+
+  // update the params
+  param_vec.push_back(joint_names_parameter);
+  param_listener_->update(param_vec);
+
   registerSensors(hardware_info);
 
   return true;
 }
 
-void IgnitionSystem::registerSensors(
-  const hardware_interface::HardwareInfo & hardware_info)
+void IgnitionSystem::registerSensors(const hardware_interface::HardwareInfo & hardware_info)
 {
   // Collect gazebo sensor handles
   size_t n_sensors = hardware_info.sensors.size();
@@ -389,23 +596,22 @@ void IgnitionSystem::registerSensors(
   // So we have resize only once the structures where the data will be stored, and we can safely
   // use pointers to the structures
 
-  this->dataPtr->ecm->Each<ignition::gazebo::components::Imu,
-    ignition::gazebo::components::Name>(
-    [&](const ignition::gazebo::Entity & _entity,
-    const ignition::gazebo::components::Imu *,
-    const ignition::gazebo::components::Name * _name) -> bool
-    {
+  this->dataPtr->ecm->Each<ignition::gazebo::components::Imu, ignition::gazebo::components::Name>(
+    [&](const ignition::gazebo::Entity & _entity, const ignition::gazebo::components::Imu *,
+    const ignition::gazebo::components::Name * _name) -> bool {
       auto imuData = std::make_shared<ImuData>();
-      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Loading sensor: " << _name->Data());
+      RCLCPP_INFO_STREAM(
+        this->nh_->get_logger(),
+        "Loading sensor: " << _name->Data());
 
-      auto sensorTopicComp = this->dataPtr->ecm->Component<
-        ignition::gazebo::components::SensorTopic>(_entity);
+      auto sensorTopicComp =
+      this->dataPtr->ecm->Component<ignition::gazebo::components::SensorTopic>(
+        _entity);
       if (sensorTopicComp) {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data());
       }
 
-      RCLCPP_INFO_STREAM(
-        this->nh_->get_logger(), "\tState:");
+      RCLCPP_INFO_STREAM(this->nh_->get_logger(), "\tState:");
       imuData->name = _name->Data();
       imuData->sim_imu_sensors_ = _entity;
 
@@ -417,15 +623,9 @@ void IgnitionSystem::registerSensors(
       }
 
       static const std::map<std::string, size_t> interface_name_map = {
-        {"orientation.x", 0},
-        {"orientation.y", 1},
-        {"orientation.z", 2},
-        {"orientation.w", 3},
-        {"angular_velocity.x", 4},
-        {"angular_velocity.y", 5},
-        {"angular_velocity.z", 6},
-        {"linear_acceleration.x", 7},
-        {"linear_acceleration.y", 8},
+        {"orientation.x", 0}, {"orientation.y", 1}, {"orientation.z", 2},
+        {"orientation.w", 3}, {"angular_velocity.x", 4}, {"angular_velocity.y", 5},
+        {"angular_velocity.z", 6}, {"linear_acceleration.x", 7}, {"linear_acceleration.y", 8},
         {"linear_acceleration.z", 9},
       };
 
@@ -434,8 +634,7 @@ void IgnitionSystem::registerSensors(
 
         size_t data_index = interface_name_map.at(state_interface.name);
         this->dataPtr->state_interfaces_.emplace_back(
-          imuData->name,
-          state_interface.name,
+          imuData->name, state_interface.name,
           &imuData->imu_sensor_data_[data_index]);
       }
       this->dataPtr->imus_.push_back(imuData);
@@ -443,33 +642,29 @@ void IgnitionSystem::registerSensors(
     });
 }
 
-CallbackReturn
-IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info)
+CallbackReturn IgnitionSystem::on_init(const hardware_interface::HardwareInfo & system_info)
 {
   RCLCPP_WARN(this->nh_->get_logger(), "On init...");
+
   if (hardware_interface::SystemInterface::on_init(system_info) != CallbackReturn::SUCCESS) {
     return CallbackReturn::ERROR;
   }
   return CallbackReturn::SUCCESS;
 }
 
-CallbackReturn IgnitionSystem::on_configure(
-  const rclcpp_lifecycle::State & /*previous_state*/)
+CallbackReturn IgnitionSystem::on_configure(const rclcpp_lifecycle::State & /*previous_state*/)
 {
-  RCLCPP_INFO(
-    this->nh_->get_logger(), "System Successfully configured!");
+  RCLCPP_INFO(this->nh_->get_logger(), "System Successfully configured!");
 
   return CallbackReturn::SUCCESS;
 }
 
-std::vector<hardware_interface::StateInterface>
-IgnitionSystem::export_state_interfaces()
+std::vector<hardware_interface::StateInterface> IgnitionSystem::export_state_interfaces()
 {
   return std::move(this->dataPtr->state_interfaces_);
 }
 
-std::vector<hardware_interface::CommandInterface>
-IgnitionSystem::export_command_interfaces()
+std::vector<hardware_interface::CommandInterface> IgnitionSystem::export_command_interfaces()
 {
   return std::move(this->dataPtr->command_interfaces_);
 }
@@ -482,6 +677,8 @@ CallbackReturn IgnitionSystem::on_activate(const rclcpp_lifecycle::State & previ
 
 CallbackReturn IgnitionSystem::on_deactivate(const rclcpp_lifecycle::State & previous_state)
 {
+  stop_spin_ = true;
+  spin_thread_.join();
   return CallbackReturn::SUCCESS;
   return hardware_interface::SystemInterface::on_deactivate(previous_state);
 }
@@ -509,18 +706,22 @@ hardware_interface::return_type IgnitionSystem::read(
 
     this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0];
     this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0];
-    // this->dataPtr->joint_effort_[j] = jointForce->Data()[0];
+    // set effort state interface to computed/propagated effort command
+    // - passthrough because of ignitionrobotics/ign-physics#124
+    this->dataPtr->joints_[i].joint_effort = this->dataPtr->joints_[i].joint_effort_cmd;
   }
 
   for (unsigned int i = 0; i < this->dataPtr->imus_.size(); ++i) {
     if (this->dataPtr->imus_[i]->topicName.empty()) {
-      auto sensorTopicComp = this->dataPtr->ecm->Component<
-        ignition::gazebo::components::SensorTopic>(this->dataPtr->imus_[i]->sim_imu_sensors_);
+      auto sensorTopicComp =
+        this->dataPtr->ecm->Component<ignition::gazebo::components::SensorTopic>(
+        this->dataPtr->imus_[i]->sim_imu_sensors_);
       if (sensorTopicComp) {
         this->dataPtr->imus_[i]->topicName = sensorTopicComp->Data();
         RCLCPP_INFO_STREAM(
-          this->nh_->get_logger(), "IMU " << this->dataPtr->imus_[i]->name <<
-            " has a topic name: " << sensorTopicComp->Data());
+          this->nh_->get_logger(),
+          "IMU " << this->dataPtr->imus_[i]->name << " has a topic name: " <<
+            sensorTopicComp->Data());
 
         this->dataPtr->node.Subscribe(
           this->dataPtr->imus_[i]->topicName, &ImuData::OnIMU,
@@ -539,17 +740,17 @@ IgnitionSystem::perform_command_mode_switch(
   for (unsigned int j = 0; j < this->dataPtr->joints_.size(); j++) {
     for (const std::string & interface_name : stop_interfaces) {
       // Clear joint control method bits corresponding to stop interfaces
-      if (interface_name == (this->dataPtr->joints_[j].name + "/" +
-        hardware_interface::HW_IF_POSITION))
+      if (interface_name ==
+        (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION))
       {
         this->dataPtr->joints_[j].joint_control_method &=
           static_cast<ControlMethod_>(VELOCITY & EFFORT);
-      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
+      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" +  // NOLINT
         hardware_interface::HW_IF_VELOCITY))
       {
         this->dataPtr->joints_[j].joint_control_method &=
           static_cast<ControlMethod_>(POSITION & EFFORT);
-      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
+      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" +  // NOLINT
         hardware_interface::HW_IF_EFFORT))
       {
         this->dataPtr->joints_[j].joint_control_method &=
@@ -559,15 +760,15 @@ IgnitionSystem::perform_command_mode_switch(
 
     // Set joint control method bits corresponding to start interfaces
     for (const std::string & interface_name : start_interfaces) {
-      if (interface_name == (this->dataPtr->joints_[j].name + "/" +
-        hardware_interface::HW_IF_POSITION))
+      if (interface_name ==
+        (this->dataPtr->joints_[j].name + "/" + hardware_interface::HW_IF_POSITION))
       {
         this->dataPtr->joints_[j].joint_control_method |= POSITION;
-      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
+      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" +  // NOLINT
         hardware_interface::HW_IF_VELOCITY))
       {
         this->dataPtr->joints_[j].joint_control_method |= VELOCITY;
-      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" + // NOLINT
+      } else if (interface_name == (this->dataPtr->joints_[j].name + "/" +  // NOLINT
         hardware_interface::HW_IF_EFFORT))
       {
         this->dataPtr->joints_[j].joint_control_method |= EFFORT;
@@ -580,42 +781,155 @@ IgnitionSystem::perform_command_mode_switch(
 
 hardware_interface::return_type IgnitionSystem::write(
   const rclcpp::Time & /*time*/,
-  const rclcpp::Duration & /*period*/)
+  const rclcpp::Duration & period)
 {
+  // refresh params
+  param_listener_->refresh_dynamic_parameters();
+  params_ = param_listener_->get_params();
+
   for (unsigned int i = 0; i < this->dataPtr->joints_.size(); ++i) {
+    // assuming every joint has axis
+    const auto * jointAxis =
+      this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
+      this->dataPtr->joints_[
+        i].sim_joint);
+
+    // update PIDs
+    this->dataPtr->joints_[i].pid_pos.SetPGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].p_pos);
+    this->dataPtr->joints_[i].pid_pos.SetIGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].i_pos);
+    this->dataPtr->joints_[i].pid_pos.SetDGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].d_pos);
+    this->dataPtr->joints_[i].pid_pos.SetIMax(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].iMax_pos);
+    this->dataPtr->joints_[i].pid_pos.SetIMin(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].iMin_pos);
+    this->dataPtr->joints_[i].pid_pos.SetCmdMax(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].cmdMax_pos);
+    this->dataPtr->joints_[i].pid_pos.SetCmdMin(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].cmdMin_pos);
+    this->dataPtr->joints_[i].pid_pos.SetCmdOffset(
+      params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdOffset_pos);
+
+    this->dataPtr->joints_[i].pid_vel.SetPGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].p_vel);
+    this->dataPtr->joints_[i].pid_vel.SetIGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].i_vel);
+    this->dataPtr->joints_[i].pid_vel.SetDGain(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].d_vel);
+    this->dataPtr->joints_[i].pid_vel.SetIMax(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].iMax_vel);
+    this->dataPtr->joints_[i].pid_vel.SetIMin(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].iMin_vel);
+    this->dataPtr->joints_[i].pid_vel.SetCmdMax(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].cmdMax_vel);
+    this->dataPtr->joints_[i].pid_vel.SetCmdMin(
+      params_.gains.joints_map[this->dataPtr->joints_[i].
+      name].cmdMin_vel);
+    this->dataPtr->joints_[i].pid_vel.SetCmdOffset(
+      params_.gains.joints_map[this->dataPtr->joints_[i].name].cmdOffset_vel);
+
     if (this->dataPtr->joints_[i].joint_control_method & VELOCITY) {
-      if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
-          this->dataPtr->joints_[i].sim_joint))
-      {
+      double velocity = this->dataPtr->joints_[i].joint_velocity;
+      double velocity_cmd_clamped = std::clamp(
+        this->dataPtr->joints_[i].joint_velocity_cmd,
+        -1.0 * jointAxis->Data().MaxVelocity(), jointAxis->Data().MaxVelocity());
+
+      double velocity_error = velocity - velocity_cmd_clamped;
+
+      // calculate target force/torque - output of inner pid
+      double target_force = this->dataPtr->joints_[i].pid_vel.Update(
+        velocity_error,
+        std::chrono::duration<double>(period.to_chrono<std::chrono::nanoseconds>()));
+
+      // remember for potential effort state interface
+      this->dataPtr->joints_[i].joint_effort_cmd = target_force;
+
+      auto forceCmd = this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
+        this->dataPtr->joints_[i].sim_joint);
+
+      if (forceCmd == nullptr) {
         this->dataPtr->ecm->CreateComponent(
           this->dataPtr->joints_[i].sim_joint,
-          ignition::gazebo::components::JointVelocityCmd({0}));
+          ignition::gazebo::components::JointForceCmd({target_force}));
       } else {
-        const auto jointVelCmd =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
-          this->dataPtr->joints_[i].sim_joint);
-        *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
-          {this->dataPtr->joints_[i].joint_velocity_cmd});
+        *forceCmd = ignition::gazebo::components::JointForceCmd({target_force});
       }
     } else if (this->dataPtr->joints_[i].joint_control_method & POSITION) {
-      // Get error in position
-      double error;
-      error = (this->dataPtr->joints_[i].joint_position -
-        this->dataPtr->joints_[i].joint_position_cmd) * *this->dataPtr->update_rate;
+      // calculate error with clamped position command
+      double position = this->dataPtr->joints_[i].joint_position;
+      double position_cmd_clamped = std::clamp(
+        this->dataPtr->joints_[i].joint_position_cmd, jointAxis->Data().Lower(),
+        jointAxis->Data().Upper());
+
+      double position_error = position - position_cmd_clamped;
+
+      double position_error_sign = copysign(1.0, position_error);
+
+      double position_error_abs_clamped =
+        std::clamp(
+        std::abs(position_error), 0.0,
+        std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()));
+
+      // move forward with calculated position error
+      position_error = position_error_sign * position_error_abs_clamped;
 
-      // Calculate target velcity
-      double target_vel = -this->dataPtr->position_proportional_gain_ * error;
+      double position_or_velocity_error = 0.0;
 
-      auto vel =
-        this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+      // check if cascade control is used for this joint
+      if (params_.mode.joints_map[this->dataPtr->joints_[i].name].use_cascade_control) {
+        // calculate target velocity - output of outer pid - input to inner pid
+        double target_vel = this->dataPtr->joints_[i].pid_pos.Update(
+          position_error, std::chrono::duration<double>(
+            period.to_chrono<std::chrono::nanoseconds>()));
+
+        double velocity_error =
+          this->dataPtr->joints_[i].joint_velocity -
+          std::clamp(
+          target_vel, -1.0 * jointAxis->Data().MaxVelocity(),
+          jointAxis->Data().MaxVelocity());
+
+        // prepare velocity error value for inner pid
+        position_or_velocity_error = velocity_error;
+      } else {
+        // prepare velocity error value for inner pid
+        position_or_velocity_error = position_error;
+      }
+
+      // calculate target force/torque - output of inner pid
+      double target_force = this->dataPtr->joints_[i].pid_vel.Update(
+        position_or_velocity_error,
+        std::chrono::duration<double>(period.to_chrono<std::chrono::nanoseconds>()));
+
+      // round the force
+      target_force = round(target_force * 10000.0) / 10000.0;
+
+      // remember for potential effort state interface
+      this->dataPtr->joints_[i].joint_effort_cmd = target_force;
+
+      auto forceCmd = this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
         this->dataPtr->joints_[i].sim_joint);
 
-      if (vel == nullptr) {
+      if (forceCmd == nullptr) {
         this->dataPtr->ecm->CreateComponent(
           this->dataPtr->joints_[i].sim_joint,
-          ignition::gazebo::components::JointVelocityCmd({target_vel}));
-      } else if (!vel->Data().empty()) {
-        vel->Data()[0] = target_vel;
+          ignition::gazebo::components::JointForceCmd({target_force}));
+      } else {
+        *forceCmd = ignition::gazebo::components::JointForceCmd({target_force});
       }
     } else if (this->dataPtr->joints_[i].joint_control_method & EFFORT) {
       if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
@@ -631,97 +945,164 @@ hardware_interface::return_type IgnitionSystem::write(
         *jointEffortCmd = ignition::gazebo::components::JointForceCmd(
           {this->dataPtr->joints_[i].joint_effort_cmd});
       }
-    } else {
-      // Fallback case is a velocity command of zero
-      double target_vel = 0.0;
-      auto vel =
-        this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
-        this->dataPtr->joints_[i].sim_joint);
-
-      if (vel == nullptr) {
-        this->dataPtr->ecm->CreateComponent(
-          this->dataPtr->joints_[i].sim_joint,
-          ignition::gazebo::components::JointVelocityCmd({target_vel}));
-      } else if (!vel->Data().empty()) {
-        vel->Data()[0] = target_vel;
-      }
     }
-  }
 
-  // set values of all mimic joints with respect to mimicked joint
-  for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
-    for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) {
-      if (mimic_interface == "position") {
-        // Get the joint position
-        double position_mimicked_joint =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
-          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
-
-        double position_mimic_joint =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointPosition>(
-          this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)->Data()[0];
-
-        double position_error =
-          position_mimic_joint - position_mimicked_joint * mimic_joint.multiplier;
-
-        double velocity_sp = (-1.0) * position_error * (*this->dataPtr->update_rate);
-
-        auto vel =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+    // set values of all mimic joints with respect to mimicked joint
+    for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
+      for (const auto & mimic_interface : mimic_joint.interfaces_to_mimic) {
+        // assuming every mimic joint has axis
+        const auto * jointAxis =
+          this->dataPtr->ecm->Component<ignition::gazebo::components::JointAxis>(
           this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
 
-        if (vel == nullptr) {
-          this->dataPtr->ecm->CreateComponent(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
-            ignition::gazebo::components::JointVelocityCmd({velocity_sp}));
-        } else if (!vel->Data().empty()) {
-          vel->Data()[0] = velocity_sp;
-        }
-      }
-      if (mimic_interface == "velocity") {
-        // get the velocity of mimicked joint
-        double velocity_mimicked_joint =
-          this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocity>(
-          this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)->Data()[0];
-
-        if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
-        {
-          this->dataPtr->ecm->CreateComponent(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
-            ignition::gazebo::components::JointVelocityCmd({0}));
-        } else {
-          const auto jointVelCmd =
-            this->dataPtr->ecm->Component<ignition::gazebo::components::JointVelocityCmd>(
+        if (mimic_interface == "position") {
+          // Get the joint position
+          double position_mimicked_joint = this->dataPtr->ecm
+            ->Component<ignition::gazebo::components::JointPosition>(
+            this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)
+            ->Data()[0];
+
+          double position_mimic_joint = this->dataPtr->ecm
+            ->Component<ignition::gazebo::components::JointPosition>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)
+            ->Data()[0];
+
+          double position_target_from_mimicked_joint = std::clamp(
+            position_mimicked_joint * mimic_joint.multiplier,
+            jointAxis->Data().Lower(), jointAxis->Data().Upper());
+
+          double position_error = position_mimic_joint - position_target_from_mimicked_joint;
+
+          // round position error for simulation stability
+          position_error = round(position_error * 10000.0) / 10000.0;
+
+          double position_error_sign = copysign(1.0, position_error);
+
+          double position_error_abs_clamped = std::clamp(
+            std::abs(position_error), 0.0,
+            std::abs(jointAxis->Data().Upper() - jointAxis->Data().Lower()));
+
+          position_error = position_error_sign * position_error_abs_clamped;
+
+          double position_or_velocity_error = 0.0;
+
+          // check if cascade control is used for this joint
+          if (params_.mode.joints_map[this->dataPtr->joints_[mimic_joint.joint_index].name].
+            use_cascade_control)
+          {
+            // calculate target velocity - output of outer pid - input to inner pid
+            double target_vel = this->dataPtr->joints_[mimic_joint.joint_index].pid_pos.Update(
+              position_error,
+              std::chrono::duration<double>(period.to_chrono<std::chrono::nanoseconds>()));
+
+            // get mimic joint velocity
+            double velocity_mimic_joint = this->dataPtr->ecm
+              ->Component<ignition::gazebo::components::JointVelocity>(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)
+              ->Data()[0];
+
+            position_or_velocity_error =
+              velocity_mimic_joint -
+              std::clamp(
+              target_vel, -1.0 * jointAxis->Data().MaxVelocity(),
+              jointAxis->Data().MaxVelocity());
+          } else {
+            position_or_velocity_error = position_error;
+          }
+
+          // set command offset - feed forward term added to the pid output
+          // that is clamped by pid max command value
+          // while taking into account mimic multiplier
+          this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
+            mimic_joint.multiplier *
+            this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd);
+
+          // calculate target force/torque - output of inner pid
+          double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
+            position_or_velocity_error,
+            std::chrono::duration<double>(period.to_chrono<std::chrono::nanoseconds>()));
+
+          // round force value for simulation stability
+          target_force = round(target_force * 10000.0) / 10000.0;
+
+          // remember for potential effort state interface
+          this->dataPtr->joints_[mimic_joint.joint_index].joint_effort_cmd = target_force;
+
+          auto forceCmd =
+            this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
             this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
-          *jointVelCmd = ignition::gazebo::components::JointVelocityCmd(
-            {mimic_joint.multiplier * velocity_mimicked_joint});
+
+          if (forceCmd == nullptr) {
+            this->dataPtr->ecm->CreateComponent(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+              ignition::gazebo::components::JointForceCmd({target_force}));
+          } else {
+            *forceCmd = ignition::gazebo::components::JointForceCmd({target_force});
+          }
         }
-      }
-      if (mimic_interface == "effort") {
-        // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
-        // Get the joint force
-        // const auto * jointForce =
-        //   _ecm.Component<ignition::gazebo::components::JointForce>(
-        //   this->dataPtr->sim_joints_[j]);
-        if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
-        {
-          this->dataPtr->ecm->CreateComponent(
-            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
-            ignition::gazebo::components::JointForceCmd({0}));
-        } else {
-          const auto jointEffortCmd =
+        if (mimic_interface == "velocity") {
+          // get the velocity of mimicked joint
+          double velocity_mimicked_joint = this->dataPtr->ecm
+            ->Component<ignition::gazebo::components::JointVelocity>(
+            this->dataPtr->joints_[mimic_joint.mimicked_joint_index].sim_joint)
+            ->Data()[0];
+
+          // get mimic joint velocity
+          double velocity_mimic_joint = this->dataPtr->ecm
+            ->Component<ignition::gazebo::components::JointVelocity>(
+            this->dataPtr->joints_[mimic_joint.joint_index].sim_joint)
+            ->Data()[0];
+
+          double velocity_error = velocity_mimic_joint - std::clamp(
+            mimic_joint.multiplier * velocity_mimicked_joint,
+            -1.0 * jointAxis->Data().MaxVelocity(),
+            jointAxis->Data().MaxVelocity());
+
+          this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.SetCmdOffset(
+            mimic_joint.multiplier *
+            this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd);
+
+          // calculate target force/torque - output of inner pid
+          double target_force = this->dataPtr->joints_[mimic_joint.joint_index].pid_vel.Update(
+            velocity_error,
+            std::chrono::duration<double>(period.to_chrono<std::chrono::nanoseconds>()));
+
+          auto forceCmd =
             this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
             this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
-          *jointEffortCmd = ignition::gazebo::components::JointForceCmd(
-            {mimic_joint.multiplier *
-              this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort});
+
+          if (forceCmd == nullptr) {
+            this->dataPtr->ecm->CreateComponent(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+              ignition::gazebo::components::JointForceCmd({target_force}));
+          } else {
+            *forceCmd = ignition::gazebo::components::JointForceCmd({target_force});
+          }
+        }
+        if (mimic_interface == "effort") {
+          // TODO(ahcorde): Revisit this part ignitionrobotics/ign-physics#124
+          // Get the joint force
+          // const auto * jointForce =
+          //   _ecm.Component<ignition::gazebo::components::JointForce>(
+          //   this->dataPtr->sim_joints_[j]);
+          if (!this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint))
+          {
+            this->dataPtr->ecm->CreateComponent(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint,
+              ignition::gazebo::components::JointForceCmd({0}));
+          } else {
+            const auto jointEffortCmd =
+              this->dataPtr->ecm->Component<ignition::gazebo::components::JointForceCmd>(
+              this->dataPtr->joints_[mimic_joint.joint_index].sim_joint);
+            *jointEffortCmd = ignition::gazebo::components::JointForceCmd(
+              {mimic_joint.multiplier *
+                this->dataPtr->joints_[mimic_joint.mimicked_joint_index].joint_effort_cmd});
+          }
         }
       }
     }
   }
-
   return hardware_interface::return_type::OK;
 }
 }  // namespace ign_ros2_control
diff --git a/ign_ros2_control_demos/config/gripper_controller.yaml b/ign_ros2_control_demos/config/gripper_controller.yaml
index d72296e1..0f5ba64d 100644
--- a/ign_ros2_control_demos/config/gripper_controller.yaml
+++ b/ign_ros2_control_demos/config/gripper_controller.yaml
@@ -5,6 +5,9 @@ controller_manager:
     gripper_controller:
       type: forward_command_controller/ForwardCommandController
 
+    gripper_action_controller:
+      type: position_controllers/GripperActionController
+
     joint_state_broadcaster:
       type: joint_state_broadcaster/JointStateBroadcaster
 
diff --git a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py
index bf12d3d8..84385598 100644
--- a/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py
+++ b/ign_ros2_control_demos/launch/gripper_mimic_joint_example.launch.py
@@ -75,6 +75,13 @@ def generate_launch_description():
         output='screen'
     )
 
+    # load action controller but don't start it
+    load_gripper_action_controller = ExecuteProcess(
+        cmd=['ros2', 'control', 'load_controller', '--set-state', 'configured',
+             'gripper_action_controller'],
+        output='screen'
+    )
+
     return LaunchDescription([
         # Launch gazebo environment
         IncludeLaunchDescription(
@@ -94,6 +101,12 @@ def generate_launch_description():
                 on_exit=[load_gripper_controller],
             )
         ),
+        RegisterEventHandler(
+            event_handler=OnProcessExit(
+                target_action=load_gripper_controller,
+                on_exit=[load_gripper_action_controller],
+            )
+        ),
         node_robot_state_publisher,
         ignition_spawn_entity,
         # Launch Arguments
diff --git a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf
index 6fa0af31..8c0164a4 100644
--- a/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_cart_effort.xacro.urdf
@@ -40,7 +40,7 @@
     <parent link="slideBar"/>
     <child link="cart"/>
     <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
-    <dynamics damping="0.0" friction="0.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
   </joint>
 
   <ros2_control name="IgnitionSystem" type="system">
diff --git a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf
index eeff47ec..5a818074 100644
--- a/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_cart_position.xacro.urdf
@@ -40,7 +40,7 @@
     <parent link="slideBar"/>
     <child link="cart"/>
     <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
-    <dynamics damping="0.0" friction="0.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
   </joint>
 
   <ros2_control name="IgnitionSystem" type="system">
@@ -57,6 +57,11 @@
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
+      <param name="p_vel">1000.0</param>
+      <param name="i_vel">100.0</param>
+      <param name="d_vel">100.0</param>
+      <param name="iMax_vel">5.0</param>
+      <param name="iMin_vel">-5.0</param>
     </joint>
   </ros2_control>
 
diff --git a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
index 59a960a1..4ba9e2b8 100644
--- a/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_cart_velocity.xacro.urdf
@@ -73,7 +73,7 @@
     <parent link="slideBar"/>
     <child link="cart"/>
     <limit effort="1000.0" lower="-15" upper="15" velocity="30"/>
-    <dynamics damping="0.0" friction="0.0"/>
+    <dynamics damping="0.5" friction="0.0"/>
   </joint>
   <link name="imu">
     <inertial>
@@ -123,6 +123,7 @@
       <state_interface name="position"/>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
+      <param name="p_vel">100.0</param>
     </joint>
     <sensor name="cart_imu_sensor">
       <state_interface name="orientation.x"/>
diff --git a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf
index e608ed9e..071f3ad3 100644
--- a/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_diff_drive.xacro.urdf
@@ -36,6 +36,7 @@
     <child link="left_wheel"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.2"/>
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <!-- left wheel Link -->
@@ -70,6 +71,7 @@
     <child link="right_wheel"/>
     <axis xyz="0 0 1"/>
     <dynamics damping="0.2"/>
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <!-- right wheel Link -->
@@ -143,6 +145,7 @@
       </command_interface>
       <state_interface name="position"/>
       <state_interface name="velocity"/>
+      <param name="p_vel">1000.0</param>
     </joint>
     <joint name="right_wheel_joint">
       <command_interface name="velocity">
@@ -151,6 +154,7 @@
       </command_interface>
       <state_interface name="position"/>
       <state_interface name="velocity"/>
+      <param name="p_vel">1000.0</param>
     </joint>
   </ros2_control>
 
diff --git a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf
index 8e0074f4..f15ae91e 100644
--- a/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_gripper_mimic_joint.xacro.urdf
@@ -57,6 +57,7 @@
     <parent link="base"/>
     <child link="finger_right"/>
     <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    <dynamics damping="3.0" />
   </joint>
   <joint name="left_finger_joint" type="prismatic">
     <mimic joint="right_finger_joint"/>
@@ -65,6 +66,7 @@
     <parent link="base"/>
     <child link="finger_left"/>
     <limit effort="1000.0" lower="0" upper="0.38" velocity="10"/>
+    <dynamics damping="3.0" />
   </joint>
   <ros2_control name="GripperIgnition" type="system">
     <hardware>
@@ -77,14 +79,22 @@
       </state_interface>
       <state_interface name="velocity"/>
       <state_interface name="effort"/>
-    </joint>
-    <joint name="left_finger_joint">
-      <param name="mimic">right_finger_joint</param>
-      <param name="multiplier">1</param>
-      <command_interface name="position"/>
-      <state_interface name="position"/>
-      <state_interface name="velocity"/>
-      <state_interface name="effort"/>
+      <param name="p_pos">100.0</param>
+      <param name="i_pos">0.0</param>
+      <param name="d_pos">20.0</param>
+      <param name="iMax_pos">5.0</param>
+      <param name="iMin_pos">-5.0</param>
+      <param name="cmdMax_pos">10.0</param>
+      <param name="cmdMin_pos">-10.0</param>
+      <param name="cmdOffset_pos">0.0</param>
+      <param name="p_vel">10.0</param>
+      <param name="i_vel">1.0</param>
+      <param name="d_vel">0.0</param>
+      <param name="iMax_vel">50</param>
+      <param name="iMin_vel">-50</param>
+      <param name="cmdMax_vel">100</param>
+      <param name="cmdMin_vel">-100</param>
+      <param name="cmdOffset_vel">0.0</param>
     </joint>
   </ros2_control>
 
@@ -92,5 +102,23 @@
     <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
       <parameters>$(find ign_ros2_control_demos)/config/gripper_controller.yaml</parameters>
     </plugin>
+    <plugin filename="mimic-joint-system" name="ign_ros2_control::MimicJointSystem">
+      <joint_name>left_finger_joint</joint_name>
+      <mimic_joint_name>right_finger_joint</mimic_joint_name>
+      <multiplier>1.0</multiplier>
+      <offset>0.0</offset>
+      <joint_index>0</joint_index>
+      <mimic_joint_index>0</mimic_joint_index>
+      <p_gain>500.0</p_gain>
+      <i_gain>0.1</i_gain>
+      <d_gain>100.0</d_gain>
+      <i_max>50.0</i_max>
+      <i_min>-50.0</i_min>
+      <cmd_max>100.0</cmd_max>
+      <cmd_min>-100.0</cmd_min>
+      <cmd_offset>0.0</cmd_offset>
+      <dead_zone>0.001</dead_zone>
+      <use_velocity_commands>false</use_velocity_commands>
+    </plugin>
   </gazebo>
 </robot>
diff --git a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf
index 7d58323d..4bf34eee 100644
--- a/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf
+++ b/ign_ros2_control_demos/urdf/test_tricycle_drive.xacro.urdf
@@ -71,6 +71,7 @@
     <child link="left_wheel" />
     <axis xyz="0 0 1" />
     <dynamics damping="0.2" />
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <!-- right wheel Link -->
@@ -99,6 +100,7 @@
     <child link="right_wheel" />
     <axis xyz="0 0 1" />
     <dynamics damping="0.2" />
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <!-- Steering Link -->
@@ -121,6 +123,8 @@
     <parent link="chassis" />
     <child link="steering_link" />
     <axis xyz="0 0 1" />
+    <dynamics damping="0.2" />
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <!-- traction wheel link -->
@@ -148,6 +152,8 @@
     <child link="wheel_front_link" />
     <origin xyz="0 0 0" rpy="-1.57 1.57 0" />
     <axis xyz="0 0 1" />
+    <dynamics damping="0.2" />
+    <limit effort="1000.0" velocity="30"/>
   </joint>
 
   <ros2_control name="IgnitionSystem" type="system">
@@ -157,11 +163,14 @@
     <joint name="steering_joint">
       <command_interface name="position" />
       <state_interface name="position" />
+      <param name="p_pos">10.0</param>
+      <param name="p_vel">100.0</param>
     </joint>
     <joint name="traction_joint">
       <command_interface name="velocity" />
       <state_interface name="velocity" />
       <state_interface name="position" />
+      <param name="p_vel">100.0</param>
     </joint>
   </ros2_control>