def force_torque(y, t):
# Force is actually zero (at least for this case, where the center of
# mass isn't moving).
force = [0, 0, 0]
# Torque is constantly applied from
q = Quaternion(y[6:10])
q.normalize()
F = Quaternion([0, 0, 0, -1])
F_lcl = (q.inverse() * F * q)[1:4]
torque = np.cross([1, 0, 0], F_lcl)
return (force, torque)
def force_torque(y, t): # Force is actually zero (at least for this case, where the center of # mass isn't moving). force = [0,0,0] # Torque is constantly applied from q = Quaternion(y[6:10]) q.normalize() F = Quaternion([0,0,0,-1]) F_lcl = (q.inverse() * F * q)[1:4] torque = np.cross([1,0,0], F_lcl) return (force, torque)
def testNormalize(self):
q = Quaternion(0.4676, -0.4679, 0.5910, 0.4616)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
q = Quaternion(-0.0918, 0.0463, -0.2413, -0.9650)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
q = Quaternion(a = 1000.0002, b = 2.03, c = 0.04, d = 40004.5)
q.normalize()
self.assertLessEqual(1 - q.magnitude(), 0.00001)
self.assertGreaterEqual(1 - q.magnitude(), -0.00001)
