def doTest2(listBones):            
    fp = open(thePosesFile, "w")
    for bone,coords in listBones:
        fp.write("\n%s %.4f\n" % (bone.name, bone.roll)) 
        writeMat(fp, "Rest", bone.matrixRest)
        writeMat(fp, "Global", bone.matrixGlobal)
        pose = bone.getPoseFromGlobal()
        writeMat(fp, "Pose", pose)
        xyz = tm.euler_from_matrix(pose, axes='sxyz')
        fp.write("XYZ %.4g %.4g %.4g\n" % tuple(xyz))
        zyx = tm.euler_from_matrix(pose, axes='szyx')
        fp.write("ZYX %.4g %.4g %.4g\n" % tuple(zyx))
        #writeMat(fp, "Pose0", bone.matrixPose)
    fp.close()

    def predictSinglePose(self, armName, curPose, prevPose, dt=1.0):
        if dt <= 0:
            print 'Error: Illegal timestamp'
            return None

        # Convert pose to numpy matrix
        curTrans = tft.translation_matrix([curPose.position.x, curPose.position.y, curPose.position.z])
        curRot = tft.quaternion_matrix([curPose.orientation.x, curPose.orientation.y ,curPose.orientation.z, curPose.orientation.w])
        curPoseMatrix = np.dot(curTrans, curRot)

        prevTrans = tft.translation_matrix([prevPose.position.x, prevPose.position.y, prevPose.position.z])
        prevRot = tft.quaternion_matrix([prevPose.orientation.x, prevPose.orientation.y ,prevPose.orientation.z, prevPose.orientation.w])
        prevPoseMatrix = np.dot(prevTrans, prevRot)
        
        deltaPoseMatrix = np.linalg.inv(prevPoseMatrix).dot(curPoseMatrix)
        deltaAngles = euler_from_matrix(deltaPoseMatrix[:3,:3])
        deltaPos = deltaPoseMatrix[:3,3]

        #deltaAngles = np.array([a / dt for a in deltaAngles])
        deltaPos = deltaPos / dt
        #deltaPoseMatrix = euler_matrix(deltaAngles[0], deltaAngles[1], deltaAngles[2])
        deltaPoseMatrix[:3,3] = deltaPos

        gpList, sysList = self.predict(armName, [curPoseMatrix], [deltaPoseMatrix])
        return tfx.pose(gpList[0], frame=curPose.frame, stamp=curPose.stamp), tfx.pose(sysList[0], frame=curPose.frame, stamp=curPose.stamp)

 def componentwiseError(self, estimatedPoses, truePoses):
     n_poses = len(estimatedPoses)
     n_dim = estimatedPoses[0].shape[0] * estimatedPoses[0].shape[1]
     n_trans = 3
     n_rot = 3
     translationError = np.zeros((n_poses, n_trans))
     rotationError = np.zeros((n_poses, n_rot))
     
     for i in range(n_poses):
         estimatedPose = estimatedPoses[i]
         truePose = truePoses[i]
         deltaPose = np.linalg.inv(estimatedPose).dot(truePose)
         
         deltaRotation = np.eye(4,4)
         deltaRotation[:3,:3] = deltaPose[:3,:3]
         
         translationError[i,:] = deltaPose[:3,3].T
         rotationError[i,:] = euler_from_matrix(deltaRotation)
         
         #if np.abs(translationError[i,1]) > 0.01:
         #    print 'Outlier', i
         
     combinedError = np.c_[translationError, rotationError]
     meanError = np.mean(np.abs(combinedError), axis=0)
     medianError = np.median(np.abs(combinedError), axis=0)
     rmsError = np.sqrt(np.mean(np.square(combinedError), axis=0))
     maxError = np.max(np.abs(combinedError), axis=0)
     
     predictionError = PredictionError(medianError, meanError, rmsError, maxError)
     return predictionError

    def init_local_transformation(self):
        """
        compute local transformation w.r.t for the first time (compute everything)
        """
        # rotation part
        self.localTransformation = numpy.eye(4)
        # print type(self), id(self), self.rotation
        try:
            angle = self.rotation[3]
        except IndexError:
            logger.exception("Failed on {0}, rotation={1}".format(type(self),self.rotation))
            raise
        direction = self.rotation[:3]
        self.localTransformation[0:3,0:3] = tf.rotation_matrix(angle, direction)[:3,:3]
        self.rpy = tf.euler_from_matrix(self.localTransformation)

        # last column
        scale = [1,1,1]
        if self.parent:
            scale = self.cumul_scale()
        scale_translation = [self.translation[i]*scale[i] for i in range(3)]

        self.localTransformation[0:3,3] = numpy.array(scale_translation)+\
            numpy.dot(numpy.eye(3)-self.localTransformation[:3,:3],
                      numpy.array(self.center))

        # last line
        self.localTransformation[3,0:4]=[0,0,0,1]

	def determine_obj_frame(self):
		if self.detected_artags != None:

			eng_poses = []
			ar_tfs = []
			for da in self.detected_artags.markers:
				ar_pose = th.aruco_to_pose_stamp(da)

				eng_pose = self.get_engine_pose_from_ps(ar_pose)

				eng_poses.append(eng_pose)
				ar_tfs.append(th.pose_stamp_to_tf(eng_pose))

			ar_tf_sum = np.zeros((4,4))
			for tf in ar_tfs:
				ar_tf_sum = ar_tf_sum + tf/len(ar_tfs)
				
			engine_frame_combined = ar_tf_sum
			
			tran = transformations.translation_from_matrix(engine_frame_combined), 
			eul = transformations.euler_from_matrix(engine_frame_combined)
			
			
			self.updated_engine_pose = str(tran[0][0]) + ' ' + str(tran[0][1]) + ' ' + str(tran[0][2]) + ' ' + str(eul[0]) + ' ' + str(eul[1]) + ' ' + str(eul[2]) 
			
			ps = th.tf_to_pose_stamp(engine_frame_combined, 'engine_frame_combined')
			
			th.broadcast_transform(ps, global_frame)
			return engine_frame_combined
		else: 
			return None

    def fromSkeleton(self, skel, animationTrack = None):
        """
        Construct a BVH object from a skeleton structure and optionally an 
        animation track. If no animation track is specified, a dummy animation
        of one frame will be added.
        NOTE: Make sure that the skeleton has only one root.
        """

        # Traverse skeleton joints in depth-first order
        for jointName in skel.getJointNames():
            bone = skel.getBone(jointName)
            if bone.parent:
                joint = self.addJoint(bone.parent.name, jointName)
                joint.channels = ["Zrotation", "Xrotation", "Yrotation"]
            else:
                joint = self.addRootJoint(bone.name)
                joint.channels = ["Xposition", "Yposition", "Zposition", "Zrotation", "Xrotation", "Yrotation"]
    
            offset = bone.getRestOffset()
            self.__calcPosition(joint, offset)
            if not bone.hasChildren():
                endJoint = self.addJoint(jointName, 'End effector')
                offset = bone.getRestTailPos() - bone.getRestHeadPos()
                self.__calcPosition(endJoint, offset)

        self.__cacheGetJoints()
        nonEndJoints = [ joint for joint in self.getJoints() if not joint.isEndConnector() ]

        if animationTrack:
            self.frameCount = animationTrack.nFrames
            self.frameTime = 1.0/animationTrack.frameRate

            jointToBoneIdx = {}
            for joint in nonEndJoints:
                jointToBoneIdx[joint.name] = skel.getBone(joint.name).index

            for fIdx in xrange(animationTrack.nFrames):
                offset = fIdx * animationTrack.nBones
                for jIdx,joint in enumerate(nonEndJoints):
                    bIdx = jointToBoneIdx[joint.name]
                    poseMat = animationTrack.data[offset + bIdx]
                    if len(joint.channels) == 6:
                        # Add transformation
                        tx, ty, tz = poseMat[:3,3]
                        joint.frames.extend([tx, ty, tz])
                    ay,ax,az = tm.euler_from_matrix(poseMat, "syxz")
                    joint.frames.extend([az/D, ax/D, ay/D])
        else:
            # Add bogus animation with one frame
            self.frameCount = 1
            self.frameTime = 1.0
            for jIdx,joint in enumerate(nonEndJoints):
                if len(joint.channels) == 6:
                    # Add transformation
                    joint.frames.extend([0.0, 0.0, 0.0, 0.0, 0.0, 0.0])
                else:
                    joint.frames.extend([0.0, 0.0, 0.0])

        for joint in self.getJoints():
            joint.calculateFrames()

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 updateRadar(self):
        # Taken from testDrawRadarOnMap.py
        fren = self.iren.GetRenderWindow().GetRenderers().GetFirstRenderer()
        t = self.start + self.step * self.count
        radar_data = loadRDR(self.rdr_pts[t])[0]

        if radar_data.shape[0] > 0:
            #Convert from radar to lidar ref-frame
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], self.radar_params)

            #Convert from lidar to IMU ref-frame
            radar_data[:, :3] = np.dot(self.lidar_params['T_from_l_to_i'][:3, :3],
                radar_data[:, :3].transpose()).transpose()

            h_radar_data = np.hstack((radar_data[:, :3], np.ones((radar_data.shape[0], 1))))

            radar_data[:, :3] = np.dot(self.imu_transforms[t],
                h_radar_data.transpose()).transpose()[:, :3]

            for i in xrange(len(self.radar_actors)):
                fren.RemoveActor(self.radar_actors[i])

            self.radar_actors = []
            self.radar_clouds = []

            for i in xrange(radar_data.shape[0]):
                self.radar_clouds.append(VtkBoundingBox(radar_data[i, :]))
                (ax, ay, az) = euler_from_matrix(self.imu_transforms[t])
                box = self.radar_clouds[i].get_vtk_box(rot=az*180/np.pi)
                self.radar_actors.append(box)
                fren.AddActor(self.radar_actors[i])

