Skip to content

Commit 4f29a45

Browse files
committed
bug fix + features
1 parent b4a5504 commit 4f29a45

1 file changed

Lines changed: 28 additions & 3 deletions

File tree

‎spatialmath/twist.py‎

Lines changed: 28 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -353,6 +353,8 @@ def _import(self, value, check=True):
353353
elif value.shape == (6,):
354354
# it's a twist vector
355355
return value
356+
elif base.ishom(value, check=check):
357+
return base.trlog(value, twist=True, check=False)
356358
raise TypeError('bad type passed')
357359

358360
@staticmethod
@@ -471,7 +473,7 @@ def w(self):
471473
# -------------------- variant constructors ----------------------------#
472474

473475
@classmethod
474-
def Revolute(cls, a, q, p=None):
476+
def Revolute(cls, a, q, pitch=None):
475477
"""
476478
Construct a new unit rotational 3D twist
477479
@@ -495,8 +497,7 @@ def Revolute(cls, a, q, p=None):
495497
"""
496498
w = base.unitvec(base.getvector(a, 3))
497499
v = -np.cross(w, base.getvector(q, 3))
498-
if p is not None:
499-
pitch = base.getvector(p, 3)
500+
if pitch is not None:
500501
v = v + pitch * w
501502
return cls(v, w)
502503

@@ -1122,6 +1123,8 @@ def _import(self, value, check=True):
11221123
elif value.shape == (3,):
11231124
# it's a twist vector
11241125
return value
1126+
elif base.ishom2(value, check=check):
1127+
return base.trlog2(value, twist=True, check=False)
11251128
raise TypeError('bad type passed')
11261129

11271130
@staticmethod
@@ -1373,6 +1376,28 @@ def exp(self, theta=None, units='rad'):
13731376
else:
13741377
return SE2([base.trexp2(self.S * t) for t in theta])
13751378

1379+
def pole(self):
1380+
"""
1381+
Pole of a 2D twist
1382+
1383+
:return: the pole of the twist
1384+
:rtype: ndarray(2)
1385+
1386+
``X.pole()`` is a point on the twist axis. For a pure translation
1387+
this point is at infinity.
1388+
1389+
Example:
1390+
1391+
.. runblock:: pycon
1392+
1393+
>>> from spatialmath import SE3, Twist3
1394+
>>> T = SE3(1, 2, 3) * SE3.Rx(0.3)
1395+
>>> S = Twist3(T)
1396+
>>> S.pole()
1397+
"""
1398+
p = np.cross(np.r_[0, 0, self.w], np.r_[self.v, 0]) / self.theta()
1399+
return p[:2]
1400+
13761401
@property
13771402
def unit(self):
13781403
"""

0 commit comments

Comments
 (0)