From f470af0d9500c97784eb434aadd413086f8e7ebb Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 09:00:57 +0200 Subject: [PATCH 1/3] =?UTF-8?q?Added=20two=20new=20control=20modes=20PD=20?= =?UTF-8?q?controller=20in=20joint=20space=20and=20PD=20contr=E2=80=A6=20(?= =?UTF-8?q?#336)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit …oller in task space The pd controller in joint space will accept joint targets and compute joint torques based on PD control scheme. The pd controller in task space will accept task space targets and use inverse kinematics to compute joint targets which are then used to compute joint torques based on PD control scheme. --------- Co-authored-by: Felix Exner --- examples/CMakeLists.txt | 4 + examples/pd_controller_example.cpp | 211 ++++++++++++++++++ include/ur_client_library/comm/control_mode.h | 7 +- .../torque_command_controller_parameters.h | 97 ++++++++ resources/external_control.urscript | 45 ++++ src/ur/ur_driver.cpp | 18 ++ tests/test_reverse_interface.cpp | 6 + tests/test_script_reader.cpp | 2 + 8 files changed, 387 insertions(+), 3 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/examples/CMakeLists.txt b/examples/CMakeLists.txt index 2d52ae3dc..8d34bc9b5 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -63,3 +63,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 000000000..4612cb0a5 --- /dev/null +++ b/examples/pd_controller_example.cpp @@ -0,0 +1,211 @@ +// 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 }; + + 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. + std::unique_ptr data_pkg = g_my_robot->getUrDriver()->getDataPackage(); + if (!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. Is the robot in remote control?"); + 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 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/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 51156da6f..df173d4a2 100644 --- a/include/ur_client_library/comm/control_mode.h +++ b/include/ur_client_library/comm/control_mode.h @@ -53,7 +53,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 }; /*! @@ -64,8 +65,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/torque_command_controller_parameters.h b/include/ur_client_library/control/torque_command_controller_parameters.h new file mode 100644 index 000000000..5a4a7be95 --- /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/resources/external_control.urscript b/resources/external_control.urscript index 301781a41..fef208615 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 @@ -90,6 +91,8 @@ global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 global friction_compensation_enabled = True +global pd_controller_gains = {{PD_CONTROLLER_GAINS_REPLACE}} +global max_joint_torques = {{MAX_JOINT_TORQUE_REPLACE}} # Global thread variables thread_move = 0 @@ -159,6 +162,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): @@ -716,6 +744,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) + direct_torque(tau, friction_comp=friction_compensation_enabled) + 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()) @@ -885,6 +924,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 @@ -925,6 +967,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/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index e860722e8..f0b3d8d7c 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -36,6 +36,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 @@ -52,6 +53,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() { @@ -145,6 +148,21 @@ void UrDriver::init(const UrDriverConfiguration& config) script_reader_.reset(new control::ScriptReader()); std::string prog = script_reader_->readScriptFile(config.script_file, data); + const RobotType robot_type = primary_client_->getRobotType(); + + const control::PDControllerGains pd_gains = control::getPdGainsFromRobotType(robot_type); + { + std::stringstream ss; + ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; + data[PD_CONTROLLER_GAINS_REPLACE] = ss.str(); + } + + { + std::stringstream ss; + ss << control::getMaxTorquesFromRobotType(robot_type); + data[MAX_JOINT_TORQUE_REPLACE] = ss.str(); + } + if (in_headless_mode_) { full_robot_program_ = "stop program\n"; diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index a365b6dd8..9898adf53 100644 --- a/tests/test_reverse_interface.cpp +++ b/tests/test_reverse_interface.cpp @@ -436,6 +436,12 @@ TEST_F(ReverseIntefaceTest, 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(ReverseIntefaceTest, write_freedrive_control_message) diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 21d949e6e..9b8688ee4 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -464,6 +464,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); From a457828d6c397eb2a441d857191daa72177227e7 Mon Sep 17 00:00:00 2001 From: Mads Holm Peters <79145214+urmahp@users.noreply.github.com> Date: Thu, 5 Jun 2025 11:33:22 +0200 Subject: [PATCH 2/3] Added commands to set pd controller gains and maximum joint torques (#342) Co-authored-by: Felix Exner --- doc/architecture/script_command_interface.rst | 22 ++++++ examples/script_command_interface.cpp | 6 ++ .../control/script_command_interface.h | 34 ++++++++- include/ur_client_library/ur/ur_driver.h | 32 ++++++++- resources/external_control.urscript | 7 ++ src/control/script_command_interface.cpp | 60 +++++++++++++++- src/ur/ur_driver.cpp | 48 +++++++++++++ tests/test_script_command_interface.cpp | 71 ++++++++++++++++++- 8 files changed, 272 insertions(+), 8 deletions(-) diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 6a419d54b..2059cec6d 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -22,6 +22,8 @@ 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. +- ``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 ---------------------- @@ -52,6 +54,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 6: endToolContact - 7: setFrictionCompensation - 8: ftRtdeInputEnable + - 9: setPDControllerGains + - 10: setMaxJointTorques 1-27 data fields specific to the command ===== ===== @@ -144,6 +148,24 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 2 sensor_mass in kg (floating point) 3-5 sensor_mesurement_offset in m, displacement from the tool flange (3d floating point) 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) ===== ===== .. note:: diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2f..a5e9637d7 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 5fead319f..106b0aa48 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -158,11 +158,11 @@ 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. + * \brief Set friction compensation for the direct_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 + * used when calling direct_torque. * * \returns True, if the write was performed successfully, false otherwise. */ @@ -188,6 +188,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. * @@ -227,6 +253,8 @@ 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_PD_CONTROLLER_GAINS = 9, ///< Set PD controller gains + SET_MAX_JOINT_TORQUES = 10, ///< Set max joint torques }; /*! diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index 15a4fd252..1f4fb3e67 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -716,11 +716,11 @@ 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. + * \brief Set friction compensation for the direct_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 + * used when calling direct_torque. * * \returns True, if the write was performed successfully, false otherwise. */ @@ -746,6 +746,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 fef208615..8046745c4 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -57,6 +57,8 @@ START_TOOL_CONTACT = 5 END_TOOL_CONTACT = 6 SET_FRICTION_COMPENSATION = 7 FT_RTDE_INPUT_ENABLE = 8 +SET_PD_CONTROLLER_GAINS = 9 +SET_MAX_JOINT_TORQUES = 10 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -857,6 +859,11 @@ thread script_commands(): # This has a known error that the resulting torques are computed with opposite sign. enable_external_ft_sensor(enabled, sensor_mass, sensor_offset, sensor_cog) {% endif %} + 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 01342c293..1b4564dca 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -293,6 +293,64 @@ bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double 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_; @@ -360,4 +418,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 f0b3d8d7c..34283fbfd 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -571,6 +571,54 @@ bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass, } } +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 211d4ce53..7c60666b2 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -431,7 +431,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 }; @@ -482,6 +481,76 @@ TEST_F(ScriptCommandInterfaceTest, test_ft_rtde_input_enable) EXPECT_EQ(received_enabled, false); } +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 = 9; + 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 = 10; + 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 11bd2e9611e6b32963c0d5e4aac376ab5c40c86d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Jul 2025 13:56:37 +0200 Subject: [PATCH 3/3] Add torque_command statement only on supported software versions (#348) This way all non-torque-control-related things should work on software versions not supporting torque control --- examples/pd_controller_example.cpp | 11 ++++++++++ examples/script_command_interface.cpp | 1 + src/control/script_command_interface.cpp | 4 ++++ src/ur/ur_driver.cpp | 27 ++++++++++++++---------- 4 files changed, 32 insertions(+), 11 deletions(-) diff --git a/examples/pd_controller_example.cpp b/examples/pd_controller_example.cpp index 4612cb0a5..6323091aa 100644 --- a/examples/pd_controller_example.cpp +++ b/examples/pd_controller_example.cpp @@ -185,6 +185,17 @@ int main(int argc, char* argv[]) 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"); diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index a5e9637d7..000a1c619 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -78,6 +78,7 @@ void sendScriptCommands() 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 }); }); } diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 1b4564dca..048e1f402 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -295,6 +295,8 @@ bool ScriptCommandInterface::ftRtdeInputEnable(const bool enabled, const double 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; @@ -327,6 +329,8 @@ bool ScriptCommandInterface::setPDControllerGains(const urcl::vector6d_t* kp, co 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; diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 34283fbfd..cebad98d3 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -144,24 +144,29 @@ void UrDriver::init(const UrDriverConfiguration& config) data[BEGIN_REPLACE] = begin_replace.str(); data["ROBOT_SOFTWARE_VERSION"] = getVersion(); - - script_reader_.reset(new control::ScriptReader()); - std::string prog = script_reader_->readScriptFile(config.script_file, data); - 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")) { - std::stringstream ss; - ss << "struct(kp=" << pd_gains.kp << ", kd=" << pd_gains.kd << ")"; - data[PD_CONTROLLER_GAINS_REPLACE] = ss.str(); + // 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 { - std::stringstream ss; - ss << control::getMaxTorquesFromRobotType(robot_type); - data[MAX_JOINT_TORQUE_REPLACE] = ss.str(); + 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()); + std::string prog = script_reader_->readScriptFile(config.script_file, data); if (in_headless_mode_) {