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_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_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_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_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)
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_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_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_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 __init__(self, realtime=False): IAtlasSimModel.__init__(self, realtime) self.sim_model = RigidBody() self.stepsize = 0.1 # s self.sim_model.start()
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)
