|
| 1 | +// -- BEGIN LICENSE BLOCK ---------------------------------------------- |
| 2 | +// Copyright 2025 Universal Robots A/S |
| 3 | +// |
| 4 | +// Redistribution and use in source and binary forms, with or without |
| 5 | +// modification, are permitted provided that the following conditions are met: |
| 6 | +// |
| 7 | +// * Redistributions of source code must retain the above copyright |
| 8 | +// notice, this list of conditions and the following disclaimer. |
| 9 | +// |
| 10 | +// * Redistributions in binary form must reproduce the above copyright |
| 11 | +// notice, this list of conditions and the following disclaimer in the |
| 12 | +// documentation and/or other materials provided with the distribution. |
| 13 | +// |
| 14 | +// * Neither the name of the {copyright_holder} nor the names of its |
| 15 | +// contributors may be used to endorse or promote products derived from |
| 16 | +// this software without specific prior written permission. |
| 17 | +// |
| 18 | +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 19 | +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 20 | +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 21 | +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 22 | +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 23 | +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 24 | +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 25 | +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 26 | +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 27 | +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 28 | +// POSSIBILITY OF SUCH DAMAGE. |
| 29 | +// -- END LICENSE BLOCK ------------------------------------------------ |
| 30 | + |
| 31 | +#include <chrono> |
| 32 | +#include <string> |
| 33 | +#include "ur_client_library/ur/tool_communication.h" |
| 34 | + |
| 35 | +#include <ur_client_library/log.h> |
| 36 | +#include <ur_client_library/example_robot_wrapper.h> |
| 37 | + |
| 38 | +using namespace urcl; |
| 39 | + |
| 40 | +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; |
| 41 | +const std::string SCRIPT_FILE = "resources/external_control.urscript"; |
| 42 | +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; |
| 43 | +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; |
| 44 | + |
| 45 | +std::unique_ptr<ExampleRobotWrapper> g_my_robot; |
| 46 | +bool g_HEADLESS = true; |
| 47 | +bool g_running = false; |
| 48 | + |
| 49 | +void sendScriptCommands() |
| 50 | +{ |
| 51 | + auto run_cmd = [](const std::string& log_output, std::function<void()> func) { |
| 52 | + const std::chrono::seconds timeout(3); |
| 53 | + if (g_running) |
| 54 | + { |
| 55 | + // We wait a fixed time so that not each command is run directly behind each other. |
| 56 | + // This is done for example purposes only, so users can follow the effect on the teach |
| 57 | + // pendant. |
| 58 | + std::this_thread::sleep_for(timeout); |
| 59 | + URCL_LOG_INFO(log_output.c_str()); |
| 60 | + func(); |
| 61 | + } |
| 62 | + }; |
| 63 | + |
| 64 | + // Keep running all commands in a loop until g_running is set to false |
| 65 | + while (g_running) |
| 66 | + { |
| 67 | + run_cmd("Setting tool voltage to 24V", |
| 68 | + []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::_24V); }); |
| 69 | + run_cmd("Enabling tool contact mode", []() { g_my_robot->getUrDriver()->startToolContact(); }); |
| 70 | + run_cmd("Setting friction_compensation variable to `false`", |
| 71 | + []() { g_my_robot->getUrDriver()->setFrictionCompensation(false); }); |
| 72 | + run_cmd("Setting tool voltage to 0V", []() { g_my_robot->getUrDriver()->setToolVoltage(urcl::ToolVoltage::OFF); }); |
| 73 | + run_cmd("Zeroing the force torque sensor", []() { g_my_robot->getUrDriver()->zeroFTSensor(); }); |
| 74 | + run_cmd("Disabling tool contact mode", []() { g_my_robot->getUrDriver()->endToolContact(); }); |
| 75 | + run_cmd("Setting friction_compensation variable to `true`", |
| 76 | + []() { g_my_robot->getUrDriver()->setFrictionCompensation(true); }); |
| 77 | + } |
| 78 | + URCL_LOG_INFO("Script command thread finished."); |
| 79 | +} |
| 80 | + |
| 81 | +int main(int argc, char* argv[]) |
| 82 | +{ |
| 83 | + urcl::setLogLevel(urcl::LogLevel::INFO); |
| 84 | + // Parse the ip arguments if given |
| 85 | + std::string robot_ip = DEFAULT_ROBOT_IP; |
| 86 | + if (argc > 1) |
| 87 | + { |
| 88 | + robot_ip = std::string(argv[1]); |
| 89 | + } |
| 90 | + |
| 91 | + // Parse how many seconds to run |
| 92 | + auto second_to_run = std::chrono::seconds(0); |
| 93 | + if (argc > 2) |
| 94 | + { |
| 95 | + second_to_run = std::chrono::seconds(std::stoi(argv[2])); |
| 96 | + } |
| 97 | + |
| 98 | + // Parse whether to run in headless mode |
| 99 | + // When not using headless mode, the global variables can be watched on the teach pendant. |
| 100 | + if (argc > 3) |
| 101 | + { |
| 102 | + g_HEADLESS = std::string(argv[3]) == "true" || std::string(argv[3]) == "1" || std::string(argv[3]) == "True" || |
| 103 | + std::string(argv[3]) == "TRUE"; |
| 104 | + } |
| 105 | + |
| 106 | + g_my_robot = |
| 107 | + std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, g_HEADLESS, "external_control.urp"); |
| 108 | + |
| 109 | + if (!g_my_robot->isHealthy()) |
| 110 | + { |
| 111 | + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); |
| 112 | + return 1; |
| 113 | + } |
| 114 | + |
| 115 | + // We will send script commands from a separate thread. That will stay active as long as |
| 116 | + // g_running is true. |
| 117 | + g_running = true; |
| 118 | + std::thread script_command_send_thread(sendScriptCommands); |
| 119 | + |
| 120 | + // We will need to keep the script running on the robot. As we use the "usual" external_control |
| 121 | + // urscript, we'll have to send keepalive signals as long as we want to keep it active. |
| 122 | + std::chrono::duration<double> time_done(0); |
| 123 | + std::chrono::duration<double> timeout(second_to_run); |
| 124 | + auto stopwatch_last = std::chrono::steady_clock::now(); |
| 125 | + auto stopwatch_now = stopwatch_last; |
| 126 | + while ((time_done < timeout || second_to_run.count() == 0) && g_my_robot->isHealthy()) |
| 127 | + { |
| 128 | + g_my_robot->getUrDriver()->writeKeepalive(); |
| 129 | + |
| 130 | + stopwatch_now = std::chrono::steady_clock::now(); |
| 131 | + time_done += stopwatch_now - stopwatch_last; |
| 132 | + stopwatch_last = stopwatch_now; |
| 133 | + std::this_thread::sleep_for( |
| 134 | + std::chrono::milliseconds(static_cast<int>(1.0 / g_my_robot->getUrDriver()->getControlFrequency()))); |
| 135 | + } |
| 136 | + |
| 137 | + URCL_LOG_INFO("Timeout reached."); |
| 138 | + g_my_robot->getUrDriver()->stopControl(); |
| 139 | + |
| 140 | + // Stop the script command thread |
| 141 | + g_running = false; |
| 142 | + script_command_send_thread.join(); |
| 143 | + |
| 144 | + return 0; |
| 145 | +} |
0 commit comments