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.
2026-09-21 05:16:09 +03:00
parent 9f0f05df87
commit 0a091ad521
43 changed files with 1342 additions and 137 deletions
@@ -43,11 +43,13 @@ def _runtime_setup(context, *args, **kwargs):
    if simulate:
        xacro_args = {
            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
            "simulate": "true",
        }
    else:
        xacro_args = {
            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
            "robot_ip": settings.robot.ip,
            "fri_port": str(settings.robot.port),
            "fri_cycle_ms": str(settings.robot.fri_cycle_ms),
@@ -90,6 +92,7 @@ def _runtime_setup(context, *args, **kwargs):
                "controller_timer": str(settings.digital_twin.webots.controller_timer),
                "controller": str(settings.controller.controller_path),
                "initial_positions_file": str(settings.controller.moveit.initial_positions),
+                "joint_limits_file": str(settings.controller.moveit.joint_limits),
            }.items(),
        ))

@@ -101,6 +104,7 @@ def _runtime_setup(context, *args, **kwargs):
            "robot_name": settings.robot.name,
            "description": description_path,
            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
            "controller_path": settings.controller.controller_path,
            "simulate": "true",
            "transform": str(settings.digital_twin.webots.transform),
@@ -112,6 +116,7 @@ def _runtime_setup(context, *args, **kwargs):
            "robot_name": settings.robot.name,
            "description": description_path,
            "initial_positions_file": settings.controller.moveit.initial_positions,
+            "joint_limits_file": settings.controller.moveit.joint_limits,
            "controller_path": settings.controller.controller_path,
            "simulate": "false",
            "transform": str(settings.digital_twin.webots.transform),
@@ -32,7 +32,8 @@ def _setup_controllers(context, *args, **kwargs):
        raise ValueError("joint_position_tau must be finite and non-negative")
    update_rate = 1000 // fri_cycle_ms

-    xacro_args = {"initial_positions_file": initial_positions_file}
+    xacro_args = {"initial_positions_file": initial_positions_file,
+                  "joint_limits_file": LaunchConfiguration("joint_limits_file").perform(context)}

    if simulate:
        xacro_args["simulate"] = "true"
@@ -1,5 +1,6 @@
 from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
+from iiwa_utils.converter import effective_joint_limits


 def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
@@ -9,7 +10,8 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
        .robot_description(
            file_path=settings.robot.description,
            mappings={
-                "initial_positions_file": settings.controller.moveit.initial_positions
+                "initial_positions_file": settings.controller.moveit.initial_positions,
+                "joint_limits_file": settings.controller.moveit.joint_limits
            },
        )
        .robot_description_semantic(file_path=settings.controller.moveit.srdf)
@@ -21,10 +23,16 @@ def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
        .to_moveit_configs()
    )

+    moveit_configs.joint_limits["robot_description_planning"] = effective_joint_limits(
+        moveit_configs.joint_limits["robot_description_planning"]
+    )
+
    common_params = [
        moveit_configs.to_dict(),
        {"robot_description": robot_description},
        {"use_sim_time": use_sim_time},
+        {"iiwa_dynamics.stopped_velocity_tolerance": settings.planning.stopped_velocity_tolerance,
+         "iiwa_dynamics.state_timeout": settings.planning.state_timeout},
    ]

    move_group = Node(
@@ -27,7 +27,10 @@ def make_web_server_node(settings, use_sim_time: bool) -> Node:
            "host": settings.web.host,
            "port": settings.web.port,
            "endpoints_path": settings.web.endpoints,
-            "joint_limits_path": settings.web.joint_limits,
+            "joint_limits_path": settings.controller.moveit.joint_limits,
+            "trajectory_state_timeout": settings.web.trajectory_state_timeout,
+            "trajectory_start_tolerance": settings.web.trajectory_start_tolerance,
+            "trajectory_stopped_velocity": settings.web.trajectory_stopped_velocity,
            "use_sim_time": use_sim_time,
        }],
    )
@@ -29,6 +29,7 @@ def _spawn_setup(context, *args, **kwargs):
        xacro_args={
            "simulate": "true",
            "initial_positions_file": initial_positions_file,
+            "joint_limits_file": LaunchConfiguration("joint_limits_file").perform(context),
        },
    )

