Trajectory point velocities and example (#241)

This PR adds the possibility to pass acceleration&velocity information
instead of time constraints to direct robot motions.

Author: urfeex

examples/CMakeLists.txt

@@ -59,3 +59,8 @@ add_executable(script_sender_example
 script_sender.cpp)
 target_compile_options(script_sender_example PUBLIC ${CXX17_FLAG})
 target_link_libraries(script_sender_example ur_client_library::urcl)
+
+add_executable(trajectory_point_interface_example
+trajectory_point_interface.cpp)
+target_compile_options(trajectory_point_interface_example PUBLIC ${CXX17_FLAG})
+target_link_libraries(trajectory_point_interface_example ur_client_library::urcl)

examples/trajectory_point_interface.cpp

@@ -0,0 +1,213 @@
+// -- 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"
+
+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::unique_ptr<urcl::UrDriver> g_my_driver;
+
+std::atomic<bool> g_trajectory_done = false;
+
+// 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;
+}
+
+void trajDoneCallback(const urcl::control::TrajectoryResult& result)
+{
+  URCL_LOG_INFO("Trajectory done with result %d", result);
+  ;
+  g_trajectory_done = true;
+}
+
+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));
+  // --------------- INITIALIZATION END -------------------
+
+  g_my_driver->registerTrajectoryDoneCallback(&trajDoneCallback);
+
+  URCL_LOG_INFO("Running MoveJ motion");
+  // --------------- MOVEJ TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> points{ { -1.57, -1.57, 0, 0, 0, 0 }, { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+    std::vector<double> motion_durations{ 5.0, 2.0 };
+    std::vector<double> blend_radii{ 0.1, 0.1 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               points.size() * 2);
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      g_my_driver->writeTrajectoryPoint(points[i], false, motion_durations[i], blend_radii[i]);
+    }
+
+    // Same motion, but parametrized with acceleration and velocity
+    motion_durations = { 0.0, 0.0 };
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      g_my_driver->writeTrajectoryPoint(points[i], 2.0, 2.0, false, motion_durations[i], blend_radii[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEJ TRAJECTORY -------------------
+
+  URCL_LOG_INFO("Running MoveL motion");
+  // --------------- MOVEL TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> points{ { -0.203, 0.263, 0.559, 0.68, -1.083, -2.076 },
+                                          { -0.203, 0.463, 0.559, 0.68, -1.083, -2.076 } };
+    std::vector<double> motion_durations{ 5.0, 5.0 };
+    std::vector<double> blend_radii{ 0.0, 0.0 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               points.size());
+    for (size_t i = 0; i < points.size(); i++)
+    {
+      // setting the cartesian parameter makes it interpret the 6d vector as a pose and use movel
+      g_my_driver->writeTrajectoryPoint(points[i], true, motion_durations[i], blend_radii[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEL TRAJECTORY -------------------
+
+  URCL_LOG_INFO("Running a spline motion");
+  // --------------- SPLINE TRAJECTORY -------------------
+  {
+    g_trajectory_done = false;
+    // Trajectory definition
+    std::vector<urcl::vector6d_t> positions{ { -1.57, -1.57, 0, 0, 0, 0 },
+                                             { -1.57, -1.57, -1.57, 0, 0, 0 },
+                                             { -1.57, -1.57, 0, 0, 0, 0 },
+                                             { -1.57, -1.6, 1.6, -0.7, 0.7, 0.2 } };
+    std::vector<urcl::vector6d_t> velocities{
+      { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 0.0, 0, 0, 0 }, { 0, 0, 1.5, 0, 0, 0 }, { 0, 0, 0, 0, 0, 0 }
+    };
+    std::vector<double> motion_durations{ 3.0, 3.0, 3.0, 3.0 };
+
+    // Trajectory execution
+    g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START,
+                                               positions.size());
+    for (size_t i = 0; i < positions.size(); i++)
+    {
+      g_my_driver->writeTrajectorySplinePoint(positions[i], velocities[i], motion_durations[i]);
+    }
+
+    while (!g_trajectory_done)
+    {
+      std::this_thread::sleep_for(std::chrono::milliseconds(100));
+      g_my_driver->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP);
+    }
+  }
+  // --------------- END MOVEJ TRAJECTORY -------------------
+
+  g_my_driver->stopControl();
+  return 0;
+}

include/ur_client_library/control/trajectory_point_interface.h

@@ -29,6 +29,8 @@
 #ifndef UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 #define UR_CLIENT_LIBRARY_TRAJECTORY_INTERFACE_H_INCLUDED
 
+#include <optional>
+
 #include "ur_client_library/control/reverse_interface.h"
 #include "ur_client_library/comm/control_mode.h"
 #include "ur_client_library/types.h"
@@ -44,6 +46,7 @@ namespace control
 enum class TrajectoryResult : int32_t
 {
 
+  TRAJECTORY_RESULT_UNKNOWN = -1,  ///< No result received, yet
   TRAJECTORY_RESULT_SUCCESS = 0,   ///< Successful execution
   TRAJECTORY_RESULT_CANCELED = 1,  ///< Canceled by user
   TRAJECTORY_RESULT_FAILURE = 2    ///< Aborted due to error during execution
@@ -76,6 +79,7 @@ class TrajectoryPointInterface : public ReverseInterface
 {
 public:
   static const int32_t MULT_TIME = 1000;
+  static const int MESSAGE_LENGTH = 21;
 
   TrajectoryPointInterface() = delete;
   /*!
@@ -103,6 +107,23 @@ class TrajectoryPointInterface : public ReverseInterface
   bool writeTrajectoryPoint(const vector6d_t* positions, const float goal_time, const float blend_radius,
                             const bool cartesian);
 
+  /*!
+   * \brief Writes information for a robot motion to the robot to be read by the URScript program.
+   *
+   * \param positions A vector of joint or cartesian targets for the robot
+   * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2]
+   * for Cartesian motions
+   * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions
+   * \param goal_time The goal time to reach the target. When a non-zero goal time is specified,
+   * this has priority over speed and acceleration settings.
+   * \param blend_radius The radius to be used for blending between control points
+   * \param cartesian True, if the written point is specified in Cartesian space, false if in joint space
+   *
+   * \returns True, if the write was performed successfully, false otherwise.
+   */
+  bool writeTrajectoryPoint(const vector6d_t* positions, const float acceleration = 1.4, const float velocity = 1.05,
+                            const float goal_time = 0, const float blend_radius = 0, const bool cartesian = false);
+
   /*!
    * \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
@@ -131,7 +152,6 @@ 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_;
 };
 

include/ur_client_library/ur/ur_driver.h

@@ -272,6 +272,23 @@ class UrDriver
   bool writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time = 0.0,
                             const float blend_radius = 0.052);
 
+  /*!
+   * \brief Writes a trajectory point onto the dedicated socket.
+   *
+   * \param positions Desired joint or cartesian positions
+   * \param cartesian True, if the point sent is cartesian, false if joint-based
+   * \param acceleration Joint acceleration of leading axis [rad/s^2] / tool acceleration [m/s^2]
+   * for Cartesian motions
+   * \param velocity Joint speed of leading axis [rad/s] / tool speed [m/s] for Cartesian motions
+   * \param goal_time Time for the robot to reach this point. When a non-zero goal time is specified,
+   * this has priority over speed and acceleration settings.
+   * \param blend_radius  The radius to be used for blending between control points
+   *
+   * \returns True on successful write.
+   */
+  bool writeTrajectoryPoint(const vector6d_t& positions, float acceleration, float velocity, const bool cartesian,
+                            const float goal_time = 0.0, const float blend_radius = 0.052);
+
   /*!
    * \brief Writes a trajectory spline point for quintic spline interpolation onto the dedicated socket.
    *

resources/external_control.urscript

@@ -479,15 +479,19 @@ thread trajectoryThread():
       end
       # MoveJ point
       if raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_JOINT:
-        movej(q, t = tmptime, r = blend_radius)
+        acceleration = raw_point[7] / MULT_jointstate
+        velocity = raw_point[13] / MULT_jointstate
+        movej(q, a = acceleration, v = velocity, t = tmptime, r = blend_radius)
 
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]
         spline_qd = [0, 0, 0, 0, 0, 0]
 
         # Movel point
       elif raw_point[INDEX_POINT_TYPE] == TRAJECTORY_POINT_CARTESIAN:
-        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], t = tmptime, r = blend_radius)
+        acceleration = raw_point[7] / MULT_jointstate
+        velocity = raw_point[13] / MULT_jointstate
+        movel(p[q[0], q[1], q[2], q[3], q[4], q[5]], a = acceleration, v = velocity, t = tmptime, r = blend_radius)
 
         # reset old acceleration
         spline_qdd = [0, 0, 0, 0, 0, 0]

src/control/trajectory_point_interface.cpp

@@ -38,7 +38,8 @@ TrajectoryPointInterface::TrajectoryPointInterface(uint32_t port) : ReverseInter
 {
 }
 
-bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time,
+bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float acceleration,
+                                                    const float velocity, const float goal_time,
                                                     const float blend_radius, const bool cartesian)
 {
   if (client_fd_ == -1)
@@ -56,16 +57,24 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
       val = htobe32(val);
       b_pos += append(b_pos, val);
     }
+    for (size_t i = 0; i < positions->size(); ++i)
+    {
+      int32_t val = static_cast<int32_t>(round(velocity * MULT_JOINTSTATE));
+      val = htobe32(val);
+      b_pos += append(b_pos, val);
+    }
+    for (size_t i = 0; i < positions->size(); ++i)
+    {
+      int32_t val = static_cast<int32_t>(round(acceleration * MULT_JOINTSTATE));
+      val = htobe32(val);
+      b_pos += append(b_pos, val);
+    }
   }
   else
   {
     b_pos += 6 * sizeof(int32_t);
   }
 
-  // Fill in velocity and acceleration, not used for this point type
-  b_pos += 6 * sizeof(int32_t);
-  b_pos += 6 * sizeof(int32_t);
-
   int32_t val = static_cast<int32_t>(round(goal_time * MULT_TIME));
   val = htobe32(val);
   b_pos += append(b_pos, val);
@@ -91,6 +100,12 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
   return server_.write(client_fd_, buffer, sizeof(buffer), written);
 }
 
+bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions, const float goal_time,
+                                                    const float blend_radius, const bool cartesian)
+{
+  return writeTrajectoryPoint(positions, 1.4, 1.05, goal_time, blend_radius, cartesian);
+}
+
 bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* positions, const vector6d_t* velocities,
                                                           const vector6d_t* accelerations, const float goal_time)
 {

src/ur/ur_driver.cpp

@@ -232,6 +232,13 @@ bool UrDriver::writeJointCommand(const vector6d_t& values, const comm::ControlMo
   return reverse_interface_->write(&values, control_mode, robot_receive_timeout);
 }
 
+bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const float acceleration, const float velocity,
+                                    const bool cartesian, const float goal_time, const float blend_radius)
+{
+  return trajectory_interface_->writeTrajectoryPoint(&positions, acceleration, velocity, goal_time, blend_radius,
+                                                     cartesian);
+}
+
 bool UrDriver::writeTrajectoryPoint(const vector6d_t& positions, const bool cartesian, const float goal_time,
                                     const float blend_radius)
 {