diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc
index 619717d7..d3ed9d36 100644
--- a/ign_ros2_control/src/MimicJointSystem.cc
+++ b/ign_ros2_control/src/MimicJointSystem.cc
@@ -1,9 +1,11 @@
 #include "ign_ros2_control/MimicJointSystem.hh"
 
-#include "ignition/gazebo/Model.hh"
+#include "ignition/gazebo/components/Joint.hh"
 #include "ignition/gazebo/components/JointForceCmd.hh"
 #include "ignition/gazebo/components/JointVelocityCmd.hh"
 #include "ignition/gazebo/components/JointPosition.hh"
+#include "ignition/gazebo/Model.hh"
+#include "ignition/gazebo/Util.hh"
 
 #include <ignition/math/PID.hh>
 
@@ -12,194 +14,425 @@
 #include <ignition/plugin/Register.hh>
 
 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)
+  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]
 
 using namespace ign_ros2_control;
 
-class ign_ros2_control::MimicJointSystemPrivate{
+class ign_ros2_control::MimicJointSystemPrivate {
 
 /// \brief Joint Entity
-public: ignition::gazebo::Entity jointEntity{ignition::gazebo::kNullEntity};
 
-          /// \brief ECM pointer
-      ignition::gazebo::EntityComponentManager * ecm{nullptr};
+public:
+  ignition::gazebo::Entity jointEntity {ignition::gazebo::kNullEntity};
+
+public:
+  ignition::gazebo::Entity mimicJointEntity {ignition::gazebo::kNullEntity};
+
+  /// \brief ECM pointer
+  ignition::gazebo::EntityComponentManager * ecm {nullptr};
 
 /// \brief Joint name
-public: std::string jointName;
 
-public: std::string mimicJointName;
+public:
+  std::string jointName;
+
+public:
+  std::string mimicJointName;
 
 /// \brief Commanded joint position
-public: double jointPosCmd{0.0};
 
-public: double multiplier{1.0};
+public:
+  double jointPosCmd {0.0};
+
+public:
+  double multiplier {1.0};
 
-public: double offset{0.0};
+public:
+  double offset {0.0};
 
 /// \brief Model interface
-public: ignition::gazebo::Model model{ignition::gazebo::kNullEntity};
+
+public:
+  ignition::gazebo::Model model {ignition::gazebo::kNullEntity};
 
 /// \brief Position PID controller.
-public: ignition::math::PID posPid;
+
+public:
+  ignition::math::PID posPid;
 
 /// \brief Joint index to be used.
-public: unsigned int jointIndex = 0u;
+
+public:
+  unsigned int jointIndex = 0u;
+
+public:
+  unsigned int mimicJointIndex = 0u;
+
+public:
+  double deadZone = 1e-6;
 
 /// \brief Operation modes
-enum OperationMode
-{
+  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;
+
+public:
+  OperationMode mode = OperationMode::PID;
 
 };
 
-MimicJointSystem::MimicJointSystem(): dataPtr(std::make_unique<ign_ros2_control::MimicJointSystemPrivate>())
+MimicJointSystem::MimicJointSystem()
+: dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ())
 {
 }
 
 MimicJointSystem::~MimicJointSystem()
 {
-    this->dataPtr.reset();
+  this->dataPtr.reset();
 }
 
-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
-    const auto model = ignition::gazebo::Model(_entity);
-    if (!model.Valid(_ecm)) {
+void MimicJointSystem::Configure(
+  const ignition::gazebo::Entity & _entity,
+  const std::shared_ptr < const sdf::Element > & _sdf,
+  ignition::gazebo::EntityComponentManager & _ecm,
+  ignition::gazebo::EventManager & _eventMgr)
+{
 
-        ignerr << "MimicJointSystem  Failed to initialize because ["<<model.Name(_ecm) <<"] (Entity="<<_entity <<")] is not a model.. ";
-        return;
+  // Make sure the controller is attached to a valid model
+  const auto model = ignition::gazebo::Model(_entity);
+  if (!model.Valid(_ecm)) {
+
+    ignerr << "MimicJointSystem  Failed to initialize because [" << model.Name(_ecm) <<
+      "] (Entity=" << _entity << ")] is not a model.. ";
+    return;
+  }
+
+  this->dataPtr->ecm = &_ecm;
+
+  // 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 < unsigned int > ("multiplier");
+  }
+
+  if (_sdf->HasElement("offset")) {
+    this->dataPtr->offset = _sdf->Get < unsigned int > ("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("dead_zone")->Get < double > ();
+  }
+
+  // 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->ecm = &_ecm;
+  this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
 
-    // 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;
-    }
+  if (_sdf->HasElement("initial_position")) {
+    this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position");
+  }
 
-    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<unsigned int>("multiplier");
-    }
+  igndbg << "[JointPositionController] system parameters:" << std::endl;
+  igndbg << "p_gain: [" << p << "]" << std::endl;
+  igndbg << "i_gain: [" << i << "]" << std::endl;
+  igndbg << "d_gain: [" << d << "]" << std::endl;
+  igndbg << "i_max: [" << iMax << "]" << std::endl;
+  igndbg << "i_min: [" << iMin << "]" << std::endl;
+  igndbg << "cmd_max: [" << cmdMax << "]" << std::endl;
+  igndbg << "cmd_min: [" << cmdMin << "]" << std::endl;
+  igndbg << "cmd_offset: [" << cmdOffset << "]" << std::endl;
+  igndbg << "initial_position: [" << this->dataPtr->jointPosCmd << "]"
+         << std::endl;
 
-    if (_sdf->HasElement("offset"))
-    {
-        this->dataPtr->offset = _sdf->Get<unsigned int>("offset");
-    }
 
-    if (_sdf->HasElement("joint_index"))
-    {
-        this->dataPtr->jointIndex = _sdf->Get<unsigned int>("joint_index");
-    }
+}
 
