def __init__(self, s=None, v=None, check=True, norm=True):
"""
A unit quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
A quaternion can be considered as a rotation about a vector in space where
q = cos (theta/2) sin(theta/2) <vx vy vz>
where <vx vy vz> is a unit vector.
:param s: scalar
:param v: vector
"""
if s is None and v is None:
self.data = [ quat.qone() ]
elif argcheck.isscalar(s) and argcheck.isvector(v,3):
self.data = [ np.r_[s, argcheck.getvector(v)] ]
elif argcheck.isvector(s,4):
self.data = [ argcheck.getvector(s) ]
elif type(s) is list:
if check:
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
self.data = [x for x in s]
else:
raise ValueError('bad argument to Quaternion constructor')
def __init__(self, value):
"""
Create a new spatial vector (abstract superclass)
:param value: Value of the
- ``SpatialVector(vec)`` is a spatial vector constructed from the 6-element array-like ``vec``
- ``SpatialVector([V1, V2, ... VN])`` is a spatial vector array with N elements, constructed from the 6-element
array-like values ``Vi``
- ``SpatialVector(A)`` is a spatial vector array with N elements, constructed from the columns of the 6xN
array ``A``.
:seealso: :func:`SpatialVelocity`, :func:`SpatialAcceleration`, :func:`SpatialForce`, :func:`SpatialMomentum`.
"""
print('spatialVec6 init')
super().__init__()
if value is None:
self.data = [np.zeros((6,))]
elif arg.isvector(value, 6):
self.data = [np.array(value)]
elif isinstance(value, list):
assert all(map(lambda x: arg.isvector(x, 6), value)), 'all elements of list must have valid shape and value for the class'
self.data = [np.array(x) for x in value]
elif arg.ismatrix(value, (6, None)):
self.data = [x for x in value.T]
else:
raise ValueError('bad arguments to constructor')
def I(self, I_new): # noqa
if ismatrix(I_new, (3, 3)):
# 3x3 matrix passed
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 9):
# 3x3 matrix passed as a 1d vector
I_new = I_new.reshape(3, 3)
if np.any(np.abs(I_new - I_new.T) > 1e-8):
raise ValueError('3x3 matrix is not symmetric')
elif isvector(I_new, 6):
# 6-vector passed, moments and products of inertia,
# [Ixx Iyy Izz Ixy Iyz Ixz]
I_new = np.array([
[I_new[0], I_new[3], I_new[5]],
[I_new[3], I_new[1], I_new[4]],
[I_new[5], I_new[4], I_new[2]]
])
elif isvector(I_new, 3):
# 3-vector passed, moments of inertia [Ixx Iyy Izz]
I_new = np.diag(I_new)
else:
raise ValueError('invalid shape passed: must be (3,3), (6,), (3,)')
self._I = I_new
def __init__(self, x=None, y=None, theta=None, *, unit='rad', check=True):
"""
Construct new SE(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SE(2) elements if applicable, default to True
:type check: bool
:return: homogeneous rigid-body transformation matrix
:rtype: SE2 instance
- ``SE2()`` is an SE2 instance representing a null motion -- the identity matrix
- ``SE2(x, y)`` is an SE2 instance representing a pure translation of (``x``, ``y``)
- ``SE2(t)`` is an SE2 instance representing a pure translation of (``x``, ``y``) where``t``=[x,y] is a 2-element array_like
- ``SE2(x, y, theta)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` radians
- ``SE2(x, y, theta, unit='deg')`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` degrees
- ``SE2(t)`` is an SE2 instance representing a translation of (``x``, ``y``) and a rotation of ``theta`` where ``t``=[x,y,theta] is a 3-element array_like
- ``SE2(T)`` is an SE2 instance with rigid-body motion described by the SE(2) matrix T which is a 3x3 numpy array. If ``check``
is ``True`` check the matrix belongs to SE(2).
- ``SE2([T1, T2, ... TN])`` is an SE2 instance containing a sequence of N rigid-body motions, each described by an SE(2) matrix
Ti which is a 3x3 numpy array. If ``check`` is ``True`` then check each matrix belongs to SE(2).
- ``SE2([X1, X2, ... XN])`` is an SE2 instance containing a sequence of N rigid-body motions, where each Xi is an SE2 instance.
"""
super().__init__() # activate the UserList semantics
if x is None and y is None and theta is None:
# SE2()
# empty constructor
self.data = [np.eye(3)]
elif x is not None:
if y is not None and theta is None:
# SE2(x, y)
self.data = [tr.transl2(x, y)]
elif y is not None and theta is not None:
# SE2(x, y, theta)
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
elif y is None and theta is None:
if argcheck.isvector(x, 2):
# SE2([x,y])
self.data = [tr.transl2(x)]
elif argcheck.isvector(x, 3):
# SE2([x,y,theta])
self.data = [tr.trot2(x[2], t=x[:2], unit=unit)]
else:
super()._arghandler(x, check=check)
else:
raise ValueError('bad arguments to constructor')
def __init__(self, v=None, w=None):
"""
Create a Plucker 3D line object
:param v: Plucker vector, Plucker object, Plucker moment
:type v: 6-element array_like, Plucker instance, 3-element array_like
:param w: Plucker direction, optional
:type w: 3-element array_like, optional
:raises ValueError: bad arguments
:return: Plucker line
:rtype: Plucker
- ``L = Plucker(X)`` creates a Plucker object from the Plucker coordinate vector
``X`` = [V,W] where V (3-vector) is the moment and W (3-vector) is the line direction.
- ``L = Plucker(L)`` creates a copy of the Plucker object ``L``.
- ``L = Plucker(V, W)`` creates a Plucker object from moment ``V`` (3-vector) and
line direction ``W`` (3-vector).
Notes:
- The Plucker object inherits from ``collections.UserList`` and has list-like
behaviours.
- A single Plucker object contains a 1D array of Plucker coordinates.
- The elements of the array are guaranteed to be Plucker coordinates.
- The number of elements is given by ``len(L)``
- The elements can be accessed using index and slice notation, eg. ``L[1]`` or
``L[2:3]``
- The Plucker instance can be used as an iterator in a for loop or list comprehension.
- Some methods support operations on the internal list.
:seealso: Plucker.PQ, Plucker.Planes, Plucker.PointDir
"""
super().__init__() # enable list powers
if w is None:
# single parameter
if isinstance(v, Plucker):
self.data = [v.A]
elif arg.isvector(v, 6):
pl = arg.getvector(v)
self.data = [pl]
else:
raise ValueError('bad argument')
else:
assert arg.isvector(v, 3) and arg.isvector(
w, 3), 'expecting two 3-vectors'
self.data = [np.r_[v, w]]
def contains(self, x, tol=50 * _eps):
"""
Test if points are on the line
:param x: 3D point
:type x: 3-element array_like, or numpy.ndarray, shape=(3,N)
:param tol: Tolerance, defaults to 50*_eps
:type tol: float, optional
:raises ValueError: Bad argument
:return: Whether point is on the line
:rtype: bool or numpy.ndarray(N) of bool
``line.contains(X)`` is true if the point ``X`` lies on the line defined by
the Plucker object self.
If ``X`` is an array with 3 rows, the test is performed on every column and
an array of booleans is returned.
"""
if arg.isvector(x, 3):
x = arg.getvector(x)
return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol
elif arg.ismatrix(x, (3, None)):
return [
np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol
for _ in x.T
]
else:
raise ValueError('bad argument')
def RPY(cls, angles, order='zyx', unit='rad'):
"""
Create an SO(3) pure rotation from roll-pitch-yaw angles
:param angles: 3-vector of roll-pitch-yaw angles
:type angles: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
``SE3.RPY(ANGLES)`` is an SE(3) rotation defined by a 3-vector of roll, pitch, yaw angles :math:`(r, p, y)`
which correspond to successive rotations about the axes specified by ``order``:
- 'zyx' [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. Convention for a mobile robot with x-axis forward
and y-axis sideways.
- 'xyz', rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. Covention for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- 'yxz', rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. Convention for a camera with z-axis parallel
to the optic axis and x-axis parallel to the pixel rows.
If ``angles`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
correponding to the rows of angles.
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
"""
if argcheck.isvector(angles, 3):
return cls(tr.rpy2tr(angles, order=order, unit=unit))
else:
return cls([tr.rpy2tr(a, order=order, unit=unit) for a in angles])
def Eul(cls, angles, *, unit='rad'):
r"""
Create an SE(3) pure rotation from Euler angles
:param angles: 3-vector of Euler angles
:type angles: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
``SE3.Eul(ANGLES)`` is an SO(3) rotation defined by a 3-vector of Euler
angles :math:`(\phi, \theta, \psi)` which correspond to consecutive
rotations about the Z, Y, Z axes respectively.
If ``angles`` is an Nx3 matrix then the result is a sequence of
rotations each defined by Euler angles corresponding to the rows of
angles.
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`spatialmath.base.transforms3d.eul2r`
"""
if argcheck.isvector(angles, 3):
return cls(tr.eul2tr(angles, unit=unit), check=False)
else:
return cls([tr.eul2tr(a, unit=unit) for a in angles], check=False)
def __mul__(left, right):
"""
multiply quaternion
:arg left: left multiplicand
:type left: Quaternion, UnitQuaternion
:arg right: right multiplicand
:type left: Quaternion, UnitQuaternion, 3-vector, float
:return: product
:rtype: Quaternion, UnitQuaternion
:raises: ValueError
============== ============== ============== ================
Multiplicands Product
------------------------------- --------------------------------
left right type result
============== ============== ============== ================
Quaternion Quaternion Quaternion Hamilton product
Quaternion UnitQuaternion Quaternion Hamilton product
Quaternion scalar Quaternion scalar product
UnitQuaternion Quaternion Quaternion Hamilton product
UnitQuaternion UnitQuaternion UnitQuaternion Hamilton product
UnitQuaternion scalar Quaternion scalar product
UnitQuaternion 3-vector 3-vector vector rotation
============== ============== ============== ================
Any other input combinations result in a ValueError.
Note that left and right can have a length greater than 1 in which case:
==== ===== ==== ================================
left right len operation
==== ===== ==== ================================
1 1 1 ``prod = left * right``
1 N N ``prod[i] = left * right[i]``
N 1 N ``prod[i] = left[i] * right``
N N N ``prod[i] = left[i] * right[i]``
N M - ``ValueError``
==== ===== ==== ================================
A scalar of length N is a list, tuple or numpy array.
A 3-vector of length N is a 3xN numpy array, where each column is a 3-vector.
"""
if type(left) == type(right):
# quaternion * quaternion case (same class)
return left.__class__( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
elif isinstance(other, Quaternion):
# quaternion * quaternion case (different class)
return Quaternion( left._op2(right, lambda x, y: quat.qqmul(x, y) ) )
elif argcheck.isscalar(right):
# quaternion * scalar case
print('scalar * quat')
return Quaternion([right*q._A for q in left])
elif isinstance(self, UnitQuaternion) and argcheck.isvector(right,3):
# scalar * vector case
return quat.qvmul(left._A, argcheck.getvector(right,3))
else:
raise ValueError('operands to * are of different types')
return left._op2(right, lambda x, y: x @ y )
def __init__(self, m=None, c=None, I=None):
"""
Create a new spatial inertia
:param m: mass
:type m: float
:param c: centre of mass relative to link frame
:type c: 3-element array_like
:param I: inertia about the centre of mass, axes aligned with link frame
:type I: numpy.array, shape=(6,6)
- ``SpatialInertia(M, C, I)`` is a spatial inertia object for a rigid-body
with mass ``M``, centre of mass at ``C`` relative to the link frame, and an
inertia matrix ``I`` (3x3) about the centre of mass.
- ``SpatialInertia(I)`` is a spatial inertia object with a value equal
to ``I`` (6x6).
"""
if m is not None and c is not None:
assert arg.isvector(c, 3), 'c must be 3-vector'
if I is None:
I = np.zeros((3,3))
else:
assert arg.ismatrix(I, (3,3)), 'I must be 3x3 matrix'
C = tr.skew(c)
self.I = np.array([
[m * np.eye(3), m @ C.T],
[m @ C, I + m * C @ C.T]
])
elif m is None and c is None and I is not None:
assert arg.ismatrix(I, (6, 6)), 'I must be 6x6 matrix'
def __init__(self, pl=None):
"""
Plucker.Plucker Create Plucker line object
P = Plucker(P1, P2) create a Plucker object that represents
the line joining the 3D points P1 (3x1) and P2 (3x1). The direction
is from P2 to P1.
P = Plucker(X) creates a Plucker object from X (6x1) = [V,W] where
V (3x1) is the moment and W (3x1) is the line direction.
P = Plucker(L) creates a copy of the Plucker object L.
Notes:
- Planes are given by the 4-vector [a b c d] to represent ax+by+cz+d=0.\
"""
if isinstance(pl, Plucker):
self.v = pl.v
self.w = pl.w
elif arg.isvector(pl, 6):
pl = arg.getvector(pl)
self.v = pl[0:3]
self.w = pl[3:6]
else:
raise ValueError('bad argument')
def __init__(self, x=None, y=None, theta=None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if x is None and y is None and theta is None:
# SE2()
# empty constructor
self.data = [np.eye(3)]
elif x is not None:
if y is not None and theta is not None:
# SE2(x, y, theta)
self.data = [tr.trot2(theta, t=[x, y], unit=unit)]
elif y is None and theta is None:
if argcheck.isvector(x, 3):
# SE2( [x,y,theta])
self.data = [tr.trot2(x[2], t=x[:2], unit=unit)]
elif isinstance(x, np.ndarray):
if x.shape == (3, 3):
# SE2( 3x3 matrix )
self.data = [x]
elif x.shape[1] == 3:
# SE2( Nx3 )
self.data = [tr.trot2(T.theta, t=T.t) for T in x]
else:
super().arghandler(x)
else:
raise ValueError('bad arguments to constructor')
def __init__(self, arg=None, *, unit='rad', check=True):
"""
Construct new SO(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SO(2) elements if applicable, default to True
:type check: bool
:return: SO(2) rotation
:rtype: SO2 instance
- ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.
- ``SO2(θ)`` is an SO2 instance representing a rotation by ``θ`` radians. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(θ, unit='deg')`` is an SO2 instance representing a rotation by ``θ`` degrees. If ``θ`` is array_like
`[θ1, θ2, ... θN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If ``check``
is ``True`` check the matrix belongs to SO(2).
- ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix
Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).
- ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
"""
if super().arghandler(arg, check=check):
return
elif argcheck.isscalar(arg):
self.data = [tr.rot2(arg, unit=unit)]
elif argcheck.isvector(arg):
self.data = [tr.rot2(x, unit=unit) for x in argcheck.getvector(arg)]
else:
raise ValueError('bad argument to constructor')
def omega(cls, w):
assert argcheck.isvector(w, 3), 'w must be a 3-vector'
w = argcheck.getvector(w)
theta = tr.norm(w)
s = math.cos(theta / 2)
v = math.sin(theta / 2) * tr.unitvec(w)
return cls(s=s, v=v)
def __init__(self, s: Any = None, v=None, check=True, norm=True):
"""
A zero quaternion is one for which M{s^2+vx^2+vy^2+vz^2 = 1}.
A quaternion can be considered as a rotation about a vector in space where
q = cos (theta/2) sin(theta/2) <vx vy vz>
where <vx vy vz> is a unit vector.
:param s: scalar
:param v: vector
"""
super().__init__()
if s is None and v is None:
self.data = [np.array([0, 0, 0, 0])]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
self.data = [np.r_[s, argcheck.getvector(v)]]
elif argcheck.isvector(s, 4):
self.data = [argcheck.getvector(s)]
elif isinstance(s, list):
if isinstance(s[0], np.ndarray):
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, self.__class__),
s)), 'all elements of list must have same type'
self.data = [x._A for x in s]
else:
raise ValueError('incorrect list')
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
self.data = [x for x in s]
elif isinstance(s, Quaternion):
self.data = s.data
else:
raise ValueError('bad argument to Quaternion constructor')
def ctraj(T0, T1, s):
"""
Cartesian trajectory between two poses
:param T0: initial pose
:type T0: SE3
:param T1: final pose
:type T1: SE3
:return T0: smooth path from ``T0`` to ``T1``
:rtype: SE3
``ctraj(T0, T1, n)`` is a Cartesian trajectory from SE3 pose ``T0`` to
``T1`` with ``n`` points that follow a trapezoidal velocity profile along
the path. The Cartesian trajectory is an SE3 instance containing ``n``
values.
``ctraj(T0, T1, s)`` as above but the elements of ``s`` specify the
fractional distance along the path, and these values are in the
range [0 1]. The i'th point corresponds to a distance ``s[i]`` along
the path.
Examples::
>>> tg = ctraj(SE3.Rand(), SE3.Rand(), 20)
>>> len(tg)
20
Notes:
- In the second case ``s`` could be generated by a scalar trajectory
generator such as ``tpoly`` or ``lspb`` (default).
- Orientation interpolation is performed using unit-quaternion
interpolation.
Reference:
- Robotics, Vision & Control, Sec 3.1.5,
Peter Corke, Springer 2011
:seealso: :func:`~roboticstoolbox.trajectory.lspb`,
:func:`~spatialmath.unitquaternion.interp`
"""
if isinstance(s, int):
s = lspb(0, 1, s).y
elif isvector(s):
s = getvector(s)
else:
raise TypeError('bad argument for time, must be int or vector')
return T0.interp(T1, s)
def __init__(self, arg = None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if arg is None:
# empty constructor
self.data = [np.eye(2)]
elif argcheck.isvector(arg):
# SO2(value)
# SO2(list of values)
self.data = [transforms.rot2(x, unit) for x in argcheck.getvector(arg)]
else:
super().arghandler(arg)
def isvalid(v, check=True):
if argcheck.isvector(v, 3):
return True
elif argcheck.ismatrix(v, (3, 3)):
# maybe be an se(2)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[2, :] == 0): # check bottom row is zero
return False
if not tr.isskew(v[:2, :2]):
# top left 2x2is skew symmetric
return False
return True
return False
def _getq(self, q=None):
"""
Get joint coordinates (Robot superclass)
:param q: passed value, defaults to None
:type q: array_like, optional
:return: passed or value from robot state
:rtype: ndarray(n,)
"""
if q is None:
return self.q
elif isvector(q, self.n):
return getvector(q, self.n)
else:
return getmatrix(q, (None, self.n))
def __init__(self, x = None, y = None, theta = None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if x is None:
# empty constructor
self.data = [np.eye(3)]
elif all(map(lambda x: isinstance(x, (int,float)), [x, y, theta])):
# SE2(x, y, theta)
self.data = [transforms.trot2(theta, t=[x,y], unit=unit)]
elif argcheck.isvector(x) and argcheck.isvector(y) and argcheck.isvector(theta):
# SE2(xvec, yvec, tvec)
xvec = argcheck.getvector(x)
yvec = argcheck.getvector(y, dim=len(xvec))
tvec = argcheck.getvector(theta, dim=len(xvec))
self.data = [transforms.trot2(_t, t=[_x, _y]) for (_x, _y, _t) in zip(xvec, yvec, argcheck.getunit(tvec, unit))]
elif isinstance(x, np.ndarray) and y is None and theta is None:
assert x.shape[1] == 3, 'array argument must be Nx3'
self.data = [transforms.trot2(_t, t=[_x, _y], unit=unit) for (_x, _y, _t) in x]
else:
super().arghandler(x)
def contains(self, x, tol=50 * _eps):
"""
Plucker.contains Test if point is on the line
``line.contains(X)`` is true if the point X (3x1) lies on the line defined by
the Plucker object self.
"""
if arg.isvector(x, 3):
return np.linalg.norm(np.cross(x - self.pp, self.w)) < tol
elif arg.ismatrix(x, (3, None)):
return [
np.linalg.norm(np.cross(_ - self.pp, self.w)) < tol
for _ in x.T
]
else:
raise ValueError('bad argument')
def __init__(self, arg=None, *, unit='rad'):
super().__init__() # activate the UserList semantics
if arg is None:
# empty constructor
if type(self) is SO2:
self.data = [np.eye(2)]
elif argcheck.isvector(arg):
# SO2(value)
# SO2(list of values)
self.data = [tr.rot2(x, unit) for x in argcheck.getvector(arg)]
elif isinstance(arg, np.ndarray) and arg.shape == (2, 2):
self.data = [arg]
else:
super().arghandler(arg)
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:param x: translation distance along the X-axis
:type x: float
:param y: translation distance along the Y-axis
:type y: float
:param z: translation distance along the Z-axis
:type z: float
:return: 4x4 homogeneous transformation matrix
:rtype: SE3 instance
- ``SE3()`` is a null motion -- the identity matrix
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` where T is a 4x4 numpy array representing an SE(3) matrix. If ``check``
is ``True`` check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` where each Ti is a 4x4 numpy array representing an SE(3) matrix, is
an SE3 instance containing N rotations. If ``check`` is ``True``
check the matrix belongs to SE(3).
- ``SE3([X1, X2, ... XN])`` where each Xi is an SE3 instance, is an SE3 instance containing N rotations.
"""
super().__init__() # activate the UserList semantics
if x is None:
# SE3()
# empty constructor
self.data = [np.eye(4)]
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [tr.transl(x, y, z)]
elif y is None and z is None:
if argcheck.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [tr.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [tr.transl(T) for T in x]
else:
super()._arghandler(x, check=check)
else:
raise ValueError('bad argument to constructor')
def __init__(self, x=None, y=None, z=None, *, check=True):
"""
Construct new SE(3) object
:rtype: SE3 instance
There are multiple call signatures:
- ``SE3()`` is an ``SE3`` instance with one value -- a 4x4 identity
matrix which corresponds to a null motion.
- ``SE3(x, y, z)`` is a pure translation of (x,y,z)
- ``SE3(T)`` is an ``SE3`` instance with the value ``T`` which is a 4x4
numpy array representing an SE(3) matrix. If ``check`` is ``True``
check the matrix belongs to SE(3).
- ``SE3([T1, T2, ... TN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Ti`` each of which is a 4x4 NumPy array
representing an SE(3) matrix. If ``check`` is ``True`` check the
matrix belongs to SE(3).
- ``SE3([X1, X2, ... XN])`` is an ``SE3`` instance with ``N`` values
given by the elements ``Xi`` each of which is an SE3 instance.
:SymPy: supported
"""
if y is None and z is None:
# just one argument passed
if super().arghandler(x, check=check):
return
elif argcheck.isvector(x, 3):
# SE3( [x, y, z] )
self.data = [tr.transl(x)]
elif isinstance(x, np.ndarray) and x.shape[1] == 3:
# SE3( Nx3 )
self.data = [tr.transl(T) for T in x]
else:
raise ValueError('bad argument to constructor')
elif y is not None and z is not None:
# SE3(x, y, z)
self.data = [tr.transl(x, y, z)]
def transl(x, y=None, z=None):
"""
Create SE(3) pure translation, or extract translation from SE(3) matrix
:param x: translation along X-axis
:type x: float
:param y: translation along Y-axis
:type y: float
:param z: translation along Z-axis
:type z: float
:return: 4x4 homogeneous transformation matrix
:rtype: numpy.ndarray, shape=(4,4)
Create a translational SE(3) matrix:
- ``T = transl( X, Y, Z )`` is an SE(3) homogeneous transform (4x4) representing a
pure translation of X, Y and Z.
- ``T = transl( V )`` as above but the translation is given by a 3-element
list, dict, or a numpy array, row or column vector.
Extract the translational part of an SE(3) matrix:
- ``P = TRANSL(T)`` is the translational part of a homogeneous transform T as a
3-element numpy array.
:seealso: :func:`~spatialmath.base.transforms2d.transl2`
"""
if np.isscalar(x):
T = np.identity(4)
T[:3, 3] = [x, y, z]
return T
elif argcheck.isvector(x, 3):
T = np.identity(4)
T[:3, 3] = argcheck.getvector(x, 3, out='array')
return T
elif argcheck.ismatrix(x, (4, 4)):
return x[:3, 3]
else:
ValueError('bad argument')
def __init__(self, arg=None, *, unit='rad', check=True):
"""
Construct new SO(2) object
:param unit: angular units 'deg' or 'rad' [default] if applicable
:type unit: str, optional
:param check: check for valid SO(2) elements if applicable, default to True
:type check: bool
:return: SO(2) rotation
:rtype: SO2 instance
- ``SO2()`` is an SO2 instance representing a null rotation -- the identity matrix.
- ``SO2(theta)`` is an SO2 instance representing a rotation by ``theta`` radians. If ``theta`` is array_like
`[theta1, theta2, ... thetaN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(theta, unit='deg')`` is an SO2 instance representing a rotation by ``theta`` degrees. If ``theta`` is array_like
`[theta1, theta2, ... thetaN]` then an SO2 instance containing a sequence of N rotations.
- ``SO2(R)`` is an SO2 instance with rotation described by the SO(2) matrix R which is a 2x2 numpy array. If ``check``
is ``True`` check the matrix belongs to SO(2).
- ``SO2([R1, R2, ... RN])`` is an SO2 instance containing a sequence of N rotations, each described by an SO(2) matrix
Ri which is a 2x2 numpy array. If ``check`` is ``True`` then check each matrix belongs to SO(2).
- ``SO2([X1, X2, ... XN])`` is an SO2 instance containing a sequence of N rotations, where each Xi is an SO2 instance.
"""
super().__init__() # activate the UserList semantics
if arg is None:
# empty constructor
if type(self) is SO2:
self.data = [np.eye(2)]
elif argcheck.isvector(arg):
# SO2(value)
# SO2(list of values)
self.data = [tr.rot2(x, unit) for x in argcheck.getvector(arg)]
elif isinstance(arg, np.ndarray) and arg.shape == (2, 2):
self.data = [arg]
else:
super()._arghandler(arg, check=check)
def RPY(cls, angles, *, order='zyx', unit='rad'):
r"""
Create an SE(3) pure rotation from roll-pitch-yaw angles
:param 𝚪: roll-pitch-yaw angles
:type 𝚪: array_like or numpy.ndarray with shape=(N,3)
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param order: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type order: str
:return: SE(3) matrix
:rtype: SE3 instance
``SE3.RPY(𝚪)`` is an SE(3) rotation defined by a 3-vector of roll,
pitch, yaw angles :math:`\Gamma=(r, p, y)` which correspond to
successive rotations about the axes specified by ``order``:
- ``'zyx'`` [default], rotate by yaw about the z-axis, then by pitch about the new y-axis,
then by roll about the new x-axis. This is the **convention** for a mobile robot with x-axis forward
and y-axis sideways.
- ``'xyz'``, rotate by yaw about the x-axis, then by pitch about the new y-axis,
then by roll about the new z-axis. This is the **convention** for a robot gripper with z-axis forward
and y-axis between the gripper fingers.
- ``'yxz'``, rotate by yaw about the y-axis, then by pitch about the new x-axis,
then by roll about the new z-axis. This is the **convention** for a camera with z-axis parallel
to the optical axis and x-axis parallel to the pixel rows.
If ``𝚪`` is an Nx3 matrix then the result is a sequence of rotations each defined by RPY angles
corresponding to the rows of ``𝚪``.
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.base.transforms3d.rpy2r`
:SymPy: supported
"""
if argcheck.isvector(angles, 3):
return cls(tr.rpy2tr(angles, order=order, unit=unit), check=False)
else:
return cls([tr.rpy2tr(a, order=order, unit=unit) for a in angles],
check=False)
def isvalid(v, check=True):
"""
Test if matrix is valid twist
:param x: array to test
:type x: numpy.ndarray
:return: true of the matrix is a 6-vector or a 4x4 se(3) element
:rtype: bool
"""
if argcheck.isvector(v, 6):
return True
elif argcheck.ismatrix(v, (4, 4)):
# maybe be an se(3)
if not all(v.diagonal() == 0): # check diagonal is zero
return False
if not all(v[3, :] == 0): # check bottom row is zero
return False
if not tr.isskew(v[:3, :3]):
# top left 3x3 is skew symmetric
return False
return True
return False
def e2h(v):
"""
Convert from Euclidean to homogeneous form
:param v: Euclidean vector or matrix
:type v: array_like
:return: homogeneous vector
:rtype: numpy.ndarray
- If ``v`` is an array, shape=(N,), return an array shape=(N+1,) where a value of 1 has
been appended
- If ``v`` is a matrix, shape=(N,M), return a matrix shape=(N+1,N), where each column has
been appended with a value of 1, ie. a row of ones has been appended to the matrix.
:seealso: e2h
"""
if argcheck.isvector(v):
# dealing with shape (N,) array
v = argcheck.getvector(v)
return np.r_[v, 1]
elif isinstance(v, np.ndarray) and len(v.shape) == 2:
# dealing with matrix
return np.vstack([v, np.ones((1, v.shape[1]))])
def h2e(v):
"""
Convert from homogeneous to Euclidean form
:param v: homogeneous vector or matrix
:type v: array_like
:return: Euclidean vector
:rtype: numpy.ndarray
- If ``v`` is an array, shape=(N,), return an array shape=(N-1,) where the elements have
all been scaled by the last element of ``v``.
- If ``v`` is a matrix, shape=(N,M), return a matrix shape=(N-1,N), where each column has
been scaled by its last element.
:seealso: e2h
"""
if argcheck.isvector(v):
# dealing with shape (N,) array
v = argcheck.getvector(v)
return v[0:-1] / v[-1]
elif isinstance(v, np.ndarray) and len(v.shape) == 2:
# dealing with matrix
return v[:-1, :] / matlib.repmat(v[-1, :], v.shape[0] - 1, 1)
