Skip to content

Fix flaky spline interpolation test #556

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
URJala wants to merge 9 commits into
base: master
Choose a base branch
from
Open

Fix flaky spline interpolation test #556

Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
8fac107
Fix flaky spline interpolation test
URJala Aug 19, 2026
4540483
Release mutex in failure path
URJala Aug 19, 2026
ea7b567
Add keep alive writes
URJala Aug 20, 2026
a8c5923
Increase future timeout to match timer in thread
URJala Aug 20, 2026
d95077e
Remove stray debugging print
URJala Aug 20, 2026
07a006e
Ensure that a trajectory always runs for at least one cycle
URJala Aug 20, 2026
d089a14
Only write trajectory noop after trajectory has been sent
URJala Aug 20, 2026
4704c98
Set `g_trajectory_running` in `sendTrajectoryAndConfirmStart`
URJala Aug 25, 2026
12d787e
remove `g_trajectory_running` from thread
URJala Aug 25, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
205 changes: 164 additions & 41 deletions tests/test_spline_interpolation.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <fstream>
#include <ostream>
#include <thread>
#include <future>
#include "test_utils.h"

using namespace urcl;
Expand Down Expand Up @@ -170,8 +171,7 @@ class SplineInterpolationTest : public ::testing::Test

// Send trajectory to robot for execution
ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size()));

urcl::control::TrajectoryControlMessage::TRAJECTORY_START, s_pos.size(), RobotReceiveTimeout::off()));
for (size_t i = 0; i < s_pos.size(); i++)
{
// QUINTIC
Expand All @@ -187,6 +187,48 @@ class SplineInterpolationTest : public ::testing::Test
}
}

bool sendTrajectoryAndConfirmStart(const std::vector<urcl::vector6d_t>& s_pos,
const std::vector<urcl::vector6d_t>& s_vel,
const std::vector<urcl::vector6d_t>& s_acc, const std::vector<double>& s_time)
{
async_ready = false;
async_stop = false;
trajectory_sent = false;

g_trajectory_running = false;

// Launch async thread to check that trajectory resets spline_travel_time
auto confirm_future = std::async(std::launch::async, &SplineInterpolationTest::confirmTrajectoryStarted, this);

// Wait for the thread to be ready
std::unique_lock<std::mutex> lk(async_mutex);
if (!async_cv.wait_for(lk, std::chrono::milliseconds(1000),
[this] { return async_ready; })) // Setting up the thread might take some time
{
async_stop = true;
lk.unlock();
confirm_future.wait();
return false;
}

// Send trajectory
sendTrajectory(s_pos, s_vel, s_acc, s_time);

// Inform thread that trajectory is sent
trajectory_sent = true;
// Wait for trajectory to start (should be quick)
if (confirm_future.wait_for(std::chrono::milliseconds(1000)) != std::future_status::ready)
{
std::cout << "Trajectory could not be confirmed to have started." << std::endl;
async_stop = true;
confirm_future.wait();
return false;
}
bool trajectory_running = confirm_future.get();
g_trajectory_running = trajectory_running;
Comment on lines +227 to +228
return trajectory_running;
}

void interpolate(const double& time, urcl::vector6d_t& positions, const std::vector<urcl::vector6d_t>& coefficients)
{
for (unsigned int i = 0; i < 6; ++i)
Expand Down Expand Up @@ -254,21 +296,115 @@ class SplineInterpolationTest : public ::testing::Test
return coefficients;
}

bool confirmTrajectoryStarted()
{
// Timers
double trajectory_start_timeout = 1;
double trajectory_start_time = 0.0;
double travel_time_reset_timeout = 1;
double travel_time_reset_time = 0.0;

// Data package pre-allocation
rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());

// Spline travel times
double spline_travel_time = 0.0;
double start_spline_travel_time = 0.0;

// Check what the current spline_travel_time is
if (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
data_pkg.getData("output_double_register_1", start_spline_travel_time);
}
else
{
std::cout << "Could not get previous spline travel time" << std::endl;
return false;
}

// Allow new trajectory to be sent
{
std::lock_guard<std::mutex> lk(async_mutex);
async_ready = true;
async_cv.notify_one();
}

