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

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

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

+  # Direct CSV/JSON trajectories require fresh feedback and a stationary start.
+  trajectory_state_timeout: 0.5       # s
+  trajectory_start_tolerance: 0.001   # rad
+  trajectory_stopped_velocity: 0.01  # rad/s
+
 foxglove:
  enabled: true  # Запускать ли foxglove_bridge вместе с роботом
  port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)