From 3f13b3daec4db370a71746023227b331821394ec Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Tue, 7 Jan 2025 11:23:06 -0600 Subject: [PATCH 1/6] Allow updating gravity vector via driver Signed-off-by: AdamPettinger --- .../control/script_command_interface.h | 11 ++++++++ include/ur_client_library/ur/ur_driver.h | 11 ++++++++ resources/external_control.urscript | 5 ++++ src/control/script_command_interface.cpp | 27 ++++++++++++++++++- src/ur/ur_driver.cpp | 20 ++++++++++++++ 5 files changed, 73 insertions(+), 1 deletion(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 5fead319..bc6855fb 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -91,6 +91,16 @@ class ScriptCommandInterface : public ReverseInterface */ bool setPayload(const double mass, const vector3d_t* cog); + /*! + * \brief Set the gravity vector + * + * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away + * from the Earth's center) + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setGravity(const vector3d_t* gravity); + /*! * \brief Set the tool voltage. * @@ -227,6 +237,7 @@ class ScriptCommandInterface : public ReverseInterface END_TOOL_CONTACT = 6, ///< End detecting tool contact SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation FT_RTDE_INPUT_ENABLE = 8, ///< Enable FT RTDE input + SET_GRAVITY = 9, ///< Set gravity vector }; /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 5aaf5314..0d31d5d7 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -596,6 +596,17 @@ class UrDriver */ bool setPayload(const float mass, const vector3d_t& cog); + /*! + * \brief Set the gravity vector. Note: It requires the external control script to be running or + * the robot to be in headless mode. + * + * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away + * from the Earth's center) + * + * \returns True on successful write. + */ + bool setGravity(const vector3d_t& gravity); + /*! * \brief Set the tool voltage. 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 301781a4..c4ad39ef 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -56,6 +56,7 @@ START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 FT_RTDE_INPUT_ENABLE = 8 +SET_GRAVITY = 9 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -756,6 +757,10 @@ thread script_commands(): mass = raw_command[2] / MULT_jointstate cog = [raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate] set_payload(mass, cog) + elif command == SET_GRAVITY: + gravity = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate] + # set_gravity(gravity) + popup(to_str(gravity), title="Received gravity") elif command == SET_TOOL_VOLTAGE: tool_voltage = raw_command[2] / MULT_jointstate set_tool_voltage(tool_voltage) diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 01342c29..e5c80fa7 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -89,6 +89,31 @@ bool ScriptCommandInterface::setPayload(const double mass, const vector3d_t* cog return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setGravity(const vector3d_t* gravity) +{ + const int message_length = 4; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_GRAVITY)); + b_pos += append(b_pos, val); + + for (auto const& grav_component : *gravity) + { + val = htobe32(static_cast(round(grav_component * 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; i++) + { + val = htobe32(0); + b_pos += append(b_pos, val); + } + size_t written; + + return server_.write(client_fd_, buffer, sizeof(buffer), written); +} + bool ScriptCommandInterface::setToolVoltage(const ToolVoltage voltage) { const int message_length = 2; @@ -360,4 +385,4 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo } } // namespace control -} // namespace urcl \ No newline at end of file +} // namespace urcl diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e1dd6e0b..012fc3db 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -298,6 +298,26 @@ bool UrDriver::setPayload(const float mass, const vector3d_t& cog) } } +bool UrDriver::setGravity(const vector3d_t& gravity) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setGravity(&gravity); + } + else + { + // URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series " + // "robots this will only work, if the robot is in remote_control mode."); + // std::stringstream cmd; + // cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + // cmd << "sec setup():" << std::endl + // << " set_gravity([" << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << "])" << std::endl + // << "end"; + // return sendScript(cmd.str()); + return true; + } +} + bool UrDriver::setToolVoltage(const ToolVoltage voltage) { // Test that the tool voltage is either 0, 12 or 24. From 5296e4f36e4f0fe83f6bf44aa8ea89b6d2f0bf31 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Thu, 9 Jan 2025 17:56:13 -0600 Subject: [PATCH 2/6] Actually set gravity in urscript Signed-off-by: AdamPettinger --- resources/external_control.urscript | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index c4ad39ef..c4ef62c2 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -759,8 +759,7 @@ thread script_commands(): set_payload(mass, cog) elif command == SET_GRAVITY: gravity = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate] - # set_gravity(gravity) - popup(to_str(gravity), title="Received gravity") + set_gravity(gravity) elif command == SET_TOOL_VOLTAGE: tool_voltage = raw_command[2] / MULT_jointstate set_tool_voltage(tool_voltage) From b65fe3554279d607560c8256a7e98fb443209ef4 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 12 Dec 2025 15:23:52 -0600 Subject: [PATCH 3/6] Run pre-commit Signed-off-by: AdamPettinger --- include/ur_client_library/control/script_command_interface.h | 2 +- include/ur_client_library/ur/ur_driver.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index bc6855fb..551cfd52 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -94,7 +94,7 @@ class ScriptCommandInterface : public ReverseInterface /*! * \brief Set the gravity vector * - * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away + * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away * from the Earth's center) * * \returns True, if the write was performed successfully, false otherwise. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 0d31d5d7..6c98d7b3 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -600,7 +600,7 @@ class UrDriver * \brief Set the gravity vector. Note: It requires the external control script to be running or * the robot to be in headless mode. * - * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away + * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away * from the Earth's center) * * \returns True on successful write. From c577668aeda1c3ff38085447b13b8913de659efd Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 12 Dec 2025 15:49:01 -0600 Subject: [PATCH 4/6] Reenable plain script fallback Signed-off-by: AdamPettinger --- src/ur/ur_driver.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 012fc3db..3f5271f5 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -306,14 +306,14 @@ bool UrDriver::setGravity(const vector3d_t& gravity) } else { - // URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series " - // "robots this will only work, if the robot is in remote_control mode."); - // std::stringstream cmd; - // cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' - // cmd << "sec setup():" << std::endl - // << " set_gravity([" << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << "])" << std::endl - // << "end"; - // return sendScript(cmd.str()); + URCL_LOG_WARN("Script command interface is not running. Falling back to sending plain script code. On e-Series " + "robots this will only work, if the robot is in remote_control mode."); + std::stringstream cmd; + cmd.imbue(std::locale::classic()); // Make sure, decimal divider is actually '.' + cmd << "sec setup():" << std::endl + << " set_gravity([" << gravity[0] << ", " << gravity[1] << ", " << gravity[2] << "])" << std::endl + << "end"; + return sendScript(cmd.str()); return true; } } From 4dac160630b3c82670cdbf15681a4ad1f8e93199 Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 12 Dec 2025 16:00:43 -0600 Subject: [PATCH 5/6] Correct comments regarding direction of passed gravity Signed-off-by: AdamPettinger --- include/ur_client_library/control/script_command_interface.h | 4 ++-- include/ur_client_library/ur/ur_driver.h | 4 ++-- src/control/script_command_interface.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 551cfd52..d2d6fe19 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -94,8 +94,8 @@ class ScriptCommandInterface : public ReverseInterface /*! * \brief Set the gravity vector * - * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away - * from the Earth's center) + * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards + * the Earth's center) given in the robot's base frame * * \returns True, if the write was performed successfully, false otherwise. */ diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 6c98d7b3..e61a8e07 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -600,8 +600,8 @@ class UrDriver * \brief Set the gravity vector. Note: It requires the external control script to be running or * the robot to be in headless mode. * - * \param gravity Gravity, a vector [x, y, z] specifying the acceleration experience by the robot (pointing away - * from the Earth's center) + * \param gravity Gravity, a vector [x, y, z] specifying the gravity vector (pointing towards + * the Earth's center) given in the robot's base frame * * \returns True on successful write. */ diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index e5c80fa7..ff3521a2 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -385,4 +385,4 @@ bool ScriptCommandInterface::robotVersionSupportsCommandOrWarn(const VersionInfo } } // namespace control -} // namespace urcl +} // namespace urcl \ No newline at end of file From b6fbb5b3fa804e4d895534a92872d49ba39304aa Mon Sep 17 00:00:00 2001 From: AdamPettinger Date: Fri, 19 Dec 2025 10:56:03 -0600 Subject: [PATCH 6/6] Add tests and documentation Signed-off-by: AdamPettinger --- doc/architecture/script_command_interface.rst | 11 +++++++ tests/test_script_command_interface.cpp | 29 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 6a419d54..89969095 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -22,6 +22,7 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun for more information. - ``setFrictionCompensation()``: Set friction compensation for torque command. - ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing. +- ``setGravity()``: Set the gravity vector for the robot. Communication protocol ---------------------- @@ -52,6 +53,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 6: endToolContact - 7: setFrictionCompensation - 8: ftRtdeInputEnable + - 9: setGravity 1-27 data fields specific to the command ===== ===== @@ -146,6 +148,15 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 6-9 sensor_cog in m, displacement from the tool flange (3d floating point) ===== ===== +.. table:: With setGravity command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-3 The gravity vector (towards the Earth's center), represented in robot's base frame (floating point) + ===== ===== + .. 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/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 211d4ce5..d9b42f1e 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -482,6 +482,35 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable) EXPECT_EQ(received_enabled, false); } +TEST_F(ScriptCommandInterfaceTest, test_set_gravity) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + vector3d_t gravity = { 0.1, 0.2, -9.81 }; + script_command_interface_->setGravity(&gravity); + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 9 is set gravity + int32_t expected_command = 9; + EXPECT_EQ(command, expected_command); + + // Test gravity + vector3d_t received_gravity; + for (unsigned int i = 0; i < gravity.size(); ++i) + { + received_gravity[i] = (double)message[i] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(received_gravity[i], gravity[i]); + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + 3, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + int main(int argc, char* argv[]) { ::testing::InitGoogleTest(&argc, argv);