Skip to content

Commit

Permalink
Add spline test to ensure it does not starts exterpolate
Browse files Browse the repository at this point in the history
  • Loading branch information
@urrsk
urrsk committed Mar 8, 2024
1 parent 1620e23 commit cb04b80
Show file tree
Hide file tree
Showing 2 changed files with 43 additions and 16 deletions.
9 changes: 4 additions & 5 deletions resources/external_control.urscript
View file
Original file line number Diff line number Diff line change
Expand Up @@ -235,18 +235,17 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c

# Make sure that we approach zero velocity slowly, when slowing down
if is_slowing_down == True:
local qd = get_actual_joint_speeds()
while norm(qd) > 0.00001:
local time_left = splineTotalTravelTime - splineTimerTraveled
local time_left = splineTotalTravelTime - splineTimerTraveled

while time_left >= 1e-5:
time_left = splineTotalTravelTime - splineTimerTraveled
# Compute scaling factor based on time left and total slowing down time
scaling_factor = time_left / slowing_down_time
scaled_step_time = get_steptime() * scaling_factor

splineTimerTraveled = splineTimerTraveled + scaled_step_time

jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor, is_slowing_down)

qd = get_actual_joint_speeds()
end
scaling_factor = 0.0
end
Expand Down
50 changes: 39 additions & 11 deletions tests/test_spline_interpolation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -310,7 +310,7 @@ class SplineInterpolationTest : public ::testing::Test
std::vector<urcl::vector6d_t> expected_positions,
std::vector<urcl::vector6d_t> actual_positions,
std::vector<urcl::vector6d_t> actual_velocities, std::vector<urcl::vector6d_t> actual_acc,
std::vector<double> speed_scaling)
std::vector<double> speed_scaling, std::vector<double> spline_time)
{
std::ofstream outfile(filename);
// Header
Expand Down Expand Up @@ -339,7 +339,8 @@ class SplineInterpolationTest : public ::testing::Test
<< "error_positions3, "
<< "error_positions4, "
<< "error_positions5, "
<< "speed_scaling"
<< "speed_scaling, "
<< "spline_time"
<< "\n";

// Data
Expand All @@ -356,7 +357,8 @@ class SplineInterpolationTest : public ::testing::Test
<< actual_positions[i][2] - expected_positions[i][2] << ", "
<< actual_positions[i][3] - expected_positions[i][3] << ", "
<< actual_positions[i][4] - expected_positions[i][4] << ", "
<< actual_positions[i][5] - expected_positions[i][5] << ", " << speed_scaling[i] << "\n";
<< actual_positions[i][5] - expected_positions[i][5] << ", " << speed_scaling[i] << ", " << spline_time[i]
<< "\n";
}
}

Expand Down Expand Up @@ -399,7 +401,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity)

// Data for logging
std::vector<urcl::vector6d_t> actual_positions, actual_velocities, actual_acc, expected_positions;
std::vector<double> time_vec;
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);
Expand Down Expand Up @@ -459,6 +461,7 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity)
actual_acc.push_back(joint_acc);
speed_scaling_vec.push_back(speed_scaling);
time_vec.push_back(plot_time);
spline_time.push_back(spline_travel_time);
plot_time += step_time_;
}
// Make sure the velocity is zero when the trajectory has finished
Expand All @@ -469,8 +472,13 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity)
EXPECT_FLOAT_EQ(joint_velocities[i], 0.0);
}

// Verify that the full trajectory have been executed
double spline_travel_time;
data_pkg->getData("output_double_register_1", spline_travel_time);
ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5);

writeTrajectoryToFile("../test_artifacts/cubic_spline_with_end_point_velocity.csv", time_vec, expected_positions,
actual_positions, actual_velocities, actual_acc, speed_scaling_vec);
actual_positions, actual_velocities, actual_acc, speed_scaling_vec, spline_time);
}

TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling)
Expand Down Expand Up @@ -512,7 +520,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee

// Data for logging
std::vector<urcl::vector6d_t> actual_positions, actual_velocities, actual_acc, expected_positions;
std::vector<double> time_vec;
std::vector<double> time_vec, spline_time;

// Send trajectory to the robot
sendTrajectory(s_pos, s_vel, s_acc, s_time);
Expand Down Expand Up @@ -614,6 +622,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee
actual_acc.push_back(joint_acc);
speed_scaling_vec.push_back(speed_scaling);
time_vec.push_back(plot_time);
spline_time.push_back(spline_travel_time);
plot_time += step_time_;
loop_count += 1;
}
Expand All @@ -626,8 +635,14 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee
EXPECT_FLOAT_EQ(joint_velocities[i], 0.0);
}

// Verify that the full trajectory have been executed
double spline_travel_time;
data_pkg->getData("output_double_register_1", spline_travel_time);
ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5);

writeTrajectoryToFile("../test_artifacts/quintic_spline_with_end_point_velocity_speedscaling.csv", time_vec,
expected_positions, actual_positions, actual_velocities, actual_acc, speed_scaling_vec);
expected_positions, actual_positions, actual_velocities, actual_acc, speed_scaling_vec,
spline_time);
}

TEST_F(SplineInterpolationTest, spline_interpolation_cubic)
Expand Down Expand Up @@ -672,7 +687,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic)

// Data for logging
std::vector<urcl::vector6d_t> actual_positions, actual_velocities, actual_acc, expected_positions;
std::vector<double> time_vec;
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);
Expand Down Expand Up @@ -724,10 +739,17 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic)
actual_acc.push_back(joint_acc);
speed_scaling_vec.push_back(speed_scaling);
time_vec.push_back(plot_time);
spline_time.push_back(spline_travel_time);
plot_time += step_time_;
}

// Verify that the full trajectory have been executed
double spline_travel_time;
data_pkg->getData("output_double_register_1", spline_travel_time);
ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5);

writeTrajectoryToFile("../test_artifacts/spline_interpolation_cubic.csv", time_vec, expected_positions,
actual_positions, actual_velocities, actual_acc, speed_scaling_vec);
actual_positions, actual_velocities, actual_acc, speed_scaling_vec, spline_time);
}

TEST_F(SplineInterpolationTest, spline_interpolation_quintic)
Expand Down Expand Up @@ -774,7 +796,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic)

// Data for logging
std::vector<urcl::vector6d_t> actual_positions, actual_velocities, actual_acc, expected_positions;
std::vector<double> time_vec;
std::vector<double> time_vec, spline_time;

// Send the trajectory to the robot
sendTrajectory(s_pos, s_vel, s_acc, s_time);
Expand Down Expand Up @@ -826,11 +848,17 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic)
actual_acc.push_back(joint_acc);
speed_scaling_vec.push_back(speed_scaling);
time_vec.push_back(plot_time);
spline_time.push_back(spline_travel_time);
plot_time += step_time_;
}

// Verify that the full trajectory have been executed
double spline_travel_time;
data_pkg->getData("output_double_register_1", spline_travel_time);
ASSERT_NEAR(spline_travel_time, s_time.back(), 1e-5);

writeTrajectoryToFile("../test_artifacts/spline_interpolation_quintic.csv", time_vec, expected_positions,
actual_positions, actual_velocities, actual_acc, speed_scaling_vec);
actual_positions, actual_velocities, actual_acc, speed_scaling_vec, spline_time);
}

int main(int argc, char* argv[])
Expand Down

0 comments on commit cb04b80

Please sign in to comment.