@@ -0,0 +1,19 @@
+planning_plugins:
+  - chomp_interface/CHOMPPlanner
+enable_failure_recovery: true
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - iiwa_planning/SetRestBoundary
+  - default_planning_response_adapters/AddRuckigTrajectorySmoothing
+  - iiwa_planning/SetRestBoundary
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+
+ridge_factor: 0.01
@@ -1,73 +1,84 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
-
-# For beginners, we downscale velocity and acceleration limits.
-# You can always specify higher scaling factors (<= 1.0) in your motion requests.  # Increase the values below to 1.0 to always move at maximum speed.
+# Single source for all seven axes: URDF, ros2_control, MoveIt and web validation.
+# KUKA iiwa 7 R800 datasheet 0000-246-832 V1.2 (2022-06-01):
+# range [deg]: 170,120,170,120,170,120,175; speed [deg/s]: 98,98,100,130,140,180,180.
+# Working hard limits never exceed either that datasheet or the previous URDF.
+# Soft bounds preserve the existing protective operating margin.
+# Acceleration and jerk below are PROJECT limits, NOT verified KUKA ratings.
 default_velocity_scaling_factor: 0.1
 default_acceleration_scaling_factor: 0.1
-
-# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
-# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
 joint_limits:
  joint1:
    has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
    has_velocity_limits: true
    max_velocity: 1.71
    has_acceleration_limits: true
    max_acceleration: 8.5521
    has_jerk_limits: true
    max_jerk: 85.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
  joint2:
    has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
    has_velocity_limits: true
    max_velocity: 1.71
    has_acceleration_limits: true
    max_acceleration: 8.5521
    has_jerk_limits: true
    max_jerk: 85.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
  joint3:
    has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
    has_velocity_limits: true
-    max_velocity: 1.75
+    max_velocity: 1.7453292519943295
    has_acceleration_limits: true
    max_acceleration: 8.7266
    has_jerk_limits: true
    max_jerk: 87.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
  joint4:
    has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
    has_velocity_limits: true
-    max_velocity: 2.27
+    max_velocity: 2.2689280275926285
    has_acceleration_limits: true
    max_acceleration: 11.3446
    has_jerk_limits: true
    max_jerk: 113.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
  joint5:
    has_position_limits: true
-    min_position: -2.97
-    max_position: 2.97
+    min_position: -2.9670597283903604
+    max_position: 2.9670597283903604
    has_velocity_limits: true
    max_velocity: 2.44
    has_acceleration_limits: true
    max_acceleration: 12.2173
    has_jerk_limits: true
    max_jerk: 122.0
+    soft_min_position: -2.8827
+    soft_max_position: 2.8827
  joint6:
    has_position_limits: true
-    min_position: -2.10
-    max_position: 2.10
+    min_position: -2.09
+    max_position: 2.09
    has_velocity_limits: true
    max_velocity: 3.14
    has_acceleration_limits: true
-    max_acceleration: 15.7080
+    max_acceleration: 15.708
    has_jerk_limits: true
    max_jerk: 157.0
+    soft_min_position: -2.0027
+    soft_max_position: 2.0027
  joint7:
    has_position_limits: true
    min_position: -3.05
@@ -75,7 +86,8 @@ joint_limits:
    has_velocity_limits: true
    max_velocity: 3.14
    has_acceleration_limits: true
-    max_acceleration: 15.7080
+    max_acceleration: 15.708
    has_jerk_limits: true
    max_jerk: 157.0
-    
+    soft_min_position: -2.9627
+    soft_max_position: 2.9627
@@ -0,0 +1,16 @@
+planning_plugins:
+  - ompl_interface/OMPLPlanner
+# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  - default_planning_response_adapters/AddTimeOptimalParameterization
+  - iiwa_planning/SetRestBoundary
+  - default_planning_response_adapters/AddRuckigTrajectorySmoothing
+  - iiwa_planning/SetRestBoundary
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
@@ -0,0 +1,16 @@
+planning_plugins:
+  - pilz_industrial_motion_planner/CommandPlanner
+default_planner_config: PTP
+request_adapters:
+  - default_planning_request_adapters/ResolveConstraintFrames
+  - default_planning_request_adapters/ValidateWorkspaceBounds
+  - default_planning_request_adapters/CheckStartStateBounds
+  - default_planning_request_adapters/CheckStartStateCollision
+response_adapters:
+  # Preserve Pilz LIN/CIRC geometry: only uniform time scaling, no joint-space Ruckig.
+  - iiwa_planning/LimitSplineDynamics
+  - default_planning_response_adapters/ValidateSolution
+  - default_planning_response_adapters/DisplayMotionPath
+capabilities: >-
+    pilz_industrial_motion_planner/MoveGroupSequenceAction
+    pilz_industrial_motion_planner/MoveGroupSequenceService
@@ -43,6 +43,8 @@ planning:
  default_frame: "base_link"  # Система отсчёта по умолчанию
  default_planner: "ompl"     # Планировщик по умолчанию
  planning_attempts: 3        # Число попыток планирования
+  stopped_velocity_tolerance: 0.01  # рад/с: проверка начала движения из покоя
+  state_timeout: 0.5  # с: максимальный возраст joint_states для проверки покоя

 web:
  enabled: true
