def test_setFragments_dae(self, client_type):
"""
Spawn a new object and modify its geometry at runtime.
"""
# Get the client for this test.
client = self.clients[client_type]
# Get a Collada fragment.
f_dae = {'f_dae': getFragDae()}
# Add a new template and spawn it.
temp = getTemplate('t1', fragments=f_dae)
assert client.addTemplates([temp]).ok
new_obj = {'templateID': temp.aid,
'rbs': {'position': (1, 1, 1), 'velocityLin': (-1, -1, -1)}}
ret = client.spawn([new_obj])
objID = ret.data[0]
assert ret.ok and ret.data == [objID]
del temp, new_obj, ret
# Query the body states to obtain the 'version' value.
ret = client.getRigidBodies(objID)
assert ret.ok
version = ret.data[objID]['rbs'].version
# Fetch-, modify-, update- and verify the geometry.
ret = client.getFragments([objID])
assert ret.ok
assert ret.data[objID]['f_dae']['fragtype'] == 'DAE'
# Change the geometry for fragment 'f_dae' to a RAW type.
assert client.setFragments({objID: {'f_dae': getFragRaw()._asdict()}}).ok
# Ensure the fragment is now indeed of type 'RAW'.
ret = client.getFragments([objID])
assert ret.ok
assert ret.data[objID]['f_dae']['fragtype'] == 'RAW'
# Ensure 'version' is different as well.
ret = client.getRigidBodies(objID)
assert ret.ok and (ret.data[objID]['rbs'].version != version)
# Change the fragment geometry once more.
version = ret.data[objID]['rbs'].version
assert client.setFragments({objID: {'f_dae': getFragDae()._asdict()}}).ok
# Ensure it now has type 'DAE' again.
ret = client.getFragments([objID])
assert ret.ok
assert ret.data[objID]['f_dae']['fragtype'] == 'DAE'
# Ensure 'version' is different as well.
ret = client.getRigidBodies(objID)
assert ret.ok and (ret.data[objID]['rbs'].version != version)
def test_spawn_and_get_state_variables(self, client_type):
"""
Spawn a new object and query its state variables.
"""
# Get the client for this test.
client = self.clients[client_type]
# Constants and parameters for this test.
templateID, objID_1 = '_templateEmpty', 1
# Query the state variables for a non existing object.
objID = 100
assert client.getRigidBodies(objID) == (True, None, {objID: None})
del objID
# Instruct Clerk to spawn a new object. Its objID must be '1'.
pos, vlin = (0, 1, 2), (-3, 4, -5)
body = getRigidBody(position=pos, velocityLin=vlin)
init = {
'templateID': templateID,
'rbs': {'position': body.position,
'velocityLin': body.velocityLin},
}
ret = client.spawn([init])
assert ret.ok and ret.data == [objID_1]
# The body parameters of the new object must match the inital state
# (plus the tweaks provided to the spawn command).
ret = client.getRigidBodies(objID_1)
assert ret.ok and (set(ret.data.keys()) == {1})
assert ret.data[objID_1]['rbs'].position == pos
assert ret.data[objID_1]['rbs'].velocityLin == vlin
# Same test but this time get all of them.
assert client.getRigidBodies(None) == ret
# Query just the state variables instead of the entire rigid body.
assert client.getObjectStates(None) == client.getObjectStates([objID_1])
ret = client.getObjectStates([objID_1])
assert ret.ok
r = ret.data[objID_1]
assert set(r.keys()) == set(['rbs', 'frag'])
r = ret.data[objID_1]['rbs']
assert r['position'] == list(pos)
assert r['velocityLin'] == list(vlin)
def main(objID, ref_pos):
"""
:param int objID: ID of sphere.
:param float ref_pos: desired position in space.
"""
# Connect to Azrael.
client = azrael.client.Client()
# Time step for polling/updating the booster values.
dt = 0.3
# Instantiate a PD controller.
PD = PDController(K_p=0.1, K_d=0.1, dt=dt)
# Periodically query the sphere's position, pass it to the controller to
# obtain the force values to moved it towards the desired position, and
# send those forces to sphere's boosters.
while True:
# Query the sphere's position.
ret = client.getRigidBodies([objID])
assert ret.ok
pos = ret.data[objID]['rbs'].position
# Call the controller with the current- and desired position.
force, err = PD.update(pos, ref_pos)
# Create the commands to apply the forces and visually update the
# "flames" coming out of the boosters.
forceCmds, fragStates = compileCommands(force)
# Send the force commands to Azrael.
assert client.controlParts(objID, forceCmds, []).ok
# Send the updated fragment- sizes and position to Azrael.
assert client.setFragmentStates({objID: fragStates}).ok
# Dump some info.
print('Pos={0:+.2f}, {1:+.2f}, {2:+.2f} '
'Err={3:+.2f}, {4:+.2f}, {5:+.2f} '
'Force={6:+.2f}, {7:+.2f}, {8:+.2f}'
.format(pos[0], pos[1], pos[2],
err[0], err[1], err[2],
force[0], force[1], force[2]))
# Wait one time step.
time.sleep(dt)
def run(self):
# Return immediately if no resets are required.
if self.period == -1:
return
# Establish connection to Azrael.
client = azrael.client.Client()
# Query all objects in the scene. These are the only objects that will
# survive the reset.
ret = client.getAllObjectIDs()
assert ret.ok
ret = client.getRigidBodies(ret.data)
assert ret.ok
allowed_objIDs = {k: v["rbs"] for k, v in ret.data.items() if v is not None}
print("Took simulation snapshot for reset: ({} objects)".format(len(allowed_objIDs)))
# Periodically reset the SV values. Set them several times because it
# is well possible that not all State Variables reach Leonard in the
# same frame, which means some objects will be reset while other are
# not. This in turn may cause strange artefacts in the next physics
# update step, especially when the objects now partially overlap.
while True:
# Wait until the timeout expires.
time.sleep(self.period)
# Remove all newly added objects.
ret = client.getAllObjectIDs()
for objID in ret.data:
if objID not in allowed_objIDs:
client.removeObject(objID)
# Forcefully reset the position and velocity of every object. Do
# this several times since network latency may result in some
# objects being reset sooner than others.
for ii in range(5):
for objID, SV in allowed_objIDs.items():
tmp = {
"position": SV.position,
"velocityLin": SV.velocityLin,
"velocityRot": SV.velocityRot,
"rotation": SV.rotation,
}
assert client.setRigidBodies({objID: tmp}).ok
time.sleep(0.1)
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)
def test_setFragments_raw(self, client_type):
"""
Spawn a new object and modify its geometry at runtime.
"""
# Get the client for this test.
client = self.clients[client_type]
# Convenience.
objID = 1
# Add a new template and spawn it.
frag = {'bar': getFragRaw()}
temp = getTemplate('t1', fragments=frag)
assert client.addTemplates([temp]).ok
new_obj = {'templateID': temp.aid,
'rbs': {'position': (1, 1, 1),
'velocityLin': (-1, -1, -1)}}
ret = client.spawn([new_obj])
assert ret.ok and ret.data == [objID]
del temp, new_obj, ret
# Query the SV to obtain the 'version' value.
ret = client.getRigidBodies(objID)
assert ret.ok
version = ret.data[objID]['rbs'].version
# Fetch-, modify-, update- and verify the geometry.
ret = client.getFragments([objID])
assert ret.ok
assert ret.data[objID]['bar']['fragtype'] == 'RAW'
# Download the fragment.
base_url = 'http://{ip}:{port}'.format(
ip=config.addr_webserver, port=config.port_webserver)
url = base_url + ret.data[objID]['bar']['url_frag'] + '/model.json'
for ii in range(10):
assert ii < 8
try:
tmp = urllib.request.urlopen(url).readall()
break
except urllib.request.URLError:
time.sleep(0.2)
tmp = json.loads(tmp.decode('utf8'))
assert FragRaw(**tmp) == frag['bar'].fragdata
# Change the fragment geometries.
cmd = {objID: {k: v._asdict() for (k, v) in frag.items()}}
assert client.setFragments(cmd).ok
ret = client.getFragments([objID])
assert ret.ok
assert ret.data[objID]['bar']['fragtype'] == 'RAW'
# Download the fragment.
url = base_url + ret.data[objID]['bar']['url_frag'] + '/model.json'
tmp = urllib.request.urlopen(url).readall()
tmp = json.loads(tmp.decode('utf8'))
assert FragRaw(**tmp) == frag['bar'].fragdata
# Ensure 'version' is different as well.
ret = client.getRigidBodies(objID)
assert ret.ok and (ret.data[objID]['rbs'].version != version)
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)