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_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_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_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_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_commandQueue(self):
"""
Add-, query, and remove commands from the command queue.
"""
# Convenience.
body_1 = getRigidBody()
body_2 = {'imass': 2, 'scale': 3}
id_1, id_2 = '0', '1'
# The command queue must be empty for every category.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert ret.data['spawn'] == []
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# Spawn two objects with id_1 and id_2.
tmp = [(id_1, body_1), (id_2, body_1)]
assert leoAPI.addCmdSpawn(tmp).ok
# Verify that the spawn commands were added.
ret = leoAPI.dequeueCommands()
assert ret.ok
spawn = ret.data['spawn']
assert {spawn[0]['objID'], spawn[1]['objID']} == {id_1, id_2}
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# De-queuing the commands once more must not return any results because
# they have already been removed.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert ret.data['spawn'] == []
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# Modify state variable for body with id_1.
newSV = {'imass': 10, 'position': [3, 4, 5]}
assert leoAPI.addCmdModifyBodyState(id_1, newSV).ok
ret = leoAPI.dequeueCommands()
modify = ret.data['modify']
assert ret.ok and len(modify) == 1
assert modify[0]['objID'] == id_1
assert modify[0]['rbs'] == newSV
del newSV
# Set the direct force and torque for id_2.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdDirectForce(id_2, force, torque).ok
ret = leoAPI.dequeueCommands()
fat = ret.data['direct_force']
assert ret.ok
assert len(fat) == 1
assert fat[0]['objID'] == id_2
assert fat[0]['force'] == force
assert fat[0]['torque'] == torque
# Set the booster force and torque for id_1.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdBoosterForce(id_1, force, torque).ok
ret = leoAPI.dequeueCommands()
fat = ret.data['booster_force']
assert ret.ok
assert len(fat) == 1
assert fat[0]['objID'] == id_1
assert fat[0]['force'] == force
assert fat[0]['torque'] == torque
# Remove an object.
assert leoAPI.addCmdRemoveObject(id_1).ok
ret = leoAPI.dequeueCommands()
assert ret.ok and ret.data['remove'][0]['objID'] == id_1
# Add commands for two objects (it is perfectly ok to add commands for
# non-existing body IDs since this is just a command queue - Leonard
# will skip commands for non-existing IDs automatically).
force, torque = [7, 8, 9], [10, 11.5, 12.5]
for objID in (id_1, id_2):
assert leoAPI.addCmdSpawn([(objID, body_1)]).ok
assert leoAPI.addCmdModifyBodyState(objID, body_2).ok
assert leoAPI.addCmdRemoveObject(objID).ok
assert leoAPI.addCmdDirectForce(objID, force, torque).ok
assert leoAPI.addCmdBoosterForce(objID, force, torque).ok
# De-queue all commands.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert len(ret.data['spawn']) == 2
assert len(ret.data['remove']) == 2
assert len(ret.data['modify']) == 2
assert len(ret.data['direct_force']) == 2
assert len(ret.data['booster_force']) == 2
def test_commandQueue(self):
"""
Add-, query, and remove commands from the command queue.
"""
# Convenience.
body_1 = getRigidBody()
body_2 = {'imass': 2, 'scale': 3}
id_1, id_2 = 0, 1
# The command queue must be empty for every category.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert ret.data['spawn'] == []
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# Spawn two objects with id_1 and id_2.
tmp = [(id_1, body_1), (id_2, body_1)]
assert leoAPI.addCmdSpawn(tmp).ok
# Verify that the spawn commands were added.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert ret.data['spawn'][0]['objID'] == id_1
assert ret.data['spawn'][1]['objID'] == id_2
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# De-queuing the commands once more must not return any results because
# they have already been removed.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert ret.data['spawn'] == []
assert ret.data['remove'] == []
assert ret.data['modify'] == []
assert ret.data['direct_force'] == []
assert ret.data['booster_force'] == []
# Modify state variable for body with id_1.
newSV = {'imass': 10, 'position': [3, 4, 5]}
assert leoAPI.addCmdModifyBodyState(id_1, newSV).ok
ret = leoAPI.dequeueCommands()
modify = ret.data['modify']
assert ret.ok and len(modify) == 1
assert modify[0]['objID'] == id_1
assert modify[0]['rbs'] == newSV
del newSV
# Set the direct force and torque for id_2.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdDirectForce(id_2, force, torque).ok
ret = leoAPI.dequeueCommands()
fat = ret.data['direct_force']
assert ret.ok
assert len(fat) == 1
assert fat[0]['objID'] == id_2
assert fat[0]['force'] == force
assert fat[0]['torque'] == torque
# Set the booster force and torque for id_1.
force, torque = [1, 2, 3], [4, 5, 6]
assert leoAPI.addCmdBoosterForce(id_1, force, torque).ok
ret = leoAPI.dequeueCommands()
fat = ret.data['booster_force']
assert ret.ok
assert len(fat) == 1
assert fat[0]['objID'] == id_1
assert fat[0]['force'] == force
assert fat[0]['torque'] == torque
# Remove an object.
assert leoAPI.addCmdRemoveObject(id_1).ok
ret = leoAPI.dequeueCommands()
assert ret.ok and ret.data['remove'][0]['objID'] == id_1
# Add commands for two objects (it is perfectly ok to add commands for
# non-existing body IDs since this is just a command queue - Leonard
# will skip commands for non-existing IDs automatically).
force, torque = [7, 8, 9], [10, 11.5, 12.5]
for objID in (id_1, id_2):
assert leoAPI.addCmdSpawn([(objID, body_1)]).ok
assert leoAPI.addCmdModifyBodyState(objID, body_2).ok
assert leoAPI.addCmdRemoveObject(objID).ok
assert leoAPI.addCmdDirectForce(objID, force, torque).ok
assert leoAPI.addCmdBoosterForce(objID, force, torque).ok
# De-queue all commands.
ret = leoAPI.dequeueCommands()
assert ret.ok
assert len(ret.data['spawn']) == 2
assert len(ret.data['remove']) == 2
assert len(ret.data['modify']) == 2
assert len(ret.data['direct_force']) == 2
assert len(ret.data['booster_force']) == 2