def writeBoneProp(fp, bone, config):
    id,key = getId("Model::%s" % bone.name)
    fp.write(
'    Model: %d, "%s", "LimbNode" {' % (id,key) +
"""
        Version: 232
        Properties70:  {
            P: "RotationActive", "bool", "", "",1
            P: "InheritType", "enum", "", "",1
            P: "ScalingMax", "Vector3D", "Vector", "",0,0,0
            P: "DefaultAttributeIndex", "int", "Integer", "",0
""")

    mat = bone.getRelativeMatrix(config)
    trans = mat[:3,3]
    e = tm.euler_from_matrix(mat, axes='sxyz')
    fp.write(
'            P: "Lcl Translation", "Lcl Translation", "", "A",%.4f,%.4f,%.4f\n' % tuple(trans) +
'            P: "Lcl Rotation", "Lcl Rotation", "", "A",%.4f,%.4f,%.4f\n' % (e[0]*R, e[1]*R, e[2]*R) +
'            P: "Lcl Scaling", "Lcl Scaling", "", "A",1,1,1\n' +
'            P: "MHName", "KString", "", "", "%s"' % bone.name +
"""
        }
        Shading: Y
        Culling: "CullingOff"
    }
""")

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 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 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 get_SVD(self):
        """
        Get the affine transformation matrix that best matches the moelcules cxns
        to each rod cxn by SVD

        We do a Euclidean (rigid) transform
        """

        print("\n\nCalculating intial affine transformations:")
        print("\n\nM = affine transformation for best fit of mol cxn -> rod cxn:")
        for i in range(len(self.cxns)):
            for it in self.mol.permutations:
                for dc in self.dc_ints: 
                    pass
            a = self.mol.cxns[0:3,:]
            b = self.cxn_xyz[i]

            M = trans.affine_matrix_from_points(a, b, shear = False, scale = False, usesvd = True)

            alpha, beta, gamma = trans.euler_from_matrix(M)
            translations = M[0:3,3]
            conn_start_ind = 1 + len(self.rods) + i*6
            self.opt_vec[conn_start_ind+0] = alpha  
            self.opt_vec[conn_start_ind+1] = beta 
            self.opt_vec[conn_start_ind+2] = gamma 
            self.opt_vec[conn_start_ind+3] = translations[0] 
            self.opt_vec[conn_start_ind+4] = translations[1]
            self.opt_vec[conn_start_ind+5] = translations[2]
            print(M)

    def addObjToCloud(self, objs_wrt_mbly):
        """ Add the mobileye returns to the 3d scene """
        mbly_vtk_boxes = []

        car_rot = self.imu_transforms_mk1[self.t, :, :3]
        objs_wrt_world = self.xformMblyToGlobal(objs_wrt_mbly)

        # Draw each box
        car_rot = euler_from_matrix(car_rot)[2] * 180 / math.pi
        for o in objs_wrt_world:
            # Create the vtk object
            box = VtkBoundingBox(o)
            actor = box.get_vtk_box(car_rot)
            color = self.mbly_obj_colors[int(o[5])]
            actor.GetProperty().SetColor(*color)
            mbly_vtk_boxes.append(box)

        # Remove old boxes
        for vtk_box in self.mbly_vtk_boxes:
            self.cloud_ren.RemoveActor(vtk_box.actor)
        # Update to new actors
        self.mbly_vtk_boxes = mbly_vtk_boxes
        # Draw new boxes
        for vtk_box in self.mbly_vtk_boxes:
            self.cloud_ren.AddActor(vtk_box.actor)

    def execute(self, iren, event):
        fren = iren.GetRenderWindow().GetRenderers().GetFirstRenderer()

        radar_data = loadRDR(self.rdr_pts[self.count])[0]
        if radar_data.shape[0] > 0:
            #Convert from radar to lidar ref-frame
            radar_data[:, :3] = calibrateRadarPts(radar_data[:, :3], self.radar_params)

            #Convert from lidar to IMU ref-frame
            radar_data[:, :3] = np.dot(self.lidar_params['T_from_l_to_i'][:3, :3], \
                radar_data[:, :3].transpose()).transpose()

            h_radar_data = np.hstack((radar_data[:, :3], np.ones((radar_data.shape[0], 1))))

            radar_data[:, :3] = np.dot(self.imu_transforms[self.count], \
                h_radar_data.transpose()).transpose()[:, :3]
            
            #Insert the Q50 position at the beginning
            me_l = 4.1
            me_w = 2.1
            me_x = self.imu_transforms[self.count, 0, 3]-me_l/2.
            me_y = self.imu_transforms[self.count, 1, 3]
            me_z = self.imu_transforms[self.count, 2, 3]
            me = [me_x, me_y, me_z, me_l, me_w, 0, 0, 0]
            radar_data = np.vstack((np.array(me), radar_data))

            for i in xrange(len(self.radar_actors)):
                fren.RemoveActor(self.radar_actors[i])

            self.radar_actors = []
            self.radar_clouds = []

            for i in xrange(radar_data.shape[0]):
                self.radar_clouds.append(VtkBoundingBox(radar_data[i, :]))
                (ax, ay, az) = euler_from_matrix(self.imu_transforms[self.count])
                box = self.radar_clouds[i].get_vtk_box(rot = az*180/math.pi)
                if i == 0:
                    box.GetProperty().SetColor((1, 0, 0))
                self.radar_actors.append(box)
                fren.AddActor(self.radar_actors[i])

            if self.count == 0:
                self.lidar_cloud = VtkPointCloud(self.lidar_data[:, :3], self.lidar_data[:,3])
                self.lidar_actor = self.lidar_cloud.get_vtk_cloud(zMin=0, zMax=255)
                fren.AddActor(self.lidar_actor)
                self.gpsPointCloud = VtkPointCloud(self.imu_transforms[:, 0:3, 3], 
                                                np.zeros(self.imu_transforms.shape[0]))
                fren.AddActor(self.gpsPointCloud.get_vtk_cloud())

            # if self.count == 0:
                # fren.ResetCamera()
                # fren.GetActiveCamera().Zoom(1.6)
            cam_center = [x for x in self.radar_actors[0].GetCenter()]
            cam_center[2] += 150
            fren.GetActiveCamera().SetPosition(cam_center)
            fren.GetActiveCamera().SetFocalPoint(self.radar_actors[0].GetCenter())

        self.count += 5
        iren.GetRenderWindow().Render()

	def __modify_euler( self , a , d ) :
		glPushMatrix()
		glLoadMatrixf( tr.euler_matrix( *a ) )
		glRotatef( d[0] , 0 , 1 , 0 )
		glRotatef( d[1] , 1 , 0 , 0 )
		a = np.array( tr.euler_from_matrix( glGetDoublev(GL_MODELVIEW_MATRIX) ) )
		glPopMatrix()
		return a

 def update_config2(self, T, q):
     self.count += 1
     T = numpy.array(T)
     self.localTransformation = T
     self.translation = T[:3,3]
     angle, direction, point = tf.rotation_from_matrix(T)
     self.rpy = tf.euler_from_matrix(T)
     self.rotation = [ direction[0],direction[1],direction[2], angle ]
     self.origin.update()

	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 test_euler_from_matrix_random_angles_all_axes(self):
     angles = (4*math.pi) * (numpy.random.random(3) - 0.5)
     successes = []
     failures = []
     for axes in transformations._AXES2TUPLE.keys():
         R0 = transformations.euler_matrix(axes=axes, *angles)
         R1 = transformations.euler_matrix(axes=axes, *transformations.euler_from_matrix(R0, axes))
         if numpy.allclose(R0, R1):
             successes.append(axes)
         else:
             failures.append(axes)
     self.assertEquals(0, len(failures), "Failed for:\n%sand succeeded for:\n%s" % (failures, successes))

