def test_add_get_multiple(self):
"""
Add multiple objects to the DB.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two object IDs for this test.
id_1, id_2 = '1', '2'
# The number of bodies in Leonard must be zero.
assert len(leo.allBodies) == 0
# Create an object and serialise it.
body_1 = getRigidBody(position=[0, 0, 0])
body_2 = getRigidBody(position=[10, 10, 10])
# Add the bodies to Leonard.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp)
leo.processCommandsAndSync()
# Verify the bodies.
assert leo.allBodies[id_1] == body_1
assert leo.allBodies[id_2] == body_2
def test_setRigidBody_advanced(self, clsLeonard):
"""
Similar to test_setRigidBody_basic but modify the collision shape
information as well, namely their mass- and type.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Parameters and constants for this test.
cshape_box = {'1': getCSBox()}
cshape_sphere = {'1': getCSSphere()}
body = getRigidBody(imass=2, scale=3, cshapes=cshape_sphere)
# Spawn an object.
objID = 1
assert leoAPI.addCmdSpawn([(objID, body)]).ok
del body
# Verify the body data.
leo.processCommandsAndSync()
assert leo.allBodies[objID].imass == 2
assert leo.allBodies[objID].scale == 3
assert leo.allBodies[objID].cshapes == cshape_sphere
# Update the body.
cs_new = {'imass': 4, 'scale': 5, 'cshapes': cshape_box}
assert leoAPI.addCmdModifyBodyState(objID, cs_new).ok
# Verify the body data.
leo.processCommandsAndSync()
ret = leo.allBodies[objID]
assert (ret.imass == 4) and (ret.scale == 5)
assert ret.cshapes == cshape_box
def test_move_single_object(self, clsLeonard):
"""
Create a single object with non-zero initial speed and ensure
Leonard moves it accordingly.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Constants and parameters for this test.
id_0 = 0
# Spawn an object.
assert leoAPI.addCmdSpawn([(id_0, getRigidBody())]).ok
# Advance the simulation by 1s and verify that nothing has moved.
leo.step(1.0, 60)
assert np.array_equal(leo.allBodies[id_0].position, [0, 0, 0])
# Give the object a velocity.
body = {'velocityLin': np.array([1, 0, 0])}
assert leoAPI.addCmdModifyBodyState(id_0, body).ok
del body
# Advance the simulation by another second and verify the objects have
# moved accordingly.
leo.step(1.0, 60)
body = leo.allBodies[id_0]
assert 0.9 <= body.position[0] < 1.1
assert body.position[1] == body.position[2] == 0
def test_add_get_remove_single(self):
"""
Add an object to the SV database.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create an object ID for the test.
id_1 = '1'
# The number of SV entries must now be zero.
assert len(leo.allBodies) == 0
# Query an object. Since none exists yet this must fail.
assert id_1 not in leo.allBodies
# Create an object and serialise it.
body = getRigidBody(cshapes={'cssphere': getCSSphere()})
# Add the object to Leonard and verify it worked.
assert leoAPI.addCmdSpawn([(id_1, body)])
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body
# Remove object id_1.
assert leoAPI.addCmdRemoveObject(id_1).ok
leo.processCommandsAndSync()
# Object must not exist anymore in the simulation.
assert id_1 not in leo.allBodies
assert len(leo.allBodies) == 0
def test_updateLocalCache(self):
"""
Update the local object cache in Leonard based on a Work Package.
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Convenience.
body_1 = getRigidBody(imass=1)
body_2 = getRigidBody(imass=2)
id_1, id_2 = 1, 2
# Spawn new objects.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Create a new State Vector to replace the old one.
body_3 = getRigidBody(imass=4, position=[1, 2, 3])
newWP = [(id_1, body_3)]
# Check the State Vector for objID=id_1 before and after the update.
assert getRigidBody(*leo.allBodies[id_1]) == body_1
leo.updateLocalCache(newWP)
assert getRigidBody(*leo.allBodies[id_1]) == body_3
def test_move_two_objects_no_collision(self, clsLeonard):
"""
Same as previous test but with two objects.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Constants and parameters for this test.
id_0, id_1 = 0, 1
body_0 = getRigidBody(position=[0, 0, 0], velocityLin=[1, 0, 0])
body_1 = getRigidBody(position=[0, 10, 0], velocityLin=[0, -1, 0])
# Create two objects.
tmp = [(id_0, body_0), (id_1, body_1)]
assert leoAPI.addCmdSpawn(tmp).ok
# Advance the simulation by 1s.
leo.step(1.0, 60)
# The objects must have moved according to their initial velocity.
pos_0 = leo.allBodies[id_0].position
pos_1 = leo.allBodies[id_1].position
assert pos_0[1] == pos_0[2] == 0
assert pos_1[0] == pos_1[2] == 0
assert 0.9 <= pos_0[0] <= 1.1
assert 8.9 <= pos_1[1] <= 9.1
def test_add_get_multiple(self):
"""
Add multiple objects to the DB.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two object IDs for this test.
id_1, id_2 = 1, 2
# The number of bodies in Leonard must be zero.
assert len(leo.allBodies) == 0
# Create an object and serialise it.
body_1 = getRigidBody(position=[0, 0, 0])
body_2 = getRigidBody(position=[10, 10, 10])
# Add the bodies to Leonard.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp)
leo.processCommandsAndSync()
# Verify the bodies.
assert leo.allBodies[id_1] == body_1
assert leo.allBodies[id_2] == body_2
def test_add_get_remove_single(self):
"""
Add an object to the SV database.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create an object ID for the test.
id_1 = 1
# The number of SV entries must now be zero.
assert len(leo.allBodies) == 0
# Query an object. Since none exists yet this must fail.
assert id_1 not in leo.allBodies
# Create an object and serialise it.
body = getRigidBody(cshapes={'cssphere': getCSSphere()})
# Add the object to Leonard and verify it worked.
assert leoAPI.addCmdSpawn([(id_1, body)])
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body
# Remove object id_1.
assert leoAPI.addCmdRemoveObject(id_1).ok
leo.processCommandsAndSync()
# Object must not exist anymore in the simulation.
assert id_1 not in leo.allBodies
assert len(leo.allBodies) == 0
def test_totalForceAndTorque_with_rotation(self):
"""
Similar to the previou 'test_totalForceAndTorque_no_rotation'
but this time the object does not have a neutral rotation in
world coordinates. This must have no effect on the direct force
values, but the booster forces must be re-oriented accordingly.
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Spawn one object rotated 180 degress around x-axis.
sv = getRigidBody(imass=1, rotation=(1, 0, 0, 0))
objID = 1
assert leoAPI.addCmdSpawn([(objID, sv)]).ok
leo.processCommandsAndSync()
del sv
# Initial force and torque must be zero.
assert leo.totalForceAndTorque(objID) == ([0, 0, 0], [0, 0, 0])
# Add booster force in z-direction.
assert leoAPI.addCmdBoosterForce(objID, [1, 2, 3], [-1, -2, -3]).ok
leo.processCommandsAndSync()
# The net forces in must have their signs flipped in the y/z
# directions, and remain unchanged for x since the object itself is
# rotated 180 degrees around the x-axis.
assert leo.totalForceAndTorque(objID) == ([1, -2, -3], [-1, 2, 3])
# The object's rotation must not effect the direct force and torque.
assert leoAPI.addCmdBoosterForce(objID, [0, 0, 0], [0, 0, 0]).ok
assert leoAPI.addCmdDirectForce(objID, [1, 2, 3], [4, 5, 6]).ok
leo.processCommandsAndSync()
assert leo.totalForceAndTorque(objID) == ([1, 2, 3], [4, 5, 6])
def test_setRigidBody(self):
"""
Set and retrieve object attributes like position, velocity,
acceleration, and rotation.
"""
# Instantiate a Leonard.
leo = getLeonard()
# Test constants.
body_new = {
'imass': 2,
'scale': 3,
'cshapes': {'csempty': getCSEmpty()},
'position': (1, 2, 5),
'velocityLin': (8, 9, 10.5),
'velocityRot': (9, 10, 11.5),
'rotation': (11, 12.5, 13, 13.5)
}
# Create a test body.
id_1 = 0
body = getRigidBody(cshapes={'csempty': getCSEmpty()})
# Add the object to the DB with ID=0.
assert leoAPI.addCmdSpawn([(id_1, body)]).ok
leo.processCommandsAndSync()
# Modify the state vector for body with id_1.
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
# Query the body again and verify the changes are in effect.
ret = leo.allBodies[id_1]
assert ret.imass == body_new['imass']
assert ret.scale == body_new['scale']
assert np.array_equal(ret.position, body_new['position'])
assert np.array_equal(ret.velocityLin, body_new['velocityLin'])
assert np.array_equal(ret.velocityRot, body_new['velocityRot'])
assert np.array_equal(ret.rotation, body_new['rotation'])
# Query the AABB, update the collision shapes, and verify that the new
# AABBs are in effect.
assert leo.allAABBs[id_1] == {}
# Modify the body state by adding a collision shape.
body_new = {'cshapes': {'cssphere': getCSSphere(radius=1)}}
assert body_new is not None
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
assert leo.allAABBs[id_1] == {'cssphere': [0, 0, 0, 1, 1, 1]}
# Modify the body state by adding a collision shape.
cs_a = getCSSphere(radius=1, pos=(1, 2, 3))
cs_b = getCSSphere(radius=2, pos=(4, 5, 6))
cshapes = {'1': cs_a, '2': getCSEmpty(), '3': cs_b}
body_new = {'cshapes': cshapes}
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
correct = {'1': [1, 2, 3, 1, 1, 1], '3': [4, 5, 6, 2, 2, 2]}
assert leo.allAABBs[id_1] == correct
def test_setRigidBody_basic(self, clsLeonard):
"""
Spawn an object, specify its State Variables explicitly, and verify the
change propagated through Azrael.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Parameters and constants for this test.
id_1 = 1
# Body data.
p = np.array([1, 2, 5])
vl = np.array([8, 9, 10.5])
vr = vl + 1
body = {'position': p, 'velocityLin': vl, 'velocityRot': vr}
del p, vl, vr
# Spawn a new object. It must have ID=1.
assert leoAPI.addCmdSpawn([(id_1, getRigidBody())]).ok
# Update the object's body.
assert leoAPI.addCmdModifyBodyState(id_1, body).ok
# Sync the commands to Leonard.
leo.processCommandsAndSync()
# Verify that the attributes were correctly updated.
ret = leo.allBodies[id_1]
assert np.array_equal(ret.position, body['position'])
assert np.array_equal(ret.velocityLin, body['velocityLin'])
assert np.array_equal(ret.velocityRot, body['velocityRot'])
def test_processCommandQueue(self):
"""
Create commands to spawn-, delete, and modify objects or their booster
values. Then verify that ``processCommandQueue`` corrently updates
Leonard's object cache.
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Convenience.
body_1 = getRigidBody(imass=1)
body_2 = getRigidBody(imass=2)
id_1, id_2 = 1, 2
# Cache must be empty.
assert len(leo.allBodies) == len(leo.allForces) == 0
# Spawn two objects.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Verify the local cache (forces and torques must default to zero).
assert getRigidBody(*leo.allBodies[id_1]) == body_1
assert getRigidBody(*leo.allBodies[id_2]) == body_2
tmp = leo.allForces[id_1]
assert tmp.forceDirect == tmp.torqueDirect == [0, 0, 0]
assert tmp.forceBoost == tmp.torqueBoost == [0, 0, 0]
del tmp
# Remove first object.
assert leoAPI.addCmdRemoveObject(id_1).ok
leo.processCommandsAndSync()
assert id_1 not in leo.allBodies
assert id_1 not in leo.allForces
# Change the State Vector of id_2.
pos = (10, 11.5, 12)
body_3 = {'position': pos}
assert leo.allBodies[id_2].position == (0, 0, 0)
assert leoAPI.addCmdModifyBodyState(id_2, body_3).ok
leo.processCommandsAndSync()
assert leo.allBodies[id_2].position == pos
# Apply a direct force and torque to id_2.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdDirectForce(id_2, force, torque).ok
leo.processCommandsAndSync()
assert leo.allForces[id_2].forceDirect == force
assert leo.allForces[id_2].torqueDirect == torque
# Specify a new force- and torque value due to booster activity.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdBoosterForce(id_2, force, torque).ok
leo.processCommandsAndSync()
assert leo.allForces[id_2].forceBoost == force
assert leo.allForces[id_2].torqueBoost == torque
def test_getGridForces(self, clsLeonard):
"""
Spawn an object, specify its State Variables explicitly, and verify the
change propagated through Azrael.
"""
# Convenience.
vg = azrael.vectorgrid
# Create pristince force grid.
assert vg.defineGrid(name='force', vecDim=3, granularity=1).ok
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Grid parameters.
Nx, Ny, Nz = 2, 3, 4
ofs = np.array([-1.1, -2.7, 3.5], np.float64)
force = np.zeros((Nx, Ny, Nz, 3))
# Compute grid values.
idPos, idVal = {}, {}
val, objID = 0, 0
for x in range(Nx):
for y in range(Ny):
for z in range(Nz):
# Assign integer values (allows for equality comparisions
# later on without having to worry about rounding effects).
force[x, y, z] = [val, val + 1, val + 2]
# Build the input dictionary for ``getGridForces``.
idPos[objID] = np.array(
[x + ofs[0], y + ofs[1], z + ofs[2]])
# Keep track of the value assigned to this position.
idVal[objID] = force[x, y, z]
# Update the counters.
val += 3
objID += 1
# Set the grid values with a region operator.
ret = vg.setRegion('force', ofs, force)
# Query the grid values at the positions specified in idPos.
ret = leo.getGridForces(idPos)
assert ret.ok
gridForces = ret.data
# Verify the value at every position we used in this test.
for objID in idPos:
# Convenience.
val_direct = idVal[objID]
val_gridforce = gridForces[objID]
# Compare: direct <--> getGridForces.
assert np.array_equal(val_direct, val_gridforce)
def test_constraint_p2p(self, clsLeonard):
"""
Link two bodies together with a Point2Point constraint and verify that
they move together.
"""
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Convenience.
id_a, id_b = 1, 2
pos_a, pos_b = (-2, 0, 0), (2, 0, 0)
distance = abs(pos_a[0] - pos_b[0])
assert distance >= 4
body_a = getRigidBody(position=pos_a, cshapes={'cssphere': getCSSphere()})
body_b = getRigidBody(position=pos_b, cshapes={'cssphere': getCSSphere()})
# Specify the constraints.
con = getP2P(rb_a=id_a, rb_b=id_b, pivot_a=pos_b, pivot_b=pos_a)
self.igor.addConstraints([con])
# Spawn both objects.
assert leoAPI.addCmdSpawn([(id_a, body_a), (id_b, body_b)]).ok
leo.processCommandsAndSync()
# Apply a force to the left sphere only.
assert leoAPI.addCmdDirectForce(id_a, [-10, 0, 0], [0, 0, 0]).ok
leo.processCommandsAndSync()
# Both object must have moved the same distance 'delta' because they
# are linked. Their distance must not have changed.
leo.step(1.0, 60)
allObjs = leo.allBodies
delta_a = allObjs[id_a].position - np.array(pos_a)
delta_b = allObjs[id_b].position - np.array(pos_b)
assert delta_a[0] < pos_a[0]
assert np.allclose(delta_a, delta_b)
tmp = abs(allObjs[id_a].position[0] - allObjs[id_b].position[0])
assert abs(tmp - distance) < 0.01
del tmp
# Unlink the objects again, apply a right-pointing force to the
# right object and verify that the left continues to move left and the
# right does not.
assert self.igor.deleteConstraints([con]) == (True, None, 1)
assert leoAPI.addCmdDirectForce(id_b, [10, 0, 0], [0, 0, 0]).ok
leo.processCommandsAndSync()
leo.step(1.0, 60)
# The distance between the spheres must have increases since they are
# not linked anymore.
tmp = abs(allObjs[id_a].position[0] - allObjs[id_b].position[0])
assert tmp > (distance + 1)
def test_add_same(self):
"""
Try to add two objects with the same ID.
"""
# Instantiate a Leonard.
leo = getLeonard()
# Convenience.
id_1 = 1
# The number of bodies in Leonard must be zero.
assert len(leo.allBodies) == 0
# Create three bodies.
body_1 = getRigidBody(imass=1)
body_2 = getRigidBody(imass=2)
body_3 = getRigidBody(imass=3)
# The command queue for spawning objects must be empty.
ret = leoAPI.dequeueCommands()
assert ret.ok and (ret.data['spawn'] == [])
# Spawn the first object, then attempt to spawn another with the same
# objID *before* Leonard gets around to add even the first one --> this
# must fail and not add anything.
assert leoAPI.addCmdSpawn([(id_1, body_1)]).ok
assert not leoAPI.addCmdSpawn([(id_1, body_2)]).ok
ret = leoAPI.dequeueCommands()
spawn = ret.data['spawn']
assert ret.ok and (len(spawn) == 1) and (spawn[0]['objID'] == id_1)
# Similar test as before, but this time Leonard has already pulled id_1
# into the simulation *before* we (attempt to) spawn another object
# with the same ID. The 'addSpawnCmd' must succeed because it cannot
# reliably verify if Leonard has an object id_1 (it can only verify if
# another such request is in the queue already -- see above). However,
# Leonard itself must ignore that request. To verify this claim we will
# now spawn a new object with the same id_1 but a different state data,
# let Leonard process the queue, and then verify that it did not
# add/modify the object with id_1.
assert leoAPI.addCmdSpawn([(id_1, body_1)]).ok
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body_1
# Spawn anoter object with id_1 but different state data and verify
# that Leonard did not modify the original body.
assert leoAPI.addCmdSpawn([(id_1, body_3)]).ok
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body_1
def test_add_same(self):
"""
Try to add two objects with the same ID.
"""
# Instantiate a Leonard.
leo = getLeonard()
# Convenience.
id_1 = '1'
# The number of bodies in Leonard must be zero.
assert len(leo.allBodies) == 0
# Create three bodies.
body_1 = getRigidBody(imass=1)
body_2 = getRigidBody(imass=2)
body_3 = getRigidBody(imass=3)
# The command queue for spawning objects must be empty.
ret = leoAPI.dequeueCommands()
assert ret.ok and (ret.data['spawn'] == [])
# Spawn the first object, then attempt to spawn another with the same
# objID *before* Leonard gets around to add even the first one --> this
# must fail and not add anything.
assert leoAPI.addCmdSpawn([(id_1, body_1)]).ok
assert not leoAPI.addCmdSpawn([(id_1, body_2)]).ok
ret = leoAPI.dequeueCommands()
spawn = ret.data['spawn']
assert ret.ok and (len(spawn) == 1) and (spawn[0]['objID'] == id_1)
# Similar test as before, but this time Leonard has already pulled id_1
# into the simulation *before* we (attempt to) spawn another object
# with the same ID. The 'addSpawnCmd' must succeed because it cannot
# reliably verify if Leonard has an object id_1 (it can only verify if
# another such request is in the queue already -- see above). However,
# Leonard itself must ignore that request. To verify this claim we will
# now spawn a new object with the same id_1 but a different state data,
# let Leonard process the queue, and then verify that it did not
# add/modify the object with id_1.
assert leoAPI.addCmdSpawn([(id_1, body_1)]).ok
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body_1
# Spawn anoter object with id_1 but different state data and verify
# that Leonard did not modify the original body.
assert leoAPI.addCmdSpawn([(id_1, body_3)]).ok
leo.processCommandsAndSync()
assert leo.allBodies[id_1] == body_1
def test_createWorkPackages(self):
"""
Create a Work Package and verify its content.
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Constants.
id_1, id_2 = 1, 2
dt, maxsteps = 2, 3
# Invalid call: list of IDs must not be empty.
assert not leo.createWorkPackage([], dt, maxsteps).ok
# Invalid call: Leonard has no object with ID 10.
assert not leo.createWorkPackage([10], dt, maxsteps).ok
# Test data.
body_1 = getRigidBody(imass=1)
body_2 = getRigidBody(imass=2)
# Add two new objects to Leonard.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Create a Work Package with two objects. The WPID must be 1.
ret = leo.createWorkPackage([id_1], dt, maxsteps)
ret_wpid, ret_wpdata = ret.data['wpid'], ret.data['wpdata']
assert (ret.ok, ret_wpid, len(ret_wpdata)) == (True, 0, 1)
# Create a second WP: it must have WPID=2 and contain two objects.
ret = leo.createWorkPackage([id_1, id_2], dt, maxsteps)
ret_wpid, ret_wpdata = ret.data['wpid'], ret.data['wpdata']
assert (ret.ok, ret_wpid, len(ret_wpdata)) == (True, 1, 2)
# Check the WP content.
WPData = azrael.leonard.WPData
WPMeta = azrael.leonard.WPMeta
data = [WPData(*_) for _ in ret.data['wpdata']]
meta = WPMeta(*ret.data['wpmeta'])
assert (meta.dt, meta.maxsteps) == (dt, maxsteps)
assert (ret.ok, len(data)) == (True, 2)
assert (data[0].aid, data[1].aid) == (id_1, id_2)
assert getRigidBody(*data[0].sv) == body_1
assert getRigidBody(*data[1].sv) == body_2
assert np.array_equal(data[0].force, [0, 0, 0])
assert np.array_equal(data[1].force, [0, 0, 0])
def test_force_grid(self, clsLeonard):
"""
Create a force grid and ensure Leonard applies its values to the
center of the mass.
"""
# Convenience.
vg = azrael.vectorgrid
# Get a Leonard instance.
leo = getLeonard(clsLeonard)
# Constants and parameters for this test.
id_0 = 0
# Spawn one object.
assert leoAPI.addCmdSpawn([(id_0, getRigidBody())]).ok
# Advance the simulation by 1s and verify that nothing has moved.
leo.step(1.0, 60)
assert np.array_equal(leo.allBodies[id_0].position, [0, 0, 0])
# Define a force grid.
assert vg.defineGrid(name='force', vecDim=3, granularity=1).ok
# Specify a non-zero value somewhere away from the object. This means
# the object must still not move.
pos = np.array([1, 2, 3], np.float64)
value = np.ones(3, np.float64)
assert vg.setValues('force', [(pos, value)]).ok
# Step the simulation and verify the object remained where it was.
leo.step(1.0, 60)
assert np.array_equal(leo.allBodies[id_0].position, [0, 0, 0])
# Specify a grid value of 1 Newton in x-direction.
pos = np.array([0, 0, 0], np.float64)
value = np.array([1, 0, 0], np.float64)
assert vg.setValues('force', [(pos, value)]).ok
# Step the simulation and verify the object moved accordingly.
leo.step(1.0, 60)
body = leo.allBodies[id_0]
assert 0.4 <= body.position[0] < 0.6
assert body.position[1] == body.position[2] == 0
def test_get_set_forceandtorque(self):
"""
Query and update the force- and torque vectors for an object.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two object IDs for this test.
id_1, id_2 = 0, 1
# Create two objects and serialise them.
body_1 = getRigidBody(position=[0, 0, 0])
body_2 = getRigidBody(position=[10, 10, 10])
# Add the two objects to the simulation.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Update the direct force and torque of the second object only.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdDirectForce(id_2, force, torque)
leo.processCommandsAndSync()
# Only the force an torque of the second object must have changed.
assert np.array_equal(leo.allForces[id_1].forceDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_1].torqueDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_2].forceDirect, force)
assert np.array_equal(leo.allForces[id_2].torqueDirect, torque)
# Update the booster force and torque of the first object only.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdBoosterForce(id_2, force, torque)
leo.processCommandsAndSync()
# Only the booster- force an torque of the second object must have
# changed.
assert np.array_equal(leo.allForces[id_1].forceDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_1].torqueDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_2].forceDirect, force)
assert np.array_equal(leo.allForces[id_2].torqueDirect, torque)
def test_get_set_forceandtorque(self):
"""
Query and update the force- and torque vectors for an object.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two object IDs for this test.
id_1, id_2 = '0', '1'
# Create two objects and serialise them.
body_1 = getRigidBody(position=[0, 0, 0])
body_2 = getRigidBody(position=[10, 10, 10])
# Add the two objects to the simulation.
tmp = [(id_1, body_1), (id_2, body_2)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Update the direct force and torque of the second object only.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdDirectForce(id_2, force, torque)
leo.processCommandsAndSync()
# Only the force an torque of the second object must have changed.
assert np.array_equal(leo.allForces[id_1].forceDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_1].torqueDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_2].forceDirect, force)
assert np.array_equal(leo.allForces[id_2].torqueDirect, torque)
# Update the booster force and torque of the first object only.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdBoosterForce(id_2, force, torque)
leo.processCommandsAndSync()
# Only the booster- force an torque of the second object must have
# changed.
assert np.array_equal(leo.allForces[id_1].forceDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_1].torqueDirect, [0, 0, 0])
assert np.array_equal(leo.allForces[id_2].forceDirect, force)
assert np.array_equal(leo.allForces[id_2].torqueDirect, torque)
def test_totalForceAndTorque_no_rotation(self):
"""
Verify that 'totalForceAndTorque' correctly adds up the direct-
and booster forces for an object that is in neutral position (ie
without rotation).
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Spawn one object.
sv = getRigidBody(imass=1, rotation=(0, 0, 0, 1))
objID = 1
assert leoAPI.addCmdSpawn([(objID, sv)]).ok
leo.processCommandsAndSync()
del sv
# Initial force and torque must be zero.
assert leo.totalForceAndTorque(objID) == ([0, 0, 0], [0, 0, 0])
# Change the direct force.
assert leoAPI.addCmdDirectForce(objID, [1, 2, 3], [4, 5, 6]).ok
leo.processCommandsAndSync()
assert leo.totalForceAndTorque(objID) == ([1, 2, 3], [4, 5, 6])
# Change the direct force.
assert leoAPI.addCmdDirectForce(objID, [1, 2, 30], [4, 5, 60]).ok
leo.processCommandsAndSync()
assert leo.totalForceAndTorque(objID) == ([1, 2, 30], [4, 5, 60])
# Reset the direct force and change the booster force.
assert leoAPI.addCmdDirectForce(objID, [0, 0, 0], [0, 0, 0]).ok
assert leoAPI.addCmdBoosterForce(objID, [-1, -2, -3], [-4, -5, -6]).ok
leo.processCommandsAndSync()
assert leo.totalForceAndTorque(objID) == ([-1, -2, -3], [-4, -5, -6])
# Direct- and booste forces must perfectly balance each other.
assert leoAPI.addCmdDirectForce(objID, [1, 2, 3], [4, 5, 6]).ok
assert leoAPI.addCmdBoosterForce(objID, [-1, -2, -3], [-4, -5, -6]).ok
leo.processCommandsAndSync()
assert leo.totalForceAndTorque(objID) == ([0, 0, 0], [0, 0, 0])
def test_set_get_AABB(self):
"""
Create a new object with an AABB and query it back again.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two IDs and body instances for this test.
id_1, id_2 = 0, 1
aabb_2 = {'cssphere': [0, 0, 0, 1, 1, 1]}
aabb_3 = {'cssphere': [0, 0, 0, 2, 2, 2]}
body_a = getRigidBody(cshapes={'cssphere': getCSSphere(radius=1)})
body_b = getRigidBody(cshapes={'cssphere': getCSSphere(radius=2)})
# Add two new objects to the DB.
tmp = [(id_1, body_a), (id_2, body_b)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Verify the two AABBs
assert leo.allAABBs[id_1] == aabb_2
assert leo.allAABBs[id_2] == aabb_3
def test_set_get_AABB(self):
"""
Create a new object with an AABB and query it back again.
"""
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Create two IDs and body instances for this test.
id_1, id_2 = '0', '1'
aabb_2 = {'cssphere': [0, 0, 0, 1, 1, 1]}
aabb_3 = {'cssphere': [0, 0, 0, 2, 2, 2]}
body_a = getRigidBody(cshapes={'cssphere': getCSSphere(radius=1)})
body_b = getRigidBody(cshapes={'cssphere': getCSSphere(radius=2)})
# Add two new objects to the DB.
tmp = [(id_1, body_a), (id_2, body_b)]
assert leoAPI.addCmdSpawn(tmp).ok
leo.processCommandsAndSync()
# Verify the two AABBs
assert leo.allAABBs[id_1] == aabb_2
assert leo.allAABBs[id_2] == aabb_3
def test_maintain_forces(self):
"""
Leonard must not reset any forces from one iteration to the next
(used to be the case at some point and thus requires a dedicated
test now).
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardDistributedZeroMQ)
# Convenience.
sv = getRigidBody(imass=1)
objID = 1
# Spawn object.
assert leoAPI.addCmdSpawn([(objID, sv)]).ok
leo.processCommandsAndSync()
# Initial force and torque must be zero.
tmp = leo.allForces[objID]
assert tmp.forceDirect == tmp.torqueDirect == [0, 0, 0]
assert tmp.forceBoost == tmp.torqueBoost == [0, 0, 0]
del tmp
# Change the direct force and verify that Leonard does not reset it.
assert leoAPI.addCmdDirectForce(objID, [1, 2, 3], [4, 5, 6]).ok
for ii in range(10):
leo.processCommandsAndSync()
tmp = leo.allForces[objID]
assert tmp.forceDirect == [1, 2, 3]
assert tmp.torqueDirect == [4, 5, 6]
assert tmp.forceBoost == [0, 0, 0]
assert tmp.torqueBoost == [0, 0, 0]
# Change the booster force and verify that Leonard does not change
# it (or the direct force specified earlier)
assert leoAPI.addCmdBoosterForce(objID, [-1, -2, -3], [-4, -5, -6]).ok
for ii in range(10):
leo.processCommandsAndSync()
tmp = leo.allForces[objID]
assert tmp.forceDirect == [1, 2, 3]
assert tmp.torqueDirect == [4, 5, 6]
assert tmp.forceBoost == [-1, -2, -3]
assert tmp.torqueBoost == [-4, -5, -6]
# Change the direct forces again.
assert leoAPI.addCmdDirectForce(objID, [3, 2, 1], [6, 5, 4]).ok
for ii in range(10):
leo.processCommandsAndSync()
tmp = leo.allForces[objID]
assert tmp.forceDirect == [3, 2, 1]
assert tmp.torqueDirect == [6, 5, 4]
assert tmp.forceBoost == [-1, -2, -3]
assert tmp.torqueBoost == [-4, -5, -6]
# Change the booster forces again.
assert leoAPI.addCmdBoosterForce(objID, [-3, -2, -1], [-6, -5, -4]).ok
for ii in range(10):
leo.processCommandsAndSync()
tmp = leo.allForces[objID]
assert tmp.forceDirect == [3, 2, 1]
assert tmp.torqueDirect == [6, 5, 4]
assert tmp.forceBoost == [-3, -2, -1]
assert tmp.torqueBoost == [-6, -5, -4]
def test_setRigidBody(self):
"""
Set and retrieve object attributes like position, velocity,
acceleration, and rotation.
"""
# Instantiate a Leonard.
leo = getLeonard()
# Test constants.
body_new = {
'imass': 2,
'scale': 3,
'cshapes': {
'csempty': getCSEmpty()
},
'position': (1, 2, 5),
'velocityLin': (8, 9, 10.5),
'velocityRot': (9, 10, 11.5),
'rotation': (11, 12.5, 13, 13.5)
}
# Create a test body.
id_1 = '0'
body = getRigidBody(cshapes={'csempty': getCSEmpty()})
# Add the object to the DB with ID=0.
assert leoAPI.addCmdSpawn([(id_1, body)]).ok
leo.processCommandsAndSync()
# Modify the state vector for body with id_1.
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
# Query the body again and verify the changes are in effect.
ret = leo.allBodies[id_1]
assert ret.imass == body_new['imass']
assert ret.scale == body_new['scale']
assert np.array_equal(ret.position, body_new['position'])
assert np.array_equal(ret.velocityLin, body_new['velocityLin'])
assert np.array_equal(ret.velocityRot, body_new['velocityRot'])
assert np.array_equal(ret.rotation, body_new['rotation'])
# Query the AABB, update the collision shapes, and verify that the new
# AABBs are in effect.
assert leo.allAABBs[id_1] == {}
# Modify the body state by adding a collision shape.
body_new = {'cshapes': {'cssphere': getCSSphere(radius=1)}}
assert body_new is not None
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
assert leo.allAABBs[id_1] == {'cssphere': [0, 0, 0, 1, 1, 1]}
# Modify the body state by adding a collision shape.
cs_a = getCSSphere(radius=1, pos=(1, 2, 3))
cs_b = getCSSphere(radius=2, pos=(4, 5, 6))
cshapes = {'1': cs_a, '2': getCSEmpty(), '3': cs_b}
body_new = {'cshapes': cshapes}
assert leoAPI.addCmdModifyBodyState(id_1, body_new).ok
leo.processCommandsAndSync()
correct = {'1': [1, 2, 3, 1, 1, 1], '3': [4, 5, 6, 2, 2, 2]}
assert leo.allAABBs[id_1] == correct
def test_controlParts(self, client_type):
"""
Create a template with boosters and factories. Then send control
commands to them and ensure the applied forces, torques, and
spawned objects are correct.
In this test the parent object moves and is oriented away from its
default.
"""
# Get the client for this test.
client = self.clients[client_type]
# Reset the SV database and instantiate a Leonard.
leo = getLeonard()
# Parameters and constants for this test.
objID_1 = 1
pos_parent = [1, 2, 3]
vel_parent = [4, 5, 6]
# Part positions relative to parent.
dir_0 = [0, 0, +2]
dir_1 = [0, 0, -1]
pos_0 = [0, 0, +3]
pos_1 = [0, 0, -4]
# Describes a rotation of 180 degrees around x-axis.
orient_parent = [1, 0, 0, 0]
# Part position in world coordinates if the parent is rotated by 180
# degrees around the x-axis. The normalisation of the direction is
# necessary because the parts will automatically normalise all
# direction vectors, including dir_0 and dir_1 which are not unit
# vectors.
dir_0_out = -np.array(dir_0) / np.sum(abs(np.array(dir_0)))
dir_1_out = -np.array(dir_1) / np.sum(abs(np.array(dir_1)))
pos_0_out = -np.array(pos_0)
pos_1_out = -np.array(pos_1)
# ---------------------------------------------------------------------
# Create a template with two factories and spawn it.
# ---------------------------------------------------------------------
# Define the parts.
boosters = {
'0': types.Booster(pos=pos_0, direction=dir_0,
minval=0, maxval=0.5, force=0),
'1': types.Booster(pos=pos_1, direction=dir_1,
minval=0, maxval=1.0, force=0)
}
factories = {
'0': types.Factory(pos=pos_0, direction=dir_0,
templateID='_templateBox',
exit_speed=[0.1, 0.5]),
'1': types.Factory(pos=pos_1, direction=dir_1,
templateID='_templateSphere',
exit_speed=[1, 5])
}
# Define the template, add it to Azrael, and spawn an instance.
temp = getTemplate('t1',
rbs=getRigidBody(),
boosters=boosters,
factories=factories)
assert client.addTemplates([temp]).ok
new_obj = {'templateID': temp.aid,
'rbs': {
'position': pos_parent,
'velocityLin': vel_parent,
'rotation': orient_parent}}
ret = client.spawn([new_obj])
assert ret.ok and (ret.data == [objID_1])
del boosters, factories, temp, new_obj
# ---------------------------------------------------------------------
# Activate booster and factories and verify that the applied force and
# torque is correct, as well as that the spawned objects have the
# correct state variables attached to them.
# ---------------------------------------------------------------------
# Create the commands to let each factory spawn an object.
exit_speed_0, exit_speed_1 = 0.2, 2
forcemag_0, forcemag_1 = 0.2, 0.4
cmd_b = {
'0': types.CmdBooster(force=forcemag_0),
'1': types.CmdBooster(force=forcemag_1),
}
cmd_f = {
'0': types.CmdFactory(exit_speed=exit_speed_0),
'1': types.CmdFactory(exit_speed=exit_speed_1),
}
# Send the commands and ascertain that the returned object IDs now
# exist in the simulation. These IDs must be '2' and '3'.
ret = client.controlParts(objID_1, cmd_b, cmd_f)
id_2, id_3 = 2, 3
assert (ret.ok, ret.data) == (True, [id_2, id_3])
# Query the state variables of the objects spawned by the factories.
ok, _, ret_SVs = client.getRigidBodies([id_2, id_3])
assert (ok, len(ret_SVs)) == (True, 2)
# Determine which body was spawned by which factory based on their
# position. We do this by looking at their initial position which
# *must* match one of the parents.
body_2, body_3 = ret_SVs[id_2]['rbs'], ret_SVs[id_3]['rbs']
if np.allclose(body_2.position, pos_1_out + pos_parent):
body_2, body_3 = body_3, body_2
# Verify the position and velocity of the spawned objects is correct.
ac = np.allclose
assert ac(body_2.velocityLin, exit_speed_0 * dir_0_out + vel_parent)
assert ac(body_2.position, pos_0_out + pos_parent)
assert ac(body_3.velocityLin, exit_speed_1 * dir_1_out + vel_parent)
assert ac(body_3.position, pos_1_out + pos_parent)
# Let Leonard sync its data and then verify it received the correct
# total force and torque exerted by the boosters.
leo.processCommandsAndSync()
forcevec_0, forcevec_1 = forcemag_0 * dir_0_out, forcemag_1 * dir_1_out
tot_force = forcevec_0 + forcevec_1
tot_torque = (np.cross(pos_0_out, forcevec_0) +
np.cross(pos_1_out, forcevec_1))
# Query the torque and force from Azrael and verify they are correct.
leo_force, leo_torque = leo.totalForceAndTorque(objID_1)
assert np.array_equal(leo_force, tot_force)
assert np.array_equal(leo_torque, tot_torque)
def test_computeCollisionSetsAABB_viaLeonard(self, dim):
"""
Create a sequence of 10 test objects and sync them to Leonard. Their
positions only differ in the ``dim`` dimension.
Then use subsets of these 10 objects to test basic collision detection.
This uses the Azrael toolchain to create objects and sync them the
Leonard. This ensures the data propagates coorectly from the
interfaces, via Leonard, to the broadphase algorithm.
"""
# Get a Leonard instance.
leo = getLeonard(azrael.leonard.LeonardBase)
# Create the IDs for the test bodies.
num_bodies = 10
# Create several rigid bodies with a spherical collision shape.
cs = {'1': getCSSphere(radius=1)}
if dim == 0:
states = [getRigidBody(position=[_, 0, 0], cshapes=cs) for _ in range(10)]
elif dim == 1:
states = [getRigidBody(position=[0, _, 0], cshapes=cs) for _ in range(10)]
elif dim == 2:
states = [getRigidBody(position=[0, 0, _], cshapes=cs) for _ in range(10)]
else:
print('Invalid dimension for this test')
assert False
# Add all objects to the Body State DB and sync with Leonard.
for objID, bs in enumerate(states):
assert leoAPI.addCmdSpawn([(objID, bs)]).ok
del states
leo.processCommandsAndSync()
# Sanity check: the number of test IDs must match the number of objects
# in Leonard.
assert len(leo.allBodies) == num_bodies
def ccsWrapper(test_objIDs, expected_objIDs):
"""
Assert that ``test_objIDs`` form the ``expected_objIDs`` collision
sets.
This is a convenience wrapper to facilitate readable tests.
"""
# Compile the set of bodies- and their AABBs for this test run.
bodies = {_: leo.allBodies[_] for _ in test_objIDs}
AABBs = {_: leo.allAABBs[_] for _ in test_objIDs}
# Determine the list of broadphase collision sets.
ret = azrael.leonard.computeCollisionSetsAABB(bodies, AABBs)
assert ret.ok
# Convert the reference data to a sorted list of sets.
expected_objIDs = sorted([set(_) for _ in expected_objIDs])
computed_objIDs = sorted([set(_) for _ in ret.data])
# Return the equality of the two list of lists.
assert expected_objIDs == computed_objIDs
# Two non-overlapping objects.
ccsWrapper([0, 9], [[0], [9]])
# Two overlapping objects.
ccsWrapper([0, 1], [[0, 1]])
# Three sets.
ccsWrapper([0, 1, 5, 8, 9], [[0, 1], [5], [8, 9]])
# Same test, but objects are passed in a different sequence. This must
# not alter the test outcome.
ccsWrapper([0, 5, 1, 9, 8], [[0, 1], [5], [8, 9]])
# All objects must form one connected set.
ccsWrapper(list(range(10)), [list(range(10))])
def test_create_constraints_with_physics(self, client_type):
"""
Spawn two rigid bodies and define a Point2Point constraint among them.
Then apply a force onto one of them and verify the second one moves
accordingly.
"""
# Reset the constraint database.
igor = azrael.igor.Igor()
assert igor.reset().ok
# Get the client for this test.
client = self.clients[client_type]
# Reset the database and instantiate a Leonard.
leo = getLeonard(azrael.leonard.LeonardBullet)
# Spawn two bodies.
pos_a, pos_b = [-2, 0, 0], [2, 0, 0]
new_objs = [
{'templateID': '_templateSphere',
'rbs': {'position': pos_a}},
{'templateID': '_templateSphere',
'rbs': {'position': pos_b}},
]
id_1, id_2 = 1, 2
assert client.spawn(new_objs) == (True, None, [id_1, id_2])
# Verify the position of the bodies.
ret = client.getObjectStates([id_1, id_2])
assert ret.ok
assert ret.data[id_1]['rbs']['position'] == pos_a
assert ret.data[id_2]['rbs']['position'] == pos_b
# Define- and add the constraints.
con = [getP2P(rb_a=id_1, rb_b=id_2, pivot_a=pos_b, pivot_b=pos_a)]
assert client.addConstraints(con) == (True, None, 1)
# Apply a force that will pull the left object further to the left.
# However, both objects must move the same distance in the same
# direction because they are now linked together.
assert client.setForce(id_1, [-10, 0, 0]).ok
leo.processCommandsAndSync()
leo.step(1.0, 60)
# Query the object positions. Due to some database timings is sometimes
# happen that the objects appear to not have moved. In that case retry
# the query a few times before moving to the comparison.
for ii in range(10):
assert ii < 9
# Query the objects and put their positions into convenience
# variables.
ret = client.getRigidBodies([id_1, id_2])
pos_a2 = ret.data[id_1]['rbs'].position
pos_b2 = ret.data[id_2]['rbs'].position
# Exit this loop if both objects have moved.
if (pos_a != pos_a2) and (pos_b != pos_b2):
break
time.sleep(0.1)
# Verify that the objects have moved to the left and maintained their
# distance.
delta_a = np.array(pos_a2) - np.array(pos_a)
delta_b = np.array(pos_b2) - np.array(pos_b)
assert delta_a[0] < pos_a[0]
assert np.allclose(delta_a, delta_b)
