Skip to content

Commit a747f78

Browse files
committed
add increment method to unit quaternion
1 parent 2ad3bae commit a747f78

2 files changed

Lines changed: 46 additions & 0 deletions

File tree

‎spatialmath/quaternion.py‎

Lines changed: 27 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1806,6 +1806,33 @@ def interp(self, s=0, start=None, shortest=False):
18061806

18071807
return UnitQuaternion(qi)
18081808

1809+
def increment(self, w, normalize=False):
1810+
"""
1811+
Quaternion incremental update
1812+
1813+
:param w: angular displacement, Euler vector
1814+
:type w: array_like(3)
1815+
:param normalize: normalize the result, defaults to False
1816+
:type normalize: bool, optional
1817+
1818+
.. note:: The object state is updated
1819+
"""
1820+
1821+
# is (v, theta) or None
1822+
vt = base.unitvec_norm(w)
1823+
1824+
if vt is None:
1825+
# zero update
1826+
return
1827+
1828+
ds = math.cos(vt[1] / 2)
1829+
dv = math.sin(vt[1] / 2) * vt[0]
1830+
1831+
updated = base.qqmul(self.A, np.r_[ds, dv])
1832+
if normalize:
1833+
updated = base.unit(updated)
1834+
self.data = [updated]
1835+
18091836
def plot(self, *args, **kwargs):
18101837
"""
18111838
Plot unit quaternion as a coordinate frame

‎tests/test_quaternion.py‎

Lines changed: 19 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -523,6 +523,25 @@ def test_interp(self):
523523
# qcompare( qq(6), UnitQuaternion.Rx(pi) )
524524
# TODO interp
525525

526+
def test_increment(self):
527+
q = UnitQuaternion()
528+
529+
q.increment([0, 0, 0])
530+
qcompare(q, UnitQuaternion())
531+
532+
q.increment([0, 0, 0], normalize=True)
533+
qcompare(q, UnitQuaternion())
534+
535+
for i in range(10):
536+
q.increment([0.1, 0, 0])
537+
qcompare(q, UnitQuaternion.Rx(1))
538+
539+
q = UnitQuaternion()
540+
for i in range(10):
541+
q.increment([0.1, 0, 0], normalize=True)
542+
qcompare(q, UnitQuaternion.Rx(1))
543+
544+
526545
def test_eq(self):
527546
q1 = UnitQuaternion([0, 1, 0, 0])
528547
q2 = UnitQuaternion([0, -1, 0, 0])

0 commit comments

Comments
 (0)