Python transformations.quaternion_from_matrix函数代码示例

def interp_transforms(T1, T2, alpha):
    assert alpha <= 1
    T_avg = alpha * T1 + (1 - alpha) * T2
    q1 = quaternion_from_matrix(T1)
    q2 = quaternion_from_matrix(T2)
    q3 = quaternion_slerp(q1, q2, alpha)
    R = quaternion_matrix(q3)
    T_avg[0:3, 0:3] = R[0:3, 0:3]
    return T_avg

 def matrix_slerp(matA, matB, alpha=0.6):
     if matA is None:
         return matB
     import transformations
     qA = transformations.quaternion_from_matrix(matA)
     qB = transformations.quaternion_from_matrix(matB)
     qC =transformations.quaternion_slerp(qA, qB, alpha)
     mat = matB.copy()
     mat[:3,3] = (alpha)*matA[:3,3] + (1-alpha)*matB[:3,3]
     mat[:3,:3] = transformations.quaternion_matrix(qC)[:3,:3]
     return mat

    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)

 def setRotationIndex(self, index, angle, useQuat):
     """
     Set the rotation for one of the three rotation channels, either as
     quaternion or euler matrix. index should be 1,2 or 3 and represents
     x, y and z axis accordingly
     """
     if useQuat:
         quat = tm.quaternion_from_matrix(self.matPose)
         log.debug("%s", str(quat))
         quat[index] = angle/1000
         log.debug("%s", str(quat))
         _normalizeQuaternion(quat)
         log.debug("%s", str(quat))
         self.matPose = tm.quaternion_matrix(quat)
         return quat[0]*1000
     else:
         angle = angle*D
         ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
         if index == 1:
             ax = angle
         elif index == 2:
             ay = angle
         elif index == 3:
             az = angle
         mat = tm.euler_matrix(ax, ay, az, axes='sxyz')
         self.matPose[:3,:3] = mat[:3,:3]
         return 1000.0

def get_roll_to(head, tail, normal):
    """
    Compute the roll angle for a bone to make the bone's local x axis align with
    the specified normal.
    """
    p1 = toZisUp3(head)
    p2 = toZisUp3(tail)
    xvec = normal

    pvec = matrix.normalize(p2-p1)
    xy = np.dot(xvec,pvec)
    yvec = matrix.normalize(pvec-xy*xvec)
    zvec = matrix.normalize(np.cross(xvec, yvec))
    mat = np.asarray((xvec,yvec,zvec), dtype=np.float32)

    try:
        assertOrthogonal(mat)
    except Exception as e:
        log.warning("Calculated matrix is not orthogonal (%s)" % e)
    quat = tm.quaternion_from_matrix(mat)
    if abs(quat[0]) < 1e-4:
        return 0
    else:
        roll = math.pi - 2*math.atan(quat[2]/quat[0])

    if roll < -math.pi:
        roll += 2*math.pi
    elif roll > math.pi:
        roll -= 2*math.pi
    return roll

    def inframe(self, pose, frame):
        """ Transform a pose from one frame to another one.

        Uses transformation matrices. Could be refactored to use directly
        quaternions.
        """
        pose = self.get(pose)

        if pose['frame'] == "map":
            orig = numpy.identity(4)
        else:
            orig = self._to_mat4(self.get(pose["frame"]))

        if frame == "map":
            dest = numpy.identity(4)
        else:
            dest = numpy.linalg.inv(self._to_mat4(self.get(frame)))

        poseMatrix = self._to_mat4(pose)

        transf = numpy.dot(dest, orig)
        transformedPose = numpy.dot(transf, poseMatrix)

        qx,qy,qz,qw = transformations.quaternion_from_matrix(transformedPose)
        x,y,z = transformations.translation_from_matrix(transformedPose)

        return {"x":x,
                "y":y,
                "z":z,
                "qx":qx,
                "qy":qy,
                "qz":qz,
                "qw":qw,
                "frame": frame}

 def getRotation(self):
     """
     Get rotation matrix of rotation of this bone in local space.
     """
     qw,qx,qy,qz = tm.quaternion_from_matrix(self.matPose)
     ax,ay,az = tm.euler_from_matrix(self.matPose, axes='sxyz')
     return (1000*qw,1000*qx,1000*qy,1000*qz, ax/D,ay/D,az/D)

