def trplot(self, end, start=None, **kwargs):
"""
Define the transform to animate
:param end: the final pose SO(3) or SE(3) to display as a coordinate frame
:type end: numpy.ndarray, shape=(3,3) or (4,4)
:param start: the initial pose SO(3) or SE(3) to display as a coordinate frame, defaults to null
:type start: numpy.ndarray, shape=(3,3) or (4,4)
:param start: an
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
# stash the final value
if tr.isrot(end):
self.end = tr.r2t(end)
else:
self.end = end
if start is None:
self.start = np.identity(4)
else:
if tr.isrot(start):
self.start = tr.r2t(start)
else:
self.start = start
# draw axes at the origin
tr.trplot(self.start, axes=self, block=None, **kwargs)
def SO3(cls, R, check=True):
if isinstance(R, SO3):
R = R.A
elif base.isrot(R, check=check):
pass
else:
raise ValueError('expecting SO3 or rotation matrix')
return cls(base.r2t(R))
def trplot(self, end, start=None, **kwargs):
"""
Define the transform to animate
:param end: the final pose SE(3) or SO(3) to display as a coordinate frame
:type end: ndarray(4,4) or ndarray(3,3)
:param start: the initial pose SE(3) or SO(3) to display as a coordinate frame, defaults to null
:type start: ndarray(4,4) or ndarray(3,3)
:param start: an
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
if not isinstance(end, (np.ndarray, np.generic)) and isinstance(
end, Iterable):
try:
if len(end) == 1:
end = end[0]
elif len(end) >= 2:
self.trajectory = end
except TypeError:
# a generator has no len()
self.trajectory = end
# stash the final value
if base.isrot(end):
self.end = base.r2t(end)
else:
self.end = end
if start is None:
self.start = np.identity(4)
else:
if base.isrot(start):
self.start = base.r2t(start)
else:
self.start = start
# draw axes at the origin
base.trplot(self.start, axes=self, **kwargs)
def isvalid(x, check=True):
"""
Test if matrix is valid SO(3)
:param x: matrix to test
:type x: numpy.ndarray
:return: ``True`` if the matrix is a valid element of SO(3), ie. it is a 3x3
orthonormal matrix with determinant of +1.
:rtype: bool
:seealso: :func:`~spatialmath.base.transform3d.isrot`
"""
return base.isrot(x, check=True)
def trplot(self, T, **kwargs):
"""
Define the transform to animate
:param T: an SO(3) or SE(3) pose to be displayed as coordinate frame
:type: numpy.ndarray, shape=(3,3) or (4,4)
Is polymorphic with ``base.trplot`` and accepts the same parameters.
This sets up the animation but doesn't execute it.
:seealso: :func:`run`
"""
# stash the final value
if tr.isrot(T):
self.T = tr.r2t(T)
else:
self.T = T
# draw axes at the origin
tr.trplot(np.eye(4), axes=self, **kwargs)
def Rt(cls, R, t, check=True):
"""
Create an SE(3) from rotation and translation
:param R: rotation
:type R: SO3 or ndarray(3,3)
:param t: translation
:type t: array_like(3)
:param check: check rotation validity, defaults to True
:type check: bool, optional
:raises ValueError: bad rotation matrix
:return: SE(3) matrix
:rtype: SE3 instance
"""
if isinstance(R, SO3):
R = R.A
elif base.isrot(R, check=check):
pass
else:
raise ValueError('expecting SO3 or rotation matrix')
return cls(base.rt2tr(R, t))
def __init__(self, s=None, v=None, norm=True, check=True):
"""
Construct a UnitQuaternion object
:arg norm: explicitly normalize the quaternion [default True]
:type norm: bool
:arg check: explicitly check dimension of passed lists [default True]
:type check: bool
:return: new unit uaternion
:rtype: UnitQuaternion
:raises: ValueError
Single element quaternion:
- ``UnitQuaternion()`` constructs the identity quaternion 1<0,0,0>
- ``UnitQuaternion(s, v)`` constructs a unit quaternion with specified
real ``s`` and ``v`` vector parts. ``v`` is a 3-vector given as a
list, tuple, numpy.ndarray
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
elements from ``v`` which is a 4-vector given as a list, tuple, numpy.ndarray
- ``UnitQuaternion(R)`` constructs a unit quaternion from an orthonormal
rotation matrix given as a 3x3 numpy.ndarray. If ``check`` is True
test the matrix for orthogonality.
Multi-element quaternion:
- ``UnitQuaternion(V)`` constructs a unit quaternion list with specified
elements from ``V`` which is an Nx4 numpy.ndarray, each row is a
quaternion. If ``norm`` is True explicitly normalize each row.
- ``UnitQuaternion(L)`` constructs a unit quaternion list from a list
of 4-element numpy.ndarrays. If ``check`` is True test each element
of the list is a 4-vector. If ``norm`` is True explicitly normalize
each vector.
"""
if s is None and v is None:
self.data = [ quat.eye() ]
elif argcheck.isscalar(s) and argcheck.isvector(v,3):
q = np.r_[ s, argcheck.getvector(v) ]
if norm:
q = quat.unit(q)
self.data = [q]
elif argcheck.isvector(s,4):
print('uq constructor 4vec')
q = argcheck.getvector(s)
# if norm:
# q = quat.unit(q)
print(q)
self.data = [q]
elif type(s) is list:
if check:
assert argcheck.isvectorlist(s,4), 'list must comprise 4-vectors'
if norm:
s = [quat.unit(q) for q in s]
self.data = s
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
if norm:
self.data = [quat.norm(x) for x in s]
else:
self.data = [x for x in s]
elif tr.isrot(s, check=check):
self.data = [ quat.r2q(s) ]
else:
raise ValueError('bad argument to UnitQuaternion constructor')
def __init__(self, s: Any = None, v=None, norm=True, check=True):
"""
Construct a UnitQuaternion object
:arg norm: explicitly normalize the quaternion [default True]
:type norm: bool
:arg check: explicitly check dimension of passed lists [default True]
:type check: bool
:return: new unit uaternion
:rtype: UnitQuaternion
:raises: ValueError
Single element quaternion:
- ``UnitQuaternion()`` constructs the identity quaternion 1<0,0,0>
- ``UnitQuaternion(s, v)`` constructs a unit quaternion with specified
real ``s`` and ``v`` vector parts. ``v`` is a 3-vector given as a
list, tuple, numpy.ndarray
- ``UnitQuaternion(v)`` constructs a unit quaternion with specified
elements from ``v`` which is a 4-vector given as a list, tuple, numpy.ndarray
- ``UnitQuaternion(R)`` constructs a unit quaternion from an orthonormal
rotation matrix given as a 3x3 numpy.ndarray. If ``check`` is True
test the matrix for orthogonality.
- ``UnitQuaternion(X)`` constructs a unit quaternion from the rotational
part of ``X`` which is SO3 or SE3 instance. If len(X) > 1 then
the resulting unit quaternion is of the same length.
Multi-element quaternion:
- ``UnitQuaternion(V)`` constructs a unit quaternion list with specified
elements from ``V`` which is an Nx4 numpy.ndarray, each row is a
quaternion. If ``norm`` is True explicitly normalize each row.
- ``UnitQuaternion(L)`` constructs a unit quaternion list from a list
of 4-element numpy.ndarrays. If ``check`` is True test each element
of the list is a 4-vector. If ``norm`` is True explicitly normalize
each vector.
"""
super().__init__()
if s is None and v is None:
self.data = [quat.eye()]
elif argcheck.isscalar(s) and argcheck.isvector(v, 3):
# UnitQuaternion(s, v) s is scalar, v is 3-vector
q = np.r_[s, argcheck.getvector(v)]
if norm:
q = quat.unit(q)
self.data = [q]
elif argcheck.isvector(s, 4):
# UnitQuaternion(q) q is 4-vector
q = argcheck.getvector(s)
if norm:
s = quat.unit(s)
self.data = [s]
elif isinstance(s, list):
# UnitQuaternion(list)
if isinstance(s[0], np.ndarray):
# list of 4-vectors
if check:
assert argcheck.isvectorlist(
s, 4), 'list must comprise 4-vectors'
self.data = s
elif isinstance(s[0], p3d.SO3):
# list of SO3/SE3
self.data = [quat.r2q(x.R) for x in s]
elif isinstance(s[0], self.__class__):
# possibly a list of objects of same type
assert all(map(lambda x: isinstance(x, type(self)),
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, p3d.SO3):
# UnitQuaternion(x) x is SO3 or SE3
self.data = [quat.r2q(x.R) for x in s]
elif isinstance(s, np.ndarray) and tr.isrot(s, check=check):
# UnitQuaternion(R) R is 3x3 rotation matrix
self.data = [quat.r2q(s)]
elif isinstance(s, np.ndarray) and tr.ishom(s, check=check):
# UnitQuaternion(T) T is 4x4 homogeneous transformation matrix
self.data = [quat.r2q(tr.t2r(s))]
elif isinstance(s, np.ndarray) and s.shape[1] == 4:
if norm:
self.data = [quat.qnorm(x) for x in s]
else:
self.data = [x for x in s]
elif isinstance(s, UnitQuaternion):
# UnitQuaternion(Q) Q is a UnitQuaternion instance, clone it
self.data = s.data
else:
raise ValueError('bad argument to UnitQuaternion constructor')
def r2q(R, check=False, tol=100):
"""
Convert SO(3) rotation matrix to unit-quaternion
:arg R: SO(3) rotation matrix
:type R: ndarray(3,3)
:param check: check validity of rotation matrix, default False
:type check: bool
:param tol: tolerance in units of eps
:type tol: float
:return: unit-quaternion
:rtype: ndarray(4)
:raises ValueError: for non SO(3) argument
Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.
.. runblock:: pycon
>>> from spatialmath.base import r2q, qprint, rotx
>>> R = rotx(90, 'deg') # rotation of 90deg about x-axis
>>> print(R)
>>> qprint(r2q(R))
.. warning:: There is no check that the passed matrix is a valid rotation matrix.
.. note:: Scalar part is always positive.
:seealso: :func:`q2r`
"""
if not base.isrot(R, check=check, tol=tol):
raise ValueError("Argument must be a valid SO(3) matrix")
qs = math.sqrt(max(0, np.trace(R) + 1)) / 2.0 # scalar part
kx = R[2, 1] - R[1, 2] # Oz - Ay
ky = R[0, 2] - R[2, 0] # Ax - Nz
kz = R[1, 0] - R[0, 1] # Ny - Ox
if (R[0, 0] >= R[1, 1]) and (R[0, 0] >= R[2, 2]):
kx1 = R[0, 0] - R[1, 1] - R[2, 2] + 1 # Nx - Oy - Az + 1
ky1 = R[1, 0] + R[0, 1] # Ny + Ox
kz1 = R[2, 0] + R[0, 2] # Nz + Ax
add = (kx >= 0)
elif R[1, 1] >= R[2, 2]:
kx1 = R[1, 0] + R[0, 1] # Ny + Ox
ky1 = R[1, 1] - R[0, 0] - R[2, 2] + 1 # Oy - Nx - Az + 1
kz1 = R[2, 1] + R[1, 2] # Oz + Ay
add = (ky >= 0)
else:
kx1 = R[2, 0] + R[0, 2] # Nz + Ax
ky1 = R[2, 1] + R[1, 2] # Oz + Ay
kz1 = R[2, 2] - R[0, 0] - R[1, 1] + 1 # Az - Nx - Oy + 1
add = (kz >= 0)
if add:
kx = kx + kx1
ky = ky + ky1
kz = kz + kz1
else:
kx = kx - kx1
ky = ky - ky1
kz = kz - kz1
kv = np.r_[kx, ky, kz]
nm = np.linalg.norm(kv)
if abs(nm) < tol * _eps:
return eye()
else:
return np.r_[qs, (math.sqrt(1.0 - qs**2) / nm) * kv]
def r2q(R, check=False, tol=100, order='sxyz'):
"""
Convert SO(3) rotation matrix to unit-quaternion
:arg R: SO(3) rotation matrix
:type R: ndarray(3,3)
:param check: check validity of rotation matrix, default False
:type check: bool
:param tol: tolerance in units of eps
:type tol: float
:param order: the order of the returned quaternion. Must be 'sxyz' or
'xyzs'. Defaults to 'sxyz'.
:type order: str
:return: unit-quaternion as Euler parameters
:rtype: ndarray(4)
:raises ValueError: for non SO(3) argument
Returns a unit-quaternion corresponding to the input SO(3) rotation matrix.
.. runblock:: pycon
>>> from spatialmath.base import r2q, qprint, rotx
>>> R = rotx(90, 'deg') # rotation of 90deg about x-axis
>>> print(R)
>>> qprint(r2q(R))
.. warning:: There is no check that the passed matrix is a valid rotation matrix.
.. note::
- Scalar part is always positive
- implements Cayley's method
:reference:
- Sarabandi, S., and Thomas, F. (March 1, 2019).
"A Survey on the Computation of Quaternions From Rotation Matrices."
ASME. J. Mechanisms Robotics. April 2019; 11(2): 021006.
`doi.org/10.1115/1.4041889 <https://doi.org/10.1115/1.4041889>`_
:seealso: :func:`q2r`
"""
if not base.isrot(R, check=check, tol=tol):
raise ValueError("Argument must be a valid SO(3) matrix")
t12p = (R[0, 1] + R[1, 0])**2
t13p = (R[0, 2] + R[2, 0])**2
t23p = (R[1, 2] + R[2, 1])**2
t12m = (R[0, 1] - R[1, 0])**2
t13m = (R[0, 2] - R[2, 0])**2
t23m = (R[1, 2] - R[2, 1])**2
d1 = (R[0, 0] + R[1, 1] + R[2, 2] + 1)**2
d2 = (R[0, 0] - R[1, 1] - R[2, 2] + 1)**2
d3 = (-R[0, 0] + R[1, 1] - R[2, 2] + 1)**2
d4 = (-R[0, 0] - R[1, 1] + R[2, 2] + 1)**2
e0 = math.sqrt(d1 + t23m + t13m + t12m) / 4.0
e1 = math.sqrt(t23m + d2 + t12p + t13p) / 4.0
e2 = math.sqrt(t13m + t12p + d3 + t23p) / 4.0
e3 = math.sqrt(t12m + t13p + t23p + d4) / 4.0
# transfer sign from rotation element differences
if R[2, 1] < R[1, 2]:
e1 = -e1
if R[0, 2] < R[2, 0]:
e2 = -e2
if R[1, 0] < R[0, 1]:
e3 = -e3
if order == 'sxyz':
return np.r_[e0, e1, e2, e3]
elif order == 'xyzs':
return np.r_[e1, e2, e3, e0]
else:
raise ValueError("order is invalid, must be 'sxyz' or 'xyzs'")
