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)