def calculatePoseError(tf0, tf1):
    numDimMat = 16
    numDimEul = 6
    numDimQuat = 7
    
    numData = len(tf0) # should be the same for tf1
    matrixPoseError = np.empty((numData, numDimMat))
    translationPoseError = np.empty((numData, numDimMat))
    rotationPoseError = np.empty((numData, numDimMat))
    
    
    for i_data in range(numData):
        subtractedTau = tf0 - tf1
        deltaTau = np.lin_alg.inverse(tf0[i_data]).dot(tf1[i_data])
        diffTranslation = deltaTau[:3,3]
        diffRotation = np.eye(4,4)
        diffRotation[:3,:3] = deltaTau[:3,:3]
        
        diffQuat = quaternion_from_matrix(diffRotation)
        diffEuler = euler_from_matrix(diffRotation)
        
        # flip quaternions on the wrong side of the hypersphere
        if diffQuat[3] < 0:
            diffQuat = -diffQuat
            
        pose_error[i_data,:] = np.r_[diff_translation, diff_rot_rep]
    return pose_error

	def __modify_quaternion( self , q , d ) :
		glPushMatrix()
		glLoadMatrixf( tr.quaternion_matrix( q ) )
		glRotatef( d[0] , 0 , 1 , 0 )
		glRotatef( d[1] , 1 , 0 , 0 )
		q = tr.quaternion_from_matrix( glGetDoublev(GL_MODELVIEW_MATRIX) )
		glPopMatrix()
		return q

 def fromMatrix(self, matrix):
     q0 = tm.quaternion_from_matrix(matrix)
     t = matrix[:3,3]
     self.even = q0
     q1 = self.odd
     q1[0] = -0.5*(t[0]*q0[1] + t[1]*q0[2] + t[2]*q0[3])
     q1[1] = 0.5*( t[0]*q0[0] + t[1]*q0[3] - t[2]*q0[2])
     q1[2] = 0.5*(-t[0]*q0[3] + t[1]*q0[0] + t[2]*q0[1])
     q1[3] = 0.5*( t[0]*q0[2] - t[1]*q0[1] + t[2]*q0[0])

def rvec2quat(rvec, cam2body):
    Rned2cam, jac = cv2.Rodrigues(rvec)
    Rned2body = cam2body.dot(Rned2cam)
    Rbody2ned = np.matrix(Rned2body).T
    # make 3x3 rotation matrix into 4x4 homogeneous matrix
    hIR = np.concatenate( (np.concatenate( (Rbody2ned, np.zeros((3,1))),1),
                           np.mat([0,0,0,1])) )
    quat = transformations.quaternion_from_matrix(hIR)
    return quat

def _plot_img(img, K, T, z=0.1):
    obj = mlab.imshow(img.T)
    quat = tf.quaternion_from_matrix(T)
    angle, axis = angle_axis_from_quaternion(quat)
    obj.actor.rotate_wxyz(angle * 180 / np.pi, *axis)
    h, w = img.shape
    xmax_pixel, ymax_pixel = w, h
    point3d = projectionto3d(K, (xmax_pixel, ymax_pixel))[0]
    origin3d = projectionto3d(K, (0, 0))[0]
    point3d = point3d * z / point3d[2]
    origin3d = origin3d * z / origin3d[2]
    center3d = (point3d + origin3d) / 2.
    xmax, ymax, _ = point3d - origin3d
    obj.actor.scale = np.array([xmax / xmax_pixel, ymax / ymax_pixel, 1.0])
    obj.actor.position = utils.apply_transform(T, center3d)
    mlab.view(distance=20 * z)
    return obj

