diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index a02297cd1..fd51dba12 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -127,6 +127,14 @@ jobs: path: test_artifacts if-no-files-found: error retention-days: 10 + - name: Upload generated scripts + uses: actions/upload-artifact@v6 + if: ${{ always() }} + with: + name: ${{matrix.env.ROBOT_MODEL}}_${{matrix.env.URSIM_VERSION}}_generated_scripts + path: test_artifacts/generated_scripts + if-no-files-found: warn + retention-days: 10 test_start_ursim: runs-on: ubuntu-latest diff --git a/doc/architecture/script_command_interface.rst b/doc/architecture/script_command_interface.rst index 89969095a..d86b3817c 100644 --- a/doc/architecture/script_command_interface.rst +++ b/doc/architecture/script_command_interface.rst @@ -23,6 +23,8 @@ At the time of writing the ``ScriptCommandInterface`` provides the following fun - ``setFrictionCompensation()``: Set friction compensation for torque command. - ``ftRtdeInputEnable()``: Enable/disable FT RTDE input processing. - ``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. Communication protocol ---------------------- @@ -54,6 +56,8 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr - 7: setFrictionCompensation - 8: ftRtdeInputEnable - 9: setGravity + - 10: setTcpOffset + - 11: setFrictionScales 1-27 data fields specific to the command ===== ===== @@ -127,7 +131,7 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1 No specific meaning / values ignored ===== ===== -.. table:: With setFrictionCompensation command +.. table:: With setFrictionCompensation command (Deprecated, use setFrictionScales instead) :widths: auto ===== ===== @@ -157,6 +161,25 @@ The robot reads from the "script_command_socket" expecting a 32 bit integer repr 1-3 The gravity vector (towards the Earth's center), represented in robot's base frame (floating point) ===== ===== +.. table:: With setTcpOffset command + :widths: auto + + ===== ===== + index meaning + ===== ===== + 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 + + ===== ===== + index meaning + ===== ===== + 1-6 Viscous friction scale factors. One number per joint, range [0-1]. 0 means no compensation for that joint. 1 means full compensation. Default is [0.9, 0.9, 0.8, 0.9, 0.9, 0.9]. (floating point) + 7-12 Coulomb friction scale factors. One number per joint, range [0-1]. 0 means no compensation for that joint. 1 means full compensation. Default is [0.8, 0.8, 0.7, 0.8, 0.8, 0.8]. (floating point) + ===== ===== + .. note:: In URScript the ``socket_read_binary_integer()`` function is used to read the data from the script command socket. The first index in that function's return value is the number of integers read, diff --git a/doc/examples/direct_torque_control.rst b/doc/examples/direct_torque_control.rst index 10e52950a..b71d21f22 100644 --- a/doc/examples/direct_torque_control.rst +++ b/doc/examples/direct_torque_control.rst @@ -6,17 +6,17 @@ Torque control example ====================== In the ``examples`` subfolder you will find a minimal example for commanding torques to the robot. -It moves the robot in the 5th axis back and forth, while reading the joint positions. To run it -make sure to +It applies a sinusoidal torque to the 6th joint (index 5) while reading the joint positions. To run +it make sure to * have an instance of a robot controller / URSim running at the configured IP address (or adapt the address to your needs) -* have PolyScope version 5.23.0 / 10.10.0 or later installed on the robot. +* have PolyScope version 5.25.1 / 10.12.1 or later installed on the robot. * run it from the package's main folder (the one where the README.md file is stored), as for simplicity reasons it doesn't use any sophisticated method to locate the required files. -This page will walk you through the `full_driver.cpp -`_ +This page will walk you through the `direct_torque_control.cpp +`_ example. Initialization @@ -34,19 +34,34 @@ recipes. :end-at: // --------------- INITIALIZATION END ------------------- .. note:: - This example requires PolyScope version 5.23.0 / 10.10.0 or later, as it uses the direct_torque_control - mode which is only available in these versions and later. If you try to run it on an older - software version, this example will print an error and exit. + This example requires PolyScope version 5.25.1 / 10.12.1 or later, as it uses + ``setFrictionScales()`` which is only available in these versions and later. If you try to run it + on an older software version, this example will skip gracefully. -Robot control loop ------------------- +Friction compensation setup +---------------------------- + +Before starting the control loop, friction compensation scales are configured using +``setFrictionScales()``. Since this example only controls a single joint (``JOINT_INDEX``), friction +compensation for the other joints is set to zero to keep them more steady. The controlled joint +gets full compensation (scale factor 1.0) for both viscous and Coulomb friction. -This example reads the robot's joint positions, commands a torque for the 5th axis and sends that -back as a joint command for the next cycle. This way, the robot will move its wrist first until a -positive limit and then back to 0. +.. literalinclude:: ../../examples/direct_torque_control.cpp + :language: c++ + :caption: examples/direct_torque_control.cpp + :linenos: + :lineno-match: + :start-at: // Scale each individual joint's friction compensation + :end-at: setFrictionScales(viscous_scale, coulomb_scale); -To read the joint data, the driver's RTDE client is used: +Move to start position +---------------------- +Before entering the torque control loop, the robot is moved to a known start position using +the ``InstructionExecutor``. This ensures a predictable initial state for the sinusoidal motion. +First, RTDE communication is started and the current joint positions are read. Then, the target +position for ``JOINT_INDEX`` is set to ``start_position`` and the robot is moved there using +``moveJ()``. .. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ @@ -54,11 +69,26 @@ To read the joint data, the driver's RTDE client is used: :linenos: :lineno-match: :start-at: // Once RTDE communication is started - :end-before: // Open loop control + :end-at: instruction_executor->moveJ(g_joint_positions, 1.4, 1.04, 0.1); -The first read operation will initialize the target buffer with the current robot position. Next, -the target joint torques are set based on the current joint positions: +Robot control loop +------------------ + +The main control loop applies a sinusoidal torque to the controlled joint for 10 full periods. +In each cycle, the latest RTDE data package is read using ``getDataPackage()``. This call blocks +until new data arrives from the robot (with an internal timeout), so the robot's RTDE cycle +effectively controls the timing of this loop. The current joint positions are then extracted from +the received package: + +.. literalinclude:: ../../examples/direct_torque_control.cpp + :language: c++ + :caption: examples/direct_torque_control.cpp + :linenos: + :lineno-match: + :start-at: // Run 10 periods of the sinusoidal + :end-before: // Open loop control +The target torque for the controlled joint is computed as a sinusoidal function of time: .. literalinclude:: ../../examples/direct_torque_control.cpp :language: c++ @@ -66,7 +96,7 @@ the target joint torques are set based on the current joint positions: :linenos: :lineno-match: :start-at: // Open loop control - :end-at: target_torques[JOINT_INDEX] = cmd_torque; + :end-at: target_torques[JOINT_INDEX] = amplitude * std::sin(omega * time); To send the control command, the robot's :ref:`reverse_interface` is used via the ``writeJointCommand()`` function: diff --git a/examples/direct_torque_control.cpp b/examples/direct_torque_control.cpp index d213cc97f..6dbee1774 100644 --- a/examples/direct_torque_control.cpp +++ b/examples/direct_torque_control.cpp @@ -28,7 +28,12 @@ // 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" #include // In a real-world example it would be better to get those values from command line parameters / a @@ -40,6 +45,12 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; const size_t JOINT_INDEX = 5; // Joint index to control, in this case joint 6 (index 5) +// This example will apply a sinusoidal torque to JOINT_INDEX, while the other joints are kept at 0.0. +constexpr double frequency = 0.2; // [Hz] +constexpr double amplitude = 2.5; // [Nm] +constexpr double omega = 2 * M_PI * frequency; +constexpr double start_position = 0.0; // [rad] + std::unique_ptr g_my_robot; urcl::vector6d_t g_joint_positions; @@ -70,37 +81,67 @@ int main(int argc, char* argv[]) return 1; } - // Torque control requires Software version 5.23 / 10.10 or higher. Error and exit on older - // software versions. + // This example requires Software version 5.25.1 / 10.12.1 or higher. Skip gracefully on older + // software versions so that CI (which runs all examples) does not fail. { 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"))) + bool version_supported = + ((robot_version.major == 5) && (robot_version >= urcl::VersionInformation::fromString("5.25.1"))) || + ((robot_version.major >= 10) && (robot_version >= urcl::VersionInformation::fromString("10.12.1"))); + if (!version_supported) { - 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()); + URCL_LOG_INFO("This direct_torque control example requires a robot with at least version 5.25.1 / 10.12.1. Your " + "robot has version %s. Skipping.", + robot_version.toString().c_str()); return 0; } } // --------------- INITIALIZATION END ------------------- - const double torque_abs = 2.5; - double cmd_torque = torque_abs; // Target torque [Nm] for joint 6 - bool passed_negative_part = false; - bool passed_positive_part = false; URCL_LOG_INFO("Start moving the robot"); urcl::vector6d_t target_torques = { 0, 0, 0, 0, 0, 0 }; + // 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); + // 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(); - auto start_time = std::chrono::system_clock::now(); 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)) + { + // This throwing should never happen unless misconfigured + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; + throw std::runtime_error(error_msg); + } + // Use motion primitives to move JOINT_INDEX to start position + g_joint_positions[JOINT_INDEX] = start_position; + auto instruction_executor = std::make_shared(g_my_robot->getUrDriver()); + instruction_executor->moveJ(g_joint_positions, 1.4, 1.04, 0.1); + + auto start_time = std::chrono::system_clock::now(); + constexpr double timestep = 0.002; // [s] + double time = 0.0; - while (!(passed_positive_part && passed_negative_part)) + // Run 10 periods of the sinusoidal + while (time < 10 * 2 * M_PI / omega) { + time += timestep; // 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 @@ -119,23 +160,7 @@ int main(int argc, char* argv[]) } // Open loop control. The target is incremented with a constant each control loop - if (passed_positive_part == false) - { - if (g_joint_positions[JOINT_INDEX] >= 2) - { - passed_positive_part = true; - cmd_torque = -torque_abs; - } - } - else if (passed_negative_part == false) - { - if (g_joint_positions[JOINT_INDEX] <= 0) - { - cmd_torque = torque_abs; - passed_negative_part = true; - } - } - target_torques[JOINT_INDEX] = cmd_torque; + target_torques[JOINT_INDEX] = amplitude * std::sin(omega * 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. Having it @@ -145,7 +170,8 @@ int main(int argc, char* argv[]) urcl::RobotReceiveTimeout::millisec(100)); if (!ret) { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + 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()); diff --git a/examples/full_driver.cpp b/examples/full_driver.cpp index 1af6ffb61..5e5db4d0b 100644 --- a/examples/full_driver.cpp +++ b/examples/full_driver.cpp @@ -71,6 +71,7 @@ int main(int argc, char* argv[]) // Increment depends on robot version double increment_constant = 0.0005; + std::cout << "Robot version: " << g_my_robot->getUrDriver()->getVersion().toString() << std::endl; if (g_my_robot->getUrDriver()->getVersion().major < 5) { increment_constant = 0.002; @@ -137,7 +138,8 @@ int main(int argc, char* argv[]) RobotReceiveTimeout::millisec(100)); if (!ret) { - URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); + 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()); diff --git a/examples/script_command_interface.cpp b/examples/script_command_interface.cpp index 6ada63f2f..8ea7e76b7 100644 --- a/examples/script_command_interface.cpp +++ b/examples/script_command_interface.cpp @@ -45,6 +45,7 @@ const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; std::unique_ptr g_my_robot; bool g_HEADLESS = true; bool g_running = false; +bool g_support_set_friction_scales = false; void sendScriptCommands() { @@ -67,13 +68,27 @@ void sendScriptCommands() run_cmd("Setting tool voltage to 24V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::_24V); }); run_cmd("Enabling tool contact mode", []() { g_my_robot->getUrDriver()->startToolContact(); }); - run_cmd("Setting friction_compensation variable to `false`", - []() { g_my_robot->getUrDriver()->setFrictionCompensation(false); }); + + if (g_support_set_friction_scales) + { + // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards. + run_cmd("Turn off friction_compensation by setting the friction scales to zeroes", + []() { g_my_robot->getUrDriver()->setFrictionScales({ 0, 0, 0, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }); }); + } run_cmd("Setting tool voltage to 0V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::OFF); }); run_cmd("Zeroing the force torque sensor", []() { g_my_robot->getUrDriver()->zeroFTSensor(); }); 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 TCP offset to [0.0, 0.0, 0.10, 0.0, 0.0, 0.0]", + []() { g_my_robot->getUrDriver()->setTcpOffset({ 0.0, 0.0, 0.10, 0.0, 0.0, 0.0 }); }); + + if (g_support_set_friction_scales) + { + // setFrictionScales is only supported from PolyScope 5.25.1 / PolyScope X 10.12.1 and upwards. + run_cmd("Turn on friction_compensation by setting the friction scales to non-zero values", []() { + g_my_robot->getUrDriver()->setFrictionScales({ 0.9, 0.9, 0.8, 0.9, 0.9, 0.9 }, + { 0.8, 0.8, 0.7, 0.8, 0.8, 0.8 }); + }); + } } URCL_LOG_INFO("Script command thread finished."); } @@ -106,6 +121,17 @@ int main(int argc, char* argv[]) g_my_robot = std::make_unique(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp"); + // Check if the robot version supports the friction scales command (requires 5.25.1 / 10.12.1) + auto version = g_my_robot->getUrDriver()->getVersion(); + g_support_set_friction_scales = (version.major == 5 && version >= urcl::VersionInformation::fromString("5.25.1")) || + (version.major >= 10 && version >= urcl::VersionInformation::fromString("10.12.1")); + if (!g_support_set_friction_scales) + { + URCL_LOG_INFO("Setting friction scales is not supported on this robot (version %s). " + "Requires at least 5.25.1 / 10.12.1. Skipping friction scale commands.", + version.toString().c_str()); + } + if (!g_my_robot->isHealthy()) { URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); diff --git a/include/ur_client_library/control/script_command_interface.h b/include/ur_client_library/control/script_command_interface.h index a0eb29c02..e339c08ee 100644 --- a/include/ur_client_library/control/script_command_interface.h +++ b/include/ur_client_library/control/script_command_interface.h @@ -177,16 +177,31 @@ class ScriptCommandInterface : public ReverseInterface bool setTcpOffset(const vector6d_t& offset); /*! - * \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. If true the direct_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. + + * \deprecated Use setFrictionScales() instead when using PolyScope 5.25.1 / PolyScope X 10.12.1 or later. * * \returns True, if the write was performed successfully, false otherwise. */ bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set viscous and Coulomb friction scale factors for direct_torque (per joint, range [0-1]). + * + * Requires PolyScope 5.25.1 / PolyScope X 10.12.1 or later. To disable friction compensation, pass both + * arrays as all zeros. Values outside [0, 1] will be silently clamped to the valid range. + * + * \param viscous_scale Scale of viscous compensation per joint, range [0-1]. + * \param coulomb_scale Scale of Coulomb compensation per joint, range [0-1]. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale); + /*! * \brief Enable or disable RTDE input for the force torque sensor. * @@ -248,6 +263,7 @@ class ScriptCommandInterface : public ReverseInterface FT_RTDE_INPUT_ENABLE = 8, ///< Enable FT RTDE input 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 }; /*! diff --git a/include/ur_client_library/helpers.h b/include/ur_client_library/helpers.h index 470f97e84..4afc20b66 100644 --- a/include/ur_client_library/helpers.h +++ b/include/ur_client_library/helpers.h @@ -109,5 +109,24 @@ bool parseBoolean(const std::string& str); */ std::vector splitString(const std::string& string_to_split, const std::string& delimiter = ","); +/*! + * \brief Clamps every element of a container to the range [0, 1] in-place. + * + * \tparam T The type of the elements in the array. + * \tparam N The size of the array. + * \param values The array whose elements will be clamped. + */ +template +void clampToUnitRange(std::array& values) +{ + for (auto& v : values) + { + if (v < 0.0) + v = 0.0; + else if (v > 1.0) + v = 1.0; + } +} + } // namespace urcl #endif // ifndef UR_CLIENT_LIBRARY_HELPERS_H_INCLUDED diff --git a/include/ur_client_library/ur/ur_driver.h b/include/ur_client_library/ur/ur_driver.h index e5e8c2f64..c87f07068 100644 --- a/include/ur_client_library/ur/ur_driver.h +++ b/include/ur_client_library/ur/ur_driver.h @@ -771,16 +771,34 @@ class UrDriver bool setTcpOffset(const vector6d_t& tcp_offset); /*! - * \brief Set friction compensation for the torque_command. If true the torque command will compensate for friction, + * \brief Set friction compensation for the direct_torque. If true the torque command will compensate for friction, * if false it will not. * + * \deprecated Use setFrictionScales() instead when using PolyScope > 5.25.1 / PolyScope X > 10.12.1. + * * \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. */ + [[deprecated("setFrictionCompensation is deprecated. Use setFrictionScales instead.")]] bool setFrictionCompensation(const bool friction_compensation_enabled); + /*! + * \brief Set viscous and Coulomb friction scale factors for direct_torque (per joint, range [0-1]). + * + * Values are per-joint scales for friction compensation. Zero means no compensation for that component. + * To disable friction compensation, pass both arrays as all zeros. Controller defaults when enabling + * are viscous_scale [0.9, 0.9, 0.8, 0.9, 0.9, 0.9] and coulomb_scale [0.8, 0.8, 0.7, 0.8, 0.8, 0.8]. + * Requires PolyScope 5.25.1 / PolyScope X 10.12.1 or later. + * + * \param viscous_scale Scale of viscous compensation per joint, range [0-1]. + * \param coulomb_scale Scale of Coulomb compensation per joint, range [0-1]. + * + * \returns True, if the write was performed successfully, false otherwise. + */ + bool setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale); + /*! * \brief Enable or disable RTDE input for the force torque sensor. * diff --git a/resources/external_control.urscript b/resources/external_control.urscript index 842b1adc2..8151bde50 100644 --- a/resources/external_control.urscript +++ b/resources/external_control.urscript @@ -58,6 +58,7 @@ SET_FRICTION_COMPENSATION = 7 FT_RTDE_INPUT_ENABLE = 8 SET_GRAVITY = 9 SET_TCP_OFFSET = 10 +SET_FRICTION_SCALES = 11 SCRIPT_COMMAND_DATA_DIMENSION = 28 FREEDRIVE_MODE_START = 1 @@ -75,6 +76,11 @@ MAX_JOINT_SPEED = 6.283185 # Any motion commands resulting in a velocity higher than that will be ignored. JOINT_IGNORE_SPEED = 20.0 +# Friction compensation mode: which set command was received last (or NotSet if none) +FRICTION_COMP_MODE_NOT_SET = 0 +FRICTION_COMP_MODE_FRICTION_LEGACY = 1 +FRICTION_COMP_MODE_FRICTION_SCALES = 2 + #Global variables are also showed in the Teach pendants variable list global violation_popup_counter = 0 global cmd_servo_state = SERVO_UNINITIALIZED @@ -91,7 +97,10 @@ global spline_qdd = [0, 0, 0, 0, 0, 0] global spline_qd = [0, 0, 0, 0, 0, 0] global tool_contact_running = False global trajectory_result = 0 -global friction_compensation_enabled = True +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 thread variables thread_move = 0 @@ -246,16 +255,36 @@ thread torqueThread(): textmsg("ExternalControl: Starting torque thread") while control_mode == MODE_TORQUE: torque = cmd_torque - {% if ROBOT_SOFTWARE_VERSION >= v5.23.0 %} - {% if ROBOT_SOFTWARE_VERSION < v6.0.0 %} - direct_torque(torque, friction_comp=friction_compensation_enabled) - {% elif ROBOT_SOFTWARE_VERSION >= v10.11.0 %} - direct_torque(torque, friction_comp=friction_compensation_enabled) + {% 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 10.11.0 and upwards.", error=True, blocking=True) + 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 5.23.0 and upwards.", error=True, blocking=True) + popup("Torque control is only supported from software 10.11.0 and upwards.", error=True, blocking=True) {% endif %} end textmsg("ExternalControl: torque thread ended") @@ -789,11 +818,12 @@ thread script_commands(): end tool_contact_running = False elif command == SET_FRICTION_COMPENSATION: - if raw_command[2] == 0: - friction_compensation_enabled = False - else: - friction_compensation_enabled = True - end + friction_compensation_mode = FRICTION_COMP_MODE_FRICTION_LEGACY + deprecated_friction_compensation_enabled = (raw_command[2] != 0) + elif command == SET_FRICTION_SCALES: + friction_compensation_mode = FRICTION_COMP_MODE_FRICTION_SCALES + viscous_scale = [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] + coulomb_scale = [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 == FT_RTDE_INPUT_ENABLE: if raw_command[2] == 0: enabled = False diff --git a/src/control/script_command_interface.cpp b/src/control/script_command_interface.cpp index 956b032f6..959ae2ceb 100644 --- a/src/control/script_command_interface.cpp +++ b/src/control/script_command_interface.cpp @@ -27,6 +27,7 @@ //---------------------------------------------------------------------- #include +#include #include namespace urcl @@ -279,6 +280,46 @@ bool ScriptCommandInterface::setFrictionCompensation(const bool friction_compens return server_.write(client_fd_, buffer, sizeof(buffer), written); } +bool ScriptCommandInterface::setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale) +{ + if (!robotVersionSupportsCommandOrWarn(urcl::VersionInformation::fromString("5.25.1"), + urcl::VersionInformation::fromString("10.12.1"), __func__)) + { + return false; + } + 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_FRICTION_SCALES)); + b_pos += append(b_pos, val); + + vector6d_t clamped_viscous_scale = viscous_scale; + vector6d_t clamped_coulomb_scale = coulomb_scale; + clampToUnitRange(clamped_viscous_scale); + clampToUnitRange(clamped_coulomb_scale); + + for (auto const& scale : clamped_viscous_scale) + { + val = htobe32(static_cast(round(scale * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + for (auto const& scale : clamped_coulomb_scale) + { + val = htobe32(static_cast(round(scale * MULT_JOINTSTATE))); + b_pos += append(b_pos, val); + } + + 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::ftRtdeInputEnable(const bool enabled, const double sensor_mass, const vector3d_t& sensor_measuring_offset, const vector3d_t& sensor_cog) { diff --git a/src/ur/ur_driver.cpp b/src/ur/ur_driver.cpp index 2c2ec764f..e0bcf724a 100644 --- a/src/ur/ur_driver.cpp +++ b/src/ur/ur_driver.cpp @@ -537,7 +537,7 @@ bool UrDriver::startToolContact() else { URCL_LOG_ERROR("Script command interface is not running. Unable to enable tool contact mode."); - return 0; + return false; } } @@ -560,7 +560,7 @@ bool UrDriver::endToolContact() else { URCL_LOG_ERROR("Script command interface is not running. Unable to end tool contact mode."); - return 0; + return false; } } @@ -573,7 +573,20 @@ bool UrDriver::setFrictionCompensation(const bool friction_compensation_enabled) else { URCL_LOG_ERROR("Script command interface is not running. Unable to set friction compensation."); - return 0; + return false; + } +} + +bool UrDriver::setFrictionScales(const vector6d_t& viscous_scale, const vector6d_t& coulomb_scale) +{ + if (script_command_interface_->clientConnected()) + { + return script_command_interface_->setFrictionScales(viscous_scale, coulomb_scale); + } + else + { + URCL_LOG_ERROR("Script command interface is not running. Unable to set friction scales."); + return false; } } @@ -587,7 +600,7 @@ bool UrDriver::ftRtdeInputEnable(const bool enabled, const double sensor_mass, else { URCL_LOG_ERROR("Script command interface is not running. Unable to set ft_rtde_input_enable."); - return 0; + return false; } } diff --git a/tests/test_script_command_interface.cpp b/tests/test_script_command_interface.cpp index ae623d463..02b886fc4 100644 --- a/tests/test_script_command_interface.cpp +++ b/tests/test_script_command_interface.cpp @@ -35,6 +35,7 @@ #include #include +#include using namespace urcl; @@ -541,6 +542,99 @@ TEST_F(ScriptCommandInterfaceTest, test_set_tcp_offset) EXPECT_EQ(message_sum, expected_message_sum); } +TEST_F(ScriptCommandInterfaceTest, test_set_friction_scales) +{ + waitForClientConnection(); + + vector6d_t viscous_scale = { 0.9, 0.9, 0.8, 0.9, 0.9, 0.9 }; + vector6d_t coulomb_scale = { 0.8, 0.8, 0.7, 0.8, 0.8, 0.8 }; + bool result = script_command_interface_->setFrictionScales(viscous_scale, coulomb_scale); + EXPECT_TRUE(result); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + int32_t expected_command = 11; + EXPECT_EQ(command, expected_command); + + for (size_t i = 0; i < 6; ++i) + { + double received_viscous = static_cast(message[i]) / script_command_interface_->MULT_JOINTSTATE; + EXPECT_DOUBLE_EQ(received_viscous, viscous_scale[i]); + } + for (size_t i = 0; i < 6; ++i) + { + double received_coulomb = static_cast(message[6 + i]) / script_command_interface_->MULT_JOINTSTATE; + EXPECT_DOUBLE_EQ(received_coulomb, coulomb_scale[i]); + } + + int32_t message_sum = std::accumulate(std::begin(message) + 12, std::end(message), 0); + EXPECT_EQ(message_sum, 0); + + vector6d_t zeros = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; + script_command_interface_->setFrictionScales(zeros, zeros); + message.clear(); + client_->readMessage(command, message); + EXPECT_EQ(command, expected_command); + for (size_t i = 0; i < 12; ++i) + { + EXPECT_EQ(message[i], 0); + } + message_sum = std::accumulate(std::begin(message) + 12, std::end(message), 0); + EXPECT_EQ(message_sum, 0); +} + +TEST_F(ScriptCommandInterfaceTest, test_set_friction_scales_clamps_to_valid_range) +{ + waitForClientConnection(); + + // Values outside [0, 1]: negative, > 1, and in-range + vector6d_t viscous_scale = { -0.1, 0.5, 1.2, 0.0, 1.0, 0.9 }; + vector6d_t coulomb_scale = { 0.3, -0.5, 1.5, 0.0, 1.0, 0.8 }; + vector6d_t expected_viscous = { 0.0, 0.5, 1.0, 0.0, 1.0, 0.9 }; + vector6d_t expected_coulomb = { 0.3, 0.0, 1.0, 0.0, 1.0, 0.8 }; + + bool result = script_command_interface_->setFrictionScales(viscous_scale, coulomb_scale); + EXPECT_TRUE(result); + + int32_t command; + std::vector message; + client_->readMessage(command, message); + + EXPECT_EQ(command, 11); + + for (size_t i = 0; i < 6; ++i) + { + double received_viscous = static_cast(message[i]) / script_command_interface_->MULT_JOINTSTATE; + EXPECT_DOUBLE_EQ(received_viscous, expected_viscous[i]) << "viscous_scale[" << i << "] should be clamped to [0, 1]"; + } + for (size_t i = 0; i < 6; ++i) + { + double received_coulomb = static_cast(message[6 + i]) / script_command_interface_->MULT_JOINTSTATE; + EXPECT_DOUBLE_EQ(received_coulomb, expected_coulomb[i]) << "coulomb_scale[" << i << "] should be clamped to [0, 1]"; + } +} + +TEST_F(ScriptCommandInterfaceTest, test_set_friction_scales_returns_false_on_old_version) +{ + control::ReverseInterfaceConfig config; + config.port = 50005; + config.robot_software_version = VersionInformation::fromString("5.24.0"); + control::ScriptCommandInterface old_version_interface(config); + std::unique_ptr old_client(new Client(50005)); + + waitFor([&old_version_interface]() { return old_version_interface.clientConnected(); }, + std::chrono::milliseconds(1000)); + + vector6d_t viscous_scale = { 0.9, 0.9, 0.8, 0.9, 0.9, 0.9 }; + vector6d_t coulomb_scale = { 0.8, 0.8, 0.7, 0.8, 0.8, 0.8 }; + bool result = old_version_interface.setFrictionScales(viscous_scale, coulomb_scale); + EXPECT_FALSE(result); + + old_client->close(); +} + 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 21d949e6e..e771d530e 100644 --- a/tests/test_script_reader.cpp +++ b/tests/test_script_reader.cpp @@ -36,6 +36,7 @@ #include "ur_client_library/control/reverse_interface.h" #include "ur_client_library/control/trajectory_point_interface.h" +#include #include #ifdef _WIN32 @@ -490,3 +491,163 @@ TEST_F(ScriptReaderTest, TestParsingExternalControl) expected_pattern = "ft_rtde_input_enable(enabled, sensor_mass, sensor_offset, sensor_cog)"; EXPECT_NE(processed_script.find(expected_pattern), std::string::npos); } + +TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion) +{ + 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(\"Torque control is only supported from software 5.23.0 and upwards.\", " + "error=True, blocking=True)"), + 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(\"Torque control is only supported from software 10.11.0 and upwards.\", " + "error=True, blocking=True)"), + std::string::npos); +} + +TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523) +{ + 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 this string to be present with spaces + EXPECT_NE(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope 5.25.1"), + std::string::npos); + + // Remove all whitespace + processed_script.erase(std::remove_if(processed_script.begin(), processed_script.end(), ::isspace), + processed_script.end()); + // Expect this string to be present (deprecated path uses friction_comp=... in direct_torque call) + EXPECT_NE(processed_script.find("friction_comp=deprecated_friction_compensation_enabled"), std::string::npos); + // Expect these string to not be present + EXPECT_EQ(processed_script.find("viscous_scale=viscous_scale"), std::string::npos); + EXPECT_EQ(processed_script.find("coulomb_scale=coulomb_scale"), std::string::npos); +} + +TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope1011) +{ + 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("10.11.0"); + + std::string processed_script = reader.readScriptFile(existing_script_file, data); + // Expect this string to be present with spaces + EXPECT_NE(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from PolyScope X " + "10.12.1"), + std::string::npos); + + // Remove all whitespace + processed_script.erase(std::remove_if(processed_script.begin(), processed_script.end(), ::isspace), + processed_script.end()); + // Expect this string to be present (deprecated path uses friction_comp=... in direct_torque call) + EXPECT_NE(processed_script.find("friction_comp=deprecated_friction_compensation_enabled"), std::string::npos); + // Expect these string to not be present + EXPECT_EQ(processed_script.find("viscous_scale=viscous_scale"), std::string::npos); + EXPECT_EQ(processed_script.find("coulomb_scale=coulomb_scale"), std::string::npos); +} + +TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler) +{ + 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.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)"), + 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)"), + 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)"), + std::string::npos); + EXPECT_EQ(processed_script.find("Friction scales (viscous_scale/coulomb_scale) are supported from"), + std::string::npos); +} + +// A test that produce all script files to a subfolder for different software version to be manually inspected +TEST_F(ScriptReaderTest, TestProduceAllScriptFiles) +{ + 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"; + + // List of software versions to test + std::vector software_versions = { + urcl::VersionInformation::fromString("3.14.3"), urcl::VersionInformation::fromString("5.9.4"), + urcl::VersionInformation::fromString("5.21.0"), urcl::VersionInformation::fromString("5.23.0"), + urcl::VersionInformation::fromString("5.25.1"), urcl::VersionInformation::fromString("10.8.0"), + urcl::VersionInformation::fromString("10.11.0"), urcl::VersionInformation::fromString("10.12.1"), + }; + + std::filesystem::path output_dir = std::filesystem::path("../test_artifacts/generated_scripts"); + std::filesystem::create_directories(output_dir); + + for (const auto& software_version : software_versions) + { + data["ROBOT_SOFTWARE_VERSION"] = software_version; + std::string processed_script = reader.readScriptFile(existing_script_file, data); + std::ofstream ofs(output_dir / (software_version.toString() + ".urscript")); + ofs << processed_script; + ofs.close(); + } +}