Merge controller into dev: bounded trajectories and shared seven-axis limits

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.

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