Skip to content

Commit de4b46e

Browse files
committed
changes to interp
1 parent 184a95f commit de4b46e

2 files changed

Lines changed: 40 additions & 19 deletions

File tree

‎spatialmath/super_pose.py‎

Lines changed: 18 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -367,38 +367,43 @@ def interp(self, end=None, s=None):
367367
368368
:param end: final pose
369369
:type end: same as ``self``
370-
:param s: interpolation coefficient, range 0 to 1
371-
:type s: array_like
370+
:param s: interpolation coefficient, range 0 to 1, or number of steps
371+
:type s: array_like or int
372372
:return: interpolated pose
373-
:rtype: SO2, SE2, SO3, SE3 instance
373+
:rtype: same as ``self``
374374
375375
- ``X.interp(Y, s)`` interpolates pose between X between when s=0
376376
and Y when s=1.
377+
- ``X.interp(Y, N)`` interpolates pose between X and Y in ``N`` steps.
377378
378379
Example:
379380
380381
.. runblock:: pycon
381382
382383
>>> x = SE3(-1, -2, 0) * SE3.Rx(-0.3)
383384
>>> y = SE3(1, 2, 0) * SE3.Rx(0.3)
384-
>>> x.interp(y, 0)
385-
>>> x.interp(y, 1)
386-
>>> x.interp(y, 0.5)
387-
>>> z = x.interp(y, np.linspace(0, 1, 11))
385+
>>> x.interp(y, 0) # this is x
386+
>>> x.interp(y, 1) # this is y
387+
>>> x.interp(y, 0.5) # this is in between
388+
>>> z = x.interp(y, 11) # in 11 steps
388389
>>> len(z)
389-
>>> y[0]
390-
>>> y[5]
390+
>>> z[0] # this is x
391+
>>> z[5] # this is in between
391392
392393
.. note::
393394
394395
- For SO3 and SE3 rotation is interpolated using quaternion spherical linear interpolation (slerp).
395-
396+
- Values of ``s`` outside the range [0,1] are silently clipped
396397
:seealso: :func:`interp1`, :func:`~spatialmath.base.transforms3d.trinterp`, :func:`~spatialmath.base.quaternions.slerp`, :func:`~spatialmath.base.transforms2d.trinterp2`
397398
398399
:SymPy: not supported
399400
"""
400-
s = base.getvector(s)
401-
s = np.clip(s, 0, 1)
401+
402+
if isinstance(s, int) and s > 1:
403+
s = np.linspace(0, 1, s)
404+
else:
405+
s = base.getvector(s)
406+
s = np.clip(s, 0, 1)
402407

403408
if len(self) > 1:
404409
raise ValueError('start pose must be a singleton')
@@ -1075,7 +1080,7 @@ def __matmul__(left, right): # lgtm[py/not-named-self] pylint: disable=no-self-
10751080
10761081
.. note:: This operator is functionally equivalent to ``*`` but is more
10771082
costly. It is useful for cases where a pose is incrementally
1078-
update over many cycle.s
1083+
update over many cycles.
10791084
10801085
:seealso: :func:`__mul__`, :func:`~spatialmath.base.trnorm`
10811086
"""

‎tests/test_quaternion.py‎

Lines changed: 22 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -356,6 +356,16 @@ def test_multiply(self):
356356
# vector x scalar
357357
nt.assert_array_almost_equal(UnitQuaternion([ry, rz, rx]) * vy, np.c_[vy, -vx, vz])
358358

359+
def test_matmul(self):
360+
361+
rx = UnitQuaternion.Rx(pi / 2)
362+
ry = UnitQuaternion.Ry(pi / 2)
363+
rz = UnitQuaternion.Rz(pi / 2)
364+
365+
qcompare(rx @ ry, rx * ry)
366+
367+
qcompare(UnitQuaternion([ry, rz, rx]) @ UnitQuaternion([rx, ry, rz]), UnitQuaternion([ry * rx, rz * ry, rx * rz]))
368+
359369
# def multiply_test_normalized(self):
360370

361371
# vx = [1, 0, 0]; vy = [0, 1, 0]; vz = [0, 0, 1]
@@ -495,26 +505,32 @@ def test_interp(self):
495505
q = rx * ry * rz
496506

497507
# from null
498-
qcompare(q.interp(0), u)
499-
qcompare(q.interp(1), q)
508+
qcompare(q.interp1(0), u)
509+
qcompare(q.interp1(1), q)
500510

501511
#self.assertEqual(length(q.interp(linspace(0,1, 10))), 10)
502512
#self.assertTrue(all( q.interp([0, 1]) == [u, q]))
503513
# TODO vectorizing
504514

505-
q0_5 = q.interp(0.5)
515+
q0_5 = q.interp1(0.5)
506516
qcompare(q0_5 * q0_5, q)
507517

518+
qq = rx.interp1(11)
519+
self.assertEqual(len(qq), 11)
520+
508521
# between two quaternions
509-
qcompare(rx.interp(0, start=q), q)
510-
qcompare(rx.interp(1, start=q), rx)
522+
qcompare(q.interp(rx, 0), q)
523+
qcompare(q.interp(rx, 1), rx)
511524

512525
# test vectorised results
513-
qq = rx.interp([0, 1], start=q)
526+
qq = q.interp(rx, [0, 1])
514527
self.assertEqual(len(qq), 2)
515528
qcompare(qq[0], q)
516529
qcompare(qq[1], rx)
517530

531+
qq = rx.interp(q, 11)
532+
self.assertEqual(len(qq), 11)
533+
518534
#self.assertTrue(all( q.interp([0, 1], dest=rx, ) == [q, rx]))
519535

520536
# test shortest option

0 commit comments

Comments
 (0)