@@ -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