urkit 0.1.0__py3-none-any.whl

This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
urkit/geometry.py ADDED
@@ -0,0 +1,413 @@
1
+ """Quaternion and rotation vector geometry helpers.
2
+
3
+ Conversion utilities for UR rotation vectors, quaternions, and
4
+ Roll-Pitch-Yaw angles. Uses the rxyz convention (same as UR
5
+ teach pendant and tf2). Also provides quaternion-based pose
6
+ transformation for frame-aware delta movements.
7
+ """
8
+
9
+ from __future__ import annotations
10
+
11
+ import math
12
+ from enum import IntEnum
13
+
14
+
15
+ def rotvec_to_quat(rv: list[float]) -> tuple[float, float, float, float]:
16
+ """Convert a rotation vector to a quaternion (x, y, z, w).
17
+
18
+ Args:
19
+ rv: Rotation vector [rx, ry, rz].
20
+
21
+ Returns:
22
+ Quaternion as (x, y, z, w).
23
+ """
24
+ ax, ay, az = rv
25
+ angle = math.sqrt(ax * ax + ay * ay + az * az)
26
+ if angle < 1e-10:
27
+ return (0.0, 0.0, 0.0, 1.0)
28
+ half_angle = angle / 2
29
+ s = math.sin(half_angle) / angle
30
+ return (s * ax, s * ay, s * az, math.cos(half_angle))
31
+
32
+
33
+ def quat_to_rotvec(q: tuple[float, float, float, float]) -> list[float]:
34
+ """Convert a quaternion (x, y, z, w) to a rotation vector.
35
+
36
+ Args:
37
+ q: Quaternion (x, y, z, w).
38
+
39
+ Returns:
40
+ Rotation vector [rx, ry, rz].
41
+ """
42
+ x, y, z, w = q
43
+ norm_xyz = math.sqrt(x * x + y * y + z * z)
44
+ angle = 2 * math.atan2(norm_xyz, w)
45
+ if norm_xyz < 1e-10:
46
+ return [0.0, 0.0, 0.0]
47
+ return [x / norm_xyz * angle, y / norm_xyz * angle, z / norm_xyz * angle]
48
+
49
+
50
+ def quat_to_rpy(q: tuple[float, float, float, float]) -> tuple[float, float, float]:
51
+ """Extract Roll-Pitch-Yaw from a quaternion (rxyz convention).
52
+
53
+ Args:
54
+ q: Quaternion (x, y, z, w).
55
+
56
+ Returns:
57
+ (roll, pitch, yaw) in radians.
58
+ """
59
+ x, y, z, w = q
60
+ roll = math.atan2(2 * (w * x + y * z), 1 - 2 * (x * x + y * y))
61
+ pitch = math.asin(min(1, max(-1, 2 * (w * y - z * x))))
62
+ yaw = math.atan2(2 * (w * z + x * y), 1 - 2 * (y * y + z * z))
63
+ return roll, pitch, yaw
64
+
65
+
66
+ def rpy_to_quat(
67
+ r: float, p: float, y: float
68
+ ) -> tuple[float, float, float, float]:
69
+ """Create a quaternion (x, y, z, w) from RPY angles (rxyz convention).
70
+
71
+ Args:
72
+ r: Roll angle in radians.
73
+ p: Pitch angle in radians.
74
+ y: Yaw angle in radians.
75
+
76
+ Returns:
77
+ Quaternion (x, y, z, w).
78
+ """
79
+ cr, sr = math.cos(r / 2), math.sin(r / 2)
80
+ cp, sp = math.cos(p / 2), math.sin(p / 2)
81
+ cy, sy = math.cos(y / 2), math.sin(y / 2)
82
+ return (
83
+ sr * cp * cy - cr * sp * sy, # x
84
+ cr * sp * cy + sr * cp * sy, # y
85
+ cr * cp * sy - sr * sp * cy, # z
86
+ cr * cp * cy + sr * sp * sy, # w
87
+ )
88
+
89
+
90
+ def orient_tcp_down(pose: list[float]) -> list[float]:
91
+ """Orient TCP downward while preserving tool heading.
92
+
93
+ Points the tool Z-axis straight down (base frame -Z) while
94
+ preserving the tool's heading (X-axis direction projected to
95
+ the XY plane). This avoids the ambiguity of RPY yaw at roll=π
96
+ and produces a smooth, predictable orientation change.
97
+
98
+ Matches the approach used in ur_bag_picking's orient_face_down.
99
+
100
+ Args:
101
+ pose: [x, y, z, rx, ry, rz] rotation vector pose.
102
+
103
+ Returns:
104
+ New pose with same position but TCP pointing downward.
105
+ """
106
+ pos = pose[:3]
107
+ rv = pose[3:]
108
+
109
+ # Current rotation matrix (columns are X, Y, Z axes)
110
+ R = _rotvec_to_matrix(rv)
111
+
112
+ # Target: Z points down
113
+ z_new = [0.0, 0.0, -1.0]
114
+
115
+ # Preserve heading: project current X axis to XY plane
116
+ x_new = [R[0][0], R[1][0], 0.0]
117
+ norm_x = math.sqrt(x_new[0] ** 2 + x_new[1] ** 2)
118
+
119
+ if norm_x < 1e-6:
120
+ # X is nearly vertical — use Y projection instead
121
+ x_new = [0.0, 0.0, 0.0]
122
+ y_new = [R[0][1], R[1][1], 0.0]
123
+ norm_y = math.sqrt(y_new[0] ** 2 + y_new[1] ** 2)
124
+
125
+ if norm_y < 1e-6:
126
+ # Both vertical — fallback to roll=π
127
+ return [pos[0], pos[1], pos[2], math.pi, 0.0, 0.0]
128
+
129
+ y_new[0] /= norm_y
130
+ y_new[1] /= norm_y
131
+ # x = y cross z (right-hand rule: y × -z)
132
+ x_new = [
133
+ y_new[1] * z_new[2] - y_new[2] * z_new[1],
134
+ y_new[2] * z_new[0] - y_new[0] * z_new[2],
135
+ y_new[0] * z_new[1] - y_new[1] * z_new[0],
136
+ ]
137
+ else:
138
+ x_new[0] /= norm_x
139
+ x_new[1] /= norm_x
140
+ # y = z cross x
141
+ y_new = [
142
+ z_new[1] * x_new[2] - z_new[2] * x_new[1],
143
+ z_new[2] * x_new[0] - z_new[0] * x_new[2],
144
+ z_new[0] * x_new[1] - z_new[1] * x_new[0],
145
+ ]
146
+
147
+ # Build rotation matrix from columns [x_new, y_new, z_new]
148
+ R_new = [
149
+ [x_new[0], y_new[0], z_new[0]],
150
+ [x_new[1], y_new[1], z_new[1]],
151
+ [x_new[2], y_new[2], z_new[2]],
152
+ ]
153
+
154
+ rv_target = _matrix_to_rotvec(R_new)
155
+
156
+ return [pos[0], pos[1], pos[2], rv_target[0], rv_target[1], rv_target[2]]
157
+
158
+
159
+ # ------------------------------------------------------------------
160
+ # Quaternion algebra helpers
161
+ # ------------------------------------------------------------------
162
+
163
+
164
+ def _quat_multiply(
165
+ a: tuple[float, float, float, float],
166
+ b: tuple[float, float, float, float],
167
+ ) -> tuple[float, float, float, float]:
168
+ """Multiply two quaternions (Hamilton product).
169
+
170
+ Returns a * b, where both are (x, y, z, w).
171
+ """
172
+ ax, ay, az, aw = a
173
+ bx, by, bz, bw = b
174
+ return (
175
+ aw * bx + ax * bw + ay * bz - az * by,
176
+ aw * by - ax * bz + ay * bw + az * bx,
177
+ aw * bz + ax * by - ay * bx + az * bw,
178
+ aw * bw - ax * bx - ay * by - az * bz,
179
+ )
180
+
181
+
182
+ def _quat_conjugate(
183
+ q: tuple[float, float, float, float],
184
+ ) -> tuple[float, float, float, float]:
185
+ """Return the conjugate of a quaternion (x, y, z, w) -> (-x, -y, -z, w)."""
186
+ x, y, z, w = q
187
+ return (-x, -y, -z, w)
188
+
189
+
190
+ def _quat_rotate_vector(
191
+ q: tuple[float, float, float, float],
192
+ v: list[float],
193
+ ) -> list[float]:
194
+ """Rotate a 3D vector by a quaternion.
195
+
196
+ Uses the q * v * q_conj formula, where v is treated as a pure
197
+ quaternion (0, vx, vy, vz).
198
+
199
+ Args:
200
+ q: Rotation quaternion (x, y, z, w).
201
+ v: 3D vector [vx, vy, vz].
202
+
203
+ Returns:
204
+ Rotated vector [vx', vy', vz'].
205
+ """
206
+ # Convert vector to pure quaternion
207
+ vq = (v[0], v[1], v[2], 0.0)
208
+ # q * v * q_conj
209
+ result = _quat_multiply(q, vq)
210
+ result = _quat_multiply(result, _quat_conjugate(q))
211
+ return [result[0], result[1], result[2]]
212
+
213
+
214
+ # ------------------------------------------------------------------
215
+ # Frame of reference for delta movements
216
+ # ------------------------------------------------------------------
217
+
218
+
219
+ class MoveFrame(IntEnum):
220
+ """Coordinate frame for relative (delta) movements.
221
+
222
+ Controls how delta vectors are interpreted during ``move_relative()`` and offsets.
223
+ - ``BASE`` — Delta is expressed in the robot's base frame.
224
+ Pressing +X always moves along the base X axis.
225
+ - ``TOOL`` — Delta is expressed in the TCP (tool) frame.
226
+ Pressing +X moves along the tool's local X axis, regardless
227
+ of how the robot is oriented.
228
+ """
229
+
230
+ BASE = 0
231
+ TOOL = 1
232
+
233
+
234
+ # ------------------------------------------------------------------
235
+ # Matrix helpers for pose composition (matches robot_mover /
236
+ # point_recording.cpp Eigen::Isometry3d approach)
237
+ # ------------------------------------------------------------------
238
+
239
+
240
+ def _rotvec_to_matrix(rv: list[float]) -> list[list[float]]:
241
+ """Convert a rotation vector to a 3x3 rotation matrix (Rodrigues formula).
242
+
243
+ Args:
244
+ rv: Rotation vector [rx, ry, rz].
245
+
246
+ Returns:
247
+ 3x3 rotation matrix as nested lists.
248
+ """
249
+ ax, ay, az = rv
250
+ angle = math.sqrt(ax * ax + ay * ay + az * az)
251
+
252
+ if angle < 1e-10:
253
+ return [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
254
+
255
+ s = math.sin(angle)
256
+ c = math.cos(angle)
257
+ oc = 1.0 - c
258
+ sx, sy, sz = ax / angle, ay / angle, az / angle
259
+
260
+ return [
261
+ [
262
+ c + sx * sx * oc,
263
+ sx * sy * oc - sz * s,
264
+ sx * sz * oc + sy * s,
265
+ ],
266
+ [
267
+ sy * sx * oc + sz * s,
268
+ c + sy * sy * oc,
269
+ sy * sz * oc - sx * s,
270
+ ],
271
+ [
272
+ sz * sx * oc - sy * s,
273
+ sz * sy * oc + sx * s,
274
+ c + sz * sz * oc,
275
+ ],
276
+ ]
277
+
278
+
279
+ def _matrix_to_rotvec(m: list[list[float]]) -> list[float]:
280
+ """Convert a 3x3 rotation matrix to a rotation vector.
281
+
282
+ Uses the closed-form arccos trace method. Handles identity,
283
+ 180°, and general cases.
284
+ """
285
+ # Trace: m[0][0] + m[1][1] + m[2][2]
286
+ tr = m[0][0] + m[1][1] + m[2][2]
287
+
288
+ if tr > 3 - 1e-10:
289
+ # Identity
290
+ return [0.0, 0.0, 0.0]
291
+
292
+ if tr < -1 + 1e-10:
293
+ # 180° rotation — extract axis from rotation matrix
294
+ # For angle=π: R[k][k] = 2*k_k^2 - 1 => k_k = sqrt((R[k][k]+1)/2)
295
+ # Off-diagonal: R[i][j] + R[j][i] = 4*k_i*k_j => k_i = sum / (4*k_j)
296
+ # rotvec = axis * π
297
+ if m[0][0] >= m[1][1] and m[0][0] >= m[2][2]:
298
+ k0 = math.sqrt((m[0][0] + 1) / 2)
299
+ k1 = (m[0][1] + m[1][0]) / (4 * k0) if k0 > 1e-15 else 0.0
300
+ k2 = (m[0][2] + m[2][0]) / (4 * k0) if k0 > 1e-15 else 0.0
301
+ elif m[1][1] >= m[0][0] and m[1][1] >= m[2][2]:
302
+ k1 = math.sqrt((m[1][1] + 1) / 2)
303
+ k0 = (m[0][1] + m[1][0]) / (4 * k1) if k1 > 1e-15 else 0.0
304
+ k2 = (m[1][2] + m[2][1]) / (4 * k1) if k1 > 1e-15 else 0.0
305
+ else:
306
+ k2 = math.sqrt((m[2][2] + 1) / 2)
307
+ k0 = (m[0][2] + m[2][0]) / (4 * k2) if k2 > 1e-15 else 0.0
308
+ k1 = (m[1][2] + m[2][1]) / (4 * k2) if k2 > 1e-15 else 0.0
309
+ return [k0 * math.pi, k1 * math.pi, k2 * math.pi]
310
+
311
+ # General case
312
+ angle = math.acos(max(-1, min(1, (tr - 1) / 2)))
313
+ if angle < 1e-10:
314
+ return [0.0, 0.0, 0.0]
315
+ s = math.sin(angle)
316
+ return [
317
+ (m[2][1] - m[1][2]) / (2 * s) * angle,
318
+ (m[0][2] - m[2][0]) / (2 * s) * angle,
319
+ (m[1][0] - m[0][1]) / (2 * s) * angle,
320
+ ]
321
+
322
+
323
+ def _mat_vec_mul(m: list[list[float]], v: list[float]) -> list[float]:
324
+ """Multiply a 3x3 matrix by a 3-element vector."""
325
+ return [
326
+ m[0][0] * v[0] + m[0][1] * v[1] + m[0][2] * v[2],
327
+ m[1][0] * v[0] + m[1][1] * v[1] + m[1][2] * v[2],
328
+ m[2][0] * v[0] + m[2][1] * v[1] + m[2][2] * v[2],
329
+ ]
330
+
331
+
332
+ def _mat_mul(a: list[list[float]], b: list[list[float]]) -> list[list[float]]:
333
+ """Multiply two 3x3 matrices."""
334
+ return [
335
+ [
336
+ a[0][0]*b[0][0] + a[0][1]*b[1][0] + a[0][2]*b[2][0],
337
+ a[0][0]*b[0][1] + a[0][1]*b[1][1] + a[0][2]*b[2][1],
338
+ a[0][0]*b[0][2] + a[0][1]*b[1][2] + a[0][2]*b[2][2],
339
+ ],
340
+ [
341
+ a[1][0]*b[0][0] + a[1][1]*b[1][0] + a[1][2]*b[2][0],
342
+ a[1][0]*b[0][1] + a[1][1]*b[1][1] + a[1][2]*b[2][1],
343
+ a[1][0]*b[0][2] + a[1][1]*b[1][2] + a[1][2]*b[2][2],
344
+ ],
345
+ [
346
+ a[2][0]*b[0][0] + a[2][1]*b[1][0] + a[2][2]*b[2][0],
347
+ a[2][0]*b[0][1] + a[2][1]*b[1][1] + a[2][2]*b[2][1],
348
+ a[2][0]*b[0][2] + a[2][1]*b[1][2] + a[2][2]*b[2][2],
349
+ ],
350
+ ]
351
+
352
+
353
+ # ------------------------------------------------------------------
354
+ # Pose transformation
355
+ # ------------------------------------------------------------------
356
+
357
+
358
+ def transform_pose_delta(
359
+ pose: list[float],
360
+ delta: list[float],
361
+ frame: MoveFrame = MoveFrame.BASE,
362
+ ) -> list[float]:
363
+ """Compute a target pose by applying a delta to the current pose.
364
+
365
+ Uses full matrix composition (matching robot_mover's
366
+ Eigen::Isometry3d and point_recording.cpp) — the rotation is
367
+ composed as a 3x3 matrix, then converted back to rotvec. This
368
+ avoids rotvec wrapping when the cumulative rotation exceeds π.
369
+
370
+ Args:
371
+ pose: Current TCP pose [x, y, z, rx, ry, rz].
372
+ delta: Delta [dx, dy, dz, droll, dpitch, dyaw].
373
+ frame: Coordinate frame for interpreting the delta.
374
+
375
+ Returns:
376
+ Target pose [x, y, z, rx, ry, rz].
377
+ """
378
+ R = _rotvec_to_matrix(pose[3:])
379
+
380
+ # Angular delta as a rotation matrix (axis-angle → Rodrigues)
381
+ dr = math.sqrt(delta[3] ** 2 + delta[4] ** 2 + delta[5] ** 2)
382
+ if dr < 1e-10:
383
+ R_delta = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
384
+ else:
385
+ ax = delta[3] / dr
386
+ ay = delta[4] / dr
387
+ az = delta[5] / dr
388
+ s = math.sin(dr)
389
+ c = math.cos(dr)
390
+ oc = 1.0 - c
391
+ R_delta = [
392
+ [c + ax*ax*oc, ax*ay*oc - az*s, ax*az*oc + ay*s],
393
+ [ay*ax*oc + az*s, c + ay*ay*oc, ay*az*oc - ax*s],
394
+ [az*ax*oc - ay*s, az*ay*oc + ax*s, c + az*az*oc],
395
+ ]
396
+
397
+ if frame == MoveFrame.BASE:
398
+ # Base frame: add linear delta directly, pre-multiply rotation
399
+ new_pos = [pose[0] + delta[0], pose[1] + delta[1], pose[2] + delta[2]]
400
+ R_new = _mat_mul(R_delta, R)
401
+ else:
402
+ # TOOL frame: rotate linear delta by current orientation,
403
+ # post-multiply rotation (matches robot_mover Eigen approach)
404
+ rotated_linear = _mat_vec_mul(R, delta[:3])
405
+ new_pos = [
406
+ pose[0] + rotated_linear[0],
407
+ pose[1] + rotated_linear[1],
408
+ pose[2] + rotated_linear[2],
409
+ ]
410
+ R_new = _mat_mul(R, R_delta)
411
+
412
+ new_rot = _matrix_to_rotvec(R_new)
413
+ return [new_pos[0], new_pos[1], new_pos[2], new_rot[0], new_rot[1], new_rot[2]]
@@ -0,0 +1,36 @@
1
+ """Gripper plugin package.
2
+
3
+ Exports the Gripper base class, factory, and configuration presets.
4
+
5
+ Usage::
6
+
7
+ from urkit import URRobot, ROBOTIQ_2F_85, DigitalGripperConfig
8
+
9
+ # Robotiq preset — one arg does everything
10
+ robot = URRobot(ip="192.168.1.100", gripper=ROBOTIQ_2F_85)
11
+
12
+ # Digital I/O gripper
13
+ robot = URRobot(ip="192.168.1.100", gripper=DigitalGripperConfig(pin=3))
14
+ """
15
+
16
+ from urkit.gripper.base import Gripper
17
+ from urkit.gripper.digital import DigitalGripper
18
+ from urkit.gripper.robotiq import RobotiqGripper
19
+ from urkit.gripper.presets import (
20
+ DigitalGripperConfig,
21
+ GripperPreset,
22
+ ROBOTIQ_2F_85,
23
+ ROBOTIQ_2F_140,
24
+ ROBOTIQ_HAND_E,
25
+ )
26
+
27
+ __all__ = [
28
+ "Gripper",
29
+ "RobotiqGripper",
30
+ "DigitalGripper",
31
+ "GripperPreset",
32
+ "DigitalGripperConfig",
33
+ "ROBOTIQ_2F_85",
34
+ "ROBOTIQ_2F_140",
35
+ "ROBOTIQ_HAND_E",
36
+ ]
urkit/gripper/base.py ADDED
@@ -0,0 +1,116 @@
1
+ """Abstract gripper plugin interface.
2
+
3
+ All gripper backends must implement this interface so that
4
+ ``robot.gripper.open()``, ``close()``, and ``set_position()`` work
5
+ uniformly regardless of the underlying hardware.
6
+ """
7
+
8
+ from __future__ import annotations
9
+
10
+ from abc import ABC, abstractmethod
11
+
12
+
13
+ class Gripper(ABC):
14
+ """Abstract base class for gripper backends.
15
+
16
+ Subclasses must implement ``open()``, ``close()``, ``set_position()``,
17
+ and ``is_connected()``. The ``activate()`` method is a no-op by
18
+ default and should be overridden by backends that require explicit
19
+ activation (e.g., Robotiq). The factory method ``Gripper.create()``
20
+ returns the appropriate backend.
21
+ """
22
+
23
+ @abstractmethod
24
+ def open(self) -> None:
25
+ """Open the gripper (release).
26
+
27
+ Raises:
28
+ GripperError: If the operation fails.
29
+ """
30
+
31
+ @abstractmethod
32
+ def close(self) -> None:
33
+ """Close the gripper (grip).
34
+
35
+ Raises:
36
+ GripperError: If the operation fails.
37
+ """
38
+
39
+ @abstractmethod
40
+ def set_position(self, position: int) -> None:
41
+ """Set the gripper to a specific position.
42
+
43
+ For Robotiq 2F grippers, position is 0-100 (0 = fully open,
44
+ 100 = fully closed). Digital grippers don't support this and
45
+ raise GripperError.
46
+
47
+ Args:
48
+ position: Gripper position.
49
+
50
+ Raises:
51
+ GripperError: If the operation fails.
52
+ """
53
+
54
+ def activate(self) -> None:
55
+ """Activate the gripper.
56
+
57
+ No-op by default. Override in subclasses that require explicit
58
+ activation (e.g., Robotiq grippers need a reset and calibration
59
+ sequence). DigitalGripper raises GripperError to signal that
60
+ activation is not needed.
61
+
62
+ Raises:
63
+ GripperError: If activation fails or is not applicable.
64
+ """
65
+
66
+ def deactivate(self) -> None:
67
+ """Deactivate the gripper.
68
+
69
+ No-op by default. Override in subclasses that support explicit
70
+ deactivation (e.g., Robotiq grippers). Call activate() again
71
+ to re-enable the gripper.
72
+ """
73
+
74
+ @abstractmethod
75
+ def is_connected(self) -> bool:
76
+ """Check if the gripper is connected and ready.
77
+
78
+ Returns:
79
+ True if the gripper is connected.
80
+ """
81
+
82
+ @classmethod
83
+ def create(cls, name: str, **kwargs) -> "Gripper":
84
+ """Factory method to create a gripper backend.
85
+
86
+ Args:
87
+ name: Backend name. Supported values:
88
+ - ``"robotiq"`` — Robotiq 2F via URScript (secondary interface)
89
+ - ``"digital"`` — Digital I/O gripper
90
+
91
+ Kwargs:
92
+ rtde_control: RTDEControlInterface (required for all backends).
93
+ pin: Digital output pin index (required for "digital" backend).
94
+ force: Gripper force 0–100 (Robotiq backend, default 100).
95
+ speed: Gripper speed 0–100 (Robotiq backend, default 100).
96
+
97
+ Returns:
98
+ A concrete Gripper instance.
99
+
100
+ Raises:
101
+ GripperError: If the backend name is unknown.
102
+ """
103
+ from urkit.gripper.robotiq import RobotiqGripper
104
+ from urkit.gripper.digital import DigitalGripper
105
+
106
+ if name == "robotiq":
107
+ return RobotiqGripper(**kwargs)
108
+ elif name == "digital":
109
+ return DigitalGripper(**kwargs)
110
+ else:
111
+ from urkit.exceptions import GripperError
112
+
113
+ raise GripperError(
114
+ f"Unknown gripper backend: '{name}'. "
115
+ f"Supported backends: robotiq, digital."
116
+ )
@@ -0,0 +1,140 @@
1
+ """Digital I/O gripper backend.
2
+
3
+ Uses RTDE IO interface to control a simple on/off gripper via
4
+ a digital output pin.
5
+ """
6
+
7
+ from __future__ import annotations
8
+
9
+ import logging
10
+ from typing import TYPE_CHECKING
11
+
12
+ from urkit.exceptions import GripperError
13
+ from urkit.gripper.base import Gripper
14
+
15
+ if TYPE_CHECKING:
16
+ from rtde_control import RTDEControlInterface
17
+ from rtde_receive import RTDEReceiveInterface
18
+
19
+ logger = logging.getLogger(__name__)
20
+
21
+
22
+ class DigitalGripper(Gripper):
23
+ """Simple digital I/O gripper backend.
24
+
25
+ Controls a gripper by setting/clearing a single digital output pin.
26
+ Supports both normally-open and normally-closed grippers via the
27
+ ``closed_when_high`` parameter.
28
+
29
+ Args:
30
+ rtde_control: RTDEControlInterface instance.
31
+ rtde_receive: RTDEReceiveInterface instance (for feedback).
32
+ pin: Digital output pin index (0–7 on e-Series).
33
+ closed_when_high: If True, HIGH signal closes the gripper
34
+ (e.g., suction cup). If False, LOW closes it
35
+ (e.g., normally-closed solenoid gripper).
36
+ Default True.
37
+
38
+ Example:
39
+ >>> # Suction cup: HIGH = grab, LOW = release
40
+ >>> gripper = DigitalGripper(rtde_c, rtde_r, pin=0)
41
+ >>> # Normally-closed solenoid: LOW = grab, HIGH = release
42
+ >>> gripper = DigitalGripper(rtde_c, rtde_r, pin=0, closed_when_high=False)
43
+ >>> gripper.open()
44
+ >>> gripper.close()
45
+ """
46
+
47
+ def __init__(
48
+ self,
49
+ rtde_control: "RTDEControlInterface",
50
+ rtde_receive: "RTDEReceiveInterface",
51
+ pin: int = 0,
52
+ closed_when_high: bool = True,
53
+ **kwargs, # robot_ip passed by factory but not used
54
+ ) -> None:
55
+ self._rtde_c = rtde_control
56
+ self._rtde_r = rtde_receive
57
+ self._pin = pin
58
+ self._closed_when_high = closed_when_high
59
+
60
+ if not 0 <= pin <= 7:
61
+ raise GripperError(
62
+ f"Digital gripper pin must be 0–7, got {pin}. "
63
+ "e-Series has 8 digital outputs (0–7)."
64
+ )
65
+
66
+ logger.info(
67
+ "Digital gripper initialized on pin %d (closed_when_high=%s)",
68
+ pin,
69
+ closed_when_high,
70
+ )
71
+
72
+ def activate(self) -> None:
73
+ """Activate the gripper.
74
+
75
+ Raises:
76
+ GripperError: Always — digital grippers don't require activation.
77
+ """
78
+ raise GripperError(
79
+ "Digital grippers do not require activation. "
80
+ "Remove the call to gripper.activate()."
81
+ )
82
+
83
+ def open(self) -> None:
84
+ """Open the gripper (release)."""
85
+ self._rtde_c.setStandardDigitalOut(self._pin, not self._closed_when_high)
86
+ logger.debug(
87
+ "Digital gripper opened (pin %d = %s)",
88
+ self._pin,
89
+ "OFF" if not self._closed_when_high else "ON",
90
+ )
91
+
92
+ def close(self) -> None:
93
+ """Close the gripper (grab)."""
94
+ self._rtde_c.setStandardDigitalOut(self._pin, self._closed_when_high)
95
+ logger.debug(
96
+ "Digital gripper closed (pin %d = %s)",
97
+ self._pin,
98
+ "ON" if self._closed_when_high else "OFF",
99
+ )
100
+
101
+ def set_position(self, position: int) -> None:
102
+ """Set the gripper position.
103
+
104
+ Raises:
105
+ GripperError: Always — digital grippers don't support position control.
106
+ """
107
+ raise GripperError(
108
+ "Digital grippers do not support set_position. "
109
+ "Use open() or close() instead."
110
+ )
111
+
112
+ def set_force(self, force: int) -> None:
113
+ """Set gripper force.
114
+
115
+ Raises:
116
+ GripperError: Always — digital grippers don't support force control.
117
+ """
118
+ raise GripperError(
119
+ "Digital grippers do not support set_force."
120
+ )
121
+
122
+ def set_speed(self, speed: int) -> None:
123
+ """Set gripper speed.
124
+
125
+ Raises:
126
+ GripperError: Always — digital grippers don't support speed control.
127
+ """
128
+ raise GripperError(
129
+ "Digital grippers do not support set_speed."
130
+ )
131
+
132
+ def is_connected(self) -> bool:
133
+ """Check if the gripper is connected.
134
+
135
+ Raises:
136
+ GripperError: Always — digital grippers can't verify connection.
137
+ """
138
+ raise GripperError(
139
+ "Digital grippers do not support connection checks."
140
+ )