8000 add option to trlog to return a twist vector rather than matrix · flyinger/spatialmath-python@8252fa0 · GitHub
[go: up one dir, main page]
Skip to content

Commit 8252fa0

Browse files
petercorkepetercorke
committed
add option to trlog to return a twist vector rather than matrix
1 parent 6f7d007 commit 8252fa0

File tree

3 files changed

+289
-211
lines changed

3 files changed

+289
-211
lines changed

‎spatialmath/base/test_transforms.py

Lines changed: 23 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -967,42 +967,64 @@ def test_trlog(self):
967967
# %%% SO(3) tests
968968
# zero rotation case
969969
nt.assert_array_almost_equal(trlog(np.eye(3)), skew([0, 0, 0]))
970+
nt.assert_array_almost_equal(trlog(np.eye(3), twist=True), np.r_[0, 0, 0])
970971

971972
# rotation by pi case
972973
nt.assert_array_almost_equal(trlog(rotx(pi)), skew([pi, 0, 0]))
973974
nt.assert_array_almost_equal(trlog(roty(pi)), skew([0, pi, 0]))
974975
nt.assert_array_almost_equal(trlog(rotz(pi)), skew([0, 0, pi]))
975976

977+
nt.assert_array_almost_equal(trlog(rotx(pi), twist=True), np.r_[pi, 0, 0])
978+
nt.assert_array_almost_equal(trlog(roty(pi), twist=True), np.r_[0, pi, 0])
979+
nt.assert_array_almost_equal(trlog(rotz(pi), twist=True), np.r_[0, 0, pi])
980+
976981
# general case
977982
nt.assert_array_almost_equal(trlog(rotx(0.2)), skew([0.2, 0, 0]))
978983
nt.assert_array_almost_equal(trlog(roty(0.3)), skew([0, 0.3, 0]))
979984
nt.assert_array_almost_equal(trlog(rotz(0.4)), skew([0, 0, 0.4]))
980985

986+
nt.assert_array_almost_equal(trlog(rotx(0.2), twist=True), np.r_[0.2, 0, 0])
987+
nt.assert_array_almost_equal(trlog(roty(0.3), twist=True), np.r_[0, 0.3, 0])
988+
nt.assert_array_almost_equal(trlog(rotz(0.4), twist=True), np.r_[0, 0, 0.4])
989+
981990
R = rotx(0.2) @ roty(0.3) @ rotz(0.4)
982991
nt.assert_array_almost_equal(trlog(R), logm(R))
992+
nt.assert_array_almost_equal(trlog(R, twist=True), vex(logm(R)))
983993

984-
# %% SE(3) tests
994+
# SE(3) tests
985995

986996
# pure translation
987997
nt.assert_array_almost_equal(trlog(transl([1, 2, 3])), np.array([[0, 0, 0, 1], [0, 0, 0, 2], [0, 0, 0, 3], [0, 0, 0, 0]]))
998+
nt.assert_array_almost_equal(trlog(transl([1, 2, 3]), twist=True), np.r_[1,2,3,0,0,0])
988999

9891000
# pure rotation
9901001
# rotation by pi case
9911002
nt.assert_array_almost_equal(trlog(trotx(pi)), skewa([0, 0, 0, pi, 0, 0]))
9921003
nt.assert_array_almost_equal(trlog(troty(pi)), skewa([0, 0, 0, 0, pi, 0]))
9931004
nt.assert_array_almost_equal(trlog(trotz(pi)), skewa([0, 0, 0, 0, 0, pi]))
9941005

1006+
nt.assert_array_almost_equal(trlog(trotx(pi), twist=True), np.r_[0, 0, 0, pi, 0, 0])
1007+
nt.assert_array_almost_equal(trlog(troty(pi), twist=True), np.r_[0, 0, 0, 0, pi, 0])
1008+
nt.assert_array_almost_equal(trlog(trotz(pi), twist=True), np.r_[0, 0, 0, 0, 0, pi])
1009+
9951010
# general case
9961011
nt.assert_array_almost_equal(trlog(trotx(0.2)), skewa([0, 0, 0, 0.2, 0, 0]))
9971012
nt.assert_array_almost_equal(trlog(troty(0.3)), skewa([0, 0, 0, 0, 0.3, 0]))
9981013
nt.assert_array_almost_equal(trlog(trotz(0.4)), skewa([0, 0, 0, 0, 0, 0.4]))
9991014

