def to_euler(self, units='rad'): euler_data = trans.euler_from_matrix(self.to_matrix(), axes='rxyz') assert units.lower() in ['rad', 'deg'] if units.lower() == 'rad': return RotationEulerRadians(*euler_data) else: return RotationEulerDegrees(*np.degrees(euler_data))
def getAttitude(self): ax,ay,az = self.lsm.readRawAccel() gx,gy,gz = self.l3g.readRawGyro() mx,my,mz = self.lsm.readRawMag() line=[ax,ay,az,gx,gy,gz,mx,my,mz] norm_accel,omega_measured,norm_magne = self.parseData(line) if(self.firstRun): self.b_d=np.array([[norm_magne[0][0]],[norm_magne[1][0]],[norm_magne[2][0]]]) self.g_i=np.array([[norm_accel[0][0]],[norm_accel[1][0]],[norm_accel[2][0]]]) self.firstRun=False b_b=np.dot(self.Cbi_hat,self.b_d) g_b=np.dot(self.Cbi_hat,self.g_i) r=-10*(np.dot(self.cross(g_b),norm_accel) + np.dot(self.cross(b_b),norm_magne)) omega_hat = omega_measured+r omega_hat_mag = math.sqrt(omega_hat[0][0]**2+omega_hat[1][0]**2+omega_hat[2][0]**2) omega_hat_mag_r = 1.0/omega_hat_mag Ak = np.eye(3) - self.cross(omega_hat)*math.sin(omega_hat_mag*self.T_sample)*omega_hat_mag_r + (1-math.cos(omega_hat_mag*self.T_sample))*np.dot(self.cross(omega_hat),self.cross(omega_hat))*omega_hat_mag_r**2 self.Cbi_hat = np.dot(Ak,self.Cbi_hat) euler=tf.euler_from_matrix(self.Cbi_hat.T, axes='szyx') euler=tuple([x*180.0/math.pi for x in euler]) return euler
def from_matrix(cls, matrix, axes='rxyz'): # Change to 4x4 if 3x3 rotation matrix is given if matrix.shape[0] == 3: mat = np.identity(4) mat[:3, :3] = matrix matrix = mat coords = trans.euler_from_matrix(matrix, axes=axes) return cls(*np.degrees(coords))
def getAttitude(self, arg): b_d = np.array([[arg.mag_field[0]], [arg.mag_field[1]], [arg.mag_field[2]]]) ax, ay, az = arg.accel gx, gy, gz = arg.gyro mx, my, mz = arg.mag line = [ax, ay, az, gx, gy, gz, mx, my, mz] norm_accel, omega_measured, norm_magne = self.parseData(line) norm_accel = np.dot(self.C_em, norm_accel) omega_measured = np.dot(self.C_em, omega_measured) norm_magne = np.dot(self.C_em, norm_magne) ''' if(self.firstRun): self.b_d=np.array([[norm_magne[0][0]],[norm_magne[1][0]],[norm_magne[2][0]]]) self.g_i=np.array([[norm_accel[0][0]],[norm_accel[1][0]],[norm_accel[2][0]]]) self.firstRun=False ''' b_b = np.dot(self.Cbi_hat, self.b_d) g_b = np.dot(self.Cbi_hat, self.g_i) r = -5 * (np.dot(self.cross(g_b), norm_accel) + 0.25 * np.dot(self.cross(b_b), norm_magne)) omega_hat = omega_measured + r omega_hat_mag = math.sqrt(omega_hat[0][0]**2 + omega_hat[1][0]**2 + omega_hat[2][0]**2) omega_hat_mag_r = 1.0 / omega_hat_mag Ak = np.eye(3) - self.cross(omega_hat) * math.sin( omega_hat_mag * self.T_sample) * omega_hat_mag_r + ( 1 - math.cos(omega_hat_mag * self.T_sample)) * np.dot( self.cross(omega_hat), self.cross(omega_hat)) * omega_hat_mag_r**2 self.Cbi_hat = np.dot(Ak, self.Cbi_hat) euler = tf.euler_from_matrix(self.Cbi_hat.T, axes='szyx') euler = tuple([x * 180.0 / math.pi for x in euler]) return euler, self.Cbi_hat
def getAttitude(self,arg): b_d = np.array([[arg.mag_field[0]], [arg.mag_field[1]] , [arg.mag_field[2]]]) ax,ay,az = arg.accel gx,gy,gz = arg.gyro mx,my,mz = arg.mag line=[ax,ay,az,gx,gy,gz,mx,my,mz] norm_accel,omega_measured,norm_magne = self.parseData(line) norm_accel = np.dot(self.C_em, norm_accel) omega_measured = np.dot(self.C_em, omega_measured) norm_magne = np.dot(self.C_em, norm_magne) ''' if(self.firstRun): self.b_d=np.array([[norm_magne[0][0]],[norm_magne[1][0]],[norm_magne[2][0]]]) self.g_i=np.array([[norm_accel[0][0]],[norm_accel[1][0]],[norm_accel[2][0]]]) self.firstRun=False ''' b_b=np.dot(self.Cbi_hat,self.b_d) g_b=np.dot(self.Cbi_hat,self.g_i) r=-5*(np.dot(self.cross(g_b),norm_accel) + 0.25*np.dot(self.cross(b_b),norm_magne)) omega_hat = omega_measured+r omega_hat_mag = math.sqrt(omega_hat[0][0]**2+omega_hat[1][0]**2+omega_hat[2][0]**2) omega_hat_mag_r = 1.0/omega_hat_mag Ak = np.eye(3) - self.cross(omega_hat)*math.sin(omega_hat_mag*self.T_sample)*omega_hat_mag_r + (1-math.cos(omega_hat_mag*self.T_sample))*np.dot(self.cross(omega_hat),self.cross(omega_hat))*omega_hat_mag_r**2 self.Cbi_hat = np.dot(Ak,self.Cbi_hat) euler=tf.euler_from_matrix(self.Cbi_hat.T, axes='szyx') euler=tuple([x*180.0/math.pi for x in euler]) return euler, self.Cbi_hat
def getAttitude(self): ax, ay, az = self.lsm.readRawAccel() gx, gy, gz = self.l3g.readRawGyro() mx, my, mz = self.lsm.readRawMag() line = [ax, ay, az, gx, gy, gz, mx, my, mz] norm_accel, omega_measured, norm_magne = self.parseData(line) if (self.firstRun): self.b_d = np.array([[norm_magne[0][0]], [norm_magne[1][0]], [norm_magne[2][0]]]) self.g_i = np.array([[norm_accel[0][0]], [norm_accel[1][0]], [norm_accel[2][0]]]) self.firstRun = False b_b = np.dot(self.Cbi_hat, self.b_d) g_b = np.dot(self.Cbi_hat, self.g_i) r = -10 * (np.dot(self.cross(g_b), norm_accel) + np.dot(self.cross(b_b), norm_magne)) omega_hat = omega_measured + r omega_hat_mag = math.sqrt(omega_hat[0][0]**2 + omega_hat[1][0]**2 + omega_hat[2][0]**2) omega_hat_mag_r = 1.0 / omega_hat_mag Ak = np.eye(3) - self.cross(omega_hat) * math.sin( omega_hat_mag * self.T_sample) * omega_hat_mag_r + ( 1 - math.cos(omega_hat_mag * self.T_sample)) * np.dot( self.cross(omega_hat), self.cross(omega_hat)) * omega_hat_mag_r**2 self.Cbi_hat = np.dot(Ak, self.Cbi_hat) euler = tf.euler_from_matrix(self.Cbi_hat.T, axes='szyx') euler = tuple([x * 180.0 / math.pi for x in euler]) return euler
np.dot(cross(b_b), norm_magne)) omega_hat = omega_measured + r omega_hat_mag = math.sqrt(omega_hat[0][0]**2 + omega_hat[1][0]**2 + omega_hat[2][0]**2) omega_hat_mag_r = 1.0 / omega_hat_mag Ak = np.eye(3) - cross(omega_hat) * math.sin( omega_hat_mag * T_sample) * omega_hat_mag_r + ( 1 - math.cos(omega_hat_mag * T_sample)) * np.dot( cross(omega_hat), cross(omega_hat)) * omega_hat_mag_r**2 Cbi_hat_new = np.dot(Ak, Cbi_hat) euler = tf.euler_from_matrix(Cbi_hat_new.T, axes='szyx') euler = tuple([x for x in euler]) #setSpeed(dac,euler[0]*5.0/180.0) #print euler #print str(Cbi_hat) counter += 1 if (counter >= 4): b_body_d = np.dot(C_d, b_d) g_body_d = np.dot(C_d, g_i) #u_p = 0.05*(np.dot(cross(g_body_d),g_b) + np.dot(cross(b_body_d),b_b)) u_d = -0.01 * omega_measured[2][0] #u_p = np.dot(np.dot(u_p.T,Cbi_hat),np.array([[0],[0],[1]]))
rbody = tracker.rigid_bodies[RIGID_BODY_NAME] assert bool(rbody), "No rigid body position found. Make sure rigid bodies are streaming from Motive (off in Motive now by default)" except KeyError: raise KeyError("Rigid Body {} not detected. Add it in Motive, so we can track it!".format(RIGID_BODY_NAME)) print(tracker.rigid_bodies) # Load up Projector with open('projector_data.pickle') as f: projdict = pickle.load(f) modelmat = np.identity(4) modelmat[:-1, :-1] = -projdict['rotation'] rot = np.degrees(trans.euler_from_matrix(modelmat, 'rxyz')) print(modelmat) print(rot) projector = rc.Camera(fov_y=projdict['fov_y'], position=projdict['position']) # projector.rot_x = -90 projector.rot_x = rot[0] - 1. projector.rot_y = rot[1] projector.rot_z = 180 - rot[2] # projector._rot_matrix = projdict['rotation'] display = pyglet.window.get_platform().get_default_display() screen = display.get_screens()[1] window = pyglet.window.Window(vsync=True, fullscreen=True, screen=screen) reader = rc.WavefrontReader(rc.resources.obj_primitives) sphere = reader.get_mesh('Sphere', scale=.01)
firstRun=False b_b=np.dot(Cbi_hat,b_d) g_b=np.dot(Cbi_hat,g_i) r=-10*(np.dot(cross(g_b),norm_accel) + np.dot(cross(b_b),norm_magne)) omega_hat = omega_measured+r omega_hat_mag = math.sqrt(omega_hat[0][0]**2+omega_hat[1][0]**2+omega_hat[2][0]**2) omega_hat_mag_r = 1.0/omega_hat_mag Ak = np.eye(3) - cross(omega_hat)*math.sin(omega_hat_mag*T_sample)*omega_hat_mag_r + (1-math.cos(omega_hat_mag*T_sample))*np.dot(cross(omega_hat),cross(omega_hat))*omega_hat_mag_r**2 Cbi_hat_new = np.dot(Ak,Cbi_hat) euler=tf.euler_from_matrix(Cbi_hat_new.T, axes='szyx') euler=tuple([x for x in euler]) #setSpeed(dac,euler[0]*5.0/180.0) #print euler #print str(Cbi_hat) counter+=1 if(counter>=4): b_body_d = np.dot(C_d,b_d) g_body_d = np.dot(C_d,g_i) #u_p = 0.05*(np.dot(cross(g_body_d),g_b) + np.dot(cross(b_body_d),b_b)) u_d = -0.01*omega_measured[2][0] #u_p = np.dot(np.dot(u_p.T,Cbi_hat),np.array([[0],[0],[1]]))