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

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

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

src/iiwa_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

@@ -63,6 +63,8 @@ class PlanningCfg:
     default_frame: str
     default_planner: str
     planning_attempts: int
+    stopped_velocity_tolerance: float = 0.01
+    state_timeout: float = 0.5
 
 
 @dataclass(frozen=True)
@@ -77,6 +79,9 @@ class WebCfg:
     port: int
     endpoints: str    # resolved absolute path to api_endpoints.yaml
     joint_limits: str  # resolved absolute path to joint_limits.yaml
+    trajectory_state_timeout: float = 0.5
+    trajectory_start_tolerance: float = 0.001
+    trajectory_stopped_velocity: float = 0.01
 
 
 @dataclass(frozen=True)
@@ -191,6 +196,9 @@ _WEB_DEFAULTS: Dict[str, Any] = {
     "port": 8007,
     "endpoints": "pkg://iiwa_config/config/api_endpoints.yaml",
     "joint_limits": "pkg://iiwa_config/config/moveit/joint_limits.yaml",
+    "trajectory_state_timeout": 0.5,
+    "trajectory_start_tolerance": 0.001,
+    "trajectory_stopped_velocity": 0.01,
 }
 
 
@@ -207,6 +215,9 @@ def _parse_web(raw: Optional[Dict[str, Any]], settings_dir: str) -> WebCfg:
         port=int(get("port")),
         endpoints=resolve_path(str(get("endpoints")), settings_dir),
         joint_limits=resolve_path(str(get("joint_limits")), settings_dir),
+        trajectory_state_timeout=float(get("trajectory_state_timeout")),
+        trajectory_start_tolerance=float(get("trajectory_start_tolerance")),
+        trajectory_stopped_velocity=float(get("trajectory_stopped_velocity")),
     )
 
 
@@ -342,6 +353,8 @@ def build_settings(settings_path: str, check_files: bool = True) -> Settings:
         default_frame=str(planning_raw.get("default_frame", "base_link")),
         default_planner=str(planning_raw.get("default_planner", "ompl")),
         planning_attempts=int(planning_raw.get("planning_attempts", 3)),
+        stopped_velocity_tolerance=float(planning_raw.get("stopped_velocity_tolerance", 0.01)),
+        state_timeout=float(planning_raw.get("state_timeout", 0.5)),
     )
 
     # tool

src/iiwa_utils/setup.py

@@ -12,6 +12,7 @@ setup(
         ('share/' + package_name, ['package.xml']),
     ],
     install_requires=['setuptools'],
+    tests_require=['pytest'],
     zip_safe=True,
     maintainer='daniel',
     maintainer_email='grabardm@ml-dev.ru',

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