def compute_state_space_trajectory_and_derivatives(p,psi,dt):

    num_timesteps = len(p)

    psi = trigutils.compute_continuous_angle_array(psi)

    p_dot     = c_[ gradient(p[:,0],dt),     gradient(p[:,1],dt),     gradient(p[:,2],dt)]
    p_dot_dot = c_[ gradient(p_dot[:,0],dt), gradient(p_dot[:,1],dt), gradient(p_dot[:,2],dt)]

    f_thrust_world            = m*p_dot_dot - f_external_world.T.A
    f_thrust_world_normalized = sklearn.preprocessing.normalize(f_thrust_world)

    z_axis_intermediate = c_[ cos(psi), zeros_like(psi), -sin(psi) ]
    y_axis              = f_thrust_world_normalized
    x_axis              = sklearn.preprocessing.normalize(cross(z_axis_intermediate, y_axis))
    z_axis              = sklearn.preprocessing.normalize(cross(y_axis, x_axis))

    theta          = zeros(num_timesteps)
    psi_recomputed = zeros(num_timesteps)
    phi            = zeros(num_timesteps)

    for ti in range(num_timesteps):

        z_axis_ti = c_[matrix(z_axis[ti]),0].T
        y_axis_ti = c_[matrix(y_axis[ti]),0].T
        x_axis_ti = c_[matrix(x_axis[ti]),0].T

        R_world_from_body_ti              = c_[z_axis_ti,y_axis_ti,x_axis_ti,[0,0,0,1]]
        psi_recomputed_ti,theta_ti,phi_ti = transformations.euler_from_matrix(R_world_from_body_ti,"ryxz")

        assert allclose(R_world_from_body_ti, transformations.euler_matrix(psi_recomputed_ti,theta_ti,phi_ti,"ryxz"))

        theta[ti]          = theta_ti
        psi_recomputed[ti] = psi_recomputed_ti
        phi[ti]            = phi_ti

    theta          = trigutils.compute_continuous_angle_array(theta)
    psi_recomputed = trigutils.compute_continuous_angle_array(psi_recomputed)
    phi            = trigutils.compute_continuous_angle_array(phi)

    assert allclose(psi_recomputed, psi)

    psi = psi_recomputed

    theta_dot     = gradient(theta,dt)
    psi_dot       = gradient(psi,dt)
    phi_dot       = gradient(phi,dt)

    theta_dot_dot = gradient(theta_dot,dt)
    psi_dot_dot   = gradient(psi_dot,dt)
    phi_dot_dot   = gradient(phi_dot,dt)

    return p, p_dot, p_dot_dot, theta, theta_dot, theta_dot_dot, psi, psi_dot, psi_dot_dot, phi, phi_dot, phi_dot_dot