feat(control): unify joint limits and bound trajectory dynamics

Use shared limits for all seven axes across MoveIt, URDF and FRI.
Apply Ruckig smoothing and continuous JTC spline checks, stretching
trajectory timing to respect velocity, acceleration and jerk limits.
Require fresh stationary feedback for direct trajectories and protect
preparation from concurrent submissions and stop requests.

Add regression tests, dependencies and bilingual FRI documentation.
Include the updated LIN speed-sweep scenario and current 60 ms filter
configuration. Acceleration and jerk limits remain project settings;
physical high-speed twitching is still under investigation.

src/iiwa_planning/CMakeLists.txt

@@ -16,6 +16,50 @@ find_package(moveit_ros_planning REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(iiwa_msgs REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
+find_package(pybind11 REQUIRED)
+pybind11_add_module(_iiwa_spline_limits src/spline_limits_py.cpp)
+target_compile_features(_iiwa_spline_limits PRIVATE cxx_std_17)
+target_include_directories(_iiwa_spline_limits PRIVATE include)
+ament_get_python_install_dir(PYTHON_INSTALL_DIR)
+install(TARGETS _iiwa_spline_limits LIBRARY DESTINATION ${PYTHON_INSTALL_DIR})
+
+add_library(iiwa_spline_limits SHARED src/limit_spline_dynamics.cpp)
+target_compile_features(iiwa_spline_limits PUBLIC cxx_std_17)
+target_include_directories(iiwa_spline_limits PRIVATE include)
+ament_target_dependencies(iiwa_spline_limits rclcpp moveit_core moveit_ros_planning pluginlib sensor_msgs)
+pluginlib_export_plugin_description_file(moveit_core planning_response_plugins.xml)
+install(TARGETS iiwa_spline_limits LIBRARY DESTINATION lib)
+
+if(BUILD_TESTING)
+  enable_testing()
+  add_executable(test_spline_limits test/test_spline_limits.cpp)
+  target_compile_features(test_spline_limits PRIVATE cxx_std_17)
+  target_compile_options(test_spline_limits PRIVATE -UNDEBUG)
+  target_include_directories(test_spline_limits PRIVATE include)
+  add_test(NAME spline_limits COMMAND test_spline_limits)
+  find_package(joint_trajectory_controller REQUIRED)
+  add_executable(test_jtc_interpolation test/test_jtc_interpolation.cpp)
+  target_compile_features(test_jtc_interpolation PRIVATE cxx_std_17)
+  target_compile_options(test_jtc_interpolation PRIVATE -UNDEBUG)
+  target_include_directories(test_jtc_interpolation PRIVATE include)
+  ament_target_dependencies(test_jtc_interpolation joint_trajectory_controller)
+  add_test(NAME jtc_interpolation COMMAND test_jtc_interpolation)
+  add_executable(test_response_adapter test/test_response_adapter.cpp)
+  target_compile_options(test_response_adapter PRIVATE -UNDEBUG)
+  ament_target_dependencies(test_response_adapter moveit_core moveit_ros_planning pluginlib)
+  add_test(NAME response_adapter COMMAND test_response_adapter)
+  add_executable(test_start_state_monitor test/test_start_state_monitor.cpp)
+  target_compile_options(test_start_state_monitor PRIVATE -UNDEBUG)
+  target_include_directories(test_start_state_monitor PRIVATE include)
+  ament_target_dependencies(test_start_state_monitor rclcpp sensor_msgs)
+  add_test(NAME start_state_monitor COMMAND test_start_state_monitor)
+  set_tests_properties(start_state_monitor PROPERTIES ENVIRONMENT "ROS_DOMAIN_ID=231;ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST" TIMEOUT 15)
+  add_test(NAME direct_trajectory COMMAND ${Python3_EXECUTABLE} -m pytest -q ${CMAKE_CURRENT_SOURCE_DIR}/test/test_prepare_trajectory.py)
+  set_tests_properties(direct_trajectory PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:$ENV{PYTHONPATH}")
+endif()
 
 add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
 target_include_directories(motion_planning_cpp PUBLIC include)

src/iiwa_planning/iiwa_planning/direct_trajectory.py

@@ -0,0 +1,26 @@
+"""Prepare position-only waypoints for the JTC quintic interpolator, off the RT thread."""
+import numpy as np
+from scipy.interpolate import make_interp_spline
+from _iiwa_spline_limits import retime
+
+
+def prepare_trajectory(times, positions, limits):
+    """Preserve waypoints, fit a C2+ rest-to-rest curve, bound its complete interpolation.
+
+    `times` starts at zero. Limits contain [lower, upper, velocity, acceleration, jerk].
+    Returns (times, positions, velocities, accelerations, uniform_time_scale).
+    Timing may increase; intermediate path can differ from position-only linear interpolation.
+    No collision checking is performed by this direct-command API.
+    """
+    t, q = np.asarray(times, dtype=float), np.asarray(positions, dtype=float)
+    if t.ndim != 1 or q.ndim != 2 or len(t) < 2 or len(t) != len(q):
+        raise ValueError('At least two complete trajectory points are required')
+    if not np.isfinite(t).all() or not np.isfinite(q).all() or t[0] != 0 or np.any(np.diff(t) < 1e-6):
+        raise ValueError('Finite waypoints and strictly increasing times starting at zero are required')
+    if q.shape[1] != len(limits):
+        raise ValueError('Joint count does not match limits')
+    zero = np.zeros(q.shape[1])
+    # With two derivative constraints per endpoint, knots coincide with all input times.
+    # Thus JTC quintics reproduce these spline segments from their endpoint p/v/a.
+    spline = make_interp_spline(t, q, k=5, bc_type=([(1, zero), (2, zero)], [(1, zero), (2, zero)]))
+    return retime(t.tolist(), q.tolist(), spline(t, 1).tolist(), spline(t, 2).tolist(), limits)

src/iiwa_planning/include/iiwa_planning/spline_limits.hpp

@@ -0,0 +1,147 @@
+#pragma once
+#include <algorithm>
+#include <array>
+#include <cmath>
+#include <stdexcept>
+#include <vector>
+
+namespace iiwa_planning {
+struct Limit {
+  double lower, upper, velocity, acceleration, jerk;
+};
+struct Sample {
+  double time;
+  std::vector<double> position, velocity, acceleration;
+};
+// Normalized time u=t/T; exactly the quintic used for JTC
+// position/velocity/acceleration points.
+inline std::vector<double> quintic(double p0, double p1, double v0, double v1,
+                                   double a0, double a1, double t) {
+  const double d = p1 - p0, v = v0 * t, w = v1 * t, a = a0 * t * t,
+               b = a1 * t * t;
+  return {p0,
+          v,
+          a / 2,
+          10 * d - 6 * v - 4 * w - 1.5 * a + .5 * b,
+          -15 * d + 8 * v + 7 * w + 1.5 * a - b,
+          6 * d - 3 * v - 3 * w - .5 * a + .5 * b};
+}
+inline double choose(unsigned n, unsigned k) {
+  double x = 1;
+  for (unsigned i = 1; i <= k; ++i)
+    x = x * (n + 1 - i) / i;
+  return x;
+}
+// Convex hull bound in Bernstein basis, tightened by de Casteljau subdivision.
+// Unlike sampling, it also bounds extrema between evaluation points.
+inline std::pair<double, double> hull(const std::vector<double> &b,
+                                      unsigned depth) {
+  if (!depth)
+    return {*std::min_element(b.begin(), b.end()),
+            *std::max_element(b.begin(), b.end())};
+  auto v = b;
+  std::vector<double> left(b.size()), right(b.size());
+  left[0] = v[0];
+  right.back() = v.back();
+  for (size_t k = 1; k < b.size(); ++k) {
+    for (size_t i = 0; i < b.size() - k; ++i)
+      v[i] = (v[i] + v[i + 1]) / 2;
+    left[k] = v[0];
+    right[b.size() - 1 - k] = v[b.size() - 1 - k];
+  }
+  const auto l = hull(left, depth - 1), r = hull(right, depth - 1);
+  return {std::min(l.first, r.first), std::max(l.second, r.second)};
+}
+inline std::pair<double, double> bounds(const std::vector<double> &power) {
+  std::vector<double> b(power.size(), 0);
+  const unsigned n = power.size() - 1;
+  for (unsigned i = 0; i <= n; ++i)
+    for (unsigned k = 0; k <= i; ++k)
+      b[i] += power[k] * choose(i, k) / choose(n, k);
+  return hull(b, 5);
+}
+inline double limit_splines(std::vector<Sample> &points,
+                            const std::vector<Limit> &limits) {
+  if (points.size() < 2 || limits.empty())
+    throw std::runtime_error("Need at least two trajectory points");
+  for (const auto &l : limits) {
+    for (double x : {l.lower, l.upper, l.velocity, l.acceleration, l.jerk})
+      if (!std::isfinite(x))
+        throw std::runtime_error("Non-finite joint limit");
+    if (l.lower >= l.upper || l.velocity <= 0 || l.acceleration <= 0 ||
+        l.jerk <= 0)
+      throw std::runtime_error("Invalid joint limits");
+  }
+  for (const auto &p : points) {
+    if (!std::isfinite(p.time) || p.time < 0 || p.time > 2147483647.0)
+      throw std::runtime_error("Invalid trajectory time");
+    for (const auto *values : {&p.position, &p.velocity, &p.acceleration}) {
+      if (values->size() != limits.size())
+        throw std::runtime_error("Incomplete trajectory derivatives");
+      for (double x : *values)
+        if (!std::isfinite(x))
+          throw std::runtime_error("Non-finite trajectory");
+    }
+  }
+  if (std::abs(points.front().time) > 1e-9)
+    throw std::runtime_error("Trajectory must start at t=0");
+  for (const auto *p : {&points.front(), &points.back()}) {
+    for (double v : p->velocity)
+      if (std::abs(v) > 1e-9)
+        throw std::runtime_error(
+            "Only stationary trajectory boundaries are supported");
+    for (double a : p->acceleration)
+      if (std::abs(a) > 1e-9)
+        throw std::runtime_error(
+            "Boundary acceleration must be zero for bounded jerk");
+  }
+  double scale = 1;
+  for (size_t i = 1; i < points.size(); ++i) {
+    const auto &p = points[i - 1];
+    const auto &q = points[i];
+    const double dt = q.time - p.time;
+    if (dt < 1e-6)
+      throw std::runtime_error(
+          "Trajectory times must strictly increase (>= 1 us)");
+    for (size_t j = 0; j < limits.size(); ++j) {
+      auto c = quintic(p.position[j], q.position[j], p.velocity[j],
+                       q.velocity[j], p.acceleration[j], q.acceleration[j], dt);
+      auto position = bounds(c);
+      const auto &l = limits[j];
+      if (position.first < l.lower - 1e-10 || position.second > l.upper + 1e-10)
+        throw std::runtime_error(
+            "Interpolated position exceeds joint limit at joint index " +
+            std::to_string(j));
+      for (unsigned order = 1; order <= 3; ++order) {
+        std::vector<double> derivative(c.size() - 1);
+        for (size_t k = 1; k < c.size(); ++k)
+          derivative[k - 1] = k * c[k] / dt;
+        c = derivative;
+        const auto b = bounds(c);
+        const double peak = std::max(std::abs(b.first), std::abs(b.second));
+        const double cap =
+            order == 1 ? l.velocity : (order == 2 ? l.acceleration : l.jerk);
+        const double ratio = peak / cap;
+        if (!std::isfinite(ratio))
+          throw std::runtime_error("Unrepresentable trajectory derivatives");
+        scale = std::max(scale, std::pow(ratio, 1.0 / order));
+      }
+    }
+  }
+  if (scale > 1.0 + 1e-9) {
+    scale *= 1.000001; // Margin for conversion of timestamps to nanoseconds.
+    for (auto &p : points) {
+      p.time *= scale;
+      if (!std::isfinite(p.time) || p.time > 2147483647.0)
+        throw std::runtime_error(
+            "Trajectory duration cannot be represented by ROS Duration");
+      for (double &v : p.velocity)
+        v /= scale;
+      for (double &a : p.acceleration)
+        a /= scale * scale;
+    }
+  } else
+    scale = 1;
+  return scale;
+}
+} // namespace iiwa_planning

src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp

@@ -0,0 +1,78 @@
+#pragma once
+#include <algorithm>
+#include <chrono>
+#include <cmath>
+#include <mutex>
+#include <rclcpp/rclcpp.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <vector>
+
+namespace iiwa_planning {
+// MoveIt's CurrentStateMonitor may omit velocities (copy_dynamics=false).
+// Observe the original telemetry instead of treating missing dynamics as zero.
+class StartStateMonitor {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node) {
+    node_ = node;
+    for (const auto &item : std::vector<std::pair<std::string, double>>{
+             {"iiwa_dynamics.stopped_velocity_tolerance", .01},
+             {"iiwa_dynamics.state_timeout", .5}})
+      if (!node->has_parameter(item.first))
+        node->declare_parameter(item.first, item.second);
+    tolerance_ = node->get_parameter("iiwa_dynamics.stopped_velocity_tolerance")
+                     .as_double();
+    timeout_ = node->get_parameter("iiwa_dynamics.state_timeout").as_double();
+    if (!std::isfinite(tolerance_) || tolerance_ <= 0 ||
+        !std::isfinite(timeout_) || timeout_ <= 0)
+      throw std::runtime_error("Invalid start-state monitoring parameters");
+    subscription_ = node->create_subscription<sensor_msgs::msg::JointState>(
+        "/joint_states", rclcpp::SensorDataQoS(),
+        [this](sensor_msgs::msg::JointState::ConstSharedPtr msg) {
+          std::lock_guard<std::mutex> lock(mutex_);
+          latest_ = msg;
+          received_ = std::chrono::steady_clock::now();
+        });
+  }
+  bool stationary(const std::vector<std::string> &names,
+                  const sensor_msgs::msg::JointState &requested) const {
+    // Explicit nonzero requested initial dynamics are never silently discarded.
+    if (!requested.velocity.empty() && !velocities_ok(names, requested))
+      return false;
+    auto node = node_.lock();
+    if (!node)
+      return velocities_ok(names,
+                           requested); // offline use requires explicit dynamics
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (!latest_ || std::chrono::duration<double>(
+                        std::chrono::steady_clock::now() - received_)
+                            .count() > timeout_)
+      return false;
+    const double age =
+        node->now().seconds() -
+        (latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
+    return std::isfinite(age) && age >= 0 && age <= timeout_ &&
+           velocities_ok(names, *latest_);
+  }
+
+private:
+  bool velocities_ok(const std::vector<std::string> &names,
+                     const sensor_msgs::msg::JointState &msg) const {
+    for (const auto &name : names) {
+      auto found = std::find(msg.name.begin(), msg.name.end(), name);
+      if (found == msg.name.end())
+        return false;
+      const size_t i = found - msg.name.begin();
+      if (i >= msg.velocity.size() || !std::isfinite(msg.velocity[i]) ||
+          std::abs(msg.velocity[i]) > tolerance_)
+        return false;
+    }
+    return true;
+  }
+  std::weak_ptr<rclcpp::Node> node_;
+  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
+  mutable std::mutex mutex_;
+  sensor_msgs::msg::JointState::ConstSharedPtr latest_;
+  std::chrono::steady_clock::time_point received_;
+  double tolerance_{.01}, timeout_{.5};
+};
+} // namespace iiwa_planning

src/iiwa_planning/package.xml

@@ -25,7 +25,13 @@
   <depend>tf_transformations</depend>
   <depend>python3-scipy</depend>
   <depend>iiwa_msgs</depend>
+  <depend>pluginlib</depend>
+  <depend>sensor_msgs</depend>
+  <build_depend>pybind11-dev</build_depend>
+  <exec_depend>python3-numpy</exec_depend>
 
+  <test_depend>joint_trajectory_controller</test_depend>
+  <test_depend>python3-pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

src/iiwa_planning/planning_response_plugins.xml

@@ -0,0 +1,8 @@
+<library path="iiwa_spline_limits">
+  <class name="iiwa_planning/LimitSplineDynamics" type="iiwa_planning::LimitSplineDynamics" base_class_type="planning_interface::PlanningResponseAdapter">
+    <description>Bound the complete JTC quintic interpolation, stretching time without changing its joint-space curve.</description>
+  </class>
+  <class name="iiwa_planning/SetRestBoundary" type="iiwa_planning::SetRestBoundary" base_class_type="planning_interface::PlanningResponseAdapter">
+    <description>Zero stationary endpoint acceleration before Ruckig; reject moving endpoints.</description>
+  </class>
+</library>

src/iiwa_planning/scripts/motion_sequence_config.json

@@ -1,42 +1,112 @@
 {
   "home": {
     "joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
-    "speed": 0.1
+    "speed": 0.5
   },
   "waypoints": [
-    {
-      "x": 0.6,
-      "y": 0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.4,
-      "planner": "ptp"
-    },
-    {
-      "x": 0.6,
-      "y": -0.1,
-      "z": 0.55,
-      "a": 3.14,
-      "b": 0.3142,
-      "c": 2.7916,
-      "speed": 0.3,
-      "planner": "lin"
-    },
-    {
-      "joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
-      "speed": 0.2
-    },
-    {
-      "x": 0.58,
-      "y": 0.0,
-      "z": 0.5,
-      "a": 3.14,
-      "b": 0.3665,
-      "c": 2.8791,
-      "speed": 0.3,
-      "planner": "lin"
-    }
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
+    {"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
+    {"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"}
   ]
 }

src/iiwa_planning/scripts/move_to_pose_server.py

@@ -117,7 +117,8 @@ class IiwaMotionServer(Node):
 
         def do_execute():
             try:
-                self._moveit.execute(plan_result.trajectory, controllers=[])
+                if not self._moveit.execute(plan_result.trajectory, controllers=[]):
+                    failed.set()
             except Exception as exc:
                 self.get_logger().error(f"Ошибка выполнения траектории: {exc}")
                 failed.set()

src/iiwa_planning/src/limit_spline_dynamics.cpp

@@ -0,0 +1,140 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include "iiwa_planning/start_state_monitor.hpp"
+#include <moveit/planning_interface/planning_response_adapter.hpp>
+#include <moveit/planning_scene/planning_scene.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <pluginlib/class_list_macros.hpp>
+
+namespace iiwa_planning {
+// TOTG can supply nonzero endpoint acceleration. Establish rest boundary
+// conditions before Ruckig, which otherwise preserves them. Positions and
+// interior derivatives stay intact.
+class SetRestBoundary : public planning_interface::PlanningResponseAdapter {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node,
+                  const std::string &) override {
+    start_monitor_.initialize(node);
+  }
+  std::string getDescription() const override {
+    return "Establish intended rest boundaries around Ruckig";
+  }
+  void adapt(const planning_scene::PlanningSceneConstPtr &,
+             const planning_interface::MotionPlanRequest &req,
+             planning_interface::MotionPlanResponse &res) const override {
+    if (!res.trajectory || res.trajectory->getWayPointCount() < 2 ||
+        !res.trajectory->getGroup()) {
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+      return;
+    }
+    if (!start_monitor_.stationary(
+            res.trajectory->getGroup()->getVariableNames(),
+            req.start_state.joint_state)) {
+      RCLCPP_ERROR(
+          rclcpp::get_logger("iiwa_spline_limits"),
+          "Fresh, stationary joint_states required for rest-to-rest planning");
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE;
+      return;
+    }
+    for (const auto index :
+         {size_t(0), res.trajectory->getWayPointCount() - 1}) {
+      auto &state = *res.trajectory->getWayPointPtr(index);
+      for (const auto &name : res.trajectory->getGroup()->getVariableNames()) {
+        state.setVariableVelocity(name, 0.0);
+        state.setVariableAcceleration(name, 0.0);
+      }
+    }
+  }
+
+private:
+  StartStateMonitor start_monitor_;
+};
+class LimitSplineDynamics : public planning_interface::PlanningResponseAdapter {
+public:
+  void initialize(const rclcpp::Node::SharedPtr &node,
+                  const std::string &) override {
+    start_monitor_.initialize(node);
+  }
+  std::string getDescription() const override {
+    return "Limit JTC quintic velocity, acceleration and jerk";
+  }
+  void adapt(const planning_scene::PlanningSceneConstPtr &,
+             const planning_interface::MotionPlanRequest &req,
+             planning_interface::MotionPlanResponse &res) const override {
+    if (!res.trajectory || res.trajectory->getWayPointCount() < 2) {
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+      return;
+    }
+    try {
+      auto &trajectory = *res.trajectory;
+      const auto *group = trajectory.getGroup();
+      if (!group)
+        throw std::runtime_error("Missing trajectory group");
+      const auto &names = group->getVariableNames();
+      if (!start_monitor_.stationary(names, req.start_state.joint_state))
+        throw std::runtime_error(
+            "Fresh stationary joint_states required before spline validation");
+      const auto model = trajectory.getRobotModel();
+      auto scaling = [](double x) {
+        return std::isfinite(x) && x > 0 && x <= 1 ? x : 1.0;
+      };
+      std::vector<Limit> limits;
+      for (const auto &name : names) {
+        const auto &b = model->getVariableBounds(name);
+        if (!b.position_bounded_ || !b.velocity_bounded_ ||
+            !b.acceleration_bounded_ || !b.jerk_bounded_)
+          throw std::runtime_error(
+              "Missing position/velocity/acceleration/jerk bounds: " + name);
+        limits.push_back(
+            {b.min_position_, b.max_position_,
+             b.max_velocity_ * scaling(req.max_velocity_scaling_factor),
+             b.max_acceleration_ * scaling(req.max_acceleration_scaling_factor),
+             b.max_jerk_});
+      }
+      std::vector<Sample> samples;
+      double t = 0;
+      for (size_t i = 0; i < trajectory.getWayPointCount(); ++i) {
+        const auto &state = *trajectory.getWayPointPtr(i);
+        if (!state.hasVelocities() || !state.hasAccelerations())
+          throw std::runtime_error(
+              "JTC guard requires positions, velocities and accelerations");
+        t += trajectory.getWayPointDurationFromPrevious(i);
+        Sample s{t, {}, {}, {}};
+        for (const auto &name : names) {
+          s.position.push_back(state.getVariablePosition(name));
+          s.velocity.push_back(state.getVariableVelocity(name));
+          s.acceleration.push_back(state.getVariableAcceleration(name));
+        }
+        samples.push_back(s);
+      }
+      const double scale = limit_splines(samples, limits);
+      if (scale > 1) {
+        for (size_t i = 0; i < samples.size(); ++i) {
+          auto &state = *trajectory.getWayPointPtr(i);
+          for (size_t j = 0; j < names.size(); ++j) {
+            state.setVariableVelocity(names[j], samples[i].velocity[j]);
+            state.setVariableAcceleration(names[j], samples[i].acceleration[j]);
+          }
+          trajectory.setWayPointDurationFromPrevious(
+              i, trajectory.getWayPointDurationFromPrevious(i) * scale);
+        }
+        RCLCPP_INFO(
+            rclcpp::get_logger("iiwa_spline_limits"),
+            "Trajectory duration scaled by %.6f to bound JTC derivatives",
+            scale);
+      }
+    } catch (const std::exception &e) {
+      RCLCPP_ERROR(rclcpp::get_logger("iiwa_spline_limits"),
+                   "Rejected trajectory: %s", e.what());
+      res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
+    }
+  }
+
+private:
+  StartStateMonitor start_monitor_;
+};
+} // namespace iiwa_planning
+PLUGINLIB_EXPORT_CLASS(iiwa_planning::LimitSplineDynamics,
+                       planning_interface::PlanningResponseAdapter)
+
+PLUGINLIB_EXPORT_CLASS(iiwa_planning::SetRestBoundary,
+                       planning_interface::PlanningResponseAdapter)

src/iiwa_planning/src/spline_limits_py.cpp

@@ -0,0 +1,30 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+namespace py = pybind11;
+PYBIND11_MODULE(_iiwa_spline_limits, m) {
+  m.def("retime", [](const std::vector<double> &t,
+                     const std::vector<std::vector<double>> &q,
+                     const std::vector<std::vector<double>> &v,
+                     const std::vector<std::vector<double>> &a,
+                     const std::vector<std::array<double, 5>> &caps) {
+    if (q.size() != t.size() || v.size() != t.size() || a.size() != t.size())
+      throw std::runtime_error("Point array sizes differ");
+    std::vector<iiwa_planning::Sample> points;
+    for (size_t i = 0; i < t.size(); ++i)
+      points.push_back({t[i], q[i], v[i], a[i]});
+    std::vector<iiwa_planning::Limit> limits;
+    for (const auto &c : caps)
+      limits.push_back({c[0], c[1], c[2], c[3], c[4]});
+    const double scale = iiwa_planning::limit_splines(points, limits);
+    std::vector<double> times;
+    std::vector<std::vector<double>> positions, velocities, accelerations;
+    for (const auto &p : points) {
+      times.push_back(p.time);
+      positions.push_back(p.position);
+      velocities.push_back(p.velocity);
+      accelerations.push_back(p.acceleration);
+    }
+    return py::make_tuple(times, positions, velocities, accelerations, scale);
+  });
+}

src/iiwa_planning/test/test_jtc_interpolation.cpp

@@ -0,0 +1,36 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <cassert>
+#include <iostream>
+#include <joint_trajectory_controller/trajectory.hpp>
+
+int main() {
+  using namespace iiwa_planning;
+  std::vector<Sample> samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}};
+  std::vector<Limit> limits{{-2, 2, 1, 1, 2}};
+  const auto scale = limit_splines(samples, limits);
+  assert(scale > 1);
+  joint_trajectory_controller::Trajectory jtc;
+  trajectory_msgs::msg::JointTrajectoryPoint a, b, out;
+  a.positions = samples[0].position;
+  a.velocities = samples[0].velocity;
+  a.accelerations = samples[0].acceleration;
+  b.positions = samples[1].position;
+  b.velocities = samples[1].velocity;
+  b.accelerations = samples[1].acceleration;
+  const auto finish = rclcpp::Time(static_cast<int64_t>(samples[1].time * 1e9));
+  const auto start = rclcpp::Time(int64_t(0));
+  double prev = 0;
+  const int n = 10000;
+  for (int i = 0; i <= n; ++i) {
+    const auto now = rclcpp::Time(finish.nanoseconds() * i / n);
+    jtc.interpolate_between_points(start, a, finish, b, now, out);
+    assert(std::abs(out.velocities[0]) <= 1.000001);
+    assert(std::abs(out.accelerations[0]) <= 1.000001);
+    if (i)
+      assert(std::abs((out.accelerations[0] - prev) / (samples[1].time / n)) <=
+             2.00001);
+    prev = out.accelerations[0];
+  }
+  std::cout << "Installed JTC interpolation stays within "
+               "velocity/acceleration/jerk limits\n";
+}

src/iiwa_planning/test/test_prepare_trajectory.py

@@ -0,0 +1,29 @@
+import numpy as np
+import pytest
+from iiwa_planning.direct_trajectory import prepare_trajectory
+
+LIMITS = [[-2, 2, 1, 1, 2]]
+
+
+def test_dense_points_are_smooth_and_retimed():
+    t, q, v, a, scale = prepare_trajectory([0, .1, .2], [[0], [.5], [1]], LIMITS)
+    assert scale > 1
+    np.testing.assert_allclose(q, [[0], [.5], [1]])
+    assert abs(v[1][0]) > 0  # do not stop at every CSV waypoint
+    np.testing.assert_allclose([v[0], v[-1], a[0], a[-1]], 0, atol=1e-9)
+    assert t[-1] > .2
+
+
+@pytest.mark.parametrize('times,positions', [([0, 0], [[0], [1]]), ([0, 1], [[0], [float('nan')]]), ([0, 1], [[0], [3]])])
+def test_bad_trajectory_is_rejected(times, positions):
+    with pytest.raises((ValueError, RuntimeError)):
+        prepare_trajectory(times, positions, LIMITS)
+
+
+def test_time_scaling_preserves_quintic_between_waypoints():
+    from scipy.interpolate import BPoly
+    times, positions, velocities, accelerations, scale = prepare_trajectory([0,.1,.2], [[0],[.5],[1]], LIMITS)
+    curve = BPoly.from_derivatives(times, [[q,v,a] for q,v,a in zip(positions,velocities,accelerations)])
+    grid = np.linspace(times[0],times[-1],10001)
+    for order,cap in [(1,1),(2,1),(3,2)]:
+        assert np.max(np.abs(curve(grid,nu=order))) <= cap * 1.000001

src/iiwa_planning/test/test_response_adapter.cpp

@@ -0,0 +1,65 @@
+#include <cassert>
+#include <iostream>
+#include <moveit/planning_interface/planning_response_adapter.hpp>
+#include <moveit/robot_model/robot_model.hpp>
+#include <moveit/robot_trajectory/robot_trajectory.hpp>
+#include <moveit/trajectory_processing/ruckig_traj_smoothing.hpp>
+#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
+#include <pluginlib/class_loader.hpp>
+#include <srdfdom/model.h>
+#include <urdf_parser/urdf_parser.h>
+
+int main() {
+  auto urdf = urdf::parseURDF(
+      R"(<robot name="test"><link name="base"/><link name="tip"/><joint name="joint1" type="revolute"><parent link="base"/><child link="tip"/><axis xyz="0 0 1"/><limit lower="-2" upper="2" velocity="1" effort="1"/></joint></robot>)");
+  auto srdf = std::make_shared<srdf::Model>();
+  assert(srdf->initString(
+      *urdf,
+      R"(<robot name="test"><group name="arm"><joint name="joint1"/></group></robot>)"));
+  auto model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
+  auto bounds = model->getVariableBounds("joint1");
+  bounds.acceleration_bounded_ = bounds.jerk_bounded_ = true;
+  bounds.min_acceleration_ = -1;
+  bounds.max_acceleration_ = 1;
+  bounds.min_jerk_ = -2;
+  bounds.max_jerk_ = 2;
+  model->getJointModel("joint1")->setVariableBounds("joint1", bounds);
+  auto trajectory =
+      std::make_shared<robot_trajectory::RobotTrajectory>(model, "arm");
+  moveit::core::RobotState state(model);
+  state.setToDefaultValues();
+  state.setVariableVelocity("joint1", 0);
+  state.setVariableAcceleration("joint1", 0);
+  trajectory->addSuffixWayPoint(state, 0);
+  state.setVariablePosition("joint1", 1);
+  trajectory->addSuffixWayPoint(state, .2);
+  assert(trajectory_processing::TimeOptimalTrajectoryGeneration()
+             .computeTimeStamps(*trajectory));
+  pluginlib::ClassLoader<planning_interface::PlanningResponseAdapter> loader(
+      "moveit_core", "planning_interface::PlanningResponseAdapter");
+  auto guard = loader.createSharedInstance("iiwa_planning/LimitSplineDynamics");
+  planning_interface::MotionPlanRequest req;
+  req.group_name = "arm";
+  req.max_velocity_scaling_factor = req.max_acceleration_scaling_factor = 1;
+  planning_interface::MotionPlanResponse res;
+  res.trajectory = trajectory;
+  res.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
+  auto boundary = loader.createSharedInstance("iiwa_planning/SetRestBoundary");
+  req.start_state.joint_state.name = {"joint1"};
+  req.start_state.joint_state.position = {0};
+  req.start_state.joint_state.velocity = {0};
+  boundary->adapt(nullptr, req, res);
+  assert(bool(res));
+  assert(trajectory_processing::RuckigSmoothing::applySmoothing(*trajectory));
+  boundary->adapt(nullptr, req, res);
+  assert(bool(res));
+  guard->adapt(nullptr, req, res);
+  assert(bool(res));
+  assert(trajectory->getDuration() > .2);
+  state.setVariablePosition("joint1", 3);
+  trajectory->addSuffixWayPoint(state, 1);
+  guard->adapt(nullptr, req, res);
+  assert(!bool(res));
+  std::cout << "Installed Ruckig + pluginlib response adapter: success and "
+               "rejection OK\n";
+}

src/iiwa_planning/test/test_spline_limits.cpp

@@ -0,0 +1,58 @@
+#include "iiwa_planning/spline_limits.hpp"
+#include <cassert>
+#include <cmath>
+#include <iostream>
+using namespace iiwa_planning;
+int main() {
+  // Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5.
+  std::vector<Sample> points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}};
+  std::vector<Limit> limits{{-2, 2, 2, 1, 2}};
+  const double scale = limit_splines(points, limits);
+  assert(scale > 3.0);
+  assert(std::abs(points.back().position[0] - 1) < 1e-12);
+  assert(limit_splines(points, limits) <= 1.000001);
+  // Consistent time scaling keeps the geometric quintic unchanged.
+  auto p = quintic(0, 1, 0, 0, 0, 0, 1);
+  auto q = quintic(0, 1, 0, 0, 0, 0, points.back().time);
+  for (int i = 0; i <= 5; ++i)
+    assert(std::abs(p[i] - q[i]) < 1e-10);
+  bool bad = false;
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {0}}, {0, {1}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  bad = false;
+  // Positions are in range but large endpoint velocities overshoot between
+  // them.
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {0}},
+                          {1, {0}, {12}, {0}},
+                          {2, {0}, {-12}, {0}},
+                          {3, {0}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  bad = false;
+  try {
+    std::vector<Sample> s{{0, {NAN}, {0}, {0}}, {1, {0}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  // Non-rest boundaries must not slip past the finite-segment jerk check.
+  bad = false;
+  try {
+    std::vector<Sample> s{{0, {0}, {0}, {1}}, {1, {1}, {0}, {0}}};
+    limit_splines(s, limits);
+  } catch (const std::exception &) {
+    bad = true;
+  }
+  assert(bad);
+  std::cout << "spline limits OK\n";
+}

src/iiwa_planning/test/test_start_state_monitor.cpp

@@ -0,0 +1,41 @@
+#include "iiwa_planning/start_state_monitor.hpp"
+#include <cassert>
+#include <iostream>
+#include <thread>
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  auto node = std::make_shared<rclcpp::Node>(
+      "iiwa_start_monitor_test", rclcpp::NodeOptions().enable_rosout(false));
+  iiwa_planning::StartStateMonitor monitor;
+  monitor.initialize(node);
+  auto pub = node->create_publisher<sensor_msgs::msg::JointState>(
+      "/joint_states", rclcpp::SensorDataQoS());
+  sensor_msgs::msg::JointState
+      requested; // Normal MoveIt request has no velocities.
+  assert(!monitor.stationary({"joint1"}, requested));
+  sensor_msgs::msg::JointState state;
+  state.name = {"joint1"};
+  state.position = {0};
+  state.velocity = {0};
+  auto publish = [&] {
+    for (int i = 0; i < 30; ++i) {
+      state.header.stamp = node->now();
+      pub->publish(state);
+      rclcpp::spin_some(node);
+      std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+  };
+  publish();
+  assert(monitor.stationary({"joint1"}, requested));
+  state.velocity = {.1};
+  publish();
+  assert(!monitor.stationary({"joint1"}, requested));
+  state.velocity = {0};
+  publish();
+  assert(monitor.stationary({"joint1"}, requested));
+  std::this_thread::sleep_for(std::chrono::milliseconds(550));
+  assert(!monitor.stationary({"joint1"}, requested));
+  rclcpp::shutdown();
+  std::cout << "Position-only MoveIt requests validated using fresh real "
+               "velocity feedback\n";
+}