Add instruction executor for high-level robot control (#242)

With the trajectory point interface there is a method of executing
motions with only the urcl present as shown in
https://github.com/UniversalRobots/Universal_Robots_Client_Library/blob/master/examples/trajectory_point_interface.cpp

However, usage is a bit cumbersome, we might want to support a one-liner
for executing ptp motions, instead.

This PR adds the `InstructionExecutor`, a high-level module that uses
existing functionality to provide simple-to-use interfaces, currently to
execute MoveJ, MoveL and sequences of the two. In future, we might
extend that to other instructions such as other motion types, but
possibly also other functionality, hence the more general name.

Author: urfeex

CMakeLists.txt

@@ -33,6 +33,7 @@ add_library(urcl SHARED
     src/ur/ur_driver.cpp
     src/ur/calibration_checker.cpp
     src/ur/dashboard_client.cpp
+    src/ur/instruction_executor.cpp
     src/ur/tool_communication.cpp
     src/ur/robot_receive_timeout.cpp
     src/ur/version_information.cpp

doc/architecture.rst

@@ -14,6 +14,7 @@ well as a couple of standalone modules to directly use subsets of the library's
    architecture/script_sender
    architecture/trajectory_point_interface
    architecture/ur_driver
+   architecture/instruction_executor
 
 
 Dataflow overview with UrDriver

doc/architecture/instruction_executor.rst

@@ -0,0 +1,23 @@
+Instruction Executor
+====================
+
+The Instruction Executor is a convenience wrapper to make common robot instructions such as point
+to point motions easily accessible. Currently, it supports the following instructions:
+
+* Excecute MoveJ point to point motions
+* Execute MoveL point to point motions
+* Execute sequences consisting of MoveJ and MoveL instructions
+
+The Instruction Executor uses the :ref:`trajectory_point_interface` and the
+:ref:`reverse_interface`
+for sending motion instructions to the robot. Hence, it requires a :ref:`ur_driver` object.
+
+As a minimal working example, please see ``examples/instruction_executor.cpp`` example:
+
+.. literalinclude:: ../../examples/instruction_executor.cpp
+   :language: c++
+   :caption: examples/instruction_executor.cpp
+   :linenos:
+   :lineno-match:
+   :start-at: g_my_driver.reset
+   :end-at: g_my_driver->stopControl();

examples/CMakeLists.txt

@@ -47,3 +47,7 @@ target_link_libraries(script_sender_example ur_client_library::urcl)
 add_executable(trajectory_point_interface_example
 trajectory_point_interface.cpp)
 target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)
+
+add_executable(instruction_executor
+instruction_executor.cpp)
+target_link_libraries(instruction_executor ur_client_library::urcl)

examples/instruction_executor.cpp

@@ -0,0 +1,135 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2024 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#include <chrono>
+#include <string>
+#include <thread>
+
+#include "ur_client_library/types.h"
+#include "ur_client_library/ur/ur_driver.h"
+#include "ur_client_library/log.h"
+#include "ur_client_library/control/trajectory_point_interface.h"
+#include "ur_client_library/ur/dashboard_client.h"
+#include "ur_client_library/ur/instruction_executor.h"
+
+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<urcl::DashboardClient> g_my_dashboard;
+std::shared_ptr<urcl::UrDriver> g_my_driver;
+
+// 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;
+}
+
+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]);
+  }
+
+  // --------------- INITIALIZATION BEGIN -------------------
+  // Making the robot ready for the program by:
+  // Connect the robot Dashboard
+  g_my_dashboard.reset(new urcl::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;
+  }
+
+  std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
+  const bool headless = true;
+  g_my_driver.reset(new urcl::UrDriver(robot_ip, SCRIPT_FILE, OUTPUT_RECIPE, INPUT_RECIPE, &handleRobotProgramState,
+                                       headless, std::move(tool_comm_setup), CALIBRATION_CHECKSUM));
+  auto instruction_executor = std::make_shared<urcl::InstructionExecutor>(g_my_driver);
+  // --------------- INITIALIZATION END -------------------
+
+  URCL_LOG_INFO("Running motion");
+  // Trajectory definition
+  std::vector<std::shared_ptr<urcl::control::MotionPrimitive>> motion_sequence{
+    std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.57, 0, 0, 0, 0 }, 0.1,
+                                                    std::chrono::seconds(5)),
+    std::make_shared<urcl::control::MoveJPrimitive>(urcl::vector6d_t{ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 }, 0.1,
+                                                    std::chrono::seconds(5)),
+
+    std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose(-0.203, 0.263, 0.559, 0.68, -1.083, -2.076), 0.1,
+                                                    std::chrono::seconds(2)),
+    std::make_shared<urcl::control::MoveLPrimitive>(urcl::Pose{ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 }, 0.1,
+                                                    std::chrono::seconds(2)),
+  };
+  instruction_executor->executeMotion(motion_sequence);
+
+  instruction_executor->moveJ({ -1.57, -1.57, 0, 0, 0, 0 });
+  instruction_executor->moveJ({ -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 });
+  instruction_executor->moveL({ -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 });
+  instruction_executor->moveL({ -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 });
+
+  g_my_driver->stopControl();
+  return 0;
+}

