def main():
model_id = 'Rs'
trav_map_original_size = 1000
trav_map_size = 200
trav_map_erosion = 2
floor_map = []
floor_graph = []
with open(os.path.join(get_model_path(model_id), 'floors.txt'), 'r') as f:
floors = sorted(list(map(float, f.readlines())))
print('floors', floors)
for f in range(len(floors)):
trav_map = Image.open(
os.path.join(get_model_path(model_id),
'floor_trav_{}.png'.format(f)))
obstacle_map = Image.open(
os.path.join(get_model_path(model_id), 'floor_{}.png'.format(f)))
trav_map = np.array(trav_map.resize((trav_map_size, trav_map_size)))
obstacle_map = np.array(
obstacle_map.resize((trav_map_size, trav_map_size)))
trav_map[obstacle_map == 0] = 0
trav_map = cv2.erode(trav_map,
np.ones((trav_map_erosion, trav_map_erosion)))
plt.figure(f, figsize=(12, 12))
plt.imshow(trav_map)
plt.show()
def load_scene_mesh(self):
"""
Load scene mesh
"""
if self.is_interactive:
filename = os.path.join(get_model_path(self.model_id),
"mesh_z_up_cleaned.obj")
else:
filename = os.path.join(get_model_path(self.model_id),
"mesh_z_up_downsampled.obj")
if not os.path.isfile(filename):
filename = os.path.join(get_model_path(self.model_id),
"mesh_z_up.obj")
collision_id = p.createCollisionShape(
p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
if self.pybullet_load_texture:
visual_id = p.createVisualShape(p.GEOM_MESH, fileName=filename)
texture_filename = get_texture_file(filename)
texture_id = p.loadTexture(texture_filename)
else:
visual_id = -1
texture_id = -1
self.mesh_body_id = p.createMultiBody(
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
p.changeDynamics(self.mesh_body_id, -1, lateralFriction=1)
if self.pybullet_load_texture:
p.changeVisualShape(self.mesh_body_id,
-1,
textureUniqueId=texture_id)
def main():
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj')
renderer = MeshRendererG2G(width=512, height=512, device_idx=0)
renderer.load_object(model_path)
renderer.add_instance(0)
print(renderer.visual_objects, renderer.instances)
print(renderer.materials_mapping, renderer.mesh_materials)
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)
for i in range(3000):
with Profiler('Render'):
frame = renderer.render(modes=('rgb', 'normal', '3d'))
print(frame)
img_np = frame[0].flip(0).data.cpu().numpy().reshape(
renderer.height, renderer.width, 4)
normal_np = frame[1].flip(0).data.cpu().numpy().reshape(
renderer.height, renderer.width, 4)
plt.imshow(np.concatenate([img_np, normal_np], axis=1))
plt.show()
def load_floor_metadata(self):
"""
Load floor metadata
"""
floor_height_path = os.path.join(get_model_path(self.model_id),
'floors.txt')
if not os.path.isfile(floor_height_path):
raise Exception('floors.txt cannot be found in model: {}'.format(
self.model_id))
with open(floor_height_path, 'r') as f:
self.floors = sorted(list(map(float, f.readlines())))
logging.debug('Floors {}'.format(self.floors))
def main():
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj')
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
# Load scenes
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=model_path,
meshScale=1.0,
flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=model_path,
meshScale=1.0)
texture_filename = get_texture_file(model_path)
texture_id = p.loadTexture(texture_filename)
mesh_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
# Load robots
turtlebot_urdf = os.path.join(gibson2.assets_path,
'models/turtlebot/turtlebot.urdf')
robot_id = p.loadURDF(turtlebot_urdf,
flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL)
# Load objects
obj_visual_filename = os.path.join(
gibson2.assets_path,
'models/ycb/002_master_chef_can/textured_simple.obj')
obj_collision_filename = os.path.join(
gibson2.assets_path,
'models/ycb/002_master_chef_can/textured_simple_vhacd.obj')
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=obj_collision_filename,
meshScale=1.0)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=obj_visual_filename,
meshScale=1.0)
object_id = p.createMultiBody(baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id,
basePosition=[1.0, 0.0, 1.0],
baseMass=0.1)
for _ in range(10000):
p.stepSimulation()
p.disconnect()
def load_floor_planes(self):
if self.is_interactive:
for f in range(len(self.floors)):
# load the floor plane with the original floor texture for each floor
plane_name = os.path.join(get_model_path(self.model_id),
"plane_z_up_{}.obj".format(f))
collision_id = p.createCollisionShape(p.GEOM_MESH,
fileName=plane_name)
visual_id = p.createVisualShape(p.GEOM_MESH,
fileName=plane_name)
texture_filename = get_texture_file(plane_name)
if texture_filename is not None:
texture_id = p.loadTexture(texture_filename)
else:
texture_id = -1
floor_body_id = p.createMultiBody(
baseCollisionShapeIndex=collision_id,
baseVisualShapeIndex=visual_id)
if texture_id != -1:
p.changeVisualShape(floor_body_id,
-1,
textureUniqueId=texture_id)
floor_height = self.floors[f]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, floor_height],
ornObj=[0, 0, 0, 1])
# Since both the floor plane and the scene mesh have mass 0 (static),
# PyBullet seems to have already disabled collision between them.
# Just to be safe, explicit disable collision between them.
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
else:
# load the default floor plane (only once) and later reset it to different floor heiights
plane_name = os.path.join(pybullet_data.getDataPath(),
"mjcf/ground_plane.xml")
floor_body_id = p.loadMJCF(plane_name)[0]
p.resetBasePositionAndOrientation(floor_body_id,
posObj=[0, 0, 0],
ornObj=[0, 0, 0, 1])
p.setCollisionFilterPair(self.mesh_body_id,
floor_body_id,
-1,
-1,
enableCollision=0)
self.floor_body_ids.append(floor_body_id)
def main():
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj')
renderer = MeshRenderer(width=512, height=512)
renderer.load_object(model_path)
renderer.add_instance(0)
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)
frames = renderer.render(modes=('rgb', 'normal', '3d'))
frames = cv2.cvtColor(np.concatenate(frames, axis=1), cv2.COLOR_RGB2BGR)
cv2.imshow('image', frames)
cv2.waitKey(0)
def load_scene_objects(self):
if not self.is_interactive:
return
self.scene_objects = []
self.scene_objects_pos = []
scene_path = get_model_path(self.model_id)
urdf_files = [
item for item in os.listdir(scene_path)
if item[-4:] == 'urdf' and item != 'scene.urdf'
]
position_files = [
item[:-4].replace('alignment_centered', 'pos') + 'txt'
for item in urdf_files
]
for urdf_file, position_file in zip(urdf_files, position_files):
logging.info('Loading urdf file {}'.format(urdf_file))
with open(os.path.join(scene_path, position_file)) as f:
pos = np.array(
[float(item) for item in f.readlines()[0].strip().split()])
obj = InteractiveObj(os.path.join(scene_path, urdf_file))
obj.load()
self.scene_objects.append(obj)
self.scene_objects_pos.append(pos)
def has_scene_urdf(self):
return os.path.exists(
os.path.join(get_model_path(self.model_id), 'scene.urdf'))
def load_scene_urdf(self):
self.mesh_body_id = p.loadURDF(
os.path.join(get_model_path(self.model_id), 'scene.urdf'))
def load_trav_map(self):
self.floor_map = []
self.floor_graph = []
for f in range(len(self.floors)):
trav_map = np.array(
Image.open(
os.path.join(get_model_path(self.model_id),
'floor_trav_{}.png'.format(f))))
obstacle_map = np.array(
Image.open(
os.path.join(get_model_path(self.model_id),
'floor_{}.png'.format(f))))
if self.trav_map_original_size is None:
height, width = trav_map.shape
assert height == width, 'trav map is not a square'
self.trav_map_original_size = height
self.trav_map_size = int(self.trav_map_original_size *
self.trav_map_default_resolution /
self.trav_map_resolution)
trav_map[obstacle_map == 0] = 0
trav_map = cv2.resize(trav_map,
(self.trav_map_size, self.trav_map_size))
trav_map = cv2.erode(
trav_map,
np.ones((self.trav_map_erosion, self.trav_map_erosion)))
trav_map[trav_map < 255] = 0
if self.build_graph:
graph_file = os.path.join(get_model_path(self.model_id),
'floor_trav_{}.p'.format(f))
if os.path.isfile(graph_file):
logging.info("Loading traversable graph")
with open(graph_file, 'rb') as pfile:
g = pickle.load(pfile)
else:
logging.info("Building traversable graph")
g = nx.Graph()
for i in range(self.trav_map_size):
for j in range(self.trav_map_size):
if trav_map[i, j] > 0:
g.add_node((i, j))
# 8-connected graph
neighbors = [(i - 1, j - 1), (i, j - 1),
(i + 1, j - 1), (i - 1, j)]
for n in neighbors:
if 0 <= n[0] < self.trav_map_size and 0 <= n[
1] < self.trav_map_size and trav_map[
n[0], n[1]] > 0:
g.add_edge(n, (i, j),
weight=l2_distance(
n, (i, j)))
# only take the largest connected component
largest_cc = max(nx.connected_components(g), key=len)
g = g.subgraph(largest_cc).copy()
with open(graph_file, 'wb') as pfile:
pickle.dump(g, pfile, protocol=pickle.HIGHEST_PROTOCOL)
self.floor_graph.append(g)
# update trav_map accordingly
trav_map[:, :] = 0
for node in g.nodes:
trav_map[node[0], node[1]] = 255
self.floor_map.append(trav_map)
def main():
global _mouse_ix, _mouse_iy, down, view_direction
if len(sys.argv) > 1:
model_path = sys.argv[1]
else:
model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj')
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)
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 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()
