Добавлен stop в mcp

Preserve dev TCP gizmo motion-priority handling while integrating Ruckig smoothing, continuous JTC spline validation and shared robot limits. Resolve launch and dependency conflicts without removing either feature set. Verified merged build of 8 packages and tests of 4 packages.

cobot-setting.yaml

@@ -3,7 +3,7 @@ robot:
   ip: "192.168.21.147"
   port: 30200
   fri_cycle_ms: 10          # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц)
-  joint_position_tau: 0.04  # EMA позиций FRI [с]; 0 отключает сглаживание
+  joint_position_tau: 0.06  # EMA позиций FRI [с]; 0 отключает сглаживание
   description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro
 
 
@@ -70,6 +70,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
@@ -78,6 +80,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)

doc/lwc-doc/docs/realtime-fri.en.md

@@ -82,3 +82,23 @@ sudo gpasswd -d "$USER" realtime
 After logging in again, processes use the regular Linux scheduler.
 
 See the official [ros2_control documentation](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism) for controller-manager priorities and limits.
+
+## Trajectory limits and smoothness
+
+`controller.moveit.joint_limits` in `cobot-setting.yaml` selects a single limits file for all seven axes, consumed by xacro, FRI, Webots, MoveIt and the web API. Hard position limits and the existing narrower protective bounds remain distinct. Command validation and planning use their intersection; the MoveIt parameter override explicitly preserves soft bounds.
+
+Position and velocity limits were checked against the [KUKA LBR iiwa 7 R800 datasheet, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Working limits were not expanded relative to the previous URDF. Upward rounding beyond published values was removed. **Acceleration and jerk limits remain project settings, not verified KUKA ratings.** Tool/payload and Sunrise settings still require independent verification.
+
+OMPL/CHOMP now run TOTG → rest-boundary normalization → Ruckig → rest-boundary normalization → `iiwa_planning/LimitSplineDynamics` → MoveIt solution validation. The initial requested state must be stationary within `planning.stopped_velocity_tolerance` (rad/s). The spline adapter bounds the complete JTC quintic polynomial in Bernstein form, including extrema between waypoints. It uniformly stretches time and scales derivatives to respect velocity, acceleration and jerk limits, rejecting position overshoot. Endpoint acceleration is normalized around Ruckig because TOTG and Ruckig duration extension can leave nonzero boundary derivatives.
+
+Pilz PTP/LIN/CIRC use the spline validator and uniform time scaling without joint-space Ruckig. This preserves the original JTC interpolation curve, not a new guarantee of ideal Cartesian interpolation. Nonstationary boundary conditions, including some blended sequences, are rejected; this implementation targets rest-to-rest motions.
+
+Position-only CSV/JSON `/trajectory/send*` requests are fitted with a smooth rest-to-rest spline and passed through the same C++ validator. Input waypoints are retained; the curve between them can differ from the former linear interpolation. This direct API does not check collisions. Responses include actual `duration` and `time_scale`; timing may increase. `validate_limits=false` cannot disable mandatory dynamics and operating-range checks.
+
+Direct commands require fresh, complete named joint position/velocity feedback and a stationary robot. The first point at zero time must match the measured position within `web.trajectory_start_tolerance`; otherwise a measured start point is prepended if the first timestamp is positive. Freshness and stationarity are checked again after preparation. `web.trajectory_state_timeout` is in seconds; `web.trajectory_stopped_velocity` is in rad/s. Concurrent direct commands are rejected while a trajectory is reserved; stopping cancels pending preparation.
+
+External clients sending directly to the JTC topic/action bypass this MoveIt/web preparation and remain responsible for their trajectories. This is not a global ROS command firewall. The FRI filter, watchdog and cycle period are unchanged. Hardware retesting is needed to assess noise and actual tracking.
+
+After rebuilding, run `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Tests exercise seven-axis consistency, spline extrema, the installed JTC interpolator, installed TOTG/Ruckig plus pluginlib, and web admission failures without connecting to a real FRI session. Rebuild `iiwa_config`, `iiwa_description` and `iiwa_bringup` as well; running processes do not update automatically.
+
+The adapters check original `/joint_states` telemetry: absent MoveIt `RobotState` velocities are not treated as zero. `planning.state_timeout` bounds telemetry age. The subscription test uses synthetic messages in separate local ROS domain 231, without FRI.

doc/lwc-doc/docs/realtime-fri.md

@@ -82,3 +82,32 @@ sudo gpasswd -d "$USER" realtime
 После повторного входа процессы снова будут работать с обычным планировщиком Linux.
 
 Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism).
+
+## Ограничения траекторий и плавность
+
+`controller.moveit.joint_limits` из `cobot-setting.yaml` задаёт единый файл для всех семи осей. Его читают URDF/xacro, FRI, Webots, MoveIt и web API. `min_position/max_position` описывают рабочие жёсткие пределы; `soft_min_position/soft_max_position` сохраняют более узкие защитные диапазоны. Планирование, командные интерфейсы и проверка API используют пересечение этих диапазонов. При загрузке MoveIt пересечение применяется явно: YAML иначе может переопределить мягкие пределы URDF.
+
+Диапазоны и скорости сверены с [листом KUKA LBR iiwa 7 R800, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Для осей A1…A7 паспортные диапазоны составляют ±170°, ±120°, ±170°, ±120°, ±170°, ±120°, ±175°; скорости с номинальной нагрузкой — 98, 98, 100, 130, 140, 180, 180°/с. Рабочие пределы не расширены относительно прежнего URDF: округления, превышавшие паспортные значения, уменьшены. **Ускорения и рывки в YAML остаются ограничениями проекта, а не подтверждёнными паспортными пределами KUKA.** Данные установленного инструмента, нагрузки и настройки Sunrise по этому листу не проверяются.
+
+Для OMPL/CHOMP обработка выполняется в порядке:
+
+1. TOTG назначает время движения.
+2. `iiwa_planning/SetRestBoundary` проверяет скорость исходного состояния и задаёт нулевые скорости/ускорения на концах движения из покоя.
+3. Ruckig сглаживает траекторию с ограничением рывка.
+4. Повторная установка граничных условий устраняет изменения производных, которые может внести продление длительности в Ruckig.
+5. `iiwa_planning/LimitSplineDynamics` проверяет весь квинтический сплайн JTC, включая промежутки между точками. Границы полиномов вычисляются в базисе Бернштейна, а не только по временной сетке. При превышении скорости, ускорения или рывка увеличивается время всей траектории; скорости и ускорения масштабируются согласованно. Выход позиции за рабочие пределы отклоняется.
+6. MoveIt проверяет решение на столкновения обычным адаптером `ValidateSolution`.
+
+Pilz PTP/LIN/CIRC проходят проверку сплайна и равномерное увеличение длительности без Ruckig в суставном пространстве. При этом сохраняется исходная кривая интерполяции JTC; точность геометрии LIN/CIRC относительно непрерывного идеального пути отдельно не улучшается. Движения с ненулевыми граничными скоростями/ускорениями (включая некоторые варианты blending) отклоняются: проверка предназначена для перемещений из покоя в покой. `planning.stopped_velocity_tolerance` задаёт порог определения неподвижного начального состояния в рад/с.
+
+CSV/JSON `/trajectory/send*` по-прежнему принимают позиции и время, но теперь строят гладкий сплайн с нулевыми граничными производными и проходят тот же C++-валидатор. Промежуточные точки сохраняются, однако кривая между ними может отличаться от прежней линейной интерполяции. Прямой API не выполняет проверку столкновений. Ответ возвращает фактические `duration` и `time_scale`; времена могут увеличиться. `validate_limits=false` не отключает обязательную проверку динамики и рабочего диапазона.
+
+Перед отправкой требуется свежая обратная связь с именами осей, положениями и скоростями; робот должен быть неподвижен. Точка при `t=0` должна совпадать с текущим положением в пределах `web.trajectory_start_tolerance`. Если первая точка позже нуля, текущее положение добавляется в начало. `web.trajectory_state_timeout` ограничивает возраст состояния; `web.trajectory_stopped_velocity` — начальную скорость. Состояние повторно проверяется после расчёта. Повторная прямая отправка во время активной траектории отклоняется; остановка отменяет ещё не завершённую подготовку.
+
+Внешний ROS-клиент, публикующий прямо в топик JTC или отправляющий ему action, **обходит подготовку MoveIt/web**. Он обязан самостоятельно формировать корректные траектории; это изменение не является общей защитой от произвольных ROS-команд. FRI-фильтр, watchdog и период обмена не менялись. Повторное аппаратное испытание необходимо для оценки шума и фактического слежения.
+
+### Проверка без оборудования
+
+После сборки можно выполнить `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Тесты проверяют согласованность семи осей в реальном и симуляционном URDF, пики между точками, результат установленного интерполятора JTC, связку установленного TOTG/Ruckig с pluginlib и отклонение устаревшего состояния web. Они не подключаются к реальному FRI-сеансу. Для нового кода требуется пересборка перечисленных пакетов и `iiwa_config`, `iiwa_description`, `iiwa_bringup`; старый запущенный процесс автоматически не обновляется.
+
+Проверка покоя в адаптерах подписывается на исходный `/joint_states`: отсутствие скоростей в `RobotState` MoveIt не трактуется как нулевая скорость. `planning.state_timeout` ограничивает возраст исходной телеметрии. Тест подписки использует отдельный локальный ROS domain 231 и синтетические сообщения, без FRI.

rosdep.yaml

@@ -37,3 +37,7 @@ python3-numpy:
   ubuntu: [python3-numpy]
 python3-scipy:
   ubuntu: [python3-scipy]
+
+# Python binding for the shared JTC spline validator.
+pybind11-dev:
+  ubuntu: [pybind11-dev]

src/iiwa_bringup/launch/iiwa.launch.py

@@ -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),
@@ -91,6 +93,7 @@ def _runtime_setup(context, *args, **kwargs):
                 "controller": str(settings.controller.controller_path),
                 "initial_positions_file": str(settings.controller.moveit.initial_positions),
                 "tcp_gizmo_enabled": str(settings.digital_twin.tcp_gizmo.enabled).lower(),
