Exemple #1
0
def apply_axial_constraint2(q, ref_q, axis, min_angle, max_angle):
    """ get delta orientation
        do axis decomposition
        restrict angle between min and max
        """
    q = normalize(q)
    inv_ref_q = quaternion_inverse(ref_q)
    inv_ref_q = normalize(inv_ref_q)
    delta_q = quaternion_multiply(inv_ref_q, q)
    delta_q = normalize(delta_q)
    swing_q, twist_q = swing_twist_decomposition(delta_q, axis)
    swing_q = normalize(swing_q)
    twist_q = normalize(twist_q)
    v, a = quaternion_to_axis_angle(twist_q)
    print("apply axial", q)
    v2, a2 = quaternion_to_axis_angle(swing_q)
    sign = 1
    if np.sum(v) < 0:
        sign = -1
    a *= sign
    print("twist swing", v, np.degrees(a), v2, np.degrees(a2))
    print("boundary", min_angle, max_angle)
    a = max(a, min_angle)
    a = min(a, max_angle)
    new_twist_q = quaternion_about_axis(a, axis)
    new_twist_q = normalize(new_twist_q)
    delta_q = quaternion_multiply(swing_q, new_twist_q)
    delta_q = normalize(delta_q)
    q = quaternion_multiply(ref_q, delta_q)
    q = normalize(q)
    return q
Exemple #2
0
    def apply_constraint_with_swing_global(self, node, frame, o, eps=0.01):
        old_q = np.array(frame[o:o + 4])
        next_node = self.bones[node].child
        parent_m = self.skeleton.nodes[node].get_global_matrix(frame)[:3, :3]
        node_m = self.skeleton.nodes[next_node].get_global_matrix(
            frame)[:3, :3]
        node_q = quaternion_from_matrix(node_m)
        node_q = normalize(node_q)
        # remove twist rotation
        swing_q, twist_q = self.skeleton.nodes[
            node].joint_constraint.split_global(parent_m, node_q)
        frame[o:o + 4] = twist_q

        # apply swing_q to parent
        parent_q = np.array(frame[o - 4:o])
        new_parent_q = quaternion_multiply(parent_q, swing_q)
        new_parent_q = normalize(new_parent_q)
        frame[o - 4:o] = new_parent_q
        new_node_pos = self.skeleton.nodes[node].children[
            0].get_global_position(frame)
        diff = np.linalg.norm(new_node_pos - self.target)
        return
        # calculate rotation fix if necessary
        if diff > eps:
            delta_q = orient_joint_to_target(self.skeleton, node, frame,
                                             new_node_pos, self.target)
            aligned_parent_q = quaternion_multiply(delta_q, new_parent_q)
            aligned_parent_q = normalize(aligned_parent_q)
            frame[o - 4:o] = aligned_parent_q
def add_reduced_frames(skeleton, a, delta, joints):
    """ returns c = a + delta where delta are the parameters of the joints list"""
    c = np.array(a)
    o = 0
    for idx, j in enumerate(skeleton.animated_joints):
        if j not in joints:
            continue
        if j == skeleton.root:
            dest = 0
            c[:3] = a[dest:dest+3] + delta[o:o+3]
            q_dest = dest+3
            q_o = o+3
            q_a = a[q_dest:q_dest + 4]
            q_delta = delta[q_o:q_o + 4]

            q_prod = quaternion_multiply(q_a, q_delta)
            c[q_dest:q_dest + 4] = q_prod / np.linalg.norm(q_prod)
            o += 7
        else:
            dest = idx* 4 + 3

            q_a = a[dest:dest + 4]
            q_delta = delta[o:o + 4]

            q_prod = quaternion_multiply(q_a, q_delta)
            c[dest:dest + 4] = q_prod / np.linalg.norm(q_prod)
            o += 4
    return c
Exemple #4
0
def align_joint(new_skeleton,
                free_joint_name,
                local_target_axes,
                global_src_up_vec,
                global_src_x_vec,
                joint_cos_map,
                apply_spine_fix=False):
    # first align the twist axis
    q, axes = align_axis(local_target_axes, "y", global_src_up_vec)
    q = normalize(q)
    # then align the swing axis
    qx, axes = align_axis(axes, "x", global_src_x_vec)
    q = quaternion_multiply(qx, q)
    q = normalize(q)

    # handle cases when twist axis alignment was lost
    dot = np.dot(axes["y"], global_src_up_vec)
    if dot <= -1:
        q180 = quaternion_about_axis(np.deg2rad(180), global_src_x_vec)
        q180 = normalize(q180)
        q = quaternion_multiply(q180, q)
        q = normalize(q)
    elif abs(dot) != 1.0:
        qy, axes = align_axis(axes, "y", global_src_up_vec)
        q = quaternion_multiply(qy, q)
        q = normalize(q)
    return q
def align_root_joint(new_skeleton, free_joint_name, axes, global_src_up_vec, global_src_x_vec,joint_cos_map, max_iter_count=10):
    # handle special case for the root joint
    # apply only the y axis rotation of the Hip to the Game_engine node
    q = [1, 0, 0, 0]
    #apply first time
    qx, axes = align_axis_in_place(axes, "x", global_src_x_vec)  # first find rotation to align x axis
    q = quaternion_multiply(qx, q)
    q = normalize(q)

    qy, axes = align_axis_in_place(axes, "y", global_src_up_vec)  # then add a rotation to let the y axis point up
    q = quaternion_multiply(qy, q)
    q = normalize(q)

    #apply second time
    qx, axes = align_axis_in_place(axes, "x", global_src_x_vec)  # first find rotation to align x axis
    q = quaternion_multiply(qx, q)
    q = normalize(q)
    qy, axes = align_axis_in_place(axes, "y", global_src_up_vec)  # then add a rotation to let the y axis point up
    q = quaternion_multiply(qy, q)
    q = normalize(q)

    # print("handle special case for pelvis")
    # handle special case of applying the x axis rotation of the Hip to the pelvis
    node = new_skeleton.nodes[free_joint_name]
    t_pose_global_m = node.get_global_matrix(new_skeleton.reference_frame)[:3, :3]
    global_original = np.dot(t_pose_global_m, joint_cos_map[free_joint_name]["y"])
    global_original = normalize(global_original)
    qoffset = find_rotation_between_vectors(OPENGL_UP_AXIS, global_original)
    q = quaternion_multiply(q, qoffset)
    q = normalize(q)
    return q
Exemple #6
0
def compute_mean_quaternion(sigma_pts, init_mean):

    #returns the quaternion mean of the quaternion state in the sigma_pts using gradient descent

    num_pts = sigma_pts.shape[1]

    # error_quat = np.empty(sigma_pts.shape)
    error_vec = np.empty([3, num_pts], dtype=np.float64)
    mean_error_vec = np.ones([
        3,
    ], dtype=np.float64)
    iteration = 0
    while (np.linalg.norm(mean_error_vec) > 0.001 and iteration < 50):
        iteration += 1
        prev_mean = tf.quaternion_inverse(init_mean)

        for i in range(0, num_pts):
            error_quat = tf.quaternion_multiply(sigma_pts[:, i], prev_mean)

            error_vec[:, i] = tf.rotvec_from_quaternion(error_quat)

        mean_error_vec = np.mean(error_vec, 1)

        init_mean = tf.quaternion_multiply(
            tf.quaternion_from_rotvec(mean_error_vec), init_mean)
    # print iteration
    return init_mean, error_vec
