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[])