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 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 run(self):
"""
"""
# Return immediately if no resets are required.
if self.period == -1:
return
# Instantiate Client.
client = azrael.client.Client()
# Query all object IDs. This happens only once which means the geometry
# swap does not affect newly generated objects.
time.sleep(1)
ret = client.getAllObjectIDs()
objIDs = ret.data
print('\n-- {} objects --\n'.format(len(objIDs)))
# Query and backup all models currently in the scene.
geo_meta = client.getFragments(objIDs).data
base_url = 'http://{}:{}'.format(
azrael.config.addr_webserver, azrael.config.port_webserver)
geo_orig = {}
for objID in objIDs:
frags = {}
for frag_name in geo_meta[objID]:
url = base_url + geo_meta[objID][frag_name]['url_frag']
url += '/model.json'
tmp = urllib.request.urlopen(url).readall()
tmp = json.loads(tmp.decode('utf8'))
tmp = FragRaw(**tmp)
frags[frag_name] = FragMeta(frag_name, 'raw', tmp)
del url, tmp
geo_orig[objID] = frags
del frags, objID
del geo_meta, base_url
# Compile a set of sphere models for all objects. These will be
# periodically swapped out for the original models.
sphere_vert, sphere_uv, sphere_rgb = loadSphere()
sphere = FragRaw(sphere_vert, sphere_uv, sphere_rgb)
geo_spheres = {}
for objID in objIDs:
tmp = {_: FragMeta(_, 'raw', sphere) for _ in geo_orig[objID]}
geo_spheres[objID] = tmp
del tmp
del sphere_vert, sphere_uv, sphere_rgb, sphere
cnt = 0
while True:
# Update the counter and pause for the specified time.
cnt = (cnt + 1) % 20
time.sleep(self.period)
# Swap out the geometry.
if (cnt % 2) == 1:
geo = geo_spheres
else:
geo = geo_orig
# Apply the new geometry to each fragment.
for objID, val in geo.items():
ret = client.updateFragments(objID, list(val.values()))
if not ret.ok:
print('--> Terminating geometry updates')
sys.exit(1)
# Modify the global scale and a fragment position.
scale = (cnt + 1) / 10
for objID in objIDs:
# Change the scale of the overall object.
new_sv = RigidBodyDataOverride(scale=scale)
client.setRigidBody(objID, new_sv)
# Move the second fragment.
x = -10 + cnt
newStates = {
objID: [FragState('frag_2', 1, [x, 0, 0], [0, 0, 0, 1])]
}
client.setFragmentStates(newStates)
# Print status for user.
print('Scale={:.1f}'.format(scale))