bool spline_travel_time_reset = start_spline_travel_time == 0.0;
while (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
// Caller wants the thread to stop for whatever reason
if (async_stop)
{
std::cout << "Async stop triggered, exiting confirm thread" << std::endl;
return false;
}

// Continuously check spline travel time
data_pkg.getData("output_double_register_1", spline_travel_time);

// Confirm that a new trajectory was transferred (Travel time = 0.0 or lower than it was before)
if (spline_travel_time < start_spline_travel_time || spline_travel_time == 0.0)
{
spline_travel_time_reset = true;
start_spline_travel_time = spline_travel_time;
}

if (spline_travel_time_reset)
{
// Keep script alive when the trajectory has been sent
if (trajectory_sent)
{
if (!g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP))
{
std::cout << "Failed to write trajectory NOOP control message, exiting confirm thread" << std::endl;
return false;
}
}

// Confirm that the new trajectory is running
if (std::abs(spline_travel_time) > start_spline_travel_time)

@urfeex urfeex Aug 21, 2026

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Wouldn't this be problematic when there was a long trajectory running beforehand? I don't remember by hard when we reset the spline_travel_time, but it seems fragile to rely on it being small in the beginning. Once it has been reset, being larger than a reasonable threshold should be sufficient, right?

@URJala URJala Aug 25, 2026 •
edited
Loading

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It only gets to this stage if we have seen that it is either zero, or at least has become smaller (where start_spline_travel_time is reset), and then we check that it advances one timestep.

{
return true;
}

@cursor cursor Bot Aug 25, 2026

Copy link
Copy Markdown

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fragile travel-time start detection

Medium Severity

confirmTrajectoryStarted treats a new trajectory as started only after spline_travel_time drops below the sampled baseline (or hits exactly 0.0), then advances past that updated baseline. jointSplineRun increments before the first register write, so a new segment often starts at about one step time, never writing 0.0. If a prior trajectory was only briefly running and the baseline is already near that same small value, the drop is never observed and confirmation times out. That is especially likely for back-to-back sendTrajectoryAndConfirmStart calls.

Fix in Cursor Fix in Web

Reviewed by Cursor Bugbot for commit 4704c98. Configure here.

if (trajectory_start_time > trajectory_start_timeout)
{
std::cout << "Trajectory didn't start within timeout, is the spline travel time written to output float "
"register 1?"
<< std::endl;
return false;
}
trajectory_start_time += step_time_;
}
else
{
if (travel_time_reset_time > travel_time_reset_timeout)
{
std::cout << "Spline travel time was not reset, was trajectory transferred to robot, and the spline travel "
"time written to output float register 1?"
<< std::endl;
return false;
}
travel_time_reset_time += step_time_;
}
}
std::cout << "Failed to get data package from RTDE, exiting confirm thread" << std::endl;
return false;
}
Comment thread
cursor[bot] marked this conversation as resolved.

void waitForTrajectoryStarted()
{
bool trajectory_started = false;
double timeout = 1;
double cur_time = 0.0;
rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe());
while (trajectory_started == false && g_my_robot->getUrDriver()->getDataPackage(data_pkg))
while (g_my_robot->getUrDriver()->getDataPackage(data_pkg))
{
double spline_travel_time = 0.0;
data_pkg.getData("output_double_register_1", spline_travel_time);

// Keep connection alive
ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP));
if (std::abs(spline_travel_time - 0.0) < 0.01)
if (std::abs(spline_travel_time) < 0.01)
{
return;
}
Expand Down Expand Up @@ -348,6 +484,13 @@ class SplineInterpolationTest : public ::testing::Test
// Deceleration variables if changed in the script, they should be changed here as well
double deceleration_time_ = 0.4189;
double max_deceleration_ = 15;

// Async variables
bool async_ready = false;
std::mutex async_mutex;
std::condition_variable async_cv;
std::atomic<bool> async_stop = false;
std::atomic<bool> trajectory_sent = false;
};

TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity)
Expand Down Expand Up @@ -381,11 +524,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity)
std::vector<double> time_vec, spline_time;

