roboticstoolbox-python 1.4.0__py3-none-any.whl → 1.4.2__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.
- roboticstoolbox/__init__.py +3 -0
- roboticstoolbox/backends/PyPlot/PyPlot.py +2 -1
- roboticstoolbox/backends/PyPlot/RobotPlot.py +9 -1
- roboticstoolbox/backends/PyPlot/RobotPlot2.py +6 -1
- roboticstoolbox/blocks/arm.py +15 -13
- roboticstoolbox/ets/ETS.py +90 -23
- roboticstoolbox/ets/ETS2.py +4 -3
- roboticstoolbox/ets/_ETS.py +72 -0
- roboticstoolbox/ets/cpp-extensions/ik.cpp +17 -0
- roboticstoolbox/mobile/DistanceTransformPlanner.py +3 -5
- roboticstoolbox/models/URDF/URDFRobot.py +25 -20
- roboticstoolbox/models/URDF/YuMi.py +2 -2
- roboticstoolbox/robot/BaseRobot.py +6 -1
- roboticstoolbox/robot/DHLink.py +1 -1
- roboticstoolbox/robot/DHRobot.py +15 -540
- roboticstoolbox/robot/Dynamics.py +12 -16
- roboticstoolbox/robot/IK.py +37 -14
- roboticstoolbox/robot/Robot.py +36 -35
- roboticstoolbox/robot/RobotKinematics.py +106 -52
- roboticstoolbox/robot/RobotPlottingMPL.py +4 -4
- roboticstoolbox/tools/__init__.py +4 -0
- roboticstoolbox/tools/trchain.py +233 -0
- {roboticstoolbox_python-1.4.0.dist-info → roboticstoolbox_python-1.4.2.dist-info}/METADATA +2 -1
- {roboticstoolbox_python-1.4.0.dist-info → roboticstoolbox_python-1.4.2.dist-info}/RECORD +27 -26
- {roboticstoolbox_python-1.4.0.dist-info → roboticstoolbox_python-1.4.2.dist-info}/WHEEL +0 -0
- {roboticstoolbox_python-1.4.0.dist-info → roboticstoolbox_python-1.4.2.dist-info}/entry_points.txt +0 -0
- {roboticstoolbox_python-1.4.0.dist-info → roboticstoolbox_python-1.4.2.dist-info}/licenses/LICENSE +0 -0
roboticstoolbox/__init__.py
CHANGED
|
@@ -512,7 +512,8 @@ class PyPlot(Connector):
|
|
|
512
512
|
|
|
513
513
|
# render the frame and save as a PIL image in the list
|
|
514
514
|
canvas = self.fig.canvas
|
|
515
|
-
|
|
515
|
+
image = _pil("RGBA", canvas.get_width_height(), bytes(canvas.buffer_rgba()))
|
|
516
|
+
return image.convert("RGB")
|
|
516
517
|
|
|
517
518
|
def _push_inline_frame(self):
|
|
518
519
|
# Push a snapshot into notebook output for inline animation.
|
|
@@ -74,7 +74,15 @@ class RobotPlot:
|
|
|
74
74
|
|
|
75
75
|
if options is not None:
|
|
76
76
|
for key, value in options.items():
|
|
77
|
-
|
|
77
|
+
# Most options (colors, linewidths) are per-artefact dicts to
|
|
78
|
+
# merge with the default; a few (jointaxislength, eelength)
|
|
79
|
+
# are plain scalars to override outright -- **-unpacking a
|
|
80
|
+
# float raises TypeError, so only merge when both sides are
|
|
81
|
+
# dicts.
|
|
82
|
+
if isinstance(defaults.get(key), dict) and isinstance(value, dict):
|
|
83
|
+
defaults[key] = {**defaults[key], **value}
|
|
84
|
+
else:
|
|
85
|
+
defaults[key] = value
|
|
78
86
|
self.options = defaults
|
|
79
87
|
|
|
80
88
|
def draw(self):
|
|
@@ -35,7 +35,12 @@ class RobotPlot2(RobotPlot):
|
|
|
35
35
|
|
|
36
36
|
if options is not None:
|
|
37
37
|
for key, value in options.items():
|
|
38
|
-
|
|
38
|
+
# See RobotPlot.__init__'s comment: eelength is a plain
|
|
39
|
+
# scalar to override outright, not a dict to merge.
|
|
40
|
+
if isinstance(defaults.get(key), dict) and isinstance(value, dict):
|
|
41
|
+
defaults[key] = {**defaults[key], **value}
|
|
42
|
+
else:
|
|
43
|
+
defaults[key] = value
|
|
39
44
|
self.options = defaults
|
|
40
45
|
|
|
41
46
|
def draw(self):
|
roboticstoolbox/blocks/arm.py
CHANGED
|
@@ -1528,30 +1528,32 @@ class FDyn_X(ContinuousBlock):
|
|
|
1528
1528
|
q0 = smb.getvector(q0, robot.n)
|
|
1529
1529
|
# append qd0, assumed to be zero
|
|
1530
1530
|
self._x0 = np.r_[q0, np.zeros((robot.n,))]
|
|
1531
|
-
|
|
1531
|
+
# Zero-acceleration placeholder for xdd until deriv() has run at
|
|
1532
|
+
# least once. output() must not compute this itself: bdsim calls a
|
|
1533
|
+
# continuous block's output() wherever the compute graph needs its
|
|
1534
|
+
# *state*-derived ports (q, qd, x, xd here), which can happen before
|
|
1535
|
+
# this block's own input is resolved -- notably here, where w
|
|
1536
|
+
# (this block's input) is itself downstream of xd (this block's
|
|
1537
|
+
# output) via the force-control feedback path in opspace.py. deriv()
|
|
1538
|
+
# doesn't have that problem: the integrator only calls it with a
|
|
1539
|
+
# genuinely resolved input. See petercorke/bdsim#81 for the general
|
|
1540
|
+
# case (stateful blocks whose output() needs a resolved input).
|
|
1541
|
+
self._qdd = np.zeros((robot.n,))
|
|
1532
1542
|
|
|
1533
1543
|
def output(self, t, inports, x):
|
|
1534
1544
|
n = self.robot.n
|
|
1535
1545
|
q = x[:n]
|
|
1536
1546
|
qd = x[n:]
|
|
1537
|
-
qdd = self._qdd # from last deriv
|
|
1547
|
+
qdd = self._qdd # cached from the last deriv() call, or the zero placeholder
|
|
1538
1548
|
|
|
1539
1549
|
T = self.robot.fkine(q)
|
|
1540
1550
|
x = smb.tr2x(T.A)
|
|
1541
1551
|
|
|
1542
1552
|
Ja = self.robot.jacob0_analytical(q, self.representation)
|
|
1543
1553
|
xd = Ja @ qd
|
|
1544
|
-
|
|
1545
|
-
|
|
1546
|
-
|
|
1547
|
-
# print(Ja)
|
|
1548
|
-
# print()
|
|
1549
|
-
|
|
1550
|
-
if qdd is None:
|
|
1551
|
-
xdd = None
|
|
1552
|
-
else:
|
|
1553
|
-
Ja_dot = self.robot.jacob0_dot(q, qd, J0=Ja)
|
|
1554
|
-
xdd = Ja @ qdd + Ja_dot @ qd
|
|
1554
|
+
|
|
1555
|
+
Ja_dot = self.robot.jacob0_dot(q, qd, J0=Ja)
|
|
1556
|
+
xdd = Ja @ qdd + Ja_dot @ qd
|
|
1555
1557
|
|
|
1556
1558
|
return [q, qd, x, xd, xdd]
|
|
1557
1559
|
|
roboticstoolbox/ets/ETS.py
CHANGED
|
@@ -23,7 +23,7 @@ from spatialmath.base import (
|
|
|
23
23
|
getmatrix,
|
|
24
24
|
)
|
|
25
25
|
from roboticstoolbox.tools.params import rtb_get_param
|
|
26
|
-
from roboticstoolbox.robot.IK import IK_GN, IK_LM, IK_NR, IK_QP
|
|
26
|
+
from roboticstoolbox.robot.IK import IK_GN, IK_LM, IK_NR, IK_QP, IKSolution
|
|
27
27
|
|
|
28
28
|
from roboticstoolbox.ets.fknm import (
|
|
29
29
|
ETS_init,
|
|
@@ -325,10 +325,17 @@ class ETS(BaseETS):
|
|
|
325
325
|
"""
|
|
326
326
|
Forward kinematics (returns raw ndarray)
|
|
327
327
|
|
|
328
|
-
:param q: Joint coordinates
|
|
328
|
+
:param q: Joint coordinates -- either *global* (length
|
|
329
|
+
``max(self.jindices) + 1``, addressed by each joint's own
|
|
330
|
+
``jindex``) or *compact* (length ``self.n``, positionally
|
|
331
|
+
ordered to match :meth:`joints`). These only differ when this
|
|
332
|
+
ETS is one branch of a larger, branched robot -- e.g. it's what
|
|
333
|
+
``ikine_LM``/``ik_LM`` return when solving for just this ETS.
|
|
334
|
+
For a whole, unbranched robot the two coincide.
|
|
329
335
|
:param base: a base transform applied before the ETS
|
|
330
336
|
:param tool: tool transform, optional
|
|
331
337
|
:param include_base: set to True if the base transform should be considered
|
|
338
|
+
:raises ValueError: if ``q``'s length matches neither interpretation
|
|
332
339
|
:returns: the transformation matrix representing the pose of the end-effector
|
|
333
340
|
:rtype: ndarray(4,4) or ndarray(m,4,4)
|
|
334
341
|
|
|
@@ -360,6 +367,7 @@ class ETS(BaseETS):
|
|
|
360
367
|
|
|
361
368
|
"""
|
|
362
369
|
|
|
370
|
+
q = self._resolve_q(q, allow_trajectory=True)
|
|
363
371
|
return ETS_fkine(self._fknm, q, base, tool, include_base, _data=self.data)
|
|
364
372
|
|
|
365
373
|
def jacob0(
|
|
@@ -407,6 +415,7 @@ class ETS(BaseETS):
|
|
|
407
415
|
|
|
408
416
|
"""
|
|
409
417
|
|
|
418
|
+
q = self._resolve_q(q)
|
|
410
419
|
return ETS_jacob0(self._fknm, q, tool, _data=self.data, _n=self.n)
|
|
411
420
|
|
|
412
421
|
def jacobe(
|
|
@@ -454,6 +463,7 @@ class ETS(BaseETS):
|
|
|
454
463
|
|
|
455
464
|
"""
|
|
456
465
|
|
|
466
|
+
q = self._resolve_q(q)
|
|
457
467
|
return ETS_jacobe(self._fknm, q, tool, _data=self.data, _n=self.n)
|
|
458
468
|
|
|
459
469
|
def hessian0(
|
|
@@ -523,6 +533,8 @@ class ETS(BaseETS):
|
|
|
523
533
|
|
|
524
534
|
"""
|
|
525
535
|
|
|
536
|
+
if q is not None:
|
|
537
|
+
q = self._resolve_q(q)
|
|
526
538
|
return ETS_hessian0(self._fknm, q, J0, tool, _data=self.data, _n=self.n)
|
|
527
539
|
|
|
528
540
|
def hessiane(
|
|
@@ -592,6 +604,8 @@ class ETS(BaseETS):
|
|
|
592
604
|
|
|
593
605
|
"""
|
|
594
606
|
|
|
607
|
+
if q is not None:
|
|
608
|
+
q = self._resolve_q(q)
|
|
595
609
|
return ETS_hessiane(self._fknm, q, Je, tool, _data=self.data, _n=self.n)
|
|
596
610
|
|
|
597
611
|
def jacob0_analytical(
|
|
@@ -1000,7 +1014,7 @@ class ETS(BaseETS):
|
|
|
1000
1014
|
joint_limits: bool = True,
|
|
1001
1015
|
k: float = 1.0,
|
|
1002
1016
|
method: L["chan", "wampler", "sugihara"] = "chan",
|
|
1003
|
-
) ->
|
|
1017
|
+
) -> IKSolution:
|
|
1004
1018
|
r"""
|
|
1005
1019
|
Fast Levenberg-Marquardt numerical inverse kinematics solver
|
|
1006
1020
|
|
|
@@ -1013,8 +1027,18 @@ class ETS(BaseETS):
|
|
|
1013
1027
|
:param joint_limits: reject solutions with joint limit violations
|
|
1014
1028
|
:param k: gain value for the damping matrix Wn
|
|
1015
1029
|
:param method: one of ``"chan"`` (default), ``"sugihara"`` or ``"wampler"``
|
|
1016
|
-
:returns:
|
|
1017
|
-
|
|
1030
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1031
|
+
``iterations``, ``searches`` and ``residual`` error value (``reason`` is
|
|
1032
|
+
always empty -- this fast C++ solver doesn't produce a granular failure
|
|
1033
|
+
reason string, unlike :meth:`ikine_LM`)
|
|
1034
|
+
:rtype: IKSolution
|
|
1035
|
+
|
|
1036
|
+
.. warning::
|
|
1037
|
+
|
|
1038
|
+
This method requires the compiled C++ extension. It raises
|
|
1039
|
+
``RuntimeError`` if that extension is unavailable, e.g. in a
|
|
1040
|
+
pure-Python build/wheel or under Pyodide/JupyterLite. Use
|
|
1041
|
+
:meth:`ikine_LM` instead in those environments.
|
|
1018
1042
|
|
|
1019
1043
|
A method which provides functionality to perform numerical inverse kinematics (IK)
|
|
1020
1044
|
using the Levenberg-Marquardt method. This is a fast solver implemented in C++.
|
|
@@ -1024,7 +1048,7 @@ class ETS(BaseETS):
|
|
|
1024
1048
|
|
|
1025
1049
|
The operation is defined by the choice of the ``method`` kwarg.
|
|
1026
1050
|
|
|
1027
|
-
The step is
|
|
1051
|
+
The step is defined as
|
|
1028
1052
|
|
|
1029
1053
|
.. math::
|
|
1030
1054
|
|
|
@@ -1112,16 +1136,23 @@ class ETS(BaseETS):
|
|
|
1112
1136
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1113
1137
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1114
1138
|
|
|
1115
|
-
.. seealso:: :meth:`ik_NR` :meth:`ik_GN`
|
|
1139
|
+
.. seealso:: :meth:`ik_NR` :meth:`ik_GN` :meth:`ikine_LM`
|
|
1116
1140
|
|
|
1117
1141
|
.. versionchanged:: 1.0.4
|
|
1118
1142
|
Merged the Levenberg-Marquardt IK solvers into the ik_LM method
|
|
1119
1143
|
|
|
1120
1144
|
"""
|
|
1121
1145
|
|
|
1122
|
-
|
|
1146
|
+
q, success, iterations, searches, residual = IK_LM_c(
|
|
1123
1147
|
self._fknm, Tep, q0, ilimit, slimit, tol, joint_limits, mask, k, method
|
|
1124
1148
|
)
|
|
1149
|
+
return IKSolution(
|
|
1150
|
+
q=q,
|
|
1151
|
+
success=bool(success),
|
|
1152
|
+
iterations=iterations,
|
|
1153
|
+
searches=searches,
|
|
1154
|
+
residual=residual,
|
|
1155
|
+
)
|
|
1125
1156
|
|
|
1126
1157
|
def ik_NR(
|
|
1127
1158
|
self,
|
|
@@ -1134,7 +1165,7 @@ class ETS(BaseETS):
|
|
|
1134
1165
|
joint_limits: bool = True,
|
|
1135
1166
|
pinv: int = True,
|
|
1136
1167
|
pinv_damping: float = 0.0,
|
|
1137
|
-
) ->
|
|
1168
|
+
) -> IKSolution:
|
|
1138
1169
|
r"""
|
|
1139
1170
|
Fast numerical inverse kinematics using Newton-Raphson optimisation
|
|
1140
1171
|
|
|
@@ -1147,8 +1178,18 @@ class ETS(BaseETS):
|
|
|
1147
1178
|
:param joint_limits: reject solutions with invalid joint configurations
|
|
1148
1179
|
:param pinv: use the pseudo-inverse instead of the normal matrix inverse
|
|
1149
1180
|
:param pinv_damping: damping factor for the pseudo-inverse
|
|
1150
|
-
:returns:
|
|
1151
|
-
|
|
1181
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1182
|
+
``iterations``, ``searches`` and ``residual`` error value (``reason`` is
|
|
1183
|
+
always empty -- this fast C++ solver doesn't produce a granular failure
|
|
1184
|
+
reason string, unlike :meth:`ikine_NR`)
|
|
1185
|
+
:rtype: IKSolution
|
|
1186
|
+
|
|
1187
|
+
.. warning::
|
|
1188
|
+
|
|
1189
|
+
This method requires the compiled C++ extension. It raises
|
|
1190
|
+
``RuntimeError`` if that extension is unavailable, e.g. in a
|
|
1191
|
+
pure-Python build/wheel or under Pyodide/JupyterLite. Use
|
|
1192
|
+
:meth:`ikine_NR` instead in those environments.
|
|
1152
1193
|
|
|
1153
1194
|
``sol = ets.ik_NR(Tep)`` are the joint coordinates (n) corresponding
|
|
1154
1195
|
to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object.
|
|
@@ -1195,11 +1236,11 @@ class ETS(BaseETS):
|
|
|
1195
1236
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1196
1237
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1197
1238
|
|
|
1198
|
-
.. seealso:: :meth:`ik_LM` :meth:`ik_GN`
|
|
1239
|
+
.. seealso:: :meth:`ik_LM` :meth:`ik_GN` :meth:`ikine_NR`
|
|
1199
1240
|
|
|
1200
1241
|
"""
|
|
1201
1242
|
|
|
1202
|
-
|
|
1243
|
+
q, success, iterations, searches, residual = IK_NR_c(
|
|
1203
1244
|
self._fknm,
|
|
1204
1245
|
Tep,
|
|
1205
1246
|
q0,
|
|
@@ -1211,6 +1252,13 @@ class ETS(BaseETS):
|
|
|
1211
1252
|
pinv,
|
|
1212
1253
|
pinv_damping,
|
|
1213
1254
|
)
|
|
1255
|
+
return IKSolution(
|
|
1256
|
+
q=q,
|
|
1257
|
+
success=bool(success),
|
|
1258
|
+
iterations=iterations,
|
|
1259
|
+
searches=searches,
|
|
1260
|
+
residual=residual,
|
|
1261
|
+
)
|
|
1214
1262
|
|
|
1215
1263
|
def ik_GN(
|
|
1216
1264
|
self,
|
|
@@ -1223,7 +1271,7 @@ class ETS(BaseETS):
|
|
|
1223
1271
|
joint_limits: bool = True,
|
|
1224
1272
|
pinv: int = True,
|
|
1225
1273
|
pinv_damping: float = 0.0,
|
|
1226
|
-
) ->
|
|
1274
|
+
) -> IKSolution:
|
|
1227
1275
|
r"""
|
|
1228
1276
|
Fast numerical inverse kinematics by Gauss-Newton optimisation
|
|
1229
1277
|
|
|
@@ -1299,11 +1347,11 @@ class ETS(BaseETS):
|
|
|
1299
1347
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1300
1348
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1301
1349
|
|
|
1302
|
-
.. seealso:: :meth:`ik_LM` :meth:`ik_NR`
|
|
1350
|
+
.. seealso:: :meth:`ik_LM` :meth:`ik_NR` :meth:`ikine_GN`
|
|
1303
1351
|
|
|
1304
1352
|
"""
|
|
1305
1353
|
|
|
1306
|
-
|
|
1354
|
+
q, success, iterations, searches, residual = IK_GN_c(
|
|
1307
1355
|
self._fknm,
|
|
1308
1356
|
Tep,
|
|
1309
1357
|
q0,
|
|
@@ -1315,6 +1363,13 @@ class ETS(BaseETS):
|
|
|
1315
1363
|
pinv,
|
|
1316
1364
|
pinv_damping,
|
|
1317
1365
|
)
|
|
1366
|
+
return IKSolution(
|
|
1367
|
+
q=q,
|
|
1368
|
+
success=bool(success),
|
|
1369
|
+
iterations=iterations,
|
|
1370
|
+
searches=searches,
|
|
1371
|
+
residual=residual,
|
|
1372
|
+
)
|
|
1318
1373
|
|
|
1319
1374
|
def ikine_LM(
|
|
1320
1375
|
self,
|
|
@@ -1351,7 +1406,10 @@ class ETS(BaseETS):
|
|
|
1351
1406
|
:param km: gain for manipulability maximisation (0.0 disables)
|
|
1352
1407
|
:param ps: minimum joint approach distance to limit (radians or metres)
|
|
1353
1408
|
:param pi: null-space influence distance (radians or metres)
|
|
1354
|
-
:returns:
|
|
1409
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1410
|
+
``iterations``, ``searches``, ``residual`` error value, and ``reason``
|
|
1411
|
+
string if applicable
|
|
1412
|
+
:rtype: IKSolution
|
|
1355
1413
|
|
|
1356
1414
|
A method which provides functionality to perform numerical inverse kinematics (IK)
|
|
1357
1415
|
using the Levenberg-Marquardt method.
|
|
@@ -1449,7 +1507,7 @@ class ETS(BaseETS):
|
|
|
1449
1507
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1450
1508
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1451
1509
|
|
|
1452
|
-
.. seealso:: :meth:`ikine_NR` :meth:`ikine_GN` :meth:`ikine_QP`
|
|
1510
|
+
.. seealso:: :meth:`ikine_NR` :meth:`ikine_GN` :meth:`ikine_QP` :meth:`ik_LM`
|
|
1453
1511
|
|
|
1454
1512
|
.. versionchanged:: 1.0.4
|
|
1455
1513
|
Added the Levenberg-Marquardt IK solver method on the `ETS` class
|
|
@@ -1510,7 +1568,10 @@ class ETS(BaseETS):
|
|
|
1510
1568
|
:param km: gain for manipulability maximisation (0.0 disables)
|
|
1511
1569
|
:param ps: minimum joint approach distance to limit (radians or metres)
|
|
1512
1570
|
:param pi: null-space influence distance (radians or metres)
|
|
1513
|
-
:returns:
|
|
1571
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1572
|
+
``iterations``, ``searches``, ``residual`` error value, and ``reason``
|
|
1573
|
+
string if applicable
|
|
1574
|
+
:rtype: IKSolution
|
|
1514
1575
|
|
|
1515
1576
|
A method which provides functionality to perform numerical inverse kinematics (IK)
|
|
1516
1577
|
using the Newton-Raphson method.
|
|
@@ -1554,7 +1615,7 @@ class ETS(BaseETS):
|
|
|
1554
1615
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1555
1616
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1556
1617
|
|
|
1557
|
-
.. seealso:: :meth:`ikine_LM` :meth:`ikine_GN` :meth:`ikine_QP`
|
|
1618
|
+
.. seealso:: :meth:`ikine_LM` :meth:`ikine_GN` :meth:`ikine_QP` :meth:`ik_NR`
|
|
1558
1619
|
|
|
1559
1620
|
.. versionchanged:: 1.0.4
|
|
1560
1621
|
Added the Newton-Raphson IK solver method on the `ETS` class
|
|
@@ -1614,7 +1675,10 @@ class ETS(BaseETS):
|
|
|
1614
1675
|
:param km: gain for manipulability maximisation (0.0 disables)
|
|
1615
1676
|
:param ps: minimum joint approach distance to limit (radians or metres)
|
|
1616
1677
|
:param pi: null-space influence distance (radians or metres)
|
|
1617
|
-
:returns:
|
|
1678
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1679
|
+
``iterations``, ``searches``, ``residual`` error value, and ``reason``
|
|
1680
|
+
string if applicable
|
|
1681
|
+
:rtype: IKSolution
|
|
1618
1682
|
|
|
1619
1683
|
A method which provides functionality to perform numerical inverse kinematics (IK)
|
|
1620
1684
|
using the Gauss-Newton method.
|
|
@@ -1673,7 +1737,7 @@ class ETS(BaseETS):
|
|
|
1673
1737
|
- J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II:
|
|
1674
1738
|
Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022).
|
|
1675
1739
|
|
|
1676
|
-
.. seealso:: :meth:`ikine_LM` :meth:`ikine_NR` :meth:`ikine_QP`
|
|
1740
|
+
.. seealso:: :meth:`ikine_LM` :meth:`ikine_NR` :meth:`ikine_QP` :meth:`ik_GN`
|
|
1677
1741
|
|
|
1678
1742
|
.. versionchanged:: 1.0.4
|
|
1679
1743
|
Added the Gauss-Newton IK solver method on the `ETS` class
|
|
@@ -1735,7 +1799,10 @@ class ETS(BaseETS):
|
|
|
1735
1799
|
:param km: gain for manipulability maximisation (0.0 disables)
|
|
1736
1800
|
:param ps: minimum joint approach distance to limit (radians or metres)
|
|
1737
1801
|
:param pi: null-space influence distance (radians or metres)
|
|
1738
|
-
:returns:
|
|
1802
|
+
:returns: an IKSolution containing joint coordinates ``q``, ``success`` flag,
|
|
1803
|
+
``iterations``, ``searches``, ``residual`` error value, and ``reason``
|
|
1804
|
+
string if applicable
|
|
1805
|
+
:rtype: IKSolution
|
|
1739
1806
|
:raises ImportError: if the package ``qpsolvers`` is not installed
|
|
1740
1807
|
|
|
1741
1808
|
A method that provides functionality to perform numerical inverse kinematics
|
roboticstoolbox/ets/ETS2.py
CHANGED
|
@@ -309,7 +309,7 @@ class ETS2(BaseETS):
|
|
|
309
309
|
- Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke
|
|
310
310
|
"""
|
|
311
311
|
|
|
312
|
-
q =
|
|
312
|
+
q = self._resolve_q(q, allow_trajectory=True)
|
|
313
313
|
l, _ = q.shape # type: ignore
|
|
314
314
|
end = self[-1]
|
|
315
315
|
|
|
@@ -375,8 +375,6 @@ class ETS2(BaseETS):
|
|
|
375
375
|
) -> NDArray:
|
|
376
376
|
# very inefficient implementation, just put a 1 in last row
|
|
377
377
|
# if its a rotation joint
|
|
378
|
-
q = getvector(q)
|
|
379
|
-
|
|
380
378
|
j = 0
|
|
381
379
|
J = np.zeros((3, self.n))
|
|
382
380
|
etjoints = self.joint_idx()
|
|
@@ -386,6 +384,9 @@ class ETS2(BaseETS):
|
|
|
386
384
|
for j in range(self.n):
|
|
387
385
|
i = etjoints[j]
|
|
388
386
|
self[i].jindex = j
|
|
387
|
+
self.__dict__.pop("jindices", None) # invalidate cached_property
|
|
388
|
+
|
|
389
|
+
q = self._resolve_q(q)
|
|
389
390
|
|
|
390
391
|
for j in range(self.n):
|
|
391
392
|
i = etjoints[j]
|
roboticstoolbox/ets/_ETS.py
CHANGED
|
@@ -399,6 +399,78 @@ class BaseETS(MutableSequence):
|
|
|
399
399
|
|
|
400
400
|
return np.array([j.jindex for j in self.joints()]) # type: ignore
|
|
401
401
|
|
|
402
|
+
def _resolve_q(self, q: ArrayLike, allow_trajectory: bool = False) -> NDArray:
|
|
403
|
+
"""
|
|
404
|
+
Resolve q to a full jindex-addressed vector
|
|
405
|
+
|
|
406
|
+
:param q: joint coordinates, either *compact* (length :attr:`n`,
|
|
407
|
+
positionally ordered to match this ETS's own :meth:`joints`) or
|
|
408
|
+
*global* (length ``max(jindices) + 1``, addressed by each
|
|
409
|
+
joint's global ``jindex`` -- e.g. the whole robot's ``q`` on a
|
|
410
|
+
branched robot, when this ETS is only one branch of it)
|
|
411
|
+
:param allow_trajectory: if True, ``q`` is normalised with
|
|
412
|
+
:func:`getmatrix` (preserving a genuine ``(m, n)`` trajectory's
|
|
413
|
+
row count, matching :meth:`eval`'s own convention); if False,
|
|
414
|
+
``q`` is flattened with :func:`getvector` first, matching
|
|
415
|
+
:meth:`jacob0`/:meth:`jacobe`/:meth:`hessian0`/:meth:`hessiane`,
|
|
416
|
+
none of which accept a trajectory
|
|
417
|
+
:raises TypeError: if ``q`` isn't numeric (from :func:`getvector`)
|
|
418
|
+
:raises ValueError: if ``q``'s length matches neither interpretation
|
|
419
|
+
:returns: a global jindex-addressed array; unchanged if ``q`` was
|
|
420
|
+
already global, scattered via :attr:`jindices` if ``q`` was
|
|
421
|
+
compact
|
|
422
|
+
|
|
423
|
+
This is the single place that disambiguates the two ``q`` shapes
|
|
424
|
+
accepted by :meth:`eval`, :meth:`jacob0`, :meth:`jacobe`,
|
|
425
|
+
:meth:`hessian0` and :meth:`hessiane` -- everything downstream of
|
|
426
|
+
this (the C++ extension and the pure-Python fallback) only ever
|
|
427
|
+
sees a global, jindex-addressed vector, exactly as before this
|
|
428
|
+
method existed.
|
|
429
|
+
|
|
430
|
+
Checking the compact length first means the common case -- a
|
|
431
|
+
single-chain robot, or any ETS whose own joints already happen to
|
|
432
|
+
carry global jindex 0..n-1 in order -- is unaffected: scattering
|
|
433
|
+
via :attr:`jindices` is then just the identity.
|
|
434
|
+
"""
|
|
435
|
+
n = self.n
|
|
436
|
+
jindices = self.jindices
|
|
437
|
+
|
|
438
|
+
q = getmatrix(q, (None, None)) if allow_trajectory else getvector(q, None)
|
|
439
|
+
|
|
440
|
+
if n == 0 or jindices.dtype == object:
|
|
441
|
+
# no joints, or at least one joint has no jindex assigned yet
|
|
442
|
+
# (e.g. an ETS2 instance before its lazy auto-assignment runs)
|
|
443
|
+
# -- nothing to safely disambiguate, leave q exactly as given
|
|
444
|
+
return q
|
|
445
|
+
|
|
446
|
+
global_len = int(jindices.max()) + 1
|
|
447
|
+
length = q.shape[-1]
|
|
448
|
+
|
|
449
|
+
if length == n and n < global_len:
|
|
450
|
+
# a genuine sub-chain (this ETS's own joints don't span the
|
|
451
|
+
# full global jindex range) -- scatter positionally
|
|
452
|
+
full = np.zeros(q.shape[:-1] + (global_len,), dtype=q.dtype)
|
|
453
|
+
full[..., jindices] = q
|
|
454
|
+
return full
|
|
455
|
+
|
|
456
|
+
if length >= global_len:
|
|
457
|
+
# already global-addressed; longer-than-needed is accepted
|
|
458
|
+
# (matches the pre-existing convention of indexing q by
|
|
459
|
+
# jindex and ignoring unused trailing entries). This also
|
|
460
|
+
# covers n == global_len, where compact and global lengths
|
|
461
|
+
# coincide -- always treating that as global (not scattering)
|
|
462
|
+
# matters when this ETS's own joints don't carry jindex in
|
|
463
|
+
# increasing order (e.g. a reversed chain from .inv()), where
|
|
464
|
+
# scattering would silently reorder q instead of leaving it
|
|
465
|
+
# alone.
|
|
466
|
+
return q
|
|
467
|
+
|
|
468
|
+
raise ValueError(
|
|
469
|
+
f"q has length {length}, expected {n} (compact, positionally "
|
|
470
|
+
f"matching this ETS's own joint order) or at least {global_len} "
|
|
471
|
+
f"(global, addressed by jindex -- e.g. the whole robot's q)"
|
|
472
|
+
)
|
|
473
|
+
|
|
402
474
|
@property
|
|
403
475
|
def qlim(self):
|
|
404
476
|
r"""
|
|
@@ -7,9 +7,13 @@
|
|
|
7
7
|
|
|
8
8
|
#include <Python.h>
|
|
9
9
|
#include <math.h>
|
|
10
|
+
#include <cmath>
|
|
10
11
|
#include <iostream>
|
|
11
12
|
#include <functional>
|
|
12
13
|
#include <Eigen/Dense>
|
|
14
|
+
#include <nanobind/nanobind.h>
|
|
15
|
+
|
|
16
|
+
namespace nb = nanobind;
|
|
13
17
|
|
|
14
18
|
// ---------------------------------------------------------------------------
|
|
15
19
|
// Shared loop kernel — all five IK solvers use this.
|
|
@@ -290,6 +294,19 @@ extern "C"
|
|
|
290
294
|
Eigen::Map<Eigen::ArrayXd> qlim_l(ets->qlim_l, ets->n);
|
|
291
295
|
Eigen::Map<Eigen::ArrayXd> q_range2(ets->q_range2, ets->n);
|
|
292
296
|
|
|
297
|
+
// A joint with a non-finite limit (inf/-inf/NaN, typically a bad
|
|
298
|
+
// value baked into a robot model's own joint-limit data) would
|
|
299
|
+
// otherwise silently propagate inf/NaN into q below, with no
|
|
300
|
+
// diagnostic at all -- mirrors the equivalent check in the
|
|
301
|
+
// pure-Python solver path (IK.py's _random_q()).
|
|
302
|
+
for (int i = 0; i < ets->n; i++)
|
|
303
|
+
{
|
|
304
|
+
if (!std::isfinite(qlim_l(i)) || !std::isfinite(q_range2(i)))
|
|
305
|
+
throw nb::value_error(
|
|
306
|
+
"Joint limit(s) are not finite -- can't generate a "
|
|
307
|
+
"random configuration within an infinite/undefined range.");
|
|
308
|
+
}
|
|
309
|
+
|
|
293
310
|
q = VectorX::Random(ets->n);
|
|
294
311
|
|
|
295
312
|
q = (q.array() + 1) * q_range2;
|
|
@@ -9,7 +9,7 @@ from spatialmath.pose2d import SE2
|
|
|
9
9
|
from spatialmath import base
|
|
10
10
|
from scipy.ndimage import *
|
|
11
11
|
import matplotlib.pyplot as plt
|
|
12
|
-
from matplotlib import
|
|
12
|
+
from matplotlib import colormaps
|
|
13
13
|
from roboticstoolbox.mobile.PlannerBase import PlannerBase
|
|
14
14
|
|
|
15
15
|
|
|
@@ -163,7 +163,7 @@ class DistanceTransformPlanner(PlannerBase):
|
|
|
163
163
|
[0, -1],
|
|
164
164
|
[1, -1],
|
|
165
165
|
[-1, 0],
|
|
166
|
-
[
|
|
166
|
+
[-1, 1],
|
|
167
167
|
[1, 0],
|
|
168
168
|
[0, 1],
|
|
169
169
|
[1, 1],
|
|
@@ -324,9 +324,7 @@ def distancexform(occgrid, goal, metric="cityblock", animate=False, summary=Fals
|
|
|
324
324
|
plt.ylabel("y")
|
|
325
325
|
ax = plt.gca()
|
|
326
326
|
plt.pause(0.001)
|
|
327
|
-
cmap =
|
|
328
|
-
cmap.set_bad("red")
|
|
329
|
-
cmap.set_over("white")
|
|
327
|
+
cmap = colormaps.get_cmap("gray").with_extremes(bad="red", over="white")
|
|
330
328
|
h = plt.imshow(display, cmap=cmap)
|
|
331
329
|
plt.colorbar(label="distance")
|
|
332
330
|
else:
|
|
@@ -90,6 +90,31 @@ def _load_rd_module(robot_name: str):
|
|
|
90
90
|
"URDF model."
|
|
91
91
|
)
|
|
92
92
|
|
|
93
|
+
# robot_descriptions clones a git repository (via GitPython, which shells
|
|
94
|
+
# out to a real git binary) the first time a given model is imported.
|
|
95
|
+
# Pyodide/JupyterLite has no subprocess execution and no git binary, so
|
|
96
|
+
# this always fails there -- not a bug, an environment limitation.
|
|
97
|
+
# Checked up front, before the candidates loop below, rather than caught
|
|
98
|
+
# per-attempt: GitPython's failure in this sandbox surfaces as a plain
|
|
99
|
+
# ImportError (message: "emscripten does not support processes"), which
|
|
100
|
+
# the loop's `except ImportError` treats as "this candidate name doesn't
|
|
101
|
+
# exist, try the next one" -- so after exhausting every candidate it fell
|
|
102
|
+
# through to a misleading "model not found"/"renamed" error instead of
|
|
103
|
+
# this one. The outcome here is deterministic regardless of which
|
|
104
|
+
# candidate name is tried, so there is nothing to gain by attempting the
|
|
105
|
+
# loop at all on this platform.
|
|
106
|
+
if sys.platform == "emscripten":
|
|
107
|
+
raise ValueError(
|
|
108
|
+
f"Toolbox uses {_rd_link()} to provide URDF robot models, "
|
|
109
|
+
"which clones a git repository on first use. That isn't "
|
|
110
|
+
"possible in this browser (Pyodide/JupyterLite) sandbox -- "
|
|
111
|
+
f'this is an expected limitation loading "{robot_name}" '
|
|
112
|
+
"here, not a bug. Try a DH- or ETS-based model instead "
|
|
113
|
+
"(e.g. rtb.models.DH.Panda()), or run this notebook in a "
|
|
114
|
+
"regular Python environment to use robot_descriptions-"
|
|
115
|
+
"backed models."
|
|
116
|
+
)
|
|
117
|
+
|
|
93
118
|
candidates = [f"{robot_name}_description", f"{robot_name}_official_description"]
|
|
94
119
|
last_error: ImportError | None = None
|
|
95
120
|
for candidate in candidates:
|
|
@@ -103,26 +128,6 @@ def _load_rd_module(robot_name: str):
|
|
|
103
128
|
except ImportError as e:
|
|
104
129
|
last_error = e
|
|
105
130
|
continue
|
|
106
|
-
except Exception as e:
|
|
107
|
-
# robot_descriptions clones a git repository (via GitPython,
|
|
108
|
-
# which shells out to a real git binary) the first time a given
|
|
109
|
-
# model is imported. Pyodide/JupyterLite has no subprocess
|
|
110
|
-
# execution and no git binary, so this always fails there --
|
|
111
|
-
# not a bug, an environment limitation. The exact exception type
|
|
112
|
-
# depends on how GitPython fails in that sandbox, so this is
|
|
113
|
-
# deliberately broad, but only ever intercepts on Pyodide.
|
|
114
|
-
if sys.platform == "emscripten":
|
|
115
|
-
raise ValueError(
|
|
116
|
-
f"Toolbox uses {_rd_link()} to provide URDF robot models, "
|
|
117
|
-
"which clones a git repository on first use. That isn't "
|
|
118
|
-
"possible in this browser (Pyodide/JupyterLite) sandbox -- "
|
|
119
|
-
f'this is an expected limitation loading "{robot_name}" '
|
|
120
|
-
"here, not a bug. Try a DH- or ETS-based model instead "
|
|
121
|
-
"(e.g. rtb.models.DH.Panda()), or run this notebook in a "
|
|
122
|
-
"regular Python environment to use robot_descriptions-"
|
|
123
|
-
"backed models."
|
|
124
|
-
) from e
|
|
125
|
-
raise
|
|
126
131
|
|
|
127
132
|
renamed_to = _find_rd_rename(robot_name, candidates)
|
|
128
133
|
if renamed_to is not None:
|
|
@@ -52,8 +52,8 @@ class YuMi(Robot):
|
|
|
52
52
|
l_gripper_links = [link for link in links if link.parent == gripper_l_base]
|
|
53
53
|
|
|
54
54
|
# New intermediate links
|
|
55
|
-
r_gripper = Link(name="r_gripper", parent=
|
|
56
|
-
l_gripper = Link(name="l_gripper", parent=
|
|
55
|
+
r_gripper = Link(name="r_gripper", parent=gripper_r_base)
|
|
56
|
+
l_gripper = Link(name="l_gripper", parent=gripper_l_base)
|
|
57
57
|
links.append(r_gripper)
|
|
58
58
|
links.append(l_gripper)
|
|
59
59
|
|
|
@@ -2058,6 +2058,9 @@ class BaseRobot(SceneNode, DynamicsMixin, RobotPlottingMPLMixin, ABC, Generic[Li
|
|
|
2058
2058
|
|
|
2059
2059
|
env = self._get_graphical_backend(backend)
|
|
2060
2060
|
|
|
2061
|
+
if movie is not None:
|
|
2062
|
+
from roboticstoolbox.backends.PyPlot import PyPlot
|
|
2063
|
+
|
|
2061
2064
|
launch_kwargs = {}
|
|
2062
2065
|
for key in ("render_mode", "inline_every_n", "inline_format", "inline_dpi"):
|
|
2063
2066
|
if key in kwargs:
|
|
@@ -2225,11 +2228,13 @@ class BaseRobot(SceneNode, DynamicsMixin, RobotPlottingMPLMixin, ABC, Generic[Li
|
|
|
2225
2228
|
already_blocked = env._add_teach_panel(self, q, handle, block)
|
|
2226
2229
|
|
|
2227
2230
|
if vellipse:
|
|
2228
|
-
vell = self.vellipse(q, centre="ee",
|
|
2231
|
+
vell = self.vellipse(q, centre="ee", add=False)
|
|
2232
|
+
env._teach_vellipse = vell
|
|
2229
2233
|
env.add(vell)
|
|
2230
2234
|
|
|
2231
2235
|
if fellipse:
|
|
2232
2236
|
fell = self.fellipse(q, centre="ee", add=False)
|
|
2237
|
+
env._teach_fellipse = fell
|
|
2233
2238
|
env.add(fell)
|
|
2234
2239
|
|
|
2235
2240
|
# Keep the plot open -- skipped if the backend already blocked
|