Exemple #7
0
def align_root_joint(axes, global_src_x_vec, max_iter_count=10):
    # handle special case for the root joint
    # apply only the y axis rotation of the Hip to the Game_engine node
    not_aligned = True
    q = [1, 0, 0, 0]
    iter_count = 0
    while not_aligned:
        qx, axes = align_axis(
            axes, "x", global_src_x_vec)  # first find rotation to align x axis
        q = quaternion_multiply(qx, q)
        q = normalize(q)
        qy, axes = align_axis(
            axes, "y",
            OPENGL_UP_AXIS)  # then add a rotation to let the y axis point up
        q = quaternion_multiply(qy, q)
        q = normalize(q)
        dot_y = np.dot(axes["y"], OPENGL_UP_AXIS)
        dot_y = min(1, max(dot_y, -1))
        a_y = math.acos(dot_y)
        dot_x = np.dot(axes["x"], global_src_x_vec)
        dot_x = min(1, max(dot_x, -1))
        a_x = math.acos(dot_x)
        iter_count += 1
        not_aligned = a_y > 0.1 or a_x > 0.1 and iter_count < max_iter_count
    return q
def axes_to_q(g_twist, g_swing, flip=False):
    q = [1, 0, 0, 0]
    q_y = quaternion_from_vector_to_vector(Y, g_twist)
    q_y = normalize(q_y)
    q = quaternion_multiply(q_y, q)
    X_prime = rotate_vector(q_y, X)
    X_prime = normalize(X_prime)
    q_x = quaternion_from_vector_to_vector(X_prime, g_swing)
    q_x = normalize(q_x)
    q = quaternion_multiply(q_x, q)
    q = normalize(q)
    Y_prime = rotate_vector(q, Y)
    dot = np.dot(Y_prime, g_twist)
    #dot = min(dot,1)
    dot = max(dot, -1)
    if dot == -1:
        q180 = quaternion_about_axis(np.deg2rad(180), g_swing)
        q180 = normalize(q180)
        q = quaternion_multiply(q180, q)
        q = normalize(q)
    elif abs(dot) != 1.0:
        q_y = quaternion_from_vector_to_vector(Y_prime, g_twist)
        q = quaternion_multiply(q_y, q)
        q = normalize(q)
    return q
Exemple #9
0
def apply_axial_constraint(q, ref_q, axis, k1, k2):
    """ lee 2000 p. 48"""
    q0 = ref_q
    w = axis
    #rel_q = quaternion_multiply(quaternion_inverse(q0), q)
    #rel_q = normalize(rel_q)
    w_prime = rotate_vector_by_quaternion(w, q)
    v = np.cross(w, w_prime)
    phi = acos(np.dot(w, w_prime))

    #remove rotation around v
    # psi_q = exp(-phi*v)
    inv_phi_q = quaternion_about_axis(-phi, v)
    psi_q = quaternion_multiply(inv_phi_q, ref_q)

    # get angle around w from psi_q
    w_prime2 = rotate_vector_by_quaternion(w, psi_q)
    w_prime2 = normalize(w_prime2)
    psi = acos(np.dot(w, w_prime2))
    if k1 < psi > k2:  # apply
        delta_psi1 = k1 - psi
        delta_psi2 = k2 - psi
        delta_psi = min(delta_psi1, delta_psi2)
        delta_q = quaternion_about_axis(delta_psi, w)
        q = quaternion_multiply(delta_q, q)
        print("apply", psi)
    return q
Exemple #10
0
 def apply_constraint_with_swing_and_lee(self, node, frame, o, eps=0.01):
     old_q = frame[o:o + 4]
     old_node_pos = self.skeleton.nodes[node].children[
         0].get_global_position(frame)
     delta_q, new_q = self.skeleton.nodes[node].joint_constraint.split(
         old_q)
     frame[o:o + 4] = new_q
     new_node_pos = self.skeleton.nodes[node].children[
         0].get_global_position(frame)
     diff = np.linalg.norm(new_node_pos - old_node_pos)
     if diff > eps:
         parent_q = frame[o - 4:o]
         new_parent_q = quaternion_multiply(parent_q, delta_q)
         new_parent_q = normalize(new_parent_q)
         frame[o - 4:o] = new_parent_q
         new_node_pos = self.skeleton.nodes[node].children[
             0].get_global_position(frame)
         diff = np.linalg.norm(new_node_pos - old_node_pos)
         if diff > eps:
             parent_q = frame[o - 4:o]
             root = None
             if self.skeleton.nodes[node].parent is not None:
                 root = self.skeleton.nodes[node].parent.node_name
             end_effector = self.skeleton.nodes[node].children[0].node_name
             print("apply lee tolani", root, node, end_effector, diff)
             local_axis = self.skeleton.nodes[node].joint_constraint.axis
             #frame[o:o + 4] = calculate_limb_joint_rotation(self.skeleton, root, node, end_effector, local_axis, frame, self.target)
             delta_q = calculate_limb_root_rotation(self.skeleton, root,
                                                    end_effector, frame,
                                                    self.target)
             new_parent_q = quaternion_multiply(parent_q, delta_q)
             new_parent_q = normalize(new_parent_q)
             frame[o - 4:o] = new_parent_q