@@ -51,6 +53,11 @@ web:
  endpoints: pkg://iiwa_config/config/api_endpoints.yaml
  joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml

+  # Direct CSV/JSON trajectories require fresh feedback and a stationary start.
+  trajectory_state_timeout: 0.5       # s
+  trajectory_start_tolerance: 0.001   # rad
+  trajectory_stopped_velocity: 0.01  # rad/s
+
 foxglove:
  enabled: true  # Запускать ли foxglove_bridge вместе с роботом
  port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)
@@ -145,6 +145,11 @@ CallbackReturn IIWAHardwareInterface::on_init(
    }
    lower_[i] = limits->second.min_position;
    upper_[i] = limits->second.max_position;
+    const auto soft = info.soft_limits.find(joint.name);
+    if (soft != info.soft_limits.end()) {
+      lower_[i] = std::max(lower_[i], soft->second.min_position);
+      upper_[i] = std::min(upper_[i], soft->second.max_position);
+    }
    max_velocity_[i] = limits->second.max_velocity;
    if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] ||
      !std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0)
@@ -107,3 +107,14 @@ TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder)
  EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
  EXPECT_EQ(hardware.on_error(state), Return::SUCCESS);
 }
+
+TEST(Hardware, RejectsInconsistentSoftLimits)
+{
+  Hardware hardware;
+  auto params = parameters();
+  joint_limits::SoftJointLimits soft;
+  soft.min_position = 1.0;
+  soft.max_position = -1.0;
+  params.hardware_info.soft_limits["joint1"] = soft;
+  EXPECT_EQ(hardware.on_init(params), Return::ERROR);
+}
@@ -3,6 +3,8 @@

    <xacro:arg name="initial_positions_file"
               default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
+    <xacro:arg name="joint_limits_file" default="$(find iiwa_config)/config/moveit/joint_limits.yaml"/>
+    <xacro:property name="joint_limits" value="${xacro.load_yaml('$(arg joint_limits_file)')['joint_limits']}"/>
    <xacro:arg name="simulate" default="false"/>
    <xacro:arg name="robot_ip" default="192.170.10.2"/>
    <xacro:arg name="fri_port" default="30200"/>
@@ -52,8 +54,8 @@

            <joint name="joint1">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint1']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint1']}</param>
@@ -64,8 +66,8 @@

            <joint name="joint2">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint2']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint2']}</param>
@@ -76,8 +78,8 @@

            <joint name="joint3">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint3']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint3']}</param>
@@ -88,8 +90,8 @@

            <joint name="joint4">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint4']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint4']}</param>
@@ -100,8 +102,8 @@

            <joint name="joint5">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint5']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint5']}</param>
@@ -112,8 +114,8 @@

            <joint name="joint6">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint6']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint6']}</param>
@@ -124,8 +126,8 @@

            <joint name="joint7">
                <command_interface name="position">
-                    <param name="min">-3.05</param>
-                    <param name="max"> 3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint7']['soft_max_position']}</param>
                </command_interface>
                <state_interface name="position">
                    <param name="initial_value">${initial_positions['joint7']}</param>
@@ -153,8 +155,8 @@

            <joint name="joint1">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint1']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -166,8 +168,8 @@

            <joint name="joint2">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint2']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -179,8 +181,8 @@

            <joint name="joint3">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint3']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -192,8 +194,8 @@

            <joint name="joint4">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint4']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -205,8 +207,8 @@

            <joint name="joint5">
                <command_interface name="position">
-                    <param name="min">-2.97</param>
-                    <param name="max"> 2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint5']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -218,8 +220,8 @@

            <joint name="joint6">
                <command_interface name="position">
-                    <param name="min">-2.09</param>
-                    <param name="max"> 2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint6']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -231,8 +233,8 @@

            <joint name="joint7">
                <command_interface name="position">
-                    <param name="min">-3.05</param>
-                    <param name="max"> 3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
+                    <param name="max">${joint_limits['joint7']['soft_max_position']}</param>
                </command_interface>

                <state_interface name="position">
