add xArm driver layer ported from littlehand
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.
This commit is contained in:
@@ -0,0 +1,99 @@
|
||||
"""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
|
||||
Reference in New Issue
Block a user