// Send the trajectory to the robot
sendTrajectory(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time);
g_trajectory_running = true;

// Make sure that the trajectory has started before we start testing for trajectory execution
waitForTrajectoryStarted();
ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time));

double old_spline_travel_time = 0.0;
double plot_time = 0.0;
Expand Down Expand Up @@ -499,11 +639,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee
std::vector<double> time_vec, spline_time;

// Send trajectory to the robot
sendTrajectory(s_pos, s_vel, s_acc, s_time);
g_trajectory_running = true;

// Make sure that trajectory has started before we start testing for trajectory execution
waitForTrajectoryStarted();
ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));

double old_spline_travel_time = 0.0;
double plot_time = 0.0;
Expand Down Expand Up @@ -663,11 +800,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic)
std::vector<double> time_vec, spline_time;

// Send the trajectory to the robot
sendTrajectory(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time);
g_trajectory_running = true;

// Make sure that trajectory has started before we start testing for trajectory execution
waitForTrajectoryStarted();
ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time));

int segment_idx = 0;
double old_spline_travel_time = 0.0;
Expand Down Expand Up @@ -774,16 +908,12 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic)
std::vector<double> time_vec, spline_time;

// Send the trajectory to the robot
sendTrajectory(s_pos, s_vel, s_acc, s_time);
g_trajectory_running = true;

// Make sure that trajectory has started before we start testing for trajectory execution
waitForTrajectoryStarted();

ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));
int segment_idx = 0;
double old_spline_travel_time = 0.0;
double plot_time = 0.0;
g_trajectory_running = true;

while (g_trajectory_running)
{
ASSERT_TRUE(g_my_robot->getUrDriver()->getDataPackage(data_pkg));
Expand Down Expand Up @@ -894,9 +1024,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline)
s_pos = { first_point, second_point, third_point };
s_vel = { zeros, zeros, zeros };
s_time = { 0.0, 1.0, 2.0 };
sendTrajectory(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time);
g_trajectory_running = true;
waitForTrajectoryStarted();

ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, std::vector<urcl::vector6d_t>(), s_time));

urcl::vector6d_t joint_positions;
while (g_trajectory_running)
{
Expand Down Expand Up @@ -968,9 +1098,9 @@ TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline)
s_vel = { zeros, zeros, zeros };
s_acc = { zeros, zeros, zeros };
s_time = { 0.0, 1.0, 2.0 };
sendTrajectory(s_pos, s_vel, s_acc, s_time);
g_trajectory_running = true;
waitForTrajectoryStarted();

ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));

urcl::vector6d_t joint_positions;
while (g_trajectory_running)
{
Expand Down Expand Up @@ -1145,13 +1275,10 @@ TEST_F(SplineInterpolationTest, new_trajectory_received_without_cancelling_the_o
}

// Send the trajectory
sendTrajectory(s_pos, s_vel, s_acc, s_time);
waitForTrajectoryStarted();
ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));

// Send the trajectory again without canceling the before one
sendTrajectory(s_pos, s_vel, s_acc, s_time);
g_trajectory_running = true;
waitForTrajectoryStarted();
ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));

// Ensure that everything goes as expected and the robot reaches the target position
urcl::vector6d_t joint_positions;
Expand Down Expand Up @@ -1203,9 +1330,7 @@ TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sendi
}

// Send the trajectory and cancel it right away
sendTrajectory(s_pos, s_vel, s_acc, s_time);
waitForTrajectoryStarted();

ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));
ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL));
EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
Expand All @@ -1214,9 +1339,7 @@ TEST_F(SplineInterpolationTest, cancel_trajectory_while_being_executed_and_sendi

// Do the same procesure again to ensure that a new trajectory can be send after canceling the before
// one in the middle of its execution
sendTrajectory(s_pos, s_vel, s_acc, s_time);
waitForTrajectoryStarted();

ASSERT_TRUE(sendTrajectoryAndConfirmStart(s_pos, s_vel, s_acc, s_time));
ASSERT_TRUE(g_my_robot->getUrDriver()->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL));
EXPECT_TRUE(waitForTrajectoryResult(std::chrono::milliseconds(500)));
Expand Down
Loading