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