grabardm/lightweight-cobot
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

fix(experement): harden tool transfers and tightening at wrist limits

Browse Source 
Exit the rack with a graded +Y move after taking or docking a tool, continue through home after a dock, and fall back from work to home when a joint plan is rejected. Wait for fresh zero-velocity joint states before planning and after joint motion, restore a screw to the planning scene before a pick retry, and twist only the flange joint while simulating tightening so the arm does not leave a reachable IK branch.
This commit is contained in:
Даниил Грабарь
2026-11-09 01:29:36 +10:00
parent a6c6474db9
commit 41144ecd2a
6 changed files with 215 additions and 70 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_experement/doc/README.md
+1 -1
View File
@@ -90,7 +90,7 @@ Webots. В `execute` первое движение начинается не р
инструментами и признаком `home_reached`.
- Винты не вставляются. После подтверждённой передачи у отверстия винт
убирается из сцены, отверстие остаётся пустым. В опытах 14–20 берётся
`torque_tool`, подходит к каждому отверстию, выполняет поворот на 60° и
`torque_tool`, подходит к каждому отверстию, выполняет поворот на 45° и
возвращается в стойку. Это видимая имитация затяжки без установки винта.
Управляемые ветви: опыт 4 — один неудачный захват люка; 5 — первая посадка
src/iiwa_experement/doc/reference-control-qa.md
+10 -7
View File
@@ -61,9 +61,10 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
## Политика движений
- После захвата инструмент поднимается вертикально на 16 см, и только затем
выполняется боковое или суставное перемещение. Так корпус инструмента не
проходит сквозь текстуры стойки.
- После захвата инструмент поднимается на 8 см, затем смещается на 10 см по
+Y от стойки, и только потом выполняется суставное перемещение. Полный
вертикальный подъём на 14–16 см упирается в предел запястья; боковой выход
даёт тот же зазор и не ломает достижимость `work`.
- Между операциями используется ближняя нейтральная поза `work`. `home`
остаётся финальной позой маршрута, позой перед установкой инструмента в
стойку и аварийной реконфигурацией.
@@ -139,9 +140,11 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
В опытах 14, 15 и 16–20 после передач винтов берётся `torque_tool`. Для
каждого отверстия выполняется `tighten_simulated`: подъём над отверстием,
осевое опускание, поворот на 60°, возврат и осевой отход. Если у дальнего
отверстия запястье упирается в предел, поворот выполняется в противоположную
сторону. Отверстие остаётся пустым; винт не вставляется. Инструмент
осевое опускание и поворот последнего сустава на 45°. Вращается только
фланец, поэтому инструмент поворачивается вокруг своей оси, не уходя в другую
ветвь IK. Если положительное направление упирается в предел, используется
отрицательное. После поворота робот уходит в `work`, как после передачи
винта. Отверстие остаётся пустым; винт не вставляется. Инструмент
возвращается в стойку, затем робот уходит домой.
## Ветви отказов
@@ -162,7 +165,7 @@ ros2 launch iiwa_experement experiment.launch.py experiment_id:=1 \
- Только симуляция Webots; FRI и физический робот не запускаются.
- Винты не вставляются; после передачи отверстие остаётся пустым.
- Затяжка имитационная: `torque_tool` подходит к отверстию, поворачивается на
60° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
45° вокруг оси инструмента и отходит. Винт к этому моменту уже передан
оператору и удалён из сцены.
- Прикреплённые объекты MoveItPy в установленной версии приводят к ложным
столкновениям. Переносимый объект ведёт supervisor; статическая сцена,
src/iiwa_experement/iiwa_experement/motion.py
+58 -1
View File
@@ -157,6 +157,7 @@ class MotionAdapter:
self.joint_correction_attempts = max(1, int(joint_correction_attempts))
self.joint_position_tolerance = float(joint_position_tolerance)
self._joint_positions: tuple[float, ...] | None = None
self._joint_velocities: tuple[float, ...] | None = None
self.stopped = False
self.stop_reason: str | None = None
self._last_joint_time = None
@@ -168,6 +169,45 @@ class MotionAdapter:
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)
if len(message.velocity) == 7:
self._joint_velocities = tuple(
float(value) for value in message.velocity
)
def joint_positions(self) -> tuple[float, ...] | None:
return self._joint_positions
def wait_stationary(
self, timeout_sec: float = 3.0, tolerance: float = 0.01,
) -> bool:
"""Wait for fresh, near-zero joint velocities before planning.
The planning pipeline rejects a motion whose recorded start state still
carries velocity. The controller action can finish before the next
zero-velocity sample reaches the planning server, so gate the next
plan on a short stable window.
"""
if self.node is None or self._freshness_check is not None:
return True
import rclpy
deadline = time.monotonic() + max(0.0, float(timeout_sec))
stable_since = None
while time.monotonic() < deadline:
rclpy.spin_once(self.node, timeout_sec=0.02)
velocities = self._joint_velocities
if velocities is None or len(velocities) != 7 or any(
abs(value) > tolerance for value in velocities
):
stable_since = None
continue
stable_since = stable_since or time.monotonic()
if time.monotonic() - stable_since >= 0.20:
return True
self.node.get_logger().warning(
'joint_states did not settle before planning'
)
return False
def _default_freshness(self) -> tuple[bool, str]:
if self._last_joint_time is None:
@@ -338,6 +378,7 @@ class MotionAdapter:
def move_named(self, name: str, speed: float = 0.2, label: str = '') -> MotionResult:
label = label or f'move_named({name})'
self.wait_stationary()
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)
@@ -353,7 +394,23 @@ class MotionAdapter:
# 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)
result = self._move_named_service(name, speed, label)
if result.success or name != 'work':
return result
# The tool rack can leave the arm in a branch from which the close
# 'work' pose is unreachable. Fall back to the globally safe home
# configuration rather than aborting the route.
home = self.geometry.named_joints.get('home')
if home is not None:
self.reset_stop()
fallback = self.move_joints(home, speed, f'{label}:home')
if fallback.success:
if self.node is not None:
self.node.get_logger().warning(
f'{label}: work unreachable; reconfigured to home'
)
return fallback
return result
def move_joints(
self,
src/iiwa_experement/iiwa_experement/operations.py
+96 -27
View File
@@ -304,6 +304,7 @@ class ReferenceOperations:
# false contact with the target itself.
self.bridge.remove(remove_before_contact)
self._require_synchronized()
self.motion.wait_stationary()
transfer = self._above(goal, transfer_gap)
approach = self._above(goal, approach_gap)
self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer')
@@ -333,7 +334,9 @@ class ReferenceOperations:
self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat')
self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear')
def _contact_with_home_retry(self, goal: Transform, label: str, **kwargs) -> None:
def _contact_with_home_retry(
self, goal: Transform, label: str, *, prepare=None, **kwargs,
) -> None:
"""Contact a target, reconfiguring through home only if planning fails.
The routes use the closer 'work' pose between tasks. A few tray and
@@ -348,6 +351,8 @@ class ReferenceOperations:
raise
self._emit('reconfigure', label=label, target='home',
reason=str(error))
if prepare is not None:
prepare()
self.motion.reset_stop()
named = self.motion.move_named('home', 0.3)
if not getattr(named, 'success', False):
@@ -392,30 +397,34 @@ class ReferenceOperations:
remove_before_contact=f'grant_{tool_name}',
)
self.supervisor.command('attach_tool', tool_name)
# Lift straight up, clear of the magazine, before any lateral or
# joint-space move. Without this the tool body sweeps through the
# rack obstacles while the arm travels to 'work'. The sockets sit
# at slightly different reachable heights, so accept the highest
# clearance that plans.
lift_error = None
for gap in (0.16, 0.14, 0.12):
# Lift a little, then translate along +Y away from the magazine
# before any large joint-space move. A full-height vertical lift
# runs into the wrist limit; the lateral exit is what actually
# keeps the tool body out of the rack textures.
transfer = self._above(goal, self.geometry.transfer_gap)
self._move(
transfer, 'lin', self.geometry.speeds['approach'],
f'take_tool({tool_name}):lift',
)
for offset in (0.10, 0.06, 0.03):
exit_pose = Transform.from_xyz_rpy(
[0.0, offset, 0.0], [0.0, 0.0, 0.0],
parent='world', child='rack_exit_offset',
).compose_relaxed(transfer)
label = (
f'take_tool({tool_name}):lift' if gap == 0.16
else f'take_tool({tool_name}):lift:{gap:.2f}'
f'take_tool({tool_name}):exit' if offset == 0.10
else f'take_tool({tool_name}):exit:{offset:.2f}'
)
try:
self._move(
self._above(goal, gap), 'lin',
self.geometry.speeds['approach'], label,
exit_pose, 'lin', self.geometry.speeds['transfer'],
label,
)
break
except OperationError as error:
if 'Планирование' not in str(error):
raise
self.motion.reset_stop()
lift_error = error
else:
raise lift_error
self._emit('take_tool', object=tool_name, phase='done',
tool=tool_name, tcp=goal.xyz)
return OperationResult(True, 'take_tool', f'{tool_name} mounted', tuple(self.events))
@@ -443,6 +452,28 @@ class ReferenceOperations:
)
self._require_synchronized()
self._retreat(goal, f'dock_tool({tool_name})')
# Step out of the rack along +Y before any joint-space move; the
# axial retreat alone can leave the wrist against its limit.
transfer = self._above(goal, self.geometry.transfer_gap)
for offset in (0.10, 0.06, 0.03):
exit_pose = Transform.from_xyz_rpy(
[0.0, offset, 0.0], [0.0, 0.0, 0.0],
parent='world', child='rack_exit_offset',
).compose_relaxed(transfer)
label = (
f'dock_tool({tool_name}):exit' if offset == 0.10
else f'dock_tool({tool_name}):exit:{offset:.2f}'
)
try:
self._move(
exit_pose, 'lin', self.geometry.speeds['transfer'],
label,
)
break
except OperationError as error:
if 'Планирование' not in str(error):
raise
self.motion.reset_stop()
self._emit('dock_tool', object=tool_name, phase='done')
return OperationResult(True, 'dock_tool', f'{tool_name} docked', tuple(self.events))
except Exception as error:
@@ -575,9 +606,18 @@ class ReferenceOperations:
self._require_synchronized()
screw_pose = self.supervisor.object_pose(def_name)
goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
def restore_screw() -> None:
# The first attempt removed the screw from the planning scene
# before its transfer was rejected. Restore it for the retry.
self.bridge.add_world(
def_name, self.geometry.screw_collision, screw_pose,
)
self._require_synchronized()
self._contact_with_home_retry(
goal, f'pick_screw({screw_id})',
remove_before_contact=def_name,
prepare=restore_screw,
)
self.supervisor.command('grab_screw', screw_id)
self._retreat(goal, f'pick_screw({screw_id})')
@@ -618,7 +658,7 @@ class ReferenceOperations:
raise
# ------------------------------------------------------------------ tightening
def tighten_hole(self, hole_id: str, twist_deg: float = 60.0) -> OperationResult:
def tighten_hole(self, hole_id: str, twist_deg: float = 45.0) -> OperationResult:
"""Simulate tightening at an empty hole with the torque tool.
The screw has already been handed to the virtual operator and removed
@@ -641,35 +681,64 @@ class ReferenceOperations:
goal = tcp_goal(target, tip)
self._contact(goal, f'tighten_hole({hole_id})')
twist_error = None
twisted = goal
twist_done = False
# Prefer a pure flange rotation: it twists the tool about its own
# axis without moving the arm to another IK branch. A Cartesian
# twist can push joint 6 into its limit, after which 'work' is no
# longer reachable.
joints = None
provider = getattr(self.motion, 'joint_positions', None)
if provider is not None:
joints = provider()
if joints is not None and len(joints) == 7:
for sign in (1.0, -1.0):
target_joints = list(joints)
target_joints[6] += math.radians(sign * twist_deg)
self.motion.wait_stationary()
result = self.motion.move_joints(
target_joints, self.geometry.speeds['contour'],
f'tighten_hole({hole_id}):joint_twist',
)
if getattr(result, 'success', False):
twist_done = True
self._emit('tighten_twist', object=hole_id,
joint=7, delta_deg=sign * twist_deg)
break
reset = getattr(self.motion, 'reset_stop', None)
if reset is not None:
reset()
if not twist_done:
for suffix, offset in (
('twist:positive', twist_deg),
('twist:negative', -twist_deg),
):
twisted = goal.compose_relaxed(Transform.from_xyz_rpy(
candidate = goal.compose_relaxed(Transform.from_xyz_rpy(
[0.0, 0.0, 0.0], [0.0, 0.0, offset],
parent='tcp', child='tighten_twist',
))
self.motion.wait_stationary()
try:
self._move(
twisted, 'lin', self.geometry.speeds['contour'],
candidate, 'lin', self.geometry.speeds['contour'],
f'tighten_hole({hole_id}):{suffix}',
)
twisted = candidate
twist_done = True
break
except OperationError as error:
# Holes on the far side of the body can sit against the
# wrist limit in one direction. A failed plan executes no
# motion, so the opposite twist is safe to try.
# Holes on the far side of the body can sit against
# the wrist limit in one direction. A failed plan
# executes no motion, so the opposite twist is safe.
if 'Планирование' not in str(error):
raise
self.motion.reset_stop()
twist_error = error
else:
if not twist_done:
raise twist_error
self._move(
goal, 'lin', self.geometry.speeds['contour'],
f'tighten_hole({hole_id}):return',
)
self._retreat(goal, f'tighten_hole({hole_id})')
# The caller's following 'work' move is the verified clearance
# move, as after a screw handoff. A separate Cartesian retreat
# from the twisted pose can be rejected by the wrist limits.
self._emit('tighten_simulated', object=hole_id, phase='done',
twist_deg=twist_deg)
return OperationResult(
src/iiwa_experement/iiwa_experement/reference_scenarios.py
+3 -4
View File
@@ -81,7 +81,8 @@ def _cap_steps(
_home(),
ReferenceStep('dock_tool', ('vacuum',)),
])
if finish_home:
# Always leave the rack through home: the dock retreat can sit against a
# wrist limit from which 'work' is not reachable.
steps.append(_home())
return steps
@@ -113,7 +114,6 @@ def _sealant_steps(
_home(),
ReferenceStep('dock_tool', ('sealant_tool',)),
])
if finish_home:
steps.append(_home())
return steps
@@ -144,9 +144,9 @@ def _screw_steps(
])
steps.append(_home())
steps.append(ReferenceStep('dock_tool', ('magnetic_tool',)))
steps.append(_home())
if 'torque_tool' in spec.tool_names:
steps.extend([
_work(),
ReferenceStep('take_tool', ('torque_tool',)),
_work(),
])
@@ -157,7 +157,6 @@ def _screw_steps(
])
steps.append(_home())
steps.append(ReferenceStep('dock_tool', ('torque_tool',)))
if finish_home:
steps.append(_home())
return steps
src/iiwa_experement/test/test_operations.py
+28 -11
View File
@@ -1,4 +1,5 @@
"""Operation sequences are visible, ordered and fail closed."""
import math
from pathlib import Path
import pytest
@@ -75,6 +76,9 @@ class FakeMotion:
self.moves = []
self.named = []
self.fail_labels = set()
self.joint_moves = []
self.joint_values = (0.0,) * 7
self.fail_positive_joint = False
def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
self.moves.append((label, planner, speed, goal))
@@ -86,9 +90,22 @@ class FakeMotion:
self.named.append((name, speed))
return MotionResult(True, 'ok', f'move_named({name})')
def joint_positions(self):
return self.joint_values
def move_joints(self, joints, speed=0.2, label=''):
self.joint_moves.append((label, tuple(joints)))
if self.fail_positive_joint and joints[6] > 0:
return MotionResult(False, 'injected joint failure', label)
self.joint_values = tuple(joints)
return MotionResult(True, 'ok', label)
def reset_stop(self):
return None
def wait_stationary(self, *args, **kwargs):
return True
def make_operations(experiment_id):
spec = load_scenario(experiment_id, SCENES)
@@ -122,6 +139,7 @@ def test_take_tool_follows_the_visible_axial_route():
'take_tool(vacuum):approach',
'take_tool(vacuum):contact',
'take_tool(vacuum):lift',
'take_tool(vacuum):exit',
]
assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
assert bridge.attached == []
@@ -138,7 +156,7 @@ def test_dock_tool_returns_the_tool_to_its_socket():
assert result.success
assert supervisor.commands[0][:2] == ('dock_tool', 'vacuum')
assert bridge.added_world[0][0] == 'grant_vacuum'
assert labels(motion)[-1] == 'dock_tool(vacuum):clear'
assert labels(motion)[-1] == 'dock_tool(vacuum):exit'
def test_cap_pick_and_place_use_the_hatch_node_transform():
@@ -213,11 +231,11 @@ def test_tighten_hole_is_a_visible_revolution_without_a_screw():
'tighten_hole(hole_1):approach',
'tighten_hole(hole_1):contact',
]
twist = motion.moves[names.index('tighten_hole(hole_1):twist:positive')][3]
contact = motion.moves[names.index('tighten_hole(hole_1):contact')][3]
assert twist.xyz == pytest.approx(contact.xyz)
relative = contact.inverse().compose_relaxed(twist)
assert abs(relative.rotation[2][2] - 1.0) < 1e-9
assert motion.joint_moves
label, joints = motion.joint_moves[0]
assert label == 'tighten_hole(hole_1):joint_twist'
assert joints[:6] == (0.0,) * 6
assert joints[6] == pytest.approx(math.radians(45.0))
# The operation is a simulation only: no supervisor command changes state.
assert supervisor.commands == []
assert bridge.attached == []
@@ -232,13 +250,12 @@ def test_screw_pick_reconfigures_to_home_when_work_is_blocked():
assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
def test_tighten_hole_reverses_the_twist_when_one_direction_is_blocked():
def test_tighten_hole_reverses_when_the_flange_limit_blocks_one_direction():
operations, supervisor, bridge, motion, geometry = make_operations(14)
motion.fail_labels.add('tighten_hole(hole_1):twist:positive')
motion.fail_positive_joint = True
assert operations.tighten_hole('hole_1').success
names = labels(motion)
assert 'tighten_hole(hole_1):twist:positive' in names
assert 'tighten_hole(hole_1):twist:negative' in names
assert len(motion.joint_moves) == 2
assert motion.joint_moves[1][1][6] == pytest.approx(math.radians(-45.0))
def test_failed_motion_stops_the_operation_and_emits_failure():