diff --git a/examples/freedrive_example.cpp b/examples/freedrive_example.cpp index e03e46aff..80aec548f 100644 --- a/examples/freedrive_example.cpp +++ b/examples/freedrive_example.cpp @@ -53,14 +53,44 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; std::unique_ptr g_my_robot; -void sendFreedriveMessageOrDie(const control::FreedriveControlMessage freedrive_action) +void runFreedrive(UrDriver& urdriver, std::chrono::seconds duration) { - bool ret = g_my_robot->getUrDriver()->writeFreedriveControlMessage(freedrive_action); - if (!ret) + URCL_LOG_INFO("Starting freedrive mode"); + + urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_START); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration || duration.count() == 0) + { + // Keeping the robot in freedrive by sending NOOP messages + urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + + urdriver.writeFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_STOP); + URCL_LOG_INFO("Stopping freedrive mode"); +} + +void runConstrainedFreedrive(UrDriver& urdriver, std::array& free_axes, std::array& feature_pose, + std::chrono::seconds duration) +{ + URCL_LOG_INFO("Starting constrained freedrive mode"); + + urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_START, free_axes, + feature_pose); + + auto start = std::chrono::steady_clock::now(); + while (std::chrono::steady_clock::now() - start < duration || duration.count() == 0) { - URCL_LOG_ERROR("Could not send joint command. Is there an external_control program running on the robot?"); - exit(1); + // Keeping the robot in constrained freedrive by sending NOOP messages + urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, free_axes, + feature_pose); + std::this_thread::sleep_for(std::chrono::milliseconds(2)); } + + urdriver.writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_STOP, free_axes, + feature_pose); + URCL_LOG_INFO("Stopping constrained freedrive mode"); } int main(int argc, char* argv[]) @@ -73,15 +103,26 @@ int main(int argc, char* argv[]) robot_ip = std::string(argv[1]); } + // Select the freedrive mode + bool constrained = false; + if (argc > 2) + { + std::string arg = argv[2]; + constrained = (arg == "true" || arg == "1"); + } + // Parse how many seconds to run auto second_to_run = std::chrono::seconds(0); - if (argc > 2) + if (argc > 3) { - second_to_run = std::chrono::seconds(std::stoi(argv[2])); + second_to_run = std::chrono::seconds(std::stoi(argv[3])); } - bool headless_mode = true; + std::array free_axes = { 1, 0, 1, 1, 0, 1 }; + std::array feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + // Initialize robot connection + bool headless_mode = true; g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, "external_control.urp"); @@ -91,24 +132,13 @@ int main(int argc, char* argv[]) return 1; } - URCL_LOG_INFO("Starting freedrive mode"); - sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_START); - - std::chrono::duration time_done(0); - std::chrono::duration timeout(second_to_run); - auto stopwatch_last = std::chrono::steady_clock::now(); - auto stopwatch_now = stopwatch_last; - - while (time_done < timeout || second_to_run.count() == 0) + // Execute selected freedrive mode + if (constrained) { - sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_NOOP); - - stopwatch_now = std::chrono::steady_clock::now(); - time_done += stopwatch_now - stopwatch_last; - stopwatch_last = stopwatch_now; - std::this_thread::sleep_for(std::chrono::milliseconds(2)); + runConstrainedFreedrive(*g_my_robot->getUrDriver(), free_axes, feature_pose, second_to_run); + } + else + { + runFreedrive(*g_my_robot->getUrDriver(), second_to_run); } - - URCL_LOG_INFO("Stopping freedrive mode"); - sendFreedriveMessageOrDie(control::FreedriveControlMessage::FREEDRIVE_STOP); } diff --git a/include/ur_client_library/control/reverse_interface.h b/include/ur_client_library/control/reverse_interface.h index c0bef8e34..c22da2f3d 100644 --- a/include/ur_client_library/control/reverse_interface.h +++ b/include/ur_client_library/control/reverse_interface.h @@ -140,12 +140,18 @@ class ReverseInterface * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off() * function to create the RobotReceiveTimeout object + * \param free_axes A 6-dimensional vector (x, y, z, rx, ry, rz) defining which axes are compliant. + * Use 1 to enable movement in an axis and 0 to lock it. + * \param feature_pose A pose vector [x, y, z, rx, ry, rz] defining the freedrive frame relative to the base frame. + * Position values (x, y, z) should be in meters, and orientation values (rx, ry, rz) in radians. * * \returns True, if the write was performed successfully, false otherwise. */ bool writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action, - const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200)); + const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200), + const std::array& free_axes = { 1, 1, 1, 1, 1, 1 }, + const std::array& feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }); /*! * \brief Set the Keepalive count. This will set the number of allowed timeout reads on the robot. @@ -228,7 +234,7 @@ class ReverseInterface return s; } - static const int MAX_MESSAGE_LENGTH = 8; + static const int MAX_MESSAGE_LENGTH = 15; std::function handle_program_state_; std::chrono::milliseconds step_time_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 9098a5e5d..ac9060122 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -611,6 +611,26 @@ class UrDriver writeFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action, const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200)); + /*! + * \brief Writes a control message in freedrive mode. + * + * \param freedrive_action The action to be taken, such as starting or stopping freedrive + * \param free_axes A 6-dimensional vector (x, y, z, rx, ry, rz) defining which axes are compliant. + * Use 1 to enable movement in an axis and 0 to lock it. + * \param feature_pose A pose vector [x, y, z, rx, ry, rz] defining the freedrive frame relative to the base frame. + * Position values (x, y, z) should be in meters, and orientation values (rx, ry, rz) in radians. + * \param robot_receive_timeout The read timeout configuration for the reverse socket running in the external + * control script on the robot. If you want to make the read function blocking then use RobotReceiveTimeout::off() + * function to create the RobotReceiveTimeout object + * + * \returns True on successful write. + */ + bool writeConstrainedFreedriveControlMessage( + const control::FreedriveControlMessage freedrive_action, + const std::array& free_axes = { 1, 1, 1, 1, 1, 1 }, + const std::array& feature_pose = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }, + const RobotReceiveTimeout& robot_receive_timeout = RobotReceiveTimeout::millisec(200)); + /*! * \brief Zero the force torque sensor (only availbe on e-Series). Note: It requires the external control script to * be running or the robot to be in headless mode diff --git a/resources/external_control.urscript b/resources/external_control.urscript index dc2c6fe69..9c8449e48 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -29,7 +29,7 @@ MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 # Data dimensions of the message received on the reverse interface -REVERSE_INTERFACE_DATA_DIMENSION = 8 +REVERSE_INTERFACE_DATA_DIMENSION = 15 TRAJECTORY_MODE_RECEIVE = 1 TRAJECTORY_MODE_CANCEL = -1 @@ -1142,7 +1142,9 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_FREEDRIVE: if params_mult[2] == FREEDRIVE_MODE_START: textmsg("Entering freedrive mode") - freedrive_mode() + free_axes = [params_mult[3], params_mult[4], params_mult[5], params_mult[6], params_mult[7], params_mult[8]] + feature_pose = p[params_mult[9]/MULT_jointstate, params_mult[10]/MULT_jointstate, params_mult[11]/MULT_jointstate, params_mult[12]/MULT_jointstate, params_mult[13]/MULT_jointstate, params_mult[14]/MULT_jointstate] + freedrive_mode(free_axes, feature_pose) elif params_mult[2] == FREEDRIVE_MODE_STOP: textmsg("Leaving freedrive mode") end_freedrive_mode() diff --git a/src/control/reverse_interface.cpp b/src/control/reverse_interface.cpp index 0cd8ad928..4575155bd 100644 --- a/src/control/reverse_interface.cpp +++ b/src/control/reverse_interface.cpp @@ -169,9 +169,11 @@ bool ReverseInterface::writeTrajectoryControlMessage(const TrajectoryControlMess } bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessage freedrive_action, - const RobotReceiveTimeout& robot_receive_timeout) + const RobotReceiveTimeout& robot_receive_timeout, + const std::array& free_axes, + const std::array& feature_pose) { - const int message_length = 2; + const int message_length = 14; if (client_fd_ == INVALID_SOCKET) { return false; @@ -197,6 +199,20 @@ bool ReverseInterface::writeFreedriveControlMessage(const FreedriveControlMessag val = htobe32(toUnderlying(freedrive_action)); b_pos += append(b_pos, val); + // Add allowed axes for movement + for (int32_t axis : free_axes) + { + val = htobe32(axis); + b_pos += append(b_pos, val); + } + + // Add feature pose scaled to microns microradians to avoid precision loss in integer conversion + for (double p : feature_pose) + { + val = htobe32(static_cast(round(p * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + // writing zeros to allow usage with other script commands for (size_t i = message_length; i < MAX_MESSAGE_LENGTH - 1; i++) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 880051339..c159c6894 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -266,6 +266,15 @@ bool UrDriver::writeFreedriveControlMessage(const control::FreedriveControlMessa return reverse_interface_->writeFreedriveControlMessage(freedrive_action, robot_receive_timeout); } +bool UrDriver::writeConstrainedFreedriveControlMessage(const control::FreedriveControlMessage freedrive_action, + const std::array& free_axes, + const std::array& feature_pose, + const RobotReceiveTimeout& robot_receive_timeout) +{ + return reverse_interface_->writeFreedriveControlMessage(freedrive_action, robot_receive_timeout, free_axes, + feature_pose); +} + bool UrDriver::zeroFTSensor() { if (getVersion().major < 5) diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 15f7ca246..b545ae886 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -43,6 +43,8 @@ std::condition_variable g_connection_condition; class TestableReverseInterface : public control::ReverseInterface { public: + using control::ReverseInterface::MAX_MESSAGE_LENGTH; + TestableReverseInterface(const control::ReverseInterfaceConfig& config) : control::ReverseInterface(config) { } @@ -85,11 +87,13 @@ class ReverseInterfaceTest : public ::testing::Test void readMessage(int32_t& read_timeout, vector6int32_t& pos, int32_t& control_mode) { + constexpr size_t MAX_MESSAGE_LENGTH = TestableReverseInterface::MAX_MESSAGE_LENGTH; + // Read message - uint8_t buf[sizeof(int32_t) * 8]; + uint8_t buf[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; uint8_t* b_pos = buf; size_t read = 0; - size_t remainder = sizeof(int32_t) * 8; + size_t remainder = sizeof(int32_t) * MAX_MESSAGE_LENGTH; while (remainder > 0) { if (!TCPSocket::read(b_pos, remainder, read)) @@ -116,6 +120,8 @@ class ReverseInterfaceTest : public ::testing::Test b_pos += sizeof(int32_t); } + b_pos = buf + (sizeof(int32_t) * (MAX_MESSAGE_LENGTH - 1)); + // Decode control mode std::memcpy(&val, b_pos, sizeof(int32_t)); control_mode = be32toh(val); @@ -478,6 +484,22 @@ TEST_F(ReverseInterfaceTest, write_freedrive_control_message) EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message); } +TEST_F(ReverseInterfaceTest, write_constrained_freedrive_message) +{ + // Wait for the client to connect to the server + EXPECT_TRUE(waitForProgramState(1000, true)); + control::FreedriveControlMessage written_freedrive_message = control::FreedriveControlMessage::FREEDRIVE_START; + + std::array free_axes = { 1, 0, 1, 0, 1, 0 }; + std::array feature_pos = { 0.1, -0.2, 0.5, 1.1, 0.0, 3.14 }; + + reverse_interface_->writeFreedriveControlMessage(written_freedrive_message, RobotReceiveTimeout::millisec(200), + free_axes, feature_pos); + + int32_t received_freedrive_message = client_->getFreedriveControlMode(); + EXPECT_EQ(toUnderlying(written_freedrive_message), received_freedrive_message); +} + TEST_F(ReverseInterfaceTest, deprecated_set_keep_alive_count) { // Wait for the client to connect to the server diff --git a/tests/test_ur_driver.cpp b/tests/test_ur_driver.cpp index f6e299134..a7d1bfd3e 100644 --- a/tests/test_ur_driver.cpp +++ b/tests/test_ur_driver.cpp @@ -155,8 +155,11 @@ TEST_F(UrDriverTest, read_existing_script_file) TEST_F(UrDriverTest, robot_receive_timeout) { // 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 }; - g_my_robot->getUrDriver()->writeJointCommand(zeros, comm::ControlMode::MODE_IDLE, RobotReceiveTimeout::millisec(200)); + std::array zeros_int = { 0, 0, 0, 0, 0, 0 }; + std::array zeros_double = { 0, 0, 0, 0, 0, 0 }; + + g_my_robot->getUrDriver()->writeJointCommand(zeros_double, comm::ControlMode::MODE_IDLE, + RobotReceiveTimeout::millisec(200)); EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); // Start robot program @@ -172,6 +175,15 @@ TEST_F(UrDriverTest, robot_receive_timeout) g_my_robot->resendRobotProgram(); EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); + // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important + g_my_robot->getUrDriver()->writeConstrainedFreedriveControlMessage( + control::FreedriveControlMessage::FREEDRIVE_NOOP, zeros_int, zeros_double, RobotReceiveTimeout::millisec(200)); + EXPECT_TRUE(g_my_robot->waitForProgramNotRunning(400)); + + // Start robot program + g_my_robot->resendRobotProgram(); + EXPECT_TRUE(g_my_robot->waitForProgramRunning(1000)); + // Robot program should time out after the robot receive timeout, whether it takes exactly 200 ms is not so important g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, RobotReceiveTimeout::millisec(200)); @@ -197,6 +209,11 @@ TEST_F(UrDriverTest, robot_receive_timeout_off) RobotReceiveTimeout::off()); EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); + g_my_robot->getUrDriver()->writeConstrainedFreedriveControlMessage(control::FreedriveControlMessage::FREEDRIVE_NOOP, + { 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }, + RobotReceiveTimeout::off()); + EXPECT_FALSE(g_my_robot->waitForProgramNotRunning(1000)); + // Program should keep running when setting receive timeout off g_my_robot->getUrDriver()->writeTrajectoryControlMessage(control::TrajectoryControlMessage::TRAJECTORY_NOOP, -1, RobotReceiveTimeout::off());