def test_add_get_remove_constraints(self, client_type):
"""
Create some bodies. Then add/query/remove constraints.
This test only verifies that the Igor interface works. It does *not*
verify that the objects are really linked in the actual simulation.
"""
# Reset the constraint database.
igor = azrael.igor.Igor()
assert igor.reset().ok
# Get the client for this test.
client = self.clients[client_type]
# Spawn the two bodies.
pos_1, pos_2, pos_3 = [-2, 0, 0], [2, 0, 0], [6, 0, 0]
new_objs = [
{'templateID': '_templateSphere', 'rbs': {'position': pos_1}},
{'templateID': '_templateSphere', 'rbs': {'position': pos_2}},
{'templateID': '_templateSphere', 'rbs': {'position': pos_3}}
]
id_1, id_2, id_3 = 1, 2, 3
assert client.spawn(new_objs) == (True, None, [id_1, id_2, id_3])
# Define the constraints.
con_1 = getP2P(rb_a=id_1, rb_b=id_2, pivot_a=pos_2, pivot_b=pos_1)
con_2 = get6DofSpring2(rb_a=id_2, rb_b=id_3)
# Verify that no constraints are currently active.
assert client.getConstraints(None) == (True, None, [])
assert client.getConstraints([id_1]) == (True, None, [])
# Add both constraints and verify they are returned correctly.
assert client.addConstraints([con_1, con_2]) == (True, None, 2)
ret = client.getConstraints(None)
assert ret.ok and (sorted(ret.data) == sorted([con_1, con_2]))
ret = client.getConstraints([id_2])
assert ret.ok and (sorted(ret.data) == sorted([con_1, con_2]))
assert client.getConstraints([id_1]) == (True, None, [con_1])
assert client.getConstraints([id_3]) == (True, None, [con_2])
# Remove the second constraint and verify the remaining constraint is
# returned correctly.
assert client.deleteConstraints([con_2]) == (True, None, 1)
assert client.getConstraints(None) == (True, None, [con_1])
assert client.getConstraints([id_1]) == (True, None, [con_1])
assert client.getConstraints([id_2]) == (True, None, [con_1])
assert client.getConstraints([id_3]) == (True, None, [])
def spawnCubes(numCols, numRows, numLayers, center=(0, 0, 0)):
"""
Spawn multiple cubes in a regular grid.
The number of cubes equals ``numCols`` * ``numRows`` * ``numLayers``. The
center of this "prism" is at ``center``.
Every cube has two boosters and two factories. The factories can themselves
spawn more (purely passive) cubes.
"""
# Get a Client instance.
client = azrael.client.Client()
# Vertices that define a Cube.
vert = 1 * np.array([
-1.0, -1.0, -1.0, -1.0, -1.0, +1.0, -1.0, +1.0, +1.0,
-1.0, -1.0, -1.0, -1.0, +1.0, +1.0, -1.0, +1.0, -1.0,
+1.0, -1.0, -1.0, +1.0, +1.0, +1.0, +1.0, -1.0, +1.0,
+1.0, -1.0, -1.0, +1.0, +1.0, -1.0, +1.0, +1.0, +1.0,
+1.0, -1.0, +1.0, -1.0, -1.0, -1.0, +1.0, -1.0, -1.0,
+1.0, -1.0, +1.0, -1.0, -1.0, +1.0, -1.0, -1.0, -1.0,
+1.0, +1.0, +1.0, +1.0, +1.0, -1.0, -1.0, +1.0, -1.0,
+1.0, +1.0, +1.0, -1.0, +1.0, -1.0, -1.0, +1.0, +1.0,
+1.0, +1.0, -1.0, -1.0, -1.0, -1.0, -1.0, +1.0, -1.0,
+1.0, +1.0, -1.0, +1.0, -1.0, -1.0, -1.0, -1.0, -1.0,
-1.0, +1.0, +1.0, -1.0, -1.0, +1.0, +1.0, -1.0, +1.0,
+1.0, +1.0, +1.0, -1.0, +1.0, +1.0, +1.0, -1.0, +1.0
])
# Convenience.
cs = CollShapeBox(1, 1, 1)
cs = CollShapeMeta('', 'box', (0, 0, 0), (0, 0, 0, 1), cs)
uv = np.array([], np.float64)
rgb = np.array([], np.uint8)
uv = np.zeros(12 * 6, np.float64)
uv[0:6] = [0, 0, 1, 0, 1, 1]
uv[6:12] = [0, 0, 1, 1, 0, 1]
uv[12:18] = [1, 0, 0, 1, 0, 0]
uv[18:24] = [1, 0, 1, 1, 0, 1]
uv[24:30] = [0, 0, 1, 1, 0, 1]
uv[30:36] = [0, 0, 1, 0, 1, 1]
uv[36:42] = [1, 1, 1, 0, 0, 0]
uv[42:48] = [1, 1, 0, 0, 0, 1]
uv[48:54] = [0, 1, 1, 0, 1, 1]
uv[54:60] = [0, 1, 0, 0, 1, 0]
uv[60:66] = [0, 1, 0, 0, 1, 0]
uv[66:72] = [1, 1, 0, 1, 1, 0]
uv = np.array(uv, np.float64)
# Compile the path to the texture file.
path_base = os.path.dirname(os.path.abspath(__file__))
path_base = os.path.join(path_base, '..', 'azrael', 'static', 'img')
fname = os.path.join(path_base, 'texture_5.jpg')
# Load the texture and convert it to flat vector because this is how OpenGL
# will want it.
img = PIL.Image.open(fname)
img = np.array(img)
rgb = np.rollaxis(np.flipud(img), 1).flatten()
# # ----------------------------------------------------------------------
# # Create templates for the factory output.
# # ----------------------------------------------------------------------
# tID_1 = 'Product1'
# tID_2 = 'Product2'
# frags_1 = [FragMeta('frag_1', 'raw', FragRaw(0.75 * vert, uv, rgb))]
# frags_2 = [FragMeta('frag_1', 'raw', FragRaw(0.24 * vert, uv, rgb))]
# t1 = Template(tID_1, [cs], frags_1, [], [])
# t2 = Template(tID_2, [cs], frags_2, [], [])
# assert client.addTemplates([t1, t2]).ok
# del frags_1, frags_2, t1, t2
# ----------------------------------------------------------------------
# Define a cube with boosters and factories.
# ----------------------------------------------------------------------
# Two boosters, one left, one right. Both point in the same direction.
b0 = types.Booster(partID='0', pos=[+0.05, 0, 0], direction=[0, 0, 1],
minval=0, maxval=10.0, force=0)
b1 = types.Booster(partID='1', pos=[-0.05, 0, 0], direction=[0, 0, 1],
minval=0, maxval=10.0, force=0)
# # Two factories, one left one right. They will eject the new objects
# # forwards and backwards, respectively.
# f0 = types.Factory(
# partID='0', pos=[+1.5, 0, 0], direction=[+1, 0, 0],
# templateID=tID_1, exit_speed=[0.1, 1])
# f1 = types.Factory(
# partID='1', pos=[-1.5, 0, 0], direction=[-1, 0, 0],
# templateID=tID_2, exit_speed=[0.1, 1])
# # Add the template.
# tID_3 = 'BoosterCube'
# frags = [FragMeta('frag_1', 'raw', FragRaw(vert, uv, rgb))]
# t3 = Template(tID_3, [cs], frags, [b0, b1], [f0, f1])
# assert client.addTemplates([t3]).ok
# del frags, t3
# ----------------------------------------------------------------------
# Define more booster cubes, each with a different texture.
# ----------------------------------------------------------------------
tID_cube = {}
templates = []
texture_errors = 0
for ii in range(numRows * numCols * numLayers):
# File name of texture.
fname = os.path.join(path_base, 'texture_{}.jpg'.format(ii + 1))
# Load the texture image. If the image is unavailable do not endow the
# cube with a texture.
try:
img = PIL.Image.open(fname)
img = np.array(img)
rgb = np.rollaxis(np.flipud(img), 1).flatten()
curUV = uv
except FileNotFoundError:
texture_errors += 1
rgb = curUV = np.array([])
# Create the template.
tID = ('BoosterCube_{}'.format(ii))
frags = [FragMeta('frag_1', 'raw', FragRaw(vert, curUV, rgb)),
FragMeta('frag_2', 'raw', FragRaw(vert, curUV, rgb))]
tmp = Template(tID, [cs], frags, [b0, b1], [])
templates.append(tmp)
# Add the templateID to a dictionary because we will need it in the
# next step to spawn the templates.
tID_cube[ii] = tID
del frags, tmp, tID, fname
if texture_errors > 0:
print('Could not load texture for {} of the {} objects'
.format(texture_errors, ii + 1))
# Define all templates.
print('Adding {} templates: '.format(ii + 1), end='', flush=True)
t0 = time.time()
assert client.addTemplates(templates).ok
print('{:.1f}s'.format(time.time() - t0))
# ----------------------------------------------------------------------
# Spawn the differently textured cubes in a regular grid.
# ----------------------------------------------------------------------
allObjs = []
cube_idx = 0
cube_spacing = 0.0
# Determine the template and position for every cube. The cubes are *not*
# spawned in this loop, but afterwards.
print('Compiling scene: ', end='', flush=True)
t0 = time.time()
for row in range(numRows):
for col in range(numCols):
for lay in range(numLayers):
# Base position of cube.
pos = np.array([col, row, lay], np.float64)
# Add space in between cubes.
pos *= -(4 + cube_spacing)
# Correct the cube's position to ensure the center of the
# grid coincides with the origin.
pos[0] += (numCols // 2) * (1 + cube_spacing)
pos[1] += (numRows // 2) * (1 + cube_spacing)
pos[2] += (numLayers // 2) * (1 + cube_spacing)
# Move the grid to position ``center``.
pos += np.array(center)
# Store the position and template for this cube.
allObjs.append({'template': tID_cube[cube_idx],
'position': pos})
cube_idx += 1
del pos
# fixme: this code assumes there are at least 4 cubes!
if len(allObjs) < 4:
print('fixme: start demo with at least 4 cubes!')
sys.exit(1)
allObjs = []
pos_0 = [2, 0, 10]
pos_1 = [-2, 0, 10]
pos_2 = [-6, 0, 10]
pos_3 = [-10, 0, 10]
allObjs.append({'template': tID_cube[0], 'position': pos_0})
allObjs.append({'template': tID_cube[1], 'position': pos_1})
allObjs.append({'template': tID_cube[2], 'position': pos_2})
allObjs.append({'template': tID_cube[3], 'position': pos_3})
# The first object cannot move (only rotate). It serves as an anchor for
# the connected bodies.
allObjs[0]['axesLockLin'] = [0, 0, 0]
allObjs[0]['axesLockRot'] = [1, 1, 1]
# Add a small damping factor to all bodies to avoid them moving around
# perpetually.
for oo in allObjs[1:]:
oo['axesLockLin'] = [0.9, 0.9, 0.9]
oo['axesLockRot'] = [0.9, 0.9, 0.9]
print('{:,} objects ({:.1f}s)'.format(len(allObjs), time.time() - t0))
del cube_idx, cube_spacing, row, col, lay
# Spawn the cubes from the templates at the just determined positions.
print('Spawning {} objects: '.format(len(allObjs)), end='', flush=True)
t0 = time.time()
ret = client.spawn(allObjs)
assert ret.ok
ids = ret.data
print('{:.1f}s'.format(time.time() - t0))
# Define the constraints.
p2p_0 = ConstraintP2P(pivot_a=[-2, 0, 0], pivot_b=[2, 0, 0])
p2p_1 = ConstraintP2P(pivot_a=[-2, 0, 0], pivot_b=[2, 0, 0])
p2p_2 = ConstraintP2P(pivot_a=[-2, 0, 0], pivot_b=[2, 0, 0])
dof = Constraint6DofSpring2(
frameInA=[0, 0, 0, 0, 0, 0, 1],
frameInB=[0, 0, 0, 0, 0, 0, 1],
stiffness=[2, 2, 2, 1, 1, 1],
damping=[1, 1, 1, 1, 1, 1],
equilibrium=[-2, -2, -2, 0, 0, 0],
linLimitLo=[-4.5, -4.5, -4.5],
linLimitHi=[4.5, 4.5, 4.5],
rotLimitLo=[-0.1, -0.2, -0.3],
rotLimitHi=[0.1, 0.2, 0.3],
bounce=[1, 1.5, 2],
enableSpring=[True, False, False, False, False, False])
constraints = [
ConstraintMeta('', 'p2p', ids[0], ids[1], p2p_0),
ConstraintMeta('', 'p2p', ids[1], ids[2], p2p_1),
ConstraintMeta('', '6DOFSPRING2', ids[2], ids[3], dof),
]
assert client.addConstraints(constraints) == (True, None, 3)
# Make 'frag_2' invisible by setting its scale to zero.
for objID in ret.data:
client.setFragmentStates({objID: [
FragState('frag_2', 0, [0, 0, 0], [0, 0, 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)