+                "joint_limits_file": str(settings.controller.moveit.joint_limits),
             }.items(),
         ))
 
@@ -102,6 +105,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),
@@ -113,6 +117,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),

src/iiwa_bringup/launch/supported/controllers.launch.py

@@ -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"

src/iiwa_bringup/launch/supported/moveit_nodes.py

@@ -1,6 +1,7 @@
 from launch_ros.actions import Node
 from moveit_configs_utils import MoveItConfigsBuilder
 from .tcp_gizmo_nodes import make_tcp_gizmo_nodes
+from iiwa_utils.converter import effective_joint_limits
 
 
 def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
@@ -10,7 +11,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)
@@ -22,10 +24,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(

src/iiwa_bringup/launch/supported/optional_nodes.py

@@ -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,
         }],
     )

src/iiwa_bringup/launch/supported/webots_spawn.launch.py

@@ -30,6 +30,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),
         },
     )
 

src/iiwa_config/config/chomp_planning.yaml

@@ -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

src/iiwa_config/config/moveit/joint_limits.yaml

@@ -1,73 +1,84 @@
-# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
+# 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):
-# For beginners, we downscale velocity and acceleration limits.
+# range [deg]: 170,120,170,120,170,120,175; speed [deg/s]: 98,98,100,130,140,180,180.
-# 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.
+# 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
+    min_position: -2.9670597283903604
-    max_position: 2.97
+    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
+    min_position: -2.09
-    max_position: 2.10
+    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
+    min_position: -2.9670597283903604
-    max_position: 2.97
+    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
+    min_position: -2.09
-    max_position: 2.10
+    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
+    min_position: -2.9670597283903604
-    max_position: 2.97
+    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
+    min_position: -2.09
-    max_position: 2.10
+    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

