diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp
index b7df5b75..50d84be1 100644
--- a/tests/test_spline_interpolation.cpp
+++ b/tests/test_spline_interpolation.cpp
@@ -39,6 +39,8 @@
 #include <memory>
 #include <math.h>
 #include <fstream>
+#include <ostream>
+#include <thread>
 
 using namespace urcl;
 
@@ -964,9 +966,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline)
   g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                               RobotReceiveTimeout::off());
 
-  // When an illegal trajectory is send to the robot, the control script should stop running and the trajectory result
-  // should be canceled
-  ASSERT_TRUE(waitForProgramNotRunning(1000));
+  // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result
+  // should be canceled.
+  ASSERT_FALSE(waitForProgramNotRunning(1000));
   EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_);
 
   // Stop consuming rtde packages
@@ -990,7 +992,7 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline)
   urcl::vector6d_t joint_positions_before;
   ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_before));
 
-  // Start consuming rtde packages to avoid pipeline overflows while testing that the control script aborts correctly
+  // Start consuming rtde packages to avoid pipeline overflows while testing
   g_consume_rtde_packages_ = true;
 
   // Create illegal trajectory
@@ -1010,9 +1012,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline)
   g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                               RobotReceiveTimeout::off());
 
-  // When an illegal trajectory is send to the robot, the control script should stop running and the trajectory result
+  // When an illegal trajectory is send to the robot, the control script should keep running but the trajectory result
   // should be canceled
-  ASSERT_TRUE(waitForProgramNotRunning(1000));
+  ASSERT_FALSE(waitForProgramNotRunning(1000));
   EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_);
 
   // Stop consuming rtde packages
@@ -1057,9 +1059,9 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline)
   g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                               RobotReceiveTimeout::off());
 
-  // When an unfeasible trajectory is send to the robot, the control script should stop running and the trajectory
+  // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory
   // result should be canceled
-  ASSERT_TRUE(waitForProgramNotRunning(1000));
+  ASSERT_FALSE(waitForProgramNotRunning(1000));
   EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_);
 
   // Stop consuming rtde packages
@@ -1105,9 +1107,9 @@ TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline)
   g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1,
                                               RobotReceiveTimeout::off());
 
-  // When an unfeasible trajectory is send to the robot, the control script should stop running and the trajectory
+  // When an unfeasible trajectory is send to the robot, the control script should keep running but the trajectory
   // result should be canceled
-  ASSERT_TRUE(waitForProgramNotRunning(1000));
+  ASSERT_FALSE(waitForProgramNotRunning(1000));
   EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED, g_trajectory_result_);
 
   // Stop consuming rtde packages
diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp
index 66f1fdd9..1cddb948 100644
--- a/tests/test_ur_driver.cpp
+++ b/tests/test_ur_driver.cpp
@@ -307,21 +307,11 @@ TEST_F(UrDriverTest, target_outside_limits_servoj)
 
   // Create physically unfeasible target
   urcl::vector6d_t joint_target = joint_positions_before;
-  joint_target[5] -= 0.5;
+  joint_target[5] -= 2.5;
 
-  double timeout = 0.2;
-  double cur_time = 0.0;
-  while (g_program_running && cur_time < timeout)
-  {
-    // Send unfeasible targets to the robot
-    readDataPackage(data_pkg);
-    g_ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200));
-    cur_time += step_time_;
-  }
-
-  // We expect the control script to stop running when targets are unfeasible meaning that the timeout shouldn't be
-  // reached
-  EXPECT_LT(cur_time, timeout);
+  // Send unfeasible targets to the robot
+  readDataPackage(data_pkg);
+  g_ur_driver_->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(200));
 
   // Ensure that the robot didn't move
   readDataPackage(data_pkg);