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..4d46c8257 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
@@ -85,6 +85,7 @@ class KukaHardwareInterface : public hardware_interface::RobotHW
   std::vector<std::string> joint_names_;
 
   std::vector<double> joint_position_;
+  std::vector<double> joint_position_last_;
   std::vector<double> joint_velocity_;
   std::vector<double> joint_effort_;
   std::vector<double> joint_position_command_;
diff --git a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
index f03f13bb6..73d305e0f 100644
--- a/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
+++ b/kuka_rsi_hw_interface/src/kuka_hardware_interface.cpp
@@ -46,9 +46,9 @@ 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)
+    joint_position_(6, 0.0), joint_position_last_(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)
 {
   in_buffer_.resize(1024);
   out_buffer_.resize(1024);
@@ -107,8 +107,13 @@ bool KukaHardwareInterface::read(const ros::Time time, const ros::Duration perio
   for (std::size_t i = 0; i < n_dof_; ++i)
   {
     joint_position_[i] = DEG2RAD * rsi_state_.positions[i];
+    if(!period.isZero())
+    {
+      joint_velocity_[i] = (joint_position_[i] - joint_position_last_[i]) / period.toSec();
+    }
   }
   ipoc_ = rsi_state_.ipoc;
+  joint_position_last_ = joint_position_;
 
   return true;
 }