@@ -12,57 +12,57 @@
    <!-- joint1: ±170° hard, ±165° soft, k_velocity=10 -->
    <xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
                        xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint1']['min_position']}" upper="${joint_limits['joint1']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint1']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint1']['soft_min_position']}" soft_upper="${joint_limits['joint1']['soft_max_position']}" k_velocity="10"/>


    <!-- joint2: ±119° hard, ±114° soft -->
    <xacro:revolute_joint jname="joint2" parent="link1" child="link2"
                        xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint2']['min_position']}" upper="${joint_limits['joint2']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint2']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint2']['soft_min_position']}" soft_upper="${joint_limits['joint2']['soft_max_position']}" k_velocity="10"/>


    <!-- joint3: ±170° hard, ±165° soft -->
    <xacro:revolute_joint jname="joint3" parent="link2" child="link3"
                        xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="1.75" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint3']['min_position']}" upper="${joint_limits['joint3']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint3']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint3']['soft_min_position']}" soft_upper="${joint_limits['joint3']['soft_max_position']}" k_velocity="10"/>


    <!-- joint4: ±119° hard, ±114° soft -->
    <xacro:revolute_joint jname="joint4" parent="link3" child="link4"
                        xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="2.27" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint4']['min_position']}" upper="${joint_limits['joint4']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint4']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint4']['soft_min_position']}" soft_upper="${joint_limits['joint4']['soft_max_position']}" k_velocity="10"/>


    <!-- joint5: ±170° hard, ±165° soft -->
    <xacro:revolute_joint jname="joint5" parent="link4" child="link5"
                        xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-2.97" upper="2.97" effort="200"
-                        velocity="2.44" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        lower="${joint_limits['joint5']['min_position']}" upper="${joint_limits['joint5']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint5']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint5']['soft_min_position']}" soft_upper="${joint_limits['joint5']['soft_max_position']}" k_velocity="10"/>


    <!-- joint6: ±119° hard, ±114° soft -->
    <xacro:revolute_joint jname="joint6" parent="link5" child="link6"
                        xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
-                        lower="-2.09" upper="2.09" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        lower="${joint_limits['joint6']['min_position']}" upper="${joint_limits['joint6']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint6']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint6']['soft_min_position']}" soft_upper="${joint_limits['joint6']['soft_max_position']}" k_velocity="10"/>


    <!-- joint7: ±174° hard, ±169° soft -->
    <xacro:revolute_joint jname="joint7" parent="link6" child="link7"
                        xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
-                        lower="-3.05" upper="3.05" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
-                        soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/>
+                        lower="${joint_limits['joint7']['min_position']}" upper="${joint_limits['joint7']['max_position']}" effort="200"
+                        velocity="${joint_limits['joint7']['max_velocity']}" damping="10.0" friction="0.1"
+                        soft_lower="${joint_limits['joint7']['soft_min_position']}" soft_upper="${joint_limits['joint7']['soft_max_position']}" k_velocity="10"/>

    <joint name="tools_joint" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
@@ -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)
@@ -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)
@@ -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
@@ -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
@@ -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>

@@ -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>
@@ -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"}
  ]
 }
@@ -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()
@@ -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)
@@ -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);
+  });
+}
@@ -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";
+}
@@ -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
@@ -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";
+}
@@ -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";
+}
@@ -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";
+}
@@ -1,4 +1,6 @@
 import yaml
+import copy
+import math
 import tempfile

 from pathlib import Path
@@ -28,9 +30,24 @@ def load_robot_description(
    raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}")


+def effective_joint_limits(data: dict) -> dict:
+    """Keep MoveIt's YAML overrides inside the same protective bounds as FRI."""
+    result = copy.deepcopy(data)
+    for name, joint in result['joint_limits'].items():
+        joint['min_position'] = max(joint['min_position'], joint.get('soft_min_position', joint['min_position']))
+        joint['max_position'] = min(joint['max_position'], joint.get('soft_max_position', joint['max_position']))
+        if not all(math.isfinite(joint[key]) for key in ('min_position', 'max_position', 'max_velocity', 'max_acceleration', 'max_jerk')):
+            raise ValueError(f'Non-finite joint limits: {name}')
+        if joint['min_position'] >= joint['max_position'] or any(joint[key] <= 0 for key in ('max_velocity', 'max_acceleration', 'max_jerk')):
+            raise ValueError(f'Invalid joint limits: {name}')
+    return result
+
+
 def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str:
    with open(yaml_path, "r") as f:
        data = yaml.safe_load(f)
+    if namespace == 'robot_description_planning':
+        data = effective_joint_limits(data)
    wrapped = {namespace: {"ros__parameters": data}}
    tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False)
    yaml.dump(wrapped, tmp, default_flow_style=False)
@@ -63,6 +63,8 @@ class PlanningCfg:
    default_frame: str
    default_planner: str
    planning_attempts: int
+    stopped_velocity_tolerance: float = 0.01
+    state_timeout: float = 0.5


@dataclass(frozen=True)
@@ -77,6 +79,9 @@ class WebCfg:
    port: int
    endpoints: str    # resolved absolute path to api_endpoints.yaml
    joint_limits: str  # resolved absolute path to joint_limits.yaml
+    trajectory_state_timeout: float = 0.5
+    trajectory_start_tolerance: float = 0.001
+    trajectory_stopped_velocity: float = 0.01


