diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
index 58be9e348..bbccef4da 100644
--- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
+++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h
@@ -779,7 +779,11 @@ updateStates(const ros::Time& sample_time, const Trajectory* const traj)
     desired_state_.velocity[joint_index] = desired_joint_state_.velocity[0];
     desired_state_.acceleration[joint_index] = desired_joint_state_.acceleration[0];
 
-    state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]);
+    if(angle_wraparound_[joint_index])
+      state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index], desired_joint_state_.position[0]);
+    else
+      state_error_.position[joint_index] = desired_joint_state_.position[0] - current_state_.position[joint_index];
+    
     state_error_.velocity[joint_index] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index];
     state_error_.acceleration[joint_index] = 0.0;