Add acceleration and velocity information to writeTrajectoryPoint

Author: urfeex

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)
 {

tests/test_trajectory_point_interface.cpp

@@ -63,16 +63,15 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                      int32_t& blend_radius_or_spline_type, int32_t& motion_type)
     {
       // Read message
-      uint8_t buf[sizeof(int32_t) * 21];
+      uint8_t buf[sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH];
       uint8_t* b_pos = buf;
       size_t read = 0;
-      size_t remainder = sizeof(int32_t) * 21;
+      size_t remainder = sizeof(int32_t) * urcl::control::TrajectoryPointInterface::MESSAGE_LENGTH;
       while (remainder > 0)
       {
         if (!TCPSocket::read(b_pos, remainder, read))
         {
-          std::cout << "Failed to read from socket, this should not happen during a test!" << std::endl;
-          break;
+          throw(std::runtime_error("Failed to read from socket, this should not happen during a test!"));
         }
         b_pos += read;
         remainder -= read;
@@ -135,6 +134,14 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
       return vel;
     }
 
+    vector6int32_t getAcceleration()
+    {
+      int32_t goal_time, blend_radius_or_spline_type, motion_type;
+      vector6int32_t pos, vel, acc;
+      readMessage(pos, vel, acc, goal_time, blend_radius_or_spline_type, motion_type);
+      return acc;
+    }
+
     int32_t getGoalTime()
     {
       int32_t goal_time, blend_radius_or_spline_type, motion_type;
@@ -218,13 +225,13 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
 private:
   std::condition_variable trajectory_end_;
   std::mutex trajectory_end_mutex_;
-  control::TrajectoryResult result_;
+  control::TrajectoryResult result_ = control::TrajectoryResult::TRAJECTORY_RESULT_UNKNOWN;
 };
 
 TEST_F(TrajectoryPointInterfaceTest, write_postions)
 {
   urcl::vector6d_t send_positions = { 1.2, 3.1, 2.2, -3.4, -1.1, -1.2 };
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, 0, false);
   vector6int32_t received_positions = client_->getPosition();
 
   EXPECT_EQ(send_positions[0], ((double)received_positions[0]) / traj_point_interface_->MULT_JOINTSTATE);
@@ -361,17 +368,38 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time)
 {
   urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
   float send_goal_time = 0.5;
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false);
+  int32_t received_goal_time = client_->getGoalTime();
+
+  EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME);
+}
+
+TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity)
+{
+  urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
+  float send_move_acceleration = 0.123;
+  float send_move_velocity = 0.456;
+  float send_goal_time = 0.5;
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
+  int32_t received_move_acceleration = client_->getAcceleration()[0];
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
+  int32_t received_move_velocity = client_->getVelocity()[0];
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_move_acceleration, send_move_velocity,
+                                              send_goal_time, 0, 0);
   int32_t received_goal_time = client_->getGoalTime();
 
+  EXPECT_EQ(send_move_acceleration, ((float)received_move_acceleration) / traj_point_interface_->MULT_JOINTSTATE);
+  EXPECT_EQ(send_move_velocity, ((float)received_move_velocity) / traj_point_interface_->MULT_JOINTSTATE);
   EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_TIME);
 }
 
 TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
 {
   urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
   float send_blend_radius = 0.5;
-  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, 0);
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 0, send_blend_radius, false);
   int32_t received_blend_radius = client_->getBlendRadius();
 
   EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_TIME);