in safety_gym/envs/world.py [0:0]
def build(self):
''' Build a world, including generating XML and moving objects '''
# Read in the base XML (contains robot, camera, floor, etc)
self.robot_base_path = os.path.join(BASE_DIR, self.robot_base)
with open(self.robot_base_path) as f:
self.robot_base_xml = f.read()
self.xml = xmltodict.parse(self.robot_base_xml) # Nested OrderedDict objects
# Convenience accessor for xml dictionary
worldbody = self.xml['mujoco']['worldbody']
# Move robot position to starting position
worldbody['body']['@pos'] = convert(np.r_[self.robot_xy, self.robot.z_height])
worldbody['body']['@quat'] = convert(rot2quat(self.robot_rot))
# We need this because xmltodict skips over single-item lists in the tree
worldbody['body'] = [worldbody['body']]
if 'geom' in worldbody:
worldbody['geom'] = [worldbody['geom']]
else:
worldbody['geom'] = []
# Add equality section if missing
if 'equality' not in self.xml['mujoco']:
self.xml['mujoco']['equality'] = OrderedDict()
equality = self.xml['mujoco']['equality']
if 'weld' not in equality:
equality['weld'] = []
# Add asset section if missing
if 'asset' not in self.xml['mujoco']:
# old default rgb1: ".4 .5 .6"
# old default rgb2: "0 0 0"
# light pink: "1 0.44 .81"
# light blue: "0.004 0.804 .996"
# light purple: ".676 .547 .996"
# med blue: "0.527 0.582 0.906"
# indigo: "0.293 0 0.508"
asset = xmltodict.parse('''
<asset>
<texture type="skybox" builtin="gradient" rgb1="0.527 0.582 0.906" rgb2="0.1 0.1 0.35"
width="800" height="800" markrgb="1 1 1" mark="random" random="0.001"/>
<texture name="texplane" builtin="checker" height="100" width="100"
rgb1="0.7 0.7 0.7" rgb2="0.8 0.8 0.8" type="2d"/>
<material name="MatPlane" reflectance="0.1" shininess="0.1" specular="0.1"
texrepeat="10 10" texture="texplane"/>
</asset>
''')
self.xml['mujoco']['asset'] = asset['asset']
# Add light to the XML dictionary
light = xmltodict.parse('''<b>
<light cutoff="100" diffuse="1 1 1" dir="0 0 -1" directional="true"
exponent="1" pos="0 0 0.5" specular="0 0 0" castshadow="false"/>
</b>''')
worldbody['light'] = light['b']['light']
# Add floor to the XML dictionary if missing
if not any(g.get('@name') == 'floor' for g in worldbody['geom']):
floor = xmltodict.parse('''
<geom name="floor" type="plane" condim="6"/>
''')
worldbody['geom'].append(floor['geom'])
# Make sure floor renders the same for every world
for g in worldbody['geom']:
if g['@name'] == 'floor':
g.update({'@size': convert(self.floor_size), '@rgba': '1 1 1 1', '@material': 'MatPlane'})
# Add cameras to the XML dictionary
cameras = xmltodict.parse('''<b>
<camera name="fixednear" pos="0 -2 2" zaxis="0 -1 1"/>
<camera name="fixedfar" pos="0 -5 5" zaxis="0 -1 1"/>
</b>''')
worldbody['camera'] = cameras['b']['camera']
# Build and add a tracking camera (logic needed to ensure orientation correct)
theta = self.robot_rot
xyaxes = dict(
x1=np.cos(theta),
x2=-np.sin(theta),
x3=0,
y1=np.sin(theta),
y2=np.cos(theta),
y3=1
)
pos = dict(
xp=0*np.cos(theta) + (-2)*np.sin(theta),
yp=0*(-np.sin(theta)) + (-2)*np.cos(theta),
zp=2
)
track_camera = xmltodict.parse('''<b>
<camera name="track" mode="track" pos="{xp} {yp} {zp}" xyaxes="{x1} {x2} {x3} {y1} {y2} {y3}"/>
</b>'''.format(**pos, **xyaxes))
worldbody['body'][0]['camera'] = [
worldbody['body'][0]['camera'],
track_camera['b']['camera']
]
# Add objects to the XML dictionary
for name, object in self.objects.items():
assert object['name'] == name, f'Inconsistent {name} {object}'
object = object.copy() # don't modify original object
object['quat'] = rot2quat(object['rot'])
if name=='box':
dim = object['size'][0]
object['dim'] = dim
object['width'] = dim/2
object['x'] = dim
object['y'] = dim
body = xmltodict.parse('''
<body name="{name}" pos="{pos}" quat="{quat}">
<freejoint name="{name}"/>
<geom name="{name}" type="{type}" size="{size}" density="{density}"
rgba="{rgba}" group="{group}"/>
<geom name="col1" type="{type}" size="{width} {width} {dim}" density="{density}"
rgba="{rgba}" group="{group}" pos="{x} {y} 0"/>
<geom name="col2" type="{type}" size="{width} {width} {dim}" density="{density}"
rgba="{rgba}" group="{group}" pos="-{x} {y} 0"/>
<geom name="col3" type="{type}" size="{width} {width} {dim}" density="{density}"
rgba="{rgba}" group="{group}" pos="{x} -{y} 0"/>
<geom name="col4" type="{type}" size="{width} {width} {dim}" density="{density}"
rgba="{rgba}" group="{group}" pos="-{x} -{y} 0"/>
</body>
'''.format(**{k: convert(v) for k, v in object.items()}))
else:
body = xmltodict.parse('''
<body name="{name}" pos="{pos}" quat="{quat}">
<freejoint name="{name}"/>
<geom name="{name}" type="{type}" size="{size}" density="{density}"
rgba="{rgba}" group="{group}"/>
</body>
'''.format(**{k: convert(v) for k, v in object.items()}))
# Append new body to world, making it a list optionally
# Add the object to the world
worldbody['body'].append(body['body'])
# Add mocaps to the XML dictionary
for name, mocap in self.mocaps.items():
# Mocap names are suffixed with 'mocap'
assert mocap['name'] == name, f'Inconsistent {name} {object}'
assert name.replace('mocap', 'obj') in self.objects, f'missing object for {name}'
# Add the object to the world
mocap = mocap.copy() # don't modify original object
mocap['quat'] = rot2quat(mocap['rot'])
body = xmltodict.parse('''
<body name="{name}" mocap="true">
<geom name="{name}" type="{type}" size="{size}" rgba="{rgba}"
pos="{pos}" quat="{quat}" contype="0" conaffinity="0" group="{group}"/>
</body>
'''.format(**{k: convert(v) for k, v in mocap.items()}))
worldbody['body'].append(body['body'])
# Add weld to equality list
mocap['body1'] = name
mocap['body2'] = name.replace('mocap', 'obj')
weld = xmltodict.parse('''
<weld name="{name}" body1="{body1}" body2="{body2}" solref=".02 1.5"/>
'''.format(**{k: convert(v) for k, v in mocap.items()}))
equality['weld'].append(weld['weld'])
# Add geoms to XML dictionary
for name, geom in self.geoms.items():
assert geom['name'] == name, f'Inconsistent {name} {geom}'
geom = geom.copy() # don't modify original object
geom['quat'] = rot2quat(geom['rot'])
geom['contype'] = geom.get('contype', 1)
geom['conaffinity'] = geom.get('conaffinity', 1)
body = xmltodict.parse('''
<body name="{name}" pos="{pos}" quat="{quat}">
<geom name="{name}" type="{type}" size="{size}" rgba="{rgba}" group="{group}"
contype="{contype}" conaffinity="{conaffinity}"/>
</body>
'''.format(**{k: convert(v) for k, v in geom.items()}))
# Append new body to world, making it a list optionally
# Add the object to the world
worldbody['body'].append(body['body'])
# Instantiate simulator
# print(xmltodict.unparse(self.xml, pretty=True))
self.xml_string = xmltodict.unparse(self.xml)
self.model = load_model_from_xml(self.xml_string)
self.sim = MjSim(self.model)
# Add render contexts to newly created sim
if self.render_context is None and self.observe_vision:
render_context = MjRenderContextOffscreen(self.sim, device_id=-1, quiet=True)
render_context.vopt.geomgroup[:] = 1
self.render_context = render_context
if self.render_context is not None:
self.render_context.update_sim(self.sim)
# Recompute simulation intrinsics from new position
self.sim.forward()