|
| 1 | +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- |
| 2 | + |
| 3 | +// -- BEGIN LICENSE BLOCK ---------------------------------------------- |
| 4 | +// Copyright 2023 Universal Robots A/S |
| 5 | +// |
| 6 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 7 | +// you may not use this file except in compliance with the License. |
| 8 | +// You may obtain a copy of the License at |
| 9 | +// |
| 10 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 11 | +// |
| 12 | +// Unless required by applicable law or agreed to in writing, software |
| 13 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 14 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 15 | +// See the License for the specific language governing permissions and |
| 16 | +// limitations under the License. |
| 17 | +// |
| 18 | +// -- END LICENSE BLOCK ------------------------------------------------ |
| 19 | + |
| 20 | +#include <ur_client_library/control/trajectory_point_interface.h> |
| 21 | +#include <ur_client_library/ur/dashboard_client.h> |
| 22 | +#include <ur_client_library/ur/ur_driver.h> |
| 23 | +#include <ur_client_library/types.h> |
| 24 | + |
| 25 | +#include <iostream> |
| 26 | +#include <memory> |
| 27 | + |
| 28 | +using namespace urcl; |
| 29 | + |
| 30 | +// In a real-world example it would be better to get those values from command line parameters / a |
| 31 | +// better configuration system such as Boost.Program_options |
| 32 | +const std::string DEFAULT_ROBOT_IP = "192.168.56.101"; |
| 33 | +const std::string SCRIPT_FILE = "resources/external_control.urscript"; |
| 34 | +const std::string OUTPUT_RECIPE = "examples/resources/rtde_output_recipe.txt"; |
| 35 | +const std::string INPUT_RECIPE = "examples/resources/rtde_input_recipe.txt"; |
| 36 | +const std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542"; |
| 37 | + |
| 38 | +std::unique_ptr<UrDriver> g_my_driver; |
| 39 | +std::unique_ptr<DashboardClient> g_my_dashboard; |
| 40 | +vector6d_t g_joint_positions; |
| 41 | + |
| 42 | +void SendTrajectory(const std::vector<vector6d_t>& p_p, const std::vector<vector6d_t>& p_v, |
| 43 | + const std::vector<vector6d_t>& p_a, const std::vector<double>& time, bool use_spline_interpolation_) |
| 44 | +{ |
| 45 | + assert(p_p.size() == time.size()); |
| 46 | + |
| 47 | + URCL_LOG_INFO("Starting joint-based trajectory forward"); |
| 48 | + g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size()); |
| 49 | + |
| 50 | + for (size_t i = 0; i < p_p.size() && p_p.size() == time.size() && p_p[i].size() == 6; i++) |
| 51 | + { |
| 52 | + // MoveJ |
| 53 | + if (!use_spline_interpolation_) |
| 54 | + { |
| 55 | + g_my_driver->writeTrajectoryPoint(p_p[i], false, time[i]); |
| 56 | + } |
| 57 | + else // Use spline interpolation |
| 58 | + { |
| 59 | + // QUINTIC |
| 60 | + if (p_v.size() == time.size() && p_a.size() == time.size() && p_v[i].size() == 6 && p_a[i].size() == 6) |
| 61 | + { |
| 62 | + g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]); |
| 63 | + } |
| 64 | + // CUBIC |
| 65 | + else if (p_v.size() == time.size() && p_v[i].size() == 6) |
| 66 | + { |
| 67 | + g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]); |
| 68 | + } |
| 69 | + else |
| 70 | + { |
| 71 | + g_my_driver->writeTrajectorySplinePoint(p_p[i], time[i]); |
| 72 | + } |
| 73 | + } |
| 74 | + } |
| 75 | + URCL_LOG_INFO("Finished Sending Trajectory"); |
| 76 | +} |
| 77 | + |
| 78 | +// We need a callback function to register. See UrDriver's parameters for details. |
| 79 | +void handleRobotProgramState(bool program_running) |
| 80 | +{ |
| 81 | + // Print the text in green so we see it better |
| 82 | + std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl; |
| 83 | +} |
| 84 | + |
| 85 | +// Callback function for trajectory execution. |
| 86 | +bool g_trajectory_running(false); |
| 87 | +void handleTrajectoryState(control::TrajectoryResult state) |
| 88 | +{ |
| 89 | + // trajectory_state = state; |
| 90 | + g_trajectory_running = false; |
| 91 | + std::string report = "?"; |
| 92 | + switch (state) |
| 93 | + { |
| 94 | + case control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS: |
| 95 | + report = "success"; |
| 96 | + break; |
| 97 | + case control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED: |
| 98 | + report = "canceled"; |
| 99 | + break; |
| 100 | + case control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE: |
| 101 | + default: |
| 102 | + report = "failure"; |
| 103 | + break; |
| 104 | + } |
| 105 | + std::cout << "\033[1;32mTrajectory report: " << report << "\033[0m\n" << std::endl; |
| 106 | +} |
| 107 | + |
| 108 | +int main(int argc, char* argv[]) |
| 109 | +{ |
| 110 | + urcl::setLogLevel(urcl::LogLevel::INFO); |
| 111 | + |
| 112 | + // Parse the ip arguments if given |
| 113 | + std::string robot_ip = DEFAULT_ROBOT_IP; |
| 114 | + if (argc > 1) |
| 115 | + { |
| 116 | + robot_ip = std::string(argv[1]); |
| 117 | + } |
| 118 | + |
| 119 | + // Making the robot ready for the program by: |
| 120 | + // Connect the the robot Dashboard |
| 121 | + g_my_dashboard.reset(new DashboardClient(robot_ip)); |
| 122 | + if (!g_my_dashboard->connect()) |
| 123 | + { |
| 124 | + URCL_LOG_ERROR("Could not connect to dashboard"); |
| 125 | + return 1; |
| 126 | + } |
| 127 | + |
| 128 | + // Stop program, if there is one running |
| 129 | + if (!g_my_dashboard->commandStop()) |
| 130 | + { |
| 131 | + URCL_LOG_ERROR("Could not send stop program command"); |
| 132 | + return 1; |
| 133 | + } |
| 134 | + |
| 135 | + // if the robot is not powered on and ready |
| 136 | + std::string robotModeRunning("RUNNING"); |
| 137 | + while (!g_my_dashboard->commandRobotMode(robotModeRunning)) |
| 138 | + { |
| 139 | + // Power it off |
| 140 | + if (!g_my_dashboard->commandPowerOff()) |
| 141 | + { |
| 142 | + URCL_LOG_ERROR("Could not send Power off command"); |
| 143 | + return 1; |
| 144 | + } |
| 145 | + |
| 146 | + // Power it on |
| 147 | + if (!g_my_dashboard->commandPowerOn()) |
| 148 | + { |
| 149 | + URCL_LOG_ERROR("Could not send Power on command"); |
| 150 | + return 1; |
| 151 | + } |
| 152 | + } |
| 153 | + |
| 154 | + // Release the brakes |
| 155 | + if (!g_my_dashboard->commandBrakeRelease()) |
| 156 | + { |
| 157 | + URCL_LOG_ERROR("Could not send BrakeRelease command"); |
| 158 | + return 1; |
| 159 | + } |
| 160 | + |
| 161 | + // Now the robot is ready to receive a program |
| 162 | + |
| 163 | + std::unique_ptr<ToolCommSetup> tool_comm_setup; |
| 164 | + const bool HEADLESS = true; |
| 165 | + g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS, |
| 166 | + std::move(tool_comm_setup), CALIBRATION_CHECKSUM)); |
| 167 | + g_my_driver->setKeepaliveCount(5); // This is for example purposes only. This will make the example running more |
| 168 | + // reliable on non-realtime systems. Do not use this in productive applications. |
| 169 | + |
| 170 | + g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState); |
| 171 | + |
| 172 | + // Once RTDE communication is started, we have to make sure to read from the interface buffer, as |
| 173 | + // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main |
| 174 | + // loop. |
| 175 | + |
| 176 | + g_my_driver->startRTDECommunication(); |
| 177 | + |
| 178 | + std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage(); |
| 179 | + |
| 180 | + if (data_pkg) |
| 181 | + { |
| 182 | + // Read current joint positions from robot data |
| 183 | + if (!data_pkg->getData("actual_q", g_joint_positions)) |
| 184 | + { |
| 185 | + // This throwing should never happen unless misconfigured |
| 186 | + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; |
| 187 | + throw std::runtime_error(error_msg); |
| 188 | + } |
| 189 | + } |
| 190 | + |
| 191 | + const unsigned number_of_points = 5; |
| 192 | + const double s_pos[number_of_points] = { 4.15364583e-03, 4.15364583e-03, -1.74088542e-02, 1.44817708e-02, 0.0 }; |
| 193 | + const double s_vel[number_of_points] = { -2.01015625e-01, 4.82031250e-02, 1.72812500e-01, -3.49453125e-01, |
| 194 | + 8.50000000e-02 }; |
| 195 | + const double s_acc[number_of_points] = { 2.55885417e+00, -4.97395833e-01, 1.71276042e+00, -5.36458333e-02, |
| 196 | + -2.69817708e+00 }; |
| 197 | + const double s_time[number_of_points] = { 1.0000000e+00, 4.00000000e+00, 8.00100000e+00, 1.25000000e+01, |
| 198 | + 4.00000000e+00 }; |
| 199 | + |
| 200 | + bool ret = false; |
| 201 | + URCL_LOG_INFO("Switch to Forward mode"); |
| 202 | + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); |
| 203 | + if (!ret) |
| 204 | + { |
| 205 | + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); |
| 206 | + return 1; |
| 207 | + } |
| 208 | + |
| 209 | + std::vector<vector6d_t> p, v, a; |
| 210 | + std::vector<double> time; |
| 211 | + |
| 212 | + unsigned int joint_to_control = 5; |
| 213 | + for (unsigned i = 0; i < number_of_points; ++i) |
| 214 | + { |
| 215 | + vector6d_t p_i = g_joint_positions; |
| 216 | + p_i[joint_to_control] = s_pos[i]; |
| 217 | + p.push_back(p_i); |
| 218 | + |
| 219 | + vector6d_t v_i; |
| 220 | + v_i[joint_to_control] = s_vel[i]; |
| 221 | + v.push_back(v_i); |
| 222 | + |
| 223 | + vector6d_t a_i; |
| 224 | + a_i[joint_to_control] = s_acc[i]; |
| 225 | + a.push_back(a_i); |
| 226 | + |
| 227 | + time.push_back(s_time[i]); |
| 228 | + } |
| 229 | + |
| 230 | + // CUBIC |
| 231 | + SendTrajectory(p, v, std::vector<vector6d_t>(), time, true); |
| 232 | + |
| 233 | + g_trajectory_running = true; |
| 234 | + while (g_trajectory_running) |
| 235 | + { |
| 236 | + std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage(); |
| 237 | + if (data_pkg) |
| 238 | + { |
| 239 | + // Read current joint positions from robot data |
| 240 | + if (!data_pkg->getData("actual_q", g_joint_positions)) |
| 241 | + { |
| 242 | + // This throwing should never happen unless misconfigured |
| 243 | + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; |
| 244 | + throw std::runtime_error(error_msg); |
| 245 | + } |
| 246 | + bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); |
| 247 | + |
| 248 | + if (!ret) |
| 249 | + { |
| 250 | + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); |
| 251 | + return 1; |
| 252 | + } |
| 253 | + } |
| 254 | + } |
| 255 | + |
| 256 | + URCL_LOG_INFO("CUBIC Movement done"); |
| 257 | + |
| 258 | + // QUINTIC |
| 259 | + SendTrajectory(p, v, a, time, true); |
| 260 | + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); |
| 261 | + |
| 262 | + g_trajectory_running = true; |
| 263 | + while (g_trajectory_running) |
| 264 | + { |
| 265 | + std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage(); |
| 266 | + if (data_pkg) |
| 267 | + { |
| 268 | + // Read current joint positions from robot data |
| 269 | + if (!data_pkg->getData("actual_q", g_joint_positions)) |
| 270 | + { |
| 271 | + // This throwing should never happen unless misconfigured |
| 272 | + std::string error_msg = "Did not find 'actual_q' in data sent from robot. This should not happen!"; |
| 273 | + throw std::runtime_error(error_msg); |
| 274 | + } |
| 275 | + bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); |
| 276 | + |
| 277 | + if (!ret) |
| 278 | + { |
| 279 | + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); |
| 280 | + return 1; |
| 281 | + } |
| 282 | + } |
| 283 | + } |
| 284 | + |
| 285 | + URCL_LOG_INFO("QUINTIC Movement done"); |
| 286 | + |
| 287 | + ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD); |
| 288 | + if (!ret) |
| 289 | + { |
| 290 | + URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?"); |
| 291 | + return 1; |
| 292 | + } |
| 293 | + return 0; |
| 294 | +} |
0 commit comments