@dataclass(frozen=True)
@@ -191,6 +196,9 @@ _WEB_DEFAULTS: Dict[str, Any] = {
    "port": 8007,
    "endpoints": "pkg://iiwa_config/config/api_endpoints.yaml",
    "joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml",
+    "trajectory_state_timeout": 0.5,
+    "trajectory_start_tolerance": 0.001,
+    "trajectory_stopped_velocity": 0.01,
 }


@@ -207,6 +215,9 @@ def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg:
        port=int(get("port")),
        endpoints=resolve_path(str(get("endpoints")), settings_dir),
        joint_limits=resolve_path(str(get("joint_limits")), settings_dir),
+        trajectory_state_timeout=float(get("trajectory_state_timeout")),
+        trajectory_start_tolerance=float(get("trajectory_start_tolerance")),
+        trajectory_stopped_velocity=float(get("trajectory_stopped_velocity")),
    )


@@ -342,6 +353,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
        default_frame=str(planning_raw.get("default_frame", "base_link")),
        default_planner=str(planning_raw.get("default_planner", "ompl")),
        planning_attempts=int(planning_raw.get("planning_attempts", 3)),
+        stopped_velocity_tolerance=float(planning_raw.get("stopped_velocity_tolerance", 0.01)),
+        state_timeout=float(planning_raw.get("state_timeout", 0.5)),
    )

    # tool
@@ -12,6 +12,7 @@ setup(
        ('share/' + package_name, ['package.xml']),
    ],
    install_requires=['setuptools'],
+    tests_require=['pytest'],
    zip_safe=True,
    maintainer='daniel',
    maintainer_email='grabardm@ml-dev.ru',
@@ -0,0 +1,45 @@
+from pathlib import Path
+import math
+import xml.etree.ElementTree as ET
+import yaml
+import xacro
+import xacro.substitution_args
+
+SRC = Path(__file__).resolve().parents[2]
+LIMITS = SRC / 'iiwa_config/config/moveit/joint_limits.yaml'
+
+
+def test_all_seven_axes_share_limits(monkeypatch):
+    monkeypatch.setattr(xacro.substitution_args, '_eval_find', lambda p: str(SRC / p))
+    data = yaml.safe_load(LIMITS.read_text())['joint_limits']
+    for simulate in ('false', 'true'):
+        robot = ET.fromstring(xacro.process_file(
+            str(SRC / 'iiwa_description/urdf/iiwa7.urdf.xacro'),
+            mappings={'simulate': simulate, 'joint_limits_file': str(LIMITS)}).toxml())
+        for i, (degrees, speed) in enumerate(zip([170,120,170,120,170,120,175], [98,98,100,130,140,180,180]), 1):
+            name = f'joint{i}'
+            limit = data[name]
+            physical = robot.find(f"joint[@name='{name}']/limit")
+            assert float(physical.attrib['lower']) == limit['min_position']
+            assert float(physical.attrib['upper']) == limit['max_position']
+            assert float(physical.attrib['velocity']) == limit['max_velocity']
+            assert limit['max_position'] <= math.radians(degrees) + 1e-12
+            assert limit['min_position'] >= -math.radians(degrees) - 1e-12
+            assert limit['max_velocity'] <= math.radians(speed) + 1e-12
+            safety = robot.find(f"joint[@name='{name}']/safety_controller")
+            assert float(safety.attrib['soft_lower_limit']) == limit['soft_min_position']
+            assert float(safety.attrib['soft_upper_limit']) == limit['soft_max_position']
+            command = robot.find(f"ros2_control/joint[@name='{name}']/command_interface")
+            assert float(command.find("param[@name='min']").text) == limit['soft_min_position']
+            assert float(command.find("param[@name='max']").text) == limit['soft_max_position']
+            assert limit['max_acceleration'] > 0 and limit['max_jerk'] > 0
+
+
+def test_planning_limits_keep_soft_margin():
+    from iiwa_utils.converter import effective_joint_limits
+    raw = yaml.safe_load(LIMITS.read_text())
+    effective = effective_joint_limits(raw)
+    for name, joint in raw['joint_limits'].items():
+        assert effective['joint_limits'][name]['min_position'] == joint['soft_min_position']
+        assert effective['joint_limits'][name]['max_position'] == joint['soft_max_position']
+        assert joint['min_position'] < joint['soft_min_position']  # input not mutated
@@ -48,7 +48,7 @@ def _resolve_path(package: str, relative: str) -> Path:
        from ament_index_python.packages import get_package_share_directory
        return Path(get_package_share_directory(package)) / relative
    except Exception:
-        src = Path(__file__).parents[3]  # .../src/iiwa_web/iiwa_web/ -> .../src/
+        src = Path(__file__).parents[2]  # .../src/iiwa_web/iiwa_web/ -> .../src/
        return src / package / relative


