def run_get_i2c_imu():
try:
nonlocal uvw
coms = get_coms_in_range()
con, pro = get_i2c_imu(coms[0])
pro.start()
acc = (float(0), ) * 3
while True:
orient1 = pro.protocol.imu.get_instant_orientation()
if np.isnan(acc).any():
acc = (float(0), ) * 3
if not np.isnan(orient1).any():
qxyz = [
orient1 * Quaternion.from_axis(v) * ~orient1
for v in uvw.transpose()
]
xyz = np.asarray([np.asarray(x.xyz)
for x in qxyz]).transpose()
obj.mlab_source.set(x=xyz[0], y=xyz[1], z=xyz[2])
time.sleep(0)
yield
except:
e = traceback.format_exc()
print(e)
print("ded")
def bounce(self, dist, norm):
if dist > self.r:
return False
norm = numpy.array(norm)
rot = self.rotation
rot_mat = rot.matrix33
low_pt = None
for xp in -1, 1:
for yp in -1, 1:
for zp in -1, 1:
point = rot_mat * Vector3([xp, yp, zp])
if low_pt is None or point[2] < low_pt[2]:
low_pt = point
if low_pt[2] >= dist:
return False
#play_sound('die-hit')
self.locked = False
rot_imp = Vector3(norm).cross(Vector3(
(*low_pt.xy, 0.0))) * (.5 + hypot(*self.speed) / 10)
riq = Quaternion.from_axis(rot_imp)
ump = -Vector3(self.speed).cross(norm)
umpq = Quaternion.from_axis(ump)
if np.isnan(umpq).any():
umpq = Quaternion()
self.rotation_speed = Quaternion().lerp(
umpq * riq * self.rotation_speed, 0.7)
self.speed *= DAMPING_BOUNCE
# Reflect speed
if linalg_norm(norm) >= 2:
1 / 0
s = self.speed
self.speed = s - 2 * s.dot(norm) * norm
self.pos += (self.r - dist) * norm
return True
def get_orientation(self, convergence_acc: float, overshoot_acc: float, convergence_mag: float,
overshoot_mag: float):
# https://www.sciencedirect.com/science/article/pii/S2405896317321201
if self.prev_up is not None:
t2 = time.time()
k_acc, k_bias_acc = get_ki_kbias(convergence_acc, overshoot_acc, t2 - self.prev_orientation_time)
k_mag, k_bias_mag = get_ki_kbias(convergence_mag, overshoot_mag, t2 - self.prev_orientation_time)
grav = np.asarray(unit_vector(self.get_grav()))
mag = unit_vector(np.asarray(self.get_mag()))
east = unit_vector(np.cross(mag, grav))
west = -east
north = -unit_vector(np.cross(east, grav))
up = -unit_vector(np.cross(north, east))
gyro = -self.get_gyro()
if self.prev_bias is not None:
if np.isnan(self.prev_bias).any():
self.prev_bias = None
else:
gyro += self.prev_bias
gy = Quaternion.from_eulers(gyro * (t2 - self.prev_orientation_time))
pred_west = (~gy * Quaternion(np.pad(self.prev_west, (0,1), 'constant', constant_values=0)) * gy)
pred_up = (~gy * Quaternion(np.pad(self.prev_up, (0,1), 'constant', constant_values=0)) * gy)
pred_north = (~gy * Quaternion(np.pad(self.prev_north, (0,1), 'constant', constant_values=0)) * gy)
pred_north_v = np.asarray(pred_north.xyz)
pred_west_v = np.asarray(pred_west.xyz)
pred_up_v = np.asarray(pred_up.xyz)
fix_north = Quaternion.from_axis(np.cross(north, pred_north_v) * k_mag)
fix_west = Quaternion.from_axis(np.cross(west, pred_west_v) * k_mag)
fix_up = Quaternion.from_axis(np.cross(up, pred_west_v) * k_acc)
x_acc = np.cross(up, pred_up_v)
x_mag = np.cross(north, pred_north_v)
pb0 = k_bias_acc*x_acc + k_bias_mag*x_mag
if self.prev_bias is not None:
self.prev_bias = self.prev_bias * (t2 - self.prev_orientation_time) + pb0
else:
self.prev_bias = pb0
true_north = unit_vector((pred_north * fix_north).xyz)
true_up = unit_vector((pred_up * fix_up).xyz)
true_west = unit_vector((pred_west * fix_west).xyz)
rot = np.asarray([true_north, true_west, true_up]).transpose()
q = Quaternion.from_matrix(rot)
if not np.isnan(north).any():
self.prev_north = north
if not np.isnan(west).any():
self.prev_west = west
if not np.isnan(up).any():
self.prev_up = up
self.prev_orientation_time = t2
return q
else:
grav = np.asarray(unit_vector(self.get_grav()))
mag = unit_vector(np.asarray(self.get_mag()))
east = unit_vector(np.cross(mag, grav))
north = -unit_vector(np.cross(east, grav))
up = -unit_vector(np.cross(north, east))
rot = np.asarray([north, -east, up]).transpose()
q = Quaternion.from_matrix(rot)
if (not np.isnan(grav).any()) and (not np.isnan(mag).any()):
self.prev_west = -east
self.prev_north = north
self.prev_up = up
self.prev_orientation_time = time.time()
return q
