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
@@ -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"
@@ -1,5 +1,6 @@
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
from iiwa_utils.converter import effective_joint_limits
def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
@@ -9,7 +10,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)
@@ -21,10 +23,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(
@@ -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,
}],
)
@@ -29,6 +29,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),
},
)