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/__init__.py +99 -0
- urkit/__main__.py +116 -0
- urkit/cli/__init__.py +6 -0
- urkit/cli/colors.py +99 -0
- urkit/cli/connection_monitor.py +154 -0
- urkit/cli/points.py +293 -0
- urkit/cli/teach.py +1181 -0
- urkit/config.py +78 -0
- urkit/connection.py +580 -0
- urkit/exceptions.py +51 -0
- urkit/geometry.py +413 -0
- urkit/gripper/__init__.py +36 -0
- urkit/gripper/base.py +116 -0
- urkit/gripper/digital.py +140 -0
- urkit/gripper/presets.py +113 -0
- urkit/gripper/robotiq.py +322 -0
- urkit/gripper/robotiq_preamble.py +1356 -0
- urkit/io.py +311 -0
- urkit/motion.py +504 -0
- urkit/points.py +277 -0
- urkit/robot.py +1510 -0
- urkit/telemetry.py +177 -0
- urkit-0.1.0.dist-info/METADATA +612 -0
- urkit-0.1.0.dist-info/RECORD +27 -0
- urkit-0.1.0.dist-info/WHEEL +5 -0
- urkit-0.1.0.dist-info/entry_points.txt +2 -0
- urkit-0.1.0.dist-info/top_level.txt +1 -0
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
|
+
)
|
urkit/gripper/digital.py
ADDED
|
@@ -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
|
+
)
|