From d21a4029edb917d319dad12ddcebf71f8d5b3fae Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Wed, 20 Mar 2024 15:23:36 +0100 Subject: [PATCH] =?UTF-8?q?Changed=20spline=20interpolation=20to=20use=20t?= =?UTF-8?q?he=20last=20commanded=20joint=20velocity=E2=80=A6=20(#195)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit … when calculating the next spline in the trajectory Also calculating the need max acceleration for speedj to distribute the change of velocity over the full timestep. This is important when speed scaling is applied. This will fix issue #194 --------- Co-authored-by: urrsk <41109954+urrsk@users.noreply.github.com> --- .github/workflows/ci.yml | 1 + .github/workflows/prerelease.yml | 1 + resources/external_control.urscript | 113 +++++--- tests/resources/rtde_output_recipe_spline.txt | 1 + tests/test_spline_interpolation.cpp | 274 ++++++++++++++---- 5 files changed, 290 insertions(+), 100 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 956aa7f5..60652b30 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -100,6 +100,7 @@ jobs: --exclude=tcp_socket.cpp \ --exclude-dir=debian \ --exclude=real_time.md \ + --exclude=dataflow.graphml \ --exclude=start_ursim.sh rosdoc_lite_check: diff --git a/.github/workflows/prerelease.yml b/.github/workflows/prerelease.yml index dd6f6dcd..6e2e3292 100644 --- a/.github/workflows/prerelease.yml +++ b/.github/workflows/prerelease.yml @@ -22,6 +22,7 @@ jobs: steps: - uses: actions/checkout@v1 - run: sudo apt-get install -y python3-pip + - run: sudo pip3 install empy==3.3.4 # Added as bloom not yet support empy v4 - run: sudo pip3 install bloom rosdep - run: sudo rosdep init - run: rosdep update --rosdistro=${{ matrix.ROS_DISTRO }} diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 34a851e3..d132651c 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -68,7 +68,8 @@ global extrapolate_count = 0 global extrapolate_max_count = 0 global control_mode = MODE_UNINITIALIZED global trajectory_points_left = 0 -global last_spline_qdd = [0, 0, 0, 0, 0, 0] +global spline_qdd = [0, 0, 0, 0, 0, 0] +global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False # Global thread variables @@ -143,14 +144,14 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end -def cubicSplineRun(end_q, end_qd, time, last_point=False): +def cubicSplineRun(end_q, end_qd, time, is_last_point=False): local str = str_cat(end_q, str_cat(end_qd, time)) textmsg("Cubic spline arg: ", str) # Zero time means zero length and therefore no motion to execute if time > 0.0: local start_q = get_target_joint_positions() - local start_qd = get_target_joint_speeds() + local start_qd = spline_qd # Coefficients0 is not included, since we do not need to calculate the position local coefficients1 = start_qd @@ -158,19 +159,19 @@ def cubicSplineRun(end_q, end_qd, time, last_point=False): local coefficients3 = (2 * start_q - 2 * end_q + start_qd * time + end_qd * time) / pow(time, 3) local coefficients4 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] local coefficients5 = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, last_point) + jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point) end end -def quinticSplineRun(end_q, end_qd, end_qdd, time, last_point=False): +def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): local str = str_cat(end_q, str_cat(end_qd, str_cat(end_qdd, time))) textmsg("Quintic spline arg: ", str) # Zero time means zero length and therefore no motion to execute if time > 0.0: local start_q = get_target_joint_positions() - local start_qd = get_target_joint_speeds() - local start_qdd = last_spline_qdd + local start_qd = spline_qd + local start_qdd = spline_qdd # Pre-calculate coefficients local TIME2 = pow(time, 2) @@ -180,16 +181,16 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, last_point=False): local coefficients3 = (-20.0 * start_q + 20.0 * end_q - 3.0 * start_qdd * TIME2 + end_qdd * TIME2 - 12.0 * start_qd * time - 8.0 * end_qd * time) / (2.0 * pow(time, 3)) local coefficients4 = (30.0 * start_q - 30.0 * end_q + 3.0 * start_qdd * TIME2 - 2.0 * end_qdd * TIME2 + 16.0 * start_qd * time + 14.0 * end_qd * time) / (2.0 * pow(time, 4)) local coefficients5 = (-12.0 * start_q + 12.0 * end_q - start_qdd * TIME2 + end_qdd * TIME2 - 6.0 * start_qd * time - 6.0 * end_qd * time) / (2.0 * pow(time, 5)) - jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, last_point) + jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, time, is_last_point) end end -def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, last_point): +def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, is_last_point): # Initialize variables local splineTimerTraveled = 0.0 local scaled_step_time = get_steptime() local scaling_factor = 1.0 - local slowing_down = False + local is_slowing_down = False local slowing_down_time = 0.0 # Interpolate the spline in whole time steps @@ -205,23 +206,23 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c # If the velocity is too large close to the end of the trajectory we scale the # trajectory, such that we follow the positional part of the trajectory, but end with zero velocity. - if (time_left <= deceleration_time) and (last_point == True): - if slowing_down == False: + if (time_left <= deceleration_time) and (is_last_point == True): + if is_slowing_down == False: # Peek what the joint velocities will be if we take a full time step local qd = jointSplinePeek(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled + get_steptime()) # Compute the time left to decelerate if we take a full time step local x = deceleration_time - (time_left - get_steptime()) - slowing_down = checkSlowDownRequired(x, qd, max_speed, max_deceleration) + is_slowing_down = checkSlowDownRequired(x, qd, max_speed, max_deceleration) - if slowing_down == True: + if is_slowing_down == True: # This will ensure that we scale the trajectory right away slowing_down_time = time_left + get_steptime() textmsg("Velocity is too fast towards the end of the trajectory. The robot will be slowing down, while following the positional part of the trajectory.") end end - if slowing_down == True: + if is_slowing_down == True: # 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 @@ -229,25 +230,22 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c end splineTimerTraveled = splineTimerTraveled + scaled_step_time - # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled) - jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor) + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor, is_slowing_down) end # Make sure that we approach zero velocity slowly, when slowing down - if slowing_down == True: - local qd = get_actual_joint_speeds() - while norm(qd) > 0.00001: - local time_left = splineTotalTravelTime - splineTimerTraveled + if is_slowing_down == True: + 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 - # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled) - jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor) - - qd = get_actual_joint_speeds() + + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, get_steptime(), scaling_factor, is_slowing_down) end scaling_factor = 0.0 end @@ -260,14 +258,20 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c timeLeftToTravel = get_steptime() end - jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, timeLeftToTravel, scaling_factor) + jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTotalTravelTime, timeLeftToTravel, scaling_factor, is_slowing_down) end -def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor): - local qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 - last_spline_qdd = 2.0 * coefficients2 + 6.0 * splineTimerTraveled * coefficients3 + 12.0 * pow(splineTimerTraveled, 2) * coefficients4 + 20.0 * pow(splineTimerTraveled, 3) * coefficients5 - qd = qd * scaling_factor - speedj(qd, 1000, timestep) +def jointSplineStep(coefficients1, coefficients2, coefficients3, coefficients4, coefficients5, splineTimerTraveled, timestep, scaling_factor, is_slowing_down=False): + local last_spline_qd = spline_qd + spline_qd = coefficients1 + 2.0 * splineTimerTraveled * coefficients2 + 3.0 * pow(splineTimerTraveled, 2) * coefficients3 + 4.0 * pow(splineTimerTraveled, 3) * coefficients4 + 5.0 * pow(splineTimerTraveled, 4) * coefficients5 + spline_qdd = 2.0 * coefficients2 + 6.0 * splineTimerTraveled * coefficients3 + 12.0 * pow(splineTimerTraveled, 2) * coefficients4 + 20.0 * pow(splineTimerTraveled, 3) * coefficients5 + + spline_qd = spline_qd * scaling_factor + + # Calculate the max needed qdd arg for speedj to distribute the velocity change over the whole timestep no matter the speed slider value + qdd_max = list_max_norm((spline_qd - last_spline_qd) / timestep) + # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled) + speedj(spline_qd, qdd_max, timestep) end # Helper function to see what the velocity will be if we take a full step @@ -298,6 +302,33 @@ def checkSlowDownRequired(x, qd, max_speed, max_deceleration): return False end +### +# @brief Find the maximum value in a list the list must be of non-zero length and contain numbers +# @param list array list +### +def list_max_norm(list): + # ensure we have something to iterate over + local length = get_list_length(list) + if length < 1: + popup("Getting the maximum of an empty list is impossible in list_max().", error = True, blocking = True) + textmsg("Getting the maximum of an empty list is impossible in list_max()") + halt + end + + # search for maximum + local i = 1 + local max = norm(list[0]) + while i < length: + local tmp = norm(list[i]) + if tmp > max: + max = tmp + end + i = i + 1 + end + + return max +end + thread trajectoryThread(): textmsg("Executing trajectory. Number of points: ", trajectory_points_left) local INDEX_TIME = TRAJECTORY_DATA_DIMENSION @@ -305,7 +336,8 @@ thread trajectoryThread(): # same index as blend parameter, depending on point type local INDEX_SPLINE_TYPE = INDEX_BLEND local INDEX_POINT_TYPE = INDEX_BLEND + 1 - last_spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] enter_critical while trajectory_points_left > 0: #reading trajectory point + blend radius + type of point (cartesian/joint based) @@ -316,24 +348,26 @@ thread trajectoryThread(): local q = [ raw_point[1]/ MULT_jointstate, raw_point[2]/ MULT_jointstate, raw_point[3]/ MULT_jointstate, raw_point[4]/ MULT_jointstate, raw_point[5]/ MULT_jointstate, raw_point[6]/ MULT_jointstate] local tmptime = raw_point[INDEX_TIME] / MULT_time local blend_radius = raw_point[INDEX_BLEND] / MULT_time - local last_point = False + local is_last_point = False if trajectory_points_left == 0: blend_radius = 0.0 - last_point = True + is_last_point = True end # MoveJ point if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT: movej(q, t=tmptime, r=blend_radius) # reset old acceleration - last_spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] # Movel point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN: movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t=tmptime, r=blend_radius) # reset old acceleration - last_spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qd = [0, 0, 0, 0, 0, 0] # Joint spline point elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT_SPLINE: @@ -341,15 +375,15 @@ thread trajectoryThread(): # Cubic spline if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC: qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] - cubicSplineRun(q, qd, tmptime, last_point) + cubicSplineRun(q, qd, tmptime, is_last_point) # reset old acceleration - last_spline_qdd = [0, 0, 0, 0, 0, 0] + spline_qdd = [0, 0, 0, 0, 0, 0] # Quintic spline elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC: qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate] qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate] - quinticSplineRun(q, qd, qdd, tmptime, last_point) + quinticSplineRun(q, qd, qdd, tmptime, is_last_point) else: textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) clear_remaining_trajectory_points() @@ -359,6 +393,7 @@ thread trajectoryThread(): end end exit_critical + stopj(STOPJ_ACCELERATION) socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") textmsg("Trajectory finished") end diff --git a/tests/resources/rtde_output_recipe_spline.txt b/tests/resources/rtde_output_recipe_spline.txt index f47f17cf..aee5c3e1 100644 --- a/tests/resources/rtde_output_recipe_spline.txt +++ b/tests/resources/rtde_output_recipe_spline.txt @@ -29,3 +29,4 @@ tcp_offset output_double_register_1 target_q target_qd +target_qdd diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index f17c3a22..f28d8899 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -79,6 +79,17 @@ void handleTrajectoryState(control::TrajectoryResult state) } } +int sign(double val) +{ + return (0.0 < val) - (val < 0.0); +} + +bool nearly_equal(double a, double b, double eps = 1e-15) +{ + const double c(a - b); + return c < eps || -c < eps; +} + class SplineInterpolationTest : public ::testing::Test { protected: @@ -142,6 +153,9 @@ class SplineInterpolationTest : public ::testing::Test static void TearDownTestSuite() { + // Set target speed scaling to 100% as one test change this value + g_ur_driver_->getRTDEWriter().sendSpeedSlider(1); + g_dashboard_client_->disconnect(); // Remove temporary file again std::remove(SPLINE_SCRIPT_FILE.c_str()); @@ -156,6 +170,11 @@ class SplineInterpolationTest : public ::testing::Test } } + void sendIdle() + { + ASSERT_TRUE(g_ur_driver_->writeKeepalive(RobotReceiveTimeout::sec())); + } + void sendTrajectory(const std::vector& s_pos, const std::vector& s_vel, const std::vector& s_acc, const std::vector& s_time) { @@ -287,8 +306,64 @@ class SplineInterpolationTest : public ::testing::Test } } + void writeTrajectoryToFile(const char* filename, std::vector time_vec, + std::vector expected_positions, + std::vector actual_positions, + std::vector actual_velocities, std::vector actual_acc, + std::vector speed_scaling, std::vector spline_time) + { + std::ofstream outfile(filename); + // Header + outfile << "time, " + << "actual_positions0, " + << "actual_positions1, " + << "actual_positions2, " + << "actual_positions3, " + << "actual_positions4, " + << "actual_positions5, " + << "actual_velocities0, " + << "actual_velocities1, " + << "actual_velocities2, " + << "actual_velocities3, " + << "actual_velocities4, " + << "actual_velocities5, " + << "actual_acceleration0, " + << "actual_acceleration1, " + << "actual_acceleration2, " + << "actual_acceleration3, " + << "actual_acceleration4, " + << "actual_acceleration5, " + << "error_positions0, " + << "error_positions1, " + << "error_positions2, " + << "error_positions3, " + << "error_positions4, " + << "error_positions5, " + << "speed_scaling, " + << "spline_time" + << "\n"; + + // Data + for (unsigned int i = 0; i < actual_positions.size(); ++i) + { + outfile << time_vec[i] << ", " << actual_positions[i][0] << ", " << actual_positions[i][1] << ", " + << actual_positions[i][2] << ", " << actual_positions[i][3] << ", " << actual_positions[i][4] << ", " + << actual_positions[i][5] << ", " << actual_velocities[i][0] << ", " << actual_velocities[i][1] << ", " + << actual_velocities[i][2] << ", " << actual_velocities[i][3] << ", " << actual_velocities[i][4] << ", " + << actual_velocities[i][5] << ", " << actual_acc[i][0] << ", " << actual_acc[i][1] << ", " + << actual_acc[i][2] << ", " << actual_acc[i][3] << ", " << actual_acc[i][4] << ", " << actual_acc[i][5] + << ", " << actual_positions[i][0] - expected_positions[i][0] << ", " + << actual_positions[i][1] - expected_positions[i][1] << ", " + << 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] << ", " << spline_time[i] + << "\n"; + } + } + // Allowed difference between expected trajectory and actual robot trajectory - double eps_ = 0.02; + const double eps_ = 0.02; // Robot step time double step_time_; @@ -303,9 +378,14 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) std::unique_ptr data_pkg; readDataPackage(data_pkg); - urcl::vector6d_t joint_positions, joint_velocities; + urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + + double speed_scaling; + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + std::vector speed_scaling_vec; std::vector s_pos, s_vel; s_pos.push_back(joint_positions); @@ -320,8 +400,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) createSegment(s_time[0], joint_positions, s_pos[0], joint_velocities, s_vel[0]); // Data for logging - std::vector actual_positions, actual_velocities, expected_positions; - std::vector time_vec; + std::vector actual_positions, actual_velocities, actual_acc, expected_positions; + std::vector time_vec, spline_time; // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); @@ -335,9 +415,10 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) while (g_trajectory_running_) { readDataPackage(data_pkg); - ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); // Keep connection alive ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); @@ -377,7 +458,10 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) expected_positions.push_back(expected_joint_positions); actual_positions.push_back(joint_positions); actual_velocities.push_back(joint_velocities); + 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 @@ -388,27 +472,36 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); } - std::ofstream outfile("../test_artifacts/cubic_spline_with_end_point_velocity.txt"); - for (unsigned int i = 0; i < actual_positions.size(); ++i) - { - outfile << time_vec[i] << "," << actual_positions[i][0] << "," << actual_positions[i][1] << "," - << actual_positions[i][2] << "," << actual_positions[i][3] << "," << actual_positions[i][4] << "," - << actual_positions[i][5] << "," << actual_velocities[i][0] << "," << actual_velocities[i][1] << "," - << actual_velocities[i][2] << "," << actual_velocities[i][3] << "," << actual_velocities[i][4] << "," - << actual_velocities[i][5] << "," << expected_positions[i][0] << "," << expected_positions[i][1] << "," - << expected_positions[i][2] << "," << expected_positions[i][3] << "," << expected_positions[i][4] << "," - << expected_positions[i][5] << "\n"; - } + // 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, spline_time); } -TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) +TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_speedscaling) { + // Set speed scaling to 25% to test interpolation with speed scaling active + const unsigned int REDUSE_FACTOR(4); + g_ur_driver_->getRTDEWriter().sendSpeedSlider(1.0 / REDUSE_FACTOR); + std::unique_ptr data_pkg; readDataPackage(data_pkg); - urcl::vector6d_t joint_positions, joint_velocities; + // Align timestep + sendIdle(); + readDataPackage(data_pkg); + + urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + + double speed_scaling; + ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); + std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; s_pos.push_back(joint_positions); @@ -426,8 +519,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) createSegment(s_time[0], joint_positions, s_pos[0], joint_velocities, s_vel[0], zeros, s_acc[0]); // Data for logging - std::vector actual_positions, actual_velocities, expected_positions; - std::vector time_vec; + std::vector actual_positions, actual_velocities, actual_acc, expected_positions; + std::vector time_vec, spline_time; // Send trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); @@ -438,11 +531,17 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) double old_spline_travel_time = 0.0; double plot_time = 0.0; + unsigned int loop_count = 0; + bool init_acc_test = true; + urcl::vector6d_t last_joint_acc = joint_acc, last_change_acc; + const double EPS_ACC_CHANGE(1e-15); while (g_trajectory_running_) { readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg->getData("target_speed_fraction", speed_scaling)); // Read spline travel time from the robot double spline_travel_time = 0.0; @@ -459,8 +558,46 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); // Ensure that we follow the joint trajectory - urcl::vector6d_t expected_joint_positions; + urcl::vector6d_t expected_joint_positions, change_acc; interpolate(spline_travel_time, expected_joint_positions, coefficients); + + if (init_acc_test && loop_count == 0) + { + last_joint_acc = joint_acc; + } + else if (init_acc_test && last_joint_acc != joint_acc) + { + init_acc_test = false; + loop_count = 1; + last_joint_acc = joint_acc; + last_change_acc.fill(0.0); + } + + if (loop_count % REDUSE_FACTOR == 0) + { + last_joint_acc = joint_acc; + last_change_acc.fill(0.0); + } + else + { + for (unsigned int i = 0; i < last_joint_acc.size(); ++i) + { + change_acc[i] = joint_acc[i] - last_joint_acc[i]; + + if (!nearly_equal(change_acc[i], 0.0, EPS_ACC_CHANGE) && !nearly_equal(last_change_acc[i], 0.0, EPS_ACC_CHANGE)) + { + // Acceleration should only increase or be constant within one scaled timescale. + // It should not fluctuate to zero or overshoot + EXPECT_EQ(sign(last_change_acc[i]), sign(change_acc[i])) + << " acceleration change direction doing " + "one scaled step" + << loop_count << " Numbers:\n" + << last_change_acc[i] << " | " << change_acc[i] << "\n"; + } + } + last_change_acc = change_acc; + } + for (unsigned int i = 0; i < 6; ++i) { EXPECT_NEAR(expected_joint_positions[i], joint_positions[i], eps_); @@ -482,8 +619,12 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) expected_positions.push_back(expected_joint_positions); actual_positions.push_back(joint_positions); actual_velocities.push_back(joint_velocities); + 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; } // Make sure the velocity is zero when the trajectory has finished @@ -494,17 +635,14 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity) EXPECT_FLOAT_EQ(joint_velocities[i], 0.0); } - std::ofstream outfile("../test_artifacts/quintic_spline_with_end_point_velocity.txt"); - for (unsigned int i = 0; i < actual_positions.size(); ++i) - { - outfile << time_vec[i] << "," << actual_positions[i][0] << "," << actual_positions[i][1] << "," - << actual_positions[i][2] << "," << actual_positions[i][3] << "," << actual_positions[i][4] << "," - << actual_positions[i][5] << "," << actual_velocities[i][0] << "," << actual_velocities[i][1] << "," - << actual_velocities[i][2] << "," << actual_velocities[i][3] << "," << actual_velocities[i][4] << "," - << actual_velocities[i][5] << "," << expected_positions[i][0] << "," << expected_positions[i][1] << "," - << expected_positions[i][2] << "," << expected_positions[i][3] << "," << expected_positions[i][4] << "," - << expected_positions[i][5] << "\n"; - } + // 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, + spline_time); } TEST_F(SplineInterpolationTest, spline_interpolation_cubic) @@ -515,10 +653,15 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) std::cout << "Failed to get data package from robot" << std::endl; GTEST_FAIL(); } - urcl::vector6d_t joint_positions; + + urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); - urcl::vector6d_t joint_velocities; ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + + double speed_scaling; + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + std::vector speed_scaling_vec; std::vector s_pos, s_vel; urcl::vector6d_t first_point = { joint_positions[0], joint_positions[1], joint_positions[2], @@ -543,8 +686,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) createSegment(s_time[0], joint_positions, s_pos[0], joint_velocities, s_vel[0]); // Data for logging - std::vector actual_positions, actual_velocities, expected_positions; - std::vector time_vec; + std::vector actual_positions, actual_velocities, actual_acc, expected_positions; + std::vector time_vec, spline_time; // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, std::vector(), s_time); @@ -561,6 +704,9 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + double spline_travel_time = 0.0; data_pkg->getData("output_double_register_1", spline_travel_time); @@ -590,21 +736,20 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) expected_positions.push_back(expected_joint_positions); actual_positions.push_back(joint_positions); actual_velocities.push_back(joint_velocities); + 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_; } - std::ofstream outfile("../test_artifacts/spline_interpolation_cubic.txt"); - for (unsigned int i = 0; i < actual_positions.size(); ++i) - { - outfile << time_vec[i] << "," << actual_positions[i][0] << "," << actual_positions[i][1] << "," - << actual_positions[i][2] << "," << actual_positions[i][3] << "," << actual_positions[i][4] << "," - << actual_positions[i][5] << "," << actual_velocities[i][0] << "," << actual_velocities[i][1] << "," - << actual_velocities[i][2] << "," << actual_velocities[i][3] << "," << actual_velocities[i][4] << "," - << actual_velocities[i][5] << "," << expected_positions[i][0] << "," << expected_positions[i][1] << "," - << expected_positions[i][2] << "," << expected_positions[i][3] << "," << expected_positions[i][4] << "," - << expected_positions[i][5] << "\n"; - } + // 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, spline_time); } TEST_F(SplineInterpolationTest, spline_interpolation_quintic) @@ -612,9 +757,14 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) std::unique_ptr data_pkg; readDataPackage(data_pkg); - urcl::vector6d_t joint_positions, joint_velocities; + urcl::vector6d_t joint_positions, joint_velocities, joint_acc; ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + + double speed_scaling; + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + std::vector speed_scaling_vec; std::vector s_pos, s_vel, s_acc; urcl::vector6d_t first_point = { joint_positions[0], joint_positions[1], joint_positions[2], @@ -645,8 +795,8 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) createSegment(s_time[0], joint_positions, s_pos[0], joint_velocities, s_vel[0], zeros, s_acc[0]); // Data for logging - std::vector actual_positions, actual_velocities, expected_positions; - std::vector time_vec; + std::vector actual_positions, actual_velocities, actual_acc, expected_positions; + std::vector time_vec, spline_time; // Send the trajectory to the robot sendTrajectory(s_pos, s_vel, s_acc, s_time); @@ -664,6 +814,9 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); + ASSERT_TRUE(data_pkg->getData("target_qdd", joint_acc)); + ASSERT_TRUE(data_pkg->getData("speed_scaling", speed_scaling)); + double spline_travel_time = 0.0; data_pkg->getData("output_double_register_1", spline_travel_time); @@ -692,21 +845,20 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) expected_positions.push_back(expected_joint_positions); actual_positions.push_back(joint_positions); actual_velocities.push_back(joint_velocities); + 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_; } - std::ofstream outfile("../test_artifacts/spline_interpolation_quintic.txt"); - for (unsigned int i = 0; i < actual_positions.size(); ++i) - { - outfile << time_vec[i] << "," << actual_positions[i][0] << "," << actual_positions[i][1] << "," - << actual_positions[i][2] << "," << actual_positions[i][3] << "," << actual_positions[i][4] << "," - << actual_positions[i][5] << "," << actual_velocities[i][0] << "," << actual_velocities[i][1] << "," - << actual_velocities[i][2] << "," << actual_velocities[i][3] << "," << actual_velocities[i][4] << "," - << actual_velocities[i][5] << "," << expected_positions[i][0] << "," << expected_positions[i][1] << "," - << expected_positions[i][2] << "," << expected_positions[i][3] << "," << expected_positions[i][4] << "," - << expected_positions[i][5] << "\n"; - } + // 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, spline_time); } int main(int argc, char* argv[])