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

feat(experement): implement reference operations and stable scene sync

Browse Source
This commit is contained in:
Даниил Грабарь
2026-11-08 15:38:36 +10:00
parent c58aa56757
commit de8c2eb9ee
13 changed files with 1322 additions and 137 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_experement/config/geometry.yaml
+14 -4
View File
@@ -128,18 +128,28 @@ scene_collisions:
type: box
size: [1.28, 1.08, 0.045]
offset_z: 0.7665
fixture:
# The fixture is a frame, not a solid block: a low base plate and two
# side rails. A full box would cover the hatch seat and make placement
# impossible.
fixture_base:
type: box
size: [0.350, 0.220, 0.041]
offset_z: 0.0105
size: [0.350, 0.220, 0.010]
offset_z: -0.005
fixture_rail:
type: box
size: [0.014, 0.156, 0.016]
offset_z: 0.008
offset_x: 0.104
body:
type: box
size: [0.300, 0.170, 0.00915]
offset_z: 0.000695
rack:
# The magazine is represented by its base plate only. Keep it just
# below the tool sockets so the vertical tool bodies do not overlap it.
type: box
size: [0.420, 0.140, 0.010]
offset_z: 0.005
offset_z: -0.015
tray_base:
type: box
size: [0.120, 0.070, 0.0024]
src/iiwa_experement/config/moveit_cpp.yaml
+61
View File
@@ -0,0 +1,61 @@
# Package-local MoveItCpp settings for the reference executor.
#
# The stock iiwa_config copy publishes the MoveItPy maintained scene on
# /monitored_planning_scene (the same topic move_group publishes) but listens
# to /planning_scene. The executor publishes its diffs to move_group, so the
# MoveItPy monitor must instead listen to move_group's maintained scene.
#
# publish_planning_scene_topic is the topic this monitor subscribes to;
# monitored_planning_scene_topic is where it republishes its own scene.
planning_scene_monitor_options:
name: "planning_scene_monitor"
robot_description: "robot_description"
joint_state_topic: "/joint_states"
attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor"
publish_planning_scene_topic: "/monitored_planning_scene"
monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene"
wait_for_initial_state_timeout: 10.0
planning_pipelines:
pipeline_names: ["ompl", "pilz_industrial_motion_planner", "chomp"]
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
max_velocity_scaling_factor: 0.5
max_acceleration_scaling_factor: 1.0
ompl_rrtc:
plan_request_params:
planning_attempts: 1
planning_pipeline: ompl
planner_id: "RRTConnectkConfigDefault"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 1.0
pilz_ptp:
plan_request_params:
planning_attempts: 1
planning_pipeline: pilz_industrial_motion_planner
planner_id: "PTP"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 0.8
pilz_lin:
plan_request_params:
planning_attempts: 1
planning_pipeline: pilz_industrial_motion_planner
planner_id: "LIN"
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 0.8
chomp_planner:
plan_request_params:
planning_attempts: 1
planning_pipeline: chomp
max_velocity_scaling_factor: 1.0
max_acceleration_scaling_factor: 1.0
planning_time: 1.5
src/iiwa_experement/iiwa_experement/grant_supervisor.py
+50 -29
View File
@@ -110,7 +110,8 @@ class GrantSupervisorNode(Node):
self.spec: ScenarioSpec = load_scenario(experiment_id, scene_config)
self.geometry: GeometrySpec = load_geometry(geometry_path)
self.state = ObjectState(self.spec, self.geometry, read_only=read_only)
self.attached: tuple[str, str] | None = None
self.tool_held: str | None = None
self.payload: tuple[str, str] | None = None
self.last_command_result: dict[str, Any] | None = None
self.rack_poses = {
name: entry['pose'] for name, entry in self.state.tools.items()
@@ -174,6 +175,15 @@ class GrantSupervisorNode(Node):
field.setSFBool(True)
except Exception:
pass
# All managed objects are teleported by the supervisor. Leaving a
# physics node attached lets Webots integrate huge velocities while
# the pose is overwritten every step.
try:
physics = node.getField('physics')
if physics is not None:
physics.remove()
except Exception:
pass
def _pose_of(self, node) -> Transform | None:
if node is None:
@@ -223,24 +233,25 @@ class GrantSupervisorNode(Node):
names.append('grant_cap')
return names
def _node_for_attachment(self) -> Any | None:
if self.attached is None:
def _tool_node(self):
if self.tool_held is None:
return None
kind, name = self.attached
if kind == 'tool':
return self.nodes.get(f'grant_{name}')
return self.nodes.get(f'grant_{self.tool_held}')
def _payload_node(self):
if self.payload is None:
return None
kind, name = self.payload
if kind == 'cap':
return self.nodes.get('grant_cap')
if kind == 'screw':
return self.nodes.get(screw_def_name(name))
return None
def _relative_transform(self) -> Transform | None:
if self.attached is None:
def _payload_relative(self) -> Transform | None:
if self.payload is None:
return None
kind, name = self.attached
if kind == 'tool':
return self.geometry.tool_tcp_to_nodes[name]
kind, _ = self.payload
if kind == 'cap':
return self.geometry.cap_tcp_to_node
if kind == 'screw':
@@ -248,19 +259,24 @@ class GrantSupervisorNode(Node):
return None
def follow(self) -> None:
if self.attached is None:
return
tcp = self.tcp_pose()
node = self._node_for_attachment()
relative = self._relative_transform()
if tcp is None or node is None or relative is None:
if tcp is None:
return
if self.tool_held is not None:
node = self._tool_node()
relative = self.geometry.tool_tcp_to_nodes[self.tool_held]
if node is not None:
pose = tcp.compose_relaxed(relative)
self._set_pose(node, pose)
kind, name = self.attached
if kind == 'tool' and name in self.state.tools:
self.state.tools[name]['pose'] = pose
elif kind == 'cap':
self.state.tools[self.tool_held]['pose'] = pose
if self.payload is not None:
node = self._payload_node()
relative = self._payload_relative()
if node is not None and relative is not None:
pose = tcp.compose_relaxed(relative)
self._set_pose(node, pose)
kind, name = self.payload
if kind == 'cap':
self.state.cap_pose = pose
elif kind == 'screw' and name in self.state.screws:
self.state.screws[name]['pose'] = pose
@@ -309,6 +325,10 @@ class GrantSupervisorNode(Node):
def _apply_impl(self, operation: str, command: dict[str, Any], request_id: str):
if operation == 'inspect':
return True, None
# A command can arrive before the per-step follow update. Apply the
# current tcp to the held object first, otherwise the attachment
# appears one control step behind (about 11 mm at transfer speed).
self.follow()
tcp = self.tcp_pose()
if tcp is None:
raise CommandError('actual tcp pose is unavailable')
@@ -319,7 +339,7 @@ class GrantSupervisorNode(Node):
if object_pose is None:
raise CommandError(f'tool node grant_{kind} is unavailable')
self.state.attach_tool(kind, tcp, object_pose, request_id=request_id)
self.attached = ('tool', kind)
self.tool_held = kind
return True, None
if operation == 'dock_tool':
node = self.nodes.get(f'grant_{kind}')
@@ -330,7 +350,7 @@ class GrantSupervisorNode(Node):
self.state.dock_tool(
kind, tcp, rack, object_pose, request_id=request_id,
)
self.attached = None
self.tool_held = None
self._set_pose(node, rack)
self.state.tools[kind]['pose'] = rack
return True, None
@@ -340,7 +360,7 @@ class GrantSupervisorNode(Node):
if object_pose is None:
raise CommandError('hatch node is unavailable')
self.state.grab_cap(tcp, object_pose, request_id=request_id)
self.attached = ('cap', 'cap')
self.payload = ('cap', 'cap')
return True, None
if operation == 'place_cap':
target = self._command_pose(command)
@@ -349,7 +369,7 @@ class GrantSupervisorNode(Node):
if object_pose is None:
raise CommandError('hatch node is unavailable')
self.state.place_cap(target, tcp, object_pose, request_id=request_id)
self.attached = None
self.payload = None
self._set_pose(node, target)
self._freeze(node)
self.state.cap_pose = target
@@ -360,7 +380,7 @@ class GrantSupervisorNode(Node):
if object_pose is None:
raise CommandError(f'screw node for {kind} is unavailable')
self.state.grab_screw(kind, tcp, object_pose, request_id=request_id)
self.attached = ('screw', kind)
self.payload = ('screw', kind)
return True, None
if operation == 'handoff_screw':
hole = str(command.get('target_name') or '')
@@ -372,7 +392,7 @@ class GrantSupervisorNode(Node):
self.state.handoff_screw(
kind, hole, target, tcp, object_pose, request_id=request_id,
)
self.attached = None
self.payload = None
if node is not None:
try:
node.remove()
@@ -394,9 +414,10 @@ class GrantSupervisorNode(Node):
payload['ready'] = self.patron_node is not None and all(
name in self.nodes for name in self._required_defs()
)
payload['attached'] = (
None if self.attached is None else list(self.attached)
)
payload['attached'] = {
'tool': self.tool_held,
'payload': None if self.payload is None else list(self.payload),
}
tcp = self.tcp_pose()
payload['tcp'] = transform_json(tcp)
payload['last_command_result'] = self.last_command_result
src/iiwa_experement/iiwa_experement/motion.py
+39 -3
View File
@@ -107,7 +107,10 @@ class MotionAdapter:
position_tolerance: float | None = None,
orientation_tolerance: float | None = None,
command_timeout_sec: float = 90.0,
settle_time_sec: float = 0.5,
settle_time_sec: float = 1.0,
stability_timeout_sec: float = 5.0,
stability_epsilon: float = 0.001,
stability_angle: float = 0.01,
) -> None:
self.node = node
self.geometry = geometry
@@ -139,6 +142,9 @@ class MotionAdapter:
)
self.command_timeout_sec = float(command_timeout_sec)
self.settle_time_sec = max(0.0, float(settle_time_sec))
self.stability_timeout_sec = max(0.0, float(stability_timeout_sec))
self.stability_epsilon = float(stability_epsilon)
self.stability_angle = float(stability_angle)
self.stopped = False
self.stop_reason: str | None = None
self._last_joint_time = None
@@ -283,8 +289,7 @@ class MotionAdapter:
base_goal, started,
)
self._settle()
actual = self.actual_tcp()
actual = self._settle_and_measure()
if actual is None:
return self._fail(label, 'actual tcp pose is unavailable after motion', base_goal, started)
distance, angle = _pose_distance(actual, base_goal)
@@ -343,6 +348,37 @@ class MotionAdapter:
timeout_sec=min(0.05, max(0.0, end - time.monotonic())),
)
def _settle_and_measure(self) -> Transform | None:
"""Wait until the controller stops moving before verifying tcp.
The action result is delivered when the trajectory finishes, but the
joint trajectory controller can still be settling. Treating the
first sample as final caused false out-of-tolerance stops.
"""
self._settle()
import rclpy
deadline = time.monotonic() + self.stability_timeout_sec
last = self.actual_tcp()
stable_since = None
while time.monotonic() < deadline and self.node is not None:
rclpy.spin_once(self.node, timeout_sec=0.05)
current = self.actual_tcp()
if current is None:
last = None
stable_since = None
continue
if last is not None:
distance, angle = _pose_distance(current, last)
if distance <= self.stability_epsilon and angle <= self.stability_angle:
stable_since = stable_since or time.monotonic()
if time.monotonic() - stable_since >= 0.25:
return current
else:
stable_since = None
last = current
return last
def _fail(
self,
label: str,
src/iiwa_experement/iiwa_experement/operations.py
+573
View File
@@ -0,0 +1,573 @@
"""Reference operations: tools, hatch, sealant contour and screw handoff."""
from __future__ import annotations
import json
import math
import uuid
from dataclasses import dataclass, field
from typing import Any, Callable, Iterable
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
from webots_ros2_msgs.srv import SetString
from .geometry import (
GeometrySpec,
Transform,
sealant_point,
tcp_goal,
world_target,
)
from .motion import MotionAdapter, MotionResult
from .planning_scene import (
PlanningSceneBridge,
attached_cap,
attached_screw,
attached_tool,
screw_object_id,
)
from .scenario import ScenarioSpec
COMMAND_SERVICE = '/iiwa_experement/object_command'
STATE_TOPIC = '/iiwa_experement/object_state'
class OperationError(RuntimeError):
"""An operation could not be completed safely."""
@dataclass(frozen=True)
class OperationResult:
success: bool
operation: str
message: str
events: tuple[dict[str, Any], ...] = field(default_factory=tuple)
def _translation(xyz) -> Transform:
return Transform.from_xyz_rpy(
[float(value) for value in xyz], [0.0, 0.0, 0.0],
parent='tcp', child='offset',
)
class SupervisorClient:
"""Synchronous facade over the Webots attachment supervisor."""
def __init__(self, node: Node, *, timeout_sec: float = 15.0) -> None:
self.node = node
self.timeout_sec = float(timeout_sec)
self.latest: dict[str, Any] | None = None
self._client = node.create_client(SetString, COMMAND_SERVICE)
self._subscription = node.create_subscription(
String, STATE_TOPIC, self._on_state, 10,
)
def _on_state(self, message: String) -> None:
try:
self.latest = json.loads(message.data)
except Exception:
pass
def _spin(self, seconds: float = 0.05) -> None:
import time
finish = time.monotonic() + seconds
while time.monotonic() < finish:
rclpy.spin_once(self.node, timeout_sec=0.02)
def wait_ready(self, timeout_sec: float = 60.0) -> dict[str, Any]:
import time
end = time.monotonic() + timeout_sec
while time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.05)
if self.latest and self.latest.get('ready'):
return self.latest
raise OperationError('supervisor did not become ready')
def wait_version(self, minimum: int, timeout_sec: float | None = None) -> dict[str, Any]:
import time
end = time.monotonic() + (self.timeout_sec if timeout_sec is None else timeout_sec)
while time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.05)
if self.latest and self.latest.get('version', -1) >= minimum:
return self.latest
raise OperationError(f'supervisor state version {minimum} was not published')
def command(
self,
op: str,
object_id: str = '',
*,
target: Transform | None = None,
target_name: str = '',
expected_state: dict[str, Any] | None = None,
) -> dict[str, Any]:
if not self._client.wait_for_service(timeout_sec=5.0):
raise OperationError(f'{COMMAND_SERVICE} is unavailable')
request_id = uuid.uuid4().hex
payload: dict[str, Any] = {
'request_id': request_id,
'op': op,
'object': object_id,
'target_name': target_name,
}
if target is not None:
payload['target'] = _transform_payload(target)
if expected_state is not None:
payload['expected_state'] = expected_state
request = SetString.Request()
request.value = json.dumps(payload)
future = self._client.call_async(request)
self._spin(0.0)
import time
end = time.monotonic() + self.timeout_sec
while not future.done() and time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.05)
if not future.done():
raise OperationError(f'supervisor command {op} timed out')
accepted = bool(future.result().success)
# The detailed result is published atomically after the transition.
state = self.latest
if state is None or state.get('last_command_result', {}).get('request_id') != request_id:
end = time.monotonic() + 2.0
while time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.02)
state = self.latest
if state and state.get('last_command_result', {}).get('request_id') == request_id:
break
result = (state or {}).get('last_command_result', {})
if not accepted or not result.get('accepted', False):
reason = result.get('error') or 'supervisor rejected the command'
raise OperationError(f'{op}({object_id or "-"}) rejected: {reason}')
return state or {}
@property
def objects(self) -> dict[str, Any]:
return dict((self.latest or {}).get('objects') or {})
def object_pose(self, name: str) -> Transform:
entry = self.objects.get(name)
if not isinstance(entry, dict):
raise OperationError(f'object {name!r} is not present in the supervisor state')
return _transform_from_payload(entry, child=name)
def logical_cap(self) -> dict[str, Any]:
return dict((self.latest or {}).get('cap') or {})
def logical_screws(self) -> dict[str, Any]:
return dict((self.latest or {}).get('screws') or {})
def _transform_payload(transform: Transform) -> dict[str, Any]:
return {
'xyz': [float(value) for value in transform.xyz],
'rotation': [
[float(value) for value in row] for row in transform.rotation
],
}
def _transform_from_payload(data: dict[str, Any], child: str = 'object') -> Transform:
xyz = data.get('xyz')
rotation = data.get('rotation')
if not isinstance(xyz, (list, tuple)) or len(xyz) != 3:
raise OperationError('object pose has no valid xyz')
if not isinstance(rotation, (list, tuple)) or len(rotation) != 3:
raise OperationError('object pose has no valid rotation')
if any(not isinstance(row, (list, tuple)) or len(row) != 3 for row in rotation):
raise OperationError('object pose rotation must be a 3x3 matrix')
return Transform(
xyz=tuple(float(value) for value in xyz),
rotation=tuple(tuple(float(value) for value in row) for row in rotation),
parent='world',
child=child,
)
class ReferenceOperations:
"""Sequence of visible robot operations for one experiment."""
def __init__(
self,
node: Node,
spec: ScenarioSpec,
geometry: GeometrySpec,
supervisor: SupervisorClient,
bridge: PlanningSceneBridge,
motion: MotionAdapter,
*,
event_sink: Callable[[dict[str, Any]], None] | None = None,
) -> None:
self.node = node
self.spec = spec
self.geometry = geometry
self.supervisor = supervisor
self.bridge = bridge
self.motion = motion
self._event_sink = event_sink
self.events: list[dict[str, Any]] = []
self.completed_sealant: set[str] = set()
self.last_sealant_point: tuple[str, float] | None = None
# ------------------------------------------------------------------ helpers
def _emit(self, operation: str, **payload) -> None:
event = {'operation': operation, **payload}
self.events.append(event)
if self._event_sink is not None:
self._event_sink(event)
def _require(self, condition: bool, message: str) -> None:
if not condition:
raise OperationError(message)
def _move(
self,
goal: Transform,
planner: str,
speed: float,
label: str,
*,
allow_fallback: bool = True,
) -> MotionResult:
result = self.motion.move_pose(goal, planner=planner, speed=speed, label=label)
if result.success:
return result
planning_failed = (
'Планирование' in result.message
or 'planning' in result.message.lower()
or 'недостижима' in result.message
)
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',
)
if fallback.success:
self._emit(
'motion_fallback', label=label, original=planner,
reason=result.message,
)
return fallback
result = fallback
raise OperationError(f'{label}: {result.message}')
def _above(self, goal: Transform, gap: float) -> Transform:
return goal.compose_relaxed(_translation([0.0, 0.0, -gap]))
def _contact(
self,
goal: Transform,
label: str,
*,
approach_gap: float | None = None,
transfer_gap: float | None = None,
remove_before_contact: str | None = None,
remove_phase: str = 'transfer',
) -> None:
approach_gap = (
self.geometry.approach_gap if approach_gap is None else approach_gap
)
transfer_gap = (
self.geometry.transfer_gap if transfer_gap is None else transfer_gap
)
if remove_before_contact is not None and remove_phase == 'transfer':
# The object is the intended contact target. Remove it from the
# world scene for the whole approach so the attached copy cannot
# duplicate it and the short axial moves are not rejected by a
# false contact with the target itself.
self.bridge.remove(remove_before_contact)
self._require_synchronized()
transfer = self._above(goal, transfer_gap)
approach = self._above(goal, approach_gap)
self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:transfer')
try:
self._move(
approach, 'ompl', self.geometry.speeds['approach'],
f'{label}:approach',
)
except OperationError as error:
message = str(error)
if 'Планирование' not in message and 'недостижима' not in message:
raise
# A pre-contact point can sit on a different IK branch. The
# collision-checked direct path to the contact is safer than
# forcing an infeasible axial segment.
self._emit('approach_skipped', label=f'{label}:approach',
reason=message)
self.motion.reset_stop()
if remove_before_contact is not None and remove_phase == 'contact':
self.bridge.remove(remove_before_contact)
self._require_synchronized()
self._move(goal, 'lin', self.geometry.speeds['contact'], f'{label}:contact')
def _retreat(self, goal: Transform, label: str) -> None:
approach = self._above(goal, self.geometry.approach_gap)
transfer = self._above(goal, self.geometry.transfer_gap)
self._move(approach, 'lin', self.geometry.speeds['approach'], f'{label}:retreat')
self._move(transfer, 'ompl', self.geometry.speeds['transfer'], f'{label}:clear')
def _require_synchronized(self) -> None:
try:
self.bridge.assert_synchronized(*self.bridge.expected_sets())
except Exception as error:
raise OperationError(f'planning scene is not synchronized: {error}') from error
def _expect_latest(self, timeout: float = 2.0) -> dict[str, Any]:
import time
end = time.monotonic() + timeout
while time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.02)
if self.supervisor.latest is not None:
return self.supervisor.latest
raise OperationError('supervisor state is unavailable')
# ------------------------------------------------------------------ tools
def take_tool(self, tool_name: str) -> OperationResult:
self._emit('take_tool', object=tool_name, phase='start')
try:
self._require_synchronized()
tool_pose = self.supervisor.object_pose(f'grant_{tool_name}')
goal = tcp_goal(tool_pose, self.geometry.tool_tcp_to_nodes[tool_name])
self._contact(
goal, f'take_tool({tool_name})',
remove_before_contact=f'grant_{tool_name}',
)
self.supervisor.command('attach_tool', tool_name)
self._retreat(goal, f'take_tool({tool_name})')
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))
except Exception as error:
self._emit('take_tool', object=tool_name, phase='failed', error=str(error))
raise
def dock_tool(self, tool_name: str) -> OperationResult:
self._emit('dock_tool', object=tool_name, phase='start')
try:
self._require_synchronized()
rack_pose = world_target(self.spec, self.geometry, f'tool:{tool_name}')
goal = tcp_goal(rack_pose, self.geometry.tool_tcp_to_nodes[tool_name])
# Remove the held tool from MoveIt before entering the socket
# region. The supervisor keeps following tcp, and the contact
# pose is verified by both the motion adapter and supervisor.
self.bridge.remove(f'grant_{tool_name}')
self._require_synchronized()
self._contact(goal, f'dock_tool({tool_name})')
self.supervisor.command('dock_tool', tool_name)
self.bridge.add_world(
f'grant_{tool_name}',
self.geometry.tool_collisions[tool_name],
rack_pose,
)
self._require_synchronized()
self._retreat(goal, f'dock_tool({tool_name})')
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:
self._emit('dock_tool', object=tool_name, phase='failed', error=str(error))
raise
# ------------------------------------------------------------------ hatch
def pick_cap(self) -> OperationResult:
self._emit('pick_cap', object='cap', phase='start')
try:
self._require_synchronized()
cap_pose = self.supervisor.object_pose('grant_cap')
goal = tcp_goal(cap_pose, self.geometry.cap_tcp_to_node)
self._contact(
goal, 'pick_cap', remove_before_contact='grant_cap',
)
self.supervisor.command('grab_cap', 'cap')
self._retreat(goal, 'pick_cap')
self._emit('pick_cap', object='cap', phase='done', tcp=goal.xyz)
return OperationResult(True, 'pick_cap', 'hatch grasped', tuple(self.events))
except Exception as error:
self._emit('pick_cap', object='cap', phase='failed', error=str(error))
raise
def place_cap(self, target_pose: Transform | None = None) -> OperationResult:
self._emit('place_cap', object='cap', phase='start')
try:
self._require_synchronized()
target = target_pose or world_target(self.spec, self.geometry, 'cap_seat')
goal = tcp_goal(target, self.geometry.cap_tcp_to_node)
self._contact(
goal, 'place_cap',
remove_before_contact='grant_cap',
remove_phase='contact',
)
self.supervisor.command('place_cap', 'cap', target=target)
self.bridge.add_world(
'grant_cap', self.geometry.cap_collision, target,
)
self._require_synchronized()
self._retreat(goal, 'place_cap')
self._emit('place_cap', object='cap', phase='done', target=target.xyz)
return OperationResult(True, 'place_cap', 'hatch placed', tuple(self.events))
except Exception as error:
self._emit('place_cap', object='cap', phase='failed', error=str(error))
raise
# ------------------------------------------------------------------ sealant
def dispense(
self,
segment_name: str,
start_fraction: float = 0.0,
end_fraction: float = 1.0,
*,
lift: bool = True,
) -> OperationResult:
self._emit('dispense', object=segment_name, phase='start')
try:
self._require_synchronized()
start = sealant_point(
self.geometry, self.spec, segment_name, start_fraction,
)
end = sealant_point(
self.geometry, self.spec, segment_name, end_fraction,
)
tip = self.geometry.tool_work_tips['sealant_tool']
start_goal = tcp_goal(start, tip)
end_goal = tcp_goal(end, tip)
if start_fraction <= 0.0 and end_fraction >= 1.0:
self._contact(start_goal, f'dispense({segment_name})')
else:
self._contact(start_goal, f'dispense({segment_name}):resume')
self._move(
end_goal, 'lin', self.geometry.speeds['contour'],
f'dispense({segment_name}):path', allow_fallback=False,
)
self.last_sealant_point = (segment_name, end_fraction)
self.completed_sealant.add(segment_name)
if lift:
self._retreat(end_goal, f'dispense({segment_name})')
self._emit('dispense', object=segment_name, phase='done',
start_fraction=start_fraction, end_fraction=end_fraction)
return OperationResult(True, 'dispense', f'{segment_name} completed', tuple(self.events))
except Exception as error:
self._emit('dispense', object=segment_name, phase='failed', error=str(error))
raise
def dispense_contour(self, segments: Iterable[str]) -> OperationResult:
"""Traverse adjacent sides continuously without lifting the nozzle."""
segments = list(segments)
self._emit('dispense_contour', segments=list(segments), phase='start')
try:
self._require_synchronized()
tip = self.geometry.tool_work_tips['sealant_tool']
first = sealant_point(self.geometry, self.spec, segments[0], 0.0)
first_goal = tcp_goal(first, tip)
self._contact(first_goal, 'dispense_contour:start')
for index, segment in enumerate(segments):
end_fraction = 1.0 if index < len(segments) - 1 else 1.0
end = sealant_point(self.geometry, self.spec, segment, end_fraction)
end_goal = tcp_goal(end, tip)
self._move(
end_goal, 'lin', self.geometry.speeds['contour'],
f'dispense_contour:{segment}', allow_fallback=False,
)
self.completed_sealant.add(segment)
self.last_sealant_point = (segment, 1.0)
self._emit('dispense', object=segment, phase='done')
self._retreat(end_goal, 'dispense_contour')
self._emit('dispense_contour', segments=list(segments), phase='done')
return OperationResult(True, 'dispense_contour', 'contour completed', tuple(self.events))
except Exception as error:
self._emit('dispense_contour', phases='failed', error=str(error))
raise
# ------------------------------------------------------------------ screws
def pick_screw(self, screw_id: str) -> OperationResult:
def_name = screw_object_id(screw_id)
self._emit('pick_screw', object=screw_id, phase='start')
try:
self._require_synchronized()
screw_pose = self.supervisor.object_pose(def_name)
goal = tcp_goal(screw_pose, self.geometry.screw_tcp_to_node)
self._contact(
goal, f'pick_screw({screw_id})',
remove_before_contact=def_name,
)
self.supervisor.command('grab_screw', screw_id)
self._retreat(goal, f'pick_screw({screw_id})')
self._emit('pick_screw', object=screw_id, phase='done')
return OperationResult(True, 'pick_screw', f'{screw_id} grasped', tuple(self.events))
except Exception as error:
self._emit('pick_screw', object=screw_id, phase='failed', error=str(error))
raise
def handoff_screw(self, screw_id: str, hole_id: str) -> OperationResult:
def_name = screw_object_id(screw_id)
self._emit('handoff_screw', object=screw_id, target=hole_id, phase='start')
try:
self._require_synchronized()
hole = world_target(self.spec, self.geometry, f'hole:{hole_id}')
goal = tcp_goal(hole, self.geometry.screw_tcp_to_node)
self._contact(
goal, f'handoff_screw({screw_id},{hole_id})',
remove_before_contact=def_name,
remove_phase='contact',
)
self.supervisor.command(
'handoff_screw', screw_id, target=hole, target_name=hole_id,
)
self._require_synchronized()
# A Cartesian retreat from the hole can leave the wrist in a
# singular branch. The caller moves to 'work' in joint space
# immediately afterwards; that is the verified clearance move.
self._emit('delivered', object=screw_id, target=hole_id,
phase='done', hole=hole_id)
return OperationResult(
True, 'handoff_screw', f'{screw_id} delivered to {hole_id}',
tuple(self.events),
)
except Exception as error:
self._emit('handoff_screw', object=screw_id, target=hole_id,
phase='failed', error=str(error))
raise
# ------------------------------------------------------------------ verifies
def inspect_cap(self, target_pose: Transform | 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()
cap = state.get('cap') or {}
if cap.get('state') != 'installed':
return {'ok': False, 'reason': f'hatch state is {cap.get("state")!r}'}
pose_entry = cap.get('pose')
if not pose_entry:
return {'ok': False, 'reason': 'hatch pose is unknown'}
pose = _transform_from_payload(pose_entry, child='cap')
distance = float(
(pose.position - target.position) @ (pose.position - target.position)
) ** 0.5
ok = distance <= self.geometry.position_tolerance
return {
'ok': ok,
'distance': distance,
'target': target.xyz,
'actual': pose.xyz,
}
def inspect_sealant(self) -> dict[str, Any]:
expected = list(self.spec.sealant_sides)
missing = [side for side in expected if side not in self.completed_sealant]
return {
'ok': not missing,
'completed': sorted(self.completed_sealant),
'missing': missing,
}
def inspect_state(self) -> dict[str, Any]:
self.supervisor.command('inspect')
return self._expect_latest()
src/iiwa_experement/iiwa_experement/planning_scene.py
+273 -66
View File
@@ -10,19 +10,24 @@ from __future__ import annotations
import copy
import math
import time
import xml.etree.ElementTree as ET
from dataclasses import dataclass
from pathlib import Path
from typing import Iterable, Sequence
import numpy as np
import rclpy
from ament_index_python.packages import get_package_share_directory
from geometry_msgs.msg import Pose
from moveit_msgs.msg import (
AllowedCollisionEntry,
AttachedCollisionObject,
CollisionObject,
PlanningScene,
)
from moveit_msgs.srv import GetPlanningScene
from rclpy.node import Node
from sensor_msgs.msg import JointState
from rclpy.parameter import Parameter
from rclpy.qos import DurabilityPolicy, QoSProfile, ReliabilityPolicy
from shape_msgs.msg import SolidPrimitive
@@ -35,7 +40,55 @@ SCENE_TOPIC = '/planning_scene'
MONITORED_TOPIC = '/monitored_planning_scene'
GET_SCENE_SERVICE = '/get_planning_scene'
SCENE_FRAME = 'base_link'
TOUCH_LINKS = ('tcp', 'patron', 'camera_holder')
TOUCH_LINKS = (
'tcp', 'patron', 'camera_holder', 'camera_corner',
'camera_hand', 'camera_optical_frame',
)
ROBOT_LINKS = (
'base_link', 'link1', 'link2', 'link3', 'link4', 'link5', 'link6',
'link7', 'patron', 'camera_holder', 'camera_corner', 'camera_hand',
'camera_optical_frame', 'tcp',
)
DEFAULT_SRDF = 'iiwa_config/config/moveit/iiwa7.srdf'
def load_srdf_disabled_pairs(path) -> list[tuple[str, str]]:
root = ET.parse(str(path)).getroot()
pairs = []
for element in root.findall('disable_collisions'):
first = element.get('link1')
second = element.get('link2')
if first and second:
pairs.append((first, second))
return pairs
def build_collision_matrix(
object_ids: Iterable[str],
*,
disabled_pairs: Iterable[tuple[str, str]] = (),
extra_allowed: Iterable[tuple[str, str]] = (),
) -> tuple[list[str], list[list[bool]]]:
"""Build a complete ACM: only explicitly disabled pairs are allowed.
This mirrors the SRDF and keeps every other robot/object and object/object
pair checked. It replaces an empty ACM that would otherwise make the
planning scene treat adjacent robot links as collisions.
"""
names = list(ROBOT_LINKS)
for name in object_ids:
if name not in names:
names.append(name)
matrix = [[False] * len(names) for _ in names]
for index in range(len(names)):
matrix[index][index] = True
for first, second in list(disabled_pairs) + list(extra_allowed):
if first in names and second in names:
row = names.index(first)
column = names.index(second)
matrix[row][column] = True
matrix[column][row] = True
return names, matrix
class SceneSynchronizationError(RuntimeError):
@@ -59,6 +112,7 @@ class AttachedSpec:
box: Box
relative_pose: Transform
touch_links: tuple[str, ...]
frame_id: str | None = None
def _box_at(pose: Transform, box: Box) -> Box:
@@ -90,7 +144,7 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
'grant_table', table, _rotated_pose(0.0, 0.0, table.offset[2], 0.0),
))
fixture = geometry.scene_collisions['fixture']
fixture = geometry.scene_collisions['fixture_base']
objects.append(SceneObject(
'grant_fixture',
fixture,
@@ -101,6 +155,20 @@ def build_world_objects(spec: ScenarioSpec, geometry: GeometrySpec) -> list[Scen
spec.fixture.yaw_deg,
),
))
rail = geometry.scene_collisions['fixture_rail']
sine = math.sin(math.radians(spec.fixture.yaw_deg))
cosine = math.cos(math.radians(spec.fixture.yaw_deg))
for sign, suffix in ((-1.0, 'a'), (1.0, 'b')):
objects.append(SceneObject(
f'grant_fixture_rail_{suffix}',
rail,
_rotated_pose(
spec.fixture.x + sign * rail.offset[0] * cosine,
spec.fixture.y + sign * rail.offset[0] * sine,
spec.fixture.z + rail.offset[2],
spec.fixture.yaw_deg,
),
))
body = geometry.scene_collisions['body']
objects.append(SceneObject(
@@ -211,7 +279,10 @@ def attached_tool(name: str, geometry: GeometrySpec) -> AttachedSpec:
offset=collision.offset,
),
relative_pose=Transform.from_xyz_rpy(
collision.offset, [0.0, 0.0, 0.0], parent='tcp', child=f'grant_{name}',
[0.0, 0.0, collision.offset[2] + 0.0552],
[0.0, 0.0, 0.0],
parent='patron',
child=f'grant_{name}',
),
touch_links=TOUCH_LINKS,
)
@@ -222,9 +293,9 @@ def attached_cap(geometry: GeometrySpec) -> AttachedSpec:
object_id='grant_cap',
box=geometry.cap_collision,
relative_pose=Transform.from_xyz_rpy(
geometry.cap_collision.offset,
[0.0, 0.0, geometry.cap_collision.offset[2] + 0.0552],
[0.0, 0.0, 0.0],
parent='tcp',
parent='patron',
child='grant_cap',
),
touch_links=TOUCH_LINKS,
@@ -237,9 +308,9 @@ def attached_screw(screw_id: str, geometry: GeometrySpec) -> AttachedSpec:
object_id=object_id,
box=geometry.screw_collision,
relative_pose=Transform.from_xyz_rpy(
geometry.screw_collision.offset,
[0.0, 0.0, geometry.screw_collision.offset[2] + 0.0552],
[0.0, 0.0, 0.0],
parent='tcp',
parent='patron',
child=object_id,
),
touch_links=TOUCH_LINKS,
@@ -274,25 +345,32 @@ def collision_object(
message.header.frame_id = SCENE_FRAME
message.id = object_id
message.operation = operation
message.pose = _pose_message(pose)
primitive = SolidPrimitive()
primitive.type = SolidPrimitive.BOX
primitive.dimensions = [float(value) for value in box.size]
message.primitives.append(primitive)
message.primitive_poses.append(_pose_message(pose))
identity = Pose()
identity.orientation.w = 1.0
message.primitive_poses.append(identity)
return message
def attached_object_message(
spec: AttachedSpec,
*,
link_name: str = 'tcp',
link_name: str | None = None,
operation: int = CollisionObject.ADD,
) -> AttachedCollisionObject:
message = AttachedCollisionObject()
message.link_name = link_name
message.link_name = link_name or spec.relative_pose.parent
message.object = collision_object(
spec.object_id, spec.box, spec.relative_pose, operation=operation,
)
# For attached objects the object pose is relative to the link.
message.object.header.frame_id = (
spec.frame_id if spec.frame_id is not None else message.link_name
)
message.touch_links = list(spec.touch_links)
return message
@@ -334,6 +412,7 @@ class PlanningSceneBridge:
*,
node: Node | None = None,
use_sim_time: bool = True,
srdf_path: str | None = None,
) -> None:
self._owns_node = node is None
if node is None:
@@ -366,8 +445,29 @@ class PlanningSceneBridge:
self._attached: dict[str, AttachedSpec] = {}
self._seen_world: set[str] = set()
self._seen_attached: set[str] = set()
self._latest_joint_state = None
self._last_full_scene: PlanningScene | None = None
self._acm_names: list[str] = []
self._acm_values: list[list[bool]] = []
self._disabled_pairs = []
srdf = srdf_path or str(
Path(get_package_share_directory('iiwa_config'))
/ 'config' / 'moveit' / 'iiwa7.srdf'
)
try:
self._disabled_pairs = load_srdf_disabled_pairs(srdf)
except Exception:
self._disabled_pairs = []
self._joint_subscription = self.node.create_subscription(
JointState, '/joint_states', self._on_joint_state, 10,
)
def _on_joint_state(self, message: JointState) -> None:
self._latest_joint_state = message
def _on_scene(self, message: PlanningScene) -> None:
if not message.is_diff:
self._last_full_scene = copy.deepcopy(message)
self._seen_world.update(
obj.id for obj in message.world.collision_objects
)
@@ -411,6 +511,137 @@ class PlanningSceneBridge:
self._publisher.publish(scene)
self._spin()
def _fetch_acm(self) -> None:
if getattr(self, '_get_scene', None) is None or self.node is None:
return
if not self._get_scene.service_is_ready():
if not self._get_scene.wait_for_service(timeout_sec=1.0):
return
request = GetPlanningScene.Request()
request.components.components = 128
future = self._get_scene.call_async(request)
end = time.monotonic() + 3.0
while time.monotonic() < end and not future.done():
rclpy.spin_once(self.node, timeout_sec=0.05)
if future.done():
try:
matrix = future.result().scene.allowed_collision_matrix
self._acm_names = list(matrix.entry_names)
self._acm_values = [
list(row.enabled) for row in matrix.entry_values
]
except Exception:
pass
def _rebuild_acm(self) -> None:
extra = []
for spec in self._attached.values():
extra.extend((spec.object_id, link) for link in spec.touch_links)
self._acm_names, self._acm_values = build_collision_matrix(
list(self._world_objects) + list(self._attached),
disabled_pairs=self._disabled_pairs,
extra_allowed=extra,
)
def _full_scene(self) -> PlanningScene:
scene = PlanningScene()
scene.is_diff = False
# Never overwrite the planner's current robot state from a planning
# scene update: the monitor receives /joint_states directly and a
# stale embedded sample makes the next trajectory start from an old
# pose. ``is_diff`` leaves the state untouched.
scene.robot_state.is_diff = True
for obj in self._world_objects.values():
scene.world.collision_objects.append(collision_object(
obj.object_id, obj.box, self._base_pose(obj.pose),
))
for spec in self._attached.values():
scene.robot_state.attached_collision_objects.append(
attached_object_message(spec)
)
if self._acm_names:
scene.allowed_collision_matrix.entry_names = list(self._acm_names)
scene.allowed_collision_matrix.entry_values = [
AllowedCollisionEntry(
enabled=[bool(value) for value in row],
)
for row in self._acm_values
]
return scene
def _refresh_joint_state(self) -> None:
"""Wait for the next /joint_states sample before a full scene update.
A full PlanningScene carries a robot state. Using a sample captured
before a preceding motion would make the MoveItPy planner start from
an old pose on the next trajectory.
"""
if self.node is None:
return
previous = None
if self._latest_joint_state is not None:
previous = (
self._latest_joint_state.header.stamp.sec,
self._latest_joint_state.header.stamp.nanosec,
)
end = time.monotonic() + 0.5
while time.monotonic() < end:
rclpy.spin_once(self.node, timeout_sec=0.02)
current = self._latest_joint_state
if current is None:
continue
stamp = (
current.header.stamp.sec,
current.header.stamp.nanosec,
)
if previous is None or stamp != previous:
return
def _relay_full_scene(self) -> None:
"""Give MoveItPy the exact move_group scene, including its ACM.
MoveItPy's monitor always listens on ``/planning_scene`` even when its
configured topics differ. Fetching the maintained scene from
move_group and republishing it once keeps both planners identical
without a feedback loop.
"""
if getattr(self, '_get_scene', None) is None or self.node is None:
return
if not self._get_scene.service_is_ready():
if not self._get_scene.wait_for_service(timeout_sec=1.0):
return
expected_world = set(self._world_objects)
expected_attached = set(self._attached)
deadline = time.monotonic() + 3.0
while time.monotonic() < deadline:
request = GetPlanningScene.Request()
request.components.components = 2 | 4 | 8 | 16 | 128
future = self._get_scene.call_async(request)
end = time.monotonic() + 1.0
while time.monotonic() < end and not future.done():
rclpy.spin_once(self.node, timeout_sec=0.05)
if not future.done():
continue
try:
scene = future.result().scene
except Exception:
continue
world, attached = scene_ids(scene)
if expected_world <= world and expected_attached <= attached:
scene.is_diff = False
scene.robot_state.is_diff = True
self._publisher.publish(scene)
self._spin()
return
# Never relay a stale scene: that would wipe the objects from the
# MoveItPy monitor and make the next motion unverified.
return
def _publish_full(self) -> None:
self._refresh_joint_state()
self._publish(self._full_scene())
self._relay_full_scene()
@staticmethod
def _diff() -> PlanningScene:
scene = PlanningScene()
@@ -440,33 +671,24 @@ class PlanningSceneBridge:
obj.object_id: obj for obj in build_world_objects(spec, geometry)
}
self._attached.clear()
scene = self._diff()
for obj in self._world_objects.values():
scene.world.collision_objects.append(collision_object(
obj.object_id, obj.box, self._base_pose(obj.pose),
))
self._publish(scene)
extra = []
for spec in self._attached.values():
extra.extend((spec.object_id, link) for link in spec.touch_links)
self._acm_names, self._acm_values = build_collision_matrix(
list(self._world_objects) + list(self._attached),
disabled_pairs=self._disabled_pairs,
extra_allowed=extra,
)
self._publish_full()
self.assert_synchronized(
self._world_objects, (), seen_world=set(self._world_objects),
)
def attach(self, spec: AttachedSpec) -> None:
if spec.object_id not in self._world_objects:
raise SceneSynchronizationError(
f'cannot attach {spec.object_id!r}: not a known world object'
)
scene = self._diff()
scene.world.collision_objects.append(collision_object(
spec.object_id, spec.box, Transform.from_xyz_rpy(
[0, 0, 0], [0, 0, 0], parent='world', child='removed',
), operation=CollisionObject.REMOVE,
))
scene.robot_state.attached_collision_objects.append(
attached_object_message(spec)
)
self._world_objects.pop(spec.object_id, None)
self._attached[spec.object_id] = spec
self._publish(scene)
self._rebuild_acm()
self._publish_full()
def detach(self, object_id: str, world_pose: Transform) -> None:
if object_id not in self._attached:
@@ -476,34 +698,20 @@ class PlanningSceneBridge:
spec = self._attached.pop(object_id)
object_spec = SceneObject(object_id, spec.box, world_pose)
self._world_objects[object_id] = object_spec
scene = self._diff()
scene.robot_state.attached_collision_objects.append(
attached_object_message(
spec, operation=CollisionObject.REMOVE,
)
)
scene.world.collision_objects.append(collision_object(
object_id, spec.box, self._base_pose(world_pose),
))
self._publish(scene)
self._rebuild_acm()
self._publish_full()
def remove(self, object_id: str) -> None:
scene = self._diff()
if object_id in self._attached:
spec = self._attached.pop(object_id)
scene.robot_state.attached_collision_objects.append(
attached_object_message(
spec, operation=CollisionObject.REMOVE,
)
)
if object_id in self._world_objects:
self._world_objects.pop(object_id)
scene.world.collision_objects.append(collision_object(
object_id, Box(size=(0.001, 0.001, 0.001)), Transform.from_xyz_rpy(
[0, 0, 0], [0, 0, 0], parent='world', child='removed',
), operation=CollisionObject.REMOVE,
))
self._publish(scene)
self._attached.pop(object_id, None)
self._world_objects.pop(object_id, None)
self._rebuild_acm()
self._publish_full()
def add_world(self, object_id: str, box: Box, world_pose: Transform) -> None:
self._attached.pop(object_id, None)
self._world_objects[object_id] = SceneObject(object_id, box, world_pose)
self._rebuild_acm()
self._publish_full()
def allow_contact(self, first: str, second: str, allowed: bool = True) -> None:
"""Change exactly one ACM pair; all other checks stay active."""
@@ -511,22 +719,21 @@ class PlanningSceneBridge:
raise SceneSynchronizationError(
'cannot update contact permissions without a monitored scene'
)
scene = self._diff()
names = list(self.last_scene.allowed_collision_matrix.entry_names)
values = [
list(row) for row in self.last_scene.allowed_collision_matrix.entry_values
]
names, values = update_contact_entry(
names, values, first, second, allowed,
if not self._acm_names:
raise SceneSynchronizationError(
'cannot update contact permissions without an ACM'
)
scene.allowed_collision_matrix.entry_names = names
scene.allowed_collision_matrix.entry_values = values
self._publish(scene)
self._acm_names, self._acm_values = update_contact_entry(
self._acm_names, self._acm_values, first, second, allowed,
)
self._publish_full()
def expected_sets(self) -> tuple[set[str], set[str]]:
return set(self._world_objects), set(self._attached)
def _query_scene(self, timeout_sec: float) -> tuple[set[str], set[str]] | None:
if getattr(self, '_get_scene', None) is None or self.node is None:
return
if not self._get_scene.service_is_ready():
if not self._get_scene.wait_for_service(timeout_sec=1.0):
return None
src/iiwa_experement/iiwa_experement/runtime_config.py
+13 -1
View File
@@ -117,6 +117,7 @@ def _materialize_setting(
base_setting: Path,
world_path: Path,
output_dir: Path,
moveit_cpp_path: Path | None = None,
) -> Path:
if not base_setting.is_file():
raise FileNotFoundError(f'base settings file does not exist: {base_setting}')
@@ -134,6 +135,11 @@ def _materialize_setting(
tool = _mapping(data.setdefault('tool', {}), 'tool')
tool['active'] = 'patron'
if moveit_cpp_path is not None:
controller = _mapping(data.setdefault('controller', {}), 'controller')
moveit = _mapping(controller.setdefault('moveit', {}), 'controller.moveit')
moveit['moveit_cpp'] = str(Path(moveit_cpp_path).resolve())
setting_path = output_dir / 'setting.yaml'
setting_path.write_text(
yaml.safe_dump(data, sort_keys=False, allow_unicode=True),
@@ -147,6 +153,7 @@ def prepare_runtime_files(
world_template: Path | str,
description_share: Path | str,
output_dir: Path | str,
moveit_cpp_path: Path | str | None = None,
) -> tuple[Path, Path]:
"""Write an isolated world/settings pair and return ``(world, setting)``.
@@ -161,5 +168,10 @@ def prepare_runtime_files(
output_dir = Path(output_dir)
output_dir.mkdir(parents=True, exist_ok=True)
world_path = _materialize_world(world_template, description_share, output_dir)
setting_path = _materialize_setting(base_setting, world_path, output_dir)
setting_path = _materialize_setting(
base_setting,
world_path,
output_dir,
None if moveit_cpp_path is None else Path(moveit_cpp_path),
)
return world_path, setting_path
src/iiwa_experement/iiwa_experement/webots_nodes.py
+4 -3
View File
@@ -309,15 +309,16 @@ def _cap_node(spec: ScenarioSpec, asset: Path) -> str:
translation 0 0.004165 0
children [ Box { size 0.1384 0.00367 0.0984 } ]
}'''
installed = spec.cap_state == 'installed'
# All managed objects are positioned by the attachment supervisor.
# A free cap with physics can be launched by the supervisor's per-step
# teleport, so it is locked just like an installed cap.
return _solid(
name='grant_cap',
translation=(pose.x, pose.y, pose.z),
rotation=rotation_axis_angle(pose.yaw_deg),
children=_mesh_shape(asset, '0.78 0.82 0.86'),
bounding=bounding,
locked=installed,
physics_mass=None if installed else 0.1,
locked=True,
)
src/iiwa_experement/launch/experiment.launch.py
+1
View File
@@ -226,6 +226,7 @@ def _setup(context, *args, **kwargs):
world_template=world_template,
description_share=description_share,
output_dir=output_dir,
moveit_cpp_path=package_share / 'config' / 'moveit_cpp.yaml',
)
except Exception:
shutil.rmtree(output_dir, ignore_errors=True)
src/iiwa_experement/test/test_grant_supervisor.py
+27 -5
View File
@@ -78,7 +78,8 @@ def make_node(experiment_id=1):
node.spec = spec
node.geometry = geometry
node.state = ObjectState(spec, geometry)
node.attached = None
node.tool_held = None
node.payload = None
node.rack_poses = {
name: entry['pose'] for name, entry in node.state.tools.items()
}
@@ -144,7 +145,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
accepted, error = node._apply('attach_tool', {'object': 'vacuum'}, 'r1')
assert accepted is True
assert error is None
assert node.attached == ('tool', 'vacuum')
assert node.tool_held == 'vacuum'
other = make_node(1)
set_tcp(other, good.compose_relaxed(
@@ -155,7 +156,7 @@ def test_attach_requires_tcp_at_the_socket_and_rejects_far_pose():
accepted, error = other._apply('attach_tool', {'object': 'vacuum'}, 'r2')
assert accepted is False
assert 'tcp is' in error
assert other.attached is None
assert other.tool_held is None
assert other.state.version == 0
@@ -170,7 +171,7 @@ def test_follow_moves_the_attached_tool_and_dock_returns_it_to_the_rack():
assert moved == pytest.approx(rack_pose.position, abs=1e-9)
accepted, _ = node._apply('dock_tool', {'object': 'vacuum'}, 'dock')
assert accepted
assert node.attached is None
assert node.tool_held is None
assert node.state.mounted_tool is None
assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
rack_pose.position, abs=1e-9,
@@ -213,8 +214,29 @@ def test_grab_and_handoff_remove_only_the_selected_screw():
def test_follow_refuses_when_tcp_is_unknown():
node = make_node(1)
node.patron_node = None
node.attached = ('tool', 'vacuum')
node.tool_held = 'vacuum'
node.follow()
assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
node.state.tools['vacuum']['pose'].position, abs=1e-9,
)
def test_mounted_tool_and_payload_follow_together():
node = make_node(1)
tool_pose = node.state.tools['vacuum']['pose']
tcp = tcp_goal(tool_pose, node.geometry.tool_tcp_to_nodes['vacuum'])
set_tcp(node, tcp)
assert node._apply('attach_tool', {'object': 'vacuum'}, 't')[0]
cap_pose = node.state.cap_pose
cap_tcp = tcp_goal(cap_pose, node.geometry.cap_tcp_to_node)
set_tcp(node, cap_tcp)
assert node._apply('grab_cap', {'object': 'cap'}, 'c')[0]
node.follow()
expected_tool = cap_tcp.compose_relaxed(node.geometry.tool_tcp_to_nodes['vacuum'])
expected_cap = cap_tcp.compose_relaxed(node.geometry.cap_tcp_to_node)
assert node.nodes['grant_vacuum'].getPosition() == pytest.approx(
expected_tool.position, abs=1e-9,
)
assert node.nodes['grant_cap'].getPosition() == pytest.approx(
expected_cap.position, abs=1e-9,
)
src/iiwa_experement/test/test_operations.py
+219
View File
@@ -0,0 +1,219 @@
"""Operation sequences are visible, ordered and fail closed."""
from pathlib import Path
import pytest
from iiwa_experement.geometry import load_geometry, world_target
from iiwa_experement.motion import MotionResult
from iiwa_experement.operations import OperationError, ReferenceOperations
from iiwa_experement.scenario import load_scenario
PACKAGE = Path(__file__).resolve().parents[1]
SCENES = PACKAGE / 'config' / 'scenes.yaml'
GEOMETRY = PACKAGE / 'config' / 'geometry.yaml'
class FakeSupervisor:
def __init__(self, spec, geometry):
self.spec = spec
self.geometry = geometry
self.commands = []
self.latest = {
'cap': {'state': spec.cap_state},
'screws': {},
}
self.fail_on = None
def object_pose(self, name):
mapping = {
'grant_cap': 'cap',
**{f'grant_{tool}': f'tool:{tool}' for tool in self.spec.tool_names},
**{
f'grant_screw_{index:02d}': f'screw:screw_{index}'
for index in range(1, 11)
},
}
target = mapping[name]
return world_target(self.spec, self.geometry, target)
def command(self, op, object_id='', **kwargs):
if self.fail_on == op:
raise OperationError(f'{op} rejected')
self.commands.append((op, object_id, kwargs))
return self.latest
class FakeBridge:
def __init__(self):
self.attached = []
self.detached = []
self.removed = []
def attach(self, spec):
self.attached.append(spec.object_id)
def detach(self, object_id, pose):
self.detached.append((object_id, pose))
def remove(self, object_id):
self.removed.append(object_id)
def add_world(self, object_id, box, pose):
self.added_world = getattr(self, 'added_world', [])
self.added_world.append((object_id, pose))
def expected_sets(self):
return set(), set()
def assert_synchronized(self, *args, **kwargs):
return None
class FakeMotion:
def __init__(self):
self.moves = []
self.named = []
self.fail_labels = set()
def move_pose(self, goal, planner='ptp', speed=0.2, label=''):
self.moves.append((label, planner, speed, goal))
if label in self.fail_labels:
return MotionResult(False, 'injected failure', label)
return MotionResult(True, 'ok', label, True, goal, goal)
def move_named(self, name, speed=0.2):
self.named.append((name, speed))
return MotionResult(True, 'ok', f'move_named({name})')
def make_operations(experiment_id):
spec = load_scenario(experiment_id, SCENES)
geometry = load_geometry(GEOMETRY)
supervisor = FakeSupervisor(spec, geometry)
bridge = FakeBridge()
motion = FakeMotion()
operations = ReferenceOperations(
node=None,
spec=spec,
geometry=geometry,
supervisor=supervisor,
bridge=bridge,
motion=motion,
)
return operations, supervisor, bridge, motion, geometry
def labels(motion):
return [label for label, _, _, _ in motion.moves]
def test_take_tool_follows_the_visible_axial_route():
operations, supervisor, bridge, motion, geometry = make_operations(1)
tip = world_target(operations.spec, geometry, 'tool:vacuum')
expected = tip.compose_relaxed(geometry.tool_tcp_to_nodes['vacuum'].inverse())
result = operations.take_tool('vacuum')
assert result.success
assert labels(motion) == [
'take_tool(vacuum):transfer',
'take_tool(vacuum):approach',
'take_tool(vacuum):contact',
'take_tool(vacuum):retreat',
'take_tool(vacuum):clear',
]
assert supervisor.commands[0][:2] == ('attach_tool', 'vacuum')
assert bridge.attached == []
contact_goal = motion.moves[2][3]
assert contact_goal.xyz == pytest.approx(expected.xyz, abs=1e-9)
assert motion.moves[0][1] == 'ompl'
assert motion.moves[1][1] == 'ompl'
assert motion.moves[2][1] == 'lin'
def test_dock_tool_returns_the_tool_to_its_socket():
operations, supervisor, bridge, motion, geometry = make_operations(1)
result = operations.dock_tool('vacuum')
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'
def test_cap_pick_and_place_use_the_hatch_node_transform():
operations, supervisor, bridge, motion, geometry = make_operations(1)
assert operations.pick_cap().success
assert bridge.attached == []
assert supervisor.commands[0][:2] == ('grab_cap', 'cap')
assert operations.place_cap().success
assert bridge.added_world[0][0] == 'grant_cap'
assert supervisor.commands[1][0] == 'place_cap'
def test_single_sealant_side_lowers_and_lifts():
operations, supervisor, bridge, motion, geometry = make_operations(6)
assert operations.dispense('side_1').success
assert labels(motion) == [
'dispense(side_1):transfer',
'dispense(side_1):approach',
'dispense(side_1):contact',
'dispense(side_1):path',
'dispense(side_1):retreat',
'dispense(side_1):clear',
]
assert operations.completed_sealant == {'side_1'}
assert operations.inspect_sealant()['ok'] is True
def test_contour_does_not_lift_between_adjacent_sides():
operations, supervisor, bridge, motion, geometry = make_operations(8)
assert operations.dispense_contour(
['side_1', 'side_2', 'side_3', 'side_4']
).success
names = labels(motion)
assert names[:3] == [
'dispense_contour:start:transfer',
'dispense_contour:start:approach',
'dispense_contour:start:contact',
]
assert names[3:7] == [
'dispense_contour:side_1',
'dispense_contour:side_2',
'dispense_contour:side_3',
'dispense_contour:side_4',
]
assert 'dispense_contour:side_1' in names
assert names[-2:] == [
'dispense_contour:retreat', 'dispense_contour:clear',
]
assert operations.completed_sealant == {
'side_1', 'side_2', 'side_3', 'side_4',
}
def test_screw_pick_and_handoff_are_one_motion_and_one_removal():
operations, supervisor, bridge, motion, geometry = make_operations(11)
assert operations.pick_screw('screw_1').success
assert bridge.attached == []
assert supervisor.commands[0][:2] == ('grab_screw', 'screw_1')
assert operations.handoff_screw('screw_1', 'hole_1').success
assert supervisor.commands[1][0] == 'handoff_screw'
assert supervisor.commands[1][2]['target_name'] == 'hole_1'
assert bridge.removed == ['grant_screw_01', 'grant_screw_01']
assert bridge.attached == []
def test_failed_motion_stops_the_operation_and_emits_failure():
operations, supervisor, bridge, motion, geometry = make_operations(1)
motion.fail_labels.add('take_tool(vacuum):contact')
with pytest.raises(OperationError):
operations.take_tool('vacuum')
assert operations.events[-1]['phase'] == 'failed'
assert bridge.attached == []
def test_failed_supervisor_command_is_not_ignored():
operations, supervisor, bridge, motion, geometry = make_operations(1)
supervisor.fail_on = 'attach_tool'
with pytest.raises(OperationError):
operations.take_tool('vacuum')
assert bridge.attached == []
src/iiwa_experement/test/test_planning_scene.py
+38 -14
View File
@@ -10,6 +10,8 @@ from iiwa_experement.planning_scene import (
PlanningSceneBridge,
SceneObject,
attached_cap,
build_collision_matrix,
load_srdf_disabled_pairs,
attached_screw,
attached_tool,
build_world_objects,
@@ -40,6 +42,11 @@ class FakeBridge(PlanningSceneBridge):
obj.object_id: obj for obj in build_world_objects(scenario, geometry)
}
self._attached = {}
self._disabled_pairs = []
self._acm_names = []
self._acm_values = []
self._latest_joint_state = None
self.node = None
self.last_scene = PlanningScene()
self.published = []
@@ -53,13 +60,14 @@ class FakeBridge(PlanningSceneBridge):
def test_initial_scene_contains_every_experiment_object():
objects = build_world_objects(spec(11), load_geometry(GEOMETRY))
ids = {obj.object_id for obj in objects}
assert {'grant_table', 'grant_fixture', 'grant_body', 'grant_rack',
assert {'grant_table', 'grant_fixture', 'grant_fixture_rail_a',
'grant_fixture_rail_b', 'grant_body', 'grant_rack',
'grant_tray', 'grant_cap', 'grant_magnetic_tool'} <= ids
assert {f'grant_screw_{index:02d}' for index in range(1, 11)} <= ids
assert 'grant_vacuum' not in ids
# table, fixture, body, rack, tray base, four tray walls, ten screws,
# installed hatch and one magnetic tool.
assert len(ids) == 4 + 1 + 4 + 10 + 1 + 1
# table, fixture base, two fixture rails, body, rack, tray base, four
# tray walls, ten screws, installed hatch and one magnetic tool.
assert len(ids) == 23
def test_experiment_nineteen_moves_tray_and_screws():
@@ -99,11 +107,11 @@ def test_attach_removes_the_world_object_and_adds_one_attachment():
assert tool.object_id not in bridge._world_objects
assert bridge._attached[tool.object_id] is tool
scene = bridge.published[-1]
assert scene.is_diff is True
assert [obj.id for obj in scene.world.collision_objects] == [tool.object_id]
assert scene.world.collision_objects[0].operation == CollisionObject.REMOVE
assert scene.is_diff is False
assert tool.object_id not in [obj.id for obj in scene.world.collision_objects]
assert len(scene.robot_state.attached_collision_objects) == 1
assert scene.robot_state.attached_collision_objects[0].link_name == 'tcp'
assert scene.robot_state.attached_collision_objects[0].object.id == tool.object_id
assert scene.robot_state.attached_collision_objects[0].link_name == 'patron'
assert 'tcp' in scene.robot_state.attached_collision_objects[0].touch_links
@@ -117,10 +125,8 @@ def test_detach_reverses_the_attachment():
assert tool.object_id in bridge._world_objects
assert tool.object_id not in bridge._attached
scene = bridge.published[-1]
assert scene.robot_state.attached_collision_objects[0].object.operation == (
CollisionObject.REMOVE
)
assert scene.world.collision_objects[0].operation == CollisionObject.ADD
assert not scene.robot_state.attached_collision_objects
assert tool.object_id in [obj.id for obj in scene.world.collision_objects]
def test_handoff_removes_only_the_delivered_screw():
@@ -138,9 +144,9 @@ def test_cap_and_screw_attachment_geometry_uses_tcp_offsets():
geometry = load_geometry(GEOMETRY)
cap = attached_cap(geometry)
screw = attached_screw('screw_3', geometry)
assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835))
assert cap.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.141835 + 0.0552))
assert cap.box.size == pytest.approx((0.1384, 0.0984, 0.00367))
assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445))
assert screw.relative_pose.xyz == pytest.approx((0.0, 0.0, 0.1445 + 0.0552))
def test_contact_permission_changes_exactly_one_pair():
@@ -153,6 +159,24 @@ def test_contact_permission_changes_exactly_one_pair():
assert matrix[0][1] is False
def test_srdf_collision_matrix_allows_only_disabled_pairs():
root = Path(__file__).resolve().parents[2]
srdf = root / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf'
pairs = load_srdf_disabled_pairs(srdf)
assert ('link1', 'link2') in pairs
names, matrix = build_collision_matrix(
['grant_body', 'grant_tool'], disabled_pairs=pairs,
)
assert 'base_link' in names and 'grant_body' in names
def allowed(first, second):
return matrix[names.index(first)][names.index(second)]
assert allowed('link1', 'link2') is True
assert allowed('base_link', 'link1') is True
assert allowed('link1', 'grant_body') is False
assert allowed('grant_body', 'grant_tool') is False
assert allowed('grant_body', 'grant_body') is True
def test_scene_ids_reports_world_and_attached_sets():
scene = PlanningScene()
scene.world.collision_objects.append(CollisionObject(id='world_a'))
src/iiwa_experement/test/test_webots_nodes.py
+5 -7
View File
@@ -232,13 +232,11 @@ def test_loose_cap_touches_the_table_and_installed_cap_touches_the_body():
assert cap_z + cap_min == pytest.approx(body_z + body_max, abs=1e-9)
def test_free_cap_has_physics_and_installed_cap_is_locked():
free_cap = by_name(nodes(1))['grant_cap']
assert 'physics Physics {' in free_cap
assert 'locked TRUE' not in free_cap
installed_cap = by_name(nodes(11))['grant_cap']
assert 'locked TRUE' in installed_cap
assert 'physics Physics {' not in installed_cap
def test_all_caps_are_locked_and_supervisor_controlled():
for experiment_id in (1, 11):
cap = by_name(nodes(experiment_id))['grant_cap']
assert 'locked TRUE' in cap
assert 'physics Physics {' not in cap
def test_ten_screws_are_distinct_and_inside_the_tray():