1015+
nt.assert_array_almost_equal(trlog(trotx(0.2), twist=True), np.r_[0, 0, 0, 0.2, 0, 0])
1016+
nt.assert_array_almost_equal(trlog(troty(0.3), twist=True), np.r_[0, 0, 0, 0, 0.3, 0])
1017+
nt.assert_array_almost_equal(trlog(trotz(0.4), twist=True), np.r_[0, 0, 0, 0, 0, 0.4])
1018+
10001019
# mixture
10011020
T = transl([1, 2, 3]) @ trotx(0.3)
10021021
nt.assert_array_almost_equal(trlog(T), logm(T))
1022+
nt.assert_array_almost_equal(trlog(T, twist=True), vexa(logm(T)))
10031023

10041024
T = transl([1, 2, 3]) @ troty(0.3)
10051025
nt.assert_array_almost_equal(trlog(T), logm(T))
1026+
nt.assert_array_almost_equal(trlog(T, twist=True), vexa(logm(T)))
1027+
10061028

10071029
# def test_trlog2(self):
10081030

‎spatialmath/base/transforms2d.py

Lines changed: 26 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -217,12 +217,16 @@ def isrot2(R, check=False):
217217
# ---------------------------------------------------------------------------------------#
218218

219219

220-
def trlog2(T, check=True):
220+
def trlog2(T, check=True, twist=False):
221221
"""
222222
Logarithm of SO(2) or SE(2) matrix
223223
224224
:param T: SO(2) or SE(2) matrix
225225
:type T: numpy.ndarray, shape=(2,2) or (3,3)
226+
:param check: check that matrix is valid
227+
:type check: bool
228+
:param twist: return a twist vector instead of matrix [default]
229+
:type twist: bool
226230
:return: logarithm
227231
:rtype: numpy.ndarray, shape=(2,2) or (3,3)
228232
:raises: ValueError
@@ -248,13 +252,22 @@ def trlog2(T, check=True):
248252

249253
if trn.iseye(T):
250254
# is identity matrix
251-
return np.zeros((3, 3))
255+
if twist:
256+
return np.zeros((3,))
257+
else:
258+
return np.zeros((3, 3))
252259
else:
253-
return scipy.linalg.logm(T)
260+
if twist:
261+
return trn.vex(scipy.linalg.logm(T))
262+
else:
263+
return scipy.linalg.logm(T)
254264

255265
elif isrot2(T, check=check):
256266
# SO(2) rotation matrix
257-
return scipy.linalg.logm(T)
267+
if twist:
268+
return trn.vex(scipy.linalg.logm(T))
269+
else:
270+
return scipy.linalg.logm(T)
258271
else:
259272
raise ValueError("Expect SO(2) or SE(2) matrix")
260273
# ---------------------------------------------------------------------------------------#
@@ -383,30 +396,30 @@ def trinterp2(start, end, s=None):
383396
384397
%## 2d homogeneous trajectory
385398
"""
386-
if argcheck.ismatrix(start, (2, 2)):
399+
if argcheck.ismatrix(end, (2, 2)):
387400
# SO(2) case
388401
if start is None:
389402
# TRINTERP2(T, s)
390403

391-
th0 = math.atan2(start[1, 0], start[0, 0])
404+
th0 = math.atan2(end[1, 0], end[0, 0])
392405

393406
th = s * th0
394407
else:
395408
# TRINTERP2(T1, start= s)
396-
assert start.shape == end.shape, 'both matrices must be same shape'
409+
assert start.shape == end.shape, 'start and end matrices must be same shape'
397410

398411
th0 = math.atan2(start[1, 0], start[0, 0])
399412
th1 = math.atan2(end[1, 0], end[0, 0])
400413

401414
th = th0 * (1 - s) + s * th1
402415

403416
return rot2(th)
404-
elif argcheck.ismatrix(start, (3, 3)):
405-
if end is None:
417+
elif argcheck.ismatrix(end, (3, 3)):
418+
if start is None:
406419
# TRINTERP2(T, s)
407420

408-
th0 = math.atan2(start[1, 0], start[0, 0])
409-
p0 = transl2(start)
421+
th0 = math.atan2(end[1, 0], end[0, 0])
422+
p0 = transl2(end)
410423

411424
th = s * th0
412425
pr = s * p0
@@ -502,9 +515,9 @@ def trplot2(*args, **kwargs): # pylint: disable=unused-argument,missing-functio
502515

503516
if _matplotlib_exists:
504517

505-
def trplot2(T, axes=None, block=True, dims=None, color='blue', frame=None,
518+
def trplot2(T, axes=None, block=True, dims=None, color='blue', frame=None, # pylint: disable=unused-argument,function-redefined
506519
textcolor=None, labels=('X', 'Y'), length=1, arrow=True,
507-
rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs): # pylint: disable=unused-argument,function-redefined
520+
rviz=False, wtl=0.2, width=1, d1=0.05, d2=1.15, **kwargs):
508521
"""
509522
Plot a 2D coordinate frame
510523

0 commit comments

Comments
 (0)
0