diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 0fb6a0eb..a1e429db 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -117,12 +117,20 @@ int main(int argc, char* argv[]) g_my_driver->startRTDECommunication(); - double increment = 0.01; + // Increment depends on robot version + double increment_constant = 0.0005; + if (g_my_driver->getVersion().major < 5) + { + increment_constant = 0.002; + } + double increment = increment_constant; - bool passed_slow_part = false; - bool passed_fast_part = false; + bool first_pass = true; + bool passed_negative_part = false; + bool passed_positive_part = false; URCL_LOG_INFO("Start moving the robot"); - while (!(passed_slow_part && passed_fast_part)) + urcl::vector6d_t joint_target = { 0, 0, 0, 0, 0, 0 }; + while (!(passed_positive_part && passed_negative_part)) { // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the // robot will effectively be in charge of setting the frequency of this loop. @@ -139,21 +147,33 @@ int main(int argc, char* argv[]) throw std::runtime_error(error_msg); } - // Simple motion command of last joint - if (g_joint_positions[5] > 3) + if (first_pass) + { + joint_target = g_joint_positions; + first_pass = false; + } + + // Open loop control. The target is incremented with a constant each control loop + if (passed_positive_part == false) { - passed_fast_part = increment > 0.01 || passed_fast_part; - increment = -3; // this large jump will activate speed scaling + increment = increment_constant; + if (g_joint_positions[5] >= 2) + { + passed_positive_part = true; + } } - else if (g_joint_positions[5] < -3) + else if (passed_negative_part == false) { - passed_slow_part = increment < 0.01 || passed_slow_part; - increment = 0.02; + increment = -increment_constant; + if (g_joint_positions[5] <= 0) + { + passed_negative_part = true; + } } - g_joint_positions[5] += increment; + joint_target[5] += increment; // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more // reliable on non-realtime systems. Use with caution in productive applications. - bool ret = g_my_driver->writeJointCommand(g_joint_positions, comm::ControlMode::MODE_SERVOJ, + bool ret = g_my_driver->writeJointCommand(joint_target, comm::ControlMode::MODE_SERVOJ, RobotReceiveTimeout::millisec(100)); if (!ret) { diff --git a/resources/external_control.urscript b/resources/external_control.urscript index d132651c..8624c489 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -58,7 +58,14 @@ UNTIL_TOOL_CONTACT_RESULT_CANCELED = 1 SPLINE_CUBIC = 1 SPLINE_QUINTIC = 2 +# Maximum allowable joint speed in rad/s +MAX_JOINT_SPEED = 6.283185 + +# Any motion commands resulting in a velocity higher than that will be ignored. +JOINT_IGNORE_SPEED = 20.0 + #Global variables are also showed in the Teach pendants variable list +global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_servo_q = get_actual_joint_positions() @@ -71,12 +78,69 @@ global trajectory_points_left = 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 trajectory_result = 0 # Global thread variables thread_move = 0 thread_trajectory = 0 thread_script_commands = 0 +### +# @brief Function to verify whether the specified target can be reached within the defined time frame while staying within +# the robot's speed limits +# +# @param target array is the joint target to reach +# @param time float is the time to reach the target +# +# @returns bool true if the target is reachable within the robot's speed limits, false otherwise +### +def targetWithinLimits(step_start, step_end, time): + local idx = 0 + while idx < 6: + local velocity = norm(step_end[idx] - step_start[idx]) / time + if velocity > JOINT_IGNORE_SPEED: + local str = str_cat(str_cat("Velocity ", velocity), str_cat(str_cat(" required to reach the received target ", step_end), str_cat(str_cat(" within ", time), " seconds is exceeding the joint velocity limits. Ignoring commands until a valid command is received."))) + if violation_popup_counter == 0: + # We want a popup when an invalid commant is sent. As long as we keep sending invalid + # commands, we do not want to repeat the popup. + popup(str, title="External Control error", blocking=False, error=True) + end + if violation_popup_counter * get_steptime() % 5.0 == 0: + # We want to have a log printed regularly. We are receiving motion commands that are not + # feasible. The user should have a chance to know about this. + textmsg(str) + end + violation_popup_counter = violation_popup_counter + 1 + return False + end + idx = idx + 1 + end + if violation_popup_counter > 0: + textmsg("Received valid command. Resuming execution.") + violation_popup_counter = 0 + end + return True +end + +def trajectory_result_to_str(trajectory_result): + if trajectory_result == TRAJECTORY_RESULT_SUCCESS: + return "SUCCESS" + end + if trajectory_result == TRAJECTORY_RESULT_CANCELED: + return "CANCELED" + end + if trajectory_result == TRAJECTORY_RESULT_FAILURE: + return "FAILURE" + end + return "UNKNOWN" +end + +def terminateProgram(): + textmsg("Terminating robot program due to targets sent to the robot is violating robot constraints.") + stopj(STOPJ_ACCELERATION) + halt +end + def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING cmd_servo_q_last = cmd_servo_q @@ -94,6 +158,7 @@ end thread servoThread(): textmsg("ExternalControl: Starting servo thread") state = SERVO_IDLE + local q_last = get_target_joint_positions() while control_mode == MODE_SERVOJ: enter_critical q = cmd_servo_q @@ -113,11 +178,17 @@ thread servoThread(): end q = extrapolate() - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + if targetWithinLimits(q_last, q, steptime): + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + q_last = q + end elif state == SERVO_RUNNING: extrapolate_count = 0 - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + if targetWithinLimits(q_last, q, steptime): + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + q_last = q + end else: extrapolate_count = 0 sync() @@ -144,13 +215,32 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end -def cubicSplineRun(end_q, end_qd, time, is_last_point=False): +# Function return value (bool) determines whether the robot is moving after this spline segment or +# not. +def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_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_q = get_target_joint_positions() + # Zero time means infinite velocity to reach the target and is therefore impossible + if time <= 0.0: + if is_first_point and time == 0.0: + # If users specify the current joint position with time 0 that may be fine, in that case just + # ignore that point + local distance = norm(end_q - get_target_joint_positions()) + if distance < 0.01: + local splineTimerTraveled = 0.0 + # USED_IN_TEST_SPLINE_INTERPOLATION write_output_float_register(1, splineTimerTraveled) + textmsg("Ignoring first point with time 0") + return False + end + end + error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion." + textmsg(error_str) + trajectory_result = TRAJECTORY_RESULT_CANCELED + popup(error_str, title="External Control error", blocking=False, error=True) + return False + elif targetWithinLimits(start_q, end_q, time): local start_qd = spline_qd # Coefficients0 is not included, since we do not need to calculate the position @@ -160,16 +250,36 @@ def cubicSplineRun(end_q, end_qd, time, is_last_point=False): 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, is_last_point) + return True and (is_last_point == False) + else: + trajectory_result = TRAJECTORY_RESULT_CANCELED + return False end end -def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): +# Function return value (bool) determines whether the robot is moving after this spline segment or +# not. +def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False, is_first_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_q = get_target_joint_positions() + # Zero time means infinite velocity to reach the target and is therefore impossible + if time <= 0.0: + if is_first_point and time == 0.0: + # If users specify the current joint position with time 0 that may be fine, in that case just + # ignore that point + local distance = norm(end_q - get_target_joint_positions()) + if distance < 0.01: + return False + end + end + error_str = "Spline time shouldn't be zero as it would require infinite velocity to reach the target. Canceling motion." + textmsg(error_str) + trajectory_result = TRAJECTORY_RESULT_CANCELED + popup(error_str, title="External Control error", blocking=False, error=True) + return False + elif targetWithinLimits(start_q, end_q, time): local start_qd = spline_qd local start_qdd = spline_qdd @@ -182,6 +292,10 @@ def quinticSplineRun(end_q, end_qd, end_qdd, time, is_last_point=False): 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, is_last_point) + return True and (is_last_point == False) + else: + trajectory_result = TRAJECTORY_RESULT_CANCELED + return False end end @@ -199,10 +313,8 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c # Maximum deceleration in rad/s^2 local max_deceleration = 15 - # Maximum allowable joint speed in rad/s - local max_speed = 6.283185 # The time needed to decelerate with 15 rad/s^2 to reach zero velocity when we move at maximum velocity. - local deceleration_time = max_speed / max_deceleration + local deceleration_time = MAX_JOINT_SPEED / max_deceleration # 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. @@ -213,7 +325,7 @@ def jointSplineRun(coefficients1, coefficients2, coefficients3, coefficients4, c 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()) - is_slowing_down = checkSlowDownRequired(x, qd, max_speed, max_deceleration) + is_slowing_down = checkSlowDownRequired(x, qd, max_deceleration) if is_slowing_down == True: # This will ensure that we scale the trajectory right away @@ -280,18 +392,22 @@ def jointSplinePeek(coefficients1, coefficients2, coefficients3, coefficients4, return qd end -# \brief Helper function to figure out if we need to slown down velocity at the last part of the last spline segment. + +### +# @brief Helper function to figure out if we need to slown down velocity at the last part of the last spline segment. # # The max_speed and max_deceleration is used to compute a linear velocity profile as function of the time left to decelerate, # then we can use the time left to decelerate to find the maximum allowed speed given the time we have left. If we are above # the maximum allowed speed, we start slowing down. # -# \param 'x' is the time left to decelerate. -# \param 'qd' is the velocity if we take a full step ahead in the spline. -# \param 'max_speed' maximum allowed joint speed 6.283185 rad/s -# \param 'max_deceleration' is the maximum allowed deceleration 15 rad/s^2. -def checkSlowDownRequired(x, qd, max_speed, max_deceleration): - local max_allowable_speed = max_speed - x * max_deceleration +# @param x float is the time left to decelerate +# @param qd array is the velocity if we take a full step ahead in the spline +# @param max_deceleration float is the maximum allowed deceleration 15 rad/s^2 +# +# @returns bool True if slow don is required, false otherwise +### +def checkSlowDownRequired(x, qd, max_deceleration): + local max_allowable_speed = MAX_JOINT_SPEED - x * max_deceleration local idx = 0 while idx < 6: if norm(qd[idx]) > max_allowable_speed: @@ -331,6 +447,8 @@ end thread trajectoryThread(): textmsg("Executing trajectory. Number of points: ", trajectory_points_left) + local is_first_point = True + local is_robot_moving = False local INDEX_TIME = TRAJECTORY_DATA_DIMENSION local INDEX_BLEND = INDEX_TIME + 1 # same index as blend parameter, depending on point type @@ -339,9 +457,15 @@ thread trajectoryThread(): spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] enter_critical - while trajectory_points_left > 0: + trajectory_result = TRAJECTORY_RESULT_SUCCESS + + while trajectory_result == TRAJECTORY_RESULT_SUCCESS and trajectory_points_left > 0: + local timeout = 0.5 + if is_robot_moving: + timeout = get_steptime() + end #reading trajectory point + blend radius + type of point (cartesian/joint based) - local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", get_steptime()) + local raw_point = socket_read_binary_integer(TRAJECTORY_DATA_DIMENSION+1+1, "trajectory_socket", timeout) trajectory_points_left = trajectory_points_left - 1 if raw_point[0] > 0: @@ -375,7 +499,7 @@ 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, is_last_point) + is_robot_moving = cubicSplineRun(q, qd, tmptime, is_last_point, is_first_point) # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] @@ -383,19 +507,23 @@ thread trajectoryThread(): 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, is_last_point) + is_robot_moving = quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point) else: textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE]) clear_remaining_trajectory_points() - socket_send_int(TRAJECTORY_RESULT_FAILURE, "trajectory_socket") + trajectory_result = TRAJECTORY_RESULT_FAILURE end end + else: + textmsg("Receiving trajectory point failed!") + trajectory_result = TRAJECTORY_RESULT_FAILURE end + is_first_point = False end exit_critical stopj(STOPJ_ACCELERATION) - socket_send_int(TRAJECTORY_RESULT_SUCCESS, "trajectory_socket") - textmsg("Trajectory finished") + socket_send_int(trajectory_result, "trajectory_socket") + textmsg("Trajectory finished with result ", trajectory_result_to_str(trajectory_result)) end def clear_remaining_trajectory_points(): @@ -424,6 +552,7 @@ end thread servoThreadP(): textmsg("Starting pose servo thread") state = SERVO_IDLE + local q_last = get_target_joint_positions() while control_mode == MODE_POSE: enter_critical q = cmd_servo_q @@ -443,11 +572,15 @@ thread servoThreadP(): end q = extrapolate() - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + if targetWithinLimits(q_last, q, steptime): + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + end elif state == SERVO_RUNNING: extrapolate_count = 0 - servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + if targetWithinLimits(q_last, q, steptime): + servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + end else: extrapolate_count = 0 sync() @@ -532,13 +665,14 @@ end # HEADER_END # NODE_CONTROL_LOOP_BEGINS -socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") -socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") -socket_open("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket") - force_mode_set_damping({{FORCE_MODE_SET_DAMPING_REPLACE}}) force_mode_set_gain_scaling({{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}) +socket_open("{{SERVER_IP_REPLACE}}", {{TRAJECTORY_SERVER_PORT_REPLACE}}, "trajectory_socket") +socket_open("{{SERVER_IP_REPLACE}}", {{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}, "script_command_socket") +# This socket should be opened last as it tells the driver when it has control over the robot +socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") + control_mode = MODE_UNINITIALIZED thread_move = 0 thread_trajectory = 0 @@ -550,7 +684,7 @@ while control_mode > MODE_STOPPED: enter_critical params_mult = socket_read_binary_integer(REVERSE_INTERFACE_DATA_DIMENSION, "reverse_socket", read_timeout) if params_mult[0] > 0: - # Convert to read timeout from milliseconds to seconds + # Convert read timeout from milliseconds to seconds read_timeout = params_mult[1] / 1000.0 if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]: # Clear remaining trajectory points diff --git a/tests/test_spline_interpolation.cpp b/tests/test_spline_interpolation.cpp index 47e3935e..28aa5c6b 100644 --- a/tests/test_spline_interpolation.cpp +++ b/tests/test_spline_interpolation.cpp @@ -39,6 +39,8 @@ #include #include #include +#include +#include using namespace urcl; @@ -51,31 +53,60 @@ std::string ROBOT_IP = "192.168.56.101"; std::unique_ptr g_ur_driver_; std::unique_ptr g_dashboard_client_; -bool g_trajectory_running_; + +bool g_program_running; +std::condition_variable g_program_not_running_cv_; +std::mutex g_program_not_running_mutex_; +std::condition_variable g_program_running_cv_; +std::mutex g_program_running_mutex_; // Helper functions for the driver void handleRobotProgramState(bool program_running) { // Print the text in green so we see it better std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; + if (program_running) + { + std::lock_guard lk(g_program_running_mutex_); + g_program_running = program_running; + g_program_running_cv_.notify_one(); + } + else + { + std::lock_guard lk(g_program_not_running_mutex_); + g_program_running = program_running; + g_program_not_running_cv_.notify_one(); + } } +bool g_trajectory_running_; +control::TrajectoryResult g_trajectory_result_; void handleTrajectoryState(control::TrajectoryResult state) { + g_trajectory_result_ = state; g_trajectory_running_ = false; - std::string report = "?"; - switch (state) +} + +bool g_rtde_read_thread_running_ = false; +bool g_consume_rtde_packages_ = false; +std::mutex g_read_package_mutex_; +std::thread g_rtde_read_thread; + +void rtdeConsumeThread() +{ + while (g_rtde_read_thread_running_) { - case control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: - report = "success"; - break; - case control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: - report = "canceled"; - break; - case control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: - default: - report = "failure"; - break; + // Consume package to prevent pipeline overflow + if (g_consume_rtde_packages_ == true) + { + std::lock_guard lk(g_read_package_mutex_); + std::unique_ptr data_pkg; + data_pkg = g_ur_driver_->getDataPackage(); + } + else + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } } } @@ -149,10 +180,18 @@ class SplineInterpolationTest : public ::testing::Test g_ur_driver_->registerTrajectoryDoneCallback(&handleTrajectoryState); g_ur_driver_->startRTDECommunication(); + // Setup rtde read thread + g_rtde_read_thread_running_ = true; + g_rtde_read_thread = std::thread(rtdeConsumeThread); } static void TearDownTestSuite() { + // Set target speed scaling to 100% as one test change this value + g_ur_driver_->getRTDEWriter().sendSpeedSlider(1); + + g_rtde_read_thread_running_ = false; + g_rtde_read_thread.join(); g_dashboard_client_->disconnect(); // Remove temporary file again std::remove(SPLINE_SCRIPT_FILE.c_str()); @@ -171,6 +210,14 @@ class SplineInterpolationTest : public ::testing::Test { step_time_ = 0.008; } + // Make sure script is running on the robot + if (g_program_running == false) + { + g_consume_rtde_packages_ = true; + g_ur_driver_->sendRobotProgram(); + ASSERT_TRUE(waitForProgramRunning(1000)); + } + g_consume_rtde_packages_ = false; } void sendIdle() @@ -299,12 +346,40 @@ class SplineInterpolationTest : public ::testing::Test } } + bool waitForProgramRunning(int milliseconds = 100) + { + std::unique_lock lk(g_program_running_mutex_); + if (g_program_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == true) + { + return true; + } + return false; + } + + bool waitForProgramNotRunning(int milliseconds = 100) + { + std::unique_lock lk(g_program_not_running_mutex_); + if (g_program_not_running_cv_.wait_for(lk, std::chrono::milliseconds(milliseconds)) == std::cv_status::no_timeout || + g_program_running == false) + { + return true; + } + return false; + } + void readDataPackage(std::unique_ptr& data_pkg) { + if (g_consume_rtde_packages_ == true) + { + URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); + GTEST_FAIL(); + } + std::lock_guard lk(g_read_package_mutex_); data_pkg = g_ur_driver_->getDataPackage(); if (data_pkg == nullptr) { - std::cout << "Failed to get data package from robot" << std::endl; + URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); GTEST_FAIL(); } } @@ -467,6 +542,8 @@ TEST_F(SplineInterpolationTest, cubic_spline_with_end_point_velocity) spline_time.push_back(spline_travel_time); plot_time += step_time_; } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + // Make sure the velocity is zero when the trajectory has finished readDataPackage(data_pkg); ASSERT_TRUE(data_pkg->getData("target_qd", joint_velocities)); @@ -629,6 +706,7 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee plot_time += step_time_; loop_count += 1; } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); // Make sure the velocity is zero when the trajectory has finished readDataPackage(data_pkg); @@ -650,13 +728,8 @@ TEST_F(SplineInterpolationTest, quintic_spline_with_end_point_velocity_with_spee TEST_F(SplineInterpolationTest, spline_interpolation_cubic) { - std::unique_ptr data_pkg = g_ur_driver_->getDataPackage(); - if (data_pkg == nullptr) - { - std::cout << "Failed to get data package from robot" << std::endl; - GTEST_FAIL(); - } - + std::unique_ptr data_pkg; + 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)); @@ -745,6 +818,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_cubic) spline_time.push_back(spline_travel_time); plot_time += step_time_; } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); // Verify that the full trajectory have been executed double spline_travel_time; @@ -854,6 +928,7 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) spline_time.push_back(spline_travel_time); plot_time += step_time_; } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); // Verify that the full trajectory have been executed double spline_travel_time; @@ -864,6 +939,262 @@ TEST_F(SplineInterpolationTest, spline_interpolation_quintic) actual_positions, actual_velocities, actual_acc, speed_scaling_vec, spline_time); } +TEST_F(SplineInterpolationTest, zero_time_trajectory_cubic_spline) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + 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 + g_consume_rtde_packages_ = true; + + // Create illegal trajectory + std::vector s_pos, s_vel; + urcl::vector6d_t first_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.1 + }; + urcl::vector6d_t second_point = { joint_positions_before[0], joint_positions_before[1], + joint_positions_before[2], joint_positions_before[3], + joint_positions_before[4], joint_positions_before[5] - 0.5 }; + s_pos.push_back(first_point); + s_pos.push_back(second_point); + + urcl::vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + s_vel.push_back(zeros); + s_vel.push_back(zeros); + + std::vector s_time = { 0.0, 1.0 }; + + // Send illegal trajectory to the robot + sendTrajectory(s_pos, s_vel, std::vector(), s_time); + 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 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 + g_consume_rtde_packages_ = false; + + // Ensure that the robot hasn't moved + readDataPackage(data_pkg); + urcl::vector6d_t joint_positions_after; + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); + } + + // If the first point is very close to the current position, then a 0 time for that is fine + first_point = { joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] }; + second_point = { -1.6, -1.72, -2.175, -0.78, 1.623, -0.5 }; + urcl::vector6d_t third_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.1 + }; + 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(), s_time); + g_trajectory_running_ = true; + waitForTrajectoryStarted(); + urcl::vector6d_t joint_positions; + while (g_trajectory_running_) + { + readDataPackage(data_pkg); + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + // Keep connection alive + ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_NEAR(joint_positions[i], third_point[i], eps_); + } +} + +TEST_F(SplineInterpolationTest, zero_time_trajectory_quintic_spline) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + 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 + g_consume_rtde_packages_ = true; + + // Create illegal trajectory + std::vector s_pos, s_vel, s_acc; + urcl::vector6d_t first_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.2 + }; + s_pos.push_back(first_point); + + urcl::vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + s_vel.push_back(zeros); + s_acc.push_back(zeros); + + std::vector s_time = { 0.0 }; + + // Send illegal trajectory to the robot + sendTrajectory(s_pos, s_vel, s_acc, s_time); + 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 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 + g_consume_rtde_packages_ = false; + + // Ensure that the robot hasn't moved + readDataPackage(data_pkg); + urcl::vector6d_t joint_positions_after; + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); + } + // + // If the first point is very close to the current position, then a 0 time for that is fine + first_point = { joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] }; + urcl::vector6d_t second_point = { -1.6, -1.72, -2.175, -0.78, 1.623, -0.5 }; + urcl::vector6d_t third_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.1 + }; + s_pos = { first_point, second_point, third_point }; + 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(); + urcl::vector6d_t joint_positions; + while (g_trajectory_running_) + { + readDataPackage(data_pkg); + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions)); + // Keep connection alive + ASSERT_TRUE(g_ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP)); + } + EXPECT_EQ(control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS, g_trajectory_result_); + + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_NEAR(joint_positions[i], third_point[i], eps_); + } +} + +TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_cubic_spline) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + 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 + g_consume_rtde_packages_ = true; + + // Create a trajectory that cannot be executed within the robots limits + std::vector s_pos, s_vel; + urcl::vector6d_t first_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.5 + }; + s_pos.push_back(first_point); + + urcl::vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + s_vel.push_back(zeros); + + std::vector s_time = { 0.02 }; + + // Send unfeasible trajectory to the robot + sendTrajectory(s_pos, s_vel, std::vector(), s_time); + 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 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 + g_consume_rtde_packages_ = false; + + // Ensure that the robot hasn't moved + readDataPackage(data_pkg); + urcl::vector6d_t joint_positions_after; + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); + } +} + +TEST_F(SplineInterpolationTest, physically_unfeasible_trajectory_quintic_spline) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + 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 + g_consume_rtde_packages_ = true; + + // Create a trajectory that cannot be executed within the robots limits + std::vector s_pos, s_vel, s_acc; + urcl::vector6d_t first_point = { + joint_positions_before[0], joint_positions_before[1], joint_positions_before[2], + joint_positions_before[3], joint_positions_before[4], joint_positions_before[5] + 0.5 + }; + s_pos.push_back(first_point); + + urcl::vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + s_vel.push_back(zeros); + s_acc.push_back(zeros); + + std::vector s_time = { 0.02 }; + + // Send unfeasible trajectory to the robot + sendTrajectory(s_pos, s_vel, s_acc, s_time); + 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 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 + g_consume_rtde_packages_ = false; + + // Ensure that the robot hasn't moved + readDataPackage(data_pkg); + urcl::vector6d_t joint_positions_after; + ASSERT_TRUE(data_pkg->getData("target_q", joint_positions_after)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions_after[i]); + } +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv); @@ -878,4 +1209,4 @@ int main(int argc, char* argv[]) } return RUN_ALL_TESTS(); -} \ No newline at end of file +} diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index 872dc4b5..d193daf5 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -70,6 +70,29 @@ void handleRobotProgramState(bool program_running) } } +bool g_rtde_read_thread_running_ = false; +bool g_consume_rtde_packages_ = false; +std::mutex g_read_package_mutex_; +std::thread g_rtde_read_thread; + +void rtdeConsumeThread() +{ + while (g_rtde_read_thread_running_) + { + // Consume package to prevent pipeline overflow + if (g_consume_rtde_packages_ == true) + { + std::lock_guard lk(g_read_package_mutex_); + std::unique_ptr data_pkg; + data_pkg = g_ur_driver_->getDataPackage(); + } + else + { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + } +} + class UrDriverTest : public ::testing::Test { protected: @@ -106,25 +129,48 @@ class UrDriverTest : public ::testing::Test g_ur_driver_.reset(new UrDriver(ROBOT_IP, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); } + g_ur_driver_->startRTDECommunication(); + // Setup rtde read thread + g_rtde_read_thread_running_ = true; + g_rtde_read_thread = std::thread(rtdeConsumeThread); } - void setUp() + void SetUp() { - g_ur_driver_->stopControl(); - waitForProgramNotRunning(1000); + step_time_ = 0.002; + if (g_ur_driver_->getVersion().major < 5) + { + step_time_ = 0.008; + } + // Make sure script is running on the robot + if (g_program_running == false) + { + g_consume_rtde_packages_ = true; + g_ur_driver_->sendRobotProgram(); + ASSERT_TRUE(waitForProgramRunning(1000)); + } + g_consume_rtde_packages_ = false; } static void TearDownTestSuite() { + g_rtde_read_thread_running_ = false; + g_rtde_read_thread.join(); g_dashboard_client_->disconnect(); } void readDataPackage(std::unique_ptr& data_pkg) { + if (g_consume_rtde_packages_ == true) + { + URCL_LOG_ERROR("Unable to read packages while consuming, this should not happen!"); + GTEST_FAIL(); + } + std::lock_guard lk(g_read_package_mutex_); data_pkg = g_ur_driver_->getDataPackage(); if (data_pkg == nullptr) { - std::cout << "Failed to get data package from robot" << std::endl; + URCL_LOG_ERROR("Timed out waiting for a new package from the robot"); GTEST_FAIL(); } } @@ -150,16 +196,21 @@ class UrDriverTest : public ::testing::Test } return false; } + + // Robot step time + double step_time_; }; TEST_F(UrDriverTest, read_non_existing_script_file) { + g_consume_rtde_packages_ = true; const std::string non_existing_script_file = ""; EXPECT_THROW(UrDriver::readScriptFile(non_existing_script_file), UrException); } TEST_F(UrDriverTest, read_existing_script_file) { + g_consume_rtde_packages_ = true; char existing_script_file[] = "urscript.XXXXXX"; int fd = mkstemp(existing_script_file); if (fd == -1) @@ -176,9 +227,7 @@ TEST_F(UrDriverTest, read_existing_script_file) TEST_F(UrDriverTest, robot_receive_timeout) { - // Start robot program - g_ur_driver_->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_consume_rtde_packages_ = true; // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; @@ -214,9 +263,7 @@ TEST_F(UrDriverTest, robot_receive_timeout) TEST_F(UrDriverTest, robot_receive_timeout_off) { - // Start robot program - g_ur_driver_->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_consume_rtde_packages_ = true; // Program should keep running when setting receive timeout off g_ur_driver_->writeKeepalive(RobotReceiveTimeout::off()); @@ -240,9 +287,7 @@ TEST_F(UrDriverTest, robot_receive_timeout_off) TEST_F(UrDriverTest, stop_robot_control) { - // Start robot program - g_ur_driver_->sendRobotProgram(); - EXPECT_TRUE(waitForProgramRunning(1000)); + g_consume_rtde_packages_ = true; vector6d_t zeros = { 0, 0, 0, 0, 0, 0 }; g_ur_driver_->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::off()); @@ -252,6 +297,68 @@ TEST_F(UrDriverTest, stop_robot_control) EXPECT_TRUE(waitForProgramNotRunning(400)); } +TEST_F(UrDriverTest, target_outside_limits_servoj) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + urcl::vector6d_t joint_positions_before; + ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions_before)); + + // Create physically unfeasible target + urcl::vector6d_t joint_target = joint_positions_before; + joint_target[5] -= 2.5; + + // 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); + urcl::vector6d_t joint_positions; + ASSERT_TRUE(data_pkg->getData("actual_q", joint_positions)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(joint_positions_before[i], joint_positions[i]); + } + + // Make sure the program is stopped + g_consume_rtde_packages_ = true; + g_ur_driver_->stopControl(); + waitForProgramNotRunning(1000); +} + +TEST_F(UrDriverTest, target_outside_limits_pose) +{ + std::unique_ptr data_pkg; + readDataPackage(data_pkg); + + urcl::vector6d_t tcp_pose_before; + ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose_before)); + + // Create physically unfeasible target + urcl::vector6d_t tcp_target = tcp_pose_before; + tcp_target[2] += 0.3; + + // Send unfeasible targets to the robot + readDataPackage(data_pkg); + g_ur_driver_->writeJointCommand(tcp_target, comm::ControlMode::MODE_POSE, RobotReceiveTimeout::millisec(200)); + + // Ensure that the robot didn't move + readDataPackage(data_pkg); + urcl::vector6d_t tcp_pose; + ASSERT_TRUE(data_pkg->getData("actual_TCP_pose", tcp_pose)); + for (unsigned int i = 0; i < 6; ++i) + { + EXPECT_FLOAT_EQ(tcp_pose_before[i], tcp_pose[i]); + } + + // Make sure the program is stopped + g_consume_rtde_packages_ = true; + g_ur_driver_->stopControl(); + waitForProgramNotRunning(1000); +} + // TODO we should add more tests for the UrDriver class. int main(int argc, char* argv[])