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 = [];
