Source code for scenic.simulators.webots.model

"""Generic Scenic world model for the Webots simulator.

This model provides a general type of object `WebotsObject` corresponding to a
node in the Webots scene tree, as well as a few more specialized objects.

Scenarios using this model cannot be launched directly from the command line
using the :option:`--simulate` option. Instead, Webots should be started first,
with a ``.wbt`` file that includes nodes for all the objects in the scenario
(see the `WebotsObject` documentation for how to specify which objects
correspond to which nodes). A supervisor node can then invoke Scenic to compile
the scenario and run dynamic simulations: see
:doc:`scenic.simulators.webots.simulator` for details.
"""

import math

import numpy
import trimesh

from scenic.core.object_types import Object2D
from scenic.core.shapes import MeshShape
from scenic.core.distributions import distributionFunction
from scenic.simulators.webots.actions import *

def _errorMsg():
    raise RuntimeError('scenario must be run from inside Webots')
simulator _errorMsg()

def is2DMode():
    from scenic.syntax.veneer import mode2D
    return mode2D

[docs]class WebotsObject: """Abstract class for Webots objects. There several ways to specify which Webots node this object corresponds to: * Set the ``webotsName`` property to the DEF name of the Webots node, which must already exist in the world loaded into Webots. * Set the ``webotsType`` property to a prefix like 'ROCK': the interface will then search for nodes called 'ROCK_0', 'ROCK_1', etc. Again the nodes must already exist in the world loaded into Webots. * Set the ``webotsAdhoc`` property to a dictionary of parameters. This will cause Scenic to dynamically create an Object in Webots, according to the parameters in the dictionary. **This is currently the only way to create objects in Webots that do not correspond to an existing node**. The parameters that can be contained in the dictionary are: * ``physics``: Whether or not physics should be enabled for this object. Default value is :python:`True`. Properties: elevation (float or None; dynamic): default ``None`` (see above). requireVisible (bool): Default value ``False`` (overriding the default from `Object`). webotsAdhoc (None | dict): None implies this object is not Adhoc. A dictionary implies this is an object that Scenic should create in Webots.. If a dictionary, provides parameters for how to instantiate the adhoc object. See `scenic.simulators.webots.model` for more details. webotsName (str): 'DEF' name of the Webots node to use for this object. webotsType (str): If ``webotsName`` is not set, the first available node with 'DEF' name consisting of this string followed by '_0', '_1', etc. will be used for this object. webotsObject: Is set at runtime to a handle to the Webots node for the object, for use with the `Supervisor API`_. Primarily for internal use. controller (str or None): name of the Webots controller to use for this object, if any (instead of a Scenic behavior). resetController (bool): Whether to restart the controller for each simulation (default ``True``). positionOffset (`Vector`): Offset to add when computing the object's position in Webots; for objects whose Webots ``translation`` field is not aligned with the center of the object. rotationOffset (tuple[float, float, float]): Offset to add when computing the object's orientation in Webots; for objects whose front is not aligned with the Webots North axis. density (float): Density of this object in kg/m^3. The corresponding Webots object must have the ``density`` field. .. _Supervisor API: https://www.cyberbotics.com/doc/reference/supervisor?tab-language=python """ elevation[dynamic]: None if is2DMode() else float(self.position.z) requireVisible: False webotsAdhoc: None webotsName: None webotsType: None webotsObject: None controller: None resetController: True positionOffset: (0, 0, 0) rotationOffset: (0, 0, 0) density: None @classmethod def _prepareSpecifiers(cls, specifiers): # Check specifiers for errors for spec in specifiers: # Raise error if elevation is specified outside of 2D mode if spec.name == "With" and tuple(spec.priorities.keys()) == ('elevation',): if not issubclass(cls, Object2D): raise RuntimeError("Elevation being specified outside of 2D mode. " "You should specify position's z component instead.") return specifiers
[docs]class Ground(WebotsObject): """Special kind of object representing a (possibly irregular) ground surface. Implemented using an `ElevationGrid`_ node in Webots. Attributes: allowCollisions (bool): default value `True` (overriding default from `Object`). webotsName (str): default value 'Ground' .. _ElevationGrid: https://www.cyberbotics.com/doc/reference/elevationgrid """ allowCollisions: True webotsName: 'GROUND' positionOffset: (-self.width/2, -self.length/2, self.baseOffset[2]) # origin of ElevationGrid is at a corner baseOffset: Vector(0, 0, -(self.height)/2 + self.baseThickness) width: 10 length: 10 shape: Ground.shapeFromHeights(self.heights, self.width, self.length, self.gridSizeX, self.gridSizeY, self.baseThickness) gridSize: 20 gridSizeX: self.gridSize gridSizeY: self.gridSize baseThickness: 0.1 terrain: () heights: Ground.heightsFromTerrain(self.terrain, self.gridSizeX, self.gridSizeY, self.width, self.length) @staticmethod @distributionFunction def heightsFromTerrain(terrain, gridSizeX, gridSizeY, width, length): for elem in terrain: if not isinstance(elem, Terrain): raise RuntimeError(f'Ground terrain element {elem} is not a Terrain') heights = [] if gridSizeX < 2 or gridSizeY < 2: raise RuntimeError(f'invalid grid size {gridSizeX} x {gridSizeY} for Ground') dx, dy = width / (gridSizeX - 1), length / (gridSizeY - 1) y = -length / 2 for i in range(gridSizeY): row = [] x = -width / 2 for j in range(gridSizeX): height = sum(elem.heightAt(x @ y) for elem in terrain) row.append(height) x += dx heights.append(tuple(row)) y += dy return tuple(heights) @staticmethod @distributionFunction def shapeFromHeights(heights, width, length, gridSizeX, gridSizeY, baseThickness): heights = [row[::-1] for row in heights[::-1]] triangles = [] ## Vertices dx, dy = width / (gridSizeX - 1), length / (gridSizeY - 1) base_x, base_y = -width / 2, length / 2 raw_vertices = numpy.asarray([[base_x+ix*dx, base_y-iy*dy, heights[iy][ix]] for iy in range(gridSizeY) for ix in range(gridSizeX)]) heightmap_vertices = raw_vertices.reshape((gridSizeX, gridSizeY, 3)) vertices = numpy.vstack((raw_vertices, numpy.asarray( [( width/2,-length/2,-baseThickness), (-width/2,-length/2,-baseThickness), (-width/2, length/2,-baseThickness), ( width/2, length/2,-baseThickness)]))) vertex_index_map = {tuple(vertices[i]):i for i in range(len(vertices))} # Create top surface for ix in range(gridSizeX-1): for iy in range(gridSizeY-1): # Calculate an interpolated middle point tl_x, tl_y, _ = heightmap_vertices[ix][iy] bl_x, bl_y, _ = heightmap_vertices[ix+1][iy+1] mean_height = (heightmap_vertices[ix][iy][2] + heightmap_vertices[ix+1][iy][2] + heightmap_vertices[ix][iy+1][2] + heightmap_vertices[ix+1][iy+1][2])/4 interpolated_point = ((tl_x + bl_x)/2, (tl_y + bl_y)/2, mean_height) triangles.extend([ (heightmap_vertices[ix][iy], interpolated_point, heightmap_vertices[ix][iy+1]), # Left (heightmap_vertices[ix][iy], heightmap_vertices[ix+1][iy], interpolated_point), # Top (heightmap_vertices[ix+1][iy], heightmap_vertices[ix+1][iy+1], interpolated_point), # Right (heightmap_vertices[ix][iy+1], interpolated_point, heightmap_vertices[ix+1][iy+1]), # Bottom ]) # Create side surfaces side_xy_vals = [(0, -width/2), (0, width/2), (1, -length/2), (1, length/2)] for side_vals in side_xy_vals: mid_side_vertices = [v for v in raw_vertices if v[side_vals[0]] == side_vals[1]] if side_vals[0] == 0: if side_vals[1] < 0: side_vertices = [(-width/2, length/2, -baseThickness)] + mid_side_vertices + [(-width/2, -length/2, -baseThickness)] + [(-width/2, length/2, -baseThickness)] else: side_vertices = [(width/2, length/2, -baseThickness)] + mid_side_vertices + [(width/2, -length/2, -baseThickness)] + [(width/2, length/2, -baseThickness)] else: if side_vals[1] < 0: side_vertices = [(-width/2, -length/2, -baseThickness)] + mid_side_vertices + [(width/2, -length/2, -baseThickness)] + [(-width/2, -length/2, -baseThickness)] else: side_vertices = [(-width/2, length/2, -baseThickness)] + mid_side_vertices + [(width/2, length/2, -baseThickness)] + [(-width/2, length/2, -baseThickness)] side_indices = [vertex_index_map[tuple(v)] for v in side_vertices] side_path = trimesh.path.Path3D(entities=[trimesh.path.entities.Line(side_indices)], vertices=vertices) flat_side_path, transform = side_path.to_planar(to_2D=None, normal=None, check=True) side_vertices_2d, side_faces = flat_side_path.triangulate() side_vertices_3d = trimesh.transformations.transform_points( numpy.hstack((side_vertices_2d,numpy.zeros([side_vertices_2d.shape[0],1], side_vertices_2d.dtype))), transform) side_triangles = [(side_vertices_3d[f1], side_vertices_3d[f3], side_vertices_3d[f2]) for f1, f2, f3 in side_faces] triangles += side_triangles # Create bottom surface triangles += [((-width/2,length/2,-baseThickness),(width/2,-length/2,-baseThickness),(-width/2,-length/2,-baseThickness)), ((-width/2,length/2,-baseThickness),(width/2,-length/2,-baseThickness),(width/2,length/2,-baseThickness))] # Create and tune mesh heightmap_mesh = trimesh.Trimesh(**trimesh.triangles.to_kwargs(triangles)) trimesh.repair.fix_winding(heightmap_mesh) heightmap_mesh.merge_vertices() assert heightmap_mesh.is_volume return MeshShape(heightmap_mesh) def startDynamicSimulation(self): super().startDynamicSimulation() self.setGeometry() def setGeometry(self): # Set basic properties of grid shape = self.webotsObject.getField('children').getMFNode(0) grid = shape.getField('geometry').getSFNode() # ElevationGrid node grid.getField('xDimension').setSFInt32(self.gridSizeX) grid.getField('xSpacing').setSFFloat(self.width / (self.gridSizeX - 1)) grid.getField('yDimension').setSFInt32(self.gridSizeY) grid.getField('ySpacing').setSFFloat(self.length / (self.gridSizeY - 1)) # Adjust length of height field as needed # (this will trigger Webots warnings, unfortunately; there seems to be no way to # update the length simultaneously with xDimension, etc.) heightField = grid.getField('height') count = heightField.getCount() size = self.gridSizeX * self.gridSizeY if count > size: for i in range(count - size): heightField.removeMF(-1) elif count < size: for i in range(size - count): heightField.insertMFFloat(-1, 0) # Set height values i = 0 for row in self.heights: for height in reversed(row): heightField.setMFFloat(i, height) i += 1
[docs]class Terrain: """Abstract class for objects added together to make a `Ground`. This is not a `WebotsObject` since it doesn't actually correspond to a Webots node. Only the overall `Ground` has a node. """ allowCollisions: True def heightAt(self, pt): offset = pt - self.position return self.heightAtOffset(offset) def heightAtOffset(self, offset): raise NotImplementedError('should be implemented by subclasses')
[docs]class Hill(Terrain): """`Terrain` shaped like a Gaussian. Attributes: height (float): height of the hill (default 1). spread (float): standard deviation as a fraction of the hill's size (default 3). """ height: 1 spread: 0.25 color: (0,0,0,0) def heightAtOffset(self, offset): dx, dy, _ = offset if not (-self.hw < dx < self.hw and -self.hl < dy < self.hl): return 0 sx, sy = dx / (self.width * self.spread), dy / (self.length * self.spread) nh = math.exp(-((sx * sx) + (sy * sy)) * 0.5) return self.height * nh