src/iiwa_config/config/ompl_planning.yaml

@@ -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

src/iiwa_config/config/pilz_industrial_motion_planner_planning.yaml

@@ -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

src/iiwa_config/config/setting.yaml

@@ -70,6 +70,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
@@ -78,6 +80,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)

src/iiwa_controller/src/IIWAHardwareInterface.cpp

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

src/iiwa_controller/test/test_hardware.cpp

@@ -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);
+}

src/iiwa_description/urdf/iiwa7.urdf.xacro

@@ -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"/>
@@ -72,8 +74,8 @@
 
             <joint name="joint1">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
-                    <param name="max"> 2.97</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>
@@ -84,8 +86,8 @@
 
             <joint name="joint2">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
-                    <param name="max"> 2.09</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>
@@ -96,8 +98,8 @@
 
             <joint name="joint3">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
-                    <param name="max"> 2.97</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>
@@ -108,8 +110,8 @@
 
             <joint name="joint4">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
-                    <param name="max"> 2.09</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>
@@ -120,8 +122,8 @@
 
             <joint name="joint5">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
-                    <param name="max"> 2.97</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>
@@ -132,8 +134,8 @@
 
             <joint name="joint6">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
-                    <param name="max"> 2.09</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>
@@ -144,8 +146,8 @@
 
             <joint name="joint7">
                 <command_interface name="position">
-                    <param name="min">-3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
-                    <param name="max"> 3.05</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>
@@ -173,8 +175,8 @@
 
             <joint name="joint1">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint1']['soft_min_position']}</param>
