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 writeAnimation(fp, bone, action, config):
    aname = "Action%s" % bone.name
    points = action[bone.name]

    for channel,default in [
            ("T", 0),
            ("R", 0),
            ("S", 1)
        ]:
        writeAnimationCurveNode(fp, bone, channel, default)

    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    translations = []
    eulers = []
    R = 180/math.pi
    for quat in points:
        mat = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat)
        translations.append(mat[:3,3])
        eul = tm.euler_from_matrix(mat, axes='sxyz')
        eulers.append((eul[0]*R, eul[1]*R, eul[2]*R))
    scales = len(points)*[(1,1,1)]

    for channel,data in [
            ("T", translations),
            ("R", eulers),
            ("S", scales)
        ]:
        for idx,coord in enumerate(["X","Y","Z"]):
            writeAnimationCurve(fp, idx, coord, bone, channel, data)

def apply_to_matrix(x, A):
    """Applies the change of basis given by the SO3 quantity
    x to the 3by3 matrix M.

    To be clear, this performs:
    R * A * transpose(R)
    where R is the rotation matrix form of x.

    >>> A = np.random.rand(3, 3)
    >>> q = trns.random_quaternion()
    >>> R = trns.quaternion_matrix(q)
    >>> B1 = R[:3, :3].dot(A).dot(R[:3, :3].T)
    >>> B2 = apply_to_matrix(q, A)
    >>> print(np.allclose(B1, B2))
    True

    """
    A = np.array(A)
    if A.shape != (3, 3):
        raise ValueError("A must be 3 by 3.")
    xrep = rep(x)
    # Get rotation matrix if not rotmat:
    if xrep == 'rotvec':
        x = matrix_from_rotvec(x)
    elif xrep == 'quaternion':
        x = trns.quaternion_matrix(x)[:3, :3]
    elif xrep == 'tfmat':
        x = x[:3, :3]
    elif xrep != 'rotmat':
        raise ValueError("Quantity to apply is not on SO3.")
    # Apply change of basis to A:
    return x.dot(A).dot(x.T)

    def loadPoseFromMhpFile(self, filepath):
        """
        Load a MHP pose file that contains a static pose. Posing data is defined
        with quaternions to indicate rotation angles.
        Sets current pose to
        """
        log.message("Mhp %s", filepath)
        fp = open(filepath, "rU", encoding="utf-8")

        boneMap = self.getBoneToIdxMapping()
        nBones = len(boneMap.keys())
        poseMats = np.zeros((nBones,4,4),dtype=np.float32)
        poseMats[:] = np.identity(4, dtype=np.float32)

        for line in fp:
            words = line.split()
            if len(words) < 5:
                continue
            elif words[1] in ["quat", "gquat"]:
                boneIdx = boneMap[words[0]]
                quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
                mat = tm.quaternion_matrix(quat)
                if words[1] == "gquat":
                    bone = self.bones[boneIdx]
                    mat = np.dot(la.inv(bone.matRestRelative), mat)
                poseMats[boneIdx] = mat[:3,:3]

        fp.close()
        self.setPose(poseMats)

 def readMhpFile(self, filepath):
     log.message("Loading MHP file %s", filepath)
     amt = self.armature
     fp = open(filepath, "rU")
     bname = None
     mat = np.identity(4, float)
     for line in fp:
         words = line.split()
         if len(words) < 4:
             continue
         if words[0] != bname:
             self.setMatrixPose(bname, mat)
             bname = words[0]
             mat = np.identity(4, float)
         if words[1] in ["quat", "gquat"]:
             quat = float(words[2]),float(words[3]),float(words[4]),float(words[5])
             mat = tm.quaternion_matrix(quat)
             if words[1] == "gquat":
                 pb = self.posebones[words[0]]
                 mat = np.dot(la.inv(pb.bone.matrixRelative), mat)
         elif words[1] == "scale":
             scale = 1+float(words[2]), 1+float(words[3]), 1+float(words[4])
             smat = tm.compose_matrix(scale=scale)
             mat = np.dot(smat, mat)
         elif words[1] == "matrix":
             rows = []
             n = 2
             for i in range(4):
                 rows.append((float(words[n]), float(words[n+1]), float(words[n+2]), float(words[n+3])))
                 n += 4
             mat = np.array(rows)
     self.setMatrixPose(bname, mat)
     fp.close()
     self.update()

    def draw_trajectory(self, pose_array, angles, ns="default_ns"):
        decimation = max(len(pose_array.poses)//6, 1)
        ps = gm.PoseStamped()
        ps.header.frame_id = pose_array.header.frame_id        
        ps.header.stamp = rospy.Time.now()
        handles = []
 
        multiplier = 5.79 
        
        for (pose,angle) in zip(pose_array.poses,angles)[::decimation]:

            ps.pose = deepcopy(pose)
            pointing_axis = transformations.quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])[:3,0]
            ps.pose.position.x -= .18*pointing_axis[0]
            ps.pose.position.y -= .18*pointing_axis[1]
            ps.pose.position.z -= .18*pointing_axis[2]
            
            
            root_link = "r_wrist_roll_link"
            valuedict = {"r_gripper_l_finger_joint":angle*multiplier,
                         "r_gripper_r_finger_joint":angle*multiplier,
                         "r_gripper_l_finger_tip_joint":angle*multiplier,
                         "r_gripper_r_finger_tip_joint":angle*multiplier,
                         "r_gripper_joint":angle}
            handles.extend(self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
        return handles

def writeAnimation(fp, bone, action, config):
    aname = "Action_%s" % goodBoneName(bone.name)
    points = action[bone.name]
    npoints = len(points)

    fp.write(
        '    <animation id="%s_pose_matrix">\n' % aname +
        '      <source id="%s_pose_matrix-input">\n' % aname +
        '        <float_array id="%s_pose_matrix-input-array" count="%d">' % (aname, npoints))

    fp.write(''.join([" %g" % (0.04*(t+1)) for t in range(npoints)]))

    fp.write(
        '</float_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-input-array" count="%d" stride="1">\n' % (aname, npoints) +
        '            <param name="TIME" type="float"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <source id="%s_pose_matrix-output">\n' % aname +
        '        <float_array id="%s_pose_matrix-output-array" count="%d">\n' % (aname, 16*npoints))

    string = '          '
    relmat = bone.getRelativeMatrix(config.meshOrientation, config.localBoneAxis, config.offset)
    for quat in points:
        mat0 = tm.quaternion_matrix(quat)
        mat = np.dot(relmat, mat0)
        string += ''.join(['%g %g %g %g  ' % tuple(mat[i,:]) for i in range(4)])
        string += '\n          '
    fp.write(string)

    fp.write(
        '</float_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-output-array" count="%d" stride="16">\n' % (aname, npoints) +
        '            <param name="TRANSFORM" type="float4x4"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <source id="%s_pose_matrix-interpolation">\n' % aname +
        '        <Name_array id="%s_pose_matrix-interpolation-array" count="%d">' % (aname, npoints))

    fp.write(npoints * 'LINEAR ')

    fp.write(
        '</Name_array>\n' +
        '        <technique_common>\n' +
        '          <accessor source="#%s_pose_matrix-interpolation-array" count="%d" stride="1">\n' % (aname, npoints) +
        '            <param name="INTERPOLATION" type="name"/>\n' +
        '          </accessor>\n' +
        '        </technique_common>\n' +
        '      </source>\n' +
        '      <sampler id="%s_pose_matrix-sampler">\n' % aname +
        '        <input semantic="INPUT" source="#%s_pose_matrix-input"/>\n' % aname +
        '        <input semantic="OUTPUT" source="#%s_pose_matrix-output"/>\n' % aname +
        '        <input semantic="INTERPOLATION" source="#%s_pose_matrix-interpolation"/>\n' % aname +
        '      </sampler>\n' +
        '      <channel source="#%s_pose_matrix-sampler" target="%s/transform"/>\n' % (aname, goodBoneName(bone.name)) +
        '    </animation>\n')

	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 step( self , dt ) :
		if not self.R.successful() : return
		self.Q = self.R.integrate(self.R.t+dt)
		if len(self.trace) > self.trace_len :
			self.trace.pop(0)
		if len(self.trace) < self.trace_len+1 :
			qm = tr.quaternion_matrix( self.Q[3:] )
			self.trace.append( np.dot( qm , self.x[-1] ) )

 def toMatrix(self):
     q0 = self.even
     mat = tm.quaternion_matrix(q0)
     q1 = self.odd        
     mat[0,3] = 2.0*(-q1[0]*q0[1] + q1[1]*q0[0] - q1[2]*q0[3] + q1[3]*q0[2])
     mat[1,3] = 2.0*(-q1[0]*q0[2] + q1[1]*q0[3] + q1[2]*q0[0] - q1[3]*q0[1])
     mat[2,3] = 2.0*(-q1[0]*q0[3] - q1[1]*q0[2] + q1[2]*q0[1] + q1[3]*q0[0])
     return mat

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 _draw_scene( self ) :
		glPushMatrix()
		glTranslatef( *self.b )
		glPushMatrix()
		glMultMatrixd( tr.quaternion_matrix( self.qb ) )
		self.frame.draw()
		glPopMatrix()

		glTranslatef( *self.c )
		glMultMatrixd( tr.quaternion_matrix( self.qc ) )
		self.frame.draw()
		glPopMatrix()

		glPushMatrix()
		glTranslatef( *self.e )
		glMultMatrixd( tr.quaternion_matrix( self.qe ) )
		self.frame.draw()
		glPopMatrix()

def compute_ref_accuracy(fid_path, original_corrs_path,
                         geo_tform):

    #Load fiducial .ply
    fid = open(fid_path, 'r')
    fid_points = np.genfromtxt(fid, dtype=float, delimiter=' ',
                                skip_header=9)
    fid.close()
    #Load original corrs .ply
    fid = open(original_corrs_path, 'r')
    original_corrs = np.genfromtxt(fid, dtype=float,
                                    delimiter=' ', skip_header=9)
    fid.close()

    #Load transformation
    #************GEO**************"
    Tfis = open(geo_tform, 'r')
    lines = []
    lines = Tfis.readlines()
    scale_geo = float(lines[0])
    Ss_geo = tf.scale_matrix(scale_geo)
    quat_line = lines[1].split(" ")
    quat_geo = np.array([float(quat_line[3]), float(quat_line[0]),
                         float(quat_line[1]), float(quat_line[2])])
    Rs_geo = tf.quaternion_matrix(quat_geo)
    trans_line = lines[2].split(" ")
    trans_geo = np.array([float(trans_line[0]), float(trans_line[1]),
                          float(trans_line[2])])
    Tfis.close()
    Hs_geo = Rs_geo.copy()
    Hs_geo[:3, 3] = trans_geo[:3]
    Hs_geo = Ss_geo.dot(Hs_geo)

    LOG.debug("\n******Geo***** \n Scale: \n%s \nR:\n%s \nT:\n%s \nH:\n%s",
        Ss_geo, Rs_geo, trans_geo, Hs_geo)

    #Compute the "reference error"
    #i.e. fiducial points - geo registered correspondances
    npoints, c = fid_points.shape
    if npoints != 30:
        LOG.warn("Number of fiducial point is NOT 30")
    if c != 3:
        LOG.error("Fiducial points has the wrong number of dimensions")
    # import code; code.interact(local=locals())

    fid_points_hom = np.hstack((fid_points, np.ones([npoints, 1]))).T
    original_corrs_hom = np.hstack((original_corrs, np.ones([npoints, 1]))).T
    geo_corrs_hom = Hs_geo.dot(original_corrs_hom)
    geo_ref_diff = geo_corrs_hom - fid_points_hom

    # import pdb; pdb.set_trace()

    delta_z = np.sqrt(geo_ref_diff[2, :] * geo_ref_diff[2, :])
    delta_r = np.sqrt(geo_ref_diff[0, :] * geo_ref_diff[0, :] +
                      geo_ref_diff[1, :] * geo_ref_diff[1, :])

    return delta_z, delta_r

	def newPose(self, poseMsg):
		self.publish()
		q = [poseMsg.orientation.x, poseMsg.orientation.y, poseMsg.orientation.z, poseMsg.orientation.w]
		rq = transformations.quaternion_matrix(q)
		al, be, ga = transformations.euler_from_matrix(rq, 'rxyz')
		self.ga = ga
		x = poseMsg.position.x
		y = poseMsg.position.y
		z = poseMsg.position.z
		self.dist = math.sqrt(x*x + y*y + z*z)
		print al, be, ga

 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()

 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 apply_pose_error(tf1s, errors, euler_representation=True):
    tf0s = []
    for (tf1, error) in zip(tf1s, errors):
        tf0 = np.eye(4)
        tf0[:3,3] = tf1[:3,3] + error[:3]
        #tf0[:3,:3] = rave.rotationMatrixFromAxisAngle(error[3:]).dot(tf1[:3,:3])
        if euler_representation:
            tf0[:3,:3] = euler_matrix(*error[3:])[:3,:3].dot(tf1[:3,:3])
        else:
            tf0[:3,:3] = quaternion_matrix(*error[3:])[:3,:3].dot(tf1[:3,:3])
        tf0s.append(tf0)
    return tf0s

    def __init__(self, T):

        if ( type(T) == str):
            Tfile = T
            #Load transformation
            Tfis = open(Tfile, 'r')
            lines = []
            lines = Tfis.readlines()
            self.scale = float(lines[0])
            self.Ss = tf.scale_matrix(self.scale)
            quat_line = lines[1].split(" ")
            self.quat = tf.unit_vector(np.array([float(quat_line[3]),
                                            float(quat_line[0]),
                                            float(quat_line[1]),
                                            float(quat_line[2])]))
            self.Hs = tf.quaternion_matrix(self.quat)
            trans_line = lines[2].split(" ")
            self.Ts = np.array([float(trans_line[0]),
                           float(trans_line[1]),
                           float(trans_line[2])])
            Tfis.close()
            self.Rs = self.Hs.copy()[:3, :3]
            self.Hs[:3, 3] = self.Ts[:3]

            self.Hs = self.Ss.dot(self.Hs)  # to add again

        elif (type(T) == np.ndarray):
            self.Hs = T
            scale, shear, angles, trans, persp = tf.decompose_matrix(T)
            self.quat = tf.quaternion_from_euler(angles[0], angles[1], angles[2])
            self.Rs = tf.quaternion_matrix(self.quat)
            self.scale =scale[0]
            self.Ts = trans / self.scale


        print "Loaded Ground Truth Transformation: "
        print self.Hs

    def calc_joint_local_matrix(self):
        self.L = np.eye(4)

        # Add in rotation
        q = np.r_[0.0, 0.0, 0.0, 1.0]
        for i in range(3):
            rot_vec = np.r_[0.0, 0.0, 0.0]
            rot_vec[i] = 1.0
            q = tr.quaternion_about_axis(np.radians(-self.rotation[i]), rot_vec)
            # q = tr.quaternion_multiply(q, qi)
            Q = np.matrix(tr.quaternion_matrix(q))
            self.L = np.dot(self.L, Q)

            # Add in translation on the first three columns of bottom row
        self.L[3, :3] = self.translation