def OA(cls, o, a):
"""
Create SO(3) rotation from two vectors
:param o: 3-vector parallel to Y- axis
:type o: array_like
:param a: 3-vector parallel to the Z-axis
:type o: array_like
:return: 3x3 rotation matrix
:rtype: SO3 instance
``SO3.OA(O, A)`` is an SO(3) rotation defined in terms of
vectors parallel to the Y- and Z-axes of its reference frame. In robotics these axes are
respectively called the orientation and approach vectors defined such that
R = [N O A] and N = O x A.
Notes:
- The A vector is the only guaranteed to have the same direction in the resulting
rotation matrix
- O and A do not have to be unit-length, they are normalized
- O and A do not have to be orthogonal, so long as they are not parallel
- The vectors O and A are parallel to the Y- and Z-axes of the equivalent coordinate frame.
:seealso: :func:`spatialmath.base.transforms3d.oa2r`
"""
return cls(quat.r2q(tr.oa2r(angles, unit=unit)), check=False)
def RPY(cls, angles, *, order='zyx', unit='rad'):
"""
Create an SO(3) rotation from roll-pitch-yaw angles
:param angles: 3-vector of roll-pitch-yaw angles
:type angles: array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param unit: rotation order: 'zyx' [default], 'xyz', or 'yxz'
:type unit: str
:return: 3x3 rotation matrix
:rtype: SO3 instance
``SO3.RPY(ANGLES)`` is an SO(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.
:seealso: :func:`~spatialmath.pose3d.SE3.rpy`, :func:`~spatialmath.pose3d.SE3.RPY`, :func:`spatialmath.base.transforms3d.rpy2r`
"""
return cls(quat.r2q(tr.rpy2r(angles, unit=unit, order=order)),
check=False)
def Eul(cls, angles, *, unit='rad'):
"""
Create an SO(3) rotation from Euler angles
:param angles: 3-vector of Euler angles
:type angles: array_like
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:return: 3x3 rotation matrix
:rtype: SO3 instance
``SO3.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.
:seealso: :func:`~spatialmath.pose3d.SE3.eul`, :func:`~spatialmath.pose3d.SE3.Eul`, :func:`spatialmath.base.transforms3d.eul2r`
"""
return cls(quat.r2q(tr.eul2r(angles, unit=unit)), check=False)
def AngVec(cls, theta, v, *, unit='rad'):
"""
Create an SO(3) rotation matrix from rotation angle and axis
:param theta: rotation
:type theta: float
:param unit: angular units: 'rad' [default], or 'deg'
:type unit: str
:param v: rotation axis, 3-vector
:type v: array_like
:return: 3x3 rotation matrix
:rtype: SO3 instance
``SO3.AngVec(THETA, V)`` is an SO(3) rotation defined by
a rotation of ``THETA`` about the vector ``V``.
Notes:
- If ``THETA == 0`` then return identity matrix.
- If ``THETA ~= 0`` then ``V`` must have a finite length.
:seealso: :func:`~spatialmath.pose3d.SE3.angvec`, :func:`spatialmath.base.transforms3d.angvec2r`
"""
return cls(quat.r2q(tr.angvec2r(theta, v, unit=unit)), check=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 Omega(cls, w):
return cls(quat.r2q(tr.angvec2r(tr.norm(w), tr.unitvec(w))),
check=False)
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')
