From 286da72469dd17f4753189025c2a4647046097e4 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/3] 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 ++++++++++++++++------------- tests/test_script_reader.cpp | 6 +-- 2 files changed, 40 insertions(+), 35 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) diff --git a/tests/test_script_reader.cpp b/tests/test_script_reader.cpp index 115a84c0..ff6f4192 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -596,21 +596,21 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler) data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.25.1"); std::string processed_script = reader.readScriptFile(existing_script_file, data); - EXPECT_NE(processed_script.find("direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), + EXPECT_NE(processed_script.find("direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), std::string::npos); EXPECT_EQ(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from"), std::string::npos); data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.26.0"); processed_script = reader.readScriptFile(existing_script_file, data); - EXPECT_NE(processed_script.find("direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), + EXPECT_NE(processed_script.find("direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), std::string::npos); EXPECT_EQ(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from"), std::string::npos); data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.12.1"); processed_script = reader.readScriptFile(existing_script_file, data); - EXPECT_NE(processed_script.find("direct_torque(torque, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), + EXPECT_NE(processed_script.find("direct_torque(tau, viscous_scale=viscous_scale, coulomb_scale=coulomb_scale)"), std::string::npos); EXPECT_EQ(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from"), std::string::npos); From e5de248a1315f687b5dcc6c35cb295b62f5ac668 Mon Sep 17 00:00:00 2001 From: Filippos Sotiropoulos Date: Mon, 2 Mar 2026 09:24:30 +0000 Subject: [PATCH 2/3] Add resolved acceleration control mode --- include/ur_client_library/comm/control_mode.h | 7 +- resources/external_control.urscript | 65 +++++++++++++++++-- tests/test_reverse_interface.cpp | 6 ++ tests/test_script_reader.cpp | 58 +++++++++++++++++ 4 files changed, 127 insertions(+), 9 deletions(-) diff --git a/include/ur_client_library/comm/control_mode.h b/include/ur_client_library/comm/control_mode.h index 1211332b..5b9f75f9 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_RESOLVED_ACCELERATION = 9, ///< Set when resolved acceleration torque control 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_RESOLVED_ACCELERATION }; // Control modes that doesn't require realtime communication diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 3398e9f9..5cdf35e1 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_RESOLVED_ACCELERATION = 9 # Data dimensions of the message received on the reverse interface REVERSE_INTERFACE_DATA_DIMENSION = 8 @@ -86,6 +87,7 @@ global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_qd = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_torque = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +global cmd_acceleration = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] global cmd_servo_q = get_joint_positions() global cmd_servo_q_last = cmd_servo_q global cmd_twist = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] @@ -290,16 +292,62 @@ def set_torque(target_torque): control_mode = MODE_TORQUE end +# Helpers for resolved acceleration control +def set_resolved_acceleration(target_acceleration): + cmd_acceleration = target_acceleration + control_mode = MODE_RESOLVED_ACCELERATION +end + thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: local tau = cmd_torque directTorqueControlCall(tau) end - textmsg("ExternalControl: torque thread ended") + textmsg("ExternalControl: Torque thread ended") stopj(STOPJ_ACCELERATION) end +# Get the measured wrench at the tool flange +def get_flange_wrench(): + local ft = get_tcp_force() #Tcp force returns the force and torques at the tool flange with base orientation + local t_end_base = pose_trans(get_target_tcp_pose(), pose_inv(get_tcp_offset())) + local current_rot_in_tool = pose_inv(p[0, 0, 0, t_end_base[3], t_end_base[4], t_end_base[5]]) + local f = pose_trans(current_rot_in_tool, p[ft[0], ft[1], ft[2], 0, 0, 0]) + local t = pose_trans(current_rot_in_tool, p[ft[3], ft[4], ft[5], 0, 0, 0]) + return [f[0], f[1], f[2], t[0], t[1], t[2]] +end + +thread resolvedAccelerationThread(): + {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} + {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} + textmsg("ExternalControl: Starting resolved acceleration thread") + while control_mode == MODE_RESOLVED_ACCELERATION: + # We do the transformation in the tool flange frame. + local external_torque = transpose(get_jacobian(tcp=p[0,0,0,0,0,0])) * get_flange_wrench() + local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() + external_torque + directTorqueControlCall(tau) + end + textmsg("ExternalControl: Resolved acceleration thread ended") + stopj(STOPJ_ACCELERATION) + {% else %} + popup("Resolved acceleration control is only supported from software 5.23.0 and upwards.", error=True, blocking=True) + {% endif %} + {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} + textmsg("ExternalControl: Starting resolved acceleration thread") + while control_mode == MODE_RESOLVED_ACCELERATION: + # We do the transformation in the tool flange frame. + local external_torque = transpose(get_jacobian(tcp=p[0,0,0,0,0,0])) * get_flange_wrench() + local tau = get_mass_matrix(include_rotor_inertia=True) * cmd_acceleration + get_coriolis_and_centrifugal_torques() + external_torque + directTorqueControlCall(tau) + end + textmsg("ExternalControl: Resolved acceleration thread ended") + stopj(STOPJ_ACCELERATION) + {% else %} + popup("Resolved acceleration control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) + {% endif %} +end + # Function return value (bool) determines whether the robot is moving after this spline segment or # not. def cubicSplineRun(end_q, end_qd, time, is_last_point=False, is_first_point=False): @@ -581,7 +629,7 @@ thread trajectoryThread(): # reset old acceleration spline_qdd = [0, 0, 0, 0, 0, 0] spline_qd = [0, 0, 0, 0, 0, 0] - + # MoveP point elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_MOVEP: acceleration = raw_point[13] / MULT_jointstate @@ -661,7 +709,7 @@ thread trajectoryThread(): elif raw_point[INDEX_POINT_TYPE] == MOTION_TYPE_OPTIMOVEL: acceleration = raw_point[13] / MULT_jointstate velocity = raw_point[7] / MULT_jointstate - + {% if ROBOT_SOFTWARE_VERSION >= v5.21.0 %} {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} optimovel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, r = blend_radius) @@ -994,7 +1042,7 @@ while control_mode > MODE_STOPPED: if params_mult[0] > 0: # Convert read timeout from milliseconds to seconds - read_timeout = params_mult[1] / 1000.0 + read_timeout = params_mult[1] / 1000.0 if control_mode != params_mult[REVERSE_INTERFACE_DATA_DIMENSION]: # Clear remaining trajectory points @@ -1028,6 +1076,8 @@ while control_mode > MODE_STOPPED: thread_move = run speedThread() elif control_mode == MODE_TORQUE: thread_move = run torqueThread() + elif control_mode == MODE_RESOLVED_ACCELERATION: + thread_move = run resolvedAccelerationThread() elif control_mode == MODE_FORWARD: kill thread_move stopj(STOPJ_ACCELERATION) @@ -1049,10 +1099,13 @@ while control_mode > MODE_STOPPED: elif control_mode == MODE_TORQUE: torque = [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_torque(torque) + elif control_mode == MODE_RESOLVED_ACCELERATION: + acceleration = [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_resolved_acceleration(acceleration) elif control_mode == MODE_FORWARD: if trajectory_cleanup_running: - textmsg("Trajectory buffer cleanup in progress, new trajectory ignored") - popup("Trajectory buffer cleanup in progress, new trajectory ignored", error = True, blocking = True) + textmsg("Trajectory buffer cleanup in progress, new trajectory ignored") + popup("Trajectory buffer cleanup in progress, new trajectory ignored", error = True, blocking = True) elif params_mult[2] == TRAJECTORY_MODE_RECEIVE: kill thread_trajectory request_trajectory_cleanup() diff --git a/tests/test_reverse_interface.cpp b/tests/test_reverse_interface.cpp index 80595db5..5648916d 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_RESOLVED_ACCELERATION; + 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_reader.cpp b/tests/test_script_reader.cpp index ff6f4192..0f9a91f0 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -519,6 +519,64 @@ TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion) std::string::npos); } +TEST_F(ScriptReaderTest, TestResolvedAccelerationPopupOnOldVersion) +{ + std::string existing_script_file = "../resources/external_control.urscript"; + ScriptReader reader; + ScriptReader::DataDict data; + data["BEGIN_REPLACE"] = ""; + data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); + data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; + data["SERVER_IP_REPLACE"] = "1.2.3.4"; + data["SERVER_PORT_REPLACE"] = "50001"; + data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; + data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.22.0"); + std::string processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_NE(processed_script.find("popup(\"Resolved acceleration control is only supported from software 5.23.0 and " + "upwards.\", error=True, blocking=True)"), + std::string::npos); + EXPECT_EQ(processed_script.find("get_mass_matrix"), std::string::npos); + + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.10.0"); + processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_NE(processed_script.find("popup(\"Resolved acceleration control is only supported from software 10.11.0 and " + "upwards.\", error=True, blocking=True)"), + std::string::npos); + EXPECT_EQ(processed_script.find("get_mass_matrix"), std::string::npos); +} + +TEST_F(ScriptReaderTest, TestResolvedAccelerationOnSupportedVersion) +{ + std::string existing_script_file = "../resources/external_control.urscript"; + ScriptReader reader; + ScriptReader::DataDict data; + data["BEGIN_REPLACE"] = ""; + data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE); + data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME); + data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000"; + data["SERVER_IP_REPLACE"] = "1.2.3.4"; + data["SERVER_PORT_REPLACE"] = "50001"; + data["TRAJECTORY_SERVER_PORT_REPLACE"] = "50003"; + data["SCRIPT_COMMAND_SERVER_PORT_REPLACE"] = "50004"; + + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("5.23.0"); + std::string processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_NE(processed_script.find("get_mass_matrix(include_rotor_inertia=True)"), std::string::npos); + EXPECT_NE(processed_script.find("get_coriolis_and_centrifugal_torques()"), std::string::npos); + EXPECT_NE(processed_script.find("directTorqueControlCall(tau)"), std::string::npos); + EXPECT_EQ(processed_script.find("Resolved acceleration control is only supported from software"), std::string::npos); + + data["ROBOT_SOFTWARE_VERSION"] = urcl::VersionInformation::fromString("10.11.0"); + processed_script = reader.readScriptFile(existing_script_file, data); + EXPECT_NE(processed_script.find("get_mass_matrix(include_rotor_inertia=True)"), std::string::npos); + EXPECT_NE(processed_script.find("get_coriolis_and_centrifugal_torques()"), std::string::npos); + EXPECT_NE(processed_script.find("directTorqueControlCall(tau)"), std::string::npos); + EXPECT_EQ(processed_script.find("Resolved acceleration control is only supported from software"), std::string::npos); +} + TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523) { std::string existing_script_file = "../resources/external_control.urscript"; From 9f26a84248c68f70f26d2fd5430264a42d10dc06 Mon Sep 17 00:00:00 2001 From: urrsk <41109954+urrsk@users.noreply.github.com> Date: Wed, 25 Mar 2026 16:22:24 +0100 Subject: [PATCH 3/3] Add resolved acceleration example --- examples/CMakeLists.txt | 4 + examples/resolved_acceleration_example.cpp | 194 +++++++++++++++++++++ 2 files changed, 198 insertions(+) create mode 100644 examples/resolved_acceleration_example.cpp diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index b7da3567..da2eab23 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(resolved_acceleration_example +resolved_acceleration_example.cpp) +target_link_libraries(resolved_acceleration_example ur_client_library::urcl) diff --git a/examples/resolved_acceleration_example.cpp b/examples/resolved_acceleration_example.cpp new file mode 100644 index 00000000..3bdd5c0b --- /dev/null +++ b/examples/resolved_acceleration_example.cpp @@ -0,0 +1,194 @@ +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2026 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 ------------------------------------------------ + +// Required before for M_PI on Windows (MSVC) +#define _USE_MATH_DEFINES +#include + +#include +#include "ur_client_library/ur/instruction_executor.h" + +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"; + +const size_t JOINT_INDEX = 5; + +// Sinusoidal reference trajectory parameters +constexpr double frequency = 0.2; // [Hz] +constexpr double position_amplitude = 0.5; // [rad] +constexpr double omega = 2 * M_PI * frequency; + +// PD controller gains for the controlled joint. +// The commanded acceleration is: qdd = Kp * (q_ref - q) + Kd * (qd_ref - qd) +// The urscript then computes the torque via inverse dynamics: +// tau = M(q) * qdd + C(q, qd) - J^T * F_ext +constexpr double KP = 50.0; // [1/s²] proportional gain +constexpr double KD = 10.0; // [1/s] derivative gain + +std::unique_ptr g_my_robot; +urcl::vector6d_t g_joint_positions; +urcl::vector6d_t g_joint_velocities; + +int main(int argc, char* argv[]) +{ + urcl::setLogLevel(urcl::LogLevel::INFO); + + std::string robot_ip = DEFAULT_ROBOT_IP; + if (argc > 1) + { + robot_ip = std::string(argv[1]); + } + + auto second_to_run = std::chrono::seconds(0); + if (argc > 2) + { + second_to_run = std::chrono::seconds(std::stoi(argv[2])); + } + + bool headless_mode = true; + g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode, + "external_control.urp"); + if (!g_my_robot->isHealthy()) + { + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); + return 1; + } + + // Resolved acceleration requires Software version 5.23.0 / 10.11.0 or higher. + { + auto robot_version = g_my_robot->getUrDriver()->getVersion(); + bool version_supported = + ((robot_version.major == 5) && (robot_version >= urcl::VersionInformation::fromString("5.23.0"))) || + ((robot_version.major >= 10) && (robot_version >= urcl::VersionInformation::fromString("10.11.0"))); + if (!version_supported) + { + URCL_LOG_INFO("This resolved acceleration example requires a robot with at least version 5.23.0 / 10.11.0. " + "Your robot has version %s. Skipping.", + robot_version.toString().c_str()); + return 0; + } + } + // --------------- INITIALIZATION END ------------------- + + URCL_LOG_INFO("Start moving the robot"); + urcl::vector6d_t target_accelerations = { 0, 0, 0, 0, 0, 0 }; + + g_my_robot->getUrDriver()->startRTDECommunication(); + + urcl::rtde_interface::DataPackage data_pkg(g_my_robot->getUrDriver()->getRTDEOutputRecipe()); + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + URCL_LOG_ERROR("Could not get fresh data package from robot"); + return 1; + } + if (!data_pkg.getData("actual_q", g_joint_positions)) + { + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + // Scale each individual joint's friction compensation. This is supported from PolyScope 5.25.1 / PolyScope X 10.12.1 + // and upwards. + urcl::vector6d_t viscous_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + urcl::vector6d_t coulomb_scale = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + + // As this example only applies to JOINT_INDEX, we can set the other joints to 0.0 to disable friction compensation to + // make them more steady. + viscous_scale[JOINT_INDEX] = 1.0; + coulomb_scale[JOINT_INDEX] = 1.0; + // g_my_robot->getUrDriver()->setFrictionScales(viscous_scale, coulomb_scale); + + // Record initial positions as the hold targets for all joints. JOINT_INDEX will get a sinusoidal + // offset on top. + urcl::vector6d_t q_ref_hold = g_joint_positions; + + auto start_time = std::chrono::system_clock::now(); + constexpr double timestep = 0.002; // [s] + double time = 0.0; + + // Run 10 full periods of the sinusoidal reference + while (time < 10 * 2 * M_PI / omega) + { + time += timestep; + if (!g_my_robot->getUrDriver()->getDataPackage(data_pkg)) + { + URCL_LOG_WARN("Could not get fresh data package from robot"); + return 1; + } + if (!data_pkg.getData("actual_q", g_joint_positions)) + { + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + if (!data_pkg.getData("actual_qd", g_joint_velocities)) + { + std::string error_msg = "Did not find 'actual_qd' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + + // PD control on all joints: hold them at their initial positions (qd_ref = 0, qdd_ff = 0) + for (size_t i = 0; i < 6; ++i) + { + target_accelerations[i] = KP * (q_ref_hold[i] - g_joint_positions[i]) + KD * (0.0 - g_joint_velocities[i]); + } + + // Apply sinusoidal reference with feedforward on JOINT_INDEX only + double q_ref = q_ref_hold[JOINT_INDEX] + position_amplitude * std::sin(omega * time); + double qd_ref = position_amplitude * omega * std::cos(omega * time); + double qdd_feedforward = -position_amplitude * omega * omega * std::sin(omega * time); + double position_error = q_ref - g_joint_positions[JOINT_INDEX]; + double velocity_error = qd_ref - g_joint_velocities[JOINT_INDEX]; + target_accelerations[JOINT_INDEX] = qdd_feedforward + KP * position_error + KD * velocity_error; + + // Send the commanded accelerations. The urscript resolvedAccelerationThread converts them to + // torques using the robot's dynamics model: tau = M(q)*qdd + C(q,qd) - J^T * F_ext + bool ret = g_my_robot->getUrDriver()->writeJointCommand(target_accelerations, + urcl::comm::ControlMode::MODE_RESOLVED_ACCELERATION, + urcl::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 1; + } + URCL_LOG_DEBUG("data_pkg:\n%s", data_pkg.toString().c_str()); + if (second_to_run.count() > 0 && (std::chrono::system_clock::now() - start_time) > second_to_run) + { + URCL_LOG_WARN("Time limit reached, stopping movement. This is expected on a simulated robot, as it doesn't " + "respond to torque-based commands."); + break; + } + } + g_my_robot->getUrDriver()->stopControl(); + URCL_LOG_INFO("Movement done"); + return 0; +}