diff --git a/cobot-setting.yaml b/cobot-setting.yaml index 8cf012f..e0bd598 100644 --- a/cobot-setting.yaml +++ b/cobot-setting.yaml @@ -3,7 +3,7 @@ robot: ip: "192.168.21.147" port: 30200 fri_cycle_ms: 10 # период FRI-цикла: 5 мс (200 Гц) или 10 мс (100 Гц) - joint_position_tau: 0.04 # EMA позиций FRI [с]; 0 отключает сглаживание + joint_position_tau: 0.06 # EMA позиций FRI [с]; 0 отключает сглаживание description: pkg://iiwa_description/urdf/iiwa7.urdf.xacro @@ -43,6 +43,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 @@ -51,6 +53,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) diff --git a/doc/lwc-doc/docs/realtime-fri.en.md b/doc/lwc-doc/docs/realtime-fri.en.md index db2998d..036eac3 100644 --- a/doc/lwc-doc/docs/realtime-fri.en.md +++ b/doc/lwc-doc/docs/realtime-fri.en.md @@ -82,3 +82,23 @@ sudo gpasswd -d "$USER" realtime After logging in again, processes use the regular Linux scheduler. See the official [ros2_control documentation](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism) for controller-manager priorities and limits. + +## Trajectory limits and smoothness + +`controller.moveit.joint_limits` in `cobot-setting.yaml` selects a single limits file for all seven axes, consumed by xacro, FRI, Webots, MoveIt and the web API. Hard position limits and the existing narrower protective bounds remain distinct. Command validation and planning use their intersection; the MoveIt parameter override explicitly preserves soft bounds. + +Position and velocity limits were checked against the [KUKA LBR iiwa 7 R800 datasheet, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Working limits were not expanded relative to the previous URDF. Upward rounding beyond published values was removed. **Acceleration and jerk limits remain project settings, not verified KUKA ratings.** Tool/payload and Sunrise settings still require independent verification. + +OMPL/CHOMP now run TOTG → rest-boundary normalization → Ruckig → rest-boundary normalization → `iiwa_planning/LimitSplineDynamics` → MoveIt solution validation. The initial requested state must be stationary within `planning.stopped_velocity_tolerance` (rad/s). The spline adapter bounds the complete JTC quintic polynomial in Bernstein form, including extrema between waypoints. It uniformly stretches time and scales derivatives to respect velocity, acceleration and jerk limits, rejecting position overshoot. Endpoint acceleration is normalized around Ruckig because TOTG and Ruckig duration extension can leave nonzero boundary derivatives. + +Pilz PTP/LIN/CIRC use the spline validator and uniform time scaling without joint-space Ruckig. This preserves the original JTC interpolation curve, not a new guarantee of ideal Cartesian interpolation. Nonstationary boundary conditions, including some blended sequences, are rejected; this implementation targets rest-to-rest motions. + +Position-only CSV/JSON `/trajectory/send*` requests are fitted with a smooth rest-to-rest spline and passed through the same C++ validator. Input waypoints are retained; the curve between them can differ from the former linear interpolation. This direct API does not check collisions. Responses include actual `duration` and `time_scale`; timing may increase. `validate_limits=false` cannot disable mandatory dynamics and operating-range checks. + +Direct commands require fresh, complete named joint position/velocity feedback and a stationary robot. The first point at zero time must match the measured position within `web.trajectory_start_tolerance`; otherwise a measured start point is prepended if the first timestamp is positive. Freshness and stationarity are checked again after preparation. `web.trajectory_state_timeout` is in seconds; `web.trajectory_stopped_velocity` is in rad/s. Concurrent direct commands are rejected while a trajectory is reserved; stopping cancels pending preparation. + +External clients sending directly to the JTC topic/action bypass this MoveIt/web preparation and remain responsible for their trajectories. This is not a global ROS command firewall. The FRI filter, watchdog and cycle period are unchanged. Hardware retesting is needed to assess noise and actual tracking. + +After rebuilding, run `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Tests exercise seven-axis consistency, spline extrema, the installed JTC interpolator, installed TOTG/Ruckig plus pluginlib, and web admission failures without connecting to a real FRI session. Rebuild `iiwa_config`, `iiwa_description` and `iiwa_bringup` as well; running processes do not update automatically. + +The adapters check original `/joint_states` telemetry: absent MoveIt `RobotState` velocities are not treated as zero. `planning.state_timeout` bounds telemetry age. The subscription test uses synthetic messages in separate local ROS domain 231, without FRI. diff --git a/doc/lwc-doc/docs/realtime-fri.md b/doc/lwc-doc/docs/realtime-fri.md index 71bc39f..9e57616 100644 --- a/doc/lwc-doc/docs/realtime-fri.md +++ b/doc/lwc-doc/docs/realtime-fri.md @@ -82,3 +82,32 @@ sudo gpasswd -d "$USER" realtime После повторного входа процессы снова будут работать с обычным планировщиком Linux. Подробнее о лимитах и приоритете `controller_manager` — в [документации ros2_control](https://control.ros.org/jazzy/doc/ros2_control/controller_manager/doc/userdoc.html#determinism). + +## Ограничения траекторий и плавность + +`controller.moveit.joint_limits` из `cobot-setting.yaml` задаёт единый файл для всех семи осей. Его читают URDF/xacro, FRI, Webots, MoveIt и web API. `min_position/max_position` описывают рабочие жёсткие пределы; `soft_min_position/soft_max_position` сохраняют более узкие защитные диапазоны. Планирование, командные интерфейсы и проверка API используют пересечение этих диапазонов. При загрузке MoveIt пересечение применяется явно: YAML иначе может переопределить мягкие пределы URDF. + +Диапазоны и скорости сверены с [листом KUKA LBR iiwa 7 R800, 0000-246-832 V1.2](https://www.kuka.com/-/media/kuka-downloads/imported/8350ff3ca11642998dbdc81dcc2ed44c/0000246832_en.pdf). Для осей A1…A7 паспортные диапазоны составляют ±170°, ±120°, ±170°, ±120°, ±170°, ±120°, ±175°; скорости с номинальной нагрузкой — 98, 98, 100, 130, 140, 180, 180°/с. Рабочие пределы не расширены относительно прежнего URDF: округления, превышавшие паспортные значения, уменьшены. **Ускорения и рывки в YAML остаются ограничениями проекта, а не подтверждёнными паспортными пределами KUKA.** Данные установленного инструмента, нагрузки и настройки Sunrise по этому листу не проверяются. + +Для OMPL/CHOMP обработка выполняется в порядке: + +1. TOTG назначает время движения. +2. `iiwa_planning/SetRestBoundary` проверяет скорость исходного состояния и задаёт нулевые скорости/ускорения на концах движения из покоя. +3. Ruckig сглаживает траекторию с ограничением рывка. +4. Повторная установка граничных условий устраняет изменения производных, которые может внести продление длительности в Ruckig. +5. `iiwa_planning/LimitSplineDynamics` проверяет весь квинтический сплайн JTC, включая промежутки между точками. Границы полиномов вычисляются в базисе Бернштейна, а не только по временной сетке. При превышении скорости, ускорения или рывка увеличивается время всей траектории; скорости и ускорения масштабируются согласованно. Выход позиции за рабочие пределы отклоняется. +6. MoveIt проверяет решение на столкновения обычным адаптером `ValidateSolution`. + +Pilz PTP/LIN/CIRC проходят проверку сплайна и равномерное увеличение длительности без Ruckig в суставном пространстве. При этом сохраняется исходная кривая интерполяции JTC; точность геометрии LIN/CIRC относительно непрерывного идеального пути отдельно не улучшается. Движения с ненулевыми граничными скоростями/ускорениями (включая некоторые варианты blending) отклоняются: проверка предназначена для перемещений из покоя в покой. `planning.stopped_velocity_tolerance` задаёт порог определения неподвижного начального состояния в рад/с. + +CSV/JSON `/trajectory/send*` по-прежнему принимают позиции и время, но теперь строят гладкий сплайн с нулевыми граничными производными и проходят тот же C++-валидатор. Промежуточные точки сохраняются, однако кривая между ними может отличаться от прежней линейной интерполяции. Прямой API не выполняет проверку столкновений. Ответ возвращает фактические `duration` и `time_scale`; времена могут увеличиться. `validate_limits=false` не отключает обязательную проверку динамики и рабочего диапазона. + +Перед отправкой требуется свежая обратная связь с именами осей, положениями и скоростями; робот должен быть неподвижен. Точка при `t=0` должна совпадать с текущим положением в пределах `web.trajectory_start_tolerance`. Если первая точка позже нуля, текущее положение добавляется в начало. `web.trajectory_state_timeout` ограничивает возраст состояния; `web.trajectory_stopped_velocity` — начальную скорость. Состояние повторно проверяется после расчёта. Повторная прямая отправка во время активной траектории отклоняется; остановка отменяет ещё не завершённую подготовку. + +Внешний ROS-клиент, публикующий прямо в топик JTC или отправляющий ему action, **обходит подготовку MoveIt/web**. Он обязан самостоятельно формировать корректные траектории; это изменение не является общей защитой от произвольных ROS-команд. FRI-фильтр, watchdog и период обмена не менялись. Повторное аппаратное испытание необходимо для оценки шума и фактического слежения. + +### Проверка без оборудования + +После сборки можно выполнить `colcon test --packages-select iiwa_planning iiwa_utils iiwa_web iiwa_controller --python-testing pytest`. Тесты проверяют согласованность семи осей в реальном и симуляционном URDF, пики между точками, результат установленного интерполятора JTC, связку установленного TOTG/Ruckig с pluginlib и отклонение устаревшего состояния web. Они не подключаются к реальному FRI-сеансу. Для нового кода требуется пересборка перечисленных пакетов и `iiwa_config`, `iiwa_description`, `iiwa_bringup`; старый запущенный процесс автоматически не обновляется. + +Проверка покоя в адаптерах подписывается на исходный `/joint_states`: отсутствие скоростей в `RobotState` MoveIt не трактуется как нулевая скорость. `planning.state_timeout` ограничивает возраст исходной телеметрии. Тест подписки использует отдельный локальный ROS domain 231 и синтетические сообщения, без FRI. diff --git a/rosdep.yaml b/rosdep.yaml index 54c02dc..3fd2561 100644 --- a/rosdep.yaml +++ b/rosdep.yaml @@ -18,3 +18,13 @@ python3-fastmcp: pip: packages: [fastmcp] + +# Used by the shared C++ JTC spline validator's Python binding. +pybind11-dev: + ubuntu: + apt: + packages: [pybind11-dev] +python3-numpy: + ubuntu: + apt: + packages: [python3-numpy] diff --git a/src/iiwa_bringup/launch/iiwa.launch.py b/src/iiwa_bringup/launch/iiwa.launch.py index c9abd26..4fd1b19 100644 --- a/src/iiwa_bringup/launch/iiwa.launch.py +++ b/src/iiwa_bringup/launch/iiwa.launch.py @@ -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), @@ -90,6 +92,7 @@ def _runtime_setup(context, *args, **kwargs): "controller_timer": str(settings.digital_twin.webots.controller_timer), "controller": str(settings.controller.controller_path), "initial_positions_file": str(settings.controller.moveit.initial_positions), + "joint_limits_file": str(settings.controller.moveit.joint_limits), }.items(), )) @@ -101,6 +104,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), @@ -112,6 +116,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), diff --git a/src/iiwa_bringup/launch/supported/controllers.launch.py b/src/iiwa_bringup/launch/supported/controllers.launch.py index 06bad4d..cf99ed6 100644 --- a/src/iiwa_bringup/launch/supported/controllers.launch.py +++ b/src/iiwa_bringup/launch/supported/controllers.launch.py @@ -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" diff --git a/src/iiwa_bringup/launch/supported/moveit_nodes.py b/src/iiwa_bringup/launch/supported/moveit_nodes.py index ea78412..7cc9b31 100644 --- a/src/iiwa_bringup/launch/supported/moveit_nodes.py +++ b/src/iiwa_bringup/launch/supported/moveit_nodes.py @@ -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( diff --git a/src/iiwa_bringup/launch/supported/optional_nodes.py b/src/iiwa_bringup/launch/supported/optional_nodes.py index 8f93d94..051928b 100644 --- a/src/iiwa_bringup/launch/supported/optional_nodes.py +++ b/src/iiwa_bringup/launch/supported/optional_nodes.py @@ -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, }], ) diff --git a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py index 018ab88..6ae6988 100644 --- a/src/iiwa_bringup/launch/supported/webots_spawn.launch.py +++ b/src/iiwa_bringup/launch/supported/webots_spawn.launch.py @@ -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), }, ) diff --git a/src/iiwa_config/config/chomp_planning.yaml b/src/iiwa_config/config/chomp_planning.yaml new file mode 100644 index 0000000..9d04a35 --- /dev/null +++ b/src/iiwa_config/config/chomp_planning.yaml @@ -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 diff --git a/src/iiwa_config/config/moveit/joint_limits.yaml b/src/iiwa_config/config/moveit/joint_limits.yaml index 15970a8..72d195f 100644 --- a/src/iiwa_config/config/moveit/joint_limits.yaml +++ b/src/iiwa_config/config/moveit/joint_limits.yaml @@ -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 - \ No newline at end of file + soft_min_position: -2.9627 + soft_max_position: 2.9627 diff --git a/src/iiwa_config/config/ompl_planning.yaml b/src/iiwa_config/config/ompl_planning.yaml new file mode 100644 index 0000000..f7b5400 --- /dev/null +++ b/src/iiwa_config/config/ompl_planning.yaml @@ -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 diff --git a/src/iiwa_config/config/pilz_industrial_motion_planner_planning.yaml b/src/iiwa_config/config/pilz_industrial_motion_planner_planning.yaml new file mode 100644 index 0000000..ae97644 --- /dev/null +++ b/src/iiwa_config/config/pilz_industrial_motion_planner_planning.yaml @@ -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 diff --git a/src/iiwa_config/config/setting.yaml b/src/iiwa_config/config/setting.yaml index cc6b634..be55f5a 100644 --- a/src/iiwa_config/config/setting.yaml +++ b/src/iiwa_config/config/setting.yaml @@ -43,6 +43,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 @@ -51,6 +53,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) diff --git a/src/iiwa_controller/src/IIWAHardwareInterface.cpp b/src/iiwa_controller/src/IIWAHardwareInterface.cpp index 087f258..91f0415 100644 --- a/src/iiwa_controller/src/IIWAHardwareInterface.cpp +++ b/src/iiwa_controller/src/IIWAHardwareInterface.cpp @@ -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) diff --git a/src/iiwa_controller/test/test_hardware.cpp b/src/iiwa_controller/test/test_hardware.cpp index d32d0b9..c39d158 100644 --- a/src/iiwa_controller/test/test_hardware.cpp +++ b/src/iiwa_controller/test/test_hardware.cpp @@ -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); +} diff --git a/src/iiwa_description/urdf/iiwa7.urdf.xacro b/src/iiwa_description/urdf/iiwa7.urdf.xacro index 8ecb474..735108b 100644 --- a/src/iiwa_description/urdf/iiwa7.urdf.xacro +++ b/src/iiwa_description/urdf/iiwa7.urdf.xacro @@ -3,6 +3,8 @@ + + @@ -52,8 +54,8 @@ - -2.97 - 2.97 + ${joint_limits['joint1']['soft_min_position']} + ${joint_limits['joint1']['soft_max_position']} ${initial_positions['joint1']} @@ -64,8 +66,8 @@ - -2.09 - 2.09 + ${joint_limits['joint2']['soft_min_position']} + ${joint_limits['joint2']['soft_max_position']} ${initial_positions['joint2']} @@ -76,8 +78,8 @@ - -2.97 - 2.97 + ${joint_limits['joint3']['soft_min_position']} + ${joint_limits['joint3']['soft_max_position']} ${initial_positions['joint3']} @@ -88,8 +90,8 @@ - -2.09 - 2.09 + ${joint_limits['joint4']['soft_min_position']} + ${joint_limits['joint4']['soft_max_position']} ${initial_positions['joint4']} @@ -100,8 +102,8 @@ - -2.97 - 2.97 + ${joint_limits['joint5']['soft_min_position']} + ${joint_limits['joint5']['soft_max_position']} ${initial_positions['joint5']} @@ -112,8 +114,8 @@ - -2.09 - 2.09 + ${joint_limits['joint6']['soft_min_position']} + ${joint_limits['joint6']['soft_max_position']} ${initial_positions['joint6']} @@ -124,8 +126,8 @@ - -3.05 - 3.05 + ${joint_limits['joint7']['soft_min_position']} + ${joint_limits['joint7']['soft_max_position']} ${initial_positions['joint7']} @@ -153,8 +155,8 @@ - -2.97 - 2.97 + ${joint_limits['joint1']['soft_min_position']} + ${joint_limits['joint1']['soft_max_position']} @@ -166,8 +168,8 @@ - -2.09 - 2.09 + ${joint_limits['joint2']['soft_min_position']} + ${joint_limits['joint2']['soft_max_position']} @@ -179,8 +181,8 @@ - -2.97 - 2.97 + ${joint_limits['joint3']['soft_min_position']} + ${joint_limits['joint3']['soft_max_position']} @@ -192,8 +194,8 @@ - -2.09 - 2.09 + ${joint_limits['joint4']['soft_min_position']} + ${joint_limits['joint4']['soft_max_position']} @@ -205,8 +207,8 @@ - -2.97 - 2.97 + ${joint_limits['joint5']['soft_min_position']} + ${joint_limits['joint5']['soft_max_position']} @@ -218,8 +220,8 @@ - -2.09 - 2.09 + ${joint_limits['joint6']['soft_min_position']} + ${joint_limits['joint6']['soft_max_position']} @@ -231,8 +233,8 @@ - -3.05 - 3.05 + ${joint_limits['joint7']['soft_min_position']} + ${joint_limits['joint7']['soft_max_position']} diff --git a/src/iiwa_description/urdf/joints.xacro b/src/iiwa_description/urdf/joints.xacro index c0b939e..f6d4484 100644 --- a/src/iiwa_description/urdf/joints.xacro +++ b/src/iiwa_description/urdf/joints.xacro @@ -12,57 +12,57 @@ + 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"/> + 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"/> + 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"/> + 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"/> + 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"/> + 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"/> + 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"/> diff --git a/src/iiwa_planning/CMakeLists.txt b/src/iiwa_planning/CMakeLists.txt index 8a4ad33..eb131a5 100644 --- a/src/iiwa_planning/CMakeLists.txt +++ b/src/iiwa_planning/CMakeLists.txt @@ -16,6 +16,50 @@ find_package(moveit_ros_planning REQUIRED) find_package(moveit_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(iiwa_msgs REQUIRED) +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) diff --git a/src/iiwa_planning/iiwa_planning/direct_trajectory.py b/src/iiwa_planning/iiwa_planning/direct_trajectory.py new file mode 100644 index 0000000..4b09a36 --- /dev/null +++ b/src/iiwa_planning/iiwa_planning/direct_trajectory.py @@ -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) diff --git a/src/iiwa_planning/include/iiwa_planning/spline_limits.hpp b/src/iiwa_planning/include/iiwa_planning/spline_limits.hpp new file mode 100644 index 0000000..7eb7d26 --- /dev/null +++ b/src/iiwa_planning/include/iiwa_planning/spline_limits.hpp @@ -0,0 +1,147 @@ +#pragma once +#include +#include +#include +#include +#include + +namespace iiwa_planning { +struct Limit { + double lower, upper, velocity, acceleration, jerk; +}; +struct Sample { + double time; + std::vector position, velocity, acceleration; +}; +// Normalized time u=t/T; exactly the quintic used for JTC +// position/velocity/acceleration points. +inline std::vector 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 hull(const std::vector &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 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 bounds(const std::vector &power) { + std::vector 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 &points, + const std::vector &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 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 diff --git a/src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp b/src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp new file mode 100644 index 0000000..1ad294e --- /dev/null +++ b/src/iiwa_planning/include/iiwa_planning/start_state_monitor.hpp @@ -0,0 +1,78 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +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>{ + {"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( + "/joint_states", rclcpp::SensorDataQoS(), + [this](sensor_msgs::msg::JointState::ConstSharedPtr msg) { + std::lock_guard lock(mutex_); + latest_ = msg; + received_ = std::chrono::steady_clock::now(); + }); + } + bool stationary(const std::vector &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 lock(mutex_); + if (!latest_ || std::chrono::duration( + 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 &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 node_; + rclcpp::Subscription::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 diff --git a/src/iiwa_planning/package.xml b/src/iiwa_planning/package.xml index 0be8b87..c93d53e 100644 --- a/src/iiwa_planning/package.xml +++ b/src/iiwa_planning/package.xml @@ -25,7 +25,13 @@ tf_transformations python3-scipy iiwa_msgs + pluginlib + sensor_msgs + pybind11-dev + python3-numpy + joint_trajectory_controller + python3-pytest ament_lint_auto ament_lint_common diff --git a/src/iiwa_planning/planning_response_plugins.xml b/src/iiwa_planning/planning_response_plugins.xml new file mode 100644 index 0000000..84a7576 --- /dev/null +++ b/src/iiwa_planning/planning_response_plugins.xml @@ -0,0 +1,8 @@ + + + Bound the complete JTC quintic interpolation, stretching time without changing its joint-space curve. + + + Zero stationary endpoint acceleration before Ruckig; reject moving endpoints. + + diff --git a/src/iiwa_planning/scripts/motion_sequence_config.json b/src/iiwa_planning/scripts/motion_sequence_config.json index 6a32c44..431dd47 100644 --- a/src/iiwa_planning/scripts/motion_sequence_config.json +++ b/src/iiwa_planning/scripts/motion_sequence_config.json @@ -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"} ] } diff --git a/src/iiwa_planning/scripts/move_to_pose_server.py b/src/iiwa_planning/scripts/move_to_pose_server.py index 1f33f09..93b69b1 100644 --- a/src/iiwa_planning/scripts/move_to_pose_server.py +++ b/src/iiwa_planning/scripts/move_to_pose_server.py @@ -117,7 +117,8 @@ class IiwaMotionServer(Node): def do_execute(): try: - self._moveit.execute(plan_result.trajectory, controllers=[]) + if not self._moveit.execute(plan_result.trajectory, controllers=[]): + failed.set() except Exception as exc: self.get_logger().error(f"Ошибка выполнения траектории: {exc}") failed.set() diff --git a/src/iiwa_planning/src/limit_spline_dynamics.cpp b/src/iiwa_planning/src/limit_spline_dynamics.cpp new file mode 100644 index 0000000..777a0b2 --- /dev/null +++ b/src/iiwa_planning/src/limit_spline_dynamics.cpp @@ -0,0 +1,140 @@ +#include "iiwa_planning/spline_limits.hpp" +#include "iiwa_planning/start_state_monitor.hpp" +#include +#include +#include +#include + +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 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 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) diff --git a/src/iiwa_planning/src/spline_limits_py.cpp b/src/iiwa_planning/src/spline_limits_py.cpp new file mode 100644 index 0000000..59d1f56 --- /dev/null +++ b/src/iiwa_planning/src/spline_limits_py.cpp @@ -0,0 +1,30 @@ +#include "iiwa_planning/spline_limits.hpp" +#include +#include +namespace py = pybind11; +PYBIND11_MODULE(_iiwa_spline_limits, m) { + m.def("retime", [](const std::vector &t, + const std::vector> &q, + const std::vector> &v, + const std::vector> &a, + const std::vector> &caps) { + if (q.size() != t.size() || v.size() != t.size() || a.size() != t.size()) + throw std::runtime_error("Point array sizes differ"); + std::vector points; + for (size_t i = 0; i < t.size(); ++i) + points.push_back({t[i], q[i], v[i], a[i]}); + std::vector 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 times; + std::vector> 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); + }); +} diff --git a/src/iiwa_planning/test/test_jtc_interpolation.cpp b/src/iiwa_planning/test/test_jtc_interpolation.cpp new file mode 100644 index 0000000..1dd54c6 --- /dev/null +++ b/src/iiwa_planning/test/test_jtc_interpolation.cpp @@ -0,0 +1,36 @@ +#include "iiwa_planning/spline_limits.hpp" +#include +#include +#include + +int main() { + using namespace iiwa_planning; + std::vector samples{{0, {0}, {0}, {0}}, {.2, {1}, {0}, {0}}}; + std::vector 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(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"; +} diff --git a/src/iiwa_planning/test/test_prepare_trajectory.py b/src/iiwa_planning/test/test_prepare_trajectory.py new file mode 100644 index 0000000..7625356 --- /dev/null +++ b/src/iiwa_planning/test/test_prepare_trajectory.py @@ -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 diff --git a/src/iiwa_planning/test/test_response_adapter.cpp b/src/iiwa_planning/test/test_response_adapter.cpp new file mode 100644 index 0000000..6e7b407 --- /dev/null +++ b/src/iiwa_planning/test/test_response_adapter.cpp @@ -0,0 +1,65 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int main() { + auto urdf = urdf::parseURDF( + R"()"); + auto srdf = std::make_shared(); + assert(srdf->initString( + *urdf, + R"()")); + auto model = std::make_shared(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(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 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"; +} diff --git a/src/iiwa_planning/test/test_spline_limits.cpp b/src/iiwa_planning/test/test_spline_limits.cpp new file mode 100644 index 0000000..5f62807 --- /dev/null +++ b/src/iiwa_planning/test/test_spline_limits.cpp @@ -0,0 +1,58 @@ +#include "iiwa_planning/spline_limits.hpp" +#include +#include +#include +using namespace iiwa_planning; +int main() { + // Rest-to-rest quintic: endpoint acceleration is zero, interior peak > 5. + std::vector points{{0, {0}, {0}, {0}}, {1, {1}, {0}, {0}}}; + std::vector 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 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 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 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 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"; +} diff --git a/src/iiwa_planning/test/test_start_state_monitor.cpp b/src/iiwa_planning/test/test_start_state_monitor.cpp new file mode 100644 index 0000000..6333a5e --- /dev/null +++ b/src/iiwa_planning/test/test_start_state_monitor.cpp @@ -0,0 +1,41 @@ +#include "iiwa_planning/start_state_monitor.hpp" +#include +#include +#include +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared( + "iiwa_start_monitor_test", rclcpp::NodeOptions().enable_rosout(false)); + iiwa_planning::StartStateMonitor monitor; + monitor.initialize(node); + auto pub = node->create_publisher( + "/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"; +} diff --git a/src/iiwa_utils/iiwa_utils/converter.py b/src/iiwa_utils/iiwa_utils/converter.py index 8caf5f2..dbe4a29 100644 --- a/src/iiwa_utils/iiwa_utils/converter.py +++ b/src/iiwa_utils/iiwa_utils/converter.py @@ -1,4 +1,6 @@ import yaml +import copy +import math import tempfile from pathlib import Path @@ -28,9 +30,24 @@ def load_robot_description( raise FileNotFoundError(f"Supported file formats: .xacro/.urdf, got: {model_path}") +def effective_joint_limits(data: dict) -> dict: + """Keep MoveIt's YAML overrides inside the same protective bounds as FRI.""" + result = copy.deepcopy(data) + for name, joint in result['joint_limits'].items(): + joint['min_position'] = max(joint['min_position'], joint.get('soft_min_position', joint['min_position'])) + joint['max_position'] = min(joint['max_position'], joint.get('soft_max_position', joint['max_position'])) + if not all(math.isfinite(joint[key]) for key in ('min_position', 'max_position', 'max_velocity', 'max_acceleration', 'max_jerk')): + raise ValueError(f'Non-finite joint limits: {name}') + if joint['min_position'] >= joint['max_position'] or any(joint[key] <= 0 for key in ('max_velocity', 'max_acceleration', 'max_jerk')): + raise ValueError(f'Invalid joint limits: {name}') + return result + + def wrap_for_ros2_params(yaml_path: str, namespace: str) -> str: with open(yaml_path, "r") as f: data = yaml.safe_load(f) + if namespace == 'robot_description_planning': + data = effective_joint_limits(data) wrapped = {namespace: {"ros__parameters": data}} tmp = tempfile.NamedTemporaryFile(mode="w", suffix=".yaml", delete=False) yaml.dump(wrapped, tmp, default_flow_style=False) diff --git a/src/iiwa_utils/iiwa_utils/setting_loader.py b/src/iiwa_utils/iiwa_utils/setting_loader.py index f31f917..2c6d676 100644 --- a/src/iiwa_utils/iiwa_utils/setting_loader.py +++ b/src/iiwa_utils/iiwa_utils/setting_loader.py @@ -63,6 +63,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) @@ -77,6 +79,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) @@ -191,6 +196,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, } @@ -207,6 +215,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")), ) @@ -342,6 +353,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 diff --git a/src/iiwa_utils/setup.py b/src/iiwa_utils/setup.py index 2bab6b0..692ccdb 100644 --- a/src/iiwa_utils/setup.py +++ b/src/iiwa_utils/setup.py @@ -12,6 +12,7 @@ setup( ('share/' + package_name, ['package.xml']), ], install_requires=['setuptools'], + tests_require=['pytest'], zip_safe=True, maintainer='daniel', maintainer_email='grabardm@ml-dev.ru', diff --git a/src/iiwa_utils/test/test_joint_limits_consistency.py b/src/iiwa_utils/test/test_joint_limits_consistency.py new file mode 100644 index 0000000..45bb412 --- /dev/null +++ b/src/iiwa_utils/test/test_joint_limits_consistency.py @@ -0,0 +1,45 @@ +from pathlib import Path +import math +import xml.etree.ElementTree as ET +import yaml +import xacro +import xacro.substitution_args + +SRC = Path(__file__).resolve().parents[2] +LIMITS = SRC / 'iiwa_config/config/moveit/joint_limits.yaml' + + +def test_all_seven_axes_share_limits(monkeypatch): + monkeypatch.setattr(xacro.substitution_args, '_eval_find', lambda p: str(SRC / p)) + data = yaml.safe_load(LIMITS.read_text())['joint_limits'] + for simulate in ('false', 'true'): + robot = ET.fromstring(xacro.process_file( + str(SRC / 'iiwa_description/urdf/iiwa7.urdf.xacro'), + mappings={'simulate': simulate, 'joint_limits_file': str(LIMITS)}).toxml()) + for i, (degrees, speed) in enumerate(zip([170,120,170,120,170,120,175], [98,98,100,130,140,180,180]), 1): + name = f'joint{i}' + limit = data[name] + physical = robot.find(f"joint[@name='{name}']/limit") + assert float(physical.attrib['lower']) == limit['min_position'] + assert float(physical.attrib['upper']) == limit['max_position'] + assert float(physical.attrib['velocity']) == limit['max_velocity'] + assert limit['max_position'] <= math.radians(degrees) + 1e-12 + assert limit['min_position'] >= -math.radians(degrees) - 1e-12 + assert limit['max_velocity'] <= math.radians(speed) + 1e-12 + safety = robot.find(f"joint[@name='{name}']/safety_controller") + assert float(safety.attrib['soft_lower_limit']) == limit['soft_min_position'] + assert float(safety.attrib['soft_upper_limit']) == limit['soft_max_position'] + command = robot.find(f"ros2_control/joint[@name='{name}']/command_interface") + assert float(command.find("param[@name='min']").text) == limit['soft_min_position'] + assert float(command.find("param[@name='max']").text) == limit['soft_max_position'] + assert limit['max_acceleration'] > 0 and limit['max_jerk'] > 0 + + +def test_planning_limits_keep_soft_margin(): + from iiwa_utils.converter import effective_joint_limits + raw = yaml.safe_load(LIMITS.read_text()) + effective = effective_joint_limits(raw) + for name, joint in raw['joint_limits'].items(): + assert effective['joint_limits'][name]['min_position'] == joint['soft_min_position'] + assert effective['joint_limits'][name]['max_position'] == joint['soft_max_position'] + assert joint['min_position'] < joint['soft_min_position'] # input not mutated diff --git a/src/iiwa_web/iiwa_web/config_loader.py b/src/iiwa_web/iiwa_web/config_loader.py index a8e5af0..d91aa45 100644 --- a/src/iiwa_web/iiwa_web/config_loader.py +++ b/src/iiwa_web/iiwa_web/config_loader.py @@ -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 diff --git a/src/iiwa_web/iiwa_web/ros_node.py b/src/iiwa_web/iiwa_web/ros_node.py index c4eb960..59aca80 100644 --- a/src/iiwa_web/iiwa_web/ros_node.py +++ b/src/iiwa_web/iiwa_web/ros_node.py @@ -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: diff --git a/src/iiwa_web/iiwa_web/trajectory.py b/src/iiwa_web/iiwa_web/trajectory.py index c4693c5..046a7e8 100644 --- a/src/iiwa_web/iiwa_web/trajectory.py +++ b/src/iiwa_web/iiwa_web/trajectory.py @@ -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="Последние лог-записи траекторного модуля") diff --git a/src/iiwa_web/package.xml b/src/iiwa_web/package.xml index 45dab2e..df14537 100644 --- a/src/iiwa_web/package.xml +++ b/src/iiwa_web/package.xml @@ -14,6 +14,8 @@ tf2_ros tf2_py moveit_msgs + iiwa_planning + python3-yaml ament_copyright ament_flake8 diff --git a/src/iiwa_web/setup.py b/src/iiwa_web/setup.py index 3db5655..029aab7 100644 --- a/src/iiwa_web/setup.py +++ b/src/iiwa_web/setup.py @@ -19,6 +19,7 @@ setup( 'python-multipart', 'fastmcp', ], + tests_require=['pytest'], zip_safe=True, maintainer='daniel', maintainer_email='grabardm@ml-dev.ru', diff --git a/src/iiwa_web/test/test_trajectory_admission.py b/src/iiwa_web/test/test_trajectory_admission.py new file mode 100644 index 0000000..a0c6876 --- /dev/null +++ b/src/iiwa_web/test/test_trajectory_admission.py @@ -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