Exemple #11
0
def qv_mult(q1, v1):
    v1 = transformations.unit_vector(v1)
    q2 = list(v1)
    q2.append(0.0)
    return transformations.quaternion_multiply(
        transformations.quaternion_multiply(q1, q2),
        transformations.quaternion_conjugate(q1))[:3]
    def __predict_nominal_state(self, imu_measurement: np.array):
        p = self.nominal_state[:3].reshape(-1, 1)
        q = self.nominal_state[3:7]
        v = self.nominal_state[7:10].reshape(-1, 1)
        a_b = self.nominal_state[10:13].reshape(-1, 1)
        w_b = self.nominal_state[13:16]
        g = self.nominal_state[16:19].reshape(-1, 1)

        w_m = imu_measurement[1:4].copy()
        a_m = imu_measurement[4:7].reshape(-1, 1).copy()
        dt = imu_measurement[0] - self.last_predict_time
        """
        dp/dt = v
        dv/dt = R(a_m - a_b) + g
        dq/dt = 0.5 * q x(quaternion product) (w_m - w_b)
        
        a_m and w_m are the measurements of IMU.
        a_b and w_b are biases of acc and gyro, respectively.
        R = R{q}, which bring the point from local coordinate to global coordinate.
        """
        w_m -= w_b
        a_m -= a_b

        # use the zero-order integration to integrate the quaternion.
        # q_{n+1} = q_n x q{(w_m - w_b) * dt}
        angle = la.norm(w_m)
        axis = w_m / angle
        R_w = tr.rotation_matrix(0.5 * dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_half_next = tr.quaternion_multiply(q, q_w)

        R_w = tr.rotation_matrix(dt * angle, axis)
        q_w = tr.quaternion_from_matrix(R_w, True)
        q_next = tr.quaternion_multiply(q, q_w)
        if q_next[0] < 0:  # force the quaternion has a positive real part.
            q_next *= -1

        # use RK4 method to integration velocity and position.
        # integrate velocity first.
        R = tr.quaternion_matrix(q)[:3, :3]
        R_half_next = tr.quaternion_matrix(q_half_next)[:3, :3]
        R_next = tr.quaternion_matrix(q_next)[:3, :3]
        v_k1 = R @ a_m + g
        v_k2 = R_half_next @ a_m + g
        # v_k3 = R_half_next @ a_m + g  # yes. v_k2 = v_k3.
        v_k3 = v_k2
        v_k4 = R_next @ a_m + g
        v_next = v + dt * (v_k1 + 2 * v_k2 + 2 * v_k3 + v_k4) / 6

        # integrate position
        p_k1 = v
        p_k2 = v + 0.5 * dt * v_k1  # k2 = v_next_half = v + 0.5 * dt * v' = v + 0.5 * dt * v_k1(evaluate at t0)
        p_k3 = v + 0.5 * dt * v_k2  # v_k2 is evaluated at t0 + 0.5*delta
        p_k4 = v + dt * v_k3
        p_next = p + dt * (p_k1 + 2 * p_k2 + 2 * p_k3 + p_k4) / 6

        self.nominal_state[:3] = p_next.reshape(3, )
        self.nominal_state[3:7] = q_next
        self.nominal_state[7:10] = v_next.reshape(3, )
Exemple #13
0
 def rotate(self, yaw, pitch, roll):
     self.yaw += yaw
     self.pitch += pitch
     self.roll += roll
     q1 = transformations.quaternion_about_axis(self.pitch, (1, 0, 0))
     q2 = transformations.quaternion_about_axis(self.yaw, (0, 0, 1))
     q3 = transformations.quaternion_about_axis(self.roll, (0, 1, 0))
     self.orientation = transformations.quaternion_multiply(transformations.quaternion_multiply(q1, q2), q3)
Exemple #14
0
    def getBlendedPose(self, poses, weights, additiveBlending=True):
        import transformations as tm

        REST_QUAT = np.asarray([1, 0, 0, 0], dtype=np.float32)

        if isinstance(poses[0], basestring):
            f_idxs = [self.getPoseNames().index(pname) for pname in poses]
        else:
            f_idxs = poses

        if not additiveBlending:
            # normalize weights
            if not isinstance(weights, np.ndarray):
                weights = np.asarray(weights, dtype=np.float32)
            t = sum(weights)
            if t < 1:
                # Fill up rest with neutral pose (neutral pose is assumed to be first frame)
                weights = np.asarray(weights + [1.0 - t], dtype=np.float32)
                f_idxs.append(0)
            weights /= t

        #print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)

        result = emptyPose(self.nBones)
        m = np.identity(4, dtype=np.float32)
        m1 = np.identity(4, dtype=np.float32)
        m2 = np.identity(4, dtype=np.float32)

        if len(f_idxs) == 1:
            for b_idx in xrange(self.nBones):
                m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                q = tm.quaternion_slerp(REST_QUAT,
                                        tm.quaternion_from_matrix(m, True),
                                        float(weights[0]))
                result[b_idx] = tm.quaternion_matrix(q)[:3, :4]
        else:
            for b_idx in xrange(self.nBones):
                m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
                q1 = tm.quaternion_slerp(REST_QUAT,
                                         tm.quaternion_from_matrix(m1, True),
                                         float(weights[0]))
                q2 = tm.quaternion_slerp(REST_QUAT,
                                         tm.quaternion_from_matrix(m2, True),
                                         float(weights[1]))
                quat = tm.quaternion_multiply(q2, q1)

                for i, f_idx in enumerate(f_idxs[2:]):
                    i += 2
                    m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
                    q = tm.quaternion_slerp(REST_QUAT,
                                            tm.quaternion_from_matrix(m, True),
                                            float(weights[i]))
                    quat = tm.quaternion_multiply(q, quat)

                result[b_idx] = tm.quaternion_matrix(quat)[:3, :4]

        return Pose(self.name + '-blended', result)
Exemple #15
0
def point_rotation_by_quaternion(point, q):
    """
    Rotate a 3D point by quaternion
    :param point: [x, y, z]
    :param q: [qw, qx, qy, qz]
    :return rotated_point: [x, y, ]
    """
    r = [0, point[0], point[1], point[2]]
    q_conj = [q[0], -1 * q[1], -1 * q[2], -1 * q[3]]
    return quaternion_multiply(quaternion_multiply(q, r), q_conj)[1:]
 def on_timer(self, event):
     # auto rotating
     qy = tr.quaternion_about_axis(0.005, [0,0,1])
     qx = tr.quaternion_about_axis(0.005, [0,1,0])
     q = tr.quaternion_multiply(qx,qy)
     self.rot = tr.quaternion_multiply(self.rot, q)
     self.model = tr.quaternion_matrix(self.rot)
     self.program_data['u_model'] = self.model
     self.program_axis['u_model'] = self.model
     self.program_plane['u_model'] = self.model
     self.update()
Exemple #17
0
 def on_timer(self, event):
     # auto rotating
     qy = tr.quaternion_about_axis(0.005, [0, 0, 1])
     qx = tr.quaternion_about_axis(0.005, [0, 1, 0])
     q = tr.quaternion_multiply(qx, qy)
     self.rot = tr.quaternion_multiply(self.rot, q)
     self.model = tr.quaternion_matrix(self.rot)
     self.program_data['u_model'] = self.model
     self.program_axis['u_model'] = self.model
     self.program_plane['u_model'] = self.model
     self.update()
Exemple #18
0
def quaternion_rotate_vector(q, vector):
    """ src: http://math.stackexchange.com/questions/40164/how-do-you-rotate-a-vector-by-a-unit-quaternion
    Args:
        q:
        vector:

    Returns:

    """
    tmp_result = quaternion_multiply(q, vector)
    return quaternion_multiply(tmp_result, quaternion_conjugate(q))[1:]
def find_rotation_analytically(new_skeleton,
                               free_joint_name,
                               target,
                               frame,
                               joint_cos_map,
                               is_root=False,
                               max_iter_count=10,
                               twist_angle=None):
    global_src_up_vec = target[0]
    if twist_angle is None:
        global_src_x_vec = target[1]
    else:
        global_src_x_vec = None
    local_target_axes = dict(joint_cos_map[free_joint_name])

    if is_root:
        q = align_root_joint(new_skeleton, free_joint_name, local_target_axes,
                             global_src_up_vec, global_src_x_vec,
                             joint_cos_map, max_iter_count)

    else:
        # first align the bone vectors
        q = [1, 0, 0, 0]
        qy, axes = align_axis_in_place(local_target_axes, "y",
                                       global_src_up_vec)
        q = quaternion_multiply(qy, q)
        q = normalize(q)

        # then align the twisting angles
        if global_src_x_vec is not None:
            qx, axes = align_axis_in_place(axes, "x", global_src_x_vec)
            q = quaternion_multiply(qx, q)
            q = normalize(q)

    #if "FK" in free_joint_name:
    #    q = to_local_cos(new_skeleton, free_joint_name, frame, q)
    if new_skeleton.nodes[free_joint_name].parent is not None:
        #if "upLeg" in free_joint_name: # it does not work for the legs for some reason
        #    q = to_local_cos(new_skeleton, new_skeleton.nodes[free_joint_name].parent.node_name, frame, q)
        #else:
        q = to_local_cos(new_skeleton,
                         new_skeleton.nodes[free_joint_name].parent.node_name,
                         frame, q)

    if twist_angle is not None:
        # separate rotation
        local_twist_axis = np.array(joint_cos_map[free_joint_name]["y"])
        swing_q, twist_q = swing_twist_decomposition(q, local_twist_axis)
        # replace
        twist_q = quaternion_about_axis(-twist_angle, local_twist_axis)
        q = quaternion_multiply(swing_q, twist_q)
        q = normalize(q)
    return q
Exemple #20
0
    def norm(self):
        """:obj:`tuple` of :obj:`numpy.ndarray`: The normalized vectors for qr and qd, respectively.
        """
        qr_c = quaternion_conjugate(self._qr)
        qd_c = quaternion_conjugate(self._qd)

        qr_norm = np.linalg.norm(quaternion_multiply(self._qr, qr_c))
        qd_norm = np.linalg.norm(
            quaternion_multiply(self._qr, qd_c) +
            quaternion_multiply(self._qd, qr_c))

        return (qr_norm, qd_norm)
Exemple #21
0
 def set_orientation(self, orientation):
     print("rotate frames")
     current_q = self.mv.frames[self.frame_idx, 3:7]
     delta_q = quaternion_multiply(orientation,
                                   quaternion_inverse(current_q))
     delta_q /= np.linalg.norm(delta_q)
     delta_m = quaternion_matrix(delta_q)[:3, :3]
     for idx in range(self.mv.n_frames):
         old_t = self.mv.frames[idx, :3]
         old_q = self.mv.frames[idx, 3:7]
         self.mv.frames[idx, :3] = np.dot(delta_m, old_t)[:3]
         self.mv.frames[idx, 3:7] = quaternion_multiply(delta_q, old_q)
def align_joint(local_target_axes, up_vec, x_vec):
    q = [1, 0, 0, 0]
    qy, axes = align_axis_in_place(local_target_axes, "y", up_vec)
    q = quaternion_multiply(qy, q)
    q = normalize(q)

    # then align the twisting angles
    if x_vec is not None:
        qx, axes = align_axis_in_place(axes, "x", x_vec)
        q = quaternion_multiply(qx, q)
        q = normalize(q)
        # print("set twist angle", free_joint_name, twist_angle)
    return q
Exemple #23
0
 def get_pose_by_trajectory(self, node, last_pose, trj_type, trj_value,
                            direction, orientation):
     pose = np.copy(node.pose)
     last_root_pos = last_pose[:3]
     root_velocity = node.velocity[:3]
     if trj_type == "euler":
         current_pos = last_root_pos + np.linalg.norm(
             root_velocity) * direction
         pose[:3] = current_pos
         pose[3:7] = quaternion_multiply(pose[3:7], node.velocity[3:7])
         pose[3:7] = quaternion_multiply(
             pose[3:7], quaternion_from_euler(orientation, 0, 0))
     return pose
Exemple #24
0
    def getBlendedPose(self, poses, weights, additiveBlending=True):
        import transformations as tm

        REST_QUAT = np.asarray([1,0,0,0], dtype=np.float32)

        if isinstance(poses[0], basestring):
            f_idxs = [self.getPoseNames().index(pname) for pname in poses]
        else:
            f_idxs = poses

        if not additiveBlending:
            # normalize weights
            if not isinstance(weights, np.ndarray):
                weights = np.asarray(weights, dtype=np.float32)
            t = sum(weights)
            if t < 1:
                # Fill up rest with neutral pose (neutral pose is assumed to be first frame)
                weights = np.asarray(weights + [1.0-t], dtype=np.float32)
                f_idxs.append(0)
            weights /= t

        #print zip([self.getPoseNames()[_f] for _f in f_idxs],weights)

        result = emptyPose(self.nBones)
        m = np.identity(4, dtype=np.float32)
        m1 = np.identity(4, dtype=np.float32)
        m2 = np.identity(4, dtype=np.float32)

        if len(f_idxs) == 1:
            for b_idx in xrange(self.nBones):
                m[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[0]))
                result[b_idx] = tm.quaternion_matrix( q )[:3,:4]
        else:
            for b_idx in xrange(self.nBones):
                m1[:3, :4] = self.getAtFramePos(f_idxs[0], True)[b_idx]
                m2[:3, :4] = self.getAtFramePos(f_idxs[1], True)[b_idx]
                q1 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m1, True), float(weights[0]))
                q2 = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m2, True), float(weights[1]))
                quat = tm.quaternion_multiply(q2, q1)

                for i,f_idx in enumerate(f_idxs[2:]):
                    i += 2
                    m[:3, :4] = self.getAtFramePos(f_idx, True)[b_idx]
                    q = tm.quaternion_slerp(REST_QUAT, tm.quaternion_from_matrix(m, True), float(weights[i]))
                    quat = tm.quaternion_multiply(q, quat)

                result[b_idx] = tm.quaternion_matrix( quat )[:3,:4]

        return Pose(self.name+'-blended', result)