-    // 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");
+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 (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) {
+
+    auto entities = entitiesFromScopedName(
+      this->dataPtr->jointName, _ecm, this->dataPtr->model.Entity());
+
+    if (!entities.empty()) {
+      if (entities.size() > 1) {
+        ignwarn << "Multiple joint entities with name ["
+                << this->dataPtr->jointName << "] found. "
+                << "Using the first one.\n";
+      }
+      this->dataPtr->jointEntity = *entities.begin();
+
+      // Validate
+      if (!_ecm.EntityHasComponentType(
+          this->dataPtr->jointEntity,
+          ignition::gazebo::components::Joint::typeId))
+      {
+        ignerr << "Entity with name[" << this->dataPtr->jointName
+               << "] is not a joint\n";
+        this->dataPtr->jointEntity = ignition::gazebo::kNullEntity;
+      } else {
+        igndbg << "Identified joint [" << this->dataPtr->jointName
+               << "] as Entity [" << this->dataPtr->jointEntity << "]\n";
+      }
     }
-    if (_sdf->HasElement("cmd_max"))
-    {
-        cmdMax = _sdf->Get<double>("cmd_max");
+  }
+
+
+  if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) {
+    return;
+  }
+
+  if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) {
+
+    auto entities = entitiesFromScopedName(
+      this->dataPtr->mimicJointName, _ecm, this->dataPtr->model.Entity());
+
+    if (!entities.empty()) {
+      if (entities.size() > 1) {
+        ignwarn << "Multiple joint entities with name ["
+                << this->dataPtr->jointName << "] found. "
+                << "Using the first one.\n";
+      }
+      this->dataPtr->mimicJointEntity = *entities.begin();
+
+      // Validate
+      if (!_ecm.EntityHasComponentType(
+          this->dataPtr->mimicJointEntity,
+          ignition::gazebo::components::Joint::typeId))
+      {
+        ignerr << "Entity with name[" << this->dataPtr->mimicJointName
+               << "] is not a joint\n";
+        this->dataPtr->mimicJointEntity = ignition::gazebo::kNullEntity;
+      } else {
+        igndbg << "Identified joint [" << this->dataPtr->mimicJointName
+               << "] as Entity [" << this->dataPtr->mimicJointEntity << "]\n";
+      }
     }
-    if (_sdf->HasElement("cmd_min"))
-    {
-        cmdMin = _sdf->Get<double>("cmd_min");
+  }
+
+
+  if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) {
+    return;
+  }
+
+  // Nothing left to do if paused.
+  if (_info.paused) {
+    return;
+  }
+
+  // Create joint position component if one doesn't exist
+  auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > (
+    this->dataPtr->jointEntity);
+  if (!jointPosComp) {
+    _ecm.CreateComponent(
+      this->dataPtr->jointEntity,
+      ignition::gazebo::components::JointPosition());
+  }
+
+  // Create joint position component if one doesn't exist
+  auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition > (
+    this->dataPtr->mimicJointEntity);
+  if (!mimicJointPosComp) {
+    _ecm.CreateComponent(
+      this->dataPtr->mimicJointEntity,
+      ignition::gazebo::components::JointPosition());
+  }
+
+  // We just created the joint position component, give one iteration for the
+  // physics system to update its size
+  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 << "[JointPositionController]: 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);
     }
