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

fix(llm-plan-executor): use PTP/LIN with checked camera collisions

Browse Source
This commit is contained in:
Даниил Грабарь
2026-11-11 12:34:54 +10:00
parent 454a2014f3
commit 0e86a4b72e
17 changed files with 756 additions and 260 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/iiwa_config/config/moveit/iiwa7.srdf
+1 -51
View File
@@ -37,66 +37,16 @@
<end_effector name="patron" parent_link="patron" group="iiwa_arm"/>
<disable_collisions link1="base_link" link2="link1" reason="Adjacent"/>
<disable_collisions link1="base_link" link2="link2" reason="Never"/>
<disable_collisions link1="base_link" link2="link3" reason="Never"/>
<disable_collisions link1="base_link" link2="link4" reason="Never"/>
<disable_collisions link1="link1" link2="link2" reason="Adjacent"/>
<disable_collisions link1="link1" link2="link3" reason="Never"/>
<disable_collisions link1="link1" link2="link4" reason="Never"/>
<disable_collisions link1="link1" link2="link5" reason="Never"/>
<disable_collisions link1="link1" link2="link6" reason="Never"/>
<disable_collisions link1="link1" link2="link7" reason="Never"/>
<disable_collisions link1="link2" link2="link3" reason="Adjacent"/>
<disable_collisions link1="link2" link2="link4" reason="Never"/>
<disable_collisions link1="link2" link2="link5" reason="Never"/>
<disable_collisions link1="link2" link2="link6" reason="Never"/>
<disable_collisions link1="link2" link2="link7" reason="Never"/>
<disable_collisions link1="link3" link2="link4" reason="Adjacent"/>
<disable_collisions link1="link3" link2="link5" reason="Never"/>
<disable_collisions link1="link3" link2="link6" reason="Never"/>
<disable_collisions link1="link3" link2="link7" reason="Never"/>
<disable_collisions link1="link4" link2="link5" reason="Adjacent"/>
<disable_collisions link1="link4" link2="link6" reason="Never"/>
<disable_collisions link1="link4" link2="link7" reason="Never"/>
<disable_collisions link1="link5" link2="link6" reason="Adjacent"/>
<disable_collisions link1="link5" link2="link7" reason="Never"/>
<disable_collisions link1="link6" link2="link7" reason="Adjacent"/>
<disable_collisions link1="link1" link2="patron" reason="Never"/>
<disable_collisions link1="link2" link2="patron" reason="Never"/>
<disable_collisions link1="link3" link2="patron" reason="Never"/>
<disable_collisions link1="link4" link2="patron" reason="Never"/>
<disable_collisions link1="link5" link2="patron" reason="Never"/>
<disable_collisions link1="link6" link2="patron" reason="Never"/>
<disable_collisions link1="link7" link2="patron" reason="Adjacent"/>
<disable_collisions link1="link7" link2="camera_holder" reason="Adjacent"/>
<disable_collisions link1="link6" link2="camera_holder" reason="Never"/>
<disable_collisions link1="link5" link2="camera_holder" reason="Never"/>
<disable_collisions link1="link4" link2="camera_holder" reason="Never"/>
<disable_collisions link1="link3" link2="camera_holder" reason="Never"/>
<disable_collisions link1="link2" link2="camera_holder" reason="Never"/>
<disable_collisions link1="link1" link2="camera_holder" reason="Never"/>
<disable_collisions link1="base_link" link2="camera_holder" reason="Never"/>
<disable_collisions link1="camera_holder" link2="patron" reason="Adjacent"/>
<disable_collisions link1="camera_holder" link2="camera_corner" reason="Adjacent"/>
<disable_collisions link1="camera_corner" link2="patron" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link7" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link6" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link5" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link4" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link3" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link2" reason="Never"/>
<disable_collisions link1="camera_corner" link2="link1" reason="Never"/>
<disable_collisions link1="camera_corner" link2="base_link" reason="Never"/>
<disable_collisions link1="camera_corner" link2="camera_hand" reason="Adjacent"/>
<disable_collisions link1="camera_hand" link2="camera_holder" reason="Never"/>
<disable_collisions link1="camera_hand" link2="patron" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link7" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link6" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link5" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link4" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link3" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link2" reason="Never"/>
<disable_collisions link1="camera_hand" link2="link1" reason="Never"/>
<disable_collisions link1="camera_hand" link2="base_link" reason="Never"/>
<disable_collisions link1="camera_holder" link2="camera_hand" reason="Adjacent"/>
</robot>
src/iiwa_config/config/tools.yaml
+3 -33
View File
@@ -24,40 +24,10 @@ tools:
tip_link: "patron"
tcp_link: "tcp"
collisions:
- [link1, patron, Never]
- [link2, patron, Never]
- [link3, patron, Never]
- [link4, patron, Never]
- [link5, patron, Never]
- [link6, patron, Never]
- [link7, patron, Adjacent]
# Only fixed-joint or constructive camera-assembly contact pairs are
# disabled. Every camera-to-robot pair must be checked by MoveIt.
- [link7, camera_holder, Adjacent]
- [link6, camera_holder, Never]
- [link5, camera_holder, Never]
- [link4, camera_holder, Never]
- [link3, camera_holder, Never]
- [link2, camera_holder, Never]
- [link1, camera_holder, Never]
- [base_link, camera_holder, Never]
- [camera_holder, patron, Adjacent]
- [camera_holder, camera_corner, Adjacent]
- [camera_corner, patron, Never]
- [camera_corner, link7, Never]
- [camera_corner, link6, Never]
- [camera_corner, link5, Never]
- [camera_corner, link4, Never]
- [camera_corner, link3, Never]
- [camera_corner, link2, Never]
- [camera_corner, link1, Never]
- [camera_corner, base_link, Never]
- [camera_corner, camera_hand, Adjacent]
- [camera_hand, camera_holder, Never]
- [camera_hand, patron, Never]
- [camera_hand, link7, Never]
- [camera_hand, link6, Never]
- [camera_hand, link5, Never]
- [camera_hand, link4, Never]
- [camera_hand, link3, Never]
- [camera_hand, link2, Never]
- [camera_hand, link1, Never]
- [camera_hand, base_link, Never]
- [camera_holder, camera_hand, Adjacent]
src/iiwa_description/urdf/tools/patron.xacro
+2 -2
View File
@@ -86,9 +86,9 @@
</geometry>
</visual>
<collision>
<origin xyz="0.0 0.014 0.0125" rpy="0.0 0.0 0.0"/>
<origin xyz="0.002 0.0155 0.013" rpy="0.0 0.0 0.0"/>
<geometry>
<box size="0.12 0.025 0.025"/>
<box size="0.124 0.031 0.026"/>
</geometry>
</collision>
</link>
src/iiwa_llm_plan_executor/config/motion_limits.yaml
+9 -2
View File
@@ -4,8 +4,8 @@
schema_version: 1
joint7:
# The neutral work pose has joint_7 = 0 rad. The first-stage hard corridor
# is +/-90 degrees around it; the -180 degree IK branch is forbidden.
# Preferred value is the neutral work pose (joint_7 = 0). This is a
# preference only; the real per-joint limits below remain authoritative.
work_reference_rad: 0.0
corridor_rad: 1.5707963267948966
@@ -19,7 +19,14 @@ trajectory:
max_cartesian_jump_m: 0.15
# The planned first point must match the measured joints used as IK seed.
start_tolerance_rad: 0.35
# Dense joint-space resampling used for collision/orientation validation.
max_sample_step_rad: 0.01
freshness:
joint_state_max_age_sec: 1.0
scene_max_age_sec: 2.0
collision:
# Minimum accepted collision distance (metres) for every sampled state.
# Camera-vs-robot pairs are enabled in the generated SRDF.
min_clearance_m: 0.003
src/iiwa_llm_plan_executor/doc/README.md
+11 -9
View File
@@ -94,12 +94,14 @@ The JSONL log with every event is written to
## Safety scope
The executor checks J7 (±90° around `work`) and the downward TCP tilt (≤10°)
on **every** planned point, seeds IK from current joints, enforces these as
MoveIt path constraints, checks scene/joint freshness and verifies the final
pose. The only logged exception is the initial empty-gripper reorientation
from home to the downward `work` posture; J7 and the final orientation are
still enforced and a reorientation with a held object is rejected. The
executor never falls back to an unconstrained `MoveToPose` goal. This is a
Webots verification, not a physical-robot validation and not a Qwen3 quality
evaluation.
The executor uses Pilz PTP for free-space working-point moves and Pilz LIN for
axial contact/retreat and the dispense contour. Every plan is densely
resampled and checked for real joint limits, self-collision (including the
camera assembly), scene collision, camera clearance, downward TCP tilt (≤10°)
and joint jumps before execution. J7 may use its full safe joint range; the
`work` value is only a preference. A lateral PTP from a contact pose is
preceded by an axial LIN retreat. The only logged orientation exception is
the initial empty-gripper reorientation from home to the downward `work`
posture. The executor never falls back to an unconstrained `MoveToPose` goal.
This is a Webots verification, not a physical-robot validation and not a
Qwen3 quality evaluation.
src/iiwa_llm_plan_executor/doc/webots-qa.md
+50 -2
View File
@@ -17,8 +17,11 @@ colcon test --base-paths src --packages-select \
colcon test-result --verbose
```
Result: build of 10 packages succeeded; `361 tests, 0 errors, 0 failures,
0 skipped`.
Result: build of 10 packages succeeded; `365 tests, 0 errors, 0 failures,
0 skipped` for the four runtime packages. The new camera/tool geometry tests
in `iiwa_utils` pass separately (`7 passed`); four pre-existing
`test_body_hatch_worlds.py` cases still fail because their `.wbt`/`.proto`
files are absent from this checkout.
`git diff --check` is clean.
@@ -166,3 +169,48 @@ recovery issue: `grasp_failed` leads through `inspect_state: holding` to
validator therefore rejects that recovery branch with its own ownership reason
(`a_retry_pick ... while holding 'cap'`); this does not repair the model graph.
Completion must not be claimed for this graph.
## Motion planning fix (PTP/LIN, camera collision model)
### Reproduction before the change
The failing `approach(cap)` request was captured with diagnostics:
```text
phase=approach
effective_phase=transfer_lin
pipeline=pilz_industrial_motion_planner
planner_id=LIN
goal_xyz=(0.6, -0.22, 0.206667) # base_link
goal_quat=(-0.382683, 0.92388, 0, 0)
start=(~0, ~0, ~0, ~0, ~0, ~0, ~0)
MoveIt error: NO_IK_SOLUTION
```
All three attempts failed in IK. The old error text about the J7 corridor was
therefore misleading: the failing planner was LIN and the actual cause was
`NO_IK_SOLUTION`, not a joint-7 boundary.
### After the change
* `detect(cap) → approach(cap)` in Webots: both nodes succeeded. Transfers
and approaches used `pilz_industrial_motion_planner/PTP`; the measured
minimum sampled clearance was **0.0288 m** (required 0.003 m), maximum J7
was 0.875 rad.
* A separate J7-rotation probe moved the arm to the `work` posture with
`joint7 = 2.0 rad` and then executed `approach(cap)`. The accepted sampled
trajectory covered `joint7 = -1.225 … 2.000 rad`, i.e. well beyond the old
±90 deg boundary, with no joint-limit, self-collision or clearance failure;
measured clearance was again **0.0288 m**.
* Camera geometry: `camera_corner` and `camera_hand` keep at least **35 mm**
AABB clearance to `link6`/`link7` at `home` and `work`. The D455 collision
box now matches its visible bounds. Only constructive fixed contact pairs
are disabled in `tools.yaml` and the regenerated SRDF; all camera-to-robot
pairs remain checked.
* Tests: `pytest src/iiwa_llm_plan_executor/test` → **122 passed**; camera /
tool collision + TCP tests in `iiwa_utils` → **7 passed**. The existing
unrelated
`test_body_hatch_worlds.py` still fails because its world files are absent
from this checkout; it is not part of the motion change.
Physical robot was not tested.
src/iiwa_llm_plan_executor/docs/interfaces.md
+43 -18
View File
@@ -160,22 +160,41 @@ is required; RGB/D is used for liveness only.
## Constrained motion adapter
`MotionPolicy` (`config/motion_limits.yaml`) checks every point before the
controller sees a plan:
`MotionPolicy` (`config/motion_limits.yaml`) checks every sampled point before
the controller sees a plan:
* `joint_7` must stay inside `work_reference_rad +/- corridor_rad`
(0 +/- 90 deg; boundaries inclusive).
* Every joint stays inside the real per-joint position limits
(`iiwa_config/config/moveit/joint_limits.yaml`, soft limits included).
Joint 7 keeps the neutral `work` value as a documented preference, but the
former +/-90 deg hard corridor is no longer a rejection boundary; safe paths
may use the full joint range.
* The tcp local +Z (working direction) must point down within
`max_tilt_deg` (10 deg).
* Consecutive points may not jump more than `max_joint_jump_rad` (0.6 rad) or
`max_cartesian_jump_m` (0.15 m); the first point must match the measured
joints used as IK seed.
* The planned trajectory is resampled in joint space at
`trajectory.max_sample_step_rad` (0.01 rad). Every sample is checked for
joint limits, self-collision, scene collision, camera clearance and tcp
orientation before execution.
* `joint_states` and the planning scene freshness are checked before planning.
Motion selection:
| Purpose | Motion |
| --- | --- |
| between `work`/`home`, to an object, tool or placement point, part transfer, safe alignment | Pilz PTP |
| axial contact, axial lift/retreat, dispense contour | Pilz LIN |
If a lateral PTP would start in a contact pose, the skill first performs the
axial LIN retreat to the saved approach height. The `dispense` contour itself
stays a continuous LIN contour; only its approach and retreat use PTP/axial
rules.
One explicit exception is recorded: if the measured start posture is not yet the
downward working posture (the calibrated home pose points the tip up) and the
gripper is empty, the first reorientation to `work` is allowed to pass through
larger tilt angles. Joint 7, joint jumps, Cartesian jumps and the final
gripper is empty, the first reorientation to `work` may pass through larger
tilt angles. Joint limits, joint jumps, Cartesian jumps and the final
downward orientation are still checked, and the event records
`reorientation: true`. A reorientation with a held object is always rejected.
@@ -183,26 +202,32 @@ downward orientation are still checked, and the event records
```python
arm.set_start_state(robot_state=clamped_robot_state) # measured joints as IK seed
arm.set_path_constraints(constraints) # hard joint_7 corridor
arm.set_path_constraints(empty_constraints) # no artificial J7 corridor
arm.set_goal_state(pose_stamped_msg=..., pose_link='tcp')
result = arm.plan(single_plan_parameters=params)
result.trajectory.get_robot_trajectory_msg() # every point checked
result.trajectory.get_robot_trajectory_msg()
# resample + joint/self/world/camera checks, then:
moveit.execute(result.trajectory, controllers=[])
```
The path constraint is a `JointConstraint` on `joint7` with
`tolerance_below/above = corridor_rad`. The planner applies it on OMPL
transfers. In this MoveIt/Pilz installation the orientation path constraint is
not honored reliably, so the 10 deg working-tilt limit is enforced **after**
planning by `MotionPolicy` on every returned point; the planner also gets a
wider internal orientation tolerance for its goal sampling. Tool transits use
OMPL with the J7 corridor, part transits and approaches use Cartesian LIN, and
short contact moves use LIN. A plan that cannot be produced or that violates
the point-wise checks is stopped; there is no unconstrained `MoveToPose`
fallback.
The planner never falls back to an unconstrained `MoveToPose` action. If a
plan is missing, the status names the phase, pipeline, planner id, target tcp
pose, start joints and MoveIt error code (for example `NO_IK_SOLUTION`). If
our validation rejects a plan, the status names the real cause: joint limit,
jump, orientation or the colliding link pair with the measured minimum
distance and required clearance.
The measured start state is clamped to the same soft joint limits used by
MoveIt (`soft_min_position`/`soft_max_position`) before it becomes the IK
seed. Without that clamp a Webots-controller overshoot of ~0.00003 rad makes
`CheckStartStateBounds` abort every following plan.
Camera collision model: the generated SRDF disables only constructive
contact pairs (`link7-camera_holder`, `camera_holder-patron`,
`camera_holder-camera_corner`, `camera_corner-camera_hand`,
`camera_holder-camera_hand`). Every camera-to-robot pair, including
`camera_hand-link6` and `camera_corner-link6`, remains checked. The D455
collision box matches its visible bounds; the measured AABB clearance from the
camera corner/hand to link6/link7 is at least 35 mm at `home` and `work`, and
the planner requires a non-zero minimum sampled clearance of 3 mm.
src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/motion_policy.py
+57 -17
View File
@@ -1,8 +1,8 @@
"""Trajectory-level TCP/J7 limits applied before anything reaches the controller.
"""Trajectory-level checks applied before anything reaches the controller.
The MoveToPose action cannot express a joint-7 corridor or a working tilt
limit, so every plan produced by the planner adapter is checked here, point
by point, before execution.
Every PTP/LIN plan is resampled and checked for joint limits, self/world
collision, working orientation and joint jumps. The old hard +/-90 deg joint-7
corridor is only a preference; real joint limits remain authoritative.
"""
from __future__ import annotations
@@ -31,9 +31,14 @@ class MotionLimits:
joint_state_max_age_sec: float
scene_max_age_sec: float
start_tolerance_rad: float = 0.35
min_clearance_m: float = 0.003
max_sample_step_rad: float = 0.01
@property
def joint7_min_rad(self) -> float:
# Kept for diagnostics/compatibility only. It is no longer a hard
# rejection boundary: safe paths may leave the preferred +/-90 deg
# band while remaining inside the real joint limits.
return self.work_joint7_rad - self.joint7_corridor_rad
@property
@@ -79,10 +84,23 @@ def load_limits(path: Path | str) -> MotionLimits:
scene_age = _finite(
freshness.get('scene_max_age_sec'), 'freshness.scene_max_age_sec',
)
collision = raw.get('collision') or {}
min_clearance = _finite(
collision.get('min_clearance_m', 0.003),
'collision.min_clearance_m',
)
if corridor <= 0 or tilt <= 0 or jump <= 0 or cartesian <= 0:
raise MotionPolicyError('motion limits must be positive')
if joint_age <= 0 or scene_age <= 0:
raise MotionPolicyError('freshness limits must be positive')
if min_clearance < 0:
raise MotionPolicyError('collision.min_clearance_m must be non-negative')
sample_step = _finite(
trajectory.get('max_sample_step_rad', 0.01),
'trajectory.max_sample_step_rad',
)
if sample_step <= 0:
raise MotionPolicyError('trajectory.max_sample_step_rad must be positive')
return MotionLimits(
work_joint7_rad=_finite(
joint7.get('work_reference_rad'), 'joint7.work_reference_rad',
@@ -97,6 +115,8 @@ def load_limits(path: Path | str) -> MotionLimits:
trajectory.get('start_tolerance_rad', 0.35),
'trajectory.start_tolerance_rad',
),
min_clearance_m=min_clearance,
max_sample_step_rad=sample_step,
)
@@ -119,8 +139,16 @@ def tcp_tilt_deg(pose: Transform) -> float:
class MotionPolicy:
def __init__(self, limits: MotionLimits) -> None:
def __init__(
self,
limits: MotionLimits,
joint_limits: dict[str, tuple[float, float]] | None = None,
) -> None:
self.limits = limits
self.joint_limits = dict(joint_limits or {})
self.joint_names = tuple(
f'joint{index}' for index in range(1, 8)
)
@property
def work_joint7(self) -> float:
@@ -189,19 +217,29 @@ class MotionPolicy:
min_j7 = min(point[6] for point in parsed)
max_j7 = max(point[6] for point in parsed)
j7_deviation = max(
abs(value - self.limits.work_joint7_rad) for value in (min_j7, max_j7)
)
epsilon = 1e-9
if (
min_j7 < self.limits.joint7_min_rad - epsilon
or max_j7 > self.limits.joint7_max_rad + epsilon
):
return MotionCheck(
False,
'joint7 leaves the hard corridor '
f'[{self.limits.joint7_min_rad:.3f}, '
f'{self.limits.joint7_max_rad:.3f}] rad '
f'(trajectory {min_j7:.3f}..{max_j7:.3f})',
{'min_j7': min_j7, 'max_j7': max_j7},
)
for index, values in enumerate(parsed):
for joint_index, value in enumerate(values):
name = self.joint_names[joint_index]
bounds = self.joint_limits.get(name)
if bounds is None:
continue
low, high = float(bounds[0]), float(bounds[1])
if value < low - epsilon or value > high + epsilon:
return MotionCheck(
False,
f'{name} at point {index} is {value:.4f} rad, '
f'outside the joint limit [{low:.4f}, {high:.4f}]',
{
'joint': name,
'value': value,
'limit': [low, high],
'point': index,
},
)
max_tilt = 0.0
start_tilt = (
@@ -281,6 +319,8 @@ class MotionPolicy:
'points': len(parsed),
'min_j7': min_j7,
'max_j7': max_j7,
'max_j7_deviation_rad': j7_deviation,
'preferred_j7_rad': self.limits.work_joint7_rad,
'max_tilt_deg': max_tilt,
'reorientation': reorientation,
'max_joint_jump_rad': max_jump,
src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/moveit_backend.py
+216 -75
View File
@@ -86,26 +86,49 @@ def _matrix_transform(matrix, parent: str, child: str) -> Transform:
)
def _error_code_name(value: int | None) -> str:
if value is None:
return 'unknown'
from moveit_msgs.msg import MoveItErrorCodes
for name in dir(MoveItErrorCodes):
if name.isupper() and getattr(MoveItErrorCodes, name) == value:
return f'{name}({value})'
return f'code {value}'
def _format_contacts(result: Any) -> str:
contacts = getattr(result, 'contacts', None)
pairs: list[str] = []
try:
for key in contacts.keys():
pairs.append(str(key))
except Exception:
try:
for item in contacts:
pairs.append(str(item))
except Exception:
pass
return ', '.join(sorted(set(pairs))) if pairs else 'unknown pair'
class MoveItPyPlanner:
"""Thin wrapper around MoveItPy with the constrained plan request."""
PHASES = {
# Pilz does not honor MoveIt path constraints, while ValidateSolution
# rejects trajectories that leave the joint-7 corridor. Transfers and
# approaches therefore use the constraint-aware OMPL pipeline.
'transfer': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0),
'approach': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0),
'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.18, 2.0),
'observe': ('ompl', 'RRTConnectkConfigDefault', 0.18, 10.0),
'ptp_transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0),
'transfer_lin': ('pilz_industrial_motion_planner', 'LIN', 0.15, 2.0),
# Short axial segments keep the working orientation; LIN remains the
# deterministic choice and is still checked point by point.
# PTP: free-space changes between working points, objects, tools,
# placement points and safe alignments. No J7 path constraint; the
# full sampled trajectory is checked afterwards.
'transfer': ('pilz_industrial_motion_planner', 'PTP', 0.25, 2.0),
'approach': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
'observe': ('pilz_industrial_motion_planner', 'PTP', 0.18, 2.0),
'align': ('pilz_industrial_motion_planner', 'PTP', 0.10, 2.0),
'escape': ('pilz_industrial_motion_planner', 'PTP', 0.15, 2.0),
# LIN: axial contact/retreat and the continuous dispense contour.
'contact': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
'lower': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
'align': ('pilz_industrial_motion_planner', 'LIN', 0.06, 2.0),
'retreat': ('pilz_industrial_motion_planner', 'LIN', 0.12, 2.0),
'dispense': ('pilz_industrial_motion_planner', 'LIN', 0.09, 2.0),
'escape': ('ompl', 'RRTConnectkConfigDefault', 0.20, 10.0),
}
def __init__(
@@ -130,6 +153,7 @@ class MoveItPyPlanner:
self._arm = None
self._model = None
self._group = None
self._scene_monitor = None
self._stopped = False
# ------------------------------------------------------------------ setup
@@ -149,6 +173,7 @@ class MoveItPyPlanner:
self._model = self._moveit.get_robot_model()
self._arm = self._moveit.get_planning_component(self.planning_group)
self._group = self._model.get_joint_model_group(self.planning_group)
self._scene_monitor = self._moveit.get_planning_scene_monitor()
except Exception as error:
raise PlannerError(f'MoveItPy is unavailable: {error}') from error
@@ -162,41 +187,10 @@ class MoveItPyPlanner:
)
def _constraints(self, goal: Transform, *, include_orientation: bool = True):
from moveit_msgs.msg import Constraints, JointConstraint, OrientationConstraint
del goal, include_orientation
from moveit_msgs.msg import Constraints
limits = self.policy.limits
constraints = Constraints()
joint = JointConstraint()
joint.joint_name = 'joint7'
joint.position = float(limits.work_joint7_rad)
joint.tolerance_above = float(limits.joint7_corridor_rad)
joint.tolerance_below = float(limits.joint7_corridor_rad)
joint.weight = 1.0
constraints.joint_constraints.append(joint)
# The model uses the same joint order as the Webots controller. With
# a hard joint-7 corridor, this constraint prevents the IK branch that
# would flip joint 7 by about -180 degrees.
if not include_orientation:
return constraints
orientation = OrientationConstraint()
orientation.header.frame_id = self.pose_frame
orientation.link_name = self.pose_link
quaternion = Rotation.from_matrix(goal.matrix).as_quat()
orientation.orientation.x = float(quaternion[0])
orientation.orientation.y = float(quaternion[1])
orientation.orientation.z = float(quaternion[2])
orientation.orientation.w = float(quaternion[3])
# Keep the planner inside a stricter band than the final policy check
# (10 deg). OMPL interpolation can otherwise drift a fraction of a
# degree above the policy boundary and reject an otherwise valid path.
tilt = math.radians(float(limits.max_tilt_deg) * 2.0)
orientation.absolute_x_axis_tolerance = tilt
orientation.absolute_y_axis_tolerance = tilt
orientation.absolute_z_axis_tolerance = tilt
orientation.weight = 1.0
constraints.orientation_constraints.append(orientation)
return constraints
return Constraints()
def _parameters(self, phase: str):
from moveit.planning import PlanRequestParameters
@@ -224,6 +218,13 @@ class MoveItPyPlanner:
if self._stopped:
raise PlannerError('planner is stopped')
base_goal = self._goal_in_pose_frame(goal)
effective_phase = phase
print(
'[iiwa_llm_plan_executor] plan request '
f'phase={phase!r} goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} '
f'goal_quat={tuple(round(float(v), 6) for v in Rotation.from_matrix(base_goal.matrix).as_quat())}',
flush=True,
)
try:
self._arm.set_start_state_to_current_state()
start_state = self._arm.get_start_state()
@@ -266,6 +267,12 @@ class MoveItPyPlanner:
self._arm.set_start_state(robot_state=clamped_state)
start_state = self._arm.get_start_state()
current = tuple(clamped)
print(
'[iiwa_llm_plan_executor] plan start '
f'measured={tuple(round(float(v), 6) for v in measured)} '
f'clamped={tuple(round(float(v), 6) for v in current)}',
flush=True,
)
start_matrix = start_state.get_global_link_transform(self.pose_link)
start_tcp = _matrix_transform(
start_matrix, self.pose_frame, self.pose_link,
@@ -275,22 +282,20 @@ class MoveItPyPlanner:
and held_object is None
)
effective_phase = phase
if phase == 'approach':
# Short approach segments are Cartesian and keep the tip down.
effective_phase = 'transfer_lin'
# Tool transfers use OMPL with the J7 corridor; part transfers
# use the explicit transfer_lin phase from the skill. Working
# tilt is always checked point by point by MotionPolicy.
if effective_phase in (
'contact', 'lower', 'align', 'dispense', 'retreat',
'transfer_lin',
):
from moveit_msgs.msg import Constraints
self._arm.set_path_constraints(Constraints())
else:
self._arm.set_path_constraints(self._constraints(
base_goal, include_orientation=False,
))
# J7 has no artificial +/-90 deg corridor and PTP/LIN goal IK is
# allowed to use the whole joint range. Safety comes from the
# dense post-plan joint/self/world/orientation checks below.
from moveit_msgs.msg import Constraints
self._arm.set_path_constraints(Constraints())
phase_params = self.PHASES.get(
effective_phase, self.PHASES['transfer'],
)
print(
'[iiwa_llm_plan_executor] plan pipeline '
f'phase={phase!r} pipeline={phase_params[0]!r} '
f'planner_id={phase_params[1]!r} planning_time={phase_params[3]}',
flush=True,
)
self._arm.set_goal_state(
pose_stamped_msg=_pose_message(base_goal, self.pose_frame),
pose_link=self.pose_link,
@@ -301,12 +306,30 @@ class MoveItPyPlanner:
except PlannerError:
raise
except Exception as error:
print(
'[iiwa_llm_plan_executor] plan exception '
f'phase={phase!r} effective_phase={effective_phase!r} '
f'error={error!r}',
flush=True,
)
raise PlannerError(f'constrained planning failed: {error}') from error
if not result or result.trajectory is None:
if result is None or result.trajectory is None:
error_code = (
None if result is None
else getattr(result.error_code, 'val', None)
)
error_name = _error_code_name(error_code)
print(
'[iiwa_llm_plan_executor] plan empty '
f'phase={phase!r} pipeline={effective_phase!r} '
f'error_code={error_name} goal_xyz={base_goal.xyz}',
flush=True,
)
raise PlannerError(
f'no constrained trajectory for phase {phase!r} '
f'(joint7 corridor {self.policy.limits.joint7_min_rad:.3f}..'
f'{self.policy.limits.joint7_max_rad:.3f} rad)'
f'planning failed for phase {phase!r} '
f'({effective_phase}, error {error_name}); '
f'goal_xyz={tuple(round(float(v), 6) for v in base_goal.xyz)} '
f'start={tuple(round(float(v), 6) for v in current)}'
)
message = result.trajectory.get_robot_trajectory_msg()
names = list(message.joint_trajectory.joint_names)
@@ -316,20 +339,24 @@ class MoveItPyPlanner:
f'planned trajectory misses joints {sorted(set(expected) - set(names))}'
)
index = {name: position for position, name in enumerate(names)}
points: list[tuple[float, ...]] = []
raw_points: list[tuple[float, ...]] = []
for waypoint in message.joint_trajectory.points:
raw_points.append(tuple(
float(waypoint.positions[index[name]]) for name in expected
))
points = self._densify(
raw_points, self.policy.limits.max_sample_step_rad,
)
poses: list[Transform] = []
from moveit.core.robot_state import RobotState
state = RobotState(self._model)
for waypoint in message.joint_trajectory.points:
ordered = tuple(
float(waypoint.positions[index[name]]) for name in expected
)
points.append(ordered)
for ordered in points:
state.set_joint_group_positions(self.planning_group, ordered)
state.update()
matrix = state.get_global_link_transform(self.pose_link)
poses.append(_matrix_transform(matrix, self.pose_frame, self.pose_link))
clearance = self._check_collisions(points)
return PlannedMotion(
points=points,
tcp_poses=poses,
@@ -343,9 +370,110 @@ class MoveItPyPlanner:
effective_phase, self.PHASES['transfer'],
)[:2],
'reorientation': reorientation,
'raw_points': len(raw_points),
'sampled_points': len(points),
'min_clearance_m': self.policy.limits.min_clearance_m,
'measured_clearance_m': clearance,
},
)
@staticmethod
def _densify(
points: list[tuple[float, ...]],
max_step: float,
) -> list[tuple[float, ...]]:
if not points:
return []
result = [points[0]]
for previous, current_point in zip(points, points[1:]):
delta = max(
abs(a - b) for a, b in zip(current_point, previous)
)
steps = max(1, int(math.ceil(delta / max_step)))
for step in range(1, steps + 1):
fraction = step / steps
result.append(tuple(
a + (b - a) * fraction
for a, b in zip(previous, current_point)
))
return result
def _check_collisions(self, points: list[tuple[float, ...]]) -> float | None:
if self._scene_monitor is None:
raise PlannerError('planning scene monitor is unavailable')
from moveit.core.collision_detection import (
CollisionRequest,
CollisionResult,
)
from moveit.core.robot_state import RobotState
requirement = float(self.policy.limits.min_clearance_m)
measured_min: float | None = None
state = RobotState(self._model)
with self._scene_monitor.read_only() as scene:
acm = getattr(scene, 'allowed_collision_matrix', None)
for index, sample in enumerate(points):
state.set_joint_group_positions(self.planning_group, sample)
state.update()
request = CollisionRequest()
request.distance = True
request.contacts = True
request.max_contacts = 20
request.max_contacts_per_pair = 2
collided = False
checked = False
last_error = None
distances: list[float] = []
contacts: list[str] = []
for function in (
'check_collision_unpadded', 'check_self_collision',
):
for arguments in (
(request, CollisionResult(), state, acm),
(request, CollisionResult(), state),
):
if acm is None and len(arguments) == 4:
continue
try:
result = arguments[1]
hit = bool(getattr(scene, function)(*arguments))
checked = True
collided = collided or hit
except TypeError as error:
last_error = error
continue
try:
distances.append(float(result.distance))
except Exception:
pass
formatted = _format_contacts(result)
if formatted != 'unknown pair':
contacts.append(formatted)
if not checked:
raise PlannerError(
'collision checking is unavailable: '
f'{last_error}'
)
distance = min(distances) if distances else None
if distance is not None:
measured_min = (
distance if measured_min is None
else min(measured_min, distance)
)
if collided or (
distance is not None and distance < requirement
):
pairs = ', '.join(sorted(set(contacts))) or 'unknown pair'
measured = (
'unknown' if distance is None else f'{distance:.4f} m'
)
raise PlannerError(
f'collision/clearance failure at sample {index}: '
f'{pairs}; minimum distance {measured}, '
f'required {requirement:.4f} m'
)
return measured_min
def execute(self, planned: PlannedMotion) -> bool:
self._ensure()
try:
@@ -382,6 +510,7 @@ class MoveItBackend:
scene_guard: Callable[[], tuple[bool, str]] | None = None,
current_joints: Callable[[], tuple[float, ...] | None] | None = None,
before_plan: Callable[[], None] | None = None,
collision_checker: Callable[[Any], str | None] | None = None,
on_event: Callable[[dict[str, Any]], None] | None = None,
) -> None:
self.geometry = geometry
@@ -391,6 +520,7 @@ class MoveItBackend:
self._scene_guard = scene_guard
self._current_joints = current_joints
self._before_plan = before_plan
self._collision_checker = collision_checker
self._on_event = on_event
self.stopped = False
self.stop_reason: str | None = None
@@ -482,9 +612,20 @@ class MoveItBackend:
except Exception:
pass
return self._fail(
f'motion policy rejected every constrained trajectory: {last_error}',
f'motion policy rejected every planned trajectory: {last_error}',
'motion_limit',
)
if self._collision_checker is not None:
try:
collision_reason = self._collision_checker(planned)
except Exception as error:
collision_reason = f'collision check failed: {error}'
if collision_reason:
try:
self.planner.stop()
except Exception:
pass
return self._fail(collision_reason, 'collision')
target = (
goal
if goal.parent == 'base_link'
@@ -505,7 +646,7 @@ class MoveItBackend:
)
self._emit(
event='motion_planned', phase=phase, held_object=held_object,
**check.details,
planner_details=dict(planned.details), **check.details,
)
try:
executed = self.planner.execute(planned)
src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/node.py
+10 -5
View File
@@ -321,7 +321,9 @@ class PlannerExecutorNode(Node):
self.limits = load_limits(
str(self.get_parameter('motion_limits_path').value),
)
self.policy = MotionPolicy(self.limits)
self.policy = MotionPolicy(
self.limits, self._read_joint_limits(),
)
self.close_inspection = frozenset(
str(value) for value in self.get_parameter(
'close_inspection_objects',
@@ -458,11 +460,9 @@ class PlannerExecutorNode(Node):
)
# ------------------------------------------------------------------ setup
def _make_planner(self):
def _read_joint_limits(self) -> dict[str, tuple[float, float]]:
import yaml
from .moveit_backend import MoveItPyPlanner
limits_path = Path(str(self.get_parameter('joint_limits_path').value))
joint_limits = {}
try:
@@ -482,12 +482,17 @@ class PlannerExecutorNode(Node):
self.get_logger().warning(
f'falling back to model joint bounds: {error}'
)
return joint_limits
def _make_planner(self):
from .moveit_backend import MoveItPyPlanner
return MoveItPyPlanner(
self.geometry,
self.policy,
planning_group=str(self.get_parameter('planning_group').value),
pose_link=str(self.get_parameter('pose_link').value),
joint_limits=joint_limits,
joint_limits=self._read_joint_limits(),
)
def _now(self) -> float:
src/iiwa_llm_plan_executor/iiwa_llm_plan_executor/skills.py
+21 -4
View File
@@ -166,6 +166,25 @@ class SkillRegistry:
goal: Transform,
phase: str,
) -> Any:
lateral = phase in ('transfer', 'approach', 'align', 'observe', 'escape')
if (
lateral
and context.phase == 'contact'
and context.approach_pose is not None
):
# A lateral PTP must never start at contact depth: first move
# straight up along the tool axis to the saved safe height.
retreat = self.motion.plan_and_execute(
context.approach_pose, 'retreat', context.held_object,
)
if not getattr(retreat, 'success', False):
raise SkillError(
'failed',
f'axial retreat before lateral motion failed: '
f'{getattr(retreat, "message", "motion failed")}',
)
context.last_verified_pose = context.approach_pose
context.mark_lifted()
carried = context.held_object or context.mounted_tool
outcome = self.motion.plan_and_execute(goal, phase, carried)
if not getattr(outcome, 'success', False):
@@ -362,8 +381,7 @@ class SkillRegistry:
context.mark_lifted()
work = context.geometry.safe_poses['work']
if not (context.at_work and context.phase == 'free'):
phase = 'transfer_lin' if context.phase != 'free' else 'transfer'
self._move(context, work, phase)
self._move(context, work, 'transfer')
context.reset_phase()
context.at_work = True
if context.capture_frame('top') is None:
@@ -601,8 +619,7 @@ class SkillRegistry:
return SkillOutcome('failed', {'reason': f'{obj!r} cannot be transferred'})
goal = self._destination_goal(context, obj, target)
safe = _above(goal, context.geometry.transfer_gap)
phase = 'transfer' if obj in context.spec.tool_names else 'transfer_lin'
self._move(context, safe, phase)
self._move(context, safe, 'transfer')
context.transfer_target = target
context.mark_approach(safe, (obj, target))
return SkillOutcome('success', {
src/iiwa_llm_plan_executor/test/test_motion_policy.py
+31 -18
View File
@@ -40,34 +40,46 @@ def down_pose(tilt_deg: float = 0.0, xyz=(0.4, 0.0, 1.2)) -> Transform:
)
def test_work_joint7_is_zero_and_corridor_is_inclusive():
def test_work_joint7_is_the_preference_not_a_hard_corridor():
limits = load_limits(LIMITS)
assert limits.work_joint7_rad == 0.0
assert limits.joint7_min_rad == -math.pi / 2
assert limits.joint7_max_rad == math.pi / 2
checker = MotionPolicy(limits)
for boundary in (-math.pi / 2, math.pi / 2):
def test_safe_joint7_beyond_the_old_ninety_degree_band_is_accepted():
checker = policy()
for value in (-2.0, 2.0):
result = checker.validate_trajectory(
[point(boundary)], [down_pose()],
[point(value)], [down_pose()],
)
assert result.ok, result.reason
def test_point_outside_the_corridor_is_rejected():
result = policy().validate_trajectory(
[point(-math.pi / 2 - 0.01)], [down_pose()],
)
assert result.ok is False
assert 'joint7' in result.reason
def test_intermediate_point_outside_the_corridor_rejects_the_whole_path():
result = policy().validate_trajectory(
[point(-1.2), point(-1.6), point(-1.2)],
result = checker.validate_trajectory(
[point(-1.8), point(-2.0), point(-1.8)],
[down_pose(), down_pose(), down_pose()],
)
assert result.ok, result.reason
assert result.details['max_j7_deviation_rad'] == pytest.approx(2.0)
def test_real_joint_limit_is_still_enforced():
checker = MotionPolicy(
load_limits(LIMITS),
joint_limits={
'joint1': (-2.967, 2.967),
'joint2': (-2.09, 2.09),
'joint3': (-2.967, 2.967),
'joint4': (-2.09, 2.09),
'joint5': (-2.967, 2.967),
'joint6': (-2.0027, 2.0027),
'joint7': (-3.05, 3.05),
},
)
result = checker.validate_trajectory(
[point(3.2)], [down_pose()],
)
assert result.ok is False
assert 'joint7' in result.reason
assert 'joint limit' in result.reason
def test_near_pi_joint_jump_is_rejected():
@@ -170,3 +182,4 @@ def test_details_report_the_checked_extremes():
assert result.details['min_j7'] == pytest.approx(-0.2)
assert result.details['max_j7'] == pytest.approx(0.1)
assert result.details['max_tilt_deg'] == pytest.approx(4.0, abs=1e-6)
assert result.details['preferred_j7_rad'] == pytest.approx(0.0)
src/iiwa_llm_plan_executor/test/test_moveit_backend.py
+44 -5
View File
@@ -112,17 +112,56 @@ def test_safe_plan_is_executed_once_and_the_final_pose_is_verified():
assert len(planner.execute_calls) == 1
def test_intermediate_out_of_corridor_point_never_reaches_execution():
def test_safe_joint7_beyond_the_old_corridor_reaches_execution():
planner = FakePlanner(planned=planned(
[point(-1.0), point(-1.8), point(-1.0)],
[point(-1.5), point(-1.7), point(-1.5)],
[BASE_GOAL, BASE_GOAL, BASE_GOAL],
))
result = backend(planner).plan_and_execute(GOAL, 'transfer')
assert result.success is True
assert len(planner.execute_calls) == 1
def test_motion_phase_selection_is_ptp_for_working_points_and_lin_for_axes():
from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner
phases = MoveItPyPlanner.PHASES
for phase in ('transfer', 'approach', 'align', 'observe', 'escape'):
assert phases[phase][0] == 'pilz_industrial_motion_planner'
assert phases[phase][1] == 'PTP'
for phase in ('contact', 'lower', 'retreat', 'dispense'):
assert phases[phase][0] == 'pilz_industrial_motion_planner'
assert phases[phase][1] == 'LIN'
def test_dense_resampling_covers_steps_between_waypoints():
from iiwa_llm_plan_executor.moveit_backend import MoveItPyPlanner
samples = MoveItPyPlanner._densify(
[(0.0,) * 7, (0.05,) * 7], 0.01,
)
assert len(samples) == 6
assert samples[0] == (0.0,) * 7
assert samples[-1] == (0.05,) * 7
assert max(
abs(a - b) for a, b in zip(samples[1], samples[0])
) <= 0.01 + 1e-12
def test_camera_self_collision_is_rejected_before_execution():
planner = FakePlanner(planned=planned([point(0.0)], [BASE_GOAL]))
result = backend(
planner,
collision_checker=lambda planned_motion: (
'collision/clearance failure: camera_hand <-> link6; '
'minimum distance 0.0005 m, required 0.0030 m'
),
).plan_and_execute(GOAL, 'transfer')
assert result.success is False
assert result.code == 'motion_limit'
assert 'joint7' in result.message
assert result.code == 'collision'
assert 'camera_hand' in result.message
assert 'link6' in result.message
assert planner.execute_calls == []
assert planner.stopped >= 1
def test_excessive_tilt_and_jump_are_rejected_without_execution():
src/iiwa_llm_plan_executor/test/test_skills.py
+23 -1
View File
@@ -440,6 +440,28 @@ def test_release_lifts_axially_before_any_lateral_motion():
assert motion.calls[-1][0].xyz[2] > contact.xyz[2]
def test_transfer_from_contact_lifts_axially_before_lateral_ptp():
registry, context, motion, supervisor = make_registry()
context.mounted_tool = 'vacuum'
context.held_object = 'cap'
supervisor.state['mounted_tool'] = 'vacuum'
supervisor.state['held_object'] = 'cap'
supervisor.state['cap']['state'] = 'held'
seat = world_target(context.spec, context.geometry, 'cap_seat')
from iiwa_experement.geometry import tcp_goal
from iiwa_llm_plan_executor.skills import _above
contact = tcp_goal(seat, context.geometry.cap_tcp_to_node)
safe = _above(contact, context.geometry.approach_gap)
context.mark_approach(safe, ('cap', 'hatch_seat'))
context.mark_contact(contact)
result = registry.execute(
node('t', 'transfer', 'cap', 'hatch_seat'), context,
)
assert result.event == 'success'
assert phases(motion) == ['retreat', 'transfer']
assert motion.calls[0][1] == 'retreat'
def test_dispense_moves_along_the_segment_without_changing_ownership():
registry, context, motion, supervisor = make_registry()
context.mounted_tool = 'sealant_tool'
@@ -490,7 +512,7 @@ def test_detect_withdraws_axially_then_uses_fresh_frames():
result = registry.execute(node('d', 'detect', 'cap'), context)
assert result.event == 'found'
assert phases(motion)[0] == 'retreat'
assert phases(motion)[1] == 'transfer_lin'
assert phases(motion)[1] == 'transfer'
assert context.phase == 'free'
context.frame_provider = lambda name: None
src/iiwa_utils/iiwa_utils/tool_manager.py
-18
View File
@@ -19,29 +19,11 @@ from typing import Any
# ---------------------------------------------------------------------------
_BASE_COLLISIONS: list[tuple[str, str, str]] = [
("base_link", "link1", "Adjacent"),
("base_link", "link2", "Never"),
("base_link", "link3", "Never"),
("base_link", "link4", "Never"),
("link1", "link2", "Adjacent"),
("link1", "link3", "Never"),
("link1", "link4", "Never"),
("link1", "link5", "Never"),
("link1", "link6", "Never"),
("link1", "link7", "Never"),
("link2", "link3", "Adjacent"),
("link2", "link4", "Never"),
("link2", "link5", "Never"),
("link2", "link6", "Never"),
("link2", "link7", "Never"),
("link3", "link4", "Adjacent"),
("link3", "link5", "Never"),
("link3", "link6", "Never"),
("link3", "link7", "Never"),
("link4", "link5", "Adjacent"),
("link4", "link6", "Never"),
("link4", "link7", "Never"),
("link5", "link6", "Adjacent"),
("link5", "link7", "Never"),
("link6", "link7", "Adjacent"),
]
src/iiwa_utils/test/test_camera_clearance.py
+150
View File
@@ -0,0 +1,150 @@
"""Visible camera geometry has a positive gap to the robot links."""
from __future__ import annotations
from pathlib import Path
import numpy as np
from scipy.spatial.transform import Rotation
from urdf_parser_py.urdf import URDF
from ament_index_python.packages import get_package_share_directory
from iiwa_utils import converter, setting_loader
ROOT = Path(__file__).resolve().parents[2]
MIN_GAP_M = 0.005
def _transform(xyz, rpy):
matrix = np.eye(4)
matrix[:3, :3] = Rotation.from_euler('xyz', rpy).as_matrix()
matrix[:3, 3] = xyz
return matrix
def _axis_rotation(axis, angle):
vector = np.asarray(axis, dtype=float)
vector /= np.linalg.norm(vector)
matrix = np.eye(4)
matrix[:3, :3] = Rotation.from_rotvec(vector * angle).as_matrix()
return matrix
def _robot_description():
settings = setting_loader.build_settings(
settings_path=str(
Path(get_package_share_directory('iiwa_config'))
/ 'config' / 'setting.yaml'
),
check_files=True,
)
return URDF.from_xml_string(converter.load_robot_description(
settings.robot.description,
settings.robot.name,
{
'simulate': 'true',
'initial_positions_file': settings.controller.moveit.initial_positions,
'joint_limits_file': settings.controller.moveit.joint_limits,
},
))
def _fk(robot, target, joints):
by_child = {joint.child: joint for joint in robot.joints}
chain = []
node = target
while node != 'base_link':
joint = by_child[node]
chain.append(joint)
node = joint.parent
result = np.eye(4)
for joint in reversed(chain):
result = result @ _transform(joint.origin.xyz, joint.origin.rpy)
if joint.type == 'revolute':
result = result @ _axis_rotation(
joint.axis, joints.get(joint.name, 0.0),
)
return result
def _stl_box(path):
raw = Path(path).read_bytes()
count = int(np.frombuffer(raw, dtype='<u4', count=1, offset=80)[0])
dtype = np.dtype([('n', '<f4', 3), ('v', '<f4', (3, 3)), ('a', '<u2')])
vertices = np.frombuffer(
raw, dtype=dtype, count=count, offset=84,
)['v'].reshape(-1, 3)
return vertices.min(axis=0), vertices.max(axis=0)
def _corners(centre, half, rotation):
signs = np.array([
[x, y, z]
for x in (-1, 1) for y in (-1, 1) for z in (-1, 1)
], dtype=float)
return centre + (signs @ np.diag(half)) @ rotation.T
def _aabb_gap(first, second):
delta = np.maximum(
0.0,
np.maximum(first.min(axis=0) - second.max(axis=0),
second.min(axis=0) - first.max(axis=0)),
)
return float(np.linalg.norm(delta))
CAMERA_BOXES = {
'camera_holder': [
([-0.035, -0.035, 0.0], [0.035, 0.035, 0.03]),
([-0.015, 0.03, 0.0], [0.015, 0.13, 0.03]),
],
'camera_corner': [
([-0.062, 0.1015, 0.0135], [0.062, 0.1305, 0.0305]),
],
'camera_hand': [
([-0.06, 0.0, 0.0], [0.064, 0.031, 0.026]),
],
}
def _gaps(joints):
robot = _robot_description()
result = {}
links = {}
for name in ('link6', 'link7'):
transform = _fk(robot, name, joints)
minimum, maximum = _stl_box(
ROOT / 'iiwa_description' / 'resource' / 'meshes'
/ 'iiwa' / 'collision' / f'{name}.STL'
)
centre = transform[:3, :3] @ ((minimum + maximum) / 2.0) + transform[:3, 3]
corners = _corners(
centre, (maximum - minimum) / 2.0, transform[:3, :3],
)
links[name] = corners
for camera, boxes in CAMERA_BOXES.items():
transform = _fk(robot, camera, joints)
for link_name, link_corners in links.items():
for minimum, maximum in boxes:
centre_local = (np.array(minimum) + np.array(maximum)) / 2.0
half = (np.array(maximum) - np.array(minimum)) / 2.0
centre = transform[:3, :3] @ centre_local + transform[:3, 3]
corners = _corners(centre, half, transform[:3, :3])
result[(camera, link_name)] = min(
result.get((camera, link_name), float('inf')),
_aabb_gap(corners, link_corners),
)
return result
def test_camera_corner_and_hand_clear_the_robot_at_work_and_home():
for joints in ({}, {'joint4': -1.57, 'joint6': 1.57}):
gaps = _gaps(joints)
for camera in ('camera_corner', 'camera_hand'):
for link in ('link6', 'link7'):
assert gaps[(camera, link)] >= MIN_GAP_M, (camera, link, gaps)
# The holder is bolted to the flange; only link7 contact is
# constructive and disabled in the SRDF.
assert gaps[('camera_holder', 'link7')] == 0.0
assert gaps[('camera_holder', 'link6')] >= MIN_GAP_M
src/iiwa_utils/test/test_tool_collisions.py
+85
View File
@@ -0,0 +1,85 @@
"""The camera/tool collision model only disables constructive contact pairs."""
from __future__ import annotations
import xml.etree.ElementTree as ET
from pathlib import Path
from iiwa_utils.tool_manager import _BASE_COLLISIONS, load_registry
ROOT = Path(__file__).resolve().parents[2]
TOOLS = ROOT / 'iiwa_config' / 'config' / 'tools.yaml'
SRDF = ROOT / 'iiwa_config' / 'config' / 'moveit' / 'iiwa7.srdf'
PATRON = ROOT / 'iiwa_description' / 'urdf' / 'tools' / 'patron.xacro'
FIXED_PAIRS = {
tuple(sorted(pair)) for pair in (
('link7', 'camera_holder'),
('camera_holder', 'patron'),
('camera_holder', 'camera_corner'),
('camera_corner', 'camera_hand'),
('camera_holder', 'camera_hand'),
)
}
def pairs(entries):
return {
tuple(sorted((str(first), str(second))))
for first, second, *_ in entries
}
def test_only_constructive_camera_pairs_are_disabled_in_the_source():
registry = load_registry(TOOLS)
collisions = registry['patron']['collisions']
assert all(reason == 'Adjacent' for _, _, reason in collisions)
assert pairs(collisions) == FIXED_PAIRS
def test_only_adjacent_robot_links_are_disabled():
assert all(reason == 'Adjacent' for _, _, reason in _BASE_COLLISIONS)
assert pairs(_BASE_COLLISIONS) == {
('base_link', 'link1'),
('link1', 'link2'),
('link2', 'link3'),
('link3', 'link4'),
('link4', 'link5'),
('link5', 'link6'),
('link6', 'link7'),
}
def test_generated_srdf_keeps_camera_robot_collisions_enabled():
root = ET.parse(SRDF).getroot()
entries = [
(
element.get('link1'),
element.get('link2'),
element.get('reason'),
)
for element in root.findall('disable_collisions')
]
assert all(reason == 'Adjacent' for _, _, reason in entries)
disabled = pairs(entries)
for camera in ('camera_holder', 'camera_corner', 'camera_hand'):
for robot_link in (
'base_link', 'link1', 'link2', 'link3', 'link4',
'link5', 'link6', 'link7',
):
pair = tuple(sorted((camera, robot_link)))
if pair in FIXED_PAIRS:
assert pair in disabled
else:
assert pair not in disabled, pair
def test_camera_hand_collision_box_matches_the_visible_d455_bounds():
root = ET.parse(PATRON).getroot()
link = root.find("link[@name='camera_hand']")
collision = link.find('collision')
origin = collision.find('origin')
box = collision.find('geometry/box')
values = [float(value) for value in box.get('size').split()]
centre = [float(value) for value in origin.get('xyz').split()]
assert values == [0.124, 0.031, 0.026]
assert all(abs(a - b) < 1e-9 for a, b in zip(centre, [0.002, 0.0155, 0.013]))