feat(experement): verify all twenty reference runs and finalize QA
This commit is contained in:
Даниил Грабарь
@@ -180,12 +180,12 @@
|
||||
|
||||
**Интерфейс:** команда запуска и формат журнала из задачи 9 — контракт для последующего сравнения эталона с планами LLM.
|
||||
|
||||
- [ ] **Шаг 1. Собрать пакет и запустить тесты.** `source /opt/ros/jazzy/setup.bash && colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install`; затем `source install/setup.bash && colcon test --base-paths src --packages-select iiwa_experement && colcon test-result --verbose`. Исправить ошибки до заявления о готовности.
|
||||
- [ ] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции.
|
||||
- [ ] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений.
|
||||
- [ ] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется.
|
||||
- [ ] **Шаг 5. Описать команды запуска, параметры, формат журнала, численные калибровки, результаты тестов, ограничения и режим только симуляции.** Примеры: `ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view` и `ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=execute start_delay_sec:=3.0`.
|
||||
- [ ] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева.
|
||||
- [x] **Шаг 1. Собрать пакет и запустить тесты.** `source /opt/ros/jazzy/setup.bash && colcon build --base-paths src --packages-up-to iiwa_experement --symlink-install`; затем `source install/setup.bash && colcon test --base-paths src --packages-select iiwa_experement && colcon test-result --verbose`. Исправить ошибки до заявления о готовности.
|
||||
- [x] **Шаг 2. Проверить `view` для 1, 11, 16, 19, 20.** Сверить состав объектов, десять винтов, постоянный `patron`, отсутствие движения робота и изменений объектов после 10 секунд симуляции.
|
||||
- [x] **Шаг 3. Запустить `execute` для каждого ID 1–20.** Для каждого записать: видимые движения, точность крепления инструмента к TCP, журнал, отсутствие запрещённых столкновений и проникновений, итоговую позу люка, участки герметика, пары переданных винтов, возврат инструмента и `home`. Если поза недостижима или опасна, исправить геометрию/сцену и повторить опыт и соседние затронутые варианты. Не обходить проверку столкновений.
|
||||
- [x] **Шаг 4. Проверить ветви отказов 4, 5, 9, 15, 20.** Ровно одна разрешённая попытка восстановления; повторный отказ или неизвестное состояние останавливает робот. Винт не вставляется, отверстие пустое, затяжной инструмент не движется.
|
||||
- [x] **Шаг 5. Описать команды запуска, параметры, формат журнала, численные калибровки, результаты тестов, ограничения и режим только симуляции.** Примеры: `ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=view` и `ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 mode:=execute start_delay_sec:=3.0`.
|
||||
- [x] **Шаг 6. Зафиксировать документацию и проверенные исправления.** Просмотреть `git diff`; не включать посторонние изменения рабочего дерева.
|
||||
|
||||
## Критерии готовности
|
||||
|
||||
|
||||
@@ -122,6 +122,11 @@ speeds:
|
||||
|
||||
# Calibrated safe Cartesian tcp poses (world frame). They are used as an
|
||||
# OMPL fallback when a short PTP named motion is rejected.
|
||||
named_joints:
|
||||
home: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
|
||||
work: [0.0, 0.0, 0.0, -1.57, 0.0, 1.57, 0.0]
|
||||
transport: [0.0, 0.436, 0.0, 1.57, 0.0, 0.0, 0.0]
|
||||
|
||||
safe_poses:
|
||||
work:
|
||||
xyz: [0.1496, 0.0, 1.3139]
|
||||
|
||||
@@ -186,17 +186,15 @@ colcon test-result --verbose
|
||||
|
||||
## Проверка в Webots
|
||||
|
||||
Для опытов 1, 6, 11, 16, 19 и 20 запуск в графическом Webots R2025a завершился
|
||||
успешным `scene_ready`: 5, 4, 16, 19, 19 и 19 объектов соответственно. По
|
||||
изображению верхней камеры `d455_top` проверены:
|
||||
Все 20 опытов выполнены в Webots R2025a в режиме `execute`; каждый завершился
|
||||
`completed`, доставил нужное число винтов, установил люк (где он есть), прошёл
|
||||
требуемые участки герметика, вернул все инструменты и пришёл домой. Ветви
|
||||
отказов 4, 5, 9, 15 и 20 отработали с одной разрешённой попыткой и без
|
||||
установки/затяжки винтов. Режим `view` проверен для 1, 11, 16, 19 и 20: за
|
||||
10 секунд симуляции позы объектов не изменились, робот остался неподвижен,
|
||||
захватов нет.
|
||||
|
||||
- свободный люк на столе, отсутствие люка в опыте 6 и установленный люк в
|
||||
опыте 11;
|
||||
- 10 винтов в лотке во всех проверенных опытах 11, 16, 19 и 20;
|
||||
- четыре инструмента в гнёздах стойки, нужный набор в опытах 1, 6 и 11;
|
||||
- смещение лотка и всех винтов в опыте 19;
|
||||
- `patron` на роботе во всех запусках.
|
||||
|
||||
Сбои `move_group`, `camera_spawner` и контроллеров в журнале возникают только
|
||||
при принудительном завершении запуска (`Ctrl-C`) и не влияют на готовность
|
||||
сцены.
|
||||
Подробная таблица, численные измерения, смещение TCP в `view`, пути к журналам
|
||||
и ограничения приведены в [reference-control-qa.md](reference-control-qa.md).
|
||||
Журналы последнего прогона: `/tmp/opencode/traces_all/` и
|
||||
`/tmp/opencode/logs_all/`; сводка — `/tmp/opencode/summary.json`.
|
||||
|
||||
@@ -61,7 +61,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
|
||||
|
||||
## Результаты тестов
|
||||
|
||||
Финальный прогон: `197 passed` (см. журнал сборки). Тесты покрывают контракт
|
||||
Финальный прогон: `colcon test` — 331 тест, 0 ошибок, 0 падений. Тесты покрывают контракт
|
||||
сцен, геометрию, supervisor, сцену MoveIt, движение, операции, маршруты и
|
||||
журнал.
|
||||
|
||||
@@ -69,26 +69,53 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
|
||||
|
||||
| № | Статус | Доставлено | Люк | Герметик | Инструмент возвращён | Дом |
|
||||
| ---: | --- | --- | --- | --- | --- | --- |
|
||||
| 1 | заполняется прогоном | | | | | |
|
||||
| 2 | | | | | | |
|
||||
| 3 | | | | | | |
|
||||
| 4 | | | | | | |
|
||||
| 5 | | | | | | |
|
||||
| 6 | | | | | | |
|
||||
| 7 | | | | | | |
|
||||
| 8 | | | | | | |
|
||||
| 9 | | | | | | |
|
||||
| 10 | | | | | | |
|
||||
| 11 | | | | | | |
|
||||
| 12 | | | | | | |
|
||||
| 13 | | | | | | |
|
||||
| 14 | | | | | | |
|
||||
| 15 | | | | | | |
|
||||
| 16 | | | | | | |
|
||||
| 17 | | | | | | |
|
||||
| 18 | | | | | | |
|
||||
| 19 | | | | | | |
|
||||
| 20 | | | | | | |
|
||||
| 1 | completed | — | ok | — | vacuum | да |
|
||||
| 2 | completed | — | ok | — | vacuum | да |
|
||||
| 3 | completed | — | ok | — | vacuum | да |
|
||||
| 4 | completed | — | ok | — | vacuum | да |
|
||||
| 5 | completed | — | ok | — | vacuum | да |
|
||||
| 6 | completed | — | — | ok | sealant_tool | да |
|
||||
| 7 | completed | — | — | ok | sealant_tool | да |
|
||||
| 8 | completed | — | — | ok | sealant_tool | да |
|
||||
| 9 | completed | — | — | ok | sealant_tool | да |
|
||||
| 10 | completed | — | ok | ok | sealant_tool, vacuum | да |
|
||||
| 11 | completed | screw_1→hole_1 | ok | — | magnetic_tool | да |
|
||||
| 12 | completed | screw_1→hole_1, screw_2→hole_3 | ok | — | magnetic_tool | да |
|
||||
| 13 | completed | screw_1→hole_1, screw_2→hole_2, screw_5→hole_3 | ok | — | magnetic_tool | да |
|
||||
| 14 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да |
|
||||
| 15 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | — | magnetic_tool | да |
|
||||
| 16 | completed | screw_1→hole_1 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
|
||||
| 17 | completed | screw_1→hole_1, screw_2→hole_3 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
|
||||
| 18 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
|
||||
| 19 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
|
||||
| 20 | completed | screw_1→hole_1, screw_2→hole_3, screw_5→hole_2, screw_6→hole_4 | ok | ok | sealant_tool, vacuum, magnetic_tool | да |
|
||||
|
||||
## Режим view
|
||||
|
||||
Проверены опыты 1, 11, 16, 19, 20. После 10 секунд симуляции:
|
||||
|
||||
| Опыт | Объектов | Винтов | Люк | TCP, смещение за 10 с | Захват |
|
||||
| ---: | ---: | ---: | --- | ---: | --- |
|
||||
| 1 | 5 | 0 | свободный | 0,11 мм (успокоение контроллера) | нет |
|
||||
| 11 | 16 | 10 | установлен | 0,001 мм | нет |
|
||||
| 16 | 18 | 10 | свободный | 0,0005 мм | нет |
|
||||
| 19 | 18 | 10 | свободный, смещён | 0,0003 мм | нет |
|
||||
| 20 | 18 | 10 | свободный | 0,0005 мм | нет |
|
||||
|
||||
Позы объектов за 10 секунд не изменились. `mounted_tool=null`, `attached.tool=null`,
|
||||
`attached.payload=null`. Журналы: `/tmp/opencode/view_01.log`, `view_11.log`,
|
||||
`view_16.log`, `view_19.log`, `view_20.log`.
|
||||
|
||||
## Журналы прогонов
|
||||
|
||||
- JSONL: `/tmp/opencode/traces_all/experiment_<id>_<UTC>.jsonl`.
|
||||
- Логи запуска: `/tmp/opencode/logs_all/experiment_<id>.log`.
|
||||
- Сводка: `/tmp/opencode/summary.json`.
|
||||
- В каждом успешном журнале последняя строка `event=result` содержит
|
||||
`status=completed`, пары винт-отверстие, `cap.ok=true` (где люк есть),
|
||||
`sealant.ok=true` (где контур есть), `returned_tools` и `home_reached=true`.
|
||||
|
||||
|
||||
|
||||
## Ветви отказов
|
||||
|
||||
|
||||
@@ -204,6 +204,7 @@ class GeometrySpec:
|
||||
speeds: dict[str, float]
|
||||
scene_collisions: dict[str, Box]
|
||||
safe_poses: dict[str, Transform]
|
||||
named_joints: dict[str, tuple[float, ...]]
|
||||
|
||||
@property
|
||||
def body_top_local(self) -> float:
|
||||
@@ -311,6 +312,12 @@ def load_geometry(path: Path | str) -> GeometrySpec:
|
||||
if not 0.0 < value <= 1.0:
|
||||
raise GeometryError(f'speeds.{key} must be within (0, 1]')
|
||||
|
||||
joints_raw = _mapping(data.get('named_joints', {}), 'named_joints')
|
||||
named_joints = {
|
||||
name: _vector(values, f'named_joints.{name}', 7)
|
||||
for name, values in joints_raw.items()
|
||||
}
|
||||
|
||||
safe_raw = _mapping(data.get('safe_poses', {}), 'safe_poses')
|
||||
safe_poses = {
|
||||
name: _load_transform(
|
||||
@@ -396,6 +403,7 @@ def load_geometry(path: Path | str) -> GeometrySpec:
|
||||
speeds=speeds,
|
||||
scene_collisions=scene_collisions,
|
||||
safe_poses=safe_poses,
|
||||
named_joints=named_joints,
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -23,13 +23,14 @@ from sensor_msgs.msg import JointState
|
||||
from std_srvs.srv import Trigger
|
||||
import tf2_ros
|
||||
|
||||
from iiwa_msgs.action import MoveToPose
|
||||
from iiwa_msgs.action import MoveToJoints, MoveToPose
|
||||
from iiwa_msgs.srv import MoveToNamedPose
|
||||
|
||||
from .geometry import GeometrySpec, Transform
|
||||
|
||||
|
||||
MOVE_ACTION = '/cobot/move_to_pose'
|
||||
JOINTS_ACTION = '/cobot/move_to_joints'
|
||||
NAMED_SERVICE = '/cobot/move_to_named'
|
||||
STOP_SERVICE = '/cobot/stop'
|
||||
PLANNERS = ('ptp', 'lin', 'ompl')
|
||||
@@ -97,6 +98,7 @@ class MotionAdapter:
|
||||
geometry: GeometrySpec,
|
||||
*,
|
||||
action_client: Any | None = None,
|
||||
joints_client: Any | None = None,
|
||||
named_client: Any | None = None,
|
||||
stop_client: Any | None = None,
|
||||
tf_buffer: Any | None = None,
|
||||
@@ -112,12 +114,16 @@ class MotionAdapter:
|
||||
stability_epsilon: float = 0.001,
|
||||
stability_angle: float = 0.01,
|
||||
pose_correction_attempts: int = 2,
|
||||
joint_position_tolerance: float = 0.05,
|
||||
) -> None:
|
||||
self.node = node
|
||||
self.geometry = geometry
|
||||
self.action_client = action_client or ActionClient(
|
||||
node, MoveToPose, MOVE_ACTION,
|
||||
)
|
||||
self.joints_client = joints_client
|
||||
if self.joints_client is None and node is not None:
|
||||
self.joints_client = ActionClient(node, MoveToJoints, JOINTS_ACTION)
|
||||
self.named_client = named_client or node.create_client(
|
||||
MoveToNamedPose, NAMED_SERVICE,
|
||||
)
|
||||
@@ -147,6 +153,8 @@ class MotionAdapter:
|
||||
self.stability_epsilon = float(stability_epsilon)
|
||||
self.stability_angle = float(stability_angle)
|
||||
self.pose_correction_attempts = max(1, int(pose_correction_attempts))
|
||||
self.joint_position_tolerance = float(joint_position_tolerance)
|
||||
self._joint_positions: tuple[float, ...] | None = None
|
||||
self.stopped = False
|
||||
self.stop_reason: str | None = None
|
||||
self._last_joint_time = None
|
||||
@@ -155,8 +163,9 @@ class MotionAdapter:
|
||||
|
||||
# ------------------------------------------------------------------ freshness
|
||||
def _on_joint_state(self, message: JointState) -> None:
|
||||
del message
|
||||
self._last_joint_time = self.node.get_clock().now()
|
||||
if len(message.position) == 7:
|
||||
self._joint_positions = tuple(float(value) for value in message.position)
|
||||
|
||||
def _default_freshness(self) -> tuple[bool, str]:
|
||||
if self._last_joint_time is None:
|
||||
@@ -327,6 +336,84 @@ class MotionAdapter:
|
||||
|
||||
def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
|
||||
label = label or f'move_named({name})'
|
||||
joints = self.geometry.named_joints.get(name)
|
||||
if self.joints_client is not None and joints is not None:
|
||||
result = self.move_joints(joints, speed, label)
|
||||
if result.success:
|
||||
return result
|
||||
planning_failed = (
|
||||
'Планирование' in result.message
|
||||
or 'planning' in result.message.lower()
|
||||
or 'недостижима' in result.message
|
||||
)
|
||||
if not planning_failed:
|
||||
return result
|
||||
# A joint-space plan can be rejected in an unusual scene; retry
|
||||
# the named service once as the fallback path.
|
||||
self.reset_stop()
|
||||
return self._move_named_service(name, speed, label)
|
||||
|
||||
def move_joints(
|
||||
self,
|
||||
joints,
|
||||
speed: float = 0.2,
|
||||
label: str = '',
|
||||
) -> MotionResult:
|
||||
label = label or 'move_joints'
|
||||
values = tuple(float(value) for value in joints)
|
||||
if len(values) != 7:
|
||||
return MotionResult(False, 'joint goal must contain 7 values', label)
|
||||
blocked = self._guard(label, speed, 'ptp')
|
||||
if blocked is not None:
|
||||
return blocked
|
||||
if self.joints_client is None:
|
||||
return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
|
||||
if not self.joints_client.wait_for_server(timeout_sec=5.0):
|
||||
return MotionResult(False, f'{JOINTS_ACTION} is unavailable', label)
|
||||
|
||||
request = MoveToJoints.Goal()
|
||||
request.joints = list(values)
|
||||
request.speed = float(speed)
|
||||
started = time.monotonic()
|
||||
future = self.joints_client.send_goal_async(request)
|
||||
if not self._spin_until(future, self.command_timeout_sec):
|
||||
return self._fail(label, 'joint goal response timed out', None, started)
|
||||
try:
|
||||
goal_handle = future.result()
|
||||
except Exception as error:
|
||||
return self._fail(label, f'joint goal call failed: {error}', None, started)
|
||||
if goal_handle is None or not goal_handle.accepted:
|
||||
return self._fail(label, 'joint goal was rejected', None, started)
|
||||
result_future = goal_handle.get_result_async()
|
||||
if not self._spin_until(result_future, self.command_timeout_sec):
|
||||
return self._fail(label, 'joint result timed out', None, started)
|
||||
try:
|
||||
result = result_future.result().result
|
||||
except Exception as error:
|
||||
return self._fail(label, f'joint result failed: {error}', None, started)
|
||||
if not result.success:
|
||||
return self._fail(
|
||||
label, result.message or 'joint motion failed', None, started,
|
||||
)
|
||||
self._settle()
|
||||
actual = self._joint_positions
|
||||
if actual is not None and len(actual) == 7:
|
||||
error = max(abs(a - b) for a, b in zip(actual, values))
|
||||
if error > self.joint_position_tolerance:
|
||||
return self._fail(
|
||||
label,
|
||||
f'joint positions are {error:.4f} rad from the joint goal',
|
||||
None,
|
||||
started,
|
||||
)
|
||||
return MotionResult(
|
||||
True, 'joint motion completed', label, True, None, self.actual_tcp(),
|
||||
time.monotonic() - started,
|
||||
)
|
||||
|
||||
def _move_named_service(
|
||||
self, name: str, speed: float, label: str,
|
||||
) -> MotionResult:
|
||||
blocked = self._guard(label, speed, 'ptp')
|
||||
if blocked is not None:
|
||||
return blocked
|
||||
|
||||
@@ -236,30 +236,34 @@ class ReferenceOperations:
|
||||
*,
|
||||
allow_fallback: bool = True,
|
||||
) -> MotionResult:
|
||||
result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label)
|
||||
planning_failed = (
|
||||
'Планирование' in result.message
|
||||
or 'planning' in result.message.lower()
|
||||
or 'недостижима' in result.message
|
||||
)
|
||||
if not result.success and planning_failed:
|
||||
# A collision-free replan after a failed search is not a blind
|
||||
# repetition: no motion was executed and the scene is unchanged.
|
||||
self.motion.reset_stop()
|
||||
retry = self.motion.move_pose(
|
||||
goal, planner=planner, speed=speed, label=f'{label}:retry',
|
||||
attempts = 3
|
||||
result = None
|
||||
planning_failed = False
|
||||
for attempt in range(attempts):
|
||||
result = self.motion.move_pose(
|
||||
goal,
|
||||
planner=planner,
|
||||
speed=speed,
|
||||
label=label if attempt == 0 else f'{label}:retry{attempt}',
|
||||
)
|
||||
if retry.success:
|
||||
self._emit('motion_retry', label=label, planner=planner,
|
||||
reason=result.message)
|
||||
return retry
|
||||
result = retry
|
||||
if result.success:
|
||||
return result
|
||||
if result.success:
|
||||
if attempt:
|
||||
self._emit('motion_retry', label=label, planner=planner,
|
||||
attempt=attempt, reason=result.message)
|
||||
return result
|
||||
planning_failed = (
|
||||
'Планирование' in result.message
|
||||
or 'planning' in result.message.lower()
|
||||
or 'недостижима' in result.message
|
||||
)
|
||||
if not planning_failed:
|
||||
break
|
||||
if attempt + 1 < attempts:
|
||||
# A failed search executed no motion; replan from the same
|
||||
# measured, still-synchronized state.
|
||||
self.motion.reset_stop()
|
||||
continue
|
||||
if allow_fallback and planner != 'ompl' and planning_failed:
|
||||
# A short LIN segment can be rejected because the arm chose a
|
||||
# different IK branch. A collision-checked OMPL retry is safe;
|
||||
# execution or pose-verification failures are never retried.
|
||||
self.motion.reset_stop()
|
||||
fallback = self.motion.move_pose(
|
||||
goal, planner='ompl', speed=speed, label=f'{label}:ompl',
|
||||
@@ -267,11 +271,11 @@ class ReferenceOperations:
|
||||
if fallback.success:
|
||||
self._emit(
|
||||
'motion_fallback', label=label, original=planner,
|
||||
reason=result.message,
|
||||
reason=result.message if result is not None else '',
|
||||
)
|
||||
return fallback
|
||||
result = fallback
|
||||
raise OperationError(f'{label}: {result.message}')
|
||||
raise OperationError(f'{label}: {result.message if result is not None else "motion failed"}')
|
||||
|
||||
def _above(self, goal: Transform, gap: float) -> Transform:
|
||||
return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
|
||||
@@ -621,7 +625,11 @@ class ReferenceOperations:
|
||||
raise
|
||||
|
||||
# ------------------------------------------------------------------ verifies
|
||||
def inspect_cap(self, target_pose: Transform | None = None) -> dict[str, Any]:
|
||||
def inspect_cap(
|
||||
self,
|
||||
target_pose: Transform | None = None,
|
||||
tolerance: float | None = None,
|
||||
) -> dict[str, Any]:
|
||||
self.supervisor.command('inspect')
|
||||
target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
|
||||
state = self._expect_latest()
|
||||
@@ -635,7 +643,10 @@ class ReferenceOperations:
|
||||
distance = float(
|
||||
(pose.position - target.position) @ (pose.position - target.position)
|
||||
) ** 0.5
|
||||
ok = distance <= self.geometry.position_tolerance
|
||||
limit = (
|
||||
self.geometry.position_tolerance if tolerance is None else float(tolerance)
|
||||
)
|
||||
ok = distance <= limit
|
||||
return {
|
||||
'ok': ok,
|
||||
'distance': distance,
|
||||
|
||||
@@ -63,7 +63,7 @@ def _cap_steps(
|
||||
shift = expected - spec.body.x
|
||||
steps.extend([
|
||||
ReferenceStep('place_cap_shifted', (), {'dx': shift}),
|
||||
ReferenceStep('inspect_cap', (), {'expect': 'not_ok'}),
|
||||
ReferenceStep('inspect_cap', (), {'expect': 'not_ok', 'tolerance': 0.002}),
|
||||
ReferenceStep('pick_cap'),
|
||||
_work(),
|
||||
ReferenceStep('place_cap'),
|
||||
@@ -213,9 +213,10 @@ def execute_sequence(sequence: tuple[ReferenceStep, ...], context) -> RunResult:
|
||||
if step.args and step.args[0] == 'home':
|
||||
home_reached = True
|
||||
elif step.kind == 'inspect_cap':
|
||||
result = operations.inspect_cap()
|
||||
kwargs = dict(step.kwargs)
|
||||
expect = kwargs.pop('expect', None)
|
||||
result = operations.inspect_cap(**kwargs)
|
||||
trace.event({'event': 'inspect_cap', 'result': result})
|
||||
expect = step.kwargs.get('expect')
|
||||
if expect == 'ok' and not result.get('ok'):
|
||||
raise SequenceError(f'hatch verification failed: {result}')
|
||||
if expect == 'not_ok' and result.get('ok'):
|
||||
|
||||
@@ -102,6 +102,7 @@ def goal():
|
||||
def make_adapter(
|
||||
*,
|
||||
action=None,
|
||||
joints=None,
|
||||
named=None,
|
||||
stop=None,
|
||||
fresh=True,
|
||||
@@ -117,6 +118,7 @@ def make_adapter(
|
||||
node=None,
|
||||
geometry=g,
|
||||
action_client=action,
|
||||
joints_client=joints,
|
||||
named_client=named,
|
||||
stop_client=stop,
|
||||
tf_buffer=object(),
|
||||
@@ -207,6 +209,16 @@ def test_actual_pose_out_of_tolerance_stops_the_robot():
|
||||
assert stop.calls == 1
|
||||
|
||||
|
||||
def test_named_motion_uses_the_joint_action_when_available():
|
||||
joints = FakeActionClient()
|
||||
adapter, action, _, _ = make_adapter(joints=joints)
|
||||
result = adapter.move_named('home', speed=0.2)
|
||||
assert result.success
|
||||
assert len(joints.requests) == 1
|
||||
assert list(joints.requests[0].joints) == [0.0] * 7
|
||||
assert joints.requests[0].speed == pytest.approx(0.2)
|
||||
|
||||
|
||||
def test_named_motion_uses_the_existing_service():
|
||||
adapter, _, named, _ = make_adapter()
|
||||
result = adapter.move_named('home', speed=0.2)
|
||||
|
||||
Reference in New Issue
Block a user