예제 #1
0
    def quaternion_mean(self, qk, qxi):
        qt = quaternion.as_quat_array(qk)
        n2 = qxi.shape[1]
        e_prev = np.ones((3, 1))

        # weights = np.zeros(n2,np.float32)
        # weights[-1] = self.lmbda/(self.n +self.lmbda)
        # weights[:-1] = 1/(2*(self.n + self.lmbda))
        for i in range(30):
            ei = np.zeros((3, n2))
            for j in range(n2):
                eq = quaternion.as_quat_array(qxi[:, j]) * qt.inverse()
                ei[:, j] = q2r(eq)
            e = np.mean(ei, axis=1)
            # e = ei * weights
            # e = np.sum(e, axis=1)

            qt = r2q(e) * qt
            if norm(e - e_prev) < 0.01:
                break
            else:
                e_prev = e
        q_avg = quaternion.as_float_array(qt).reshape(-1, 1)
        q_ei = ei
        return q_avg, q_ei
예제 #2
0
def compute_angle_from_quats(quats, joint, leg):
    """Convert rotation matrix to joint angle
    Input:  quats (Frames, 8)
            joint (Knee / Hip / Ankle)
            leg (Left / Right)
    Output: Joint angle (Frames, 3)
    """

    quat1 = Q.as_quat_array(quats[:, :4])
    quat2 = Q.as_quat_array(quats[:, 4:])
    diff = quat1.conj() * quat2
    rmat = Q.as_rotation_matrix(diff)

    _r = R.from_matrix(rmat)
    angle = _r.as_rotvec() * 180 / np.pi
    angle[:, [0, 1, 2]] = angle[:, [2, 0, 1]]

    flx_fct, add_fct, rot_fct = [-1, 1, 1]
    
    if leg == 'Right':
        add_fct, rot_fct = [-1, -1]
    elif leg == 'Left':
        add_fct, rot_fct = [1, 1]

    flip = np.array([flx_fct, add_fct, rot_fct])
    angle = angle * flip
    
    return angle