include/ur_client_library/control/motion_primitives.h

@@ -0,0 +1,95 @@
+// -- BEGIN LICENSE BLOCK ----------------------------------------------
+// Copyright 2025 Universal Robots A/S
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+//    * Redistributions of source code must retain the above copyright
+//      notice, this list of conditions and the following disclaimer.
+//
+//    * Redistributions in binary form must reproduce the above copyright
+//      notice, this list of conditions and the following disclaimer in the
+//      documentation and/or other materials provided with the distribution.
+//
+//    * Neither the name of the {copyright_holder} nor the names of its
+//      contributors may be used to endorse or promote products derived from
+//      this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+// -- END LICENSE BLOCK ------------------------------------------------
+
+#ifndef UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+#define UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED
+
+#include <chrono>
+#include <ur_client_library/types.h>
+
+namespace urcl
+{
+namespace control
+{
+
+enum class MotionType : uint8_t
+{
+  MOVEJ = 0,
+  MOVEL = 1,
+  MOVEP = 2,
+  MOVEC = 3,
+  SPLINE = 51,
+  UNKNOWN = 255
+};
+struct MotionPrimitive
+{
+  MotionType type;
+  std::chrono::duration<double> duration;
+  double acceleration;
+  double velocity;
+  double blend_radius;
+};
+
+struct MoveJPrimitive : public MotionPrimitive
+{
+  MoveJPrimitive(const urcl::vector6d_t& target, const double blend_radius = 0,
+                 const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
+                 const double acceleration = 1.4, const double velocity = 1.04)
+  {
+    type = MotionType::MOVEJ;
+    target_joint_configuration = target;
+    this->duration = duration;
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+  }
+
+  urcl::vector6d_t target_joint_configuration;
+};
+
+struct MoveLPrimitive : public MotionPrimitive
+{
+  MoveLPrimitive(const urcl::Pose& target, const double blend_radius = 0,
+                 const std::chrono::duration<double> duration = std::chrono::milliseconds(0),
+                 const double acceleration = 1.4, const double velocity = 1.04)
+  {
+    type = MotionType::MOVEL;
+    target_pose = target;
+    this->duration = duration;
+    this->acceleration = acceleration;
+    this->velocity = velocity;
+    this->blend_radius = blend_radius;
+  }
+
+  urcl::Pose target_pose;
+};
+}  // namespace control
+}  // namespace urcl
+#endif  // UR_CLIENT_LIBRARY_MOTION_PRIMITIVES_H_INCLUDED

include/ur_client_library/control/reverse_interface.h

@@ -141,13 +141,20 @@ class ReverseInterface
                "commands.")]] virtual void
   setKeepaliveCount(const uint32_t count);
 
+  void registerDisconnectionCallback(std::function<void(const int)> disconnection_fun)
+  {
+    disconnection_callback_ = disconnection_fun;
+  }
+
 protected:
   virtual void connectionCallback(const int filedescriptor);
 
   virtual void disconnectionCallback(const int filedescriptor);
 
   virtual void messageCallback(const int filedescriptor, char* buffer, int nbytesrecv);
 
+  std::function<void(const int)> disconnection_callback_ = nullptr;
+
   int client_fd_;
   comm::TCPServer server_;
 

include/ur_client_library/control/trajectory_point_interface.h

@@ -52,6 +52,8 @@ enum class TrajectoryResult : int32_t
   TRAJECTORY_RESULT_FAILURE = 2    ///< Aborted due to error during execution
 };
 
+std::string trajectoryResultToString(const TrajectoryResult result);
+
 /*!
  * Spline types
  */

include/ur_client_library/types.h

@@ -31,6 +31,23 @@ using vector6d_t = std::array<double, 6>;
 using vector6int32_t = std::array<int32_t, 6>;
 using vector6uint32_t = std::array<uint32_t, 6>;
 
+struct Pose
+{
+  Pose() : x(0.0), y(0.0), z(0.0), rx(0.0), ry(0.0), rz(0.0)
+  {
+  }
+  Pose(const double x, const double y, const double z, const double rx, const double ry, const double rz)
+    : x(x), y(y), z(z), rx(rx), ry(ry), rz(rz)
+  {
+  }
+  double x;
+  double y;
+  double z;
+  double rx;
+  double ry;
+  double rz;
+};
+
 template <class T, std::size_t N>
 std::ostream& operator<<(std::ostream& out, const std::array<T, N>& item)
 {
@@ -59,4 +76,4 @@ constexpr typename std::underlying_type<E>::type toUnderlying(const E e) noexcep
 {
   return static_cast<typename std::underlying_type<E>::type>(e);
 }
-}  // namespace urcl
+}  // namespace urcl