From e68b31b11c6e1752691967d7a47f0dd6922b923b Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 23 Mar 2026 14:46:07 +0100 Subject: [PATCH 1/2] Refactor: Extract directTorqueControlCall helper function Extract the version-dependent direct_torque call logic from torqueThread() into a standalone directTorqueControlCall(tau) function. This allows reuse of the same logic in other control threads without code duplication. --- resources/external_control.urscript | 69 ++++++++++++++++------------- 1 file changed, 37 insertions(+), 32 deletions(-) diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 164db735..3398e9f9 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -249,6 +249,41 @@ thread speedThread(): stopj(STOPJ_ACCELERATION) end +# Helper function to call the direct torque control function depending on the software version +def directTorqueControlCall(tau): + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES: + {% if ROBOT_SOFTWARE_VERSION < v5.25.1 %} + popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope 5.25.1. This robot runs an older version.", error=True, blocking=True) + {% else %} + direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale) + {% endif %} + elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY: + direct_torque(tau, friction_comp=deprecated_friction_compensation_enabled) + else: + direct_torque(tau) + end + {% else %} + popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} + {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} + if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES: + {% if ROBOT_SOFTWARE_VERSION < v10.12.1 %} + popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope X 10.12.1. This robot runs an older version.", error=True, blocking=True) + {% else %} + direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale) + {% endif %} + elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY: + direct_torque(tau, friction_comp=deprecated_friction_compensation_enabled) + else: + direct_torque(tau) + end + {% else %} + popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) + {% endif %} +end + # Helpers for torque control def set_torque(target_torque): cmd_torque = target_torque @@ -258,38 +293,8 @@ end thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: - torque = cmd_torque - {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} - {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} - if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES: - {% if ROBOT_SOFTWARE_VERSION < v5.25.1 %} - popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope 5.25.1. This robot runs an older version.", error=True, blocking=True) - {% else %} - direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale) - {% endif %} - elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY: - direct_torque(torque, friction_comp=deprecated_friction_compensation_enabled) - else: - direct_torque(torque) # Uses robot defaults - end - {% else %} - popup("Torque control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) - {% endif %} - {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} - if friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_SCALES: - {% if ROBOT_SOFTWARE_VERSION < v10.12.1 %} - popup("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope X 10.12.1. This robot runs an older version.", error=True, blocking=True) - {% else %} - direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale) - {% endif %} - elif friction_compensation_mode == FRICTION_COMP_MODE_FRICTION_LEGACY: - direct_torque(torque, friction_comp=deprecated_friction_compensation_enabled) - else: - direct_torque(torque) # Uses robot defaults - end - {% else %} - popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) - {% endif %} + local tau = cmd_torque + directTorqueControlCall(tau) end textmsg("ExternalControl: torque thread ended") stopj(STOPJ_ACCELERATION) From 15172c7310d7ae3aab9e941b145ec3934c6fbe24 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Mon, 23 Mar 2026 14:49:11 +0100 Subject: [PATCH 2/2] Added PD torque controller in joint space (#336) --- doc/architecture/script_command_interface.rst | 25 ++ examples/CMakeLists.txt | 4 + examples/pd_controller_example.cpp | 225 ++++++++++++++++++ examples/script_command_interface.cpp | 7 + include/ur_client_library/comm/control_mode.h | 7 +- .../control/script_command_interface.h | 28 +++ .../torque_command_controller_parameters.h | 97 ++++++++ include/ur_client_library/ur/ur_driver.h | 27 +++ resources/external_control.urscript | 52 ++++ src/control/script_command_interface.cpp | 64 ++++- src/ur/ur_driver.cpp | 71 ++++++ tests/test_reverse_interface.cpp | 6 + tests/test_script_command_interface.cpp | 71 +++++- tests/test_script_reader.cpp | 12 + 14 files changed, 691 insertions(+), 5 deletions(-) create mode 100644 examples/pd_controller_example.cpp create mode 100644 include/ur_client_library/control/torque_command_controller_parameters.h diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index d86b3817..4fe3d280 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -25,6 +25,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun - ``setGravity()``: Set the gravity vector for the robot. - ``setTcpOffset()``: Set the TCP offset of the robot. - ``setFrictionScales()``: Set viscous and Coulomb friction scale factors for direct torque control. +- ``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 ---------------------- @@ -58,6 +60,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 9: setGravity - 10: setTcpOffset - 11: setFrictionScales + - 12: setPDControllerGains + - 13: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -152,6 +156,25 @@ 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 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) + ===== ===== + .. table:: With setGravity command :widths: auto @@ -170,6 +193,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1-6 TCP offset as [x, y, z, rx, ry, rz] given in the flange coordinate system in SI units (meters and radians, floating point) ===== ===== + + .. table:: With setFrictionScales command :widths: auto diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b7da3567..98ec7358 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -62,3 +62,7 @@ target_link_libraries(instruction_executor ur_client_library::urcl) add_executable(external_fts_through_rtde external_fts_through_rtde.cpp) target_link_libraries(external_fts_through_rtde ur_client_library::urcl) + +add_executable(pd_controller_example +pd_controller_example.cpp) +target_link_libraries(pd_controller_example ur_client_library::urcl) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp new file mode 100644 index 00000000..615e78bc --- /dev/null +++ b/examples/pd_controller_example.cpp @@ -0,0 +1,225 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2025 Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +// -- END LICENSE BLOCK ------------------------------------------------ + +#include "ur_client_library/example_robot_wrapper.h" +#include "ur_client_library/ur/dashboard_client.h" +#include "ur_client_library/ur/ur_driver.h" +#include "ur_client_library/types.h" +#include "ur_client_library/ur/instruction_executor.h" + +#include +#include +#include +#include + +using namespace urcl; + +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; +const std::string SCRIPT_FILE = "resources/external_control.urscript"; +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; + +std::unique_ptr g_my_robot; + +void signal_callback_handler(int signum) +{ + std::cout << "Caught signal " << signum << std::endl; + if (g_my_robot != nullptr) + { + // Stop control of the robot + g_my_robot->getUrDriver()->stopControl(); + } + // Terminate program + exit(signum); +} + +struct DataStorage +{ + std::vector target; + std::vector actual; + std::vector time; + + void write_to_file(const std::string& filename) + { + std::fstream output(filename, std::ios::out); + output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," + "actual3,actual4,actual5\n"; + for (size_t i = 0; i < time.size(); ++i) + { + output << time[i]; + for (auto& t : target[i]) + { + output << "," << t; + } + for (auto& a : actual[i]) + { + output << "," << a; + } + output << "\n"; + } + output.close(); + } +}; + +bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_name, + const comm::ControlMode control_mode, const urcl::vector6d_t& amplitude) +{ + const int32_t running_time = 13; + double time = 0.0; + + // Reserve space for expected amount of data + data_storage.actual.reserve(running_time * 500); + data_storage.target.reserve(running_time * 500); + data_storage.time.reserve(running_time * 500); + + urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + + bool first_pass = true; + while (time < running_time) + { + const auto t_start = std::chrono::high_resolution_clock::now(); + // 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. + // In a real-world application this thread should be scheduled with real-time priority in order + // to ensure that this is called in time. + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + URCL_LOG_WARN("Could not get fresh data package from robot"); + return false; + } + // Read current joint positions from robot data + if (!data_pkg.getData(actual_data_name, actual)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = + "Did not find '" + actual_data_name + "' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + if (first_pass) + { + target = actual; + start = actual; + for (size_t i = 0; i < start.size(); ++i) + { + start[i] = start[i] - amplitude[i]; + } + first_pass = false; + } + + for (size_t i = 0; i < target.size(); ++i) + { + target[i] = start[i] + amplitude[i] * cos(time); + } + + // 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_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100)); + if (!ret) + { + URCL_LOG_ERROR("Could not send joint command. Make sure that the robot is in remote control mode and connected " + "with a network cable."); + return false; + } + + // Increment time and log data + const auto t_end = std::chrono::high_resolution_clock::now(); + const double elapsed_time_ms = std::chrono::duration(t_end - t_start).count() / 1000; + time = time + elapsed_time_ms; + data_storage.actual.push_back(actual); + data_storage.target.push_back(target); + data_storage.time.push_back(time); + } + + return true; +} + +int main(int argc, char* argv[]) +{ + // This will make sure that we stop controlling the robot if the user presses ctrl-c + signal(SIGINT, signal_callback_handler); + + urcl::setLogLevel(urcl::LogLevel::INFO); + + // Parse the ip arguments if given + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + const bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode); + + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + { + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + if (robot_version < urcl::VersionInformation::fromString("5.23.0") || + (robot_version.major > 5 && robot_version < urcl::VersionInformation::fromString("10.10.0"))) + { + URCL_LOG_ERROR("This example requires a robot with at least version 5.23.0 / 10.10.0. Your robot has version %s.", + robot_version.toString().c_str()); + return 0; + } + } + + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); + + URCL_LOG_INFO("Move the robot to initial position"); + instruction_executor->moveJ(urcl::vector6d_t{ 0, -1.67, -0.65, -1.59, 1.61, 5.09 }, 0.5, 0.2, 5); + + DataStorage joint_controller_storage; + + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main + // loop. + g_my_robot->getUrDriver()->startRTDECommunication(); + URCL_LOG_INFO("Start controlling the robot using the PD controller"); + const bool completed_joint_control = + pd_control_loop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, + { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); + if (!completed_joint_control) + { + URCL_LOG_ERROR("Didn't complete pd control in joint space"); + g_my_robot->getUrDriver()->stopControl(); + return 1; + } + + return 0; +} diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 8ea7e76b..bd975f2e 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -89,6 +89,13 @@ void sendScriptCommands() { 0.8, 0.8, 0.7, 0.8, 0.8, 0.8 }); }); } + 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 }); + }); + // The following will have no effect on PolyScope < 5.23 / 10.10 + 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/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 1211332b..255b119d 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -54,7 +54,8 @@ enum class ControlMode : int32_t MODE_TOOL_IN_CONTACT = 7, ///< Used only internally in the script, when robot is in tool contact, clear by endToolContact() MODE_TORQUE = 8, ///< Set when torque control is active. - END ///< This is not an actual control mode, but used internally to get the number of control modes + MODE_PD_CONTROLLER_JOINT = 9, ///< Set when PD control in joint space is active + END ///< This is not an actual control mode, but used internally to get the number of control modes }; /*! @@ -65,8 +66,8 @@ class ControlModeTypes public: // Control modes that require realtime communication static const inline std::vector REALTIME_CONTROL_MODES = { - ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, ControlMode::MODE_POSE, - ControlMode::MODE_TORQUE + ControlMode::MODE_SERVOJ, ControlMode::MODE_SPEEDJ, ControlMode::MODE_SPEEDL, + ControlMode::MODE_POSE, ControlMode::MODE_TORQUE, ControlMode::MODE_PD_CONTROLLER_JOINT }; // Control modes that doesn't require realtime communication diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index 92aa4cd9..d368f1b3 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -222,6 +222,32 @@ class ScriptCommandInterface : public ReverseInterface const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 }, const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 }); + /*! + * \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 direct_torque + * 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. + * + * \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 + * direct_torque 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. * @@ -264,6 +290,8 @@ class ScriptCommandInterface : public ReverseInterface SET_GRAVITY = 9, ///< Set gravity vector SET_TCP_OFFSET = 10, ///< Set TCP offset SET_FRICTION_SCALES = 11, ///< Set viscous and Coulomb friction scales for direct_torque + SET_PD_CONTROLLER_GAINS = 12, ///< Set PD controller gains + SET_MAX_JOINT_TORQUES = 13, ///< Set max joint torques }; /*! diff --git a/include/ur_client_library/control/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h new file mode 100644 index 00000000..5a4a7be9 --- /dev/null +++ b/include/ur_client_library/control/torque_command_controller_parameters.h @@ -0,0 +1,97 @@ +#include "ur_client_library/ur/datatypes.h" +#include "ur_client_library/log.h" + +#include + +namespace urcl +{ +namespace control +{ + +struct PDControllerGains +{ + vector6d_t kp; + vector6d_t kd; +}; + +// UR3 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 560.0, 560.0, 350.0, 163.0, 163.0, 163.0 }, + /*.kd*/ { 47.32, 47.32, 37.42, 25.5, 25.5, 25.5 } +}; + +// UR5 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 900.0, 900.0, 900.0, 500.0, 500.0, 500.0 }, + /*.kd*/ { 60.0, 60.0, 60.0, 44.72, 44.72, 44.72 } +}; + +// UR10 PD controller gains, needs to be tuned for the specific purpose. +constexpr PDControllerGains UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE{ + /*.kp*/ { 1300.0, 1300.0, 900.0, 560.0, 560.0, 560.0 }, + /*.kd*/ { 72.11, 72.11, 60.0, 47.32, 47.32, 47.32 } +}; + +constexpr vector6d_t MAX_UR3_JOINT_TORQUE = { 54.0, 54.0, 28.0, 9.0, 9.0, 9.0 }; + +constexpr vector6d_t MAX_UR5_JOINT_TORQUE = { 150.0, 150.0, 150.0, 28.0, 28.0, 28.0 }; + +constexpr vector6d_t MAX_UR10_JOINT_TORQUE = { 330.0, 330.0, 150.0, 54.0, 54.0, 54.0 }; + +/*! + * \brief Get pd gains for specific robot type + * + * \param robot_type Robot type to get gains for + * + * \returns PD gains for the specific robot type + */ +inline PDControllerGains getPdGainsFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return UR3_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR5: + return UR5_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + case RobotType::UR10: + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + default: + std::stringstream ss; + ss << "Default gains has not been tuned for robot type " << robotTypeString(robot_type) + << ". Defaulting to UR10 gains."; + URCL_LOG_WARN(ss.str().c_str()); + return UR10_PD_CONTROLLER_PARAMETERS_JOINT_SPACE; + } +} + +/*! + * \brief Get max torques specific robot type + * + * \param robot_type Robot type to get max torque for + * + * \returns max torque for the specific robot type + */ +inline vector6d_t getMaxTorquesFromRobotType(RobotType robot_type) +{ + switch (robot_type) + { + case RobotType::UR3: + return MAX_UR3_JOINT_TORQUE; + case RobotType::UR5: + return MAX_UR5_JOINT_TORQUE; + case RobotType::UR10: + return MAX_UR10_JOINT_TORQUE; + case RobotType::UR16: + // Same joints as ur10 + return MAX_UR10_JOINT_TORQUE; + default: + std::stringstream ss; + ss << "Max joint torques not collected for robot type " << robotTypeString(robot_type) + << ". Defaulting to UR10 max joint torques."; + URCL_LOG_WARN(ss.str().c_str()); + return MAX_UR10_JOINT_TORQUE; + } +} + +} // namespace control +} // namespace urcl \ No newline at end of file diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 9098a5e5..f6a80f87 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -781,6 +781,7 @@ class UrDriver * * \returns True, if the write was performed successfully, false otherwise. */ + [[deprecated("setFrictionCompensation is deprecated. Use setFrictionScales instead.")]] bool setFrictionCompensation(const bool friction_compensation_enabled); /*! @@ -818,6 +819,32 @@ class UrDriver const vector3d_t& sensor_measuring_offset = { 0.0, 0.0, 0.0 }, const vector3d_t& sensor_cog = { 0.0, 0.0, 0.0 }); + /*! + * \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 direct_torque + * 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. + * + * \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 + * direct_torque 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 3398e9f9..1c42b9ae 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -28,6 +28,7 @@ MODE_POSE = 5 MODE_FREEDRIVE = 6 MODE_TOOL_IN_CONTACT = 7 MODE_TORQUE = 8 +MODE_PD_CONTROLLER_JOINT = 9 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -59,6 +60,8 @@ FT_RTDE_INPUT_ENABLE = 8 SET_GRAVITY = 9 SET_TCP_OFFSET = 10 SET_FRICTION_SCALES = 11 +SET_PD_CONTROLLER_GAINS = 12 +SET_MAX_JOINT_TORQUES = 13 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -101,6 +104,8 @@ global friction_compensation_mode = FRICTION_COMP_MODE_NOT_SET global deprecated_friction_compensation_enabled = True global viscous_scale = [0.9, 0.9, 0.8, 0.9, 0.9, 0.9] global coulomb_scale = [0.8, 0.8, 0.7, 0.8, 0.8, 0.8] +global pd_controller_gains = {{PD_CONTROLLER_GAINS_REPLACE}} +global max_joint_torques = {{MAX_JOINT_TORQUE_REPLACE}} # Global thread variables thread_move = 0 @@ -174,6 +179,31 @@ def terminateProgram(): halt end +### +# @brief Function to clamp each element of a list in between +- clamp value. +# +# @param values array is the list of values to clamp +# @param clampvalues array is the list of clamp values +# +# @returns array of values within +- clamp value +### +def clamp_array(values, clampValues): + if length(values) != length(clampValues): + popup("List of values has different length than list of clamp values.", error = True, blocking = True) + end + ret = values + j = 0 + while j < length(values): + if values[j] > clampValues[j]: + ret[j] = clampValues[j] + elif values[j] < -clampValues[j]: + ret[j] = -clampValues[j] + end + j = j + 1 + end + return ret +end + def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING if targetWithinLimits(cmd_servo_q, q, steptime): @@ -795,6 +825,17 @@ thread servoThreadP(): stopj(STOPJ_ACCELERATION) end +thread PDControlThread(): + while control_mode == MODE_PD_CONTROLLER_JOINT: + local q_err = cmd_servo_q - get_actual_joint_positions() + local tau = pd_controller_gains.kp * q_err - pd_controller_gains.kd * get_actual_joint_speeds() + tau = clamp_array(tau, max_joint_torques) + directTorqueControlCall(tau) + end + textmsg("PD Control thread ended") + stopj(STOPJ_ACCELERATION) +end + def tool_contact_detection(): # Detect tool contact in the directions that the TCP is moving step_back = tool_contact(direction = get_actual_tcp_speed()) @@ -904,6 +945,11 @@ thread script_commands(): elif command == SET_TCP_OFFSET: tcp_offset = p[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] set_tcp(tcp_offset) + 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 @@ -1036,6 +1082,9 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_POSE: cmd_servo_q = get_joint_positions() thread_move = run servoThreadP() + elif control_mode == MODE_PD_CONTROLLER_JOINT: + cmd_servo_q = get_joint_positions() + thread_move = run PDControlThread() end end @@ -1081,6 +1130,9 @@ while control_mode > MODE_STOPPED: textmsg("Leaving freedrive mode") end_freedrive_mode() end + elif control_mode == MODE_PD_CONTROLLER_JOINT: + q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(q) end # Tool contact is running, but hasn't been detected if tool_contact_running == True and control_mode != MODE_TOOL_IN_CONTACT: diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 648e6ee6..8a1eaa78 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -386,6 +386,68 @@ bool ScriptCommandInterface::setTcpOffset(const vector6d_t& offset) return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, const urcl::vector6d_t* kd) +{ + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); + 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) +{ + robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("10.10.0"), __func__); + 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_; @@ -454,4 +516,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 f77a9f5c..3ad969bf 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -37,6 +37,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/helpers.h" #include "ur_client_library/primary/primary_parser.h" +#include "ur_client_library/control/torque_command_controller_parameters.h" #include "ur_client_library/helpers.h" #include #include @@ -55,6 +56,8 @@ static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE"); static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE"); static const std::string TRAJECTORY_PORT_REPLACE("TRAJECTORY_SERVER_PORT_REPLACE"); static const std::string SCRIPT_COMMAND_PORT_REPLACE("SCRIPT_COMMAND_SERVER_PORT_REPLACE"); +static const std::string PD_CONTROLLER_GAINS_REPLACE("PD_CONTROLLER_GAINS_REPLACE"); +static const std::string MAX_JOINT_TORQUE_REPLACE("MAX_JOINT_TORQUE_REPLACE"); UrDriver::~UrDriver() { @@ -141,6 +144,26 @@ void UrDriver::init(const UrDriverConfiguration& config) } data[BEGIN_REPLACE] = begin_replace.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); + const RobotType robot_type = primary_client_->getRobotType(); + + const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); + std::stringstream pd_gains_ss; + if (robot_version_ < urcl::VersionInformation::fromString("5.10.0")) + { + // Structs are only available in URScript 5.10 and later. It isn't used pre 5.23, so we can safely set it to 0. + pd_gains_ss << 0; + } + else + { + pd_gains_ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + } + data[PD_CONTROLLER_GAINS_REPLACE] = pd_gains_ss.str(); + + std::stringstream max_torques_ss; + max_torques_ss << control::getMaxTorquesFromRobotType(robot_type); + data[MAX_JOINT_TORQUE_REPLACE] = max_torques_ss.str(); + data["ROBOT_SOFTWARE_VERSION"] = getVersion(); script_reader_.reset(new control::ScriptReader()); @@ -626,6 +649,54 @@ bool UrDriver::setTcpOffset(const vector6d_t& tcp_offset) } } +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_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 80595db5..b8cbe2a7 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -451,6 +451,12 @@ TEST_F(ReverseInterfaceTest, write_control_mode) received_control_mode = client_->getControlMode(); EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); + + expected_control_mode = comm::ControlMode::MODE_PD_CONTROLLER_JOINT; + reverse_interface_->write(&pos, expected_control_mode); + received_control_mode = client_->getControlMode(); + + EXPECT_EQ(toUnderlying(expected_control_mode), received_control_mode); } TEST_F(ReverseInterfaceTest, write_freedrive_control_message) diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index 68d143dd..3b617e38 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -433,7 +433,6 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable) { // Wait for the client to connect to the server waitForClientConnection(); - double sensor_mass = 1.42; vector3d_t sensor_measuring_offset = { 0.1, 0.2, 0.3 }; vector3d_t sensor_cog = { 0.01, 0.02, 0.03 }; @@ -491,6 +490,7 @@ TEST_F(ScriptCommandInterfaceTest, test_set_gravity) vector3d_t gravity = { 0.1, 0.2, -9.81 }; script_command_interface_->setGravity(&gravity); + int32_t command; std::vector message; client_->readMessage(command, message); @@ -636,6 +636,75 @@ TEST_F(ScriptCommandInterfaceTest, test_set_friction_scales_returns_false_on_old old_client->close(); } +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); + + // 12 is set PD controller gains + int32_t expected_command = 12; + 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); + + // 13 is set max joint torques + int32_t expected_command = 13; + 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); diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 115a84c0..3e54afb3 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -465,6 +465,8 @@ TEST_F(ScriptReaderTest, TestParsingExternalControl) data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("3.12.1"); std::string processed_script = reader.readScriptFile(existing_script_file, data); @@ -505,6 +507,8 @@ TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion) data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.22.0"); std::string processed_script = reader.readScriptFile(existing_script_file, data); @@ -532,6 +536,8 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523 data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.23.0"); std::string processed_script = reader.readScriptFile(existing_script_file, data); @@ -562,6 +568,8 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope101 data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.11.0"); std::string processed_script = reader.readScriptFile(existing_script_file, data); @@ -593,6 +601,8 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler) data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.25.1"); std::string processed_script = reader.readScriptFile(existing_script_file, data); @@ -630,6 +640,8 @@ TEST_F(ScriptReaderTest, TestProduceAllScriptFiles) data["SERVER_PORT_REPLACE"] = "50001"; data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + data["PD_CONTROLLER_GAINS_REPLACE"] = "struct(kp=100, kd=20)"; + data["MAX_JOINT_TORQUE_REPLACE"] = "[150, 150, 150, 28, 28, 28]"; // List of software versions to test std::vector software_versions = {