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.
This commit is contained in:
Даниил Грабарь
2026-09-21 05:16:09 +03:00
parent 9f0f05df87
commit 0a091ad521
43 changed files with 1342 additions and 137 deletions
@@ -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