-                    <param name="max"> 2.97</param>
+                    <param name="max">${joint_limits['joint1']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -186,8 +188,8 @@
 
             <joint name="joint2">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint2']['soft_min_position']}</param>
-                    <param name="max"> 2.09</param>
+                    <param name="max">${joint_limits['joint2']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -199,8 +201,8 @@
 
             <joint name="joint3">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint3']['soft_min_position']}</param>
-                    <param name="max"> 2.97</param>
+                    <param name="max">${joint_limits['joint3']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -212,8 +214,8 @@
 
             <joint name="joint4">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint4']['soft_min_position']}</param>
-                    <param name="max"> 2.09</param>
+                    <param name="max">${joint_limits['joint4']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -225,8 +227,8 @@
 
             <joint name="joint5">
                 <command_interface name="position">
-                    <param name="min">-2.97</param>
+                    <param name="min">${joint_limits['joint5']['soft_min_position']}</param>
-                    <param name="max"> 2.97</param>
+                    <param name="max">${joint_limits['joint5']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -238,8 +240,8 @@
 
             <joint name="joint6">
                 <command_interface name="position">
-                    <param name="min">-2.09</param>
+                    <param name="min">${joint_limits['joint6']['soft_min_position']}</param>
-                    <param name="max"> 2.09</param>
+                    <param name="max">${joint_limits['joint6']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">
@@ -251,8 +253,8 @@
 
             <joint name="joint7">
                 <command_interface name="position">
-                    <param name="min">-3.05</param>
+                    <param name="min">${joint_limits['joint7']['soft_min_position']}</param>
-                    <param name="max"> 3.05</param>
+                    <param name="max">${joint_limits['joint7']['soft_max_position']}</param>
                 </command_interface>
 
                 <state_interface name="position">

src/iiwa_description/urdf/joints.xacro

@@ -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"
+                        lower="${joint_limits['joint1']['min_position']}" upper="${joint_limits['joint1']['max_position']}" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint1']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint2']['min_position']}" upper="${joint_limits['joint2']['max_position']}" effort="200"
-                        velocity="1.71" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint2']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint3']['min_position']}" upper="${joint_limits['joint3']['max_position']}" effort="200"
-                        velocity="1.75" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint3']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint4']['min_position']}" upper="${joint_limits['joint4']['max_position']}" effort="200"
-                        velocity="2.27" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint4']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint5']['min_position']}" upper="${joint_limits['joint5']['max_position']}" effort="200"
-                        velocity="2.44" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint5']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint6']['min_position']}" upper="${joint_limits['joint6']['max_position']}" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint6']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
+                        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"
+                        lower="${joint_limits['joint7']['min_position']}" upper="${joint_limits['joint7']['max_position']}" effort="200"
-                        velocity="3.14" damping="10.0" friction="0.1"
+                        velocity="${joint_limits['joint7']['max_velocity']}" damping="10.0" friction="0.1"
-                        soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/>
+                        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"/>

src/iiwa_planning/CMakeLists.txt

@@ -28,6 +28,50 @@ target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
 target_include_directories(tcp_gizmo PRIVATE include)
 ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
   action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
+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

@@ -32,7 +32,13 @@
   <depend>action_msgs</depend>
   <depend>rcl_interfaces</depend>
   <depend>tf2_eigen</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_cmake_gtest</test_depend>
   <test_depend>ament_cmake_pytest</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.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
