|
| 1 | +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- |
| 2 | + |
| 3 | +// -- BEGIN LICENSE BLOCK ---------------------------------------------- |
| 4 | +// Copyright 2025 Universal Robots A/S |
| 5 | +// |
| 6 | +// Redistribution and use in source and binary forms, with or without |
| 7 | +// modification, are permitted provided that the following conditions are met: |
| 8 | +// |
| 9 | +// * Redistributions of source code must retain the above copyright |
| 10 | +// notice, this list of conditions and the following disclaimer. |
| 11 | +// |
| 12 | +// * Redistributions in binary form must reproduce the above copyright |
| 13 | +// notice, this list of conditions and the following disclaimer in the |
| 14 | +// documentation and/or other materials provided with the distribution. |
| 15 | +// |
| 16 | +// * Neither the name of the {copyright_holder} nor the names of its |
| 17 | +// contributors may be used to endorse or promote products derived from |
| 18 | +// this software without specific prior written permission. |
| 19 | +// |
| 20 | +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
| 21 | +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
| 22 | +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
| 23 | +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE |
| 24 | +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
| 25 | +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
| 26 | +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
| 27 | +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN |
| 28 | +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
| 29 | +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 30 | +// POSSIBILITY OF SUCH DAMAGE. |
| 31 | +// -- END LICENSE BLOCK ------------------------------------------------ |
| 32 | + |
| 33 | +#include "ur_client_library/example_robot_wrapper.h" |
| 34 | +#include "ur_client_library/ur/dashboard_client.h" |
| 35 | +#include "ur_client_library/ur/ur_driver.h" |
| 36 | +#include "ur_client_library/types.h" |
| 37 | +#include "ur_client_library/ur/instruction_executor.h" |
| 38 | + |
| 39 | +#include <iostream> |
| 40 | +#include <memory> |
| 41 | +#include <cmath> |
| 42 | +#include <signal.h> |
| 43 | + |
| 44 | +using namespace urcl; |
| 45 | + |
| 46 | +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; |
| 47 | +const std::string SCRIPT_FILE = "resources/external_control.urscript"; |
| 48 | +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; |
| 49 | +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; |
| 50 | + |
| 51 | +std::unique_ptr<ExampleRobotWrapper> g_my_robot; |
| 52 | + |
| 53 | +void signal_callback_handler(int signum) |
| 54 | +{ |
| 55 | + std::cout << "Caught signal " << signum << std::endl; |
| 56 | + if (g_my_robot != nullptr) |
| 57 | + { |
| 58 | + // Stop control of the robot |
| 59 | + g_my_robot->getUrDriver()->stopControl(); |
| 60 | + } |
| 61 | + // Terminate program |
| 62 | + exit(signum); |
| 63 | +} |
| 64 | + |
| 65 | +struct DataStorage |
| 66 | +{ |
| 67 | + std::vector<urcl::vector6d_t> target; |
| 68 | + std::vector<urcl::vector6d_t> actual; |
| 69 | + std::vector<double> time; |
| 70 | + |
| 71 | + void write_to_file(const std::string& filename) |
| 72 | + { |
| 73 | + std::fstream output(filename, std::ios::out); |
| 74 | + output << "timestamp,target0,target1,target2,target3,target4,target5,actual0,actual1,actual2," |
| 75 | + "actual3,actual4,actual5\n"; |
| 76 | + for (size_t i = 0; i < time.size(); ++i) |
| 77 | + { |
| 78 | + output << time[i]; |
| 79 | + for (auto& t : target[i]) |
| 80 | + { |
| 81 | + output << "," << t; |
| 82 | + } |
| 83 | + for (auto& a : actual[i]) |
| 84 | + { |
| 85 | + output << "," << a; |
| 86 | + } |
| 87 | + output << "\n"; |
| 88 | + } |
| 89 | + output.close(); |
| 90 | + } |
| 91 | +}; |
| 92 | + |
| 93 | +bool pd_control_loop(DataStorage& data_storage, const std::string& actual_data_name, |
| 94 | + const comm::ControlMode control_mode, const urcl::vector6d_t& amplitude) |
| 95 | +{ |
| 96 | + const int32_t running_time = 13; |
| 97 | + double time = 0.0; |
| 98 | + |
| 99 | + // Reserve space for expected amount of data |
| 100 | + data_storage.actual.reserve(running_time * 500); |
| 101 | + data_storage.target.reserve(running_time * 500); |
| 102 | + data_storage.time.reserve(running_time * 500); |
| 103 | + |
| 104 | + urcl::vector6d_t actual, target, start = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; |
| 105 | + |
| 106 | + bool first_pass = true; |
| 107 | + while (time < running_time) |
| 108 | + { |
| 109 | + const auto t_start = std::chrono::high_resolution_clock::now(); |
| 110 | + // Read latest RTDE package. This will block for a hard-coded timeout (see UrDriver), so the |
| 111 | + // robot will effectively be in charge of setting the frequency of this loop. |
| 112 | + // In a real-world application this thread should be scheduled with real-time priority in order |
| 113 | + // to ensure that this is called in time. |
| 114 | + std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_robot->getUrDriver()->getDataPackage(); |
| 115 | + if (!data_pkg) |
| 116 | + { |
| 117 | + URCL_LOG_WARN("Could not get fresh data package from robot"); |
| 118 | + return false; |
| 119 | + } |
| 120 | + // Read current joint positions from robot data |
| 121 | + if (!data_pkg->getData(actual_data_name, actual)) |
| 122 | + { |
| 123 | + // This throwing should never happen unless misconfigured |
| 124 | + std::string error_msg = "Did not find" + actual_data_name + "in data sent from robot. This should not happen!"; |
| 125 | + throw std::runtime_error(error_msg); |
| 126 | + } |
| 127 | + |
| 128 | + if (first_pass) |
| 129 | + { |
| 130 | + target = actual; |
| 131 | + start = actual; |
| 132 | + for (size_t i = 0; i < start.size(); ++i) |
| 133 | + { |
| 134 | + start[i] = start[i] - amplitude[i]; |
| 135 | + } |
| 136 | + first_pass = false; |
| 137 | + } |
| 138 | + |
| 139 | + for (size_t i = 0; i < target.size(); ++i) |
| 140 | + { |
| 141 | + target[i] = start[i] + amplitude[i] * cos(time); |
| 142 | + } |
| 143 | + |
| 144 | + // Setting the RobotReceiveTimeout time is for example purposes only. This will make the example running more |
| 145 | + // reliable on non-realtime systems. Use with caution in productive applications. |
| 146 | + bool ret = g_my_robot->getUrDriver()->writeJointCommand(target, control_mode, RobotReceiveTimeout::millisec(100)); |
| 147 | + if (!ret) |
| 148 | + { |
| 149 | + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); |
| 150 | + return false; |
| 151 | + } |
| 152 | + |
| 153 | + // Increment time and log data |
| 154 | + const auto t_end = std::chrono::high_resolution_clock::now(); |
| 155 | + const double elapsed_time_ms = std::chrono::duration<double, std::milli>(t_end - t_start).count() / 1000; |
| 156 | + time = time + elapsed_time_ms; |
| 157 | + data_storage.actual.push_back(actual); |
| 158 | + data_storage.target.push_back(target); |
| 159 | + data_storage.time.push_back(time); |
| 160 | + } |
| 161 | + |
| 162 | + return true; |
| 163 | +} |
| 164 | + |
| 165 | +int main(int argc, char* argv[]) |
| 166 | +{ |
| 167 | + // This will make sure that we stop controlling the robot if the user presses ctrl-c |
| 168 | + signal(SIGINT, signal_callback_handler); |
| 169 | + |
| 170 | + urcl::setLogLevel(urcl::LogLevel::INFO); |
| 171 | + |
| 172 | + // Parse the ip arguments if given |
| 173 | + std::string robot_ip = DEFAULT_ROBOT_IP; |
| 174 | + if (argc > 1) |
| 175 | + { |
| 176 | + robot_ip = std::string(argv[1]); |
| 177 | + } |
| 178 | + |
| 179 | + const bool headless_mode = true; |
| 180 | + g_my_robot = std::make_unique<ExampleRobotWrapper>(robot_ip, OUTPUT_RECIPE, INPUT_RECIPE, headless_mode); |
| 181 | + |
| 182 | + if (!g_my_robot->isHealthy()) |
| 183 | + { |
| 184 | + URCL_LOG_ERROR("Something in the robot initialization went wrong. Exiting. Please check the output above."); |
| 185 | + return 1; |
| 186 | + } |
| 187 | + |
| 188 | + auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_robot->getUrDriver()); |
| 189 | + |
| 190 | + URCL_LOG_INFO("Move the robot to initial position"); |
| 191 | + instruction_executor->moveJ(urcl::vector6d_t{ 0, -1.67, -0.65, -1.59, 1.61, 5.09 }, 0.5, 0.2, 5); |
| 192 | + |
| 193 | + DataStorage joint_controller_storage; |
| 194 | + |
| 195 | + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as |
| 196 | + // otherwise we will get pipeline overflows. Therefor, do this directly before starting your main |
| 197 | + // loop. |
| 198 | + g_my_robot->getUrDriver()->startRTDECommunication(); |
| 199 | + URCL_LOG_INFO("Start controlling the robot using the PD controller"); |
| 200 | + const bool completed_joint_control = |
| 201 | + pd_control_loop(joint_controller_storage, "actual_q", comm::ControlMode::MODE_PD_CONTROLLER_JOINT, |
| 202 | + { 0.2, 0.2, 0.2, 0.2, 0.2, 0.2 }); |
| 203 | + if (!completed_joint_control) |
| 204 | + { |
| 205 | + URCL_LOG_ERROR("Didn't complete pd control in joint space"); |
| 206 | + g_my_robot->getUrDriver()->stopControl(); |
| 207 | + return 1; |
| 208 | + } |
| 209 | + |
| 210 | + return 0; |
| 211 | +} |
0 commit comments