Exemple #25
0
def lamePath(dist, ang, side=50.8, r=3, oset=10, compAng=numpy.radians(0)):

    q = numpy.array([[0., 0., -0.71, 0.71], [0, 0, 1, 0], [0., 0., 0.71,
                                                           0.71]])
    if numpy.round(ang % 3.141, 2) > .01: d = side / numpy.tan(ang)
    else: d = 0
    do = oset * numpy.cos(ang)
    zo = oset * numpy.sin(ang)
    s = numpy.array([[-do, side / 2, -zo], [d + do, side / 2, side + zo]])
    s1 = numpy.array([[d + do, -side / 2, zo + side], [-do, -side / 2, -zo]])
    top = [[d, ((side / 2) + oset), side], [d, -((side / 2) + oset), side]]

    topAng = (numpy.pi / 2) - ang
    botAng = -topAng

    topAng = thresh(topAng, numpy.radians(50))
    botAng = thresh(botAng, numpy.radians(50))

    qBot = transformations.quaternion_multiply(
        q[1], transformations.quaternion_about_axis(botAng, [0, 1, 0]))

    print '\n\n\QBOT', qBot

    angVec = [numpy.cos(ang), 0, numpy.sin(ang)]

    q[0] = transformations.quaternion_multiply(
        q[0], transformations.quaternion_about_axis(-compAng, angVec))
    q[1] = transformations.quaternion_multiply(
        q[1], transformations.quaternion_about_axis(topAng, [0, 1, 0]))
    q[2] = transformations.quaternion_multiply(
        q[2], transformations.quaternion_about_axis(compAng, angVec))

    q = unitVector(q)

    s = numpy.hstack((s, [q[0], q[0]]))
    s2 = numpy.hstack((s1, [q[2], q[2]]))
    top = numpy.hstack((top, [q[1], q[1]]))

    top = numpy.array([s, top, s2])

    numpy.set_printoptions(precision=3, suppress=True)
    top[:, :, 0] -= numpy.min(top[:, :, 0]) + dist

    bf = 1.375
    bottom = [[-dist + (bf * do), -((side / 2) + oset), side],
              [-dist + (bf * do), (side / 2) + oset, side]]

    bottom = numpy.hstack((bottom, [qBot, qBot]))

    return (top, bottom)
Exemple #26
0
def quaternion_fromto_test(tb):
    q1 = tb.utils.ArrayToQuat([0, 0, 0, 1])
    q2 = tb.utils.ArrayToQuat([0, 0, 0, 1])
    q3 = tb.utils.QuaternionFromTo(q1, q2)

    q4 = tb.utils.MultiplyQuaternion(q1, q3)

    print q3
    print q4

    print transformations.quaternion_multiply(tb.utils.QuatToArray(q2),
                                              tb.utils.QuatToArray(q3))

    raw_input("Press enter to cont...")
Exemple #27
0
def virtual(worktarget=[945, 0, 0], cartesians=[945, 0, 1375]):
    # returns a virtual target that the robot can actually reach
    # virtual target will be pointed towards worktarget and located at cartesians

    # Quaternion [0,0,1,0] has z vector [0,0,-1]
    z0 = [0, 0, -1]
    zq0 = [0, 0, 1, 0]
    a = [[0], [0], [1], [1]]
    if (len(cartesians) == 3) & (len(worktarget) == 3):

        #unit direction vector from defined robot position to worktarget position
        dvec = unitv((worktarget - numpy.array(cartesians)))

        if (~parallel(dvec, z0)):
            rotvec = numpy.cross(dvec, z0)
            rotangle = numpy.arcsin(norm(rotvec) / (norm(z0) * norm(dvec)))
            rotquat = transformations.quaternion_about_axis(-rotangle, rotvec)
            virtualresult = [
                cartesians,
                transformations.quaternion_multiply(rotquat, zq0)
            ]
        else:
            virtualresult = [cartesians, zq0]

        return virtualresult
    else:
        return False
