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