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