def _import(self, value, check=True):
if isinstance(value, np.ndarray) and self.isvalid(value, check=check):
if value.shape == (4, 4):
# it's an se(3)
return base.vexa(value)
elif value.shape == (6, ):
# it's a twist vector
return value
elif base.ishom(value, check=check):
return base.trlog(value, twist=True, check=False)
raise TypeError('bad type passed')
def isvalid(x, check=True):
"""
Test if matrix is a valid SE(3)
:param x: matrix to test
:type x: numpy.ndarray
:return: ``True`` if the matrix is 4x4 and a valid element of SE(3), ie. it
is a valid homogeneous transformation matrix.
:rtype: bool
:seealso: :func:`~spatialmath.base.transforms3d.ishom`
"""
return base.ishom(x, check=check)
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 = [quat.unit(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], p3d.SO3):
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):
self.data = [quat.r2q(s.R)]
elif isinstance(s, np.ndarray) and tr.isrot(s, check=check):
self.data = [quat.r2q(s)]
elif isinstance(s, np.ndarray) and tr.ishom(s, check=check):
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):
self.data = s.data
else:
raise ValueError('bad argument to UnitQuaternion constructor')