diff --git a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
index 0fcf61ba..849e6295 100644
--- a/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
+++ b/ign_ros2_control/include/ign_ros2_control/MimicJointSystem.hh
@@ -1,19 +1,16 @@
-/*
- * 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.
- *
-*/
+// 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
@@ -24,8 +21,8 @@
  */
 //----------------------------------------------------------------------
 
-#ifndef IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_
-#define IGNITION_GAZEBO_SYSTEMS_MIMICJOINTSYSTEM_HH_
+#ifndef IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
+#define IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
 
 //! [header]
 #include <memory>
@@ -57,37 +54,49 @@ namespace ign_ros2_control
   class MimicJointSystemPrivate;
 
   class MimicJointSystem:
-    // This class is a system.
-    public ignition::gazebo::System,
-    public ignition::gazebo::ISystemConfigure,
-    // This class also implements the ISystemPreUpdate, ISystemUpdate,
-    // and ISystemPostUpdate interfaces.
-    public ignition::gazebo::ISystemPreUpdate,
-    public ignition::gazebo::ISystemUpdate,
-    public ignition::gazebo::ISystemPostUpdate
+  // This class is a system.
+  public ignition::gazebo::System,
+  public ignition::gazebo::ISystemConfigure,
+  // This class also implements the ISystemPreUpdate, ISystemUpdate,
+  // and ISystemPostUpdate interfaces.
+  public ignition::gazebo::ISystemPreUpdate,
+  public ignition::gazebo::ISystemUpdate,
+  public ignition::gazebo::ISystemPostUpdate
   {
-    public: MimicJointSystem();
+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 &_ecm) override;
+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 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;
 
-    public: void PostUpdate(const ignition::gazebo::UpdateInfo &_info,
-                const ignition::gazebo::EntityComponentManager &_ecm) override;
+private:
+    /// \brief Private data pointer
 
-  private:
-      /// \brief Private data pointer
-      private: std::unique_ptr<MimicJointSystemPrivate> dataPtr;
+private:
+    std::unique_ptr < MimicJointSystemPrivate > dataPtr;
   };
-}
+}  // namespace ign_ros2_control
 //! [header]
 
-#endif
+#endif  // IGN_ROS2_CONTROL__MIMICJOINTSYSTEM_HH_
diff --git a/ign_ros2_control/src/MimicJointSystem.cc b/ign_ros2_control/src/MimicJointSystem.cc
index 56b083bf..aff360db 100644
--- a/ign_ros2_control/src/MimicJointSystem.cc
+++ b/ign_ros2_control/src/MimicJointSystem.cc
@@ -1,19 +1,16 @@
-/*
- * 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.
- *
-*/
+// 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
@@ -42,11 +39,10 @@
 
 #include "ignition/gazebo/Model.hh"
 
-using namespace ign_ros2_control;
 
