From 165a7c1fca44de845257b8a006ce891388dde559 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Mon, 2 Jun 2025 14:34:27 +0200 Subject: [PATCH 1/3] Added commands to set pd controller gains and maximum joint torques --- doc/architecture/script_command_interface.rst | 23 ++++++ examples/script_command_interface.cpp | 6 ++ .../control/script_command_interface.h | 26 +++++++ include/ur_client_library/ur/ur_driver.h | 24 +++++++ resources/external_control.urscript | 7 ++ src/control/script_command_interface.cpp | 58 +++++++++++++++ src/ur/ur_driver.cpp | 48 +++++++++++++ tests/test_script_command_interface.cpp | 70 +++++++++++++++++++ 8 files changed, 262 insertions(+) diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index cd82f7c1..68a35ac1 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -21,6 +21,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun `_ for more information. - ``setFrictionCompensation()``: Set friction compensation for torque command. +- ``setPDControllerGains()``: Set PD gains for the PD control loop running in the external control script. +- ``setMaxJointTorques()``: Set Max joint torques for the PD control loop running in the external control script. Communication protocol ---------------------- @@ -50,6 +52,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 5: startToolContact - 6: endToolContact - 7: setFrictionCompensation + - 8: setPDControllerGains + - 9: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -132,6 +136,25 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1 friction_compensation_enabled enable/disable friction compensation for torque command. ===== ===== +.. table:: With setPDControllerGains command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 Kp gains for each joint, used in the PD control loop running in the external control script (floating point) + 7-12 Kd gains for each joint, used in the PD control loop running in the external control script (floating point) + ===== ===== + +.. table:: With setMaxJointTorques command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 1-6 max_joint_torques for each joint, used to clamp the torques between +-max_joint_torques before applying them to the robot in the PD control loop running in the external control script (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/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2..a5e9637d 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -74,6 +74,12 @@ void sendScriptCommands() run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); run_cmd("Setting friction_compensation variable to `true`", []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); + run_cmd("Setting PD controller gains", []() { + g_my_robot->getUrDriver()->setPDControllerGains({ 500.0, 500.0, 300.0, 124.0, 124.0, 124.0 }, + { 44.72, 44.72, 34.64, 22.27, 22.27, 22.27 }); + }); + run_cmd("Setting max joint torques", + []() { g_my_robot->getUrDriver()->setMaxJointTorques({ 27.0, 27.0, 14.0, 4.5, 4.5, 4.5 }); }); } URCL_LOG_INFO("Script command thread finished."); } diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 98eacdcc..d011e4e8 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -158,6 +158,30 @@ class ScriptCommandInterface : public ReverseInterface */ bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * torque_command function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t* max_joint_torques); + /*! * \brief Returns whether a client/robot is connected to this server. * @@ -196,6 +220,8 @@ class ScriptCommandInterface : public ReverseInterface START_TOOL_CONTACT = 5, ///< Start detecting tool contact END_TOOL_CONTACT = 6, ///< End detecting tool contact SET_FRICTION_COMPENSATION = 7, ///< Set friction compensation + SET_PD_CONTROLLER_GAINS = 8, ///< Set PD controller gains + SET_MAX_JOINT_TORQUES = 9, ///< Set max joint torques }; bool client_connected_; diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 58ba23f6..83b2f3e1 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -725,6 +725,30 @@ class UrDriver */ bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set gains for the PD controller running in the external control script. The PD controller computes joint + * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command + * function. The gains can be used to change the response of the controller. Be aware that changing the controller + * response can make it unstable. + * + * \param kp A vector6d of proportional gains for each of the joints in the robot. + * \param kd A vector6d of derivative gains for each of the joints in the robot. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd); + + /*! + * \brief Set the maximum joint torques for the PD controller running in the external control script. The PD + * controller will clamp the torques between +-max_joint_torques before aplying them to the robot using the + * torque_command function. + * + * \param max_joint_torques A vector6d of the maximum joint torques for each of the joints. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setMaxJointTorques(const urcl::vector6d_t& max_joint_torques); + /*! * \brief Write a keepalive signal only. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index df0bbcdb..56892fb2 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -53,6 +53,8 @@ END_FORCE_MODE = 4 START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 +SET_PD_CONTROLLER_GAINS = 8 +SET_MAX_JOINT_TORQUES = 9 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -757,6 +759,11 @@ thread script_commands(): else: friction_compensation_enabled = True end + elif command == SET_PD_CONTROLLER_GAINS: + pd_controller_gains.kp = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] + pd_controller_gains.kd = [raw_command[8] / MULT_jointstate, raw_command[9] / MULT_jointstate, raw_command[10] / MULT_jointstate, raw_command[11] / MULT_jointstate, raw_command[12] / MULT_jointstate, raw_command[13] / MULT_jointstate] + elif command == SET_MAX_JOINT_TORQUES: + max_joint_torques = [raw_command[2] / MULT_jointstate, raw_command[3] / MULT_jointstate, raw_command[4] / MULT_jointstate, raw_command[5] / MULT_jointstate, raw_command[6] / MULT_jointstate, raw_command[7] / MULT_jointstate] end end end diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 0a436349..a9dfc805 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -245,6 +245,64 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) +{ + const int message_length = 13; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_PD_CONTROLLER_GAINS)); + b_pos += append(b_pos, val); + + for (auto const& p_gain : *kp) + { + val = htobe32(static_cast(round(p_gain * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + for (auto const& d_gain : *kd) + { + val = htobe32(static_cast(round(d_gain * 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::setMaxJointTorques(const urcl::vector6d_t* max_joint_torques) +{ + const int message_length = 7; + uint8_t buffer[sizeof(int32_t) * MAX_MESSAGE_LENGTH]; + uint8_t* b_pos = buffer; + + int32_t val = htobe32(toUnderlying(ScriptCommand::SET_MAX_JOINT_TORQUES)); + b_pos += append(b_pos, val); + + for (auto const& max_torque : *max_joint_torques) + { + val = htobe32(static_cast(round(max_torque * 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::clientConnected() { return client_connected_; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 438f3fb1..0c096af6 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -573,6 +573,54 @@ bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) } } +bool UrDriver::setPDControllerGains(const urcl::vector6d_t& kp, const urcl::vector6d_t& kd) +{ + if (!std::all_of(kp.begin(), kp.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kp should be larger than zero"); + } + + if (!std::all_of(kd.begin(), kd.end(), [](double v) { return v >= 0.0; })) + { + throw InvalidRange("kd should be larger than zero"); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setPDControllerGains(&kp, &kd); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set PD Controller gains."); + return 0; + } +} + +bool UrDriver::setMaxJointTorques(const urcl::vector6d_t& max_joint_torques) +{ + const urcl::vector6d_t max_torques_for_robot_type = + control::getMaxTorquesFromRobotType(primary_client_->getRobotType()); + if (!std::equal(max_joint_torques.begin(), max_joint_torques.end(), max_torques_for_robot_type.begin(), + [](double v1, double v2) { return v1 <= v2 && v1 >= 0.0; })) + { + std::stringstream ss; + ss << "The max joint torques should be smaller or equal to the maximum joint torques for the robot type and larger " + "than zero. Provided max joint torques " + << max_joint_torques << " and maximum joint torques for the robot type " << max_torques_for_robot_type; + throw InvalidRange(ss.str().c_str()); + } + + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setMaxJointTorques(&max_joint_torques); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set max joint torques."); + 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 10115bb1..26ff6077 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -421,6 +421,76 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_compensation) EXPECT_EQ(message_sum, expected_message_sum); } +TEST_F(ScriptCommandInterfaceTest, test_set_pd_controller_gains) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t kp = { 220.2, 220.2, 300.0, 10.32, 10.32, 10.32 }; + urcl::vector6d_t kd = { 29.68, 29.68, 35.0, 6.4, 6.4, 6.4 }; + script_command_interface_->setPDControllerGains(&kp, &kd); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 8 is set PD controller gains + int32_t expected_command = 8; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& p_gain : kp) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(p_gain, received_gain); + message_idx = message_idx + 1; + } + + for (auto& d_gain : kd) + { + const double received_gain = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(d_gain, received_gain); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, std::end(message), 0); + int32_t expected_message_sum = 0; + EXPECT_EQ(message_sum, expected_message_sum); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_max_joint_torques) +{ + // Wait for the client to connect to the server + waitForClientConnection(); + + urcl::vector6d_t max_joint_torques = { 100.0, 150.0, 21.2, 10.32, 10.32, 10.32 }; + script_command_interface_->setMaxJointTorques(&max_joint_torques); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + // 9 is set max joint torques + int32_t expected_command = 9; + EXPECT_EQ(command, expected_command); + + int32_t message_idx = 0; + + for (auto& max_torque : max_joint_torques) + { + const double received_max_torque = (double)message[message_idx] / script_command_interface_->MULT_JOINTSTATE; + EXPECT_EQ(max_torque, received_max_torque); + message_idx = message_idx + 1; + } + + // The rest of the message should be zero + int32_t message_sum = std::accumulate(std::begin(message) + message_idx, 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); From fc3e289475769c8307ac220e8674ae66bd0bd234 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 10:54:06 +0200 Subject: [PATCH 2/3] Apply suggestions from code review Co-authored-by: Felix Exner --- include/ur_client_library/control/script_command_interface.h | 1 + 1 file changed, 1 insertion(+) diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index d011e4e8..650c6e93 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -163,6 +163,7 @@ class ScriptCommandInterface : public ReverseInterface * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command * function. The gains can be used to change the response of the controller. Be aware that changing the controller * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for each robot model. * * \param kp A vector6d of proportional gains for each of the joints in the robot. * \param kd A vector6d of derivative gains for each of the joints in the robot. From 570987622cce9d72fe15f6b55a43c6c4cc5a3f6a Mon Sep 17 00:00:00 2001 From: Mads Holm Peters Date: Thu, 5 Jun 2025 10:56:14 +0200 Subject: [PATCH 3/3] Update documentation --- include/ur_client_library/control/script_command_interface.h | 3 ++- include/ur_client_library/ur/ur_driver.h | 2 ++ 2 files changed, 4 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 650c6e93..a0bf8f5a 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -163,7 +163,8 @@ class ScriptCommandInterface : public ReverseInterface * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command * function. The gains can be used to change the response of the controller. Be aware that changing the controller * response can make it unstable. - * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for each robot model. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. * * \param kp A vector6d of proportional gains for each of the joints in the robot. * \param kd A vector6d of derivative gains for each of the joints in the robot. diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 83b2f3e1..4c20c1f8 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -730,6 +730,8 @@ class UrDriver * torques based on either tcp poses or joint poses and applies the torques to the robot using the torque_command * function. The gains can be used to change the response of the controller. Be aware that changing the controller * response can make it unstable. + * The PD controller can be used without explicitly defining those gains, as it contains a set of default gains for + * each robot model. * * \param kp A vector6d of proportional gains for each of the joints in the robot. * \param kd A vector6d of derivative gains for each of the joints in the robot.