Skip to content

Commit 30ee4e6

Browse files
petercorkepetercorke
committed
fix bug in trinterp, add additional unit tests for 2D and 3D interp
1 parent d22417b commit 30ee4e6

3 files changed

Lines changed: 44 additions & 13 deletions

File tree

‎spatialmath/base/transforms3d.py‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -1648,12 +1648,12 @@ def trinterp(start, end, s):
16481648

16491649
if start is None:
16501650
# TRINTERP(T, s)
1651-
q0 = r2q(t2r(end))
1651+
q0 = r2q(end)
16521652
qr = qslerp(qeye(), q0, s)
16531653
else:
16541654
# TRINTERP(T0, T1, s)
1655-
q0 = r2q(t2r(start))
1656-
q1 = r2q(t2r(end))
1655+
q0 = r2q(start)
1656+
q1 = r2q(end)
16571657
qr = qslerp(q0, q1, s)
16581658

16591659
return q2r(qr)
@@ -3458,7 +3458,7 @@ def tranimate(T: Union[SO3Array, SE3Array], **kwargs) -> str:
34583458
# theta, vec = tr2angvec(T)
34593459
# print(theta, vec)
34603460
# print(trlog(T, twist=True))
3461-
3462-
X = transl(3, 4, -4)
3463-
trplot(X, width=2, style="rviz", block=True)
3464-
pass
3461+
R = rotx(np.pi / 2)
3462+
s = tranimate(R, movie=True)
3463+
with open("z.html", "w") as f:
3464+
print(f"<html>{s}</html", file=f)

‎tests/base/test_transforms2d.py‎

Lines changed: 20 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -110,26 +110,22 @@ def test_xyt2tr(self):
110110
nt.assert_array_almost_equal(T, rt2tr(rot2(0.2), [1, 2]))
111111

112112
def test_trinv2(self):
113-
114113
T = rt2tr(rot2(0.2), [1, 2])
115114
nt.assert_array_almost_equal(trinv2(T) @ T, np.eye(3))
116115

117116
def test_tradjoint2(self):
118-
119117
T = xyt2tr([1, 2, 0.2])
120118
X = [1, 2, 3]
121119
nt.assert_almost_equal(tradjoint2(T) @ X, vexa(T @ skewa(X) @ trinv2(T)))
122120

123121
def test_points2tr2(self):
124-
125122
p1 = np.random.uniform(size=(2, 5))
126123
T = xyt2tr([1, 2, 0.2])
127124
p2 = homtrans(T, p1)
128125
T2 = points2tr2(p1, p2)
129126
nt.assert_almost_equal(T, T2)
130127

131128
def test_icp2d(self):
132-
133129
p1 = np.random.uniform(size=(2, 30))
134130
T = xyt2tr([1, 2, 0.2])
135131

@@ -141,7 +137,6 @@ def test_icp2d(self):
141137
nt.assert_almost_equal(T, T2)
142138

143139
def test_print2(self):
144-
145140
T = transl2(1, 2) @ trot2(0.3)
146141

147142
s = trprint2(T, file=None)
@@ -198,6 +193,20 @@ def test_checks(self):
198193
nt.assert_equal(ishom2(T, True), False)
199194

200195
def test_trinterp2(self):
196+
R0 = rot2(-0.3)
197+
R1 = rot2(0.3)
198+
199+
nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=0), np.eye(2))
200+
nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=1), R1)
201+
nt.assert_array_almost_equal(
202+
trinterp2(start=None, end=R1, s=0.5), rot2(0.3 / 2)
203+
)
204+
205+
nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=0), np.eye(2))
206+
nt.assert_array_almost_equal(trinterp2(start=None, end=R1, s=1), R1)
207+
nt.assert_array_almost_equal(
208+
trinterp2(start=None, end=R1, s=0.5), rot2(0.3 / 2)
209+
)
201210

202211
T0 = trot2(-0.3)
203212
T1 = trot2(0.3)
@@ -206,6 +215,12 @@ def test_trinterp2(self):
206215
nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=1), T1)
207216
nt.assert_array_almost_equal(trinterp2(start=T0, end=T1, s=0.5), np.eye(3))
208217

218+
nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=0), np.eye(3))
219+
nt.assert_array_almost_equal(trinterp2(start=None, end=T1, s=1), T1)
220+
nt.assert_array_almost_equal(
221+
trinterp2(start=None, end=T1, s=0.5), trot2(0.3 / 2)
222+
)
223+
209224
T0 = transl2(-1, -2)
210225
T1 = transl2(1, 2)
211226

@@ -242,5 +257,4 @@ def test_plot(self):
242257

243258
# ---------------------------------------------------------------------------------------#
244259
if __name__ == "__main__":
245-
246260
unittest.main()

‎tests/base/test_transforms3d.py‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -521,13 +521,30 @@ def test_print(self):
521521
self.assertFalse("zyx" in s)
522522

523523
def test_trinterp(self):
524+
R0 = rotx(-0.3)
525+
R1 = rotx(0.3)
526+
527+
nt.assert_array_almost_equal(trinterp(start=R0, end=R1, s=0), R0)
528+
nt.assert_array_almost_equal(trinterp(start=R0, end=R1, s=1), R1)
529+
nt.assert_array_almost_equal(trinterp(start=R0, end=R1, s=0.5), np.eye(3))
530+
531+
nt.assert_array_almost_equal(trinterp(start=None, end=R1, s=0), np.eye(3))
532+
nt.assert_array_almost_equal(trinterp(start=None, end=R1, s=1), R1)
533+
nt.assert_array_almost_equal(trinterp(start=None, end=R1, s=0.5), rotx(0.3 / 2))
534+
524535
T0 = trotx(-0.3)
525536
T1 = trotx(0.3)
526537

527538
nt.assert_array_almost_equal(trinterp(start=T0, end=T1, s=0), T0)
528539
nt.assert_array_almost_equal(trinterp(start=T0, end=T1, s=1), T1)
529540
nt.assert_array_almost_equal(trinterp(start=T0, end=T1, s=0.5), np.eye(4))
530541

542+
nt.assert_array_almost_equal(trinterp(start=None, end=T1, s=0), np.eye(4))
543+
nt.assert_array_almost_equal(trinterp(start=None, end=T1, s=1), T1)
544+
nt.assert_array_almost_equal(
545+
trinterp(start=None, end=T1, s=0.5), trotx(0.3 / 2)
546+
)
547+
531548
T0 = transl(-1, -2, -3)
532549
T1 = transl(1, 2, 3)
533550

0 commit comments

Comments
 (0)