def test_constant_linear_accelerating_rotation(self): """Test rotational acceleration of constant-motion body in each axis.""" for axis in range(3): for i in range(self.M): b = RigidBody() txyz = [ random.uniform(-1.0, 1.0) if axis == i else 0 for i in range(3) ] mass = random.uniform(0.1, 100) vxyz = random_vector() b.set_vxyz(vxyz) I_cm = np.eye(3) * mass * 0.4 b.set_mass(mass) b.force_torque = lambda y, t: ([0, 0, 0], txyz) b.f_Icm = lambda y, t: I_cm b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() wxyz_f = [t / (0.4 * mass) for t in txyz] self.assertTrue(vdiff_len(b.get_wxyz(), wxyz_f) < EPS_B) self.assertTrue(vdiff_len(b.get_vxyz(), vxyz) < EPS_A) self.assertTrue( vdiff_len(b.get_xyz(), vxyz) / vlength(vxyz) < EPS_B)
def test_accelerating_linear_accelerating_rotation(self): """Test rotational acceleration of accelerating body in each axis.""" for axis in range(3): for i in range(self.M): b = RigidBody() txyz = [ random.uniform(-1.0, 1.0) if axis == i else 0 for i in range(3) ] mass = random.uniform(0.1, 100) fxyz = random_vector() I_cm = np.eye(3) * mass * 0.4 b.set_mass(mass) b.force_torque = lambda y, t: (fxyz, txyz) b.f_Icm = lambda y, t: I_cm b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() wxyz_f = [t / (0.4 * mass) for t in txyz] axyz = [f / mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5 * a for a in axyz] self.assertTrue(vdiff_len(b.get_wxyz(), wxyz_f) < EPS_B) self.assertTrue( vdiff_len(b.get_vxyz(), axyz) / axyz_len < EPS_B) self.assertTrue( vdiff_len(b.get_xyz(), xyz_f) / axyz_len < EPS_B)
def test_geosynchronous(self): b = RigidBody() c = CraftModel() mass = 5.0 b.f_mass = lambda y, t: mass b.f_Icm = lambda y, t: 0.4 * np.eye(3) * mass #g_ft = CraftModel.gravity_force_factory() b.force_torque = CraftModel.gravity_force_factory(mass=5.0) r = (EarthFrame.G * EarthFrame.earth_mass / (EarthFrame.omega_earth**2))**(1.0 / 3.0) v = r * EarthFrame.omega_earth #print("r = %f, v = %f" % (r,v)) b.set_xyz([r, 0, 0]) b.set_vxyz([0, -v, 0]) b.set_epoch(EarthFrame.epoch_j2000) b.start() #print(b) for i in range(60): b.step(60.0) lat, lon, elev = EarthFrame.ECITime2LatLonElev( b.get_xyz(), b.getDatetime()) #print(b) #print(lat, lon, elev) self.assertTrue(abs(lat - 0.0) < 1e-3) self.assertTrue(abs(lon - 0.0) < 1e-3) self.assertTrue(abs(elev + EarthFrame.earth_radius - r) < 1e-3) self.assertTrue(abs(np.linalg.norm(b.get_vxyz()) - v) < 1e-3)
def test_accelerating_linear_constant_rotation(self): """Test rotation of stationary body in all axes while in constant linear motion.""" for w in self.w_states: b = RigidBody() fxyz = random_vector() mass = random.uniform(0.1, 100) b.set_mass(mass) b.force_torque = lambda y, t: (fxyz, [0, 0, 0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4 * mass * 1.0**2 b.set_wxyz(w) b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() #print("w%s -> %f:%s" % (b.get_wxyz(), vlength(b.get_Q()),b.get_Q())) self.assertTrue( vdiff_len(b.get_Q(), [1, 0, 0, 0]) < 1e-4 or vdiff_len(b.get_Q(), [-1, 0, 0, 0]) < 1e-4) self.assertTrue(vdiff_len(b.get_wxyz(), w) < EPS_A) axyz = [f / mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5 * a for a in axyz] self.assertTrue(vdiff_len(b.get_vxyz(), axyz) / axyz_len < EPS_B) self.assertTrue(vdiff_len(b.get_xyz(), xyz_f) / axyz_len < EPS_B)
def test_geosynchronous(self): b = RigidBody() c = CraftModel() mass = 5.0 b.f_mass = lambda y, t: mass b.f_Icm = lambda y, t: 0.4 * np.eye(3) * mass # g_ft = CraftModel.gravity_force_factory() b.force_torque = CraftModel.gravity_force_factory(mass=5.0) r = (EarthFrame.G * EarthFrame.earth_mass / (EarthFrame.omega_earth ** 2)) ** (1.0 / 3.0) v = r * EarthFrame.omega_earth # print("r = %f, v = %f" % (r,v)) b.set_xyz([r, 0, 0]) b.set_vxyz([0, -v, 0]) b.set_epoch(EarthFrame.epoch_j2000) b.start() # print(b) for i in range(60): b.step(60.0) lat, lon, elev = EarthFrame.ECITime2LatLonElev(b.get_xyz(), b.getDatetime()) # print(b) # print(lat, lon, elev) self.assertTrue(abs(lat - 0.0) < 1e-3) self.assertTrue(abs(lon - 0.0) < 1e-3) self.assertTrue(abs(elev + EarthFrame.earth_radius - r) < 1e-3) self.assertTrue(abs(np.linalg.norm(b.get_vxyz()) - v) < 1e-3)
def test_accelerating_linear_accelerating_rotation(self): """Test rotational acceleration of accelerating body in each axis.""" for axis in range(3): for i in range(self.M): b = RigidBody() txyz = [random.uniform(-1.0,1.0) if axis==i else 0 for i in range(3)] mass = random.uniform(0.1,100) fxyz = random_vector() I_cm = np.eye(3) *mass*0.4 b.set_mass(mass) b.force_torque = lambda y, t: (fxyz,txyz) b.f_Icm = lambda y, t: I_cm b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() wxyz_f = [t / (0.4*mass) for t in txyz] axyz = [f/mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5*a for a in axyz] self.assertTrue(vdiff_len(b.get_wxyz(), wxyz_f) < EPS_B) self.assertTrue(vdiff_len(b.get_vxyz(), axyz)/axyz_len < EPS_B) self.assertTrue(vdiff_len(b.get_xyz(), xyz_f)/axyz_len < EPS_B)
def test_gyroscope_precession(self): """Verify that gyroscopic precession works as expected.""" b = RigidBody() w_mult = 100 w = w_mult / (2 * np.pi) I_cm = np.eye(3) mass = 1.0 b.f_mass = lambda y,t: mass b.f_Icm = lambda y,t: I_cm b.set_wxyz([w,0,0]) 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) b.force_torque = force_torque EPS = 1e-2 N = 200 b.start() for i in range(N): b.step(w_mult/(N*4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([0,1,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue(vdiff_len([0,1,0], RigidBody.local2body(b.get_Q(),[1,0,0])) < EPS) for i in range(N): b.step(w_mult/(N*4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([-1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue(vdiff_len([-1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0])) < EPS) for i in range(N): b.step(w_mult/(N*4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([0,-1,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue(vdiff_len([0,-1,0], RigidBody.local2body(b.get_Q(),[1,0,0])) < EPS) for i in range(N): b.step(w_mult/(N*4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue(vdiff_len([1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0])) < EPS)
def test_static(self): """Test unmoving body case.""" b = RigidBody() b.set_Q([1, 0, 0, 0]) b.set_xyz([0, 0, 0]) b.set_wxyz([0, 0, 0]) b.set_vxyz([0, 0, 0]) b.force_torque = lambda y, t: ([0, 0, 0], [0, 0, 0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4 * b.mass * 1.0**2 b.start() for i in range(self.N): b.step(1.0 / self.N) self.assertTrue(vdiff_len(b.get_Q(), [1, 0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_xyz(), [0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(), [0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_vxyz(), [0, 0, 0]) < EPS_A)
def test_static(self): """Test unmoving body case.""" b = RigidBody() b.set_Q([1,0,0,0]) b.set_xyz([0,0,0]) b.set_wxyz([0,0,0]) b.set_vxyz([0,0,0]) b.force_torque = lambda y, t: ([0,0,0],[0,0,0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4*b.mass * 1.0**2 b.start() for i in range(self.N): b.step(1.0/self.N) self.assertTrue(vdiff_len(b.get_Q(),[1,0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_xyz(),[0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(),[0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_vxyz(),[0,0,0]) < EPS_A)
def test_fixed_linear_accelerating_rotation(self): """Test rotational acceleration of stationary body in each axis.""" for axis in range(3): for i in range(self.M): b = RigidBody() txyz = [random.uniform(-1.0,1.0) if axis==i else 0 for i in range(3)] mass = random.uniform(0.1,100) I_cm = np.eye(3) *mass*0.4 b.set_mass(mass) b.force_torque = lambda y, t: ([0,0,0],txyz) b.f_Icm = lambda y, t: I_cm b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() wxyz_f = [t / (0.4*mass) for t in txyz] self.assertTrue(vdiff_len(b.get_wxyz(), wxyz_f) < EPS_B)
def test_constant_linear(self): """Test constant linear motion in three dimensions without rotation.""" for i in range(self.M): b = RigidBody() vxyz = random_vector() b.set_Q([1,0,0,0]) b.set_xyz([0,0,0]) b.set_wxyz([0,0,0]) b.set_vxyz(vxyz) b.force_torque = lambda y, t: ([0,0,0], [0,0,0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4*b.mass * 1.0**2 b.start() #print("v=%s\ns=%s\nl=%s\n" % (vxyz,b,vdiff_len(b.get_xyz(),vxyz))) for i in range(self.N): b.step(1.0/self.N) self.assertTrue(vdiff_len(b.get_Q(),[1,0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(),[0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_vxyz(),vxyz) < EPS_A) #print("v=%s\ns=%s\nl=%s\n" % (vxyz,b,vdiff_len(b.get_xyz(),vxyz))) self.assertTrue(vdiff_len(b.get_xyz(),vxyz) / vlength(vxyz) < EPS_B)
def test_constant_linear(self): """Test constant linear motion in three dimensions without rotation.""" for i in range(self.M): b = RigidBody() vxyz = random_vector() b.set_Q([1, 0, 0, 0]) b.set_xyz([0, 0, 0]) b.set_wxyz([0, 0, 0]) b.set_vxyz(vxyz) b.force_torque = lambda y, t: ([0, 0, 0], [0, 0, 0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4 * b.mass * 1.0**2 b.start() #print("v=%s\ns=%s\nl=%s\n" % (vxyz,b,vdiff_len(b.get_xyz(),vxyz))) for i in range(self.N): b.step(1.0 / self.N) self.assertTrue(vdiff_len(b.get_Q(), [1, 0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(), [0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_vxyz(), vxyz) < EPS_A) #print("v=%s\ns=%s\nl=%s\n" % (vxyz,b,vdiff_len(b.get_xyz(),vxyz))) self.assertTrue( vdiff_len(b.get_xyz(), vxyz) / vlength(vxyz) < EPS_B)
def test_accelerating_linear(self): """Test linear acceleration in three dimensions without rotation.""" for i in range(self.M): b = RigidBody() fxyz = random_vector() mass = random.uniform(0.1, 100) b.set_Q([1, 0, 0, 0]) b.set_xyz([0, 0, 0]) b.set_wxyz([0, 0, 0]) b.set_vxyz([0, 0, 0]) b.set_mass(mass) b.force_torque = lambda y, t: (fxyz, [0, 0, 0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4 * mass * 1.0**2 b.start() for i in range(self.N): b.step(1.0 / self.N) self.assertTrue(vdiff_len(b.get_Q(), [1, 0, 0, 0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(), [0, 0, 0]) < EPS_A) axyz = [f / mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5 * a for a in axyz] self.assertTrue(vdiff_len(b.get_vxyz(), axyz) / axyz_len < EPS_B) self.assertTrue(vdiff_len(b.get_xyz(), xyz_f) / axyz_len < EPS_B)
def test_accelerating_linear(self): """Test linear acceleration in three dimensions without rotation.""" for i in range(self.M): b = RigidBody() fxyz = random_vector() mass = random.uniform(0.1,100) b.set_Q([1,0,0,0]) b.set_xyz([0,0,0]) b.set_wxyz([0,0,0]) b.set_vxyz([0,0,0]) b.set_mass(mass) b.force_torque = lambda y, t: (fxyz,[0,0,0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4*mass * 1.0**2 b.start() for i in range(self.N): b.step(1.0/self.N) self.assertTrue(vdiff_len(b.get_Q(),[1,0,0,0]) < EPS_A) self.assertTrue(vdiff_len(b.get_wxyz(),[0,0,0]) < EPS_A) axyz = [f/mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5*a for a in axyz] self.assertTrue(vdiff_len(b.get_vxyz(), axyz)/axyz_len < EPS_B) self.assertTrue(vdiff_len(b.get_xyz(), xyz_f)/axyz_len < EPS_B)
def test_accelerating_linear_constant_rotation(self): """Test rotation of stationary body in all axes while in constant linear motion.""" for w in self.w_states: b = RigidBody() fxyz = random_vector() mass = random.uniform(0.1,100) b.set_mass(mass) b.force_torque = lambda y, t: (fxyz,[0,0,0]) b.f_Icm = lambda y, t: np.eye(3) * 0.4*mass * 1.0**2 b.set_wxyz(w) b.start() for i in range(self.N): b.step(1.0 / self.N) #if i % 10 == 0: b.normalize_Q() #print("w%s -> %f:%s" % (b.get_wxyz(), vlength(b.get_Q()),b.get_Q())) self.assertTrue(vdiff_len(b.get_Q(), [1,0,0,0]) < 1e-4 or vdiff_len(b.get_Q(), [-1,0,0,0])< 1e-4) self.assertTrue(vdiff_len(b.get_wxyz(), w) < EPS_A) axyz = [f/mass for f in fxyz] axyz_len = vlength(axyz) xyz_f = [0.5*a for a in axyz] self.assertTrue(vdiff_len(b.get_vxyz(), axyz)/axyz_len < EPS_B) self.assertTrue(vdiff_len(b.get_xyz(), xyz_f)/axyz_len < EPS_B)
def test_gyroscope_precession(self): """Verify that gyroscopic precession works as expected.""" b = RigidBody() w_mult = 100 w = w_mult / (2 * np.pi) I_cm = np.eye(3) mass = 1.0 b.f_mass = lambda y, t: mass b.f_Icm = lambda y, t: I_cm b.set_wxyz([w, 0, 0]) 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) b.force_torque = force_torque EPS = 1e-2 N = 200 b.start() for i in range(N): b.step(w_mult / (N * 4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([0,1,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue( vdiff_len([0, 1, 0], RigidBody.local2body(b.get_Q(), [1, 0, 0])) < EPS) for i in range(N): b.step(w_mult / (N * 4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([-1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue( vdiff_len([-1, 0, 0], RigidBody.local2body(b.get_Q(), [1, 0, 0])) < EPS) for i in range(N): b.step(w_mult / (N * 4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([0,-1,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue( vdiff_len([0, -1, 0], RigidBody.local2body(b.get_Q(), [1, 0, 0])) < EPS) for i in range(N): b.step(w_mult / (N * 4)) b.normalize_Q() #print("t: %8f, %s" % (b.t, RigidBody.local2body(b.get_Q(),[1,0,0]))) #print("diff = %f" % vdiff_len([1,0,0], RigidBody.local2body(b.get_Q(),[1,0,0]))) self.assertTrue( vdiff_len([1, 0, 0], RigidBody.local2body(b.get_Q(), [1, 0, 0])) < EPS)