Exemple #28
0
def graspExecute(limb, W, H, Ang):
    endEffPos = pixelToWorld(W, H)
    quat1 = transformations.quaternion_about_axis(Ang, (0, 0, 1))
    quat2 = transformations.quaternion_about_axis(np.pi, (1, 0, 0))
    quat = transformations.quaternion_multiply(quat1, quat2)
    # print(quat)

    top_grasp_joint = ik_service_client(limb='right',
                                        use_advanced_options=True,
                                        p_x=endEffPos[0],
                                        p_y=endEffPos[1],
                                        p_z=0.5,
                                        q_x=quat[0],
                                        q_y=quat[1],
                                        q_z=quat[2],
                                        q_w=quat[3])

    down_grasp_joint = ik_service_client(limb='right',
                                         use_advanced_options=True,
                                         p_x=endEffPos[0],
                                         p_y=endEffPos[1],
                                         p_z=0.1,
                                         q_x=quat[0],
                                         q_y=quat[1],
                                         q_z=quat[2],
                                         q_w=quat[3])

    move(limb, positions=top_grasp_joint, move_speed=0.2)
    rospy.sleep(2)
    move(limb, positions=down_grasp_joint, move_speed=0.2)
    rospy.sleep(2)
    move(limb, positions=top_grasp_joint, move_speed=0.2)
    def _create_actions_from_unity_format(self):
        if self._constraints_dict is None:
            return

        p = self._constraints_dict["startPosition"]
        q = self._constraints_dict["startOrientation"]
        start_pose = dict()
        start_pose["position"] = np.array([p["x"], p["y"], p["z"]])
        q = np.array([q["w"], q["x"], q["y"], q["z"]])
        q = quaternion_multiply([0, 0, 1, 0], q)
        start_pose["orientation"] = np.degrees(euler_from_quaternion(q))
        print("start rotation", start_pose["orientation"])
        self._controller.start_pose = start_pose
        self._action_sequence = []
        print("create objects from constraints")
        scene = self._parent.app_manager.getDisplayedScene()
        a = [None, []]
        a[0] = self._constraints_dict["name"]
        a[1] = []
        for c in self._constraints_dict["frameConstraints"]:
            p = c["position"]
            p = np.array([p["x"], p["y"], p["z"]])
            # q = c["orientation"]
            # c["orientation"] = np.array([q["w"], q["x"], q["y"], q["z"]])
            annotation = c["keyframe"]
            joint_name = c["joint"]
            scene_object = PositionConstraintObject(p, 2.5)
            scene.addObject(scene_object)
            a[1].append(
                ConstraintDefinition(scene_object,
                                     joint_name,
                                     annotation=annotation))

        self._action_sequence.append(a)
        self.update_action_sequence_list()
def transform_quaternion_frames_legacy(quat_frames,
                                       angles,
                                       offset,
                                       rotation_order=None):
    """ Applies a transformation on the root joint of a list quaternion frames.
    Parameters
    ----------
    *quat_frames: np.ndarray
    \tList of frames where the rotation is represented as euler angles in degrees.
    *angles: list of floats
    \tRotation angles in degrees
    *offset:  np.ndarray
    \tTranslation
    """
    if rotation_order is None:
        rotation_order = DEFAULT_ROTATION_ORDER
    offset = np.array(offset)
    if round(angles[0], 3) == 0 and round(angles[2], 3) == 0:
        rotation_q = quaternion_about_axis(np.deg2rad(angles[1]), [0, 1, 0])
    else:
        rotation_q = euler_to_quaternion(angles, rotation_order)
    rotation_matrix = euler_angles_to_rotation_matrix(angles,
                                                      rotation_order)[:3, :3]
    for frame in quat_frames:
        ot = frame[:3]
        oq = frame[3:7]
        frame[:3] = np.dot(rotation_matrix, ot) + offset
        frame[3:7] = quaternion_multiply(rotation_q, oq)
    return quat_frames
Exemple #31
0
    def apply_joint_rotation_offset(self,
                                    joint_name,
                                    euler,
                                    frame_range=None,
                                    blend_window_size=None):
        motion = self.get_motion()
        print("euler ", euler)
        delta_q = quaternion_from_euler(*np.radians(euler))
        if motion.frames.ndim == 1:
            motion.frames = motion.frames.reshape((1, len(motion.frames)))
        j_offset = self.get_skeleton().animated_joints.index(
            joint_name) * 4 + 3
        if frame_range is None:
            start_frame = 0
            end_frame = motion.n_frames
        else:
            start_frame, end_frame = frame_range
            end_frame = min(motion.n_frames, end_frame + 1)
        for idx in range(start_frame, end_frame):
            old_q = motion.frames[idx, j_offset:j_offset + 4]
            motion.frames[idx, j_offset:j_offset + 4] = quaternion_multiply(
                delta_q, old_q)

        if frame_range is not None and blend_window_size is not None:
            joint_list = [joint_name]
            offset = self.skeleton.nodes[
                joint_name].quaternion_frame_index * 4 + 3
            joint_index_list = list(range(offset, offset + 4))
            motion.frames = apply_blending(self.skeleton, motion.frames,
                                           joint_list, joint_index_list,
                                           start_frame, end_frame - 1,
                                           blend_window_size)
Exemple #32
0
 def get_rotation_from_pc(self, src_name, target_name, src_pc, src_frame):
     child_name = self.src_child_map[src_name]
     global_src_up_vec, global_src_x_vec = self.estimate_src_joint_cos(
         src_name, child_name, target_name, src_pc)
     if "Spine" in src_name:
         print("axis", src_name, global_src_up_vec, global_src_up_vec)
     q = [1, 0, 0, 0]
     local_target_axes = self.src_cos_map[src_name]
     qy, axes = align_axis(local_target_axes, "y", global_src_up_vec)
     q = quaternion_multiply(qy, q)
     q = normalize(q)
     if global_src_x_vec is not None:
         qx, axes = align_axis(axes, "x", global_src_x_vec)
         q = quaternion_multiply(qx, q)
         q = normalize(q)
     return to_local_cos(self.src_skeleton, src_name, src_frame, q)
Exemple #33
0
 def to_local_cos2(self, joint_name, frame, q):
     # bring into parent coordinate system
     parent_joint = self.skeleton.nodes[joint_name].parent.node_name
     pm = self.skeleton.nodes[parent_joint].get_global_matrix(frame)#[:3, :3]
     inv_p = quaternion_inverse(quaternion_from_matrix(pm))
     normalize(inv_p)
     return quaternion_multiply(inv_p, q)
 def rotate_around(self, angle, axis):
     '''
     Rotate the render object around the axis.
     Raises an error if self.orientation is None.
     '''
     ori = self.orientation
     if ori is None:
         raise NotImplemented("This RenderObject has no orientation")
     self.orientation = quaternion_multiply(ori, quaternion_about_axis(angle, axis))
 def rotate(self, roll, pitch, yaw):
     '''
     Rotate the render object around the axes. 
     Raises an error if self.orientation is None.
     '''
     ori = self.orientation
     if ori is None:
         raise 
     self.orientation = quaternion_multiply(quaternion_from_euler(roll, pitch, yaw), ori)
    def on_mouse_move(self, event):
        # drag to rotate
        if event.is_dragging:# and not self.timer.running:
            # drag delta
            dx = event.pos[0]-self.last_drag_pos[0]
            dy = event.pos[1]-self.last_drag_pos[1]
            dampen = 0.05
            # quaternion rotation
            qy = tr.quaternion_about_axis(dx*dampen, [0,-1,0])
            qx = tr.quaternion_about_axis(dy*dampen, [-1,0,0])
            q = tr.quaternion_multiply(qx,qy)
            self.rot = tr.quaternion_multiply(self.rot, q)
            # rotate model
            self.model = tr.quaternion_matrix(self.rot)
            # update model
            self.program_data['u_model'] = self.model
            self.program_axis['u_model'] = self.model
            self.program_plane['u_model'] = self.model
            self.update()

            self.last_drag_pos = event.pos
        pass