-class ign_ros2_control::MimicJointSystemPrivate {
-
-/// \brief Joint Entity
+class ign_ros2_control::MimicJointSystemPrivate
+{
+  /// \brief Joint Entity
 
 public:
   ignition::gazebo::Entity jointEntity {ignition::gazebo::kNullEntity};
@@ -54,7 +50,7 @@ class ign_ros2_control::MimicJointSystemPrivate {
 public:
   ignition::gazebo::Entity mimicJointEntity {ignition::gazebo::kNullEntity};
 
-/// \brief Joint name
+  /// \brief Joint name
 
 public:
   std::string jointName;
@@ -62,7 +58,7 @@ class ign_ros2_control::MimicJointSystemPrivate {
 public:
   std::string mimicJointName;
 
-/// \brief Commanded joint position
+  /// \brief Commanded joint position
 
 public:
   double jointPosCmd {0.0};
@@ -73,17 +69,17 @@ class ign_ros2_control::MimicJointSystemPrivate {
 public:
   double offset {0.0};
 
-/// \brief Model interface
+  /// \brief Model interface
 
 public:
   ignition::gazebo::Model model {ignition::gazebo::kNullEntity};
 
-/// \brief Position PID controller.
+  /// \brief Position PID controller.
 
 public:
   ignition::math::PID posPid;
 
-/// \brief Joint index to be used.
+  /// \brief Joint index to be used.
 
 public:
   unsigned int jointIndex = 0u;
@@ -95,7 +91,7 @@ class ign_ros2_control::MimicJointSystemPrivate {
   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
+  /// \brief Operation modes
   enum OperationMode
   {
     /// \brief Use PID to achieve positional control
@@ -105,310 +101,302 @@ class ign_ros2_control::MimicJointSystemPrivate {
     ABS
   };
 
-/// \brief Joint position mode
+  /// \brief Joint position mode
 
 public:
   OperationMode mode = OperationMode::PID;
-
 };
 
-MimicJointSystem::MimicJointSystem()
-: dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ())
+namespace ign_ros2_control
 {
-}
 
-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;
+  MimicJointSystem::MimicJointSystem()
+  : dataPtr(std::make_unique < ign_ros2_control::MimicJointSystemPrivate > ())
+  {
   }
 
-  // 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;
-  }
+  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;
+    }
 
-  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;
-  }
+    // Get params from SDF
+    this->dataPtr->jointName = _sdf->Get < std::string > ("joint_name");
 
-  if (_sdf->HasElement("multiplier")) {
-    this->dataPtr->multiplier = _sdf->Get < double > ("multiplier");
-  }
+    if (this->dataPtr->jointName.empty()) {
+      ignerr << "MimicJointSystem found an empty joint_name parameter. "
+             << "Failed to initialize.";
+      return;
+    }
 
-  if (_sdf->HasElement("offset")) {
-    this->dataPtr->offset = _sdf->Get < double > ("offset");
-  }
+    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("joint_index")) {
-    this->dataPtr->jointIndex = _sdf->Get < unsigned int > ("joint_index");
-  }
+    if (_sdf->HasElement("multiplier")) {
+      this->dataPtr->multiplier = _sdf->Get < double > ("multiplier");
+    }
 
-  if (_sdf->HasElement("mimic_joint_index")) {
-    this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index");
-  }
+    if (_sdf->HasElement("offset")) {
+      this->dataPtr->offset = _sdf->Get < double > ("offset");
+    }
 
-  if (_sdf->HasElement("dead_zone")) {
-    this->dataPtr->deadZone = _sdf->Get < double > ("dead_zone");
-  }
+    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");
-  }
-  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;
+    if (_sdf->HasElement("mimic_joint_index")) {
+      this->dataPtr->mimicJointIndex = _sdf->Get < unsigned int > ("mimic_joint_index");
     }
-  }
 
-  this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
+    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;
+      }
+    }
 
-  if (_sdf->HasElement("initial_position")) {
-    this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position");
-  }
+    this->dataPtr->posPid.Init(p, i, d, iMax, iMin, cmdMax, cmdMin, cmdOffset);
 
-  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 (_sdf->HasElement("initial_position")) {
+      this->dataPtr->jointPosCmd = _sdf->Get < double > ("initial_position");
+    }
 
-  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;
+    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";
   }
 
-  // If the joint hasn't been identified yet, look for it
-  if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity ||
-    this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity)
+  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;
+    }
 
-    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 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;
-  }
+    if (this->dataPtr->jointEntity == ignition::gazebo::kNullEntity) {
+      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);
+    if (this->dataPtr->mimicJointEntity == ignition::gazebo::kNullEntity) {
+      return;
     }
-    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);
+    if (_info.paused) {
+      return;
     }
-    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);
+    auto jointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition >
+      (this->dataPtr->jointEntity);
+    if (!jointPosComp) {
+      _ecm.CreateComponent(
+        this->dataPtr->jointEntity,
+        ignition::gazebo::components::JointPosition());
+    }
 
