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.
This commit is contained in:
Даниил Грабарь
@@ -43,11 +43,13 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
if simulate:
|
||||
xacro_args = {
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions,
|
||||
"joint_limits_file": settings.controller.moveit.joint_limits,
|
||||
"simulate": "true",
|
||||
}
|
||||
else:
|
||||
xacro_args = {
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions,
|
||||
"joint_limits_file": settings.controller.moveit.joint_limits,
|
||||
"robot_ip": settings.robot.ip,
|
||||
"fri_port": str(settings.robot.port),
|
||||
"fri_cycle_ms": str(settings.robot.fri_cycle_ms),
|
||||
@@ -91,6 +93,7 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
"controller": str(settings.controller.controller_path),
|
||||
"initial_positions_file": str(settings.controller.moveit.initial_positions),
|
||||
"tcp_gizmo_enabled": str(settings.digital_twin.tcp_gizmo.enabled).lower(),
|
||||
"joint_limits_file": str(settings.controller.moveit.joint_limits),
|
||||
}.items(),
|
||||
))
|
||||
|
||||
@@ -102,6 +105,7 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
"robot_name": settings.robot.name,
|
||||
"description": description_path,
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions,
|
||||
"joint_limits_file": settings.controller.moveit.joint_limits,
|
||||
"controller_path": settings.controller.controller_path,
|
||||
"simulate": "true",
|
||||
"transform": str(settings.digital_twin.webots.transform),
|
||||
@@ -113,6 +117,7 @@ def _runtime_setup(context, *args, **kwargs):
|
||||
"robot_name": settings.robot.name,
|
||||
"description": description_path,
|
||||
"initial_positions_file": settings.controller.moveit.initial_positions,
|
||||
"joint_limits_file": settings.controller.moveit.joint_limits,
|
||||
"controller_path": settings.controller.controller_path,
|
||||
"simulate": "false",
|
||||
"transform": str(settings.digital_twin.webots.transform),
|
||||
|
||||
@@ -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,6 +1,7 @@
|
||||
from launch_ros.actions import Node
|
||||
from moveit_configs_utils import MoveItConfigsBuilder
|
||||
from .tcp_gizmo_nodes import make_tcp_gizmo_nodes
|
||||
from iiwa_utils.converter import effective_joint_limits
|
||||
|
||||
|
||||
def make_moveit_nodes(settings, robot_description: str, use_sim_time: bool):
|
||||
@@ -10,7 +11,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)
|
||||
@@ -22,10 +24,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,
|
||||
}],
|
||||
)
|
||||
|
||||
@@ -30,6 +30,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),
|
||||
},
|
||||
)
|
||||
|
||||
|
||||
@@ -0,0 +1,19 @@
|
||||
planning_plugins:
|
||||
- chomp_interface/CHOMPPlanner
|
||||
enable_failure_recovery: true
|
||||
# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
|
||||
request_adapters:
|
||||
- default_planning_request_adapters/ResolveConstraintFrames
|
||||
- default_planning_request_adapters/ValidateWorkspaceBounds
|
||||
- default_planning_request_adapters/CheckStartStateBounds
|
||||
- default_planning_request_adapters/CheckStartStateCollision
|
||||
response_adapters:
|
||||
- default_planning_response_adapters/AddTimeOptimalParameterization
|
||||
- iiwa_planning/SetRestBoundary
|
||||
- default_planning_response_adapters/AddRuckigTrajectorySmoothing
|
||||
- iiwa_planning/SetRestBoundary
|
||||
- iiwa_planning/LimitSplineDynamics
|
||||
- default_planning_response_adapters/ValidateSolution
|
||||
- default_planning_response_adapters/DisplayMotionPath
|
||||
|
||||
ridge_factor: 0.01
|
||||
@@ -1,73 +1,84 @@
|
||||
# joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed
|
||||
|
||||
# For beginners, we downscale velocity and acceleration limits.
|
||||
# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed.
|
||||
# Single source for all seven axes: URDF, ros2_control, MoveIt and web validation.
|
||||
# KUKA iiwa 7 R800 datasheet 0000-246-832 V1.2 (2022-06-01):
|
||||
# range [deg]: 170,120,170,120,170,120,175; speed [deg/s]: 98,98,100,130,140,180,180.
|
||||
# Working hard limits never exceed either that datasheet or the previous URDF.
|
||||
# Soft bounds preserve the existing protective operating margin.
|
||||
# Acceleration and jerk below are PROJECT limits, NOT verified KUKA ratings.
|
||||
default_velocity_scaling_factor: 0.1
|
||||
default_acceleration_scaling_factor: 0.1
|
||||
|
||||
# Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration]
|
||||
# Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits]
|
||||
joint_limits:
|
||||
joint1:
|
||||
has_position_limits: true
|
||||
min_position: -2.97
|
||||
max_position: 2.97
|
||||
min_position: -2.9670597283903604
|
||||
max_position: 2.9670597283903604
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.71
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 8.5521
|
||||
has_jerk_limits: true
|
||||
max_jerk: 85.0
|
||||
soft_min_position: -2.8827
|
||||
soft_max_position: 2.8827
|
||||
joint2:
|
||||
has_position_limits: true
|
||||
min_position: -2.10
|
||||
max_position: 2.10
|
||||
min_position: -2.09
|
||||
max_position: 2.09
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.71
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 8.5521
|
||||
has_jerk_limits: true
|
||||
max_jerk: 85.0
|
||||
soft_min_position: -2.0027
|
||||
soft_max_position: 2.0027
|
||||
joint3:
|
||||
has_position_limits: true
|
||||
min_position: -2.97
|
||||
max_position: 2.97
|
||||
min_position: -2.9670597283903604
|
||||
max_position: 2.9670597283903604
|
||||
has_velocity_limits: true
|
||||
max_velocity: 1.75
|
||||
max_velocity: 1.7453292519943295
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 8.7266
|
||||
has_jerk_limits: true
|
||||
max_jerk: 87.0
|
||||
soft_min_position: -2.8827
|
||||
soft_max_position: 2.8827
|
||||
joint4:
|
||||
has_position_limits: true
|
||||
min_position: -2.10
|
||||
max_position: 2.10
|
||||
min_position: -2.09
|
||||
max_position: 2.09
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2.27
|
||||
max_velocity: 2.2689280275926285
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 11.3446
|
||||
has_jerk_limits: true
|
||||
max_jerk: 113.0
|
||||
soft_min_position: -2.0027
|
||||
soft_max_position: 2.0027
|
||||
joint5:
|
||||
has_position_limits: true
|
||||
min_position: -2.97
|
||||
max_position: 2.97
|
||||
min_position: -2.9670597283903604
|
||||
max_position: 2.9670597283903604
|
||||
has_velocity_limits: true
|
||||
max_velocity: 2.44
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 12.2173
|
||||
has_jerk_limits: true
|
||||
max_jerk: 122.0
|
||||
soft_min_position: -2.8827
|
||||
soft_max_position: 2.8827
|
||||
joint6:
|
||||
has_position_limits: true
|
||||
min_position: -2.10
|
||||
max_position: 2.10
|
||||
min_position: -2.09
|
||||
max_position: 2.09
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 15.7080
|
||||
max_acceleration: 15.708
|
||||
has_jerk_limits: true
|
||||
max_jerk: 157.0
|
||||
soft_min_position: -2.0027
|
||||
soft_max_position: 2.0027
|
||||
joint7:
|
||||
has_position_limits: true
|
||||
min_position: -3.05
|
||||
@@ -75,7 +86,8 @@ joint_limits:
|
||||
has_velocity_limits: true
|
||||
max_velocity: 3.14
|
||||
has_acceleration_limits: true
|
||||
max_acceleration: 15.7080
|
||||
max_acceleration: 15.708
|
||||
has_jerk_limits: true
|
||||
max_jerk: 157.0
|
||||
|
||||
soft_min_position: -2.9627
|
||||
soft_max_position: 2.9627
|
||||
|
||||
@@ -0,0 +1,16 @@
|
||||
planning_plugins:
|
||||
- ompl_interface/OMPLPlanner
|
||||
# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
|
||||
request_adapters:
|
||||
- default_planning_request_adapters/ResolveConstraintFrames
|
||||
- default_planning_request_adapters/ValidateWorkspaceBounds
|
||||
- default_planning_request_adapters/CheckStartStateBounds
|
||||
- default_planning_request_adapters/CheckStartStateCollision
|
||||
response_adapters:
|
||||
- default_planning_response_adapters/AddTimeOptimalParameterization
|
||||
- iiwa_planning/SetRestBoundary
|
||||
- default_planning_response_adapters/AddRuckigTrajectorySmoothing
|
||||
- iiwa_planning/SetRestBoundary
|
||||
- iiwa_planning/LimitSplineDynamics
|
||||
- default_planning_response_adapters/ValidateSolution
|
||||
- default_planning_response_adapters/DisplayMotionPath
|
||||
@@ -0,0 +1,16 @@
|
||||
planning_plugins:
|
||||
- pilz_industrial_motion_planner/CommandPlanner
|
||||
default_planner_config: PTP
|
||||
request_adapters:
|
||||
- default_planning_request_adapters/ResolveConstraintFrames
|
||||
- default_planning_request_adapters/ValidateWorkspaceBounds
|
||||
- default_planning_request_adapters/CheckStartStateBounds
|
||||
- default_planning_request_adapters/CheckStartStateCollision
|
||||
response_adapters:
|
||||
# Preserve Pilz LIN/CIRC geometry: only uniform time scaling, no joint-space Ruckig.
|
||||
- iiwa_planning/LimitSplineDynamics
|
||||
- default_planning_response_adapters/ValidateSolution
|
||||
- default_planning_response_adapters/DisplayMotionPath
|
||||
capabilities: >-
|
||||
pilz_industrial_motion_planner/MoveGroupSequenceAction
|
||||
pilz_industrial_motion_planner/MoveGroupSequenceService
|
||||
@@ -70,6 +70,8 @@ planning:
|
||||
default_frame: "base_link" # Система отсчёта по умолчанию
|
||||
default_planner: "ompl" # Планировщик по умолчанию
|
||||
planning_attempts: 3 # Число попыток планирования
|
||||
stopped_velocity_tolerance: 0.01 # рад/с: проверка начала движения из покоя
|
||||
state_timeout: 0.5 # с: максимальный возраст joint_states для проверки покоя
|
||||
|
||||
web:
|
||||
enabled: true
|
||||
@@ -78,6 +80,11 @@ web:
|
||||
endpoints: pkg://iiwa_config/config/api_endpoints.yaml
|
||||
joint_limits: pkg://iiwa_config/config/moveit/joint_limits.yaml
|
||||
|
||||
# Direct CSV/JSON trajectories require fresh feedback and a stationary start.
|
||||
trajectory_state_timeout: 0.5 # s
|
||||
trajectory_start_tolerance: 0.001 # rad
|
||||
trajectory_stopped_velocity: 0.01 # rad/s
|
||||
|
||||
foxglove:
|
||||
enabled: true # Запускать ли foxglove_bridge вместе с роботом
|
||||
port: 8765 # WebSocket-порт, к которому подключается Foxglove Studio (по умолчанию 8765)
|
||||
|
||||
@@ -145,6 +145,11 @@ CallbackReturn IIWAHardwareInterface::on_init(
|
||||
}
|
||||
lower_[i] = limits->second.min_position;
|
||||
upper_[i] = limits->second.max_position;
|
||||
const auto soft = info.soft_limits.find(joint.name);
|
||||
if (soft != info.soft_limits.end()) {
|
||||
lower_[i] = std::max(lower_[i], soft->second.min_position);
|
||||
upper_[i] = std::min(upper_[i], soft->second.max_position);
|
||||
}
|
||||
max_velocity_[i] = limits->second.max_velocity;
|
||||
if (!std::isfinite(lower_[i]) || !std::isfinite(upper_[i]) || lower_[i] >= upper_[i] ||
|
||||
!std::isfinite(max_velocity_[i]) || max_velocity_[i] <= 0.0)
|
||||
|
||||
@@ -107,3 +107,14 @@ TEST(Hardware, ConfiguredRealHardwareCleansUpInDependencyOrder)
|
||||
EXPECT_EQ(hardware.on_cleanup(state), Return::SUCCESS);
|
||||
EXPECT_EQ(hardware.on_error(state), Return::SUCCESS);
|
||||
}
|
||||
|
||||
TEST(Hardware, RejectsInconsistentSoftLimits)
|
||||
{
|
||||
Hardware hardware;
|
||||
auto params = parameters();
|
||||
joint_limits::SoftJointLimits soft;
|
||||
soft.min_position = 1.0;
|
||||
soft.max_position = -1.0;
|
||||
params.hardware_info.soft_limits["joint1"] = soft;
|
||||
EXPECT_EQ(hardware.on_init(params), Return::ERROR);
|
||||
}
|
||||
|
||||
@@ -3,6 +3,8 @@
|
||||
|
||||
<xacro:arg name="initial_positions_file"
|
||||
default="$(find iiwa_config)/config/moveit/initial_positions.yaml"/>
|
||||
<xacro:arg name="joint_limits_file" default="$(find iiwa_config)/config/moveit/joint_limits.yaml"/>
|
||||
<xacro:property name="joint_limits" value="${xacro.load_yaml('$(arg joint_limits_file)')['joint_limits']}"/>
|
||||
<xacro:arg name="simulate" default="false"/>
|
||||
<xacro:arg name="robot_ip" default="192.170.10.2"/>
|
||||
<xacro:arg name="fri_port" default="30200"/>
|
||||
@@ -72,8 +74,8 @@
|
||||
|
||||
<joint name="joint1">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint1']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint1']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint1']}</param>
|
||||
@@ -84,8 +86,8 @@
|
||||
|
||||
<joint name="joint2">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint2']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint2']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint2']}</param>
|
||||
@@ -96,8 +98,8 @@
|
||||
|
||||
<joint name="joint3">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint3']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint3']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint3']}</param>
|
||||
@@ -108,8 +110,8 @@
|
||||
|
||||
<joint name="joint4">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint4']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint4']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint4']}</param>
|
||||
@@ -120,8 +122,8 @@
|
||||
|
||||
<joint name="joint5">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint5']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint5']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint5']}</param>
|
||||
@@ -132,8 +134,8 @@
|
||||
|
||||
<joint name="joint6">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint6']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint6']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint6']}</param>
|
||||
@@ -144,8 +146,8 @@
|
||||
|
||||
<joint name="joint7">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.05</param>
|
||||
<param name="max"> 3.05</param>
|
||||
<param name="min">${joint_limits['joint7']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint7']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_positions['joint7']}</param>
|
||||
@@ -173,8 +175,8 @@
|
||||
|
||||
<joint name="joint1">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint1']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint1']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -186,8 +188,8 @@
|
||||
|
||||
<joint name="joint2">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint2']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint2']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -199,8 +201,8 @@
|
||||
|
||||
<joint name="joint3">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint3']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint3']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -212,8 +214,8 @@
|
||||
|
||||
<joint name="joint4">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint4']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint4']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -225,8 +227,8 @@
|
||||
|
||||
<joint name="joint5">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.97</param>
|
||||
<param name="max"> 2.97</param>
|
||||
<param name="min">${joint_limits['joint5']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint5']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -238,8 +240,8 @@
|
||||
|
||||
<joint name="joint6">
|
||||
<command_interface name="position">
|
||||
<param name="min">-2.09</param>
|
||||
<param name="max"> 2.09</param>
|
||||
<param name="min">${joint_limits['joint6']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint6']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
@@ -251,8 +253,8 @@
|
||||
|
||||
<joint name="joint7">
|
||||
<command_interface name="position">
|
||||
<param name="min">-3.05</param>
|
||||
<param name="max"> 3.05</param>
|
||||
<param name="min">${joint_limits['joint7']['soft_min_position']}</param>
|
||||
<param name="max">${joint_limits['joint7']['soft_max_position']}</param>
|
||||
</command_interface>
|
||||
|
||||
<state_interface name="position">
|
||||
|
||||
@@ -12,57 +12,57 @@
|
||||
<!-- joint1: ±170° hard, ±165° soft, k_velocity=10 -->
|
||||
<xacro:revolute_joint jname="joint1" parent="base_link" child="link1"
|
||||
xyz="0 0 0.3375" rpy="0 0 0" axis="0 0 1"
|
||||
lower="-2.97" upper="2.97" effort="200"
|
||||
velocity="1.71" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
|
||||
lower="${joint_limits['joint1']['min_position']}" upper="${joint_limits['joint1']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint1']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint1']['soft_min_position']}" soft_upper="${joint_limits['joint1']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint2: ±119° hard, ±114° soft -->
|
||||
<xacro:revolute_joint jname="joint2" parent="link1" child="link2"
|
||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
||||
lower="-2.09" upper="2.09" effort="200"
|
||||
velocity="1.71" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
|
||||
lower="${joint_limits['joint2']['min_position']}" upper="${joint_limits['joint2']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint2']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint2']['soft_min_position']}" soft_upper="${joint_limits['joint2']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint3: ±170° hard, ±165° soft -->
|
||||
<xacro:revolute_joint jname="joint3" parent="link2" child="link3"
|
||||
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
|
||||
lower="-2.97" upper="2.97" effort="200"
|
||||
velocity="1.75" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
|
||||
lower="${joint_limits['joint3']['min_position']}" upper="${joint_limits['joint3']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint3']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint3']['soft_min_position']}" soft_upper="${joint_limits['joint3']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint4: ±119° hard, ±114° soft -->
|
||||
<xacro:revolute_joint jname="joint4" parent="link3" child="link4"
|
||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 -1"
|
||||
lower="-2.09" upper="2.09" effort="200"
|
||||
velocity="2.27" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
|
||||
lower="${joint_limits['joint4']['min_position']}" upper="${joint_limits['joint4']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint4']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint4']['soft_min_position']}" soft_upper="${joint_limits['joint4']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint5: ±170° hard, ±165° soft -->
|
||||
<xacro:revolute_joint jname="joint5" parent="link4" child="link5"
|
||||
xyz="0 -0.3993 0" rpy="1.5708 0 0" axis="0 0 1"
|
||||
lower="-2.97" upper="2.97" effort="200"
|
||||
velocity="2.44" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.8827" soft_upper="2.8827" k_velocity="10"/>
|
||||
lower="${joint_limits['joint5']['min_position']}" upper="${joint_limits['joint5']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint5']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint5']['soft_min_position']}" soft_upper="${joint_limits['joint5']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint6: ±119° hard, ±114° soft -->
|
||||
<xacro:revolute_joint jname="joint6" parent="link5" child="link6"
|
||||
xyz="0 0 0" rpy="-1.5708 0 0" axis="0 0 1"
|
||||
lower="-2.09" upper="2.09" effort="200"
|
||||
velocity="3.14" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.0027" soft_upper="2.0027" k_velocity="10"/>
|
||||
lower="${joint_limits['joint6']['min_position']}" upper="${joint_limits['joint6']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint6']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint6']['soft_min_position']}" soft_upper="${joint_limits['joint6']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
|
||||
<!-- joint7: ±174° hard, ±169° soft -->
|
||||
<xacro:revolute_joint jname="joint7" parent="link6" child="link7"
|
||||
xyz="0 -0.126 0" rpy="1.5708 0 0" axis="0 0 1"
|
||||
lower="-3.05" upper="3.05" effort="200"
|
||||
velocity="3.14" damping="10.0" friction="0.1"
|
||||
soft_lower="-2.9627" soft_upper="2.9627" k_velocity="10"/>
|
||||
lower="${joint_limits['joint7']['min_position']}" upper="${joint_limits['joint7']['max_position']}" effort="200"
|
||||
velocity="${joint_limits['joint7']['max_velocity']}" damping="10.0" friction="0.1"
|
||||
soft_lower="${joint_limits['joint7']['soft_min_position']}" soft_upper="${joint_limits['joint7']['soft_max_position']}" k_velocity="10"/>
|
||||
|
||||
<joint name="tools_joint" type="fixed">
|
||||
<origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
|
||||
|
||||
@@ -28,6 +28,50 @@ target_compile_features(tcp_gizmo PUBLIC cxx_std_17)
|
||||
target_include_directories(tcp_gizmo PRIVATE include)
|
||||
ament_target_dependencies(tcp_gizmo rclcpp rclcpp_action control_msgs iiwa_msgs geometry_msgs trajectory_msgs
|
||||
action_msgs moveit_servo moveit_ros_planning moveit_core tf2_eigen)
|
||||
find_package(pluginlib REQUIRED)
|
||||
find_package(sensor_msgs REQUIRED)
|
||||
find_package(Python3 REQUIRED COMPONENTS Interpreter Development.Module)
|
||||
find_package(pybind11 REQUIRED)
|
||||
pybind11_add_module(_iiwa_spline_limits src/spline_limits_py.cpp)
|
||||
target_compile_features(_iiwa_spline_limits PRIVATE cxx_std_17)
|
||||
target_include_directories(_iiwa_spline_limits PRIVATE include)
|
||||
ament_get_python_install_dir(PYTHON_INSTALL_DIR)
|
||||
install(TARGETS _iiwa_spline_limits LIBRARY DESTINATION ${PYTHON_INSTALL_DIR})
|
||||
|
||||
add_library(iiwa_spline_limits SHARED src/limit_spline_dynamics.cpp)
|
||||
target_compile_features(iiwa_spline_limits PUBLIC cxx_std_17)
|
||||
target_include_directories(iiwa_spline_limits PRIVATE include)
|
||||
ament_target_dependencies(iiwa_spline_limits rclcpp moveit_core moveit_ros_planning pluginlib sensor_msgs)
|
||||
pluginlib_export_plugin_description_file(moveit_core planning_response_plugins.xml)
|
||||
install(TARGETS iiwa_spline_limits LIBRARY DESTINATION lib)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
enable_testing()
|
||||
add_executable(test_spline_limits test/test_spline_limits.cpp)
|
||||
target_compile_features(test_spline_limits PRIVATE cxx_std_17)
|
||||
target_compile_options(test_spline_limits PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_spline_limits PRIVATE include)
|
||||
add_test(NAME spline_limits COMMAND test_spline_limits)
|
||||
find_package(joint_trajectory_controller REQUIRED)
|
||||
add_executable(test_jtc_interpolation test/test_jtc_interpolation.cpp)
|
||||
target_compile_features(test_jtc_interpolation PRIVATE cxx_std_17)
|
||||
target_compile_options(test_jtc_interpolation PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_jtc_interpolation PRIVATE include)
|
||||
ament_target_dependencies(test_jtc_interpolation joint_trajectory_controller)
|
||||
add_test(NAME jtc_interpolation COMMAND test_jtc_interpolation)
|
||||
add_executable(test_response_adapter test/test_response_adapter.cpp)
|
||||
target_compile_options(test_response_adapter PRIVATE -UNDEBUG)
|
||||
ament_target_dependencies(test_response_adapter moveit_core moveit_ros_planning pluginlib)
|
||||
add_test(NAME response_adapter COMMAND test_response_adapter)
|
||||
add_executable(test_start_state_monitor test/test_start_state_monitor.cpp)
|
||||
target_compile_options(test_start_state_monitor PRIVATE -UNDEBUG)
|
||||
target_include_directories(test_start_state_monitor PRIVATE include)
|
||||
ament_target_dependencies(test_start_state_monitor rclcpp sensor_msgs)
|
||||
add_test(NAME start_state_monitor COMMAND test_start_state_monitor)
|
||||
set_tests_properties(start_state_monitor PROPERTIES ENVIRONMENT "ROS_DOMAIN_ID=231;ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST" TIMEOUT 15)
|
||||
add_test(NAME direct_trajectory COMMAND ${Python3_EXECUTABLE} -m pytest -q ${CMAKE_CURRENT_SOURCE_DIR}/test/test_prepare_trajectory.py)
|
||||
set_tests_properties(direct_trajectory PROPERTIES ENVIRONMENT "PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_SOURCE_DIR}:$ENV{PYTHONPATH}")
|
||||
endif()
|
||||
|
||||
add_executable(motion_planning_cpp src/motion_planning_cpp.cpp)
|
||||
target_include_directories(motion_planning_cpp PUBLIC include)
|
||||
|
||||
@@ -0,0 +1,26 @@
|
||||
"""Prepare position-only waypoints for the JTC quintic interpolator, off the RT thread."""
|
||||
import numpy as np
|
||||
from scipy.interpolate import make_interp_spline
|
||||
from _iiwa_spline_limits import retime
|
||||
|
||||
|
||||
def prepare_trajectory(times, positions, limits):
|
||||
"""Preserve waypoints, fit a C2+ rest-to-rest curve, bound its complete interpolation.
|
||||
|
||||
`times` starts at zero. Limits contain [lower, upper, velocity, acceleration, jerk].
|
||||
Returns (times, positions, velocities, accelerations, uniform_time_scale).
|
||||
Timing may increase; intermediate path can differ from position-only linear interpolation.
|
||||
No collision checking is performed by this direct-command API.
|
||||
"""
|
||||
t, q = np.asarray(times, dtype=float), np.asarray(positions, dtype=float)
|
||||
if t.ndim != 1 or q.ndim != 2 or len(t) < 2 or len(t) != len(q):
|
||||
raise ValueError('At least two complete trajectory points are required')
|
||||
if not np.isfinite(t).all() or not np.isfinite(q).all() or t[0] != 0 or np.any(np.diff(t) < 1e-6):
|
||||
raise ValueError('Finite waypoints and strictly increasing times starting at zero are required')
|
||||
if q.shape[1] != len(limits):
|
||||
raise ValueError('Joint count does not match limits')
|
||||
zero = np.zeros(q.shape[1])
|
||||
# With two derivative constraints per endpoint, knots coincide with all input times.
|
||||
# Thus JTC quintics reproduce these spline segments from their endpoint p/v/a.
|
||||
spline = make_interp_spline(t, q, k=5, bc_type=([(1, zero), (2, zero)], [(1, zero), (2, zero)]))
|
||||
return retime(t.tolist(), q.tolist(), spline(t, 1).tolist(), spline(t, 2).tolist(), limits)
|
||||
@@ -0,0 +1,147 @@
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <array>
|
||||
#include <cmath>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
namespace iiwa_planning {
|
||||
struct Limit {
|
||||
double lower, upper, velocity, acceleration, jerk;
|
||||
};
|
||||
struct Sample {
|
||||
double time;
|
||||
std::vector<double> position, velocity, acceleration;
|
||||
};
|
||||
// Normalized time u=t/T; exactly the quintic used for JTC
|
||||
// position/velocity/acceleration points.
|
||||
inline std::vector<double> quintic(double p0, double p1, double v0, double v1,
|
||||
double a0, double a1, double t) {
|
||||
const double d = p1 - p0, v = v0 * t, w = v1 * t, a = a0 * t * t,
|
||||
b = a1 * t * t;
|
||||
return {p0,
|
||||
v,
|
||||
a / 2,
|
||||
10 * d - 6 * v - 4 * w - 1.5 * a + .5 * b,
|
||||
-15 * d + 8 * v + 7 * w + 1.5 * a - b,
|
||||
6 * d - 3 * v - 3 * w - .5 * a + .5 * b};
|
||||
}
|
||||
inline double choose(unsigned n, unsigned k) {
|
||||
double x = 1;
|
||||
for (unsigned i = 1; i <= k; ++i)
|
||||
x = x * (n + 1 - i) / i;
|
||||
return x;
|
||||
}
|
||||
// Convex hull bound in Bernstein basis, tightened by de Casteljau subdivision.
|
||||
// Unlike sampling, it also bounds extrema between evaluation points.
|
||||
inline std::pair<double, double> hull(const std::vector<double> &b,
|
||||
unsigned depth) {
|
||||
if (!depth)
|
||||
return {*std::min_element(b.begin(), b.end()),
|
||||
*std::max_element(b.begin(), b.end())};
|
||||
auto v = b;
|
||||
std::vector<double> left(b.size()), right(b.size());
|
||||
left[0] = v[0];
|
||||
right.back() = v.back();
|
||||
for (size_t k = 1; k < b.size(); ++k) {
|
||||
for (size_t i = 0; i < b.size() - k; ++i)
|
||||
v[i] = (v[i] + v[i + 1]) / 2;
|
||||
left[k] = v[0];
|
||||
right[b.size() - 1 - k] = v[b.size() - 1 - k];
|
||||
}
|
||||
const auto l = hull(left, depth - 1), r = hull(right, depth - 1);
|
||||
return {std::min(l.first, r.first), std::max(l.second, r.second)};
|
||||
}
|
||||
inline std::pair<double, double> bounds(const std::vector<double> &power) {
|
||||
std::vector<double> b(power.size(), 0);
|
||||
const unsigned n = power.size() - 1;
|
||||
for (unsigned i = 0; i <= n; ++i)
|
||||
for (unsigned k = 0; k <= i; ++k)
|
||||
b[i] += power[k] * choose(i, k) / choose(n, k);
|
||||
return hull(b, 5);
|
||||
}
|
||||
inline double limit_splines(std::vector<Sample> &points,
|
||||
const std::vector<Limit> &limits) {
|
||||
if (points.size() < 2 || limits.empty())
|
||||
throw std::runtime_error("Need at least two trajectory points");
|
||||
for (const auto &l : limits) {
|
||||
for (double x : {l.lower, l.upper, l.velocity, l.acceleration, l.jerk})
|
||||
if (!std::isfinite(x))
|
||||
throw std::runtime_error("Non-finite joint limit");
|
||||
if (l.lower >= l.upper || l.velocity <= 0 || l.acceleration <= 0 ||
|
||||
l.jerk <= 0)
|
||||
throw std::runtime_error("Invalid joint limits");
|
||||
}
|
||||
for (const auto &p : points) {
|
||||
if (!std::isfinite(p.time) || p.time < 0 || p.time > 2147483647.0)
|
||||
throw std::runtime_error("Invalid trajectory time");
|
||||
for (const auto *values : {&p.position, &p.velocity, &p.acceleration}) {
|
||||
if (values->size() != limits.size())
|
||||
throw std::runtime_error("Incomplete trajectory derivatives");
|
||||
for (double x : *values)
|
||||
if (!std::isfinite(x))
|
||||
throw std::runtime_error("Non-finite trajectory");
|
||||
}
|
||||
}
|
||||
if (std::abs(points.front().time) > 1e-9)
|
||||
throw std::runtime_error("Trajectory must start at t=0");
|
||||
for (const auto *p : {&points.front(), &points.back()}) {
|
||||
for (double v : p->velocity)
|
||||
if (std::abs(v) > 1e-9)
|
||||
throw std::runtime_error(
|
||||
"Only stationary trajectory boundaries are supported");
|
||||
for (double a : p->acceleration)
|
||||
if (std::abs(a) > 1e-9)
|
||||
throw std::runtime_error(
|
||||
"Boundary acceleration must be zero for bounded jerk");
|
||||
}
|
||||
double scale = 1;
|
||||
for (size_t i = 1; i < points.size(); ++i) {
|
||||
const auto &p = points[i - 1];
|
||||
const auto &q = points[i];
|
||||
const double dt = q.time - p.time;
|
||||
if (dt < 1e-6)
|
||||
throw std::runtime_error(
|
||||
"Trajectory times must strictly increase (>= 1 us)");
|
||||
for (size_t j = 0; j < limits.size(); ++j) {
|
||||
auto c = quintic(p.position[j], q.position[j], p.velocity[j],
|
||||
q.velocity[j], p.acceleration[j], q.acceleration[j], dt);
|
||||
auto position = bounds(c);
|
||||
const auto &l = limits[j];
|
||||
if (position.first < l.lower - 1e-10 || position.second > l.upper + 1e-10)
|
||||
throw std::runtime_error(
|
||||
"Interpolated position exceeds joint limit at joint index " +
|
||||
std::to_string(j));
|
||||
for (unsigned order = 1; order <= 3; ++order) {
|
||||
std::vector<double> derivative(c.size() - 1);
|
||||
for (size_t k = 1; k < c.size(); ++k)
|
||||
derivative[k - 1] = k * c[k] / dt;
|
||||
c = derivative;
|
||||
const auto b = bounds(c);
|
||||
const double peak = std::max(std::abs(b.first), std::abs(b.second));
|
||||
const double cap =
|
||||
order == 1 ? l.velocity : (order == 2 ? l.acceleration : l.jerk);
|
||||
const double ratio = peak / cap;
|
||||
if (!std::isfinite(ratio))
|
||||
throw std::runtime_error("Unrepresentable trajectory derivatives");
|
||||
scale = std::max(scale, std::pow(ratio, 1.0 / order));
|
||||
}
|
||||
}
|
||||
}
|
||||
if (scale > 1.0 + 1e-9) {
|
||||
scale *= 1.000001; // Margin for conversion of timestamps to nanoseconds.
|
||||
for (auto &p : points) {
|
||||
p.time *= scale;
|
||||
if (!std::isfinite(p.time) || p.time > 2147483647.0)
|
||||
throw std::runtime_error(
|
||||
"Trajectory duration cannot be represented by ROS Duration");
|
||||
for (double &v : p.velocity)
|
||||
v /= scale;
|
||||
for (double &a : p.acceleration)
|
||||
a /= scale * scale;
|
||||
}
|
||||
} else
|
||||
scale = 1;
|
||||
return scale;
|
||||
}
|
||||
} // namespace iiwa_planning
|
||||
@@ -0,0 +1,78 @@
|
||||
#pragma once
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <mutex>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <sensor_msgs/msg/joint_state.hpp>
|
||||
#include <vector>
|
||||
|
||||
namespace iiwa_planning {
|
||||
// MoveIt's CurrentStateMonitor may omit velocities (copy_dynamics=false).
|
||||
// Observe the original telemetry instead of treating missing dynamics as zero.
|
||||
class StartStateMonitor {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node) {
|
||||
node_ = node;
|
||||
for (const auto &item : std::vector<std::pair<std::string, double>>{
|
||||
{"iiwa_dynamics.stopped_velocity_tolerance", .01},
|
||||
{"iiwa_dynamics.state_timeout", .5}})
|
||||
if (!node->has_parameter(item.first))
|
||||
node->declare_parameter(item.first, item.second);
|
||||
tolerance_ = node->get_parameter("iiwa_dynamics.stopped_velocity_tolerance")
|
||||
.as_double();
|
||||
timeout_ = node->get_parameter("iiwa_dynamics.state_timeout").as_double();
|
||||
if (!std::isfinite(tolerance_) || tolerance_ <= 0 ||
|
||||
!std::isfinite(timeout_) || timeout_ <= 0)
|
||||
throw std::runtime_error("Invalid start-state monitoring parameters");
|
||||
subscription_ = node->create_subscription<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", rclcpp::SensorDataQoS(),
|
||||
[this](sensor_msgs::msg::JointState::ConstSharedPtr msg) {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
latest_ = msg;
|
||||
received_ = std::chrono::steady_clock::now();
|
||||
});
|
||||
}
|
||||
bool stationary(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &requested) const {
|
||||
// Explicit nonzero requested initial dynamics are never silently discarded.
|
||||
if (!requested.velocity.empty() && !velocities_ok(names, requested))
|
||||
return false;
|
||||
auto node = node_.lock();
|
||||
if (!node)
|
||||
return velocities_ok(names,
|
||||
requested); // offline use requires explicit dynamics
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
if (!latest_ || std::chrono::duration<double>(
|
||||
std::chrono::steady_clock::now() - received_)
|
||||
.count() > timeout_)
|
||||
return false;
|
||||
const double age =
|
||||
node->now().seconds() -
|
||||
(latest_->header.stamp.sec + latest_->header.stamp.nanosec * 1e-9);
|
||||
return std::isfinite(age) && age >= 0 && age <= timeout_ &&
|
||||
velocities_ok(names, *latest_);
|
||||
}
|
||||
|
||||
private:
|
||||
bool velocities_ok(const std::vector<std::string> &names,
|
||||
const sensor_msgs::msg::JointState &msg) const {
|
||||
for (const auto &name : names) {
|
||||
auto found = std::find(msg.name.begin(), msg.name.end(), name);
|
||||
if (found == msg.name.end())
|
||||
return false;
|
||||
const size_t i = found - msg.name.begin();
|
||||
if (i >= msg.velocity.size() || !std::isfinite(msg.velocity[i]) ||
|
||||
std::abs(msg.velocity[i]) > tolerance_)
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
std::weak_ptr<rclcpp::Node> node_;
|
||||
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr subscription_;
|
||||
mutable std::mutex mutex_;
|
||||
sensor_msgs::msg::JointState::ConstSharedPtr latest_;
|
||||
std::chrono::steady_clock::time_point received_;
|
||||
double tolerance_{.01}, timeout_{.5};
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
@@ -32,7 +32,13 @@
|
||||
<depend>action_msgs</depend>
|
||||
<depend>rcl_interfaces</depend>
|
||||
<depend>tf2_eigen</depend>
|
||||
<depend>pluginlib</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<build_depend>pybind11-dev</build_depend>
|
||||
<exec_depend>python3-numpy</exec_depend>
|
||||
|
||||
<test_depend>joint_trajectory_controller</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
<test_depend>ament_cmake_pytest</test_depend>
|
||||
|
||||
@@ -0,0 +1,8 @@
|
||||
<library path="iiwa_spline_limits">
|
||||
<class name="iiwa_planning/LimitSplineDynamics" type="iiwa_planning::LimitSplineDynamics" base_class_type="planning_interface::PlanningResponseAdapter">
|
||||
<description>Bound the complete JTC quintic interpolation, stretching time without changing its joint-space curve.</description>
|
||||
</class>
|
||||
<class name="iiwa_planning/SetRestBoundary" type="iiwa_planning::SetRestBoundary" base_class_type="planning_interface::PlanningResponseAdapter">
|
||||
<description>Zero stationary endpoint acceleration before Ruckig; reject moving endpoints.</description>
|
||||
</class>
|
||||
</library>
|
||||
@@ -1,42 +1,112 @@
|
||||
{
|
||||
"home": {
|
||||
"joints": [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0],
|
||||
"speed": 0.1
|
||||
"speed": 0.5
|
||||
},
|
||||
"waypoints": [
|
||||
{
|
||||
"x": 0.6,
|
||||
"y": 0.1,
|
||||
"z": 0.55,
|
||||
"a": 3.14,
|
||||
"b": 0.3142,
|
||||
"c": 2.7916,
|
||||
"speed": 0.4,
|
||||
"planner": "ptp"
|
||||
},
|
||||
{
|
||||
"x": 0.6,
|
||||
"y": -0.1,
|
||||
"z": 0.55,
|
||||
"a": 3.14,
|
||||
"b": 0.3142,
|
||||
"c": 2.7916,
|
||||
"speed": 0.3,
|
||||
"planner": "lin"
|
||||
},
|
||||
{
|
||||
"joints": [0.5, 0.3, 0.0, -1.2, 0.0, 1.4, 0.0],
|
||||
"speed": 0.2
|
||||
},
|
||||
{
|
||||
"x": 0.58,
|
||||
"y": 0.0,
|
||||
"z": 0.5,
|
||||
"a": 3.14,
|
||||
"b": 0.3665,
|
||||
"c": 2.8791,
|
||||
"speed": 0.3,
|
||||
"planner": "lin"
|
||||
}
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.4,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.4,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.45,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.45,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.5,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.5,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.55,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.55,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.1,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.2,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.3,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.4,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.5,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.6,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.7,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.8,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":0.9,"planner":"lin"},
|
||||
{"x":0.6,"y":0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"},
|
||||
{"x":0.6,"y":-0.4,"z":0.5,"a":3.14,"b":0.0,"c":-3.14,"speed":1.0,"planner":"lin"}
|
||||
]
|
||||
}
|
||||
|
||||
@@ -0,0 +1,140 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include "iiwa_planning/start_state_monitor.hpp"
|
||||
#include <moveit/planning_interface/planning_response_adapter.hpp>
|
||||
#include <moveit/planning_scene/planning_scene.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <pluginlib/class_list_macros.hpp>
|
||||
|
||||
namespace iiwa_planning {
|
||||
// TOTG can supply nonzero endpoint acceleration. Establish rest boundary
|
||||
// conditions before Ruckig, which otherwise preserves them. Positions and
|
||||
// interior derivatives stay intact.
|
||||
class SetRestBoundary : public planning_interface::PlanningResponseAdapter {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node,
|
||||
const std::string &) override {
|
||||
start_monitor_.initialize(node);
|
||||
}
|
||||
std::string getDescription() const override {
|
||||
return "Establish intended rest boundaries around Ruckig";
|
||||
}
|
||||
void adapt(const planning_scene::PlanningSceneConstPtr &,
|
||||
const planning_interface::MotionPlanRequest &req,
|
||||
planning_interface::MotionPlanResponse &res) const override {
|
||||
if (!res.trajectory || res.trajectory->getWayPointCount() < 2 ||
|
||||
!res.trajectory->getGroup()) {
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return;
|
||||
}
|
||||
if (!start_monitor_.stationary(
|
||||
res.trajectory->getGroup()->getVariableNames(),
|
||||
req.start_state.joint_state)) {
|
||||
RCLCPP_ERROR(
|
||||
rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Fresh, stationary joint_states required for rest-to-rest planning");
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_ROBOT_STATE;
|
||||
return;
|
||||
}
|
||||
for (const auto index :
|
||||
{size_t(0), res.trajectory->getWayPointCount() - 1}) {
|
||||
auto &state = *res.trajectory->getWayPointPtr(index);
|
||||
for (const auto &name : res.trajectory->getGroup()->getVariableNames()) {
|
||||
state.setVariableVelocity(name, 0.0);
|
||||
state.setVariableAcceleration(name, 0.0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
StartStateMonitor start_monitor_;
|
||||
};
|
||||
class LimitSplineDynamics : public planning_interface::PlanningResponseAdapter {
|
||||
public:
|
||||
void initialize(const rclcpp::Node::SharedPtr &node,
|
||||
const std::string &) override {
|
||||
start_monitor_.initialize(node);
|
||||
}
|
||||
std::string getDescription() const override {
|
||||
return "Limit JTC quintic velocity, acceleration and jerk";
|
||||
}
|
||||
void adapt(const planning_scene::PlanningSceneConstPtr &,
|
||||
const planning_interface::MotionPlanRequest &req,
|
||||
planning_interface::MotionPlanResponse &res) const override {
|
||||
if (!res.trajectory || res.trajectory->getWayPointCount() < 2) {
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
return;
|
||||
}
|
||||
try {
|
||||
auto &trajectory = *res.trajectory;
|
||||
const auto *group = trajectory.getGroup();
|
||||
if (!group)
|
||||
throw std::runtime_error("Missing trajectory group");
|
||||
const auto &names = group->getVariableNames();
|
||||
if (!start_monitor_.stationary(names, req.start_state.joint_state))
|
||||
throw std::runtime_error(
|
||||
"Fresh stationary joint_states required before spline validation");
|
||||
const auto model = trajectory.getRobotModel();
|
||||
auto scaling = [](double x) {
|
||||
return std::isfinite(x) && x > 0 && x <= 1 ? x : 1.0;
|
||||
};
|
||||
std::vector<Limit> limits;
|
||||
for (const auto &name : names) {
|
||||
const auto &b = model->getVariableBounds(name);
|
||||
if (!b.position_bounded_ || !b.velocity_bounded_ ||
|
||||
!b.acceleration_bounded_ || !b.jerk_bounded_)
|
||||
throw std::runtime_error(
|
||||
"Missing position/velocity/acceleration/jerk bounds: " + name);
|
||||
limits.push_back(
|
||||
{b.min_position_, b.max_position_,
|
||||
b.max_velocity_ * scaling(req.max_velocity_scaling_factor),
|
||||
b.max_acceleration_ * scaling(req.max_acceleration_scaling_factor),
|
||||
b.max_jerk_});
|
||||
}
|
||||
std::vector<Sample> samples;
|
||||
double t = 0;
|
||||
for (size_t i = 0; i < trajectory.getWayPointCount(); ++i) {
|
||||
const auto &state = *trajectory.getWayPointPtr(i);
|
||||
if (!state.hasVelocities() || !state.hasAccelerations())
|
||||
throw std::runtime_error(
|
||||
"JTC guard requires positions, velocities and accelerations");
|
||||
t += trajectory.getWayPointDurationFromPrevious(i);
|
||||
Sample s{t, {}, {}, {}};
|
||||
for (const auto &name : names) {
|
||||
s.position.push_back(state.getVariablePosition(name));
|
||||
s.velocity.push_back(state.getVariableVelocity(name));
|
||||
s.acceleration.push_back(state.getVariableAcceleration(name));
|
||||
}
|
||||
samples.push_back(s);
|
||||
}
|
||||
const double scale = limit_splines(samples, limits);
|
||||
if (scale > 1) {
|
||||
for (size_t i = 0; i < samples.size(); ++i) {
|
||||
auto &state = *trajectory.getWayPointPtr(i);
|
||||
for (size_t j = 0; j < names.size(); ++j) {
|
||||
state.setVariableVelocity(names[j], samples[i].velocity[j]);
|
||||
state.setVariableAcceleration(names[j], samples[i].acceleration[j]);
|
||||
}
|
||||
trajectory.setWayPointDurationFromPrevious(
|
||||
i, trajectory.getWayPointDurationFromPrevious(i) * scale);
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Trajectory duration scaled by %.6f to bound JTC derivatives",
|
||||
scale);
|
||||
}
|
||||
} catch (const std::exception &e) {
|
||||
RCLCPP_ERROR(rclcpp::get_logger("iiwa_spline_limits"),
|
||||
"Rejected trajectory: %s", e.what());
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::INVALID_MOTION_PLAN;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
StartStateMonitor start_monitor_;
|
||||
};
|
||||
} // namespace iiwa_planning
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_planning::LimitSplineDynamics,
|
||||
planning_interface::PlanningResponseAdapter)
|
||||
|
||||
PLUGINLIB_EXPORT_CLASS(iiwa_planning::SetRestBoundary,
|
||||
planning_interface::PlanningResponseAdapter)
|
||||
@@ -0,0 +1,30 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <pybind11/pybind11.h>
|
||||
#include <pybind11/stl.h>
|
||||
namespace py = pybind11;
|
||||
PYBIND11_MODULE(_iiwa_spline_limits, m) {
|
||||
m.def("retime", [](const std::vector<double> &t,
|
||||
const std::vector<std::vector<double>> &q,
|
||||
const std::vector<std::vector<double>> &v,
|
||||
const std::vector<std::vector<double>> &a,
|
||||
const std::vector<std::array<double, 5>> &caps) {
|
||||
if (q.size() != t.size() || v.size() != t.size() || a.size() != t.size())
|
||||
throw std::runtime_error("Point array sizes differ");
|
||||
std::vector<iiwa_planning::Sample> points;
|
||||
for (size_t i = 0; i < t.size(); ++i)
|
||||
points.push_back({t[i], q[i], v[i], a[i]});
|
||||
std::vector<iiwa_planning::Limit> limits;
|
||||
for (const auto &c : caps)
|
||||
limits.push_back({c[0], c[1], c[2], c[3], c[4]});
|
||||
const double scale = iiwa_planning::limit_splines(points, limits);
|
||||
std::vector<double> times;
|
||||
std::vector<std::vector<double>> positions, velocities, accelerations;
|
||||
for (const auto &p : points) {
|
||||
times.push_back(p.time);
|
||||
positions.push_back(p.position);
|
||||
velocities.push_back(p.velocity);
|
||||
accelerations.push_back(p.acceleration);
|
||||
}
|
||||
return py::make_tuple(times, positions, velocities, accelerations, scale);
|
||||
});
|
||||
}
|
||||
@@ -0,0 +1,36 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <joint_trajectory_controller/trajectory.hpp>
|
||||
|
||||
int main() {
|
||||
using namespace iiwa_planning;
|
||||
std::vector<Sample> samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}};
|
||||
std::vector<Limit> limits{{-2, 2, 1, 1, 2}};
|
||||
const auto scale = limit_splines(samples, limits);
|
||||
assert(scale > 1);
|
||||
joint_trajectory_controller::Trajectory jtc;
|
||||
trajectory_msgs::msg::JointTrajectoryPoint a, b, out;
|
||||
a.positions = samples[0].position;
|
||||
a.velocities = samples[0].velocity;
|
||||
a.accelerations = samples[0].acceleration;
|
||||
b.positions = samples[1].position;
|
||||
b.velocities = samples[1].velocity;
|
||||
b.accelerations = samples[1].acceleration;
|
||||
const auto finish = rclcpp::Time(static_cast<int64_t>(samples[1].time * 1e9));
|
||||
const auto start = rclcpp::Time(int64_t(0));
|
||||
double prev = 0;
|
||||
const int n = 10000;
|
||||
for (int i = 0; i <= n; ++i) {
|
||||
const auto now = rclcpp::Time(finish.nanoseconds() * i / n);
|
||||
jtc.interpolate_between_points(start, a, finish, b, now, out);
|
||||
assert(std::abs(out.velocities[0]) <= 1.000001);
|
||||
assert(std::abs(out.accelerations[0]) <= 1.000001);
|
||||
if (i)
|
||||
assert(std::abs((out.accelerations[0] - prev) / (samples[1].time / n)) <=
|
||||
2.00001);
|
||||
prev = out.accelerations[0];
|
||||
}
|
||||
std::cout << "Installed JTC interpolation stays within "
|
||||
"velocity/acceleration/jerk limits\n";
|
||||
}
|
||||
@@ -0,0 +1,29 @@
|
||||
import numpy as np
|
||||
import pytest
|
||||
from iiwa_planning.direct_trajectory import prepare_trajectory
|
||||
|
||||
LIMITS = [[-2, 2, 1, 1, 2]]
|
||||
|
||||
|
||||
def test_dense_points_are_smooth_and_retimed():
|
||||
t, q, v, a, scale = prepare_trajectory([0, .1, .2], [[0], [.5], [1]], LIMITS)
|
||||
assert scale > 1
|
||||
np.testing.assert_allclose(q, [[0], [.5], [1]])
|
||||
assert abs(v[1][0]) > 0 # do not stop at every CSV waypoint
|
||||
np.testing.assert_allclose([v[0], v[-1], a[0], a[-1]], 0, atol=1e-9)
|
||||
assert t[-1] > .2
|
||||
|
||||
|
||||
@pytest.mark.parametrize('times,positions', [([0, 0], [[0], [1]]), ([0, 1], [[0], [float('nan')]]), ([0, 1], [[0], [3]])])
|
||||
def test_bad_trajectory_is_rejected(times, positions):
|
||||
with pytest.raises((ValueError, RuntimeError)):
|
||||
prepare_trajectory(times, positions, LIMITS)
|
||||
|
||||
|
||||
def test_time_scaling_preserves_quintic_between_waypoints():
|
||||
from scipy.interpolate import BPoly
|
||||
times, positions, velocities, accelerations, scale = prepare_trajectory([0,.1,.2], [[0],[.5],[1]], LIMITS)
|
||||
curve = BPoly.from_derivatives(times, [[q,v,a] for q,v,a in zip(positions,velocities,accelerations)])
|
||||
grid = np.linspace(times[0],times[-1],10001)
|
||||
for order,cap in [(1,1),(2,1),(3,2)]:
|
||||
assert np.max(np.abs(curve(grid,nu=order))) <= cap * 1.000001
|
||||
@@ -0,0 +1,65 @@
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <moveit/planning_interface/planning_response_adapter.hpp>
|
||||
#include <moveit/robot_model/robot_model.hpp>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.hpp>
|
||||
#include <moveit/trajectory_processing/ruckig_traj_smoothing.hpp>
|
||||
#include <moveit/trajectory_processing/time_optimal_trajectory_generation.hpp>
|
||||
#include <pluginlib/class_loader.hpp>
|
||||
#include <srdfdom/model.h>
|
||||
#include <urdf_parser/urdf_parser.h>
|
||||
|
||||
int main() {
|
||||
auto urdf = urdf::parseURDF(
|
||||
R"(<robot name="test"><link name="base"/><link name="tip"/><joint name="joint1" type="revolute"><parent link="base"/><child link="tip"/><axis xyz="0 0 1"/><limit lower="-2" upper="2" velocity="1" effort="1"/></joint></robot>)");
|
||||
auto srdf = std::make_shared<srdf::Model>();
|
||||
assert(srdf->initString(
|
||||
*urdf,
|
||||
R"(<robot name="test"><group name="arm"><joint name="joint1"/></group></robot>)"));
|
||||
auto model = std::make_shared<moveit::core::RobotModel>(urdf, srdf);
|
||||
auto bounds = model->getVariableBounds("joint1");
|
||||
bounds.acceleration_bounded_ = bounds.jerk_bounded_ = true;
|
||||
bounds.min_acceleration_ = -1;
|
||||
bounds.max_acceleration_ = 1;
|
||||
bounds.min_jerk_ = -2;
|
||||
bounds.max_jerk_ = 2;
|
||||
model->getJointModel("joint1")->setVariableBounds("joint1", bounds);
|
||||
auto trajectory =
|
||||
std::make_shared<robot_trajectory::RobotTrajectory>(model, "arm");
|
||||
moveit::core::RobotState state(model);
|
||||
state.setToDefaultValues();
|
||||
state.setVariableVelocity("joint1", 0);
|
||||
state.setVariableAcceleration("joint1", 0);
|
||||
trajectory->addSuffixWayPoint(state, 0);
|
||||
state.setVariablePosition("joint1", 1);
|
||||
trajectory->addSuffixWayPoint(state, .2);
|
||||
assert(trajectory_processing::TimeOptimalTrajectoryGeneration()
|
||||
.computeTimeStamps(*trajectory));
|
||||
pluginlib::ClassLoader<planning_interface::PlanningResponseAdapter> loader(
|
||||
"moveit_core", "planning_interface::PlanningResponseAdapter");
|
||||
auto guard = loader.createSharedInstance("iiwa_planning/LimitSplineDynamics");
|
||||
planning_interface::MotionPlanRequest req;
|
||||
req.group_name = "arm";
|
||||
req.max_velocity_scaling_factor = req.max_acceleration_scaling_factor = 1;
|
||||
planning_interface::MotionPlanResponse res;
|
||||
res.trajectory = trajectory;
|
||||
res.error_code = moveit_msgs::msg::MoveItErrorCodes::SUCCESS;
|
||||
auto boundary = loader.createSharedInstance("iiwa_planning/SetRestBoundary");
|
||||
req.start_state.joint_state.name = {"joint1"};
|
||||
req.start_state.joint_state.position = {0};
|
||||
req.start_state.joint_state.velocity = {0};
|
||||
boundary->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
assert(trajectory_processing::RuckigSmoothing::applySmoothing(*trajectory));
|
||||
boundary->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
guard->adapt(nullptr, req, res);
|
||||
assert(bool(res));
|
||||
assert(trajectory->getDuration() > .2);
|
||||
state.setVariablePosition("joint1", 3);
|
||||
trajectory->addSuffixWayPoint(state, 1);
|
||||
guard->adapt(nullptr, req, res);
|
||||
assert(!bool(res));
|
||||
std::cout << "Installed Ruckig + pluginlib response adapter: success and "
|
||||
"rejection OK\n";
|
||||
}
|
||||
@@ -0,0 +1,58 @@
|
||||
#include "iiwa_planning/spline_limits.hpp"
|
||||
#include <cassert>
|
||||
#include <cmath>
|
||||
#include <iostream>
|
||||
using namespace iiwa_planning;
|
||||
int main() {
|
||||
// Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5.
|
||||
std::vector<Sample> points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}};
|
||||
std::vector<Limit> limits{{-2, 2, 2, 1, 2}};
|
||||
const double scale = limit_splines(points, limits);
|
||||
assert(scale > 3.0);
|
||||
assert(std::abs(points.back().position[0] - 1) < 1e-12);
|
||||
assert(limit_splines(points, limits) <= 1.000001);
|
||||
// Consistent time scaling keeps the geometric quintic unchanged.
|
||||
auto p = quintic(0, 1, 0, 0, 0, 0, 1);
|
||||
auto q = quintic(0, 1, 0, 0, 0, 0, points.back().time);
|
||||
for (int i = 0; i <= 5; ++i)
|
||||
assert(std::abs(p[i] - q[i]) < 1e-10);
|
||||
bool bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {0}}, {0, {1}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
bad = false;
|
||||
// Positions are in range but large endpoint velocities overshoot between
|
||||
// them.
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {0}},
|
||||
{1, {0}, {12}, {0}},
|
||||
{2, {0}, {-12}, {0}},
|
||||
{3, {0}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {NAN}, {0}, {0}}, {1, {0}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
// Non-rest boundaries must not slip past the finite-segment jerk check.
|
||||
bad = false;
|
||||
try {
|
||||
std::vector<Sample> s{{0, {0}, {0}, {1}}, {1, {1}, {0}, {0}}};
|
||||
limit_splines(s, limits);
|
||||
} catch (const std::exception &) {
|
||||
bad = true;
|
||||
}
|
||||
assert(bad);
|
||||
std::cout << "spline limits OK\n";
|
||||
}
|
||||
@@ -0,0 +1,41 @@
|
||||
#include "iiwa_planning/start_state_monitor.hpp"
|
||||
#include <cassert>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
int main(int argc, char **argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<rclcpp::Node>(
|
||||
"iiwa_start_monitor_test", rclcpp::NodeOptions().enable_rosout(false));
|
||||
iiwa_planning::StartStateMonitor monitor;
|
||||
monitor.initialize(node);
|
||||
auto pub = node->create_publisher<sensor_msgs::msg::JointState>(
|
||||
"/joint_states", rclcpp::SensorDataQoS());
|
||||
sensor_msgs::msg::JointState
|
||||
requested; // Normal MoveIt request has no velocities.
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
sensor_msgs::msg::JointState state;
|
||||
state.name = {"joint1"};
|
||||
state.position = {0};
|
||||
state.velocity = {0};
|
||||
auto publish = [&] {
|
||||
for (int i = 0; i < 30; ++i) {
|
||||
state.header.stamp = node->now();
|
||||
pub->publish(state);
|
||||
rclcpp::spin_some(node);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
};
|
||||
publish();
|
||||
assert(monitor.stationary({"joint1"}, requested));
|
||||
state.velocity = {.1};
|
||||
publish();
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
state.velocity = {0};
|
||||
publish();
|
||||
assert(monitor.stationary({"joint1"}, requested));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(550));
|
||||
assert(!monitor.stationary({"joint1"}, requested));
|
||||
rclcpp::shutdown();
|
||||
std::cout << "Position-only MoveIt requests validated using fresh real "
|
||||
"velocity feedback\n";
|
||||
}
|
||||
@@ -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)
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
@@ -48,7 +48,7 @@ def _resolve_path(package: str, relative: str) -> Path:
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
return Path(get_package_share_directory(package)) / relative
|
||||
except Exception:
|
||||
src = Path(__file__).parents[3] # .../src/iiwa_web/iiwa_web/ -> .../src/
|
||||
src = Path(__file__).parents[2] # .../src/iiwa_web/iiwa_web/ -> .../src/
|
||||
return src / package / relative
|
||||
|
||||
|
||||
@@ -60,7 +60,8 @@ def _parse_joint_limits_data(data: dict) -> tuple[list[str], list[tuple[float, f
|
||||
while f"joint{i}" in joints:
|
||||
j = joints[f"joint{i}"]
|
||||
names.append(f"joint{i}")
|
||||
limits.append((j["min_position"], j["max_position"]))
|
||||
limits.append((max(j["min_position"], j.get("soft_min_position", j["min_position"])),
|
||||
min(j["max_position"], j.get("soft_max_position", j["max_position"]))))
|
||||
i += 1
|
||||
return names, limits
|
||||
|
||||
|
||||
@@ -15,8 +15,12 @@ class CobotWebNode(Node):
|
||||
self.declare_parameter('port', 8007)
|
||||
self.declare_parameter('endpoints_path', '')
|
||||
self.declare_parameter('joint_limits_path', '')
|
||||
self.declare_parameter('trajectory_state_timeout', 0.5)
|
||||
self.declare_parameter('trajectory_start_tolerance', 0.001)
|
||||
self.declare_parameter('trajectory_stopped_velocity', 0.01)
|
||||
|
||||
self._topic_cache: dict = {}
|
||||
self._topic_received: dict = {}
|
||||
self._pub_registry: dict = {}
|
||||
self._service_clients: dict = {}
|
||||
self._action_clients: dict = {}
|
||||
@@ -47,9 +51,15 @@ class CobotWebNode(Node):
|
||||
with self._lock:
|
||||
return self._topic_cache.get(topic_name)
|
||||
|
||||
def get_latest_with_age(self, topic_name: str):
|
||||
with self._lock:
|
||||
return (self._topic_cache.get(topic_name),
|
||||
time.monotonic() - self._topic_received.get(topic_name, float('-inf')))
|
||||
|
||||
def _handle_message(self, topic_name: str, msg):
|
||||
with self._lock:
|
||||
self._topic_cache[topic_name] = msg
|
||||
self._topic_received[topic_name] = time.monotonic()
|
||||
|
||||
def call_service(self, srv_type, srv_name: str, request, timeout: float = 5.0):
|
||||
if srv_name not in self._service_clients:
|
||||
|
||||
@@ -1,5 +1,11 @@
|
||||
import csv
|
||||
import io
|
||||
import math
|
||||
import threading
|
||||
import time
|
||||
import yaml
|
||||
from pathlib import Path
|
||||
from iiwa_planning.direct_trajectory import prepare_trajectory
|
||||
from collections import deque
|
||||
from datetime import datetime
|
||||
from builtin_interfaces.msg import Duration
|
||||
@@ -7,7 +13,7 @@ from fastapi import APIRouter, File, HTTPException, Query, UploadFile
|
||||
from pydantic import BaseModel, Field
|
||||
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
|
||||
|
||||
from .config_loader import load_joint_limits, load_joint_names
|
||||
from .config_loader import load_joint_limits, load_joint_names, _resolve_path
|
||||
from .ros_node import get_bridge
|
||||
|
||||
router = APIRouter(prefix="/trajectory", tags=["trajectory"])
|
||||
@@ -15,6 +21,10 @@ router = APIRouter(prefix="/trajectory", tags=["trajectory"])
|
||||
TOPIC = "/iiwa_arm_controller/joint_trajectory"
|
||||
JOINT_NAMES = load_joint_names()
|
||||
N_JOINTS = len(JOINT_NAMES)
|
||||
_prepare_lock = threading.Lock()
|
||||
_admission_lock = threading.Lock()
|
||||
_generation = 0
|
||||
_direct_until = 0.0
|
||||
|
||||
_log_lines: deque[str] = deque(maxlen=300)
|
||||
|
||||
@@ -24,13 +34,13 @@ def _log(msg: str) -> None:
|
||||
|
||||
|
||||
def _to_duration(seconds: float) -> Duration:
|
||||
sec = int(seconds)
|
||||
nanosec = int(round((seconds - sec) * 1e9))
|
||||
total_ns = round(seconds * 1e9)
|
||||
sec, nanosec = divmod(total_ns, 1_000_000_000)
|
||||
return Duration(sec=sec, nanosec=nanosec)
|
||||
|
||||
|
||||
def _validate_limits(points: list[list[float]]) -> None:
|
||||
limits = load_joint_limits()
|
||||
limits = load_joint_limits(path=get_bridge().get_parameter("joint_limits_path").value or None)
|
||||
for row_idx, positions in enumerate(points):
|
||||
for j, (pos, (lo, hi)) in enumerate(zip(positions, limits)):
|
||||
if not (lo <= pos <= hi):
|
||||
@@ -52,6 +62,81 @@ def _build_msg(rows: list[tuple[list[float], float]]) -> JointTrajectory:
|
||||
return msg
|
||||
|
||||
|
||||
def _stationary_state():
|
||||
bridge = get_bridge()
|
||||
state, age = bridge.get_latest_with_age('/joint_states')
|
||||
timeout = bridge.get_parameter('trajectory_state_timeout').value
|
||||
tolerance = bridge.get_parameter('trajectory_start_tolerance').value
|
||||
stopped = bridge.get_parameter('trajectory_stopped_velocity').value
|
||||
if any(not math.isfinite(x) or x <= 0 for x in (timeout, tolerance, stopped)):
|
||||
raise HTTPException(503, 'Invalid direct trajectory monitoring parameters')
|
||||
if state is None or age > timeout:
|
||||
raise HTTPException(409, 'Fresh joint_states required before sending a trajectory')
|
||||
stamp = state.header.stamp.sec + state.header.stamp.nanosec * 1e-9
|
||||
source_age = bridge.get_clock().now().nanoseconds * 1e-9 - stamp
|
||||
if not math.isfinite(source_age) or not 0 <= source_age <= timeout:
|
||||
raise HTTPException(409, 'joint_states timestamp is stale or in the future')
|
||||
try:
|
||||
indexes = [list(state.name).index(name) for name in JOINT_NAMES]
|
||||
current = [state.position[i] for i in indexes]
|
||||
velocity = [state.velocity[i] for i in indexes]
|
||||
except (ValueError, IndexError):
|
||||
raise HTTPException(409, 'Complete named position and velocity feedback required')
|
||||
if any(not math.isfinite(x) for x in current + velocity) or any(abs(v) > stopped for v in velocity):
|
||||
raise HTTPException(409, 'Direct trajectories must start from a stationary robot')
|
||||
return current, tolerance
|
||||
|
||||
|
||||
def _build_safe_msg(rows):
|
||||
bridge = get_bridge()
|
||||
current, tolerance = _stationary_state()
|
||||
path = bridge.get_parameter('joint_limits_path').value
|
||||
resolved = Path(path) if path else _resolve_path('iiwa_config', 'config/moveit/joint_limits.yaml')
|
||||
data = yaml.safe_load(resolved.read_text())['joint_limits']
|
||||
caps = []
|
||||
for name in JOINT_NAMES:
|
||||
j = data[name]
|
||||
caps.append([max(j['min_position'], j.get('soft_min_position', j['min_position'])),
|
||||
min(j['max_position'], j.get('soft_max_position', j['max_position'])),
|
||||
j['max_velocity'], j['max_acceleration'], j['max_jerk']])
|
||||
rows = [(list(q), float(t)) for q, t in rows]
|
||||
if rows[0][1] > 0:
|
||||
rows.insert(0, (current, 0.0))
|
||||
elif rows[0][1] == 0 and max(abs(a-b) for a,b in zip(rows[0][0],current)) > tolerance:
|
||||
raise HTTPException(422, 'A waypoint at t=0 must match the current joint positions')
|
||||
else:
|
||||
rows[0] = (current, rows[0][1])
|
||||
try:
|
||||
times, positions, velocities, accelerations, scale = prepare_trajectory(
|
||||
[t for _, t in rows], [q for q, _ in rows], caps)
|
||||
except (ValueError, RuntimeError) as exc:
|
||||
raise HTTPException(422, str(exc)) from exc
|
||||
msg = _build_msg(list(zip(positions, times)))
|
||||
for point, v, a in zip(msg.points, velocities, accelerations):
|
||||
point.velocities = v
|
||||
point.accelerations = a
|
||||
return msg, times[-1], scale
|
||||
|
||||
|
||||
def _send_rows(rows):
|
||||
global _direct_until
|
||||
with _prepare_lock:
|
||||
with _admission_lock:
|
||||
generation = _generation
|
||||
if time.monotonic() < _direct_until:
|
||||
raise HTTPException(409, 'A direct trajectory is still active; stop it before replacing it')
|
||||
msg, duration, scale = _build_safe_msg(rows)
|
||||
with _admission_lock:
|
||||
if generation != _generation:
|
||||
raise HTTPException(409, 'Trajectory preparation was canceled by stop')
|
||||
current, tolerance = _stationary_state()
|
||||
if max(abs(a-b) for a,b in zip(current, msg.points[0].positions)) > tolerance:
|
||||
raise HTTPException(409, 'Robot moved during trajectory preparation; retry from its new state')
|
||||
_publish(msg)
|
||||
_direct_until = time.monotonic() + duration + get_bridge().get_parameter('trajectory_state_timeout').value
|
||||
return msg, duration, scale
|
||||
|
||||
|
||||
def _publish(msg: JointTrajectory) -> None:
|
||||
get_bridge().publish(TOPIC, JointTrajectory, msg)
|
||||
|
||||
@@ -80,9 +165,9 @@ def send_trajectory(req: SendRequest):
|
||||
if req.validate_limits:
|
||||
_validate_limits([r[0] for r in rows])
|
||||
|
||||
_publish(_build_msg(rows))
|
||||
msg, duration, time_scale = _send_rows(rows)
|
||||
_log(f"[send] {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
|
||||
return {"status": "sent", "points": len(rows)}
|
||||
return {"status": "sent", "points": len(msg.points), "duration": duration, "time_scale": time_scale}
|
||||
|
||||
|
||||
@router.post("/send_csv", summary="Загрузить CSV и отправить траекторию")
|
||||
@@ -166,26 +251,30 @@ async def send_csv_trajectory(
|
||||
if validate_limits:
|
||||
_validate_limits([r[0] for r in rows])
|
||||
|
||||
_publish(_build_msg(rows))
|
||||
msg, duration, time_scale = _send_rows(rows)
|
||||
_log(f"[csv] {file.filename} → {len(rows)} точек, t_end={rows[-1][1]:.2f}с")
|
||||
return {"status": "sent", "points": len(rows), "filename": file.filename}
|
||||
return {"status": "sent", "points": len(msg.points), "filename": file.filename, "duration": duration, "time_scale": time_scale}
|
||||
|
||||
|
||||
def send_stop_trajectory() -> None:
|
||||
"""Publish a hold-position (or empty) trajectory to freeze joint motion."""
|
||||
bridge = get_bridge()
|
||||
joint_states = bridge.get_latest("/joint_states")
|
||||
if joint_states is not None and len(joint_states.position) >= N_JOINTS:
|
||||
current_positions = list(joint_states.position[:N_JOINTS])
|
||||
hold_msg = _build_msg([(current_positions, 0.5)])
|
||||
_publish(hold_msg)
|
||||
_log("[stop] отправлена точка удержания текущей позиции")
|
||||
else:
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = JOINT_NAMES
|
||||
_publish(msg)
|
||||
_log("[stop] joint_states недоступны, отправлена пустая траектория")
|
||||
|
||||
"""Cancel preparation and publish hold; malformed feedback falls back to cancellation."""
|
||||
global _generation, _direct_until
|
||||
with _admission_lock:
|
||||
_generation += 1
|
||||
_direct_until = 0.0
|
||||
bridge = get_bridge()
|
||||
state = bridge.get_latest('/joint_states')
|
||||
try:
|
||||
current = [state.position[list(state.name).index(name)] for name in JOINT_NAMES]
|
||||
if not all(math.isfinite(x) for x in current):
|
||||
raise ValueError('Non-finite feedback')
|
||||
_publish(_build_msg([(current, 0.5)]))
|
||||
_log('[stop] отправлена точка удержания текущей позиции')
|
||||
except (AttributeError, ValueError, IndexError):
|
||||
msg = JointTrajectory()
|
||||
msg.joint_names = JOINT_NAMES
|
||||
_publish(msg)
|
||||
_log('[stop] отправлена пустая траектория: неполная обратная связь')
|
||||
|
||||
|
||||
@router.get("/logs", summary="Последние лог-записи траекторного модуля")
|
||||
|
||||
@@ -14,6 +14,8 @@
|
||||
<exec_depend>tf2_ros</exec_depend>
|
||||
<exec_depend>tf2_py</exec_depend>
|
||||
<exec_depend>moveit_msgs</exec_depend>
|
||||
<exec_depend>iiwa_planning</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
|
||||
@@ -19,6 +19,7 @@ setup(
|
||||
'python-multipart',
|
||||
'fastmcp',
|
||||
],
|
||||
tests_require=['pytest'],
|
||||
zip_safe=True,
|
||||
maintainer='daniel',
|
||||
maintainer_email='grabardm@ml-dev.ru',
|
||||
|
||||
@@ -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
|
||||
Reference in New Issue
Block a user