def test_multiagent():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
turtlebot3 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
s.import_robot(turtlebot3)
turtlebot1.set_position([1, 0, 0.5])
turtlebot2.set_position([0, 0, 0.5])
turtlebot3.set_position([-1, 0, 0.5])
nbody = p.getNumBodies()
for i in range(100):
#turtlebot1.apply_action(1)
#turtlebot2.apply_action(1)
#turtlebot3.apply_action(1)
s.step()
s.disconnect()
assert nbody == 7
def main():
config = parse_config('../configs/jr2_reaching.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
#scene = BuildingScene('Rs',
# build_graph=True,
# pybullet_load_texture=True)
scene = StadiumScene()
s.import_scene(scene)
jr = JR2_Kinova_Head(config)
#turtlebot = Turtlebot(config)
s.import_robot(jr)
for _ in range(10):
obj = YCBObject('003_cracker_box')
s.import_object(obj)
obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3),
[0, 0, 0, 1])
for i in range(10000):
with Profiler('Simulator step'):
jr.apply_action([0, 0, 0.1, 0.1, 0, 0, 0, 0, 0])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def main():
config = parse_config('../configs/jr2_reaching.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
scene = StadiumScene()
s.import_scene(scene)
jr = JR2_Kinova(config)
s.import_robot(jr)
marker = VisualMarker(visual_shape=p.GEOM_SPHERE,
rgba_color=[0, 0, 1, 0.3],
radius=0.5)
s.import_object(marker)
marker.set_position([0, 4, 1])
for i in range(10000):
with Profiler('Simulator step'):
jr.apply_action([0, 0, 0, 0, 0, 0, 0])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def test_import_stadium():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
print(s.objects)
assert s.objects == list(range(4))
s.disconnect()
def test_import_building_viewing():
s = Simulator(mode='headless')
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
assert s.objects == list(range(2))
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
turtlebot3 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
s.import_robot(turtlebot3)
turtlebot1.set_position([0.5, 0, 0.5])
turtlebot2.set_position([0, 0, 0.5])
turtlebot3.set_position([-0.5, 0, 0.5])
for i in range(10):
s.step()
#turtlebot1.apply_action(np.random.randint(4))
#turtlebot2.apply_action(np.random.randint(4))
#turtlebot3.apply_action(np.random.randint(4))
s.disconnect()
def test_import_box():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
print(s.objects)
# wall = [pos, dim]
wall = [[[0, 7, 1.01], [10, 0.2, 1]], [[0, -7, 1.01], [6.89, 0.1, 1]],
[[7, -1.5, 1.01], [0.1, 5.5, 1]], [[-7, -1, 1.01], [0.1, 6, 1]],
[[-8.55, 5, 1.01], [1.44, 0.1, 1]], [[8.55, 4, 1.01], [1.44, 0.1, 1]]]
obstacles = [[[-0.5, 2, 1.01], [3.5, 0.1, 1]], [[4.5, -1, 1.01], [1.5, 0.1, 1]],
[[-4, -2, 1.01], [0.1, 2, 1]], [[2.5, -4, 1.01], [1.5, 0.1, 1]]]
for i in range(len(wall)):
curr = wall[i]
obj = BoxShape(curr[0], curr[1])
s.import_object(obj)
for i in range(len(obstacles)):
curr = obstacles[i]
obj = BoxShape(curr[0], curr[1])
s.import_object(obj)
config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml'))
turtlebot1 = Turtlebot(config)
turtlebot2 = Turtlebot(config)
s.import_robot(turtlebot1)
s.import_robot(turtlebot2)
turtlebot1.set_position([6., -6., 0.])
turtlebot2.set_position([-3., 4., 0.])
for i in range(100):
s.step()
s.disconnect()
def test_jr2():
s = Simulator(mode='gui')
try:
scene = BuildingScene('Ohoopee')
s.import_scene(scene)
jr2 = JR2_Kinova(config)
s.import_robot(jr2)
jr2.set_position([-6, 0, 0.1])
obj3 = InteractiveObj(os.path.join(gibson2.assets_path, 'models',
'scene_components', 'door.urdf'),
scale=2)
s.import_interactive_object(obj3)
obj3.set_position_rotation(
[-5, -1, 0], [0, 0, np.sqrt(0.5), np.sqrt(0.5)])
jr2.apply_action([0.005, 0.005, 0, 0, 0, 0, 0, 0, 0, 0])
for _ in range(400):
s.step()
jr2.apply_action([
0, 0, 0, 0, 3.1408197119196117, -1.37402907967774,
-0.8377005721485424, -1.9804208517373096, 0.09322135043256494,
2.62937740156038
])
for _ in range(400):
s.step()
finally:
s.disconnect()
def test_import_building():
s = Simulator(mode='headless')
scene = BuildingScene('Ohoopee')
s.import_scene(scene, texture_scale=0.4)
for i in range(15):
s.step()
assert s.objects == list(range(2))
s.disconnect()
def test_turtlebot():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_humanoid():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_jr2():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
jr2 = JR2(config)
s.import_robot(jr2)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_quadrotor():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
quadrotor = Quadrotor(config)
s.import_robot(quadrotor)
nbody = p.getNumBodies()
s.disconnect()
assert nbody == 5
def test_import_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('003_cracker_box')
s.import_object(obj)
objs = s.objects
s.disconnect()
assert objs == list(range(5))
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.calc_state()
s.step()
s.disconnect()
def test_fetch():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
config = parse_config(
os.path.join(gibson2.root_path, '../test/test_continuous.yaml'))
fetch = Fetch(config)
s.import_robot(fetch)
for i in range(100):
fetch.apply_action(np.array([0] * 2))
fetch.calc_state()
s.step()
s.disconnect()
def test_humanoid_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
humanoid = Humanoid(config)
s.import_robot(humanoid)
humanoid.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = humanoid.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def run_demo(self):
config = parse_config(os.path.join(gibson2.assets_path, 'example_configs/turtlebot_demo.yaml'))
s = Simulator(mode='gui', image_width=700, image_height=700)
scene = BuildingScene('Rs_interactive', is_interactive=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for i in range(10000):
turtlebot.apply_action([0.1,0.5])
s.step()
s.disconnect()
def test_turtlebot_position():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 0, 5])
nbody = p.getNumBodies()
pos = turtlebot.get_position()
s.disconnect()
assert nbody == 5
assert np.allclose(pos, np.array([0, 0, 5]))
def test_ant():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
s.disconnect()
assert nbody == 6
def test_import_many_object():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
for i in range(30):
obj = YCBObject('003_cracker_box')
s.import_object(obj)
for j in range(1000):
s.step()
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 33)
def test_import_human():
s = Simulator(mode='gui')
scene = StadiumScene()
s.import_scene(scene)
obj = Pedestrian()
s.import_object(obj)
for j in range(100):
s.step()
obj.reset_position_orientation([j * 0.001, 0, 0], [0, 0, 0, 1])
last_obj = s.objects[-1]
s.disconnect()
assert (last_obj == 4)
def test_simulator():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
obj = YCBObject('006_mustard_bottle')
for i in range(10):
s.import_object(obj)
obj = YCBObject('002_master_chef_can')
for i in range(10):
s.import_object(obj)
for i in range(1000):
s.step()
s.disconnect()
def test_ant():
s = Simulator(mode='headless', timestep=1 / 40.0)
scene = StadiumScene()
s.import_scene(scene)
ant = Ant(config)
s.import_robot(ant)
ant2 = Ant(config)
s.import_robot(ant2)
ant2.set_position([0, 2, 2])
nbody = p.getNumBodies()
for i in range(100):
s.step()
#ant.apply_action(np.random.randint(17))
#ant2.apply_action(np.random.randint(17))
s.disconnect()
assert nbody == 6
def benchmark(render_to_tensor=False, resolution=512):
config = parse_config('../configs/turtlebot_demo.yaml')
s = Simulator(mode='headless', image_width=resolution, image_height=resolution, render_to_tensor=render_to_tensor)
scene = BuildingScene('Rs',
build_graph=True,
pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
n_frame = 500
start = time.time()
for i in range(n_frame):
turtlebot.apply_action([0.1,0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
physics_render_elapsed = time.time() - start
print("physics simulation + rendering rgb, resolution {}, render_to_tensor {}: {} fps".format(resolution,
render_to_tensor,
n_frame/physics_render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
render_elapsed = time.time() - start
print("Rendering rgb, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame/render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('3d'))
render_elapsed = time.time() - start
print("Rendering 3d, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame / render_elapsed))
start = time.time()
for i in range(n_frame):
rgb = s.renderer.render_robot_cameras(modes=('normal'))
render_elapsed = time.time() - start
print("Rendering normal, resolution {}, render_to_tensor {}: {} fps".format(resolution, render_to_tensor,
n_frame / render_elapsed))
s.disconnect()
def show_action_sensor_space():
s = Simulator(mode='headless')
scene = StadiumScene()
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
turtlebot.set_position([0, 1, 0.5])
ant = Ant(config)
s.import_robot(ant)
ant.set_position([0, 2, 0.5])
h = Humanoid(config)
s.import_robot(h)
h.set_position([0, 3, 2])
jr = JR2(config)
s.import_robot(jr)
jr.set_position([0, 4, 0.5])
jr2 = JR2_Kinova(config)
s.import_robot(jr2)
jr2.set_position([0, 5, 0.5])
husky = Husky(config)
s.import_robot(husky)
husky.set_position([0, 6, 0.5])
quad = Quadrotor(config)
s.import_robot(quad)
quad.set_position([0, 7, 0.5])
for robot in s.robots:
print(type(robot), len(robot.ordered_joints), robot.calc_state().shape)
for i in range(100):
s.step()
s.disconnect()
def main():
config = parse_config('../configs/turtlebot_demo.yaml')
s = Simulator(mode='gui', image_width=512, image_height=512)
scene = BuildingScene('Rs',
build_graph=True,
pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for _ in range(10):
obj = YCBObject('003_cracker_box')
obj.load()
obj.set_position_orientation(np.random.uniform(low=0, high=2, size=3), [0,0,0,1])
for i in range(10000):
with Profiler('Simulator step'):
turtlebot.apply_action([0.1,0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def test_import_rbo_object():
s = Simulator(mode='headless')
try:
scene = StadiumScene()
s.import_scene(scene)
obj = RBOObject('book')
s.import_articulated_object(obj)
obj2 = RBOObject('microwave')
s.import_articulated_object(obj2)
obj.set_position([0, 0, 2])
obj2.set_position([0, 1, 2])
obj3 = InteractiveObj(
os.path.join(gibson2.assets_path, 'models', 'scene_components', 'door.urdf'))
s.import_articulated_object(obj3)
for i in range(100):
s.step()
finally:
s.disconnect()
class BaseEnv(gym.Env):
'''
a basic environment, step, observation and reward not implemented
'''
def __init__(self,
config_file,
model_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param action_timestep: environment executes action per action_timestep second
:param physics_timestep: physics timestep for pybullet
:param device_idx: device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.simulator = Simulator(
mode=mode,
timestep=physics_timestep,
use_fisheye=self.config.get('fisheye', False),
image_width=self.config.get('image_width', 128),
image_height=self.config.get('image_height', 128),
vertical_fov=self.config.get('vertical_fov', 90),
device_idx=device_idx,
auto_sync=False)
self.simulator_loop = int(self.action_timestep /
self.simulator.timestep)
self.load()
def reload(self, config_file):
"""
Reload another config file, this allows one to change the envrionment on the fly
:param config_file: new config file path
"""
self.config = parse_config(config_file)
self.simulator.reload()
self.load()
def reload_model(self, model_id):
"""
Reload another model, this allows one to change the envrionment on the fly
:param model_id: new model_id
"""
self.config['model_id'] = model_id
self.simulator.reload()
self.load()
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'empty':
scene = EmptyScene()
elif self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
waypoint_resolution=self.config.get('waypoint_resolution',
0.2),
num_waypoints=self.config.get('num_waypoints', 10),
build_graph=self.config.get('build_graph', False),
trav_map_resolution=self.config.get('trav_map_resolution',
0.1),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
is_interactive=self.config.get('is_interactive', False),
pybullet_load_texture=self.config.get('pybullet_load_texture',
False),
)
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
elif self.config['robot'] == 'Locobot':
robot = Locobot(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot)
def clean(self):
"""
Clean up
"""
if self.simulator is not None:
self.simulator.disconnect()
def simulator_step(self):
"""
Step the simulation, this is different from environment step where one can get observation and reward
"""
self.simulator.step()
def step(self, action):
"""
Overwritten by subclasses
"""
return NotImplementedError()
def reset(self):
"""
Overwritten by subclasses
"""
return NotImplementedError()
def set_mode(self, mode):
self.simulator.mode = mode
class BaseEnv(gym.Env):
'''
a basic environment, step, observation and reward not implemented
'''
def __init__(self,
config_file,
model_id=None,
mode='headless',
device_idx=0):
"""
:param config_file: config_file path
:param model_id: override model_id in config file
:param mode: headless or gui mode
:param device_idx: which GPU to run the simulation and rendering on
"""
self.config = parse_config(config_file)
if model_id is not None:
self.config['model_id'] = model_id
self.simulator = Simulator(
mode=mode,
use_fisheye=self.config.get('fisheye', False),
resolution=self.config.get('resolution', 64),
fov=self.config.get('fov', 90),
device_idx=device_idx)
self.load()
def reload(self, config_file):
"""
Reload another config file, this allows one to change the envrionment on the fly
:param config_file: new config file path
"""
self.config = parse_config(config_file)
self.simulator.reload()
self.load()
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(
self.config['model_id'],
build_graph=self.config.get('build_graph', False),
trav_map_erosion=self.config.get('trav_map_erosion', 2),
should_load_replaced_objects=self.config.get(
'should_load_replaced_objects', False))
# scene: class_id = 0
# robot: class_id = 1
# objects: class_id > 1
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True),
class_id=0)
if self.config['robot'] == 'Turtlebot':
robot = Turtlebot(self.config)
elif self.config['robot'] == 'Husky':
robot = Husky(self.config)
elif self.config['robot'] == 'Ant':
robot = Ant(self.config)
elif self.config['robot'] == 'Humanoid':
robot = Humanoid(self.config)
elif self.config['robot'] == 'JR2':
robot = JR2(self.config)
elif self.config['robot'] == 'JR2_Kinova':
robot = JR2_Kinova(self.config)
elif self.config['robot'] == 'Freight':
robot = Freight(self.config)
elif self.config['robot'] == 'Fetch':
robot = Fetch(self.config)
else:
raise Exception('unknown robot type: {}'.format(
self.config['robot']))
self.scene = scene
self.robots = [robot]
for robot in self.robots:
self.simulator.import_robot(robot, class_id=1)
def clean(self):
"""
Clean up
"""
if self.simulator is not None:
self.simulator.disconnect()
def simulator_step(self):
"""
Step the simulation, this is different from environment step where one can get observation and reward
"""
self.simulator.step()
def step(self, action):
return NotImplementedError
def reset(self):
return NotImplementedError
def set_mode(self, mode):
self.simulator.mode = mode
import yaml
from gibson2.core.physics.robot_locomotors import Turtlebot
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import BuildingScene, StadiumScene
from gibson2.utils.utils import parse_config
import pytest
import pybullet as p
import numpy as np
from gibson2.core.render.profiler import Profiler
config = parse_config('../configs/turtlebot_p2p_nav.yaml')
s = Simulator(mode='gui', resolution=512, render_to_tensor=False)
scene = BuildingScene('Bolton')
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
p.resetBasePositionAndOrientation(scene.ground_plane_mjcf[0],
posObj=[0, 0, -10],
ornObj=[0, 0, 0, 1])
for i in range(2000):
with Profiler('simulator step'):
turtlebot.apply_action([0.1, 0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
