Spline merge

.github/workflows/ci.yml

@@ -12,13 +12,13 @@ jobs:
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
             CTEST_OUTPUT_ON_FAILURE: 1
             ROBOT_MODEL: 'ur5'
-            URSIM_VERSION: '3.12.1'
+            URSIM_VERSION: '3.14.3'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/cb3'
           - DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
             CTEST_OUTPUT_ON_FAILURE: 1
             ROBOT_MODEL: 'ur5e'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
 
     steps:

.github/workflows/industrial-ci.yml

@@ -25,7 +25,7 @@ jobs:
             DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS_Driver#master https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS_Driver/master/.melodic.rosinstall"
             DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             ROBOT_MODEL: 'ur5e'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
           - ROS_DISTRO: noetic
@@ -35,7 +35,7 @@ jobs:
             BUILDER: catkin_tools
             DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             ROBOT_MODEL: 'ur5e'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
           - ROS_DISTRO: galactic
@@ -44,7 +44,7 @@ jobs:
             DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#galactic"
             DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             ROBOT_MODEL: 'ur5e'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
             NOT_TEST_DOWNSTREAM: true
@@ -54,7 +54,7 @@ jobs:
             DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#humble https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS2_Driver/humble/Universal_Robots_ROS2_Driver-not-released.humble.repos"
             DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             ROBOT_MODEL: 'ur5e'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
             NOT_TEST_DOWNSTREAM: true
@@ -64,7 +64,7 @@ jobs:
             DOWNSTREAM_WORKSPACE: "github:UniversalRobots/Universal_Robots_ROS2_Driver#main https://raw.githubusercontent.com/UniversalRobots/Universal_Robots_ROS2_Driver/main/Universal_Robots_ROS2_Driver-not-released.rolling.repos"
             DOCKER_RUN_OPTS: --network ursim_net
             BEFORE_INIT: 'apt-get update -qq && apt-get install -y iproute2 iputils-ping && ip addr && ping -c5 192.168.56.101'
-            URSIM_VERSION: '5.5.1'
+            URSIM_VERSION: '5.9.4'
             ROBOT_MODEL: 'ur5e'
             PROGRAM_FOLDER: 'tests/resources/dockerursim/programs/e-series'
             NOT_TEST_DOWNSTREAM: true

README.md

@@ -32,8 +32,8 @@ implemented in order to create external applications leveraging the versatility
 robotic manipulators.
 
 ## Requirements
- * **Polyscope** (The software running on the robot controller) version **3.12.0** (for CB3-Series),
-   or **5.5.1** (for e-Series) or higher. If you use an older Polyscope version it is suggested to
+ * **Polyscope** (The software running on the robot controller) version **3.14.3** (for CB3-Series),
+   or **5.9.4** (for e-Series) or higher. If you use an older Polyscope version it is suggested to
    update your robot. If for some reason (please tell us in the issues why) you cannot upgrade your
    robot, please see the [version compatibility table](doc/polyscope_compatibility.md) for a
    compatible tag.

doc/polyscope_compatibility.md

@@ -14,4 +14,4 @@ your robot.
 
 |Polyscope version | Maximum tag | Breaking changes |
 |------------------|-------------|------------------|
