def __init__(self, *args):
"""Initializes the orientation in any of the following ways:
u, theta: (array_like, float) Axis-angle. Represents a
single rotation by a given angle `theta` in radians
about a fixed axis represented by the unit vector `u`.
rot_vec: (array_like) Rotation vector. Corresponds
to the 3-element representation of the axis-angle,
i.e. `rot_vec = theta * u`.
rotm: (array_like) a `3x3` orthogonal rotation matrix.
quat: (Quaternion or UnitQuaternion) a unit quaternion. The
quaternion is normalized if it's a `Quaternion` object.
If no arguments are passed, an identity Orientation is initialized.
"""
if len(args) == 0:
self._quat = quaternion.UnitQuaternion()
elif len(args) == 1:
if utils.is_iterable(args[0]):
if len(args[0]) == 3 and utils.is_1d_iterable(
args[0]): # rot_vec
self._rot_vec = np.asarray(args[0], dtype="float64")
self._rotvec2quat()
elif len(args[0]) == 3 and np.asarray(
args[0]).shape == (3, 3): # rotm
rotm = np.asarray(args[0], dtype="float64")
self._rotm2quat(rotm)
else:
raise ValueError(
"[!] Could not parse constructor arguments.")
elif isinstance(args[0], quaternion.Quaternion): # quat
self._quat = quaternion.UnitQuaternion(args[0])
else:
raise ValueError("[!] Expecting array_like or Quaternion.")
elif len(args) == 2:
if utils.is_iterable(args[0]) and isinstance(
args[1], (int, float)): # axang
if len(args[0]) != 3:
raise ValueError("[!] Axis must be length 3 array_like.")
self._u = np.asarray(args[0], dtype="float64")
self._theta = float(args[1])
if np.isclose(self._theta,
0.0): # no unique axis, default to i
self._u = np.array([1, 0, 0], dtype="float64")
if not np.isclose(np.dot(self._u, self._u), 1.0):
self._u /= np.linalg.norm(self._u)
self._axang2quat()
else:
raise ValueError("[!] Expecting (axis, angle) tuple.")
else:
raise ValueError("[!] Incorrect number of arguments.")
def __add__(self, other):
"""Left quaternion addition.
Addition can be performed with:
- A ``Quaternion`` or ``quaternion_like`` object.
- An ``array_like`` of length 3 to represent a purely-complex quaternion.
- A :obj:`float` to represent a purely-real quaternion.
"""
if isinstance(other, Quaternion):
return self.__class__(self._q + other._q)
elif utils.is_iterable(other):
if len(other) == 3:
return self.__class__(
self._q[0],
self._q[1:] + other,
)
elif len(other) == 4:
return self.__class__(self._q + other)
else:
raise ValueError(
"[!] Expecting length 3 or length 4 array_like.")
elif isinstance(other, (int, float)):
return self.__class__(
self._q[0] + float(other),
self._q[1:],
)
else:
raise NotImplemented # noqa F901
def __rmul__(self, other):
"""Quaternion multiplication.
"""
if isinstance(other, Quaternion):
return self.__class__(
(other._q[0] * self._q[0]) - np.dot(other._q[1:], self._q[1:]),
(other._q[1:] * self._q[0]) + (self._q[1:] * other._q[0]) +
np.cross(other._q[1:], self._q[1:]),
)
elif utils.is_iterable(other):
other = np.asarray(other, dtype="float64")
if len(other) == 3:
return self.__class__(
-np.dot(other, self._q[1:]),
(other * self._q[0]) + np.cross(other, self._q[1:]),
)
elif len(other) == 4:
return self.__class__(
(other[0] * self._q[0]) - np.dot(other[1:], self._q[1:]),
(other[1:] * self._q[0]) + (self._q[1:] * other[0]) +
np.cross(other[1:], self._q[1:]),
)
else:
raise ValueError(
"[!] Expecting length 3 or length 4 array_like.")
elif isinstance(other, (int, float)): # scalar multiplication
return self.__class__(
float(other) * self._q[0],
float(other) * self._q[1:],
)
else:
raise NotImplemented
def __mul__(self, other):
"""Orientation multiplication.
"""
if utils.is_iterable(other):
if utils.is_batch_vectors(other) or utils.is_single_vector(other):
return np.dot(self.rotm, np.asarray(other, dtype="float64"))
else:
if all(isinstance(x, Orientation)
for x in other): # batch of orientations
accessor = lambda x: x._quat
elif all(
isinstance(x, quaternion.UnitQuaternion)
for x in other): # batch of quaternions
accessor = lambda x: x
else:
raise NotImplemented
quat_prod = self._quat
for o in other:
quat_prod = quat_prod * accessor(o)
quat_prod.fnormalize(True)
return self.__class__(quat_prod)
else:
if isinstance(other, Orientation):
other = other._quat
elif isinstance(other, quaternion.UnitQuaternion):
other = other
else:
raise NotImplemented
quat_prod = self._quat * other
quat_prod.fnormalize(True)
return self.__class__(quat_prod)
def _check_input(self, arr):
"""Checks that the input is a `3x3` array_like.
"""
if is_iterable(arr):
if np.asarray(arr).shape == (3, 3):
return True
return False
return False
def __init__(self, *args):
"""Initializes the quaternion in any of the following ways:
Args:
s (:obj:`float`): The real component. Assumes complex component is zero.
v (``array_like``): The complex component. Assumes real component is zero.
q (``array_like``): An iterable of 4 floats defining the real and complex components.
You can also provide them separately as two function arguments ``s, v``.
quat (:obj:`Quaternion`): An instance of this class. This is useful for copying
a quaternion or creating a ``UnitQuaternion`` object from an existing quaternion.
If no arguments are passed, a zero quaternion is initialized.
"""
self._q = np.empty(4, dtype="float64")
if len(args) == 0:
self._q = np.zeros(4, dtype="float64")
elif len(args) == 1:
if utils.is_iterable(args[0]):
if len(args[0]) == 4: # [s, x, y, z]
self._q[0] = float(args[0][0])
self._q[1:] = args[0][1:]
elif len(args[0]) == 3: # pure quaternion
self._q[0] = 0.
self._q[1:] = args[0]
else:
raise ValueError(
"[!] Expecting a length 4 array_like or pure quaternion."
)
elif isinstance(args[0], Quaternion):
self._q = np.array(args[0]._q, dtype="float64")
else: # real quaternion
self._q[0] = float(args[0])
self._q[1:] = np.zeros(3, dtype="float64")
elif len(args) == 2: # s, v
if utils.is_iterable(args[0]):
raise ValueError("[!] `s` must be a float.")
self._q[0] = float(args[0])
if not utils.is_iterable(args[1]):
raise ValueError("[!] `v` must be array_like.")
self._q[1:] = args[1]
else:
raise ValueError("[!] Incorrect number of arguments.")
def __mul__(self, other):
"""Unit-quaternion multiplication.
Multiplication is interpreted as a:
- Rotation if ``other`` is a length 3 array_like.
- Rotation if ``other`` is a pure quaternion.
- Regular quaternion multiplication if ``other`` is a non-pure
quaternion.
Note: quaternion-scalar multiplication is not supported
because it has no effect on the unit-quaternion. This
is because we normalize to unit-norm in the constructor.
We use Rodrigues' formula rather than quaternion conjugation
``q * p * q.inv()`` since it is more efficient.
Returns:
An :obj:`ndarray`, ``Quaternion`` or ``UnitQuaternion``:
- :obj:`ndarray`: The rotated vector if the multiplication
was interpreted as a rotation.
- ``Quaternion``: The quaternion product if the multiplication
was interpreted as regular quaternion multiplication.
- ``UnitQuaternion``: If both operands were unit-quaternions.
"""
if utils.is_iterable(other):
other = np.asarray(other, dtype=np.float64)
if len(other) == 3: # vector rotation
return self._rodrigues_rotation(other)
elif len(other) == 4:
if other[0] == 0: # pure quaternion, i.e. vector rotation
return self._rodrigues_rotation(other[1:])
return super().__mul__(
other) # regular quaternion multiplication
elif other.ndim == 2 and other.shape[0] > 1 and other.shape[
1] == 3: # batch of vectors
res = np.empty_like(other)
for i in range(other.shape[0]):
res[i] = self._rodrigues_rotation(other[i])
return res
else:
raise ValueError("[!] array_like is not compatible.")
elif isinstance(other, UnitQuaternion):
return super().__mul__(
other) # the product of unit-quaternions is a unit-quaternion
elif isinstance(other, Quaternion):
if other.is_pure(): # pure quaternion, i.e. vector rotation
return Quaternion(self._rodrigues_rotation(other._q[1:]))
# regular quaternion multiplication
res = super().__mul__(other)
return Quaternion(res.array)
else:
raise NotImplemented # noqa F901
def __mul__(self, other):
"""Quaternion multiplication.
Interpreted as quaternion-quaternion or scalar-quaternion multiplication.
Args:
other (``quaternion_like`` or :obj:`float`): The quaternion to
multiply with or scalar to scale by.
Returns:
``Quaternion``
"""
if isinstance(other, Quaternion):
return self.__class__(
(self._q[0] * other._q[0]) - self._q[1:] @ other._q[1:],
(self._q[0] * other._q[1:]) + (other._q[0] * self._q[1:]) +
np.cross(self._q[1:], other._q[1:]),
)
elif utils.is_iterable(other):
other = np.asarray(other, dtype=np.float64)
if len(other) == 3:
return self.__class__(
-self._q[1:] @ other,
(self._q[0] * other) + np.cross(self._q[1:], other),
)
elif len(other) == 4:
return self.__class__(
(self._q[0] * other[0]) - self._q[1:] @ other[1:],
(self._q[0] * other[1:]) + (other[0] * self._q[1:]) +
np.cross(self._q[1:], other[1:]),
)
else:
raise ValueError(
"[!] Expecting length 3 or length 4 array_like.")
elif isinstance(other, (int, float)): # scalar multiplication
return self.__class__(
float(other) * self._q[0],
float(other) * self._q[1:],
)
else:
raise NotImplemented # noqa F901
def slerp(self, other, t):
"""Spherical linear interpolation between two unit-quaternions.
Args:
other (`quaternion_like`): The quaternion to interpolate with.
t (:obj:`float`): The interpolation parameter between 0 and 1.
"""
if not isinstance(other, Quaternion):
other = Quaternion(other)
other = other.normalize() # a valid rotation must have unit-norm
if self.dot(other) < 0.0: # ensure shortest path
other = -other # reverse quaternion
if utils.is_iterable(t):
t = np.clip(t, 0, 1)
res = []
for t_ in t:
res.append((other * self.inv())**t_ * self)
return np.asarray(res)
elif isinstance(t, (int, float)):
return (other * self.inv())**float(t) * self
else:
raise ValueError("[!] `t` must be a float or array of floats.")
def __init__(self, *args):
"""Initializes the 6-DOF pose in any of the following ways:
pose: (array_like) A list of 6 floats. The first three
represent the 3-D position [x, y, z] and the last
three represent the 3-D orientation in rotation
vector format [rx, ry, rz].
transform: (array_like) A 4x4 transform representing
the 3-D position and 3-D orientation.
position, orientation: (array_like, `Orientation`) The
first argument is an array_like of length 3 representing
the 3-D position [x, y, z]. The second argument is an
instance of the `Orientation` class representing the
3-D orientation.
position, rot_vec: (array_like, array_like) The first
argument is an array_like of length 3 representing
the 3-D position [x, y, z]. The second argument
is an array_like of length 3 specifying the 3-D
orientation in rotation vector format [rx, ry, rz].
position, rotm: (array_like, array_like) The first
argument is an array_like of length 3 representing
the 3-D position [x, y, z]. The second argument
is an array_like representing the 3-D orientation
in rotation matrix format.
position, quat: (array_like, UnitQuaternion) The first
argument is an array_like of length 3 representing
the 3-D position [x, y, z]. The second argument
is an array_like representing the 3-D orientation
in unit-quaternion format.
If no arguments are passed, an identity Pose is initialized.
"""
if len(args) == 0:
self._position = np.zeros(3, dtype="float64")
self._orientation = Orientation()
elif len(args) == 1: # pose
if utils.is_1d_iterable(args[0]):
if len(args[0]) == 6 or np.asarray(args[0]).shape == (1, 6):
pose = np.asarray(args[0]).astype("float64").squeeze()
self._position = pose[:3]
self._orientation = Orientation(pose[3:])
else:
raise ValueError(
"[!] Expecting [x, y, z, rx, ry, rz] pose.")
elif np.asarray(args[0]).shape == (4, 4): # transform
transform = np.asarray(args[0], dtype="float64")
self._position = transform[:3, 3]
self._orientation = Orientation(transform[:3, :3])
else:
raise ValueError("[!] Expecting 6-D pose or 4x4 transform.")
elif len(args) == 2:
if utils.is_iterable(args[0]) and len(args[0]) == 3:
self._position = np.asarray(args[0], dtype="float64")
if isinstance(args[1], Orientation):
self._orientation = Orientation(args[1].quat)
elif isinstance(args[1], quaternion.UnitQuaternion):
self._orientation = Orientation(args[1])
elif utils.is_iterable(args[1]):
if utils.is_1d_iterable(args[1]) and len(args[1]) == 3:
self._orientation = Orientation(
np.asarray(args[1], dtype="float64"))
elif np.asarray(args[1]).shape == (3, 3):
self._orientation = Orientation(
np.asarray(args[1], dtype="float64"))
else:
raise ValueError(
"[!] Expecting rotation vector or rotation matrix."
)
else:
raise ValueError(
"[!] Could not parse orientation argument.")
else:
raise ValueError("[!] Could not parse position argument.")
else:
raise ValueError("[!] Incorrect number of arguments.")
# construct 4x4 transform
self._transform = np.eye(4)
self._transform[:3, :3] = self._orientation.rotm
self._transform[:3, 3] = self._position