diff --git a/moveit_ros/moveit_servo/config/servo_parameters.yaml b/moveit_ros/moveit_servo/config/servo_parameters.yaml
index b16fc6bf0c..2aefbb45a7 100644
--- a/moveit_ros/moveit_servo/config/servo_parameters.yaml
+++ b/moveit_ros/moveit_servo/config/servo_parameters.yaml
@@ -42,6 +42,13 @@ servo:
                   must be passed to the node during launch time."
   }
 
+  end_effector_frame: {
+    type: string,
+    default_value: "",
+    description: "The end effector frame for the active planning group. \
+                  If not set, it will be overridden by command-frame of incoming commands"
+  }
+
   active_subgroup: {
     type: string,
     default_value: "",
diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
index c4596711e5..dbd8c7ca8f 100644
--- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
+++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
@@ -223,6 +223,9 @@ class Servo
 
   // Variables
 
+  // The end effector frame for the active planning group.
+  std::string end_effector_frame_;
+
   StatusCode servo_status_;
   // This needs to be threadsafe so it can be updated in realtime.
   std::atomic<CommandType> expected_command_type_;
diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
index 22917e7bb5..07945dc1e0 100644
--- a/moveit_ros/moveit_servo/src/servo.cpp
+++ b/moveit_ros/moveit_servo/src/servo.cpp
@@ -79,6 +79,8 @@ Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr<const servo::P
 
   moveit::core::RobotStatePtr robot_state = planning_scene_monitor_->getStateMonitor()->getCurrentState();
 
+  end_effector_frame_ = servo_params_.end_effector_frame;
+
   // Create the collision checker and start collision checking.
   collision_monitor_ =
       std::make_unique<CollisionMonitor>(planning_scene_monitor_, servo_params_, std::ref(collision_velocity_scale_));
@@ -576,54 +578,64 @@ std::optional<TwistCommand> Servo::toPlanningFrame(const TwistCommand& command,
 {
   Eigen::VectorXd transformed_twist = command.velocities;
 
-  if (command.frame_id != planning_frame)
+  // Look up the transform between the planning and command frames.
+  const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
+  if (!planning_to_command_tf_maybe.has_value())
+  {
+    return std::nullopt;
+  }
+  const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
+
+  if (servo_params_.apply_twist_commands_about_ee_frame)
   {
-    // Look up the transform between the planning and command frames.
-    const auto planning_to_command_tf_maybe = getPlanningToCommandFrameTransform(command.frame_id, planning_frame);
-    if (!planning_to_command_tf_maybe.has_value())
+
+    const auto planning_to_ee_frame_maybe = getPlanningToCommandFrameTransform(
+      (end_effector_frame_.empty() ? command.frame_id : end_effector_frame_),
+      planning_frame);
+    if (!planning_to_ee_frame_maybe.has_value())
     {
       return std::nullopt;
     }
-    const auto& planning_to_command_tf = *planning_to_command_tf_maybe;
+    const auto& planning_to_ee_frame = *planning_to_ee_frame_maybe;
+    RCLCPP_DEBUG(logger_, "Applying twist about end effector frame: %s",
+                  (end_effector_frame_.empty() ? command.frame_id.c_str() : end_effector_frame_.c_str()));
+
+    // If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
+    const auto planning_to_command_rotation = planning_to_command_tf.linear();
+    const auto planning_to_ee_frame_rotation = planning_to_ee_frame.linear();
+    const Eigen::Vector3d translation_vector =
+        planning_to_command_rotation *
+        Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
+    const Eigen::Vector3d angular_vector =
+        planning_to_ee_frame_rotation *
+        Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);
+
+    // Update the values of the original command message to reflect the change in frame
+    transformed_twist.head<3>() = translation_vector;
+    transformed_twist.tail<3>() = angular_vector;
+  }
+  else
+  {
+    // If the twist command is applied about the planning frame, the spatial twist is calculated
+    // as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
+    // The above equation defines twist as [angular; linear], but in our convention it is
+    // [linear; angular] so the adjoint matrix is also reordered accordingly.
+    Eigen::MatrixXd adjoint(6, 6);
 
-    if (servo_params_.apply_twist_commands_about_ee_frame)
-    {
-      // If the twist command is applied about the end effector frame, simply apply the rotation of the transform.
-      const auto planning_to_command_rotation = planning_to_command_tf.linear();
-      const Eigen::Vector3d translation_vector =
-          planning_to_command_rotation *
-          Eigen::Vector3d(command.velocities[0], command.velocities[1], command.velocities[2]);
-      const Eigen::Vector3d angular_vector =
-          planning_to_command_rotation *
-          Eigen::Vector3d(command.velocities[3], command.velocities[4], command.velocities[5]);
-
-      // Update the values of the original command message to reflect the change in frame
-      transformed_twist.head<3>() = translation_vector;
-      transformed_twist.tail<3>() = angular_vector;
-    }
-    else
-    {
-      // If the twist command is applied about the planning frame, the spatial twist is calculated
-      // as shown in Equation 3.83 in http://hades.mech.northwestern.edu/images/7/7f/MR.pdf.
-      // The above equation defines twist as [angular; linear], but in our convention it is
-      // [linear; angular] so the adjoint matrix is also reordered accordingly.
-      Eigen::MatrixXd adjoint(6, 6);
-
-      const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
-      const Eigen::Vector3d& translation = planning_to_command_tf.translation();
-
-      Eigen::Matrix3d skew_translation;
-      skew_translation.row(0) << 0, -translation(2), translation(1);
-      skew_translation.row(1) << translation(2), 0, -translation(0);
-      skew_translation.row(2) << -translation(1), translation(0), 0;
-
-      adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
-      adjoint.topRightCorner(3, 3) = rotation;
-      adjoint.bottomLeftCorner(3, 3) = rotation;
-      adjoint.bottomRightCorner(3, 3).setZero();
-
-      transformed_twist = adjoint * transformed_twist;
-    }
+    const Eigen::Matrix3d& rotation = planning_to_command_tf.rotation();
+    const Eigen::Vector3d& translation = planning_to_command_tf.translation();
+
+    Eigen::Matrix3d skew_translation;
+    skew_translation.row(0) << 0, -translation(2), translation(1);
+    skew_translation.row(1) << translation(2), 0, -translation(0);
+    skew_translation.row(2) << -translation(1), translation(0), 0;
+
+    adjoint.topLeftCorner(3, 3) = skew_translation * rotation;
+    adjoint.topRightCorner(3, 3) = rotation;
+    adjoint.bottomLeftCorner(3, 3) = rotation;
+    adjoint.bottomRightCorner(3, 3).setZero();
+
+    transformed_twist = adjoint * transformed_twist;
   }
 
   return TwistCommand{ planning_frame, transformed_twist };
diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp
index ecaa486bec..ebf8165f46 100644
--- a/moveit_ros/moveit_servo/src/servo_node.cpp
+++ b/moveit_ros/moveit_servo/src/servo_node.cpp
@@ -344,12 +344,13 @@ void ServoNode::servoLoop()
   // while Servo uses RCL_ROS_TIME
   while (servo_node_start >
          convertClockType(planning_scene_monitor_->getLastUpdateTime(), servo_node_start.get_clock_type()))
-  {
-    RCLCPP_INFO(node_->get_logger(), "Waiting for planning scene monitor to receive robot state update.");
-    rclcpp::sleep_for(std::chrono::seconds(1));
-  }
   {
     RCLCPP_INFO(node_->get_logger(), "Waiting to receive robot state update.");
+    // Output the time of planning_scene_monitor_->getLastUpdateTime()
+    RCLCPP_INFO(node_->get_logger(), "Last planning scene update time: %f",
+                planning_scene_monitor_->getLastUpdateTime().seconds());
+    // Output the current time
+    RCLCPP_INFO(node_->get_logger(), "Current time: %f", node_->now().seconds());
     rclcpp::sleep_for(std::chrono::seconds(1));
   }
   KinematicState current_state = servo_->getCurrentRobotState(true /* block for current robot state */);
diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp
index 7ce13c1805..8d5ba010e0 100644
--- a/moveit_ros/moveit_servo/src/utils/common.cpp
+++ b/moveit_ros/moveit_servo/src/utils/common.cpp
@@ -197,6 +197,20 @@ composeTrajectoryMessage(const servo::Params& servo_params, const std::deque<Kin
     add_point(joint_trajectory, joint_cmd_rolling_window[i]);
   }
 
+  // If it is not empty, add a point with a speed of 0 to ensure that the robot can stop
+
+  if (!joint_cmd_rolling_window.empty())
+  {
+    auto& last_state = joint_cmd_rolling_window.back();
+    KinematicState stop_state = last_state;
+    for (long i = 0; i < stop_state.velocities.size(); ++i)
+    {
+      stop_state.velocities[i] = 0.0;
+    }
+    stop_state.time_stamp += rclcpp::Duration(0, 100000);  // add 0.1s to ensure stop
+    add_point(joint_trajectory, stop_state);
+  }
+
   return joint_trajectory;
 }
 
@@ -501,7 +515,21 @@ planning_scene_monitor::PlanningSceneMonitorPtr createPlanningSceneMonitor(const
   }
   else
   {
-    planning_scene_monitor->requestPlanningSceneState();
+    int cnt = 0;
+    while (!planning_scene_monitor->requestPlanningSceneState())
+    {
+      RCLCPP_WARN_THROTTLE(
+          node->get_logger(), *node->get_clock(), 5000,
+          "Waiting for planning scene monitor to get the planning scene from the primary planning scene monitor...");
+      rclcpp::sleep_for(std::chrono::milliseconds(100));
+      cnt++;
+      if (cnt > 120)
+      {
+        RCLCPP_ERROR(node->get_logger(),
+                     "Failed to get planning scene from primary planning scene monitor after 12 seconds.");
+        break;
+      }
+    }
   }
 
   planning_scene_monitor->setPlanningScenePublishingFrequency(PLANNING_SCENE_PUBLISHING_FREQUENCY);
diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
index eca1c474b9..9ba59b55d4 100644
--- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
+++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
@@ -404,12 +404,10 @@ void CurrentStateMonitor::jointStateCallback(const sensor_msgs::msg::JointState:
   }
 
   // callbacks, if needed
-  if (update)
-  {
+  if (update){
     for (JointStateUpdateCallback& update_callback : update_callbacks_)
       update_callback(joint_state);
   }
-
   // notify waitForCurrentState() *after* potential update callbacks
   state_update_condition_.notify_all();
 }