-      "x": 0.6,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
-      "y": 0.1,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
-      "z": 0.55,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
-      "a": 3.14,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
-      "b": 0.3142,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
-      "c": 2.7916,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
-      "speed": 0.4,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
-      "planner": "ptp"
+    {"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.6,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
-      "y": -0.1,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
-      "z": 0.55,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
-      "a": 3.14,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
-      "b": 0.3142,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
-      "c": 2.7916,
+    {"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
-      "speed": 0.3,
+    {"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
-      "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"},
-    {
+
-      "joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
-      "speed": 0.2
+    {"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.58,
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
-      "y": 0.0,
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
-      "z": 0.5,
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
-      "a": 3.14,
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
-      "b": 0.3665,
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
-      "c": 2.8791,
+    {"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
-      "speed": 0.3,
+    {"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
-      "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/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";
+}

src/iiwa_utils/iiwa_utils/converter.py

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

src/iiwa_utils/iiwa_utils/setting_loader.py

@@ -77,6 +77,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)
@@ -91,6 +93,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)
@@ -223,6 +228,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,
 }
 
 
@@ -239,6 +247,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")),
     )
 
 
@@ -382,6 +393,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

src/iiwa_utils/test/test_joint_limits_consistency.py

@@ -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

src/iiwa_web/iiwa_web/config_loader.py

@@ -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
 

src/iiwa_web/iiwa_web/main.py

@@ -3,7 +3,7 @@ from contextlib import asynccontextmanager
 
 import rclpy
 import uvicorn
-from fastapi import FastAPI
+from fastapi import FastAPI, APIRouter
 from fastmcp import FastMCP
 from sensor_msgs.msg import JointState
 from std_srvs.srv import Trigger
@@ -13,6 +13,34 @@ from .ros_node import CobotWebNode, get_bridge, set_bridge
 from . import runner, trajectory, positions
 
 
+stop_router = APIRouter(tags=["stop"])
+
+@stop_router.post("/stop", summary="Остановить всё: runner, траекторию и планировщик")
+def stop_all():
+    runner.stop_if_running()
+    trajectory.send_stop_trajectory()
+
+    try:
+        result = get_bridge().call_service(
+            Trigger,
+            "cobot/stop",
+            Trigger.Request()
+        )
+
+        return {
+            "status": "stopped",
+            "success": result.success,
+            "message": result.message,
+        }
+
+    except RuntimeError:
+        return {
+            "status": "stopped",
+            "success": True,
+            "message": "Планировщик не запущен",
+        }
+
+
 def main():
     rclpy.init()
     node = CobotWebNode()
@@ -31,9 +59,10 @@ def main():
     _schema_app.include_router(runner.router)
     _schema_app.include_router(trajectory.router)
     _schema_app.include_router(positions.router)
+    _schema_app.include_router(stop_router)
 
     mcp = FastMCP.from_fastapi(app=_schema_app)
-    mcp_http = mcp.http_app(path='/mcp')
+    mcp_http = mcp.http_app(path='/')
 
     @asynccontextmanager
     async def lifespan(_: FastAPI):
@@ -46,18 +75,9 @@ def main():
     app.include_router(runner.router)
     app.include_router(trajectory.router)
     app.include_router(positions.router)
+    app.include_router(stop_router)
     app.mount("/mcp", mcp_http)
 
-    @app.post("/stop", tags=["stop"], summary="Остановить всё: runner, траекторию и планировщик")
-    def stop_all():
-        runner.stop_if_running()
-        trajectory.send_stop_trajectory()
-        try:
-            result = get_bridge().call_service(Trigger, "cobot/stop", Trigger.Request())
-            return {"status": "stopped", "success": result.success, "message": result.message}
-        except RuntimeError:
-            return {"status": "stopped", "success": True, "message": "Планировщик не запущен"}
-
     uvicorn.run(app, host=host, port=port)
 
 

src/iiwa_web/iiwa_web/ros_node.py

@@ -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:

src/iiwa_web/iiwa_web/trajectory.py

@@ -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)
+    total_ns = round(seconds * 1e9)
-    nanosec = int(round((seconds - sec) * 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."""
+    """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()
-    joint_states = bridge.get_latest("/joint_states")
+        state = bridge.get_latest('/joint_states')
-    if joint_states is not None and len(joint_states.position) >= N_JOINTS:
+        try:
-        current_positions = list(joint_states.position[:N_JOINTS])
+            current = [state.position[list(state.name).index(name)] for name in JOINT_NAMES]
-        hold_msg = _build_msg([(current_positions, 0.5)])
+            if not all(math.isfinite(x) for x in current):
-        _publish(hold_msg)
+                raise ValueError('Non-finite feedback')
-        _log("[stop] отправлена точка удержания текущей позиции")
+            _publish(_build_msg([(current, 0.5)]))
-    else:
+            _log('[stop] отправлена точка удержания текущей позиции')
+        except (AttributeError, ValueError, IndexError):
             msg = JointTrajectory()
             msg.joint_names = JOINT_NAMES
             _publish(msg)
-        _log("[stop] joint_states недоступны, отправлена пустая траектория")
+            _log('[stop] отправлена пустая траектория: неполная обратная связь')
-
 
 
 @router.get("/logs", summary="Последние лог-записи траекторного модуля")

src/iiwa_web/package.xml

@@ -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>

src/iiwa_web/setup.py

@@ -19,6 +19,7 @@ setup(
         'python-multipart',
         'fastmcp',
     ],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',

src/iiwa_web/test/test_trajectory_admission.py

@@ -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