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.
@@ -61,6 +61,9 @@ __all__ = [
61
61
  "rtb_path_to_datafile",
62
62
  "rtb_set_param",
63
63
  "rtb_get_param",
64
+ "TrChainToken",
65
+ "trchain",
66
+ "trchain2",
64
67
  # mobile
65
68
  "VehicleBase",
66
69
  "Bicycle",
@@ -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
- return _pil("RGB", canvas.get_width_height(), canvas.tostring_rgb())
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
- defaults[key] = {**defaults[key], **options[key]}
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
- defaults[key] = {**defaults[key], **options[key]}
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):
@@ -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
- self._qdd = None
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
- # print(q)
1545
- # print(qd)
1546
- # print(xd)
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
 
@@ -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
- ) -> tuple[NDArray, int, int, int, float]:
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: tuple (q, success, iterations, searches, residual)
1017
- :rtype: tuple
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 deined as
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
- return IK_LM_c(
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
- ) -> tuple[NDArray, int, int, int, float]:
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: tuple (q, success, iterations, searches, residual)
1151
- :rtype: tuple
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
- return IK_NR_c(
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
- ) -> tuple[NDArray, int, int, int, float]:
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
- return IK_GN_c(
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: IK solution
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: IK solution
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: IK solution
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: IK solution
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
@@ -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 = getmatrix(q, (None, None))
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]
@@ -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 cm
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
- [0, 0],
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 = cm.get_cmap("gray")
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=gripper_l_base)
56
- l_gripper = Link(name="l_gripper", parent=gripper_r_base)
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", scale=0.5, add=False)
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