diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index f52aa48b..cd82f7c1 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -20,6 +20,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun contact example `_ for more information. +- ``setFrictionCompensation()``: Set friction compensation for torque command. Communication protocol ---------------------- @@ -48,6 +49,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 4: endForceMode - 5: startToolContact - 6: endToolContact + - 7: setFrictionCompensation 1-27 data fields specific to the command ===== ===== @@ -121,6 +123,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1 No specific meaning / values ignored ===== ===== +.. table:: With setFrictionCompensation command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1 friction_compensation_enabled enable/disable friction compensation for torque command. + ===== ===== + .. note:: In URScript the ``socket_read_binary_integer()`` function is used to read the data from the script command socket. The first index in that function's return value is the number of integers read, diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 2512c024..98eacdcc 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -147,6 +147,17 @@ class ScriptCommandInterface : public ReverseInterface */ bool endToolContact(); + /*! + * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, + * if false it will not. + * + * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be + * used when calling torque_command + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! * \brief Returns whether a client/robot is connected to this server. * @@ -177,13 +188,14 @@ class ScriptCommandInterface : public ReverseInterface enum class ScriptCommand : int32_t { - ZERO_FTSENSOR = 0, ///< Zero force torque sensor - SET_PAYLOAD = 1, ///< Set payload - SET_TOOL_VOLTAGE = 2, ///< Set tool voltage - START_FORCE_MODE = 3, ///< Start force mode - END_FORCE_MODE = 4, ///< End force mode - START_TOOL_CONTACT = 5, ///< Start detecting tool contact - END_TOOL_CONTACT = 6, ///< End detecting tool contact + ZERO_FTSENSOR = 0, ///< Zero force torque sensor + SET_PAYLOAD = 1, ///< Set payload + SET_TOOL_VOLTAGE = 2, ///< Set tool voltage + START_FORCE_MODE = 3, ///< Start force mode + END_FORCE_MODE = 4, ///< End force mode + START_TOOL_CONTACT = 5, ///< Start detecting tool contact + END_TOOL_CONTACT = 6, ///< End detecting tool contact + SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation }; bool client_connected_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index d064c812..58ba23f6 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -714,6 +714,17 @@ class UrDriver */ bool endToolContact(); + /*! + * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, + * if false it will not. + * + * \param friction_compensation_enabled Will set a friction_compensation_enabled variable in urscript, which will be + * used when calling torque_command + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 266b8c8b..8e67212f 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -49,6 +49,7 @@ START_FORCE_MODE = 3 END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 +SET_FRICTION_COMPENSATION = 7 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -81,6 +82,7 @@ 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 friction_compensation_enabled = True # Global thread variables thread_move = 0 @@ -691,6 +693,12 @@ thread script_commands(): socket_send_int(UNTIL_TOOL_CONTACT_RESULT_CANCELED, "script_command_socket") end tool_contact_running = False + elif command == SET_FRICTION_COMPENSATION: + if raw_command[2] == 0: + friction_compensation_enabled = False + else: + friction_compensation_enabled = True + end end end end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index ca2fb6d8..0a436349 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -222,6 +222,29 @@ bool ScriptCommandInterface::endToolContact() return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compensation_enabled) +{ + const int message_length = 2; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_FRICTION_COMPENSATION)); + b_pos += append(b_pos, val); + + val = htobe32(friction_compensation_enabled); + 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; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + bool ScriptCommandInterface::clientConnected() { return client_connected_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e6108229..6e30579c 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -527,6 +527,19 @@ bool UrDriver::endToolContact() } } +bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setFrictionCompensation(friction_compensation_enabled); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set friction compensation."); + return 0; + } +} + bool UrDriver::writeKeepalive(const RobotReceiveTimeout& robot_receive_timeout) { vector6d_t* fake = nullptr; diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index d834502a..1f1c0274 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -382,6 +382,45 @@ TEST_F(ScriptCommandInterfaceTest, test_tool_contact_callback) EXPECT_EQ(toUnderlying(received_result_), toUnderlying(send_result)); } +TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + script_command_interface_->setFrictionCompensation(true); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 7 is set friction compensation + int32_t expected_command = 7; + EXPECT_EQ(command, expected_command); + + int32_t expected_friction_compensation = 1; + EXPECT_EQ(message[0], expected_friction_compensation); + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); + + script_command_interface_->setFrictionCompensation(false); + + message.clear(); + client_->readMessage(command, message); + + EXPECT_EQ(command, expected_command); + + expected_friction_compensation = 0; + EXPECT_EQ(message[0], expected_friction_compensation); + + // The rest of the message should be zero + message_sum = std::accumulate(std::begin(message) + 1, std::end(message), 0); + expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);