Skip to content

Commit e2555b1

Browse files
committed
constructor doco
allow more types to be passed to constructor new constructor for SE3
1 parent 350768f commit e2555b1

3 files changed

Lines changed: 80 additions & 37 deletions

File tree

‎spatialmath/pose2d.py‎

Lines changed: 7 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -74,7 +74,10 @@ def __init__(self, arg=None, *, unit='rad', check=True):
7474
"""
7575
super().__init__()
7676

77-
if super().arghandler(arg, check=check):
77+
if isinstance(arg, SE2):
78+
self.data = [tr.t2r(x) for x in arg.data]
79+
80+
elif super().arghandler(arg, check=check):
7881
return
7982

8083
elif argcheck.isscalar(arg):
@@ -288,6 +291,9 @@ def __init__(self, x=None, y=None, theta=None, *, unit='rad', check=True):
288291
if super().arghandler(x, check=check):
289292
return
290293

294+
if isinstance(x, SO2):
295+
self.data = [tr.r2t(_x) for _x in x.data]
296+
291297
elif argcheck.isscalar(x):
292298
self.data = [tr.trot2(x, unit=unit)]
293299
elif len(x) == 2:

‎spatialmath/pose3d.py‎

Lines changed: 69 additions & 27 deletions
Original file line numberDiff line numberDiff line change
@@ -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

14401479
if __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

‎spatialmath/quaternion.py‎

Lines changed: 4 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -940,20 +940,19 @@ def __init__(self, s: Any = None, v=None, norm=True, check=True):
940940
list, tuple, or ndarray(3). If ``norm`` is True the resulting
941941
quaternion is normalized.
942942
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
943-
elements from ``v`` which is a 4-vector given as a list, tuple, or ndarray(4)
943+
elements from ``v`` which is a 4-vector given as a list, tuple, or ndarray(4). Also known
944+
as the Euler parameters.
945+
- ``UnitQuaternion(M)`` construct a new unit quaternion with ``N`` values where ``Q`` is a Nx4 NumPy array
946+
whose rows are the quaternion in vector form
944947
- ``UnitQuaternion(R)`` constructs a unit quaternion from an SO(3)
945948
rotation matrix given as a ndarray(3,3). If ``check`` is True
946949
test the rotation submatrix for orthogonality.
947-
- ``UnitQuaternion(T)`` constructs a unit quaternion from an SE(3)
948-
homogeneous transformation matrix given as a ndarray(4,4). If ``check`` is True
949-
test the matrix for orthogonality.
950950
- ``UnitQuaternion(X)`` constructs a unit quaternion from the rotational
951951
part of ``X`` which is an SO3 or SE3 instance. If len(X) > 1 then
952952
the resulting unit quaternion is of the same length.
953953
- ``UnitQuaternion([q1, q2 .. qN])`` construct a new unit quaternion with ``N`` values where each element is a 4-vector
954954
- ``UnitQuaternion([Q1, Q2 .. QN])`` construct a new unit quaternion with ``N`` values where each element is a UnitQuaternion instance
955955
- ``UnitQuaternion([X1, X2 .. XN])`` construct a new unit quaternion with ``N`` values where each element is an SO3 or SE3 instance
956-
- ``UnitQuaternion(M)`` construct a new unit quaternion with ``N`` values where ``Q`` is a Nx4 NumPy array.
957956
958957
Example:
959958
@@ -973,10 +972,6 @@ def __init__(self, s: Any = None, v=None, norm=True, check=True):
973972
# create unit quaternion
974973
self.data = [base.unit(q) for q in self.data]
975974

976-
elif isinstance(s, np.ndarray) and base.isrot(s, check=check):
977-
# UnitQuaternion(R) R is 3x3 rotation matrix
978-
self.data = [base.r2q(s)]
979-
980975
elif isinstance(s, np.ndarray):
981976
# passed a NumPy array, it could be:
982977
# an SO(3) or SE(3) matrix

0 commit comments

Comments
 (0)