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:
@@ -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),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
Reference in New Issue
Block a user