@@ -60,7 +60,8 @@ def _parse_joint_limits_data(data: dict) -> tuple[list[str], list[tuple[float, f
    while f"joint{i}" in joints:
        j = joints[f"joint{i}"]
        names.append(f"joint{i}")
-        limits.append((j["min_position"], j["max_position"]))
+        limits.append((max(j["min_position"], j.get("soft_min_position", j["min_position"])),
+                       min(j["max_position"], j.get("soft_max_position", j["max_position"]))))
        i += 1
    return names, limits

@@ -15,8 +15,12 @@ class CobotWebNode(Node):
        self.declare_parameter('port', 8007)
        self.declare_parameter('endpoints_path', '')
        self.declare_parameter('joint_limits_path', '')
+        self.declare_parameter('trajectory_state_timeout', 0.5)
+        self.declare_parameter('trajectory_start_tolerance', 0.001)
+        self.declare_parameter('trajectory_stopped_velocity', 0.01)

        self._topic_cache: dict = {}
+        self._topic_received: dict = {}
        self._pub_registry: dict = {}
        self._service_clients: dict = {}
        self._action_clients: dict = {}
@@ -47,9 +51,15 @@ class CobotWebNode(Node):
        with self._lock:
            return self._topic_cache.get(topic_name)

+    def get_latest_with_age(self, topic_name: str):
+        with self._lock:
+            return (self._topic_cache.get(topic_name),
+                    time.monotonic() - self._topic_received.get(topic_name, float('-inf')))
+
    def _handle_message(self, topic_name: str, msg):
        with self._lock:
            self._topic_cache[topic_name] = msg
+            self._topic_received[topic_name] = time.monotonic()

    def call_service(self, srv_type, srv_name: str, request, timeout: float = 5.0):
        if srv_name not in self._service_clients:
@@ -1,5 +1,11 @@
 import csv
 import io
+import math
+import threading
+import time
+import yaml
+from pathlib import Path
+from iiwa_planning.direct_trajectory import prepare_trajectory
 from collections import deque
 from datetime import datetime
 from builtin_interfaces.msg import Duration
@@ -7,7 +13,7 @@ from fastapi import APIRouter, File, HTTPException, Query, UploadFile
 from pydantic import BaseModel, Field
 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint

-from .config_loader import load_joint_limits, load_joint_names
+from .config_loader import load_joint_limits, load_joint_names, _resolve_path
 from .ros_node import get_bridge

 router = APIRouter(prefix="/trajectory", tags=["trajectory"])
@@ -15,6 +21,10 @@ router = APIRouter(prefix="/trajectory", tags=["trajectory"])
 TOPIC = "/iiwa_arm_controller/joint_trajectory"
 JOINT_NAMES = load_joint_names()
 N_JOINTS = len(JOINT_NAMES)
+_prepare_lock = threading.Lock()
+_admission_lock = threading.Lock()
+_generation = 0
+_direct_until = 0.0

 _log_lines: deque[str] = deque(maxlen=300)

@@ -24,13 +34,13 @@ def _log(msg: str) -> None:


 def _to_duration(seconds: float) -> Duration:
-    sec = int(seconds)
-    nanosec = int(round((seconds - sec) * 1e9))
+    total_ns = round(seconds * 1e9)
+    sec, nanosec = divmod(total_ns, 1_000_000_000)
    return Duration(sec=sec, nanosec=nanosec)


 def _validate_limits(points: list[list[float]]) -> None:
-    limits = load_joint_limits()
+    limits = load_joint_limits(path=get_bridge().get_parameter("joint_limits_path").value or None)
    for row_idx, positions in enumerate(points):
        for j, (pos, (lo, hi)) in enumerate(zip(positions, limits)):
            if not (lo <= pos <= hi):
@@ -52,6 +62,81 @@ def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
    return msg


+def _stationary_state():
+    bridge = get_bridge()
+    state, age = bridge.get_latest_with_age('/joint_states')
+    timeout = bridge.get_parameter('trajectory_state_timeout').value
+    tolerance = bridge.get_parameter('trajectory_start_tolerance').value
+    stopped = bridge.get_parameter('trajectory_stopped_velocity').value
+    if any(not math.isfinite(x) or x <= 0 for x in (timeout, tolerance, stopped)):
+        raise HTTPException(503, 'Invalid direct trajectory monitoring parameters')
+    if state is None or age > timeout:
+        raise HTTPException(409, 'Fresh joint_states required before sending a trajectory')
+    stamp = state.header.stamp.sec + state.header.stamp.nanosec * 1e-9
+    source_age = bridge.get_clock().now().nanoseconds * 1e-9 - stamp
+    if not math.isfinite(source_age) or not 0 <= source_age <= timeout:
+        raise HTTPException(409, 'joint_states timestamp is stale or in the future')
+    try:
+        indexes = [list(state.name).index(name) for name in JOINT_NAMES]
+        current = [state.position[i] for i in indexes]
+        velocity = [state.velocity[i] for i in indexes]
+    except (ValueError, IndexError):
+        raise HTTPException(409, 'Complete named position and velocity feedback required')
+    if any(not math.isfinite(x) for x in current + velocity) or any(abs(v) > stopped for v in velocity):
+        raise HTTPException(409, 'Direct trajectories must start from a stationary robot')
+    return current, tolerance
+
+
+def _build_safe_msg(rows):
+    bridge = get_bridge()
+    current, tolerance = _stationary_state()
+    path = bridge.get_parameter('joint_limits_path').value
+    resolved = Path(path) if path else _resolve_path('iiwa_config', 'config/moveit/joint_limits.yaml')
+    data = yaml.safe_load(resolved.read_text())['joint_limits']
+    caps = []
+    for name in JOINT_NAMES:
+        j = data[name]
+        caps.append([max(j['min_position'], j.get('soft_min_position', j['min_position'])),
+                     min(j['max_position'], j.get('soft_max_position', j['max_position'])),
+                     j['max_velocity'], j['max_acceleration'], j['max_jerk']])
+    rows = [(list(q), float(t)) for q, t in rows]
+    if rows[0][1] > 0:
+        rows.insert(0, (current, 0.0))
+    elif rows[0][1] == 0 and max(abs(a-b) for a,b in zip(rows[0][0],current)) > tolerance:
+        raise HTTPException(422, 'A waypoint at t=0 must match the current joint positions')
+    else:
+        rows[0] = (current, rows[0][1])
+    try:
+        times, positions, velocities, accelerations, scale = prepare_trajectory(
+            [t for _, t in rows], [q for q, _ in rows], caps)
+    except (ValueError, RuntimeError) as exc:
+        raise HTTPException(422, str(exc)) from exc
+    msg = _build_msg(list(zip(positions, times)))
+    for point, v, a in zip(msg.points, velocities, accelerations):
+        point.velocities = v
+        point.accelerations = a
+    return msg, times[-1], scale
+
+
+def _send_rows(rows):
+    global _direct_until
+    with _prepare_lock:
+        with _admission_lock:
+            generation = _generation
+            if time.monotonic() < _direct_until:
+                raise HTTPException(409, 'A direct trajectory is still active; stop it before replacing it')
+        msg, duration, scale = _build_safe_msg(rows)
+        with _admission_lock:
+            if generation != _generation:
+                raise HTTPException(409, 'Trajectory preparation was canceled by stop')
+            current, tolerance = _stationary_state()
+            if max(abs(a-b) for a,b in zip(current, msg.points[0].positions)) > tolerance:
+                raise HTTPException(409, 'Robot moved during trajectory preparation; retry from its new state')
+            _publish(msg)
+            _direct_until = time.monotonic() + duration + get_bridge().get_parameter('trajectory_state_timeout').value
+        return msg, duration, scale
+
+
 def _publish(msg: JointTrajectory) -> None:
    get_bridge().publish(TOPIC, JointTrajectory, msg)

@@ -80,9 +165,9 @@ def send_trajectory(req: SendRequest):
    if req.validate_limits:
        _validate_limits([r[0] for r in rows])

-    _publish(_build_msg(rows))
+    msg, duration, time_scale = _send_rows(rows)
    _log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
-    return {"status": "sent", "points": len(rows)}
+    return {"status": "sent", "points": len(msg.points), "duration": duration, "time_scale": time_scale}


@router.post("/send_csv", summary="Загрузить CSV и отправить траекторию")
@@ -166,26 +251,30 @@ async def send_csv_trajectory(
    if validate_limits:
        _validate_limits([r[0] for r in rows])

-    _publish(_build_msg(rows))
+    msg, duration, time_scale = _send_rows(rows)
    _log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
-    return {"status": "sent", "points": len(rows), "filename": file.filename}
+    return {"status": "sent", "points": len(msg.points), "filename": file.filename, "duration": duration, "time_scale": time_scale}


 def send_stop_trajectory() -> None:
-    """Publish a hold-position (or empty) trajectory to freeze joint motion."""
-    bridge = get_bridge()
-    joint_states = bridge.get_latest("/joint_states")
-    if joint_states is not None and len(joint_states.position) >= N_JOINTS:
-        current_positions = list(joint_states.position[:N_JOINTS])
-        hold_msg = _build_msg([(current_positions, 0.5)])
-        _publish(hold_msg)
-        _log("[stop] отправлена точка удержания текущей позиции")
-    else:
-        msg = JointTrajectory()
-        msg.joint_names = JOINT_NAMES
-        _publish(msg)
-        _log("[stop] joint_states недоступны, отправлена пустая траектория")
-
+    """Cancel preparation and publish hold; malformed feedback falls back to cancellation."""
+    global _generation, _direct_until
+    with _admission_lock:
+        _generation += 1
+        _direct_until = 0.0
+        bridge = get_bridge()
+        state = bridge.get_latest('/joint_states')
+        try:
+            current = [state.position[list(state.name).index(name)] for name in JOINT_NAMES]
+            if not all(math.isfinite(x) for x in current):
+                raise ValueError('Non-finite feedback')
+            _publish(_build_msg([(current, 0.5)]))
+            _log('[stop] отправлена точка удержания текущей позиции')
+        except (AttributeError, ValueError, IndexError):
+            msg = JointTrajectory()
+            msg.joint_names = JOINT_NAMES
+            _publish(msg)
+            _log('[stop] отправлена пустая траектория: неполная обратная связь')


@router.get("/logs", summary="Последние лог-записи траекторного модуля")
@@ -14,6 +14,8 @@
  <exec_depend>tf2_ros</exec_depend>
  <exec_depend>tf2_py</exec_depend>
  <exec_depend>moveit_msgs</exec_depend>
+  <exec_depend>iiwa_planning</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>

  <test_depend>ament_copyright</test_depend>
  <test_depend>ament_flake8</test_depend>
@@ -19,6 +19,7 @@ setup(
        'python-multipart',
        'fastmcp',
    ],
+    tests_require=['pytest'],
    zip_safe=True,
    maintainer='daniel',
    maintainer_email='grabardm@ml-dev.ru',
@@ -0,0 +1,75 @@
+import time
+from pathlib import Path
+from types import SimpleNamespace
+import pytest
+from sensor_msgs.msg import JointState
+from fastapi import HTTPException
+from iiwa_web import trajectory as api
+
+
+@pytest.fixture
+def bridge(monkeypatch):
+    msg = JointState()
+    msg.name = [f'joint{i}' for i in range(7,0,-1)]
+    msg.position = [0.] * 7
+    msg.velocity = [0.] * 7
+    msg.header.stamp.sec = 100
+    params = dict(trajectory_state_timeout=.5, trajectory_start_tolerance=.001,
+                  trajectory_stopped_velocity=.01,
+                  joint_limits_path=str(Path(__file__).resolve().parents[2] / 'iiwa_config/config/moveit/joint_limits.yaml'))
+    published = []
+    node = SimpleNamespace(
+        state=msg, age=0., published=published,
+        get_parameter=lambda key: SimpleNamespace(value=params[key]),
+        get_clock=lambda: SimpleNamespace(now=lambda: SimpleNamespace(nanoseconds=100_000_000_000)),
+        publish=lambda topic, typ, payload: published.append(payload))
+    node.get_latest = lambda topic: node.state
+    node.get_latest_with_age = lambda topic: (node.state, node.age)
+    monkeypatch.setattr(api, 'get_bridge', lambda: node)
+    monkeypatch.setattr(api, '_direct_until', 0.)
+    return node
+
+
+def test_stale_and_moving_state_not_published(bridge):
+    for age, speed in [(1.,0.),(0.,.2)]:
+        bridge.age=age; bridge.state.velocity[0]=speed
+        with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert not bridge.published
+
+
+def test_normal_preparation_and_duplicate_admission(bridge):
+    msg, duration, scale = api._send_rows([([.1]*7,.1)])
+    assert duration > .1 and scale > 1
+    assert list(msg.points[0].positions) == [0.]*7
+    assert list(msg.points[-1].positions) == [.1]*7
+    assert len(msg.points[1].accelerations) == 7
+    with pytest.raises(HTTPException): api._send_rows([([.2]*7,1.)])
+    assert len(bridge.published) == 1
+
+
+def test_state_rechecked_after_preparation(bridge, monkeypatch):
+    original=api.prepare_trajectory
+    def prepare(*args):
+        result=original(*args)
+        bridge.state.position[0]=.02
+        return result
+    monkeypatch.setattr(api,'prepare_trajectory',prepare)
+    with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert not bridge.published
+
+
+def test_stop_cancels_inflight_preparation(bridge, monkeypatch):
+    original=api.prepare_trajectory
+    def prepare(*args):
+        result=original(*args)
+        api.send_stop_trajectory()
+        return result
+    monkeypatch.setattr(api,'prepare_trajectory',prepare)
+    with pytest.raises(HTTPException): api._send_rows([([.1]*7,1.)])
+    assert len(bridge.published)==1  # stop only
+
+
+def test_stop_handles_missing_named_feedback(bridge):
+    bridge.state.name=[]
+    api.send_stop_trajectory()
+    assert len(bridge.published)==1 and not bridge.published[0].points