Fix traj point time precision (UniversalRobots#482)

Precision of trajectory point duration was using floor-rounding on
millisecond precision. Thus, e.g. `0.019998 sec` would be transferred as
`19 ms`.

This changes the following:

- Change time precision for time from milliseconds to microseconds. This
effectively makes the longest duration for a trajectory segment ~36
minutes, which should not be a problem in real-world applications.
If a segment longer than the maximum time is sent, this results in a clear error.
- Use nearest integer round instead of floor round if time with more
precision than 1 microsecond is given. e.g. 0.0199989 sec will be rounded
to 19999 us rather than 19998 us.
- Use Micrometer precision for the blend radius. Probably not that much
relevant, but it is using the same multiplier constant.

Backwards compatibility should be given, as long as users haven't
hardcoded the constant's value. The change on the wire protocol is
injected by the application through the TrajectoryInterface's constant, so even a
custom script code would get the
updated constant injected correctly decoding the changed wire data.

Author: urfeex

doc/architecture/trajectory_point_interface.rst

@@ -69,4 +69,9 @@ representations in 21 datafields. The data fields have the following meaning:
 where
 
 - ``MULT_JOINTSTATE``: 1000000
-- ``MULT_TIME``: 1000
+- ``MULT_TIME``: 1000000
+
+.. note::
+   With ``MULT_TIME`` being 1000000, the maximum duration that can be sent is 2147 seconds, while
+   precision is cut off at 1 microsecond. (The same applies to the blend radius, respectively being
+   max 2147 m and 1 μm precision.)

include/ur_client_library/control/trajectory_point_interface.h

@@ -63,7 +63,7 @@ std::string trajectoryResultToString(const TrajectoryResult result);
 class TrajectoryPointInterface : public ReverseInterface
 {
 public:
-  static const int32_t MULT_TIME = 1000;
+  static const int32_t MULT_TIME = 1000000;
   static const int MESSAGE_LENGTH = 21;
 
   TrajectoryPointInterface() = delete;
@@ -148,6 +148,8 @@ class TrajectoryPointInterface : public ReverseInterface
   virtual void messageCallback(const socket_t filedescriptor, char* buffer, int nbytesrecv) override;
 
 private:
+  const double MAX_GOAL_TIME_ = static_cast<double>(std::numeric_limits<int32_t>::max()) / MULT_TIME;
+
   std::list<HandlerFunction<void(TrajectoryResult)>> trajectory_end_callbacks_;
   uint32_t next_done_callback_id_ = 0;
 };

src/control/trajectory_point_interface.cpp

@@ -69,6 +69,12 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
     return false;
   }
 
+  if (primitive->duration.count() > MAX_GOAL_TIME_)
+  {
+    URCL_LOG_ERROR("Motion primitive duration is too long. Maximum allowed duration is %f seconds.", MAX_GOAL_TIME_);
+    return false;
+  }
+
   if (client_fd_ == -1)
   {
     return false;
@@ -215,12 +221,12 @@ bool TrajectoryPointInterface::writeTrajectoryPoint(const vector6d_t* positions,
     primitive = std::make_shared<MoveLPrimitive>(
         urcl::Pose{ (*positions)[0], (*positions)[1], (*positions)[2], (*positions)[3], (*positions)[4],
                     (*positions)[5] },
-        blend_radius, std::chrono::milliseconds(static_cast<int>(goal_time * 1000)), acceleration, velocity);
+        blend_radius, std::chrono::duration<double>(static_cast<double>(goal_time)), acceleration, velocity);
   }
   else
   {
     primitive = std::make_shared<MoveJPrimitive>(*positions, blend_radius,
-                                                 std::chrono::milliseconds(static_cast<int>(goal_time * 1000)),
+                                                 std::chrono::duration<double>(static_cast<double>(goal_time)),
                                                  acceleration, velocity);
   }
 
@@ -255,7 +261,7 @@ bool TrajectoryPointInterface::writeTrajectorySplinePoint(const vector6d_t* posi
   }
 
   return writeMotionPrimitive(std::make_shared<SplinePrimitive>(
-      *positions, *velocities, target_accelerations, std::chrono::milliseconds(static_cast<int>(goal_time * 1000))));
+      *positions, *velocities, target_accelerations, std::chrono::duration<double>(static_cast<double>(goal_time))));
 }
 
 void TrajectoryPointInterface::connectionCallback(const socket_t filedescriptor)

tests/test_trajectory_point_interface.cpp

@@ -34,6 +34,7 @@
 #include <ur_client_library/comm/tcp_socket.h>
 #include <ur_client_library/control/motion_primitives.h>
 #include <cmath>
+#include <limits>
 #include "ur_client_library/exceptions.h"
 
 using namespace urcl;
@@ -227,8 +228,8 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                 (double)spl.pos[4] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                 (double)spl.pos[5] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             },
-            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
-            std::chrono::milliseconds(spl.goal_time),
+            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE,
+            std::chrono::microseconds(spl.goal_time),
             (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
       }
@@ -241,8 +242,8 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                         ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
-            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
-            std::chrono::milliseconds(spl.goal_time),
+            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE,
+            std::chrono::microseconds(spl.goal_time),
             (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
       }
@@ -255,7 +256,7 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                         ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
-            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
+            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.vel[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE);
       }
@@ -274,7 +275,7 @@ class TrajectoryPointInterfaceTest : public ::testing::Test
                         ((double)spl.pos[3]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[4]) / control::TrajectoryPointInterface::MULT_JOINTSTATE,
                         ((double)spl.pos[5]) / control::TrajectoryPointInterface::MULT_JOINTSTATE },