-    if (_sdf->HasElement("cmd_offset"))
-    {
-        cmdOffset = _sdf->Get<double>("cmd_offset");
+    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 << "[JointPositionController]: 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);
     }
-    if (_sdf->HasElement("use_velocity_commands"))
+    return;
+  }
+
+  // Get error in position
+  double error;
+  {
+    error = mimicJointPosComp->Data().at(this->dataPtr->mimicJointIndex) -
+      ( jointPosComp->Data().at(this->dataPtr->jointIndex) * this->dataPtr->multiplier +
+      this->dataPtr->offset);
+  }
+
+  // TODO(livanov93): parametrize dead zone
+  if (fabs(error) > this->dataPtr->deadZone) {
+
+    // Check if the mode is ABS
+    if (this->dataPtr->mode ==
+      MimicJointSystemPrivate::OperationMode::ABS)
     {
-        auto useVelocityCommands = _sdf->Get<bool>("use_velocity_commands");
-        if (useVelocityCommands)
-        {
-            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->mimicJointEntity);
+
+      if (vel == nullptr) {
+        _ecm.CreateComponent(
+          this->dataPtr->mimicJointEntity,
+          ignition::gazebo::components::JointVelocityCmd({targetVel}));
+      } else {
+        *vel = ignition::gazebo::components::JointVelocityCmd({targetVel});
+      }
+      return;
     }
 
-    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");
+    // Update force command.
+    double force = this->dataPtr->posPid.Update(error, _info.dt);
+
+    auto forceComp =
+      _ecm.Component < ignition::gazebo::components::JointForceCmd >
+      (this->dataPtr->mimicJointEntity);
+    if (forceComp == nullptr) {
+      _ecm.CreateComponent(
+        this->dataPtr->mimicJointEntity,
+        ignition::gazebo::components::JointForceCmd({force}));
+    } else {
+      *forceComp = ignition::gazebo::components::JointForceCmd({force});
     }
 
+  }
 
-}
 
-void MimicJointSystem::PreUpdate(const ignition::gazebo::UpdateInfo &_info,
-    ignition::gazebo::EntityComponentManager &_ecm)
-{
-//  ignmsg << "MimicJointSystem::PreUpdate" << std::endl;
 }
 
-void MimicJointSystem::Update(const ignition::gazebo::UpdateInfo &_info,
-    ignition::gazebo::EntityComponentManager &_ecm)
+void MimicJointSystem::Update(
+  const ignition::gazebo::UpdateInfo & _info,
+  ignition::gazebo::EntityComponentManager & _ecm)
 {
 //  ignmsg << "MimicJointSystem::Update" << std::endl;
 }
 
-void MimicJointSystem::PostUpdate(const ignition::gazebo::UpdateInfo &_info,
-    const ignition::gazebo::EntityComponentManager &_ecm)
+void MimicJointSystem::PostUpdate(
+  const ignition::gazebo::UpdateInfo & _info,
+  const ignition::gazebo::EntityComponentManager & _ecm)
 {
 //  ignmsg << "MimicJointSystem::PostUpdate" << std::endl;
 }
diff --git a/ign_ros2_control/src/ign_system.cpp b/ign_ros2_control/src/ign_system.cpp
index da59ead9..ce36ed01 100644
--- a/ign_ros2_control/src/ign_system.cpp
+++ b/ign_ros2_control/src/ign_system.cpp
@@ -921,7 +921,7 @@ hardware_interface::return_type IgnitionSystem::write(
   }
 
   // set values of all mimic joints with respect to mimicked joint
-  for (const auto & mimic_joint : this->dataPtr->mimic_joints_) {
+  /*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 =
@@ -1045,7 +1045,7 @@ hardware_interface::return_type IgnitionSystem::write(
         }
       }
     }
-  }
+  }*/
 
   return hardware_interface::return_type::OK;
 }