-| < 3.12.0 / 5.5.1 | [polyscope_compat_break_1](https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/polyscope_compat_break_1) | [tcp_offset in RTDE interface](https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/110)|
+| < 3.14.3 / 5.9.4 | [polyscope_compat_break_1](https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/polyscope_compat_break_1) | [tcp_offset in RTDE interface](https://github.com/UniversalRobots/Universal_Robots_Client_Library/pull/110)|

examples/CMakeLists.txt

@@ -35,6 +35,11 @@ add_executable(dashboard_example
 target_compile_options(dashboard_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(dashboard_example ur_client_library::urcl)
 
+add_executable(spline_example
+spline_example.cpp)
+target_compile_options(spline_example PUBLIC ${CXX17_FLAG})
+target_link_libraries(spline_example ur_client_library::urcl)
+
 add_executable(tool_contact_example
 tool_contact_example.cpp)
 target_compile_options(tool_contact_example PUBLIC ${CXX17_FLAG})

examples/full_driver.cpp

@@ -110,6 +110,8 @@ int main(int argc, char* argv[])
   const bool HEADLESS = true;
   g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
                                  std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
+                                      // reliable on non-realtime systems. Do not use this in productive applications.
 
   // 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

examples/spline_example.cpp

@@ -0,0 +1,294 @@
+// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
+
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2023 Universal Robots A/S
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+//
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#include <ur_client_library/control/trajectory_point_interface.h>
+#include <ur_client_library/ur/dashboard_client.h>
+#include <ur_client_library/ur/ur_driver.h>
+#include <ur_client_library/types.h>
+
+#include <iostream>
+#include <memory>
+
+using namespace urcl;
+
+// In a real-world example it would be better to get those values from command line parameters / a
+// better configuration system such as Boost.Program_options
+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 std::string CALIBRATION_CHECKSUM = "calib_12788084448423163542";
+
+std::unique_ptr<UrDriver> g_my_driver;
+std::unique_ptr<DashboardClient> g_my_dashboard;
+vector6d_t g_joint_positions;
+
+void SendTrajectory(const std::vector<vector6d_t>& p_p, const std::vector<vector6d_t>& p_v,
+                    const std::vector<vector6d_t>& p_a, const std::vector<double>& time, bool use_spline_interpolation_)
+{
+  assert(p_p.size() == time.size());
+
+  URCL_LOG_INFO("Starting joint-based trajectory forward");
+  g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, p_p.size());
+
+  for (size_t i = 0; i < p_p.size() && p_p.size() == time.size() && p_p[i].size() == 6; i++)
+  {
+    // MoveJ
+    if (!use_spline_interpolation_)
+    {
+      g_my_driver->writeTrajectoryPoint(p_p[i], false, time[i]);
+    }
+    else  // Use spline interpolation
+    {
+      // QUINTIC
+      if (p_v.size() == time.size() && p_a.size() == time.size() && p_v[i].size() == 6 && p_a[i].size() == 6)
+      {
+        g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], p_a[i], time[i]);
+      }
+      // CUBIC
+      else if (p_v.size() == time.size() && p_v[i].size() == 6)
+      {
+        g_my_driver->writeTrajectorySplinePoint(p_p[i], p_v[i], time[i]);
+      }
+      else
+      {
+        g_my_driver->writeTrajectorySplinePoint(p_p[i], time[i]);
+      }
+    }
+  }
+  URCL_LOG_INFO("Finished Sending Trajectory");
+}
+
+// We need a callback function to register. See UrDriver's parameters for details.
+void handleRobotProgramState(bool program_running)
+{
+  // Print the text in green so we see it better
+  std::cout << "\033[1;32mProgram running: " << std::boolalpha << program_running << "\033[0m\n" << std::endl;
+}
+
+// Callback function for trajectory execution.
+bool g_trajectory_running(false);
+void handleTrajectoryState(control::TrajectoryResult state)
+{
+  // trajectory_state = state;
+  g_trajectory_running = false;
+  std::string report = "?";
+  switch (state)
+  {
+    case control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS:
+      report = "success";
+      break;
+    case control::TrajectoryResult::TRAJECTORY_RESULT_CANCELED:
+      report = "canceled";
+      break;
+    case control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE:
+    default:
+      report = "failure";
+      break;
+  }
+  std::cout << "\033[1;32mTrajectory report: " << report << "\033[0m\n" << std::endl;
+}
+
+int main(int argc, char* argv[])
+{
+  urcl::setLogLevel(urcl::LogLevel::INFO);
+
+  // Parse the ip arguments if given
+  std::string robot_ip = DEFAULT_ROBOT_IP;
+  if (argc > 1)
+  {
+    robot_ip = std::string(argv[1]);
+  }
+
+  // Making the robot ready for the program by:
+  // Connect the the robot Dashboard
+  g_my_dashboard.reset(new DashboardClient(robot_ip));
+  if (!g_my_dashboard->connect())
+  {
+    URCL_LOG_ERROR("Could not connect to dashboard");
+    return 1;
+  }
+
+  // Stop program, if there is one running
+  if (!g_my_dashboard->commandStop())
+  {
+    URCL_LOG_ERROR("Could not send stop program command");
+    return 1;
+  }
+
+  // if the robot is not powered on and ready
+  std::string robotModeRunning("RUNNING");
+  while (!g_my_dashboard->commandRobotMode(robotModeRunning))
+  {
+    // Power it off
+    if (!g_my_dashboard->commandPowerOff())
+    {
+      URCL_LOG_ERROR("Could not send Power off command");
+      return 1;
+    }
+
+    // Power it on
+    if (!g_my_dashboard->commandPowerOn())
+    {
+      URCL_LOG_ERROR("Could not send Power on command");
+      return 1;
+    }
+  }
+
+  // Release the brakes
+  if (!g_my_dashboard->commandBrakeRelease())
+  {
+    URCL_LOG_ERROR("Could not send BrakeRelease command");
+    return 1;
+  }
+
+  // Now the robot is ready to receive a program
+
+  std::unique_ptr<ToolCommSetup> tool_comm_setup;
+  const bool HEADLESS = true;
+  g_my_driver.reset(new UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState, HEADLESS,
+                                 std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+  g_my_driver->setKeepaliveCount(5);  // This is for example purposes only. This will make the example running more
+                                      // reliable on non-realtime systems. Do not use this in productive applications.
+
+  g_my_driver->registerTrajectoryDoneCallback(&handleTrajectoryState);
+
+  // Once RTDE communication is started, we have to make sure to read from the interface buffer, as
+  // otherwise we will get pipeline overflows. Therefore, do this directly before starting your main
+  // loop.
+
+  g_my_driver->startRTDECommunication();
+
+  std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
+
+  if (data_pkg)
+  {
+    // Read current joint positions from robot data
+    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);
+    }
+  }
+
+  const unsigned number_of_points = 5;
+  const double s_pos[number_of_points] = { 4.15364583e-03, 4.15364583e-03, -1.74088542e-02, 1.44817708e-02, 0.0 };
+  const double s_vel[number_of_points] = { -2.01015625e-01, 4.82031250e-02, 1.72812500e-01, -3.49453125e-01,
+                                           8.50000000e-02 };
+  const double s_acc[number_of_points] = { 2.55885417e+00, -4.97395833e-01, 1.71276042e+00, -5.36458333e-02,
+                                           -2.69817708e+00 };
+  const double s_time[number_of_points] = { 1.0000000e+00, 4.00000000e+00, 8.00100000e+00, 1.25000000e+01,
+                                            4.00000000e+00 };
+
+  bool ret = false;
+  URCL_LOG_INFO("Switch to Forward mode");
+  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+  if (!ret)
+  {
+    URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+    return 1;
+  }
+
+  std::vector<vector6d_t> p, v, a;
+  std::vector<double> time;
+
+  unsigned int joint_to_control = 5;
+  for (unsigned i = 0; i < number_of_points; ++i)
+  {
+    vector6d_t p_i = g_joint_positions;
+    p_i[joint_to_control] = s_pos[i];
+    p.push_back(p_i);
+
+    vector6d_t v_i;
+    v_i[joint_to_control] = s_vel[i];
+    v.push_back(v_i);
+
+    vector6d_t a_i;
+    a_i[joint_to_control] = s_acc[i];
+    a.push_back(a_i);
+
+    time.push_back(s_time[i]);
+  }
+
+  // CUBIC
+  SendTrajectory(p, v, std::vector<vector6d_t>(), time, true);
+
+  g_trajectory_running = true;
+  while (g_trajectory_running)
+  {
+    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
+    if (data_pkg)
+    {
+      // Read current joint positions from robot data
+      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);
+      }
+      bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+
+      if (!ret)
+      {
+        URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+        return 1;
+      }
+    }
+  }
+
+  URCL_LOG_INFO("CUBIC Movement done");
+
+  // QUINTIC
+  SendTrajectory(p, v, a, time, true);
+  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+
+  g_trajectory_running = true;
+  while (g_trajectory_running)
+  {
+    std::unique_ptr<rtde_interface::DataPackage> data_pkg = g_my_driver->getDataPackage();
+    if (data_pkg)
+    {
+      // Read current joint positions from robot data
+      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);
+      }
+      bool ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+
+      if (!ret)
+      {
+        URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+        return 1;
+      }
+    }
+  }
+
+  URCL_LOG_INFO("QUINTIC Movement done");
+
+  ret = g_my_driver->writeJointCommand(vector6d_t(), comm::ControlMode::MODE_FORWARD);
+  if (!ret)
+  {
+    URCL_LOG_ERROR("Could not send joint command. Is the robot in remote control?");
+    return 1;
+  }
+  return 0;
+}