cristhian
eccd9e3c24
Brings across the robot layer from monolitic-littlehand-python, which was written against this same controller. The point of it is that a command either moved the arm or raised: the SDK returns a code for a motion the controller quietly refused, wait_move calls that refusal finished, and arm.error_code is the report stream's stale copy. So errors are read live, arrival is confirmed rather than assumed, and a move whose straight-line path leaves the joint envelope is refused before it is commanded. Failures are typed, because the distinctions decide the response: an unreachable pose is not an unavailable arm, and a joint past its limit needs a motion nobody should start without warning the room. read_report is the deliberate exception to all of it: a report-cache read that costs no round trip and cannot block, which is the only read that works while a motion is in flight.
100 lines
3.2 KiB
Python
100 lines
3.2 KiB
Python
"""End effectors: what the arm carries at its flange, and what that lets it do.
|
|
|
|
"Grasp" is the same symbolic command whatever is bolted on; what differs is how
|
|
it is realized (suction on, fingers closed). That difference lives here, so the
|
|
event loop never names a tool.
|
|
|
|
Each effector declares the driver method that switches it and how far past the
|
|
flange its tip sits, which is what `tcp_pose` is offset by. Adding a tool is a
|
|
method on the driver and a class here.
|
|
"""
|
|
|
|
from abc import ABC
|
|
from typing import ClassVar
|
|
|
|
from dora_ulite6.errors import EffectorUnsupportedError
|
|
|
|
|
|
class EndEffector(ABC):
|
|
"""Base class for whatever is mounted at the flange."""
|
|
|
|
# The driver method that switches this tool on and off; None for a bare
|
|
# flange, which asks nothing of the arm.
|
|
drives: ClassVar[str | None]
|
|
# Distance from the flange to the tool tip along the flange Z axis, in mm.
|
|
offset_mm: ClassVar[float]
|
|
|
|
def attach(self, robot) -> None:
|
|
"""Take hold."""
|
|
self._switch(robot, True)
|
|
|
|
def detach(self, robot) -> None:
|
|
"""Let go."""
|
|
self._switch(robot, False)
|
|
|
|
def _switch(self, robot, engaged: bool) -> None:
|
|
"""Call the driver method this effector drives.
|
|
|
|
Using the declared name rather than a literal is what keeps ``drives``
|
|
and the call from drifting apart.
|
|
"""
|
|
getattr(robot, self.drives)(engaged)
|
|
|
|
def verify(self, robot) -> None:
|
|
"""Raise unless this arm has the method this effector switches.
|
|
|
|
Checked when the arm is built rather than at first use: a configured
|
|
effector the driver has no method for is a setup mistake, and it should
|
|
stop the node joining the dataflow rather than surface mid-motion.
|
|
"""
|
|
if self.drives and not hasattr(robot, self.drives):
|
|
raise EffectorUnsupportedError(
|
|
f"{type(self).__name__} needs a driver with {self.drives}(); "
|
|
f"{type(robot).__name__} has none"
|
|
)
|
|
|
|
|
|
class VacuumEffector(EndEffector):
|
|
"""A suction cup: it comes down onto an object's top face and holds it by vacuum."""
|
|
|
|
drives = "set_vacuum"
|
|
offset_mm = 61.0
|
|
|
|
|
|
class GripperEffector(EndEffector):
|
|
"""The Lite6 two-finger gripper."""
|
|
|
|
drives = "set_gripper"
|
|
offset_mm = 65.0
|
|
|
|
|
|
class NoEffector(EndEffector):
|
|
"""A bare flange: the arm can reach and point, but cannot hold anything."""
|
|
|
|
drives = None
|
|
offset_mm = 0.0
|
|
|
|
def attach(self, robot) -> None:
|
|
"""Refuse: there is nothing mounted to grasp with."""
|
|
raise EffectorUnsupportedError("this arm has no effector to grasp with")
|
|
|
|
def detach(self, robot) -> None:
|
|
"""Refuse: there is nothing mounted to release with."""
|
|
raise EffectorUnsupportedError("this arm has no effector to release with")
|
|
|
|
|
|
EFFECTORS: dict[str, type[EndEffector]] = {
|
|
"vacuum": VacuumEffector,
|
|
"gripper": GripperEffector,
|
|
"none": NoEffector,
|
|
}
|
|
|
|
|
|
def get_effector(name: str) -> EndEffector:
|
|
"""Return the effector registered under ``name``, or raise listing which exist."""
|
|
try:
|
|
return EFFECTORS[name]()
|
|
except KeyError:
|
|
available = ", ".join(sorted(EFFECTORS))
|
|
raise ValueError(f"unknown effector {name!r}; available: {available}") from None
|