def __init__(self, base_trans=None, base_rotate=None, parent=None): if base_trans is None: base_trans = Vector3([0.0, 0.0, 0.0]) if base_rotate is None: base_rotate = Quaternion.from_eulers([0.0, 0.0, 0.0]) if type(base_trans) is not Vector3: raise TypeError("Oops, trans is not a vector.") if type(base_rotate) is not Quaternion: raise TypeError("Oops, rotate is not a vector.") if parent is not None and type(parent) is not ReferenceFrame: raise TypeError("parent is not a ReferenceFrame.") self.parent = parent self.base_trans = base_trans self.base_rotate = base_rotate self.setTranslation([0.0, 0.0, 0.0]) self.setRotation([0.0, 0.0, 0.0])
def __init__(self, base_trans=None, base_rotate=None, parent=None): if base_trans is None: base_trans = Vector3([0., 0., 0.]); if base_rotate is None: base_rotate = Quaternion.from_eulers([0., 0., 0.]); if type(base_trans) is not Vector3: raise TypeError("Oops, trans is not a vector."); if type(base_rotate) is not Quaternion: raise TypeError("Oops, rotate is not a vector."); if parent is not None and type(parent) is not ReferenceFrame: raise TypeError("parent is not a ReferenceFrame."); self.parent = parent; self.base_trans = base_trans; self.base_rotate = base_rotate; self.setTranslation([0., 0., 0.]); self.setRotation([0., 0., 0.]);
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
def __init__(self, parent_ref, config): irot, itrans = config.getInitialPose() self.ref = ReferenceFrame(Vector3(itrans), Quaternion.from_eulers(irot), parent_ref) self.legs = []
def setRotation(self, rotate): self.rotate_raw = rotate self.rotate = self.base_rotate * Quaternion.from_eulers(rotate)
def setRotation(self, rotate): self.rotate_raw = rotate; self.rotate = self.base_rotate * Quaternion.from_eulers(rotate);
def __init__(self, parent_ref, config): irot, itrans = config.getInitialPose(); self.ref = ReferenceFrame(Vector3(itrans), Quaternion.from_eulers(irot), parent_ref); self.legs = [];