def unit(self):
"""Return an equivalent unit quaternion
Code retrieved from: https://github.com/petercorke/robotics-toolbox-python/blob/master/robot/Quaternion.py
Original authors: Luis Fernando Lara Tobar and Peter Corke
@rtype: quaternion
@return: equivalent unit quaternion
"""
return UnitQuaternion( [quat.unit(q._A) for q in self], norm=False)
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')