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_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