def computeRoll(head, tail, normal, bone=None):
    if normal is None:
        return 0

    p1 = m2b(head)
    p2 = m2b(tail)
    xvec = normal
    pvec = getUnitVector(p2-p1)
    xy = np.dot(xvec,pvec)
    yvec = getUnitVector(pvec-xy*xvec)
    zvec = getUnitVector(np.cross(xvec, yvec))
    if zvec is None:
        return 0
    else:
        mat = np.array((xvec,yvec,zvec))

    checkOrthogonal(mat)
    quat = tm.quaternion_from_matrix(mat)
    if abs(quat[0]) < 1e-4:
        return 0
    else:
        roll = math.pi - 2*math.atan(quat[2]/quat[0])

    if roll < -math.pi:
        roll += 2*math.pi
    elif roll > math.pi:
        roll -= 2*math.pi
    return roll

    if bone and bone.name in ["forearm.L", "forearm.R"]:
        log.debug("B  %s" % bone.name)
        log.debug(" H  %.4g %.4g %.4g" % tuple(head))
        log.debug(" T  %.4g %.4g %.4g" % tuple(tail))
        log.debug(" N  %.4g %.4g %.4g" % tuple(normal))
        log.debug(" P  %.4g %.4g %.4g" % tuple(pvec))
        log.debug(" X  %.4g %.4g %.4g" % tuple(xvec))
        log.debug(" Y  %.4g %.4g %.4g" % tuple(yvec))
        log.debug(" Z  %.4g %.4g %.4g" % tuple(zvec))
        log.debug(" Q  %.4g %.4g %.4g %.4g" % tuple(quat))
        log.debug(" R  %.4g" % roll)

    return roll

def contacts_to_baxter_hand_pose(contact1, contact2):
    c1 = np.array(contact1)
    c2 = np.array(contact2)

    # compute gripper center and axis
    center = 0.5 * (c1 + c2)
    y_axis = c2 - c1
    y_axis = y_axis / np.linalg.norm(y_axis)
    z_axis = np.array([y_axis[1], -y_axis[0], 0])  # the z axis will always be in the table plane for now
    z_axis = z_axis / np.linalg.norm(z_axis)
    x_axis = np.cross(y_axis, z_axis)

    # convert to hand pose
    R_obj_gripper = np.array([x_axis, y_axis, z_axis]).T
    t_obj_gripper = center
    T_obj_gripper = np.eye(4)
    T_obj_gripper[:3, :3] = R_obj_gripper
    T_obj_gripper[:3, 3] = t_obj_gripper
    q_obj_gripper = transformations.quaternion_from_matrix(T_obj_gripper)

    return t_obj_gripper, q_obj_gripper

def get_a2b(a, b, rep_out='rotmat'):
    """Returns an SO3 quantity that will align vector a with vector b.
    The output will be in the representation selected by rep_out
    ('rotmat', 'quaternion', or 'rotvec').

    >>> a = trns.random_vector(3)
    >>> b = trns.random_vector(3)
    >>> R = get_a2b(a, b)
    >>> p1 = R.dot(a)
    >>> print(np.allclose(np.cross(p1, b), [0, 0, 0]))
    True
    >>> p1.dot(b) > 0
    True
    >>> q = get_a2b(a, b, 'quaternion')
    >>> p2 = apply_to_points(q, a)
    >>> print(np.allclose(np.cross(p2, b), [0, 0, 0]))
    True
    >>> r = get_a2b(a, b, 'rotvec')
    >>> p3 = apply_to_points(r, a)
    >>> print(np.allclose(np.cross(p3, b), [0, 0, 0]))
    True

    """
    a = normalize(a)
    b = normalize(b)
    cosine = a.dot(b)
    if np.isclose(abs(cosine), 1):
        R = np.eye(3)
    else:
        axis = np.cross(a, b)
        angle = np.arctan2(npl.norm(axis), cosine)
        R = trns.rotation_matrix(angle, axis)
    if rep_out == 'rotmat':
        return R[:3, :3]
    elif rep_out == 'quaternion':
        return trns.quaternion_from_matrix(R)
    elif rep_out == 'rotvec':
        return get_rotvec(R)
    else:
        raise ValueError("Invalid rep_out. Choose 'rotmat', 'quaternion', or 'rotvec'.")