@@ -68,7 +68,10 @@ def __init__(self, arg=None, *, check=True):
6868 """
6969 super ().__init__ ()
7070
71- if not super ().arghandler (arg , check = check ):
71+ if isinstance (arg , SE3 ):
72+ self .data = [base .t2r (x ) for x in arg .data ]
73+
74+ elif not super ().arghandler (arg , check = check ):
7275 raise ValueError ('bad argument to constructor' )
7376
7477 @staticmethod
@@ -689,21 +692,23 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
689692
690693 :rtype: SE3 instance
691694
692- There are multiple call signatures:
695+ There are multiple call signatures that return an ``SE3`` instance
696+ with one or more values.
693697
694- - ``SE3()`` is an ``SE3`` instance with one value -- a 4x4 identity
695- matrix which corresponds to a null motion.
698+ - ``SE3()`` null motion, value is the identity matrix.
696699 - ``SE3(x, y, z)`` is a pure translation of (x,y,z)
697- - ``SE3(T)`` is an ``SE3`` instance with the value ``T`` which is a 4x4
698- numpy array representing an SE(3) matrix. If ``check`` is ``True``
699- check the matrix belongs to SE(3).
700- - ``SE3(X)`` is an ``SE3`` instance with the same value as ``X``, ie.
701- a copy.
702- - ``SE3([T1, T2, ... TN])`` is an ``SE3`` instance with ``N`` values
700+ - ``SE3(T)`` where ``T`` is a 4x4 Numpy array representing an SE(3)
701+ matrix. If ``check`` is ``True`` check the matrix belongs to SE(3).
702+ - ``SE3([T1, T2, ... TN])`` has ``N`` values
703703 given by the elements ``Ti`` each of which is a 4x4 NumPy array
704704 representing an SE(3) matrix. If ``check`` is ``True`` check the
705705 matrix belongs to SE(3).
706- - ``SE3([X1, X2, ... XN])`` is an ``SE3`` instance with ``N`` values
706+ - ``SE3(X)`` where ``X`` is:
707+ - ``SE3`` is a copy of ``X``
708+ - ``SO3`` is the rotation of ``X`` with zero translation
709+ - ``SE2`` is the z-axis rotation and x- and y-axis translation of
710+ ``X``
711+ - ``SE3([X1, X2, ... XN])`` has ``N`` values
707712 given by the elements ``Xi`` each of which is an SE3 instance.
708713
709714 :SymPy: supported
@@ -713,6 +718,16 @@ def __init__(self, x=None, y=None, z=None, *, check=True):
713718
714719 if super ().arghandler (x , check = check ):
715720 return
721+ elif isinstance (x , SO3 ):
722+ self .data = [base .r2t (_x ) for _x in x .data ]
723+ elif isinstance (x , SE2 ):
724+ def convert (x ):
725+ # convert SE(2) to SE(3)
726+ out = np .identity (4 , dtype = x .dtype )
727+ out [:2 ,:2 ] = x [:2 ,:2 ]
728+ out [:2 ,3 ] = x [:2 ,2 ]
729+ return out
730+ self .data = [convert (_x ) for _x in x .data ]
716731 elif base .isvector (x , 3 ):
717732 # SE3( [x, y, z] )
718733 self .data = [base .transl (x )]
@@ -896,7 +911,7 @@ def jacob(self):
896911 """
897912 return base .tr2jac (self .A )
898913
899- def Twist3 (self ):
914+ def twist (self ):
900915 """
901916 SE(3) as twist
902917
@@ -906,7 +921,7 @@ def Twist3(self):
906921 Example::
907922
908923 >>> x = SE3(1,2,3)
909- >>> x.Twist3 ()
924+ >>> x.twist ()
910925 Twist3([1, 2, 3, 0, 0, 0])
911926
912927 :seealso: :func:`spatialmath.twist.Twist3`
@@ -1425,31 +1440,58 @@ def Tz(cls, z):
14251440 return cls ([base .transl (0 , 0 , _z ) for _z in base .getvector (z )], check = False )
14261441
14271442 @classmethod
1428- def SO3 (cls , R , t = None , check = True ):
1443+ def Rt (cls , R , t , check = True ):
1444+ """
1445+ Create an SE(3) from rotation and translation
1446+
1447+ :param R: rotation
1448+ :type R: SO3 or ndarray(3,3)
1449+ :param t: translation
1450+ :type t: array_like(3)
1451+ :param check: check rotation validity, defaults to True
1452+ :type check: bool, optional
1453+ :raises ValueError: bad rotation matrix
1454+ :return: SE(3) matrix
1455+ :rtype: SE3 instance
1456+ """
14291457 if isinstance (R , SO3 ):
14301458 R = R .A
14311459 elif base .isrot (R , check = check ):
14321460 pass
14331461 else :
14341462 raise ValueError ('expecting SO3 or rotation matrix' )
1435- if t is None :
1436- return cls (base .r2t (R ))
1437- else :
1438- return cls (base .rt2tr (R , t ))
1463+
1464+ return cls (base .rt2tr (R , t ))
1465+
1466+ # @classmethod
1467+ # def SO3(cls, R, t=None, check=True):
1468+ # if isinstance(R, SO3):
1469+ # R = R.A
1470+ # elif base.isrot(R, check=check):
1471+ # pass
1472+ # else:
1473+ # raise ValueError('expecting SO3 or rotation matrix')
1474+ # if t is None:
1475+ # return cls(base.r2t(R))
1476+ # else:
1477+ # return cls(base.rt2tr(R, t))
14391478
14401479if __name__ == '__main__' : # pragma: no cover
14411480
14421481 import pathlib
14431482
1483+ a = SE3 (1 ,2 ,3 )
1484+ b = SO3 (a )
1485+ print (b )
1486+
14441487 a = SO3 .RPY ([.1 , .2 , .3 ])
1445- print (a )
1446- a = SO3 .RPY (.1 , .2 , .3 )
1447- print (a )
1448-
1449- SE3 ._bgcolor = 'yellow'
1450- # SE3._format = '{:< 6.2f}'
1451- SE3 ._supress_small = False
1452- a = SE3 .Rx (np .pi / 2 )
1453- print (a )
1488+ b = SE3 (a )
1489+ print (b )
1490+
1491+ from spatialmath import SE2
1492+ a = SE2 (1 ,2 ,.4 )
1493+ b = SE3 (a )
1494+ print (b )
1495+
14541496
14551497 exec (open (pathlib .Path (__file__ ).parent .parent .absolute () / "tests" / "test_pose3d.py" ).read ()) # pylint: disable=exec-used
0 commit comments