refactor(skills): shared skill_lib for arm primitives + camera geometry - #548
Conversation
Introduce workspace/skill_lib/ — plain Python modules skills import directly, no Skill classes, no roster entries. Functions take the interfaces they need as explicit arguments. The hardware lessons now live exactly once: - skill_lib/arm.py: verified gripper open (overcurrent-trip reboot recovery), close with the strength-cap lesson, health-checked cartesian moves (FK verify, recover + retry, raise ArmUnhealthy), servo recovery, reach clamps. - skill_lib/geometry.py: head-camera <-> floor pinhole projection, moved verbatim from pick_any_object (pure math; the webapp panel mirrors it). gripper_open / gripper_close become thin wrappers over the lib — and gripper_open GAINS the trip-recovery verification that previously only pick_any_object had. pick_any_object drops ~150 lines of private helpers (_move_arm, _recover_arm, geometry, inline reach clamps) for lib calls, and calls the lib directly instead of round-tripping gripper skills through the invoker. Import contract: skills import the lib at module top — the loader puts the repo root on sys.path only while a skill module executes (namespace-package import, verified live). catalog.reload_all/reload_selective now evict cached workspace.skill_lib modules so a normal skills reload picks up lib edits too. No behavior or compatibility changes otherwise: skill IDs, innate.skills chaining, panel debug events, and TUNABLE keys are unchanged. Verified live on the robot: reload loads all 23 skills, pick_any_object acks its params through the lib imports, and gripper_close/gripper_open ran end-to-end via the skill CLI (claw physically cycled and returned to open).
Greptile SummaryThis PR introduces
Confidence Score: 3/5The consolidation is architecturally clean, but pick_any_object discards the open_checked return value in _open_gripper_checked, meaning a gripper that fails to open after servo recovery will let the skill continue into the grasp with a closed claw rather than aborting. The refactoring correctly extracts hardware knowledge into one place and gripper_open.py is handled properly. The gap is specifically in pick_any_object._open_gripper_checked, which calls armlib.open_checked without capturing or acting on the bool it returns. If the recovery sequence does not clear the trip, the pick proceeds as if the gripper is open — the robot will attempt to grasp and lift an object with a closed hand. workspace/innate_skills/pick_any_object.py (_open_gripper_checked return-value handling) and workspace/skill_lib/arm.py (open_checked post-retry j6 re-verification). Important Files Changed
Sequence Diagram%%{init: {'theme': 'neutral'}}%%
sequenceDiagram
participant S as Skill (gripper_open / pick_any_object)
participant AL as armlib (skill_lib/arm.py)
participant M as manipulation interface
participant JS as joint_states (RobotState)
Note over S,JS: open_checked flow
S->>AL: open_checked(manipulation, get_j6, ...)
AL->>M: torque_on()
AL->>M: "open_gripper(blocking=True)"
M-->>AL: ok
AL->>JS: get_j6()
JS-->>AL: j6 value
alt "j6 >= 0.10 (opened OK)"
AL-->>S: bool(ok)
else "j6 < 0.10 (trip detected)"
AL->>AL: recover() — reboot_servos + torque_on
AL->>M: "open_gripper(blocking=True) [retry]"
M-->>AL: ok2
AL-->>S: bool(ok2)
Note over S: return value discarded in _open_gripper_checked
end
Note over S,JS: move_checked flow
S->>AL: move_checked(manipulation, x, y, z, pitch, ...)
loop attempt 1 then 2
AL->>M: "move_to_cartesian_pose(blocking=True)"
M-->>AL: ok
AL->>M: get_current_end_effector_pose()
M-->>AL: ee_xyz
alt "ok and err <= tol"
AL-->>S: True
else
AL->>AL: recover() [attempt 1 only]
end
end
AL-->>S: raises ArmUnhealthy
%%{init: {'theme': 'base', 'themeVariables': {"darkMode": true, "background": "#0d1117", "primaryColor": "#21262d", "primaryTextColor": "#e6edf3", "primaryBorderColor": "#8b949e", "lineColor": "#8b949e", "textColor": "#e6edf3", "edgeLabelBackground": "#161b22", "actorBkg": "#21262d", "actorBorder": "#8b949e", "actorTextColor": "#e6edf3", "actorLineColor": "#8b949e", "signalColor": "#8b949e", "signalTextColor": "#e6edf3", "noteBkgColor": "#373320", "noteBorderColor": "#d4a72c", "noteTextColor": "#f0e6c0", "labelBoxBkgColor": "#21262d", "labelBoxBorderColor": "#8b949e", "labelTextColor": "#e6edf3", "loopTextColor": "#e6edf3", "activationBkgColor": "#30363d", "activationBorderColor": "#8b949e"}}}%%
sequenceDiagram
participant S as Skill (gripper_open / pick_any_object)
participant AL as armlib (skill_lib/arm.py)
participant M as manipulation interface
participant JS as joint_states (RobotState)
Note over S,JS: open_checked flow
S->>AL: open_checked(manipulation, get_j6, ...)
AL->>M: torque_on()
AL->>M: "open_gripper(blocking=True)"
M-->>AL: ok
AL->>JS: get_j6()
JS-->>AL: j6 value
alt "j6 >= 0.10 (opened OK)"
AL-->>S: bool(ok)
else "j6 < 0.10 (trip detected)"
AL->>AL: recover() — reboot_servos + torque_on
AL->>M: "open_gripper(blocking=True) [retry]"
M-->>AL: ok2
AL-->>S: bool(ok2)
Note over S: return value discarded in _open_gripper_checked
end
Note over S,JS: move_checked flow
S->>AL: move_checked(manipulation, x, y, z, pitch, ...)
loop attempt 1 then 2
AL->>M: "move_to_cartesian_pose(blocking=True)"
M-->>AL: ok
AL->>M: get_current_end_effector_pose()
M-->>AL: ee_xyz
alt "ok and err <= tol"
AL-->>S: True
else
AL->>AL: recover() [attempt 1 only]
end
end
AL-->>S: raises ArmUnhealthy
|
![greptile-apps[bot]](https://avatars.githubusercontent.com/in/867647?s=60&v=4){kind=small}
| def _open_gripper_checked(self): | ||
| """Open the claw, and make sure it actually opened. A prior hard | ||
| close can overcurrent-trip the gripper servo — a hardware fault | ||
| torque_on alone can't clear — after which open silently no-ops and | ||
| the hand stays shut straight into the grasp. Reboot clears the trip.""" | ||
| gripper_open() | ||
| j6 = self._gripper_j6() | ||
| if j6 is not None and j6 < 0.10: | ||
| self.logger.warning( | ||
| f"[PickAnyObject] gripper did not open (j6={j6:.3f}); " | ||
| "rebooting servos to clear a trip, then retrying") | ||
| self._dbg("gripper_reboot", j6=_r(j6)) | ||
| self._recover_arm() | ||
| gripper_open() | ||
| """Open the claw, verified (trip recovery) — see armlib.open_checked.""" | ||
| armlib.open_checked( | ||
| self.manipulation, self._gripper_j6, logger=self.logger, | ||
| on_reboot=lambda j6: self._dbg("gripper_reboot", j6=_r(j6)), | ||
| ) |
There was a problem hiding this comment.
_open_gripper_checked silently discards open_checked return value
armlib.open_checked(...) is called without capturing its return value. If the gripper fails to open even after the servo reboot and retry, open_checked returns False but _open_gripper_checked proceeds as if success — the skill continues into _push_to_floor with a closed claw. The parallel call in gripper_open.py (line 48) correctly checks if not ok: return FAILURE, but this in-context call does not. Previously, routing through innate.skills.gripper_open() could raise SkillFailed on a hardware motion failure; now that path is gone and the failure is swallowed entirely.
| def _open_gripper_checked(self): | |
| """Open the claw, and make sure it actually opened. A prior hard | |
| close can overcurrent-trip the gripper servo — a hardware fault | |
| torque_on alone can't clear — after which open silently no-ops and | |
| the hand stays shut straight into the grasp. Reboot clears the trip.""" | |
| gripper_open() | |
| j6 = self._gripper_j6() | |
| if j6 is not None and j6 < 0.10: | |
| self.logger.warning( | |
| f"[PickAnyObject] gripper did not open (j6={j6:.3f}); " | |
| "rebooting servos to clear a trip, then retrying") | |
| self._dbg("gripper_reboot", j6=_r(j6)) | |
| self._recover_arm() | |
| gripper_open() | |
| """Open the claw, verified (trip recovery) — see armlib.open_checked.""" | |
| armlib.open_checked( | |
| self.manipulation, self._gripper_j6, logger=self.logger, | |
| on_reboot=lambda j6: self._dbg("gripper_reboot", j6=_r(j6)), | |
| ) | |
| def _open_gripper_checked(self): | |
| """Open the claw, verified (trip recovery) — see armlib.open_checked.""" | |
| ok = armlib.open_checked( | |
| self.manipulation, self._gripper_j6, logger=self.logger, | |
| on_reboot=lambda j6: self._dbg("gripper_reboot", j6=_r(j6)), | |
| ) | |
| if not ok: | |
| raise armlib.ArmUnhealthy("gripper failed to open after trip recovery") |
| def open_checked(manipulation, get_j6, percent=100.0, duration=1.0, | ||
| logger=None, on_reboot=None): | ||
| """Open the gripper and VERIFY it opened. A prior hard close can | ||
| overcurrent-TRIP the gripper servo — a hardware error torque_on alone | ||
| can't clear — after which open silently does nothing and the hand stays | ||
| shut. If j6 says it didn't open, reboot to clear the trip and retry. | ||
| on_reboot(j6) is called before the reboot (telemetry hook).""" | ||
| manipulation.torque_on() # a torque-disabled servo won't move at all | ||
| ok = manipulation.open_gripper(percent=percent, duration=duration, blocking=True) | ||
| j6 = get_j6() | ||
| if j6 is not None and j6 < 0.10: | ||
| if logger: | ||
| logger.warning(f"[arm] gripper did not open (j6={j6:.3f}); " | ||
| "rebooting servos to clear a trip, then retrying") | ||
| if on_reboot: | ||
| on_reboot(j6) | ||
| recover(manipulation, logger) | ||
| ok = manipulation.open_gripper(percent=percent, duration=duration, blocking=True) | ||
| return bool(ok) |
There was a problem hiding this comment.
open_checked does not re-verify j6 after recovery + retry
After rebooting the servos and calling open_gripper a second time, the function returns bool(ok) without reading get_j6() again. If the servo is still tripped, ok may be True (command accepted) while the gripper physically stays closed, and open_checked returns True to its caller. Adding a second j6 check after the retry would close this gap — since gripper_open.py now uses this path too, the silent false-success affects the standalone skill as well as pick_any_object.
1 participant
Stacked on #542 (base:
theo/pick-wrist-servo).What
Introduces
workspace/skill_lib/— plain Python modules skills import directly. NoSkillclasses, no roster entries, no invoker round-trip; functions take the interfaces they need as explicit arguments. The idea: short blocking device commands, recovery sequences, and pure math are library functions; anything the agent/operator invokes by name or that runs long enough to need cancellation stays aSkill.The hardware lessons this robot taught us now live in exactly one place:
skill_lib/arm.py— verified gripper open (overcurrent-trip → reboot recovery), close with the strength-cap lesson, health-checked cartesian moves (FK verify → recover + retry → raiseArmUnhealthy), servo recovery, reach clamps.skill_lib/geometry.py— head-camera ↔ floor pinhole projection, moved verbatim frompick_any_object(pure math; the webapp panel mirrors it).Effect on existing skills
gripper_open/gripper_closebecome thin wrappers over the lib — andgripper_opengains the trip-recovery verification that previously onlypick_any_objecthad.pick_any_objectdrops ~150 lines of private helpers (_move_arm,_recover_arm, the geometry functions, inline reach clamps) for lib calls, and calls the lib directly instead of round-tripping the gripper skills through the invoker.Import contract
Skills import the lib at module top: the loader puts the repo root on
sys.pathonly while a skill module executes (namespace-package import — verified live).catalog.reload_all/reload_selectivenow evict cachedworkspace.skill_libmodules before reloading, so a normal skills reload picks up lib edits too (otherwisesys.moduleskeeps serving the pre-edit lib).Compatibility
No behavior or interface changes: skill IDs,
innate.skillschaining, panel debug events, andTUNABLEkeys are all unchanged.Testing
Verified live on the robot:
/brain/reload_primitivesloads all 23 code skills (same count/IDs).pick_any_objectacks its tuning params through the new lib imports.gripper_close @strength=0.1thengripper_openran end-to-end via the skill CLI — claw physically cycled and returned to its starting joint value (j6 = 0.463).Note for reviewers
The
catalog.pychange is inbrain_client(ROS package), so it takes effect on the next colcon build + skills-server restart. Everything underworkspace/is live-reloadable and already running onmars.