-  if (fabs(error) > this->dataPtr->deadZone) {
+    auto mimicJointPosComp = _ecm.Component < ignition::gazebo::components::JointPosition >
+      (this->dataPtr->mimicJointEntity);
+    if (!mimicJointPosComp) {
+      _ecm.CreateComponent(
+        this->dataPtr->mimicJointEntity,
+        ignition::gazebo::components::JointPosition());
+    }
 
-    // Check if the mode is ABS
-    if (this->dataPtr->mode ==
-      MimicJointSystemPrivate::OperationMode::ABS)
+    if (jointPosComp == nullptr || jointPosComp->Data().empty() || mimicJointPosComp == nullptr ||
+      mimicJointPosComp->Data().empty())
     {
-      // 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;
+      return;
+    }
 
-      // Limit the maximum change to maxMovement
-      if (abs(error) > maxMovement) {
-        targetVel = (error < 0) ? this->dataPtr->posPid.CmdMax() :
-          -this->dataPtr->posPid.CmdMax();
-      } else {
-        targetVel = -error;
+    // 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;
+    }
 
-      // 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});
+    // 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;
     }
 
-    // If the mode is not ABS, default mode is PID
-    // (e.g.) MimicJointSystemPrivate::OperationMode::PID
+    // 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;
+      }
 
-    // Update force command.
-    double force = this->dataPtr->posPid.Update(error, _info.dt);
+      // If the mode is not ABS, default mode is PID
+      // (e.g.) MimicJointSystemPrivate::OperationMode::PID
 
-    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});
+      // 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::Update(
+    const ignition::gazebo::UpdateInfo & /*_info*/,
+    ignition::gazebo::EntityComponentManager & /*_ecm*/)
+  {
+  }
 
-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*/)
+  {
+  }
+}  // namespace ign_ros2_control
 
 //! [registerMimicJointSystem]
 
 IGNITION_ADD_PLUGIN(
   ign_ros2_control::MimicJointSystem,
   ignition::gazebo::System,
-  MimicJointSystem::ISystemConfigure,
-  MimicJointSystem::ISystemPreUpdate,
-  MimicJointSystem::ISystemUpdate,
-  MimicJointSystem::ISystemPostUpdate)
+  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_system.cpp b/ign_ros2_control/src/ign_system.cpp
index be6b8733..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>
@@ -47,8 +49,6 @@
 
 #include <ignition/transport/Node.hh>
 
-#include <iostream>
-
 #include <hardware_interface/hardware_info.hpp>
 #include <normApi.h>
 
@@ -452,7 +452,8 @@ bool IgnitionSystem::initSim(
                 " ' to mimic");
       }
       RCLCPP_INFO_STREAM(
-        this->nh_->get_logger(), "Joint '" << joint_name << "'is mimicking joint '" << mimicked_joint
+        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);
     }
@@ -603,7 +604,8 @@ void IgnitionSystem::registerSensors(const hardware_interface::HardwareInfo & ha
         this->nh_->get_logger(),
         "Loading sensor: " << _name->Data());
 
-      auto sensorTopicComp = this->dataPtr->ecm->Component<ignition::gazebo::components::SensorTopic>(
+      auto sensorTopicComp =
+      this->dataPtr->ecm->Component<ignition::gazebo::components::SensorTopic>(
         _entity);
       if (sensorTopicComp) {
         RCLCPP_INFO_STREAM(this->nh_->get_logger(), "Topic name: " << sensorTopicComp->Data());
@@ -704,7 +706,8 @@ hardware_interface::return_type IgnitionSystem::read(
 
     this->dataPtr->joints_[i].joint_position = jointPositions->Data()[0];
     this->dataPtr->joints_[i].joint_velocity = jointVelocity->Data()[0];
-    // set effort state interface to computed/propagated effort command - passthrough because of ignitionrobotics/ign-physics#124
+    // 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;
   }
 
@@ -1007,7 +1010,8 @@ hardware_interface::return_type IgnitionSystem::write(
             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
+          // 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 *
@@ -1101,8 +1105,7 @@ hardware_interface::return_type IgnitionSystem::write(
   }
   return hardware_interface::return_type::OK;
 }
-
-} // namespace ign_ros2_control
+}  // namespace ign_ros2_control
 
 #include "pluginlib/class_list_macros.hpp"  // NOLINT
 PLUGINLIB_EXPORT_CLASS(