diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
index 42b7d3836c..a709557f4d 100644
--- a/moveit_ros/moveit_servo/src/servo_node.cpp
+++ b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -234,6 +234,7 @@ std::optional<KinematicState> ServoNode::processJointJogCommand(const moveit::co
       next_joint_state = result.second;
       RCLCPP_DEBUG_STREAM(node_->get_logger(), "Joint jog command timed out. Halting to a stop.");
     }
+    last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
   }
 
   return next_joint_state;
@@ -266,6 +267,7 @@ std::optional<KinematicState> ServoNode::processTwistCommand(const moveit::core:
       next_joint_state = result.second;
       RCLCPP_DEBUG_STREAM(node_->get_logger(), "Twist command timed out. Halting to a stop.");
     }
+    last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
   }
 
   return next_joint_state;
@@ -295,6 +297,7 @@ std::optional<KinematicState> ServoNode::processPoseCommand(const moveit::core::
       next_joint_state = result.second;
       RCLCPP_DEBUG_STREAM(node_->get_logger(), "Pose command timed out. Halting to a stop.");
     }
+    last_commanded_state_ = servo_->getCurrentRobotState(true /* block for current robot state */);
   }
 
   return next_joint_state;
@@ -414,4 +417,4 @@ void ServoNode::servoLoop()
 }  // namespace moveit_servo
 
 #include "rclcpp_components/register_node_macro.hpp"
-RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode)
+RCLCPP_COMPONENTS_REGISTER_NODE(moveit_servo::ServoNode)
\ No newline at end of file