def plus(x1, x2):
    """Returns the SO3 quantity that represents first rotating
    by x1 and then by x2 (i.e. the composition of x2 on x1, call
    it "x1+x2"). The output will be in the same representation as the
    inputs, but the inputs must be in the same representation as each other.

    >>> R1 = trns.random_rotation_matrix()[:3, :3]
    >>> R2 = trns.random_rotation_matrix()[:3, :3]
    >>> R3 = R2.dot(R1)  # rotating vector v by R3 is R2*R1*v
    >>> R = plus(R1, R2)
    >>> print(np.allclose(R, R3))
    True
    >>> M1 = make_affine(R1, [4, 4, 4])
    >>> M2 = make_affine(R2, [5, 5, 5])
    >>> M3 = make_affine(R2.dot(R1), R2.dot([4, 4, 4]) + [5, 5, 5])
    >>> M = plus(M1, M2)
    >>> print(np.allclose(M, M3))
    True
    >>> print(np.allclose(M[:3, :3], R))
    True
    >>> q1 = trns.quaternion_from_matrix(R1)
    >>> q2 = trns.quaternion_from_matrix(R2)
    >>> q3 = trns.quaternion_multiply(q2, q1)
    >>> q = plus(q1, q2)
    >>> print(np.allclose(q, q3))
    True
    >>> print(np.allclose(trns.quaternion_matrix(q)[:3, :3], R))
    True
    >>> R1plusR2plusR3 = reduce(plus, [R1, R2, R3])  # using plus in succession
    >>> print(rep(R1plusR2plusR3))
    rotmat

    """
    xrep = rep(x1)
    xrep2 = rep(x2)
    if xrep != xrep2:
        raise ValueError('Values to "add" are not in the same SO3 representation.')
    if xrep == 'quaternion':
        return normalize(trns.quaternion_multiply(x2, x1))
    elif xrep in ['rotmat', 'tfmat']:
        return x2.dot(x1)
    elif xrep == 'rotvec':
        # Adding rotvecs is only valid for small perturbations...
        # Perform operation on actual SO3 manifold instead:
        x1 = matrix_from_rotvec(x1)
        x2 = matrix_from_rotvec(x2)
        return get_rotvec(x2.dot(x1))
    else:
        raise ValueError('Values to "add" are not on SO3.')
