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,50 @@
|
||||
"""The pose that crosses the driver boundary, and the tool-tip offset applied to it."""
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation
|
||||
|
||||
|
||||
@dataclass
|
||||
class RobotPose:
|
||||
"""A robot pose: position in mm, orientation in degrees, both in the base frame.
|
||||
|
||||
The controller reports and accepts the flange pose. ``tcp_pose`` on the
|
||||
dataflow is this pose pushed out to the mounted tool's tip by
|
||||
:func:`with_tool_offset`.
|
||||
"""
|
||||
|
||||
x: float
|
||||
y: float
|
||||
z: float
|
||||
roll: float = 180.0
|
||||
pitch: float = 0.0
|
||||
yaw: float = 0.0
|
||||
|
||||
@classmethod
|
||||
def from_sequence(cls, values) -> "RobotPose":
|
||||
"""Build a pose from ``[x, y, z, roll, pitch, yaw]``; a position alone is enough."""
|
||||
return cls(**dict(zip(("x", "y", "z", "roll", "pitch", "yaw"), values, strict=False)))
|
||||
|
||||
def as_list(self) -> list[float]:
|
||||
"""Return the pose as ``[x, y, z, roll, pitch, yaw]``, the order the SDK uses."""
|
||||
return [self.x, self.y, self.z, self.roll, self.pitch, self.yaw]
|
||||
|
||||
|
||||
def rotation_matrix(pose: RobotPose) -> np.ndarray:
|
||||
"""Return the 3x3 base-frame rotation of ``pose``."""
|
||||
return Rotation.from_euler("xyz", [pose.roll, pose.pitch, pose.yaw], degrees=True).as_matrix()
|
||||
|
||||
|
||||
def with_tool_offset(pose: RobotPose, offset_mm: float) -> RobotPose:
|
||||
"""Return ``pose`` moved ``offset_mm`` along its own Z axis.
|
||||
|
||||
The offset is along the *tool* Z, not the base Z, so a tilted flange puts the
|
||||
tip where the tool actually reaches rather than straight down from it.
|
||||
Orientation is unchanged: sliding along the axis does not turn anything.
|
||||
"""
|
||||
if offset_mm == 0.0:
|
||||
return pose
|
||||
tip = np.array([pose.x, pose.y, pose.z]) + rotation_matrix(pose) @ np.array([0.0, 0.0, offset_mm])
|
||||
return RobotPose(tip[0], tip[1], tip[2], pose.roll, pose.pitch, pose.yaw)
|
||||
Reference in New Issue
Block a user