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)