예제 #3
0
def test_numpy_array_conversion(Qs):
    "Check conversions between array as quaternions and array as floats"
    # First, just check 1-d array
    Q = Qs[Qs_nonnan][:12]  # Select first 3x4=12 non-nan elements in Qs
    assert Q.dtype == np.dtype(np.quaternion)
    q = quaternion.as_float_array(Q)  # View as array of floats
    assert q.dtype == np.dtype(np.float)
    assert q.shape == (12, 4)  # This is the expected shape
    for j in range(12):
        for k in range(4):  # Check each component individually
            assert q[j][k] == Q[j].components[k]
    assert np.array_equal(quaternion.as_quat_array(q), Q)  # Check that we can go backwards
    # Next, see how that works if I flatten the q array
    q = q.flatten()
    assert q.dtype == np.dtype(np.float)
    assert q.shape == (48,)
    for j in range(48):
        assert q[j] == Q[j // 4].components[j % 4]
    assert np.array_equal(quaternion.as_quat_array(q), Q)  # Check that we can go backwards
    # Finally, reshape into 2-d array, and re-check
    P = Q.reshape(3, 4)  # Reshape into 3x4 array of quaternions
    p = quaternion.as_float_array(P)  # View as array of floats
    assert p.shape == (3, 4, 4)  # This is the expected shape
    for j in range(3):
        for k in range(4):
            for l in range(4):  # Check each component individually
                assert p[j][k][l] == Q[4 * j + k].components[l]
    assert np.array_equal(quaternion.as_quat_array(p), P)  # Check that we can go backwards
예제 #4
0
    def from_sxs(cls, w_sxs, override_exception_from_invalidity=False):
        """Construct this object from an `sxs.WaveformModes` object

        Note that the resulting object will likely contain references to the same
        underlying data contained in the original object; modifying one will modify the
        other.  You can make a copy of the result — using code like
        `WaveformModes.from_sxs(w_sxs).copy()` — to obtain separate data.

        """
        import quaternion
        constructor_statement = (
            f"WaveformModes.from_sxs({w_sxs}, "
            f"override_exception_from_invalidity={override_exception_from_invalidity})"
        )

        try:
            frameType = [n.lower()
                         for n in FrameNames].index(w_sxs.frame_type.lower())
        except ValueError:
            frameType = 0

        try:
            dataType = [n.lower()
                        for n in DataNames].index(w_sxs.data_type.lower())
        except ValueError:
            dataType = 0

        kwargs = dict(
            t=w_sxs.t,
            #frame=,  # see below
            data=w_sxs.data,
            history=w_sxs._metadata.get("history", []),
            version_hist=w_sxs._metadata.get("version_hist", []),
            frameType=frameType,
            dataType=dataType,
            r_is_scaled_out=w_sxs._metadata.get("r_is_scaled_out", True),
            m_is_scaled_out=w_sxs._metadata.get("m_is_scaled_out", True),
            override_exception_from_invalidity=
            override_exception_from_invalidity,
            constructor_statement=constructor_statement,
            ell_min=w_sxs.ell_min,
            ell_max=w_sxs.ell_max,
        )

        frame = w_sxs.frame.ndarray
        if np.array_equal(frame, [[1., 0, 0, 0]]):
            pass  # The default will handle itself
        elif frame.shape[0] == 1:
            kwargs["frame"] = quaternion.as_quat_array(frame[0, :])
        elif frame.shape[0] == w_sxs.n_times:
            kwargs["frame"] = quaternion.as_quat_array(frame)
        else:
            raise ValueError(
                f"Frame size ({frame.size}) should be 1 or "
                f"equal to the number of time steps ({self.n_times})")

        return cls(**kwargs)
예제 #5
0
 def abs_errors_orienation(y_true, y_pred):
     quat_true = quaternion.as_quat_array(y_true[..., [6, 3, 4, 5]])
     quat_pred = quaternion.as_quat_array(y_pred[..., [6, 3, 4, 5]])
     errors = (quat_true * quat_pred.conjugate())
     errors = quaternion.as_float_array(errors)
     errors_vec, errors_w = errors[:, 1:], errors[:, 0]
     norms = np.linalg.norm(errors_vec, axis=-1)
     angles = np.arctan2(norms, np.abs(errors_w))
     return np.degrees(2 * angles)
예제 #6
0
    def state_addition(self, x_, Ky):
        q_x_ = quaternion.as_quat_array(x_[:X_QUAT_END, 0])
        q_Ky = quaternion.as_quat_array(Ky[:X_QUAT_END, 0])
        q_x = q_Ky * q_x_
        q_x = quaternion.as_float_array(q_x).reshape(-1, 1)
        wpv_x_ = x_[X_ANG_VEL_START:, [0]]
        wpv_Ky = Ky[X_ANG_VEL_START:, [0]]

        wpv_x = wpv_x_ + wpv_Ky

        x = np.vstack((q_x, wpv_x))
        return x
예제 #7
0
def main():
    import quaternion

    for j in range(100):

        q1 = torch.rand(7, 4)
        q1_norm = normalize_quaternion(q1)
        q1_inverse = inverse_quaternion(q1_norm)
        q1_conjugate = conjugate_quaternion(q1_norm)
        q1_norm_numpy = quaternion.as_quat_array(q1)
        identity = multiply_quaternion(q1_inverse, q1_norm)

        q2 = torch.rand(7, 4)
        q2_norm = normalize_quaternion(q2)
        q2_norm_numpy = quaternion.as_quat_array(q2)

        our_mult = multiply_quaternion(q2_norm, q1_norm)
        our_mult_optimized = multiply_quaternion_optimized(q2_norm, q1_norm)

        for i in range(len(q1)):
            q1_numpy_norm = q1_norm_numpy[i].normalized()
            q1_numpy_conjugate = q1_numpy_norm.conjugate()
            q1_numpy_inverse = q1_numpy_norm.inverse()

            q2_numpy_norm = q2_norm_numpy[i].normalized()
            mult = q2_numpy_norm * q1_numpy_norm

            pdb.set_trace()

            try:
                assert equality_quaternion(
                    q1_numpy_norm, q1_norm[i]), 'Normalize does not work'
                assert equality_quaternion(
                    q1_numpy_conjugate,
                    q1_conjugate[i]), 'Conjugate does not work'
                assert equality_quaternion(
                    q1_numpy_inverse, q1_inverse[i]), 'Inverse does not work'
                assert equality_quaternion(
                    q1_numpy_inverse * q1_numpy_norm,
                    identity[i]), 'Inverse does not work'
                assert equality_quaternion(
                    mult, our_mult[i]), 'addition does not work'
                assert equality_quaternion(
                    mult,
                    our_mult_optimized[i]), 'addition optimized does not work'
                # assert sum(abs(quaternion.as_float_array(q2_numpy)-q2_norm[i])) < THR, 'Normalize does not work'
            except Exception as e:
                print(e)
                pdb.set_trace()
        print('Test is passed ', j)
def rotate_vect(angles, vect, axis):
    '''
    Rotate an array of vectors using Quaternions
    params:
    angles = array of angles that will be used for rotation
    vect   = array of vectors that will be rotated
    axis   = array of vectors that will be used as rotation axis
    
    returns:
    
    v_prime = array of rotated vectors
    
    '''
    assert vect.shape == axis.shape
    #angles = np.array(angles).reshape(3,1) # for broadcast
    theta = np.radians(angles)  # turn to radians
    #theta   = angles
    if len(vect.shape) == 3:
        vector_zeros = np.zeros((vect.shape[0], vect.shape[1], 1))
        axis_zeros = np.zeros((axis.shape[0], axis.shape[1], 1))
        ax = 2
    elif len(vect.shape) == 2:
        vector_zeros = np.zeros((vect.shape[0], 1))
        axis_zeros = np.zeros((axis.shape[0], 1))
        ax = 2
    else:
        raise ValueError('Vector Dimensions must be rank 2 or 3. Got',
                         len(vect.shape))

    vector = np.array(
        np.concatenate([vector_zeros, vect],
                       range(len(
                           vect.shape))[-1]))  # Prepare vector for quaternian
    rot_axis = np.array(
        np.concatenate([axis_zeros, axis],
                       range(len(
                           vect.shape))[-1]))  # Prepare axis for quaternian

    # Determining vector prime using quaternians
    axis_angle = np.multiply(
        (theta * 0.5),
        np.divide(rot_axis, np.linalg.norm(rot_axis, axis=ax, keepdims=True)))
    vec = quat.as_quat_array(vector)
    qlog = quat.as_quat_array(axis_angle)
    q = np.exp(qlog)
    v_prime = q * vec * np.conjugate(q)
    v_prime = quat.as_float_array(
        v_prime)[:, :,
                 1:]  # Discard the real part and return the rotated vector
    return v_prime
예제 #9
0
    def get_transformation(self, timestamps=None, arrays_first=True):
        """ Get the transformation across all reference frames.

        Parameters
        ----------
        timestamps: array_like, shape (n_timestamps,), optional
            Timestamps to which the transformation should be matched. If not
            provided the matcher will call `get_timestamps` for the target
            timestamps.

        arrays_first: bool, default True
            If True and timestamps aren't provided, the first array in the
            list defines the sampling of the timestamps. Otherwise, the first
            reference frame in the list defines the sampling.

        Returns
        -------
        translation: array_like, shape (3,) or (n_timestamps, 3)
            The translation across all reference frames.

        rotation: array_like, shape (4,) or (n_timestamps, 4)
            The rotation across all reference frames.

        timestamps: array_like, shape (n_timestamps,) or None
            The timestamps for which the transformation is defined.
        """
        from rigid_body_motion.utils import rotate_vectors

        if timestamps is None:
            timestamps = self.get_timestamps(arrays_first)

        translation = np.zeros(3) if timestamps is None else np.zeros((1, 3))
        rotation = quaternion(1.0, 0.0, 0.0, 0.0)

        for frame in self.frames:
            t, r = self._transform_from_frame(frame, timestamps)
            if frame.inverse:
                translation = rotate_vectors(
                    1 / as_quat_array(r), translation - np.array(t)
                )
                rotation = 1 / as_quat_array(r) * rotation
            else:
                translation = rotate_vectors(
                    as_quat_array(r), translation
                ) + np.array(t)
                rotation = as_quat_array(r) * rotation

        return translation, as_float_array(rotation), timestamps
def createSequence(keyframes, dt=0.1):
    timestamps = [frame.timestamp for frame in keyframes]
    rotations = quaternion.as_quat_array([[frame.frame.orientation.x, frame.frame.orientation.y, frame.frame.orientation.z, frame.frame.orientation.w] for frame in keyframes])

    # Coordinate frame conversion
    multiplier_quat = quaternion.from_float_array([np.sqrt(2) / 2, -np.sqrt(2) / 2, 0, 0])
    rotations = [rot * multiplier_quat for rot in rotations]

    positions = np.array([[frame.frame.position.x, frame.frame.position.y, frame.frame.position.z] for frame in keyframes])

    time_interps = list(np.arange(timestamps[0], timestamps[-1], dt)) + [timestamps[-1]]
    lerp = interp1d(timestamps, positions, axis=0) 
    lin_interps = lerp(time_interps)

    rot_interps = []
    for i in range(len(keyframes) - 1):
        rot_interps.extend(quaternion.as_float_array(quaternion.slerp(rotations[i], rotations[i + 1], timestamps[i], timestamps[i + 1], np.arange(timestamps[i], timestamps[i + 1], dt))))
    rot_interps.append(quaternion.as_float_array(rotations[-1]))

    sequence = []
    for pos, ori in zip(lin_interps, rot_interps):
        pose = Pose()
        pose.position.x, pose.position.y, pose.position.z = pos
        pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z = ori
        pose_stamped = PoseStamped()
        pose_stamped.pose = pose
        sequence.append(pose_stamped)


    g_poses[0] = sequence[-1].pose
    g_poses[1] = sequence[-1].pose

    return time_interps, sequence 
예제 #11
0
파일: sfm.py 프로젝트: INFINITSY/monodepth2
 def set_state(self):
     index = 0
     frame_count = 0
     for frame_id in self.frame_ids:
         # position 3x1
         self.pred_poses[frame_id][:, [3]] = self.states[index:(index + 3)]
         index += 3
         # orientation as rotation matrix 3x3
         q = quaternion.as_quat_array(self.states[index:(index + 4), 0])
         R = quaternion.as_rotation_matrix(q)
         self.pred_poses[frame_id][:, :3] = R
         index += 4
         # scale 1x1
         # first save scale value
         scale = self.states[index]
         index += 1
         for feature_pts in self.long_track_features:
             # depth value at feature coordinates 1x1
             pt_x = int(feature_pts[frame_count][0])
             pt_y = int(feature_pts[frame_count][1])
             depth = self.states[index]
             self.pred_depths[frame_id][pt_y, pt_x] = depth
             index += 1
         # multiply all depth by scale
         self.pred_depths[frame_id] *= scale
         frame_count += 1
예제 #12
0
def as_quaternion(*args):
    """
    Recombine the arguments to produce a numpy array with quaternion dtype.

    Parameters
    ----------
    *args : ndarray with dtype:float or complex
        The quaternion array components: If there is 4 components, then we assume it is the four compoents of the
        quaternion array: w, x, y, z. If there is only two, they are casted to complex and correspond respectively
        to w + i.x and y + j.z.

    Returns
    -------

    """
    if len(args) == 4:
        # we assume here that the for components have been provided w, x, y, z
        w, x, y, z = args

    if len(args) == 2:
        r, i = args
        w, x, y, z = r.real, r.imag, i.real, i.imag

    data = as_quat_array(list(zip(w.flatten(), x.flatten(), y.flatten(), z.flatten())))
    return data.reshape(w.shape)
예제 #13
0
    def __init__(self, q):

        if q.dtype != 'quaternion':
            raise ValueError('array should be of quaternion type')

        self.signal = q

        # compute H-extension of the signal
        N = np.size(q, 0)
        Q = qfft.Qfft(q)

        # filter frequencies
        h = np.zeros((N, 4))
        h[0, 0] = 1
        h[N // 2, 0] = 1
        h[1:N // 2, 0] = 2
        hq = quaternion.as_quat_array(h)

        self.Hembedding = qfft.iQfft(Q * hq)

        # Compute Euler angle form
        a, theta, chi, phi = utils.quat2euler(self.Hembedding)

        self.a = a
        self.theta = theta
        self.chi = chi
        self.phi = phi
예제 #14
0
    def _transform_from_frame(cls, frame, timestamps):
        """ Get the transform from a frame resampled to the timestamps. """
        if timestamps is None and frame.timestamps is not None:
            raise ValueError("Cannot convert timestamped to static transform")

        if frame.timestamps is None:
            if timestamps is None:
                translation = frame.translation
                rotation = frame.rotation
            else:
                translation = np.tile(frame.translation, (len(timestamps), 1))
                rotation = np.tile(frame.rotation, (len(timestamps), 1))
        elif frame.discrete:
            translation = np.tile(frame.translation[0], (len(timestamps), 1))
            for t, ts in zip(frame.translation, frame.timestamps):
                translation[timestamps >= ts, :] = t
            rotation = np.tile(frame.rotation[0], (len(timestamps), 1))
            for r, ts in zip(frame.rotation, frame.timestamps):
                rotation[timestamps >= ts, :] = r
        else:
            # TODO method + optional scipy dependency?
            translation = interp1d(
                frame.timestamps.astype(float), frame.translation, axis=0
            )(timestamps.astype(float))
            rotation = as_float_array(
                squad(
                    as_quat_array(frame.rotation),
                    frame.timestamps.astype(float),
                    timestamps.astype(float),
                )
            )

        return translation, rotation
예제 #15
0
    def _resample_array(cls, array, timestamps):
        """ Resample an array to the timestamps. """
        if timestamps is None and array.timestamps is not None:
            raise ValueError("Cannot convert timestamped to static array")

        if array.timestamps is None:
            if timestamps is None:
                return array.data
            else:
                return np.tile(array.data, (len(timestamps), 1))
        else:
            # TODO better way to check if quaternion
            if array.data.shape[-1] == 4:
                return as_float_array(
                    squad(
                        as_quat_array(array.data),
                        array.timestamps.astype(float),
                        timestamps.astype(float),
                    )
                )
            else:
                # TODO method + optional scipy dependency?
                return interp1d(
                    array.timestamps.astype(float), array.data, axis=0
                )(timestamps.astype(float))
예제 #16
0
    def swapdims(self, dim1, dim2, inplace=False):
        """
        Swap dimension the complex array.

        swapdims and swapaxes are alias.

        Parameters
        ----------
        dims : int, str or tuple of int or str, optional, default=(0,)
            Dimension names or indexes along which the method should be applied.
        inplace : bool, optional, default=False
            Flag to say that the method return a new object (default)
            or not (inplace=True)

        Returns
        -------
        transposed
            Same object or a copy depending on the ``inplace`` flag.
        """

        new = super().swapdims(dim1, dim2, inplace=inplace)

        # we need also to swap the quaternion
        # WARNING: this work only for 2D - when swapdims is equivalent to a 2D transpose
        # TODO: implement something for any n-D array (n>2)
        if self.is_quaternion:
            # here if it is is_quaternion
            # we should interchange the imaginary component
            w, x, y, z = as_float_array(new._data).T
            q = as_quat_array(list(zip(w.T.flatten(), y.T.flatten(), x.T.flatten(), z.T.flatten())))
            new._data = q.reshape(new.shape)

        return new
예제 #17
0
파일: servo.py 프로젝트: erytheis/glyx
    def __init__(self,
                 coordinates=(0, 0, 0),
                 euler=(0, 0, 0),
                 position=0,
                 quaternion=None):
        # TODO rename coordinates into relative position
        self.base_coordinates = np.asanyarray(coordinates)
        self.euler = np.asanyarray(euler)
        self.position = position

        self.quaternion = quaternion
        if quaternion == None:
            self.quaternion = from_euler_angles(self.euler)

        self.angular_speed = 0

        # Finding orientation vectors
        self.orientation_vector_x = np.asanyarray(
            rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_FRONT))
        self.orientation_vector_y = np.asanyarray(
            rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_SIDE))
        self.orientation_vector_z = np.asanyarray(
            rotate_vectors(self.quaternion, DEFAULT_ORIENTATION_TOP))

        self.joined_servo_rotation_quaternion = as_quat_array(
            np.insert((self.orientation_vector_y * math.sin(math.pi / 4)), 0,
                      -math.sin(math.pi / 4)))

        # Calculate coordinates of a joint
        self.update_joint_coordinates()
예제 #18
0
    def rotate_ligand(self,
                      qrotor=None,
                      geometrical_center='heavy',
                      centered=False):

        if geometrical_center == 'heavy':
            tmp_list = self.molcomplex.ligand._heavy_atoms_indices
        elif geometrical_center == 'CA':
            tmp_list = self.molcomplex.ligand._ca_atoms_indices
        elif geometrical_center == 'All':
            tmp_list = np.arange(self.molcomplex.ligand.n_atoms)

        if type(qrotor) != quaternion.quaternion:
            qrotor = quaternion.as_quat_array(qrotor)

        tmp_positions = self.get_ligand_positions()
        tmp_unit = tmp_positions.unit

        if centered:
            tmp_positions = quaternion.rotate_vectors(
                qrotor, tmp_positions._value) * tmp_unit
        else:
            geometrical_center_positions = utils.geometrical_center(
                tmp_positions, tmp_list)
            tmp_positions = tmp_positions - geometrical_center_positions
            tmp_positions = quaternion.rotate_vectors(
                qrotor, tmp_positions._value) * tmp_unit
            tmp_positions = tmp_positions + geometrical_center_positions
            del (geometrical_center_positions)

        self.set_ligand_positions(tmp_positions)
        del (tmp_positions, tmp_unit, tmp_list)
예제 #19
0
    def pulse(self, spins):
        """
		Calculate pulse evolution as applied to spin object

		Arguments
		---------
		spins : <Spins> object

		Returns
		-------
		array: quaternion array
			evolved spins are returned as a quaternion array and are also 
			stored in the 'mag' attribute of the 'Spins' object argument

		"""
        if hasattr(self, 'amplitudes'):
            q = quat.as_quat_array([1., 0., 0., 0.] * len(spins))
            incr = self.increment
            for rf in self.amplitudes:
                beff = (spins.q_offsets + rf) * incr
                q = np.exp(-0.5 * beff) * q

            mag = q * spins.magnetisation * np.conjugate(q)
            spins.magnetisation = mag
            return mag
        else:
            print(
                "Cannot pulse spins. Pulse calibration may be required first:")
            print("Call the <calibrate> method to set pulse amplitudes.")
예제 #20
0
def get_corrected_orientation(E, target_orientation, evs):
    # Correct cs orientation so that it aligns with standard defined as foot touchdown

    E_quats = quaternion.from_rotation_matrix(E)
    target_quats = np.repeat(
        quaternion.from_rotation_matrix(target_orientation), len(evs))
    # Get relative rotation between marker-cs and ideal-cs during touchdowns
    rel_quats = E_quats[evs].conj() * target_quats
    rel_quat_array = quaternion.as_float_array(rel_quats)
    # Find the average relative orientation between the ideal cs and all touchdown cs
    eigvals, eigvecs = np.linalg.eig(rel_quat_array.T @ rel_quat_array)
    av_rel_quat_array = eigvecs[:, np.argmax(eigvals)]
    # Check sign of the averaged quaternion for consistency
    av_signs = np.sign(av_rel_quat_array)
    ar_signs = np.sign(rel_quat_array)
    if (av_signs != ar_signs).all():
        av_rel_quat_array = -1 * av_rel_quat_array
    elif (av_signs == ar_signs).all():
        pass
    else:
        pass

    av_rel_quats = np.repeat(quaternion.as_quat_array(av_rel_quat_array),
                             len(E_quats))
    corrected_quats = E_quats * av_rel_quats
    corrected_orientation = quaternion.as_rotation_matrix(corrected_quats)

    return corrected_orientation
예제 #21
0
def qmul(*q, qaxis=-1):
    """ Quaternion multiplication.

    Parameters
    ----------
    q: iterable of array_like
        Arrays containing quaternions to multiply. Their dtype can be
        quaternion, otherwise `qaxis` specifies the axis representing
        the quaternions.

    qaxis: int, default -1
        If `q` are not quaternion dtype, axis of the quaternion arrays
        representing the coordinates of the quaternions.

    Returns
    -------
    qm: ndarray
        A new array containing the multiplied quaternions.
    """
    # TODO xarray support
    if len(q) < 2:
        raise ValueError("Please provide at least 2 quaternions to multiply")

    if all(qq.dtype != quaternion for qq in q):
        q = (as_quat_array(np.swapaxes(qq, qaxis, -1)) for qq in q)
        qm = reduce(operator.mul, q, 1)
        return np.swapaxes(as_float_array(qm), -1, qaxis)
    elif all(qq.dtype == quaternion for qq in q):
        return reduce(operator.mul, q, 1)
    else:
        raise ValueError(
            "Either all or none of the provided quaternions must be "
            "quaternion dtype"
        )
예제 #22
0
def qinv(q, qaxis=-1):
    """ Quaternion inverse.

    Parameters
    ----------
    q: array_like
        Array containing quaternions whose inverse is to be computed. Its dtype
        can be quaternion, otherwise `qaxis` specifies the axis representing
        the quaternions.

    qaxis: int, default -1
        If `q` is not quaternion dtype, axis of the quaternion array
        representing the coordinates of the quaternions.

    Returns
    -------
    qi: ndarray
        A new array containing the inverse values.
    """
    # TODO xarray support
    if q.dtype != quaternion:
        q = np.swapaxes(q, qaxis, -1)
        qi = as_float_array(1 / as_quat_array(q))
        return np.swapaxes(qi, -1, qaxis)
    else:
        return 1 / q
예제 #23
0
 def __init__(self, init=None):
     if isinstance(init, type(self)):
         self.__quat = init.__quat
     elif isinstance(init, np.quaternion):
         self.__quat = init
     else:
         self.__quat = quaternion.as_quat_array(np.array(init, copy=False))
예제 #24
0
파일: data.py 프로젝트: ehu-ai/domrand
def load_eval_data(eval_data_path, data_shape='asus'):
    """Read the data from the real world stored in jpgs"""
    imgs = []
    labels = []
    pickle_path = os.path.join(eval_data_path, "ep_data.pkl")
    with open(pickle_path, "rb") as f:
        context = pickle.load(f)
    files = sorted(os.listdir(eval_data_path))
    obj_world_pose = context["obj_world_pose"]
    obj_world_pos = obj_world_pose[:3]
    obj_world_quat = quaternion.as_quat_array(obj_world_pose[3:])
    zrot = quaternion.as_rotation_vector(obj_world_quat)[-1]
    pose = np.zeros((3,))
    pose[:2] = obj_world_pos[:2]
    pose[2] = zrot
    for f in files:
        if not f.endswith('.png'):
            continue

        labels.append(pose.copy())
        filename = os.path.join(eval_data_path, f)
        img = plt.imread(filename)
        if data_shape == 'asus':
            img = preproc_image(img, dtype=np.float32)
        # plt.imshow(img); plt.show()
        # img = (img / 127.5) - 1.0
        imgs.append(img)
    return np.array(imgs, dtype=np.float32), np.array(labels, dtype=np.float32)
예제 #25
0
def angular_velocity(W, include_frame_velocity=False):
    """Angular velocity of Waveform

    This function calculates the angular velocity of a Waveform object from
    its modes.  This was introduced in Sec. II of "Angular velocity of
    gravitational radiation and the corotating frame"
    <http://arxiv.org/abs/1302.2919>.  Essentially, this is the angular
    velocity of the rotating frame in which the time dependence of the modes
    is minimized.

    The vector is given in the (possibly rotating) mode frame (X,Y,Z),
    which is not necessarily equal to the inertial frame (x,y,z).

    """

    # Calculate the <Ldt> vector and <LL> matrix at each instant
    l = W.LdtVector()
    ll = W.LLMatrix()

    # Solve <Ldt> = - <LL> . omega
    omega = -np.linalg.solve(ll, l)

    if include_frame_velocity and len(W.frame) == W.n_times:
        from quaternion import derivative, as_quat_array, as_float_array

        Rdot = as_quat_array(derivative(as_float_array(W.frame), W.t))
        omega += as_float_array(2 * Rdot * W.frame.conjugate())[:, 1:]

    return omega
예제 #26
0
파일: Xsens.py 프로젝트: davidsoncolin/IMS
    def cook(self, location, interface, attrs):
        points = self.client.getQuatData(0)

        if not np.all(np.isnan(points[:23, 0])):

            if self.skelDict is None:
                print "New Skel"
                self.skelDict = makeXSensSkelDict(points[:23, :])
            else:
                T = points[:23, :3]
                R = points[:23, 3:].astype(np.float)
                R = quaternion.as_quat_array(R)
                self.skelDict['Gs'] = np.array(genGs(R, T), dtype=np.float32)

            skelAttrs = {
                'skelDict': self.skelDict,
                'rootMat': np.eye(3, 4),
                'originalRootMat': np.eye(3, 4),
                'subjectName': self.getName(),
                'boneColour': (0.3, 0.42, 0.66, 1.)
            }
            interface.createChild(interface.name(),
                                  'skeleton',
                                  atLocation=interface.parentPath(),
                                  attrs=skelAttrs)
예제 #27
0
def arr_quat_to_rots(quats):
    ret = q.as_rotation_vector(q.as_quat_array(quats))
    ret = ret.reshape(-1, 3)
    ret = ret[:, 1] - 3.14
    ret[np.where(ret > 3.14)] -= 6.28
    ret[np.where(ret < -3.14)] += 6.28
    return ret
예제 #28
0
def test_as_float_quat(Qs):
    qs = Qs[Qs_nonnan]
    for quats in [qs, np.vstack((qs,)*3), np.vstack((qs,)*(3*5)).reshape((3, 5)+qs.shape),
                  np.vstack((qs,)*(3*5*6)).reshape((3, 5, 6)+qs.shape)]:
        floats = quaternion.as_float_array(quats)
        assert floats.shape == quats.shape+(4,)
        assert allclose(quaternion.as_quat_array(floats), quats)
예제 #29
0
    def _generate_goal(self):
        original_obj_pos, original_obj_quat = self._p.getBasePositionAndOrientation(
            self.object_bodies[self.goal_object_key])

        while True:
            target_xyz = self.np_random.uniform(self.robot.target_bound_lower,
                                                self.robot.target_bound_upper)
            # on the table
            target_xyz[-1] = self.object_initial_pos[self.goal_object_key][2]
            if np.linalg.norm(target_xyz - original_obj_pos) > 0.06:
                break
        target_obj_euler = quat.as_euler_angles(
            quat.as_quat_array([1., 0., 0., 0.]))
        target_obj_euler[-1] = self.np_random.uniform(-1.0, 1.0) * np.pi
        target_obj_quat = quat.as_float_array(
            quat.from_euler_angles(target_obj_euler))
        target_obj_quat = np.concatenate(
            [target_obj_quat[1:], [target_obj_quat[0]]], axis=-1)

        self.desired_goal = np.concatenate((target_xyz, target_obj_euler))

        if self.visualize_target:
            self.set_object_pose(
                self.object_bodies[self.goal_object_key + '_target'],
                target_xyz, target_obj_quat)
예제 #30
0
 def rotate_vector(self, rotation_quat, vec):
     """
     Rotate a given vector vec by the quaternion rotation_quat
     """
     quat_vec = qt.as_quat_array(np.insert(vec, 0, 0, axis=-1))
     return qt.as_float_array(rotation_quat * quat_vec *
                              rotation_quat.conj())[..., 1:]
예제 #31
0
    def transpose(self, *dims, inplace=False):
        """
        Transpose the complex array.

        Parameters
        ----------
        dims : int, str or tuple of int or str, optional, default=(0,)
            Dimension names or indexes along which the method should be applied.
        inplace : bool, optional, default=False
            Flag to say that the method return a new object (default)
            or not (inplace=True)

        Returns
        -------
        transposed
            Same object or a copy depending on the ``inplace`` flag.
        """

        new = super().transpose(*dims, inplace=inplace)

        if new.is_quaternion:
            # here if it is hypercomplex quaternion
            # we should interchange the imaginary component
            w, x, y, z = as_float_array(new._data).T
            q = as_quat_array(
                list(
                    zip(w.T.flatten(), y.T.flatten(), x.T.flatten(),
                        z.T.flatten())))
            new._data = q.reshape(new.shape)

        return new
예제 #32
0
    def reset(self):
        """
		Reset all spins to their initial vector specified by 'init_mag'

		"""
        tmp = np.tile([0] + list(self.initial_magnetisation),
                      len(self)).reshape(len(self), 4)
        self.magnetisation = quat.as_quat_array(tmp)
예제 #33
0
def test_as_rotation_vector():
    np.random.seed(1234)
    n_tests = 1000
    vecs = np.random.uniform(high=math.pi/math.sqrt(3), size=n_tests*3).reshape((n_tests, 3))
    quats = np.zeros(vecs.shape[:-1]+(4,))
    quats[..., 1:] = vecs[...]
    quats = quaternion.as_quat_array(quats)
    quats = np.exp(quats/2)
    quat_vecs = quaternion.as_rotation_vector(quats)
    assert allclose(quat_vecs, vecs)
예제 #34
0
    def get_orientation(self, method='madgwick', initwindow=0.5, beta=2.86, lCa=(0.0, -0.3, -0.7), Ca=None):
        if method.lower() == 'ekf':
            dt = np.mean(np.diff(self.t))
            if Ca is None:
                Ca = np.power(10, np.array(lCa)) / dt
            else:
                Ca = np.array(Ca) / dt
            self._get_orientation_ekf(Ca=Ca)

            inertialbasis = []

            for rpy in self.orient_sensor:
                QT = self._getQT(rpy)

                inertialbasis.append(QT.dot(np.eye(3)))

            inertialbasis = np.array(inertialbasis)

            self.worldbasis = np.matmul(self.chip2world_rot, inertialbasis)

            self.accdyn_world = np.matmul(self.chip2world_rot, self.accdyn_sensor.T).T
            self.accdyn = self.accdyn_world

        elif method.lower() in ['madgwick', 'integrate_gyro']:
            if method.lower() == 'integrate_gyro':
                beta = 0.0

            self._get_orientation_madgwick(initwindow=initwindow, beta=beta)

            qorient_world = self.qchip2world.conj() * self.qorient * self.qchip2world
            self.qorient_world = qorient_world

            self.orient_world = np.array([quaternion.as_euler_angles(q1) for q1 in qorient_world])
            self.orient = self.orient_world

            # make accdyn into a quaternion with zero real part
            qacc = np.zeros((self.accdyn_sensor.shape[0], 4))
            qacc[:, 1:] = self.accdyn_sensor

            # rotate accdyn into the world coordinate system
            qaccdyn_world = self.qchip2world.conj() * quaternion.as_quat_array(qacc) * self.qchip2world
            self.accdyn_world = np.array([q.components[1:] for q in qaccdyn_world])
            self.accdyn = self.accdyn_world


        return self.orient_world
예제 #35
0
def integrate_angular_velocity(Omega, t0, t1, R0=None, tolerance=1e-12):
    """Compute frame with given angular velocity

    Parameters
    ==========
    Omega: tuple or callable
        Angular velocity from which to compute frame.  Can be
          1) a 2-tuple of float arrays (t, v) giving the angular velocity vector at a series of times,
          2) a function of time that returns the 3-vector angular velocity, or
          3) a function of time and orientation (t, R) that returns the 3-vector angular velocity
        In case 1, the angular velocity will be interpolated to the required times.  Note that accuracy
        is poor in case 1.
    t0: float
        Initial time
    t1: float
        Final time
    R0: quaternion, optional
        Initial frame orientation.  Defaults to 1 (the identity orientation).
    tolerance: float, optional
        Absolute tolerance used in integration.  Defaults to 1e-12.

    Returns
    =======
    t: float array
    R: quaternion array

    """
    import warnings
    from scipy.integrate import ode

    if R0 is None:
        R0 = quaternion.one

    input_is_tabulated = False

    try:
        t_Omega, v = Omega
        from scipy.interpolate import InterpolatedUnivariateSpline
        Omega_x = InterpolatedUnivariateSpline(t_Omega, v[:, 0])
        Omega_y = InterpolatedUnivariateSpline(t_Omega, v[:, 1])
        Omega_z = InterpolatedUnivariateSpline(t_Omega, v[:, 2])
        def Omega_func(t, R):
            return [Omega_x(t), Omega_y(t), Omega_z(t)]
        Omega_func(t0, R0)
        input_is_tabulated = True
    except (TypeError, ValueError):
        def Omega_func(t, R):
            return Omega(t, R)
        try:
            Omega_func(t0, R0)
        except TypeError:
            def Omega_func(t, R):
                return Omega(t)
            Omega_func(t0, R0)

    def RHS(t, y):
        R = quaternion.quaternion(*y)
        return (0.5 * quaternion.quaternion(0.0, *Omega_func(t, R)) * R).components

    y0 = R0.components

    if input_is_tabulated:
        from scipy.integrate import solve_ivp
        t = t_Omega
        t_span = [t_Omega[0], t_Omega[-1]]
        solution = solve_ivp(RHS, t_span, y0, t_eval=t_Omega, atol=tolerance, rtol=100*np.finfo(float).eps)
        R = quaternion.from_float_array(solution.y.T)
    else:
        solver = ode(RHS)
        solver.set_initial_value(y0, t0)
        solver.set_integrator('dop853', nsteps=1, atol=tolerance, rtol=0.0)
        solver._integrator.iwork[2] = -1  # suppress Fortran-printed warning
        t = appending_array((int(t1-t0),))
        t.append(solver.t)
        R = appending_array((int(t1-t0), 4))
        R.append(solver.y)
        warnings.filterwarnings("ignore", category=UserWarning)
        t_last = solver.t
        while solver.t < t1:
            solver.integrate(t1, step=True)
            if solver.t > t_last:
                t.append(solver.t)
                R.append(solver.y)
                t_last = solver.t
        warnings.resetwarnings()
        t = t.a
        R = quaternion.as_quat_array(R.a)

    return t, R