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