diff --git a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
index 9e1f919b3..92486283a 100644
--- a/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
+++ b/kuka_rsi_hw_interface/include/kuka_rsi_hw_interface/kuka_hardware_interface.h
@@ -56,8 +56,6 @@
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 
-// Timers
-#include <chrono>
 
 // UDP server
 #include <kuka_rsi_hw_interface/udp_server.h>
@@ -112,6 +110,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
   ros::Duration control_period_;
   ros::Duration elapsed_time_;
   double loop_hz_;
+  double krc_multiplier_;
 
   // Interfaces
   hardware_interface::JointStateInterface joint_state_interface_;
@@ -126,7 +125,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
   void configure();
   bool read(const ros::Time time, const ros::Duration period);
   bool write(const ros::Time time, const ros::Duration period);
-
+  ros::Duration getPeriod() const { return control_period_; }
 };
 
 } // namespace kuka_rsi_hw_interface
diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
index f03f13bb6..83b674d2b 100644
--- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
+++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
@@ -48,7 +48,7 @@ namespace kuka_rsi_hw_interface
 KukaHardwareInterface::KukaHardwareInterface() :
     joint_position_(6, 0.0), joint_velocity_(6, 0.0), joint_effort_(6, 0.0), joint_position_command_(6, 0.0), joint_velocity_command_(
         6, 0.0), joint_effort_command_(6, 0.0), joint_names_(6), rsi_initial_joint_positions_(6, 0.0), rsi_joint_position_corrections_(
-        6, 0.0), ipoc_(0), n_dof_(6)
+        6, 0.0), ipoc_(0), n_dof_(6), krc_multiplier_(1.0)
 {
   in_buffer_.resize(1024);
   out_buffer_.resize(1024);
@@ -104,6 +104,11 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio
   }
 
   rsi_state_ = RSIState(in_buffer_);
+  if(rsi_state_.ipoc > ipoc_)
+  {
+    control_period_.fromSec(krc_multiplier_ * (rsi_state_.ipoc - ipoc_) / 1000.0);
+  }
+
   for (std::size_t i = 0; i < n_dof_; ++i)
   {
     joint_position_[i] = DEG2RAD * rsi_state_.positions[i];
@@ -141,6 +146,7 @@ void KukaHardwareInterface::start()
   if (bytes < 100)
   {
     bytes = server_->recv(in_buffer_);
+    krc_multiplier_ = 12.0;
   }
 
   rsi_state_ = RSIState(in_buffer_);
diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp
index ba587e348..ccd9c83d5 100644
--- a/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp
+++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface_node.cpp
@@ -56,8 +56,6 @@ int main(int argc, char** argv)
   // Set up timers
   ros::Time timestamp;
   ros::Duration period;
-  auto stopwatch_last = std::chrono::steady_clock::now();
-  auto stopwatch_now = stopwatch_last;
 
   controller_manager::ControllerManager controller_manager(&kuka_rsi_hw_interface, nh);
 
@@ -65,9 +63,6 @@ int main(int argc, char** argv)
 
   // Get current time and elapsed time since last read
   timestamp = ros::Time::now();
-  stopwatch_now = std::chrono::steady_clock::now();
-  period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
-  stopwatch_last = stopwatch_now;
 
   // Run as fast as possible
   while (ros::ok())
@@ -82,9 +77,7 @@ int main(int argc, char** argv)
 
     // Get current time and elapsed time since last read
     timestamp = ros::Time::now();
-    stopwatch_now = std::chrono::steady_clock::now();
-    period.fromSec(std::chrono::duration_cast<std::chrono::duration<double>>(stopwatch_now - stopwatch_last).count());
-    stopwatch_last = stopwatch_now;
+    period = kuka_rsi_hw_interface.getPeriod();
 
     // Update the controllers
     controller_manager.update(timestamp, period);
diff --git a/kuka_rsi_simulator/scripts/kuka_rsi_simulator b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
index 15131e167..638341087 100755
--- a/kuka_rsi_simulator/scripts/kuka_rsi_simulator
+++ b/kuka_rsi_simulator/scripts/kuka_rsi_simulator
@@ -38,7 +38,7 @@ node_name = 'kuka_rsi_simulation'
 rsi_act_pub = rospy.Publisher(node_name + '/rsi/state', String, queue_size=1)
 rsi_cmd_pub = rospy.Publisher(node_name + '/rsi/command', String, queue_size=1)
 
-cycle_time = 0.004
+cycle_time = 4
 act_joint_pos = np.array([0, -90, 90, 0, 90, 0]).astype(np.float64)
 cmd_joint_pos = act_joint_pos.copy()
 des_joint_correction_absolute = np.zeros(6)
@@ -58,6 +58,7 @@ if __name__ == '__main__':
     sen_type = args.sen
 
     rospy.init_node(node_name)
+    rate = rospy.Rate(1000.0/cycle_time)
     rospy.loginfo('{}: Started'.format(node_name))
 
     try:
@@ -83,8 +84,8 @@ if __name__ == '__main__':
             rsi_cmd_pub.publish(recv_msg)
             des_joint_correction_absolute, ipoc_recv = parse_rsi_xml_sen(recv_msg)
             act_joint_pos = cmd_joint_pos + des_joint_correction_absolute
-            ipoc += 1
-            time.sleep(cycle_time / 2)
+            ipoc += cycle_time
+            rate.sleep()
         except socket.timeout, msg:
             rospy.logwarn('{}: Socket timed out'.format(node_name))
             timeout_count += 1