def test_set_get_xyz(self): """Test setters and getters for state position vector""" b = RigidBody() for xyz in self.testVectors: b.set_xyz(xyz) self.assertEqual(b.state_vector[0:3], xyz) self.assertEqual(b.get_xyz(), xyz)
def test_set_get_mass(self): b = RigidBody() for i in range(100): m = random.uniform(1e-6,1e6) b.set_mass(m) self.assertEqual(b.get_mass(), m) self.assertEqual(b.mass, m)
def test_set_get_vxyz(self): """Test setters and getters for state linear velocity""" b = RigidBody() for vxyz in self.testVectors: b.set_vxyz(vxyz) self.assertEqual(b.state_vector[3:6], vxyz) self.assertEqual(b.get_vxyz(), vxyz)
def test_set_get_wxyz(self): """Test setters and getters for state angular velocity""" b = RigidBody() for wxyz in self.testVectors: b.set_wxyz(wxyz) self.assertEqual(b.state_vector[10:13], wxyz) self.assertEqual(b.get_wxyz(), wxyz)
def test_set_get_mass(self): b = RigidBody() for i in range(100): m = random.uniform(1e-6, 1e6) b.set_mass(m) self.assertEqual(b.get_mass(), m) self.assertEqual(b.mass, m)
class AtlasSimModelSimulation(IAtlasSimModel): def __init__(self, realtime=False): IAtlasSimModel.__init__(self, realtime) self.sim_model = RigidBody() self.stepsize = 0.1 # s self.sim_model.start() def getSimData(self): self.sim_model.step(self.stepsize) sv = self.sim_model.state_vector date = self.sim_model.getDatetime() t = self.sim_model.t pxyz = sv[0:3] vxyz = sv[3:6] q = sv[6:10] wxyz = sv[10:12] costates = sv[13:] return AtlasSimData(date, t, pxyz, vxyz, q, wxyz, costates) def run(self): s = sched.scheduler(time.time, time.sleep) for i in range(100): d = self.getSimData() self.update_views(d)
def test_time(self): """Test that time passes correctly.""" b = RigidBody() b.t = 0 b.start() b.step(0.1) self.assertTrue(abs(b.t - 0.1) < EPS_A) b.step(0.2) self.assertTrue(abs(b.t - 0.3) < EPS_A) b.step(0.3) self.assertTrue(abs(b.t - 0.6) < EPS_A)
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_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_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_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_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 __init__(self, realtime=False): IAtlasSimModel.__init__(self, realtime) self.sim_model = RigidBody() self.stepsize = 0.1 # s self.sim_model.start()
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_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_set_get_Q(self): """Test setters and getters for state quaternion""" b = RigidBody() Q = [1,0,0,0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0,1,0,0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0,0,1,0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0,0,0,1] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0.5,0,0,0] b.set_Q(Q) Q = [1,0,0,0] for i in range(len(Q)): self.assertTrue(b.get_Q()[i] - Q[i] < EPS_A) self.assertTrue(b.state_vector[6+i] - Q[i] < EPS_A) Q = [3,-4,0,0] b.set_Q(Q) Q = [3/5,-4/5,0,0] for i in range(len(Q)): self.assertTrue(b.get_Q()[i] - Q[i] < EPS_A) self.assertTrue(b.state_vector[6+i] - Q[i] < EPS_A)
def test_torque_free_precession(self): """Verify that angular momentum L is conserved in the global reference frame for asymmetric bodies.""" for i in range(self.M): b = RigidBody() mass, I_cm = random_body() #print("%s\n%s\n" %(I_cm, np.linalg.cholesky(I_cm))) b.f_mass = lambda y,t: mass b.f_Icm = lambda y,t: I_cm b.set_wxyz(random_vector(scale=1e1)) L_g_0 = b.get_Lxyz_global() len_L_g_0 = vlength(L_g_0) #print("L_g_0 = %s" % L_g_0) b.start() for i in range(self.N): b.step(1.0/self.N) #print("L_g = %s, %f" % (b.get_Lxyz_global(), vdiff_len(b.get_Lxyz_global(), L_g_0)/len_L_g_0)) self.assertTrue(vdiff_len(b.get_Lxyz_global(), L_g_0) / len_L_g_0 < 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_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_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_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_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_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_constant_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() vxyz = random_vector() b.set_vxyz(vxyz) 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_xyz(), vxyz) / vlength(vxyz) < EPS_A)
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_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_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_torque_free_precession(self): """Verify that angular momentum L is conserved in the global reference frame for asymmetric bodies.""" for i in range(self.M): b = RigidBody() mass, I_cm = random_body() #print("%s\n%s\n" %(I_cm, np.linalg.cholesky(I_cm))) b.f_mass = lambda y, t: mass b.f_Icm = lambda y, t: I_cm b.set_wxyz(random_vector(scale=1e1)) L_g_0 = b.get_Lxyz_global() len_L_g_0 = vlength(L_g_0) #print("L_g_0 = %s" % L_g_0) b.start() for i in range(self.N): b.step(1.0 / self.N) #print("L_g = %s, %f" % (b.get_Lxyz_global(), vdiff_len(b.get_Lxyz_global(), L_g_0)/len_L_g_0)) self.assertTrue( vdiff_len(b.get_Lxyz_global(), L_g_0) / len_L_g_0 < EPS_B)
def test_set_get_Q(self): """Test setters and getters for state quaternion""" b = RigidBody() Q = [1, 0, 0, 0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0, 1, 0, 0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0, 0, 1, 0] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0, 0, 0, 1] b.set_Q(Q) self.assertEqual(b.state_vector[6:10], Q) self.assertEqual(b.get_Q(), Q) Q = [0.5, 0, 0, 0] b.set_Q(Q) Q = [1, 0, 0, 0] for i in range(len(Q)): self.assertTrue(b.get_Q()[i] - Q[i] < EPS_A) self.assertTrue(b.state_vector[6 + i] - Q[i] < EPS_A) Q = [3, -4, 0, 0] b.set_Q(Q) Q = [3 / 5, -4 / 5, 0, 0] for i in range(len(Q)): self.assertTrue(b.get_Q()[i] - Q[i] < EPS_A) self.assertTrue(b.state_vector[6 + i] - Q[i] < EPS_A)
def test_constant_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() vxyz = random_vector() b.set_vxyz(vxyz) 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_xyz(),vxyz)/vlength(vxyz) < EPS_A)