Fix spline point velocity and acceleration precision

doc/architecture/trajectory_point_interface.rst

@@ -57,11 +57,11 @@ representations in 21 datafields. The data fields have the following meaning:
           - For all MOVEC variants this field contains the via point (same
             joint-vs-pose interpretation as the target at indices 0-5, see the motion type at
             index 20).
-          - trajectory point velocities (multiplied by ``MULT_JOINTSTATE``) for spline joint types
+          - trajectory point velocities (multiplied by ``MULT_VEL_ACC``) for spline joint types
 
    12-17  Depending on the motion type, this represents either
 
-          - trajectory point accelerations (multiplied by ``MULT_JOINTSTATE``) for spline joint
+          - trajectory point accelerations (multiplied by ``MULT_VEL_ACC``) for spline joint
             types.
 
           - for all other motion types
@@ -112,12 +112,19 @@ where
 
 - ``MULT_JOINTSTATE``: 1000000
 - ``MULT_TIME``: 1000000
+- ``MULT_VEL_ACC``: 100000000
 
 .. 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.)
 
+.. note::
+   Spline point velocities and accelerations use the finer ``MULT_VEL_ACC`` scaling, giving 1e-8
+   resolution with a maximum magnitude of ~21.47 rad/s (rad/s²). The coarser ``MULT_JOINTSTATE``
+   scaling quantizes near-zero accelerations, causing reconstructed acceleration
+   profile to become jagged, which can trigger controller faults. Values exceeding the maximum magnitude are rejected by the library.
+
 .. note::
    The ``*_POSE`` / ``*_JOINT`` motion-type variants let callers mix joint-space and Cartesian
    targets freely through the high-level APIs (see :ref:`instruction_executor` and the

include/ur_client_library/control/trajectory_point_interface.h

@@ -63,6 +63,8 @@ std::string trajectoryResultToString(const TrajectoryResult result);
 class TrajectoryPointInterface : public ReverseInterface
 {
 public:
+  // Spline vel/acc need finer resolution: quantising near-zero values at 1e-6 causes controller faults.
+  static const int32_t MULT_VEL_ACC = 100000000;
   static const int MESSAGE_LENGTH = 21;
 
   TrajectoryPointInterface() = delete;
@@ -148,6 +150,7 @@ class TrajectoryPointInterface : public ReverseInterface
 
 private:
   const double MAX_GOAL_TIME_ = static_cast<double>(std::numeric_limits<int32_t>::max()) / MULT_TIME;
+  const double MAX_VEL_ACC_ = static_cast<double>(std::numeric_limits<int32_t>::max()) / MULT_VEL_ACC;
 
   std::list<HandlerFunction<void(TrajectoryResult)>> trajectory_end_callbacks_;
   uint32_t next_done_callback_id_ = 0;

resources/external_control.urscript

@@ -7,6 +7,7 @@ steptime = get_steptime()
 textmsg("ExternalControl: steptime=", steptime)
 MULT_jointstate = {{JOINT_STATE_REPLACE}}
 MULT_time = {{TIME_REPLACE}}
+MULT_velacc = {{VEL_ACC_REPLACE}}
 
 DEBUG = False
 
@@ -668,16 +669,16 @@ thread trajectoryThread():
 
         # Cubic spline
         if raw_point[INDEX_SPLINE_TYPE] == SPLINE_CUBIC:
-          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
+          qd = [ raw_point[7] / MULT_velacc, raw_point[8] / MULT_velacc, raw_point[9] / MULT_velacc, raw_point[10] / MULT_velacc, raw_point[11] / MULT_velacc, raw_point[12] / MULT_velacc]
           is_robot_moving = cubicSplineRun(q, qd, tmptime, is_last_point, is_first_point)
 
           # reset old acceleration
           spline_qdd = [0, 0, 0, 0, 0, 0]
 
           # Quintic spline
         elif raw_point[INDEX_SPLINE_TYPE] == SPLINE_QUINTIC:
-          qd = [ raw_point[7] / MULT_jointstate, raw_point[8] / MULT_jointstate, raw_point[9] / MULT_jointstate, raw_point[10] / MULT_jointstate, raw_point[11] / MULT_jointstate, raw_point[12] / MULT_jointstate]
-          qdd = [ raw_point[13]/ MULT_jointstate, raw_point[14]/ MULT_jointstate, raw_point[15]/ MULT_jointstate, raw_point[16]/ MULT_jointstate, raw_point[17]/ MULT_jointstate, raw_point[18]/ MULT_jointstate]
+          qd = [ raw_point[7] / MULT_velacc, raw_point[8] / MULT_velacc, raw_point[9] / MULT_velacc, raw_point[10] / MULT_velacc, raw_point[11] / MULT_velacc, raw_point[12] / MULT_velacc]
+          qdd = [ raw_point[13]/ MULT_velacc, raw_point[14]/ MULT_velacc, raw_point[15]/ MULT_velacc, raw_point[16]/ MULT_velacc, raw_point[17]/ MULT_velacc, raw_point[18]/ MULT_velacc]
           is_robot_moving = quinticSplineRun(q, qd, qdd, tmptime, is_last_point, is_first_point)
         else:
           textmsg("Unknown spline type given:", raw_point[INDEX_POINT_TYPE])

src/control/trajectory_point_interface.cpp

@@ -29,6 +29,8 @@
 #include <ur_client_library/control/trajectory_point_interface.h>
 #include <ur_client_library/exceptions.h>
 #include <urcl_3rdparty/portable_endian.h>
+#include <algorithm>
+#include <cmath>
 #include <math.h>
 #include <cstdint>
 #include <stdexcept>
@@ -97,6 +99,23 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
     return false;
   }
 
+  if (primitive->type == MotionType::SPLINE)
+  {
+    auto spline_primitive = std::static_pointer_cast<control::SplinePrimitive>(primitive);
+    auto within_range = [this](const vector6d_t& values) {
+      return std::all_of(values.begin(), values.end(),
+                         [this](const double value) { return std::abs(value) <= MAX_VEL_ACC_; });
+    };
+    if (!within_range(spline_primitive->target_velocities) ||
+        (spline_primitive->target_accelerations.has_value() &&
+         !within_range(spline_primitive->target_accelerations.value())))
+    {
+      URCL_LOG_ERROR("Spline point velocity or acceleration out of range. Maximum allowed magnitude is %f.",
+                     MAX_VEL_ACC_);
+      return false;
+    }
+  }
+
   if (client_fd_ == -1)
   {
     return false;
@@ -180,24 +199,18 @@ bool TrajectoryPointInterface::writeMotionPrimitive(const std::shared_ptr<contro
   }
 
   size_t index = 0;
-  for (auto const& pos : first_block)
-  {
-    int32_t val = static_cast<int32_t>(round(pos * MULT_JOINTSTATE));
-    buffer[index] = htobe32(val);
-    index++;
-  }
-  for (auto const& item : second_block)
-  {
-    int32_t val = static_cast<int32_t>(round(item * MULT_JOINTSTATE));
-    buffer[index] = htobe32(val);
-    index++;
-  }
-  for (auto const& item : third_block)
-  {
-    int32_t val = static_cast<int32_t>(round(item * MULT_JOINTSTATE));
-    buffer[index] = htobe32(val);
-    index++;
-  }
+  auto write_block = [&buffer, &index](const vector6d_t& block, const int32_t mult) {
+    for (auto const& item : block)
+    {
+      buffer[index++] = htobe32(static_cast<int32_t>(round(item * mult)));
+    }
+  };
+
+  // Only spline points carry per-sample vel/acc in the second and third block.
+  const int32_t vel_acc_mult = primitive->type == MotionType::SPLINE ? MULT_VEL_ACC : MULT_JOINTSTATE;
+  write_block(first_block, MULT_JOINTSTATE);
+  write_block(second_block, vel_acc_mult);
+  write_block(third_block, vel_acc_mult);
 
   int32_t val = static_cast<int32_t>(round(primitive->duration.count() * MULT_TIME));
   buffer[index] = htobe32(val);

src/ur/ur_driver.cpp

@@ -49,6 +49,7 @@ namespace urcl
 static const std::string BEGIN_REPLACE("BEGIN_REPLACE");
 static const std::string JOINT_STATE_REPLACE("JOINT_STATE_REPLACE");
 static const std::string TIME_REPLACE("TIME_REPLACE");
+static const std::string VEL_ACC_REPLACE("VEL_ACC_REPLACE");
 static const std::string SERVO_J_REPLACE("SERVO_J_REPLACE");
 static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE");
 static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE");
@@ -111,6 +112,7 @@ void UrDriver::init(const UrDriverConfiguration& config)
   control::ScriptReader::DataDict data;
   data[JOINT_STATE_REPLACE] = std::to_string(control::ReverseInterface::MULT_JOINTSTATE);
   data[TIME_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_TIME);
+  data[VEL_ACC_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_VEL_ACC);
   std::ostringstream out;
   out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
   data[SERVO_J_REPLACE] = out.str();

tests/test_script_reader.cpp

@@ -460,6 +460,7 @@ TEST_F(ScriptReaderTest, TestParsingExternalControl)
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";
@@ -500,6 +501,7 @@ TEST_F(ScriptReaderTest, TestDirectTorquePopupOnOldVersion)
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";
@@ -527,6 +529,7 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope523
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";
@@ -557,6 +560,7 @@ TEST_F(ScriptReaderTest, TestFrictionCompensationConstantsAndHandlerPolyScope101
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";
@@ -588,6 +592,7 @@ TEST_F(ScriptReaderTest, TestFrictionScalesConstantsAndHandler)
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";
@@ -861,6 +866,7 @@ TEST_F(ScriptReaderTest, TestProduceAllScriptFiles)
   data["BEGIN_REPLACE"] = "";
   data["JOINT_STATE_REPLACE"] = std::to_string(urcl::control::ReverseInterface::MULT_JOINTSTATE);
   data["TIME_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_TIME);
+  data["VEL_ACC_REPLACE"] = std::to_string(urcl::control::TrajectoryPointInterface::MULT_VEL_ACC);
   data["SERVO_J_REPLACE"] = "lookahead_time=0.03, gain=2000";
   data["SERVER_IP_REPLACE"] = "1.2.3.4";
   data["SERVER_PORT_REPLACE"] = "50001";