Implemented spline interpolation in joint space

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); // decrease realtime requirements since example is running in
+                                     // CI
 
   // 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,288 @@
+// 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_)
+{
+  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;
+  }
+
+  // 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); // decrease realtime requirements since example is running in
+                                     // CI
+
+  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, 2.50000000e+01 / 2,
+                                            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;
+}

examples/tool_contact_example.cpp

@@ -114,6 +114,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); // decrease realtime requirements since example is running in
+                                     // CI
 
   g_my_driver->registerToolContactResultCallback(&handleToolContactResult);
 

include/ur_client_library/control/trajectory_point_interface.h

@@ -49,6 +49,26 @@ enum class TrajectoryResult : int32_t
   TRAJECTORY_RESULT_FAILURE = 2    ///< Aborted due to error during execution
 };
 
+/*!
+ * Spline types
+ */
+enum class TrajectorySplineType : int32_t
+{
+  SPLINE_QUADRATIC = 0,
+  SPLINE_CUBIC = 1,
+  SPLINE_QUINTIC = 2
+};
+
+/*!
+ * Motion Types
+ */
+enum class TrajectoryMotionType : int32_t
+{
+  JOINT_POINT = 0,
+  CARTESIAN_POINT = 1,
+  JOINT_POINT_SPLINE = 2
+};
+
 /*!
  * \brief The TrajectoryPointInterface class handles trajectory forwarding to the robot. Full
  * trajectories are forwarded to the robot controller and are executed there.
@@ -57,8 +77,6 @@ class TrajectoryPointInterface : public ReverseInterface
 {
 public:
   static const int32_t MULT_TIME = 1000;
-  static const int32_t JOINT_POINT = 0;
-  static const int32_t CARTESIAN_POINT = 1;
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -86,6 +104,20 @@ class TrajectoryPointInterface : public ReverseInterface
   bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius,
                             const bool cartesian);
 
+  /*!
+   * \brief Writes needed information to the robot to be read by the URScript program including
+   * velocity and acceleration information. Depending on the information given the robot will do quadratic (positions only), cubic (positions and velocities) or quintic (positions, velocities and accelerations) interpolation.
+   *
+   * \param positions A vector of joint or Cartesian target positions for the robot.
+   * \param velocities A vector of joint or Cartesian target velocities for the robot.
+   * \param accelerations A vector of joint or Cartesian target accelerations for the robot.
+   * \param goal_time The goal time to reach the target point.
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
+                                  const vector6d_t* accelerations, const float goal_time);
+
   void setTrajectoryEndCallback(std::function<void(TrajectoryResult)> callback)
   {
     handle_trajectory_end_ = callback;
@@ -99,6 +131,7 @@ class TrajectoryPointInterface : public ReverseInterface
   virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv) override;
 
 private:
+  static const int MESSAGE_LENGTH = 21;
   std::function<void(TrajectoryResult)> handle_trajectory_end_;
 };