def main():
scenes = ['Bolton', 'Connellsville', 'Pleasant', 'Cantwell', 'Placida', 'Nicut', 'Brentsville', 'Samuels', 'Oyens', 'Kerrtown']
for scene in scenes:
print('scene: ', scene, '-' * 50)
p.connect(p.DIRECT)
p.setGravity(0,0,-9.8)
p.setTimeStep(1./240.)
scene = BuildingScene(scene,
is_interactive=True,
build_graph=True,
pybullet_load_texture=True)
scene.load()
p.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 load(self):
if self.config['scene'] == 'stadium':
scene = StadiumScene()
elif self.config['scene'] == 'building':
scene = BuildingScene(self.config['model_id'])
self.simulator.import_scene(scene)
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)
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 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 main():
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
scene = BuildingScene('Rs', build_graph=True, pybullet_load_texture=True)
scene.load()
np.random.seed(0)
for _ in range(10):
random_floor = scene.get_random_floor()
p1 = scene.get_random_point_floor(random_floor)[1]
p2 = scene.get_random_point_floor(random_floor)[1]
shortest_path, geodesic_distance = scene.get_shortest_path(
random_floor, p1[:2], p2[:2], entire_path=True)
print('random point 1:', p1)
print('random point 2:', p2)
print('geodesic distance between p1 and p2', geodesic_distance)
print('shortest path from p1 to p2:', shortest_path)
for _ in range(24000): # at least 100 seconds
p.stepSimulation()
time.sleep(1. / 240.)
p.disconnect()
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)
elif self.config['robot'] == 'DexHandRobot':
robot = DexHandRobot(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 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 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 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 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')
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'):
turtlebot.apply_action([0.1, 0.1])
s.step()
rgb = s.renderer.render_robot_cameras(modes=('rgb'))
s.disconnect()
def rollout(traj, headless=False):
SCENE, traj_id, instr_id, instr, starting_coords = traj
# instr = "Keep going and don't stop"
# starting_coords = [0, 0, 0]
seq = tok.encode_sentence(instr)
tokens = tok.split_sentence(instr)
seq_lengths = [np.argmax(seq == padding_idx, axis=0)]
seq = torch.from_numpy(np.expand_dims(seq, 0)).cuda()
# seq_lengths[seq_lengths == 0] = seq.shape[1] # Full length
ctx, h_t, c_t, ctx_mask = encoder(seq, seq_lengths)
question = h_t
pre_feat = torch.zeros(batch_size, opts.img_feat_input_dim).cuda()
pre_ctx_attend = torch.zeros(batch_size, opts.rnn_hidden_size).cuda()
# Gibson stuff
# 72 fov for 600, 60 for 480
# mode = gui for debug, headless for run
s = Simulator(mode='gui', resolution=640, fov=75, panorama=True)
scene = BuildingScene(SCENE)
# scene = StadiumScene()
ids = s.import_scene(scene)
robot = Turtlebot(config)
ped_id = s.import_robot(robot)
heading_feat_tensor = torch.Tensor(heading_elevation_feat()).view([im_per_ob, 128]).cuda()
s.step()
robot.set_position(starting_coords)
def apply_action(bot: robot, action_idx: int, depth_ok: list, headless=False) -> bool:
print(action_idx)
# action_idx is expected to be 0-13, TODO: make nicer...
if action_idx == 0 or action_idx > 12 or not depth_ok[action_idx - 1]:
print("STOP")
return True
action_idx -= 1
#if action_idx < 3 or (action_idx < 12 and action_idx > 9):
bot.turn_right(0.5235988 * action_idx)
s.step()
if(not headless):
time.sleep(0.2)
bot.move_forward(0.5)
return False
# else:
# if action_idx < 7:
# bot.turn_left(1.57)
# else:
# bot.turn_right(1.57)
bot_is_running = True
while bot_is_running:
s.step()
gib_out = s.renderer.render_robot_cameras(modes=('rgb', '3d'))
rgb = gib_out[::2]
depth = np.array(gib_out[1::2])
processed_rgb = list(map(transform_img, rgb))
batch_obs = np.concatenate(processed_rgb)
imgnet_input = torch.Tensor(batch_obs).cuda()
imgnet_output = torch.zeros([im_per_ob, 2048]).cuda()
# depth processing and filtering
# depth: [36, ]
depth *= depth
depth = depth[:, :, :, :3].sum(axis=3)
depth = np.sqrt(depth)
# filter out 0 distances that are presumably from infinity dist
depth[depth < 0.0001] = 10
# TODO: generalize to non-horizontal moves
depth_ok = depth[12:24, 200:440, 160:480].min(axis=2).min(axis=1)
fig=plt.figure(figsize=(8, 2))
for n, i in enumerate([0, 3, 6, 9]):
fig.add_subplot(1, 4, n + 1)
plt.imshow(depth[12 + i])
plt.show()
# depth_ok *= depth_ok > 1
print(depth_ok)
depth_ok = depth_ok > 0.8
print(depth_ok)
# Each observation has 36 inputs
# We pass rgb images through frozen embedder
for i in range(im_per_ob // B_S):
def hook_fn(m, last_input, o):
imgnet_output[i*B_S:(i+1)*B_S, :] = \
o.detach().squeeze(2).squeeze(2)
imgnet_input[B_S * i : B_S * (i + 1)]
# imgnet_output[B_S * i : B_S * (i + 1)] = resnet(minibatch).detach()
imgnet_output = torch.cat([imgnet_output, heading_feat_tensor], 1)
pano_img_feat = imgnet_output.view([1, im_per_ob, 2176])
navigable_feat = torch.zeros([1, 16, 2176]).cuda()
navigable_feat[0, 1:13] = imgnet_output[12:24] * torch.Tensor(depth_ok).cuda().view(12, 1)
# TODO: make nicer as stated above
navigable_index = [list(map(int, depth_ok))]
print(navigable_index)
# NB: depth_ok replaces navigable_index
h_t, c_t, pre_ctx_attend, img_attn, ctx_attn, logit, value, navigable_mask = model(
pano_img_feat, navigable_feat, pre_feat, question, h_t, c_t, ctx,
pre_ctx_attend, navigable_index, ctx_mask)
print("ATTN")
print(ctx_attn[0])
print(img_attn[0])
plt.bar(range(len(tokens)), ctx_attn.detach().cpu()[0][:len(tokens)])
plt.xticks(range(len(tokens)), tokens)
plt.show()
plt.bar(range(16), img_attn.detach().cpu()[0])
plt.show()
print("NMASK")
print(navigable_mask)
logit.data.masked_fill_((navigable_mask == 0).data, -float('inf'))
m = torch.Tensor([[False] + list(map(lambda b: not b, navigable_index[0])) + [False, False, False]], dtype=bool).cuda()
logit.data.masked_fill_(m, -float('inf'))
action = _select_action(logit, [False])
ended = apply_action(robot, action[0], depth_ok)
bot_is_running = not ended or not headless
if not headless:
time.sleep(.3)
import pybullet as p
import numpy as np
from assistive_gym.envs.env import AssistiveEnv
from assistive_gym.envs.agents.pr2 import PR2
from assistive_gym.envs.agents.agent import Agent
from assistive_gym.envs.agents.tool import Tool
import assistive_gym.envs.agents.human as h
from gibson2.core.physics.scene import BuildingScene
env = AssistiveEnv(render=True)
env.reset()
# Load iGibson environment
scene = BuildingScene('Placida',
is_interactive=True,
build_graph=True,
pybullet_load_texture=True)
scene.load()
# Change position of a chair (the 7th object)
chair = Agent()
chair.init(7, env.id, env.np_random, indices=-1)
chair.set_base_pos_orient([-2.8, 1.15, 0.36], [0, 0, -np.pi / 6.0])
p.removeBody(4, physicsClientId=env.id)
# Create human
human = env.create_human(controllable=False,
controllable_joint_indices=h.head_joints,
fixed_base=False,
human_impairment='none',
gender='random',
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()
def main():
global _mouse_ix, _mouse_iy, down, view_direction
###################################################################################
# load the scene to get traversable area (disable the whole area for performance) #
###################################################################################
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
scene = BuildingScene('Allensville',
build_graph=True,
pybullet_load_texture=True)
objects = scene.load()
random_floor = scene.get_random_floor()
p1 = scene.get_random_point_floor(random_floor)[1]
p2 = scene.get_random_point_floor(random_floor)[1]
shortest_path, geodesic_distance = scene.get_shortest_path(
random_floor, p1[:2], p2[:2], entire_path=True)
print('random point 1:', p1)
print('random point 2:', p2)
print('geodesic distance between p1 and p2', geodesic_distance)
print('shortest path from p1 to p2:', shortest_path)
p.disconnect()
###################################################################################
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Allensville'),
'mesh_z_up.obj')
# set width, height (has to be equal for panorama)
renderer = MeshRenderer(width=512, height=512)
renderer.load_object(model_path)
renderer.add_instance(0)
print(renderer.visual_objects, renderer.instances)
print(renderer.materials_mapping, renderer.mesh_materials)
# set camera_pose / view direction
camera_pose = np.array([0, 0, 1.2])
view_direction = np.array([1, 0, 0])
renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1])
renderer.set_fov(90)
px = 0
py = 0
_mouse_ix, _mouse_iy = -1, -1
down = False
def change_dir(event, x, y, flags, param):
global _mouse_ix, _mouse_iy, down, view_direction
if event == cv2.EVENT_LBUTTONDOWN:
_mouse_ix, _mouse_iy = x, y
down = True
if event == cv2.EVENT_MOUSEMOVE:
if down:
dx = (x - _mouse_ix) / 100.0
dy = (y - _mouse_iy) / 100.0
_mouse_ix = x
_mouse_iy = y
r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0],
[-np.sin(dy), 0, np.cos(dy)]])
r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0],
[np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]])
view_direction = r1.dot(r2).dot(view_direction)
elif event == cv2.EVENT_LBUTTONUP:
down = False
cv2.namedWindow('test')
cv2.setMouseCallback('test', change_dir)
while True:
with Profiler('Render'):
frame = renderer.render(modes=('rgb', 'normal', '3d'))
cv2.imshow(
'test',
cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR))
q = cv2.waitKey(1)
if q == ord('w'):
px += 0.05
elif q == ord('s'):
px -= 0.05
elif q == ord('a'):
py += 0.05
elif q == ord('d'):
py -= 0.05
elif q == ord('q'):
break
camera_pose = np.array([px, py, 1.2])
renderer.set_camera(camera_pose, camera_pose + view_direction,
[0, 0, 1])
renderer.release()
def test_import_building_big():
s = Simulator(mode='headless')
scene = BuildingScene('Rs')
s.import_scene(scene, texture_scale=1)
assert s.objects == list(range(2))
s.disconnect()
from gibson2.core.simulator import Simulator
from gibson2.core.physics.scene import BuildingScene, StadiumScene
from gibson2.core.physics.interactive_objects import Pedestrian
import networkx as nx
import matplotlib.pyplot as plt
import cv2
import gibson2
from PIL import Image
import numpy as np
import os
import pybullet as p
s = Simulator(mode='gui', resolution=512)
scene = BuildingScene("Maugansville")
ids = s.import_scene(scene)
print(ids)
trav_map = np.array(
Image.open(os.path.join(gibson2.dataset_path, 'Maugansville/floor_trav_1_v3.png')))
obstacle_map = np.array(Image.open(os.path.join(gibson2.dataset_path, 'Maugansville/floor_1.png')))
trav_map[obstacle_map == 0] = 0
trav_map = cv2.erode(trav_map, np.ones((30, 30)))
plt.figure(figsize=(12, 12))
plt.imshow(trav_map)
plt.show()
def dist(a, b):
(x1, y1) = a
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')
scene = BuildingScene('Ohoopee')
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()