-            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_TIME,
+            (double)spl.blend_radius_or_spline_type / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.acc[1] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             (double)spl.acc[0] / control::TrajectoryPointInterface::MULT_JOINTSTATE,
             static_cast<int32_t>(round((double)spl.acc[2] / control::TrajectoryPointInterface::MULT_JOINTSTATE)));
@@ -384,7 +385,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_quintic_joint_spline)
   EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE);
 
   // Goal time
-  EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_TIME));
+  EXPECT_EQ(send_goal_time, ((double)received_data.goal_time / traj_point_interface_->MULT_JOINTSTATE));
 
   // Spline type
   EXPECT_EQ(static_cast<int32_t>(control::TrajectorySplineType::SPLINE_QUINTIC),
@@ -428,7 +429,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_cubic_joint_spline)
   EXPECT_EQ(send_acc[5], ((double)received_data.acc[5]) / traj_point_interface_->MULT_JOINTSTATE);
 
   // Goal time
-  EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_TIME);
+  EXPECT_EQ(send_goal_time, ((double)received_data.goal_time) / traj_point_interface_->MULT_JOINTSTATE);
 
   // Spline type
   EXPECT_EQ(static_cast<int32_t>(control::TrajectorySplineType::SPLINE_CUBIC),
@@ -479,7 +480,62 @@ TEST_F(TrajectoryPointInterfaceTest, write_goal_time)
   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);
+  EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE);
+}
+
+// Wire format: int32 microseconds (MULT_TIME). Duration must reach writeMotionPrimitive as seconds
+// in double form so encoding uses round(seconds * MULT_TIME), not trunc(int(ms)).
+TEST_F(TrajectoryPointInterfaceTest, write_goal_time_preserves_submillisecond_precision)
+{
+  urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
+  const float send_goal_time = 0.5006f;
+  const int32_t expected_encoded = static_cast<int32_t>(
+      std::round(static_cast<double>(send_goal_time) * static_cast<double>(traj_point_interface_->MULT_TIME)));
+
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, send_goal_time, 0, false);
+  EXPECT_EQ(expected_encoded, client_->getGoalTime());
+
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 1.4f, 1.05f, send_goal_time, 0, false);
+  EXPECT_EQ(expected_encoded, client_->getGoalTime());
+
+  // High-precision duration: must match round(goal_time * MULT_TIME), not trunc(goal_time * MULT_TIME).
+  const float precise_goal_time = 1.234567f;
+  const int32_t expected_precise = static_cast<int32_t>(
+      std::round(static_cast<double>(precise_goal_time) * static_cast<double>(traj_point_interface_->MULT_TIME)));
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, precise_goal_time, 0, false);
+  EXPECT_EQ(expected_precise, client_->getGoalTime());
+  traj_point_interface_->writeTrajectoryPoint(&send_positions, 1.4f, 1.05f, precise_goal_time, 0, false);
+  EXPECT_EQ(expected_precise, client_->getGoalTime());
+
+  urcl::vector6d_t send_vel = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  urcl::vector6d_t send_acc = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  traj_point_interface_->writeTrajectorySplinePoint(&send_positions, &send_vel, &send_acc, send_goal_time);
+  EXPECT_EQ(expected_encoded, client_->getGoalTime());
+  traj_point_interface_->writeTrajectorySplinePoint(&send_positions, &send_vel, &send_acc, precise_goal_time);
+  EXPECT_EQ(expected_precise, client_->getGoalTime());
+}
+
+// Segment duration is capped by int32 microseconds on the wire (~35.79 minutes).
+TEST_F(TrajectoryPointInterfaceTest, write_rejects_goal_time_above_max_encodable_duration)
+{
+  const double max_goal_time_seconds = static_cast<double>(std::numeric_limits<int32_t>::max()) /
+                                       static_cast<double>(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  const float too_long_goal_time = static_cast<float>(max_goal_time_seconds + 1.0);
+  const float almost_too_long_goal_time = static_cast<float>(max_goal_time_seconds - 1.0);
+
+  urcl::vector6d_t send_positions = { 0, 0, 0, 0, 0, 0 };
+  EXPECT_FALSE(traj_point_interface_->writeTrajectoryPoint(&send_positions, too_long_goal_time, 0, false));
+  EXPECT_FALSE(traj_point_interface_->writeTrajectoryPoint(&send_positions, 1.4f, 1.05f, too_long_goal_time, 0, false));
+  EXPECT_TRUE(traj_point_interface_->writeTrajectoryPoint(&send_positions, almost_too_long_goal_time, 0, false));
+  EXPECT_TRUE(
+      traj_point_interface_->writeTrajectoryPoint(&send_positions, 1.4f, 1.05f, almost_too_long_goal_time, 0, false));
+
+  urcl::vector6d_t send_vel = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  urcl::vector6d_t send_acc = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
+  EXPECT_FALSE(
+      traj_point_interface_->writeTrajectorySplinePoint(&send_positions, &send_vel, &send_acc, too_long_goal_time));
+  EXPECT_TRUE(traj_point_interface_->writeTrajectorySplinePoint(&send_positions, &send_vel, &send_acc,
+                                                                almost_too_long_goal_time));
 }
 
 TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity)
@@ -500,7 +556,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_acceleration_velocity)
 
   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);
+  EXPECT_EQ(send_goal_time, ((float)received_goal_time) / traj_point_interface_->MULT_JOINTSTATE);
 }
 
 TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
@@ -510,7 +566,7 @@ TEST_F(TrajectoryPointInterfaceTest, write_blend_radius)
   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);
+  EXPECT_EQ(send_blend_radius, ((float)received_blend_radius) / traj_point_interface_->MULT_JOINTSTATE);
 }
 
 TEST_F(TrajectoryPointInterfaceTest, write_cartesian)