def minus(x1, x2):
    """Returns the SO3 quantity representing the minimal rotation from
    orientation x1 to orientation x2 (i.e. the inverse composition of x2 on x1, 
    call it "x2-x1"). The output will be in the same representation as the
    inputs, but the inputs must be in the same representation as each other.

    >>> A = trns.rotation_matrix(.2, [0, 1, 0])  # rotation of 0.2 rad about +y axis
    >>> B = trns.rotation_matrix(-.2, [0, 1, 0])  # rotation of -0.2 rad about +y axis
    >>> AtoB = minus(A, B)  # this is "B - A", the rotation from A to B
    >>> angle, axis = angle_axis(AtoB)  # should be a rotation of (-0.2)-(0.2) = -0.4 rad about +y axis
    >>> print(angle, axis)
    (0.39999999999999997, array([-0., -1., -0.]))
    >>> # The above is 0.4 rad about -y axis, which is equivalent to the expected -0.4 rad about the +y axis.
    >>> # As usual, the angle is always returned between 0 and pi, with the accordingly correct axis.
    >>> # The rotations need not be about the same axis of course:
    >>> NtoA = trns.random_quaternion()  # some rotation from frame N to frame A
    >>> NtoB = trns.random_quaternion()  # some rotation from frame N to frame B
    >>> AtoB = minus(NtoA, NtoB)  # We say "AtoB = NtoB - NtoA"
    >>> NtoAtoB = plus(NtoA, AtoB)  # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
    >>> print(np.allclose(NtoAtoB, NtoB))
    True
    >>> # Evidently, plus and minus are inverse operations.
    >>> # "x1 + (x2 - x1) = x2"  reads as "(N to x1) plus (x1 to x2) = (N to x2)"
    >>> A[:3, 3] = trns.random_vector(3)
    >>> B[:3, 3] = trns.random_vector(3)
    >>> C1 = B.dot(npl.inv(A))
    >>> C2 = minus(A, B)
    >>> print(np.allclose(C1, C2))
    True

    """
    xrep = rep(x1)
    xrep2 = rep(x2)
    if xrep != xrep2:
        raise ValueError('Values to "subtract" are not in the same SO3 representation.')
    if xrep == 'quaternion':
        return normalize(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
    elif xrep == 'rotmat':
        return x2.dot(x1.T)
    elif xrep == 'tfmat':
        return x2.dot(npl.inv(x1))  # inverse of a tfmat is not its transpose
    elif xrep == 'rotvec':
        # Subtracting rotvecs is only valid for small perturbations...
        # Perform operation on actual SO3 manifold instead:
        x1 = matrix_from_rotvec(x1)
        x2 = matrix_from_rotvec(x2)
        return get_rotvec(x2.dot(x1.T))
    else:
        raise ValueError('Values to "subtract" are not on SO3.')
def slerp(x1, x2, fraction):
    """Spherical Linear intERPolation. Returns an SO3 quantity representing an orientation
    between x1 and x2. The fraction of the path from x1 to x2 is the fraction input, and it
    must be between 0 and 1. The output will be in the same representation as the inputs, but
    the inputs must be in the same representation as each other.

    >>> x1 = make_affine(trns.random_rotation_matrix()[:3, :3], [1, 1, 1])
    >>> x2 = make_affine(trns.random_rotation_matrix()[:3, :3], [2, 2, 2])
    >>> nogo = slerp(x1, x2, 0)
    >>> print(np.allclose(nogo, x1))
    True
    >>> allgo = slerp(x1, x2, 1)
    >>> print(np.allclose(allgo, x2))
    True
    >>> first_25 = slerp(x1, x2, 0.25)  # 0 to 25 percent from x1 to x2
    >>> last_75 = minus(first_25, x2)  # 25 to 75 percent from x1 to x2
    >>> wholeway = plus(first_25, last_75)
    >>> print(np.allclose(wholeway, x2))
    True
    >>> x1 = trns.quaternion_from_matrix(x1)
    >>> x2 = trns.quaternion_from_matrix(x2)
    >>> mine = slerp(x1, x2, 0.3)
    >>> his = trns.quaternion_slerp(x1, x2, 0.3)  # only works on quaternions
    >>> print(np.allclose(mine, his))
    True

    """
    xrep = rep(x1)
    xrep2 = rep(x2)
    if xrep != xrep2:
        raise ValueError('Values to slerp between are not in the same SO3 representation.')
    if xrep == 'quaternion':
        r12 = get_rotvec(trns.quaternion_multiply(x2, trns.quaternion_inverse(x1)))
        x12 = normalize(quaternion_from_rotvec(fraction * r12))
    elif xrep == 'rotmat':
        r12 = get_rotvec(x2.dot(x1.T))
        x12 = matrix_from_rotvec(fraction * r12)
    elif xrep == 'tfmat':
        M12 = x2.dot(npl.inv(x1))
        r12 = get_rotvec(M12)
        x12 = make_affine(matrix_from_rotvec(fraction * r12), fraction * M12[:3, 3])
    elif xrep == 'rotvec':
        R1 = matrix_from_rotvec(x1)
        R2 = matrix_from_rotvec(x2)
        x12 = fraction * get_rotvec(R2.dot(R1.T))
    else:
        raise ValueError("One or both values to slerp between are not on SO3.")
    return plus(x1, x12)
Exemple #40
0
def computeCameraPoseFromAircraft(image, cam, ref,
                                  yaw_bias=0.0, roll_bias=0.0,
                                  pitch_bias=0.0, alt_bias=0.0):
    lla, ypr, ned2body = image.get_aircraft_pose()
    aircraft_lat, aircraft_lon, aircraft_alt = lla
    #print "aircraft quat =", world2body
    msl = aircraft_alt + image.alt_bias + alt_bias

    (camera_yaw, camera_pitch, camera_roll) = cam.get_mount_params()
    body2cam = transformations.quaternion_from_euler(camera_yaw * d2r,
                                                     camera_pitch * d2r,
                                                     camera_roll * d2r,
                                                     'rzyx')
    ned2cam = transformations.quaternion_multiply(ned2body, body2cam)
    (yaw, pitch, roll) = transformations.euler_from_quaternion(ned2cam, 'rzyx')
    ned = navpy.lla2ned( aircraft_lat, aircraft_lon, aircraft_alt,
                         ref[0], ref[1], ref[2] )
    #print "aircraft=%s ref=%s ned=%s" % (image.get_aircraft_pose(), ref, ned)
    return (ned.tolist(), [yaw/d2r, pitch/d2r, roll/d2r])
def minus(q1, q2):
    """Returns a quaternion representing the minimal rotation from
    orientation q1 to orientation q2 (i.e. the inverse composition
    of q2 on q1, call it "q2-q1").

    >>> NtoA = trns.random_quaternion()  # some rotation from frame N to frame A
    >>> NtoB = trns.random_quaternion()  # some rotation from frame N to frame B
    >>> AtoB = minus(NtoA, NtoB)  # We say "AtoB = NtoB - NtoA"
    >>> NtoAtoB = plus(NtoA, AtoB)  # NtoAtoB == NtoB and we say "NtoAtoB = NtoA + AtoB"
    >>> print(np.allclose(NtoAtoB, NtoB))
    True
    >>> # Evidently, plus and minus are inverse operations.
    >>> # "q1 + (q2 - q1) = q2"  reads as "(N to q1) plus (q1 to q2) = (N to q2)"
    >>> q = plus(NtoA, minus(NtoA, NtoB))
    >>> print(np.allclose(q, NtoB))
    True

    """
    return normalize(trns.quaternion_multiply(q2, trns.quaternion_inverse(q1)))
def plus(q1, q2):
    """Returns the quaternion that represents first rotating
    by q1 and then by q2 (i.e. the composition of q2 on q1,
    call it "q1+q2").

    >>> q1 = trns.random_quaternion()
    >>> q2 = trns.random_quaternion()
    >>> q12 = plus(q1, q2)
    >>>
    >>> p1 = trns.random_vector(3)
    >>> p2 = qapply_points(q1, p1)
    >>> p3a = qapply_points(q2, p2)
    >>>
    >>> p3b = qapply_points(q12, p1)
    >>> print(np.allclose(p3a, p3b))
    True

    """
    return normalize(trns.quaternion_multiply(q2, q1))
Exemple #43
0
	def ode( self , t , Q ) :
		w = Q[:3]
		q = Q[3:]
		q = q / np.linalg.norm( q )

		qm = tr.inverse_matrix( tr.quaternion_matrix(q) )

		if self.drawstate & GRAVITY :
			self.G = np.resize( np.dot( qm , self.g ) , 3 )
			N = np.cross( self.r , self.G )
		else :
			N = np.zeros(3)

#        print self.G , N , np.linalg.norm(self.G) , np.linalg.norm(w)

		QP = np.empty(7,np.float64)
		QP[:3] = np.dot( self.Mi , ( N + np.cross( np.dot(self.M,w) , w ) ) )
		qw = np.empty(4,np.float64)
		qw[0]  = 0
		qw[1:] = w
		QP[3:] = tr.quaternion_multiply( q , qw ) / 2.0

		return QP
bgain = 0.001

yawEst = []
yawINS = []
pitchEst = []
pitchINS = []
rollEst = []
rollINS = []
yaw_meas = []
omegas = []
yaw_crct = []
for i,t in enumerate(time):
#   integrate ins
    omega = np.array([0,p[i], q[i], r[i]])
    omega_bias = np.array([0,p[i]-biasEst[0], q[i]-biasEst[1], r[i]-biasEst[2]])
    q_ins = q_est + 0.5*dt*trans.quaternion_multiply(q_est, omega_bias)
    q_insOnly = q_insOnly + 0.5*dt*trans.quaternion_multiply(q_insOnly, omega)
    yawINS.append(trans.euler_from_quaternion(q_insOnly)[2])
    pitchINS.append(trans.euler_from_quaternion(q_insOnly)[0]) 
    rollINS.append(trans.euler_from_quaternion(q_insOnly)[1])


#   generate truth quaternion
    #quat_meas = trans.quaternion_from_euler(yaw[i],pitch[i],roll[i])
    q_meas = trans.quaternion_from_euler(roll[i],pitch[i],yaw[i])
        
    yaw_meas.append(trans.euler_from_quaternion(q_meas)[0])
        
    q_error = trans.quaternion_multiply(q_meas,trans.quaternion_inverse(q_ins))
    q_error = q_error / np.linalg.norm(q_error)
    q_error = np.hstack((1,q_error[1:4]))
Exemple #45
0
import robot
import transformations as tr
import numpy as np
r = robot.Robot()
print "joints"
print r.getJoints()
r.setCartesian([[800,0,1000], [0,0,1,0]]) #quat [0,0,1,0] is tool
# flange pointed down
quat_down = [0,0,1,0]
quat_stuff = tr.quaternion_multiply(quat_down,
tr.quaternion_about_axis(np.radians(30), [0,1,0])) #rotate 30 degrees
# around the Y axis
r.setCartesian([[800,0,1000], quat_stuff])
r.close()
            flight_mode = 'auto'            

        body2cam = transformations.quaternion_from_euler( cam_yaw * d2r,
                                                          cam_pitch * d2r,
                                                          cam_roll * d2r,
                                                          'rzyx')

        #print 'att:', [yaw_rad, pitch_rad, roll_rad]
        ned2body = transformations.quaternion_from_euler(yaw_rad,
                                                         pitch_rad,
                                                         roll_rad,
                                                         'rzyx')
        body2ned = transformations.quaternion_inverse(ned2body)
        
        #print 'ned2body(q):', ned2body
        ned2cam_q = transformations.quaternion_multiply(ned2body, body2cam)
        ned2cam = np.matrix(transformations.quaternion_matrix(np.array(ned2cam_q))[:3,:3]).T
        #print 'ned2cam:', ned2cam
        R = ned2proj.dot( ned2cam )
        rvec, jac = cv2.Rodrigues(R)
        ned = navpy.lla2ned( lat_deg, lon_deg, altitude_m,
                             ref[0], ref[1], ref[2] )
        #print 'ned:', ned
        tvec = -np.matrix(R) * np.matrix(ned).T
        R, jac = cv2.Rodrigues(rvec)
        # is this R the same as the earlier R?
        PROJ = np.concatenate((R, tvec), axis=1)
        #print 'PROJ:', PROJ
        #print lat_deg, lon_deg, altitude, ref[0], ref[1], ref[2]
        #print ned
Exemple #47
0
    def createAnimationTrack(self, skel=None, name=None):
        """
        Create an animation track from the motion stored in this BHV file.
        """
        def _bvhJointName(boneName):
            # Remove the tail from duplicate bone names (added by the BVH parser)
            import re
            if not boneName:
                return boneName
            r = re.search("(.*)_\d+$", boneName)
            if r:
                return r.group(1)
            return boneName

        def _createAnimation(jointsData, name, frameTime, nFrames):
            nJoints = len(jointsData)
            #nFrames = len(jointsData[0])

            # Interweave joints animation data, per frame with joints in breadth-first order
            animData = np.hstack(jointsData).reshape(nJoints*nFrames,3,4)
            framerate = 1.0/frameTime
            return animation.AnimationTrack(name, animData, nFrames, framerate)

        if name is None:
            name = self.name

        if skel is None:
            jointsData = [joint.matrixPoses for joint in self.getJoints() if not joint.isEndConnector()]
            # We leave out end effectors as they should not have animation data

            return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
        elif isinstance(skel, list):
            # skel parameter is a list of joint or bone names
            jointsOrder = skel

            jointsData = []
            for jointName in jointsOrder:
                jointName = _bvhJointName(jointName)
                if jointName and self.getJointByCanonicalName(jointName) is not None:
                    poseMats = self.getJointByCanonicalName(jointName).matrixPoses.copy()
                    jointsData.append(poseMats)
                else:
                    jointsData.append(animation.emptyTrack(self.frameCount))

            return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
        else:
            # skel parameter is a Skeleton
            jointsData = []
            for bone in skel.getBones():
                if len(bone.reference_bones) > 0:
                    # Combine the rotations of reference bones to influence this bone
                    bvhJoints = []
                    for bonename in bone.reference_bones:
                        jointname = _bvhJointName(bonename)
                        joint = self.getJointByCanonicalName(jointname)
                        if joint:
                            bvhJoints.append(joint)

                    if len(bvhJoints) == 0:
                        poseMats = animation.emptyTrack(self.frameCount)
                    elif len(bvhJoints) == 1:
                        poseMats = bvhJoints[0].matrixPoses.copy()
                    else:  # len(bvhJoints) >= 2:
                        # Combine the rotations using quaternions to simplify math and normalizing (rotations only)
                        poseMats = animation.emptyTrack(self.frameCount)
                        m = np.identity(4, dtype=np.float32)
                        for f_idx in xrange(self.frameCount):
                            m[:3,:4] = bvhJoints[0].matrixPoses[f_idx]
                            q1 = tm.quaternion_from_matrix(m, True)
                            m[:3,:4] = bvhJoints[1].matrixPoses[f_idx]
                            q2 = tm.quaternion_from_matrix(m, True)

                            quat = tm.quaternion_multiply(q2, q1)

                            for bvhJoint in bvhJoints[2:]:
                                m[:3,:4] = bvhJoint.matrixPoses[f_idx]
                                q = tm.quaternion_from_matrix(m, True)
                                quat = tm.quaternion_multiply(q, quat)

                            poseMats[f_idx] = tm.quaternion_matrix( quat )[:3,:4]

                    jointsData.append(poseMats)
                else:
                    # Map bone to joint by bone name
                    jointName = _bvhJointName(bone.name)
                    joint = self.getJointByCanonicalName(jointName)
                    if joint:
                        jointsData.append( joint.matrixPoses.copy() )
                    else:
                        jointsData.append(animation.emptyTrack(self.frameCount))

            return _createAnimation(jointsData, name, self.frameTime, self.frameCount)
def FindVariance(quaternions,delta_t=0.1,motion_frequency=3,plot=False,filt_type='ellip',method='kevin',attitude='azelbore'):
    '''
    Purpose: Find the variance of a set of quaternions. Low Frequency components 
    are assumed to be invalid and will be discarded. Intended for analyzing
    high-frequency variations in a set of rotation quaternions, specifically for 
    the DayStar platform.
        
    Inputs: quaternions - List of quaternion arrays, formatted 'SXYZ' I think.
            delta_t - (optional) Time between quaternion observations [s].
                      Assumed (and must be) uniform for all frames
            motion_frequency - (optional) The Frequency we want to filter below
            plot - (optional) Set to generate plots of the Fourier filterig as 
                   we do it, to see how the signal changes
            variable - (optional), the name of the signal being filtered. Used for
                       plot labeling.
            {filt_type} - (optional) Specify filter type. Either:
                    * 'ellip' - try a scipy elliptical filter
                    * 'brick' - try a simple brick wall filter
                    
    Outputs: var - The computed variance of all observations. Meant to be some 
                   indication of DayStar performance

    **NOTE**
    MOTION_FREQUENCY KEYWORD MUST BE GREATER THAN 1 FOR ELLIPTICAL FILTER. 
    It is an inverse ratio of the full spectrum that we wish to cut off. So a
    '4' will cutoff (1/4) of the frequency spectrum.
    '''
    
    # Hey, do this later smarter
    quats=[]
    qtmp=quaternions[0]
    
    for q in quaternions:
        # Get rotation from epoch by multiplying each quaternion by the sum
        # of all previous quaternions
        qtmp=transform.quaternion_multiply(qtmp,q)     
        
        if method == 'kevin':
            quats.append(np.array(np.hstack([qtmp[3],qtmp[0:3]])))
        else:
            quats.append(np.array(np.hstack([qtmp[0:3],qtmp[3]])))

    if attitude == 'azelbore':
        # 3D frame unit vectors
        x = np.array([1, 0, 0])
        y = np.array([0, 1, 0])
        z = np.array([0, 0, 1])
        
        Y = []
        P = []
        R = []
        
        for q in quats:
            # Find rotation matrix from quaternion:
            M = quat2dcm(q)
            
            # Rotate the x and y axes of the 3D frame
            xhat = np.dot(M,x)
            yhat = np.dot(M,y)
            
            # Find azimuth and elevation:
            az = np.arctan2(xhat[1], xhat[0])
            el = np.arcsin(xhat[2])
            
            # Find unrotated boresite y-axis:
            yhatp = np.array([-np.sin(az), np.cos(az), 0])
            
            # Find the boresight rotation:
            phi = np.arccos(np.dot(yhat,yhatp))
            if yhat[2] < 0:
                phi = -phi
            
            # Store data
            Y.append(az)
            P.append(-el)
            R.append(phi)
            
        y_filt = high_pass(Y,  cutoff=motion_frequency, delta=delta_t, plot=plot,
                           variable='Yaw', filt_type=filt_type, color='blue') #radians
        p_filt = high_pass(P,  cutoff=motion_frequency, delta=delta_t, plot=plot,
                           variable='Pitch', filt_type=filt_type, color='purple') #radians
        r_filt = high_pass(R, cutoff=motion_frequency, delta=delta_t, plot=plot,
                           variable='Roll', filt_type=filt_type, color='green') #radians
            
    else:
        [Y,P,R]=quat2ypr(quats,method=method)
    
        y_filt = high_pass(Y,cutoff=motion_frequency,delta=delta_t,plot=plot,
                           variable='yaw',filt_type=filt_type,color='blue') #radians
        p_filt = high_pass(P,cutoff=motion_frequency,delta=delta_t,plot=plot,
                           variable='pitch',filt_type=filt_type,color='purple') #radians
        r_filt = high_pass(R,cutoff=motion_frequency,delta=delta_t,plot=plot,
                           variable='roll',filt_type=filt_type,color='green') #radians


    # Project filtered results on 2d plane:
    #project2d(y_filt,p_filt,r_filt)
    project2dquat(quats)
    project2d(y_filt,p_filt,r_filt)
        
    #Standard deviation
    #obs_std = np.sqrt(np.std(r_filt)**2 + np.std(p_filt)**2 + np.std(y_filt)**2)    
    y_std = 3600*(np.std(y_filt)*180/math.pi)
    p_std = 3600*(np.std(p_filt)*180/math.pi)
    r_std = 3600*(np.std(r_filt)*180/math.pi)
    
    y_var = y_std**2
    p_var = p_std**2
    r_var = r_std**2
    #var = 3600*(obs_std*180/math.pi)**2  # arcseconds

    return y_var,p_var,r_var,y_filt,p_filt,r_filt