diff --git a/moveit_ros/moveit_servo/tests/test_utils.cpp b/moveit_ros/moveit_servo/tests/test_utils.cpp
index d54eb08be5..bd6eb9fcfc 100644
--- a/moveit_ros/moveit_servo/tests/test_utils.cpp
+++ b/moveit_ros/moveit_servo/tests/test_utils.cpp
@@ -152,8 +152,8 @@ TEST(ServoUtilsUnitTests, ApproachingSingularityScaling)
   servo_params.move_group_name = "panda_arm";
   const moveit::core::JointModelGroup* joint_model_group =
       robot_state->getJointModelGroup(servo_params.move_group_name);
+  robot_state->setToDefaultValues();
 
-  rclcpp::sleep_for(std::chrono::milliseconds(500));
   Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
   // Home state
   Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
@@ -178,8 +178,8 @@ TEST(ServoUtilsUnitTests, HaltForSingularityScaling)
   servo_params.move_group_name = "panda_arm";
   const moveit::core::JointModelGroup* joint_model_group =
       robot_state->getJointModelGroup(servo_params.move_group_name);
+  robot_state->setToDefaultValues();
 
-  rclcpp::sleep_for(std::chrono::milliseconds(500));
   Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
   // Home state
@@ -205,9 +205,9 @@ TEST(ServoUtilsUnitTests, LeavingSingularityScaling)
   servo_params.move_group_name = "panda_arm";
   const moveit::core::JointModelGroup* joint_model_group =
       robot_state->getJointModelGroup(servo_params.move_group_name);
+  robot_state->setToDefaultValues();
 
-  rclcpp::sleep_for(std::chrono::milliseconds(500));
-  Eigen::Vector<double, 6> cartesian_delta{ 0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  Eigen::Vector<double, 6> cartesian_delta{ -0.005, 0.0, 0.0, 0.0, 0.0, 0.0 };
 
   // Home state
   Eigen::Vector<double, 7> state_ready{ 0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785 };
@@ -215,14 +215,7 @@ TEST(ServoUtilsUnitTests, LeavingSingularityScaling)
   auto scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
   ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::NO_WARNING);
 
-  // Approach singularity
-  Eigen::Vector<double, 7> state_approaching_singularity{ 0.0, 0.334, 0.0, -1.177, 0.0, 1.510, 0.785 };
-  robot_state->setJointGroupActivePositions(joint_model_group, state_approaching_singularity);
-  scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
-  ASSERT_EQ(scaling_result.second, moveit_servo::StatusCode::DECELERATE_FOR_APPROACHING_SINGULARITY);
-
   // Move away from singularity
-  cartesian_delta(0) *= -1;
   Eigen::Vector<double, 7> state_leaving_singularity{ 0.0, 0.3458, 0.0, -1.1424, 0.0, 1.4865, 0.785 };
   robot_state->setJointGroupActivePositions(joint_model_group, state_leaving_singularity);
   scaling_result = moveit_servo::velocityScalingFactorForSingularity(robot_state, cartesian_delta, servo_params);
diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp
index 97f1907211..b21ec11f1c 100644
--- a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp
+++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp
@@ -206,6 +206,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter&
           const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_);
           emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces;
           emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces;
+          emitter << YAML::Key << "allow_nonzero_velocity_at_trajectory_end" << YAML::Value << true;
         }
       }
       emitter << YAML::EndMap;