def main():
config = parse_config('../configs/turtlebot_demo.yaml')
settings = MeshRendererSettings(enable_shadow=False, msaa=False)
s = Simulator(mode='gui',
image_width=256,
image_height=256,
rendering_settings=settings)
scene = StaticIndoorScene('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])
print(s.renderer.instances)
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_building_viewing():
download_assets()
download_demo_data()
config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml'))
s = Simulator(mode='headless')
scene = StaticIndoorScene('Rs')
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_igsdf(scene_name, scene_source):
hdr_texture = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background', 'probe_02.hdr')
hdr_texture2 = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background', 'probe_03.hdr')
if scene_source == "IG":
scene_dir = get_ig_scene_path(scene_name)
elif scene_source == "CUBICASA":
scene_dir = get_cubicasa_scene_path(scene_name)
else:
scene_dir = get_3dfront_scene_path(scene_name)
light_modulation_map_filename = os.path.join(
scene_dir, 'layout', 'floor_lighttype_0.png')
background_texture = os.path.join(
gibson2.ig_dataset_path, 'scenes', 'background',
'urban_street_01.jpg')
scene = InteractiveIndoorScene(
scene_name,
texture_randomization=False,
object_randomization=False,
scene_source=scene_source)
settings = MeshRendererSettings(env_texture_filename=hdr_texture,
env_texture_filename2=hdr_texture2,
env_texture_filename3=background_texture,
light_modulation_map_filename=light_modulation_map_filename,
enable_shadow=True, msaa=True,
light_dimming_factor=1.0)
s = Simulator(mode='iggui', image_width=960,
image_height=720, device_idx=0, rendering_settings=settings)
s.import_ig_scene(scene)
fpss = []
np.random.seed(0)
_,(px,py,pz) = scene.get_random_point()
s.viewer.px = px
s.viewer.py = py
s.viewer.pz = 1.7
s.viewer.update()
for i in range(3000):
if i == 2500:
logId = p.startStateLogging(loggingType=p.STATE_LOGGING_PROFILE_TIMINGS, fileName='trace_beechwood')
start = time.time()
s.step()
end = time.time()
print("Elapsed time: ", end - start)
print("Frequency: ", 1 / (end - start))
fpss.append(1 / (end - start))
p.stopStateLogging(logId)
s.disconnect()
print("end")
plt.plot(fpss)
plt.show()
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_import_building():
download_assets()
download_demo_data()
s = Simulator(mode='headless')
scene = StaticIndoorScene('Rs')
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():
s = Simulator(mode='gui', image_width=512,
image_height=512, device_idx=0)
scene = InteractiveIndoorScene(
'Rs_int', texture_randomization=True, object_randomization=False)
s.import_ig_scene(scene)
for i in range(10000):
if i % 1000 == 0:
scene.randomize_texture()
s.step()
s.disconnect()
def main():
s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0)
scene = InteractiveIndoorScene('Rs_int',
texture_randomization=False,
object_randomization=False,
load_object_categories=['chair'],
load_room_types=['living_room'])
s.import_ig_scene(scene)
for _ in range(1000):
s.step()
s.disconnect()
def main():
s = Simulator(mode='gui', image_width=512, image_height=512, device_idx=0)
for random_seed in range(10):
scene = InteractiveIndoorScene('Rs_int',
texture_randomization=False,
object_randomization=True,
object_randomization_idx=random_seed)
s.import_ig_scene(scene)
for i in range(1000):
s.step()
s.reload()
s.disconnect()
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 main():
s = Simulator(mode='gui', image_width=512,
image_height=512, device_idx=0)
scene = InteractiveIndoorScene(
'Rs_int', texture_randomization=False, object_randomization=False)
s.import_ig_scene(scene)
np.random.seed(0)
for _ in range(10):
pt = scene.get_random_point_by_room_type('living_room')[1]
print('random point in living_room', pt)
for _ in range(1000):
s.step()
s.disconnect()
def run_demo(self):
config = parse_config(
os.path.join(os.path.dirname(gibson2.__file__),
'../examples/configs/turtlebot_demo.yaml'))
s = Simulator(mode='gui', image_width=700, image_height=700)
scene = StaticIndoorScene('Rs', pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for i in range(1000):
turtlebot.apply_action([0.1, 0.5])
s.step()
s.disconnect()
def test_simulator():
download_assets()
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 debug_renderer_scaling():
s = Simulator(mode='gui',
image_width=512,
image_height=512,
physics_timestep=1 / float(100))
scene = EmptyScene()
s.import_scene(scene, render_floor_plane=True)
urdf_path = '/cvgl2/u/chengshu/ig_dataset_v5/objects/window/103070/103070_avg_size_0.urdf'
obj = ArticulatedObject(urdf_path)
s.import_object(obj)
obj.set_position([0, 0, 0])
embed()
z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]])
obj.set_position([0, 0, z])
embed()
for _ in range(100000000000):
s.step()
def main():
parser = argparse.ArgumentParser()
parser.add_argument('--scene',
type=str,
help='Name of the scene in the iG Dataset')
args = parser.parse_args()
settings = MeshRendererSettings(enable_shadow=True, msaa=False)
s = Simulator(mode='gui',
image_width=256,
image_height=256,
rendering_settings=settings)
scene = iGSDFScene(args.scene)
s.import_ig_scene(scene)
for i in range(10000):
with Profiler('Simulator step'):
s.step()
s.disconnect()
def generate_trav_map(scene_name, scene_source, load_full_scene=True):
if scene_source not in SCENE_SOURCE:
raise ValueError('Unsupported scene source: {}'.format(scene_source))
if scene_source == "IG":
scene_dir = get_ig_scene_path(scene_name)
elif scene_source == "CUBICASA":
scene_dir = get_cubicasa_scene_path(scene_name)
else:
scene_dir = get_3dfront_scene_path(scene_name)
random.seed(0)
scene = InteractiveIndoorScene(scene_name,
build_graph=False,
texture_randomization=False,
scene_source=scene_source)
if not load_full_scene:
scene._set_first_n_objects(3)
s = Simulator(mode='headless',
image_width=512,
image_height=512,
device_idx=0)
s.import_ig_scene(scene)
if load_full_scene:
scene.open_all_doors()
for i in range(20):
s.step()
vertices_info, faces_info = s.renderer.dump()
s.disconnect()
if load_full_scene:
trav_map_filename_format = 'floor_trav_{}.png'
obstacle_map_filename_format = 'floor_{}.png'
else:
trav_map_filename_format = 'floor_trav_no_obj_{}.png'
obstacle_map_filename_format = 'floor_no_obj_{}.png'
gen_trav_map(vertices_info,
faces_info,
output_folder=os.path.join(scene_dir, 'layout'),
trav_map_filename_format=trav_map_filename_format,
obstacle_map_filename_format=obstacle_map_filename_format)
class ToyEnv(object):
def __init__(self):
config = parse_config('../../configs/turtlebot_demo.yaml')
hdr_texture = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'probe_02.hdr')
hdr_texture2 = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'probe_03.hdr')
light_modulation_map_filename = os.path.join(gibson2.ig_dataset_path,
'scenes', 'Rs_int',
'layout',
'floor_lighttype_0.png')
background_texture = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'urban_street_01.jpg')
settings = MeshRendererSettings(enable_shadow=False, enable_pbr=False)
self.s = Simulator(mode='headless',
image_width=400,
image_height=400,
rendering_settings=settings)
scene = StaticIndoorScene('Rs')
self.s.import_scene(scene)
#self.s.import_ig_scene(scene)
self.robot = Turtlebot(config)
self.s.import_robot(self.robot)
for _ in range(5):
obj = YCBObject('003_cracker_box')
self.s.import_object(obj)
obj.set_position_orientation(
np.random.uniform(low=0, high=2, size=3), [0, 0, 0, 1])
print(self.s.renderer.instances)
def step(self, a):
self.robot.apply_action(a)
self.s.step()
frame = self.s.renderer.render_robot_cameras(modes=('rgb'))[0]
return frame
def close(self):
self.s.disconnect()
def test_import_igsdf():
scene = InteractiveIndoorScene('Rs_int',
texture_randomization=False,
object_randomization=False)
s = Simulator(mode='headless',
image_width=512,
image_height=512,
device_idx=0)
s.import_ig_scene(scene)
s.renderer.use_pbr(use_pbr=True, use_pbr_mapping=True)
for i in range(10):
# if i % 100 == 0:
# scene.randomize_texture()
start = time.time()
s.step()
end = time.time()
print("Elapsed time: ", end - start)
print("Frequency: ", 1 / (end - start))
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 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):
s.step()
s.disconnect()
assert nbody == 7
def benchmark_rendering(scene_list, rendering_presets_list, modality_list):
config = parse_config(os.path.join(gibson2.root_path, '../test/test.yaml'))
assets_version = get_ig_assets_version()
print('assets_version', assets_version)
result = {}
for scene_name in scene_list:
for rendering_preset in rendering_presets_list:
scene = InteractiveIndoorScene(scene_name,
texture_randomization=False,
object_randomization=False)
settings = NamedRenderingPresets[rendering_preset]
if rendering_preset == 'VISUAL_RL':
image_width = 128
image_height = 128
else:
image_width = 512
image_height = 512
s = Simulator(mode='headless',
image_width=image_width,
image_height=image_height,
device_idx=0,
rendering_settings=settings,
physics_timestep=1 / 240.0)
s.import_ig_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for mode in modality_list:
for _ in range(10):
s.step()
_ = s.renderer.render_robot_cameras(modes=(mode))
start = time.time()
for _ in range(200):
_ = s.renderer.render_robot_cameras(modes=(mode))
end = time.time()
fps = 200 / (end - start)
result[(scene_name, rendering_preset, mode)] = fps
s.disconnect()
return result
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 = Cube(curr[0], curr[1])
s.import_object(obj)
for i in range(len(obstacles)):
curr = obstacles[i]
obj = Cube(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_import_rbo_object():
s = Simulator(mode='headless')
try:
scene = StadiumScene()
s.import_scene(scene)
obj = RBOObject('book')
s.import_object(obj)
obj2 = RBOObject('microwave')
s.import_object(obj2)
obj.set_position([0, 0, 2])
obj2.set_position([0, 1, 2])
obj3 = ArticulatedObject(
os.path.join(gibson2.assets_path, 'models', 'scene_components',
'door.urdf'))
s.import_object(obj3)
for i in range(100):
s.step()
finally:
s.disconnect()
def main():
config = parse_config('../configs/turtlebot_demo.yaml')
settings = MeshRendererSettings()
s = Simulator(mode='gui',
image_width=256,
image_height=256,
rendering_settings=settings)
scene = StaticIndoorScene('Rs',
build_graph=True,
pybullet_load_texture=True)
s.import_scene(scene)
turtlebot = Turtlebot(config)
s.import_robot(turtlebot)
for i in range(10000):
with Profiler('Simulator step'):
turtlebot.apply_action([0.1, -0.1])
s.step()
lidar = s.renderer.get_lidar_all()
print(lidar.shape)
# TODO: visualize lidar scan
s.disconnect()
def render_physics_gifs(main_urdf_file_and_offset):
step_per_sec = 100
s = Simulator(mode='headless',
image_width=512,
image_height=512,
physics_timestep=1 / float(step_per_sec))
root_dir = '/cvgl2/u/chengshu/ig_dataset_v5/objects'
obj_count = 0
for i, obj_class_dir in enumerate(sorted(os.listdir(root_dir))):
obj_class = obj_class_dir
# if obj_class not in SELECTED_CLASSES:
# continue
obj_class_dir = os.path.join(root_dir, obj_class_dir)
for obj_inst_dir in os.listdir(obj_class_dir):
# if obj_inst_dir != '14402':
# continue
imgs = []
scene = EmptyScene()
s.import_scene(scene, render_floor_plane=True)
obj_inst_name = obj_inst_dir
# urdf_path = obj_inst_name + '.urdf'
obj_inst_dir = os.path.join(obj_class_dir, obj_inst_dir)
urdf_path, offset = main_urdf_file_and_offset[obj_inst_dir]
# urdf_path = os.path.join(obj_inst_dir, urdf_path)
# print('urdf_path', urdf_path)
obj = ArticulatedObject(urdf_path)
s.import_object(obj)
push_visual_marker = VisualMarker(radius=0.1)
s.import_object(push_visual_marker)
push_visual_marker.set_position([100, 100, 0.0])
z = stable_z_on_aabb(obj.body_id, [[0, 0, 0], [0, 0, 0]])
# offset is translation from the bounding box center to the base link frame origin
# need to add another offset that is the translation from the base link frame origin to the inertial frame origin
# p.resetBasePositionAndOrientation() sets the inertial frame origin instead of the base link frame origin
# Assuming the bounding box center is at (0, 0, z) where z is half of the extent in z-direction
base_link_to_inertial = p.getDynamicsInfo(obj.body_id, -1)[3]
obj.set_position([
offset[0] + base_link_to_inertial[0],
offset[1] + base_link_to_inertial[1], z
])
center, extent = get_center_extent(obj.body_id)
# the bounding box center should be at (0, 0) on the xy plane and the bottom of the bounding box should touch the ground
if not (np.linalg.norm(center[:2]) < 1e-3
and np.abs(center[2] - extent[2] / 2.0) < 1e-3):
print('-' * 50)
print('WARNING:', urdf_path, 'xy error',
np.linalg.norm(center[:2]), 'z error',
np.abs(center[2] - extent[2] / 2.0))
height = extent[2]
max_half_extent = max(extent[0], extent[1]) / 2.0
px = max_half_extent * 2
py = max_half_extent * 2
pz = extent[2] * 1.2
camera_pose = np.array([px, py, pz])
# camera_pose = np.array([0.01, 0.01, 3])
s.renderer.set_camera(camera_pose, [0, 0, 0], [0, 0, 1])
# drop 1 second
for _ in range(step_per_sec):
s.step()
frame = s.renderer.render(modes=('rgb'))
imgs.append(
Image.fromarray(
(frame[0][:, :, :3] * 255).astype(np.uint8)))
ray_start = [max_half_extent * 1.5, max_half_extent * 1.5, 0]
ray_end = [-100.0, -100.0, 0]
unit_force = np.array([-1.0, -1.0, 0.0])
unit_force /= np.linalg.norm(unit_force)
force_mag = 100.0
ray_zs = [height * 0.5]
valid_ray_z = False
for trial in range(5):
for ray_z in ray_zs:
ray_start[2] = ray_z
ray_end[2] = ray_z
res = p.rayTest(ray_start, ray_end)
if res[0][0] == obj.body_id:
valid_ray_z = True
break
if valid_ray_z:
break
increment = 1 / (2**(trial + 1))
ray_zs = np.arange(increment / 2.0, 1.0, increment) * height
# push 4 seconds
for i in range(step_per_sec * 4):
res = p.rayTest(ray_start, ray_end)
object_id, link_id, _, hit_pos, hit_normal = res[0]
if object_id != obj.body_id:
break
push_visual_marker.set_position(hit_pos)
p.applyExternalForce(object_id, link_id,
unit_force * force_mag, hit_pos,
p.WORLD_FRAME)
s.step()
# print(hit_pos)
frame = s.renderer.render(modes=('rgb'))
rgb = frame[0][:, :, :3]
# add red border
border_width = 10
border_color = np.array([1.0, 0.0, 0.0])
rgb[:border_width, :, :] = border_color
rgb[-border_width:, :, :] = border_color
rgb[:, :border_width, :] = border_color
rgb[:, -border_width:, :] = border_color
imgs.append(Image.fromarray((rgb * 255).astype(np.uint8)))
gif_path = '{}/visualizations/{}_cm_physics_v3.gif'.format(
obj_inst_dir, obj_inst_name)
imgs = imgs[::4]
imgs[0].save(gif_path,
save_all=True,
append_images=imgs[1:],
optimize=True,
duration=40,
loop=0)
obj_count += 1
# print(obj_count, gif_path, len(imgs), valid_ray_z, ray_z)
print(obj_count, gif_path)
s.reload()
def main():
config = parse_config('../configs/fetch_p2p_nav.yaml')
s = Simulator(mode='gui', physics_timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
fetch = Fetch(config)
s.import_robot(fetch)
robot_id = fetch.robot_ids[0]
arm_joints = joints_from_names(robot_id, [
'torso_lift_joint', 'shoulder_pan_joint', 'shoulder_lift_joint',
'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint',
'wrist_flex_joint', 'wrist_roll_joint'
])
#finger_joints = joints_from_names(robot_id, ['l_gripper_finger_joint', 'r_gripper_finger_joint'])
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
x, y, z = fetch.get_end_effector_position()
#set_joint_positions(robot_id, finger_joints, [0.04,0.04])
print(x, y, z)
visual_marker = p.createVisualShape(p.GEOM_SPHERE, radius=0.02)
marker = p.createMultiBody(baseVisualShapeIndex=visual_marker)
#max_limits = [0,0] + get_max_limits(robot_id, arm_joints) + [0.05,0.05]
#min_limits = [0,0] + get_min_limits(robot_id, arm_joints) + [0,0]
#rest_position = [0,0] + list(get_joint_positions(robot_id, arm_joints)) + [0.04,0.04]
max_limits = [0, 0] + get_max_limits(robot_id, arm_joints)
min_limits = [0, 0] + get_min_limits(robot_id, arm_joints)
rest_position = [0, 0] + list(get_joint_positions(robot_id, arm_joints))
joint_range = list(np.array(max_limits) - np.array(min_limits))
joint_range = [item + 1 for item in joint_range]
jd = [0.1 for item in joint_range]
print(max_limits)
print(min_limits)
def accurateCalculateInverseKinematics(robotid, endEffectorId, targetPos,
threshold, maxIter):
sample_fn = get_sample_fn(robotid, arm_joints)
set_joint_positions(robotid, arm_joints, sample_fn())
it = 0
while it < maxIter:
jointPoses = p.calculateInverseKinematics(robotid,
endEffectorId,
targetPos,
lowerLimits=min_limits,
upperLimits=max_limits,
jointRanges=joint_range,
restPoses=rest_position,
jointDamping=jd)
set_joint_positions(robotid, arm_joints, jointPoses[2:10])
ls = p.getLinkState(robotid, endEffectorId)
newPos = ls[4]
dist = np.linalg.norm(np.array(targetPos) - np.array(newPos))
if dist < threshold:
break
it += 1
print("Num iter: " + str(it) + ", threshold: " + str(dist))
return jointPoses
while True:
with Profiler("Simulation step"):
fetch.robot_body.reset_position([0, 0, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
threshold = 0.01
maxIter = 100
joint_pos = accurateCalculateInverseKinematics(
robot_id, fetch.parts['gripper_link'].body_part_index,
[x, y, z], threshold, maxIter)[2:10]
#set_joint_positions(robot_id, finger_joints, [0.04, 0.04])
s.step()
keys = p.getKeyboardEvents()
for k, v in keys.items():
if (k == p.B3G_RIGHT_ARROW and (v & p.KEY_IS_DOWN)):
x += 0.01
if (k == p.B3G_LEFT_ARROW and (v & p.KEY_IS_DOWN)):
x -= 0.01
if (k == p.B3G_UP_ARROW and (v & p.KEY_IS_DOWN)):
y += 0.01
if (k == p.B3G_DOWN_ARROW and (v & p.KEY_IS_DOWN)):
y -= 0.01
if (k == ord('z') and (v & p.KEY_IS_DOWN)):
z += 0.01
if (k == ord('x') and (v & p.KEY_IS_DOWN)):
z -= 0.01
p.resetBasePositionAndOrientation(marker, [x, y, z], [0, 0, 0, 1])
s.disconnect()
def main():
config = parse_config(
os.path.join(gibson2.example_config_path,
'turtlebot_demo.yaml')) #robot configuration file path
# settings = MeshRendererSettings() #generating renderer settings object
#generating simulator object
#s = Simulator(mode='headless', #simulating without gui
s = Simulator(
mode='headless', #simulating with gui
# image_width=256, #robot camera pixel width
# image_height=256, #robot camera pixel height
# vertical_fov=40, #robot camera view angle (from floor to ceiling 40 degrees)
)
#rendering_settings=settings)
#generating scene object
scene = InteractiveIndoorScene(
'Benevolence_1_int', #scene name: Benevolence, floor number: 1, does it include interactive objects: yes (int). I pick Benevolence on purpose as it is the most memory friendly scene in the iGibson dataset.
build_graph=
True, #builds the connectivity graph over the given facedown traversibility map (floor plan)
waypoint_resolution=
0.1, #radial distance between 2 consecutive waypoints (10 cm)
pybullet_load_texture=True
) #do you want to include texture and material properties? (you need this for object interaction)
s.import_ig_scene(scene) #loading the scene object in the simulator object
turtlebot = Turtlebot(config) #generating the robot object
s.import_robot(
turtlebot) #loading the robot object in the simulator object
init_pos = turtlebot.get_position(
) #getting the initial position of the robot base [X:meters, Y:meters, Z:meters] (base: robot's main body. it may have links and the associated joints too. links and joints have positions and orientations as well.)
init_or = turtlebot.get_rpy(
) #getting the initial Euler angles of the robot base [Roll: radians, Pitch: radians, Yaw: radians]
#sampling random goal states in a desired room of the apartment
np.random.seed(0)
goal = scene.get_random_point_by_room_type('living_room')[
1] #sampling a random point in the living room
rnd_path = scene.get_shortest_path(
0, init_pos[0:2], goal[0:2], entire_path=True
)[0] #generate the "entire" a* path between the initial and goal nodes
for i in range(len(rnd_path[0]) - 1):
with Profiler(
'Simulator step'
): #iGibson simulation loop requieres this context manager
rgb_camera = np.array(
s.renderer.render_robot_cameras(modes='rgb')
) #probing RGB data, you can also probe rgbd or even optical flow if robot has that property in its config file (.yaml file)
plt.imshow(rgb_camera[0, :, :, 3]) #calling sampled RGB data
lidar = s.renderer.get_lidar_all() #probing 360 degrees lidar data
delta_pos = smt_path[:, i +
1] - smt_path[:,
i] #direction vector between 2 consucutive waypoints
delta_yaw = np.arctan2(
delta_pos[1], delta_pos[0]
) #the yaw angle of the robot base while following the sampled bezier path
delta_qua = e2q(
init_or[0], init_or[1],
delta_yaw) #transforming robot base Euler angles to quaternion
turtlebot.set_position_orientation([
smt_path[0, i], smt_path[1, i], init_pos[2]
], delta_qua) #setting the robot base position and the orientation
s.step() #proceed one step ahead in simulation time
s.disconnect()
class BaseEnv(gym.Env):
'''
Base Env class, follows OpenAI Gym interface
Handles loading scene and robot
Functions like reset and step are not implemented
'''
def __init__(self,
config_file,
scene_id=None,
mode='headless',
action_timestep=1 / 10.0,
physics_timestep=1 / 240.0,
render_to_tensor=False,
device_idx=0):
"""
:param config_file: config_file path
:param scene_id: override scene_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 scene_id is not None:
self.config['scene_id'] = scene_id
self.mode = mode
self.action_timestep = action_timestep
self.physics_timestep = physics_timestep
self.texture_randomization_freq = self.config.get(
'texture_randomization_freq', None)
self.object_randomization_freq = self.config.get(
'object_randomization_freq', None)
self.object_randomization_idx = 0
self.num_object_randomization_idx = 10
enable_shadow = self.config.get('enable_shadow', False)
enable_pbr = self.config.get('enable_pbr', True)
texture_scale = self.config.get('texture_scale', 1.0)
settings = MeshRendererSettings(enable_shadow=enable_shadow,
enable_pbr=enable_pbr,
msaa=False,
texture_scale=texture_scale)
self.simulator = Simulator(
mode=mode,
physics_timestep=physics_timestep,
render_timestep=action_timestep,
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,
render_to_tensor=render_to_tensor,
rendering_settings=settings)
self.load()
def reload(self, config_file):
"""
Reload another config file
Thhis allows one to change the configuration 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, scene_id):
"""
Reload another scene model
This allows one to change the scene on the fly
:param scene_id: new scene_id
"""
self.config['scene_id'] = scene_id
self.simulator.reload()
self.load()
def reload_model_object_randomization(self):
"""
Reload the same model, with the next object randomization random seed
"""
if self.object_randomization_freq is None:
return
self.object_randomization_idx = (self.object_randomization_idx + 1) % \
(self.num_object_randomization_idx)
self.simulator.reload()
self.load()
def get_next_scene_random_seed(self):
"""
Get the next scene random seed
"""
if self.object_randomization_freq is None:
return None
return self.scene_random_seeds[self.scene_random_seed_idx]
def load(self):
"""
Load the scene and robot
"""
if self.config['scene'] == 'empty':
scene = EmptyScene()
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
elif self.config['scene'] == 'stadium':
scene = StadiumScene()
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
elif self.config['scene'] == 'gibson':
scene = StaticIndoorScene(
self.config['scene_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),
pybullet_load_texture=self.config.get('pybullet_load_texture',
False),
)
self.simulator.import_scene(scene,
load_texture=self.config.get(
'load_texture', True))
elif self.config['scene'] == 'igibson':
scene = InteractiveIndoorScene(
self.config['scene_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),
trav_map_type=self.config.get('trav_map_type', 'with_obj'),
pybullet_load_texture=self.config.get('pybullet_load_texture',
False),
texture_randomization=self.texture_randomization_freq
is not None,
object_randomization=self.object_randomization_freq
is not None,
object_randomization_idx=self.object_randomization_idx,
should_open_all_doors=self.config.get('should_open_all_doors',
False),
load_object_categories=self.config.get(
'load_object_categories', None),
load_room_types=self.config.get('load_room_types', None),
load_room_instances=self.config.get('load_room_instances',
None),
)
# TODO: Unify the function import_scene and take out of the if-else clauses
first_n = self.config.get('_set_first_n_objects', -1)
if first_n != -1:
scene._set_first_n_objects(first_n)
self.simulator.import_ig_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)
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 close(self):
"""
Synonymous function with clean
"""
self.clean()
def simulator_step(self):
"""
Step the simulation.
This is different from environment step that returns the next
observation, reward, done, info.
"""
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):
"""
Set simulator mode
"""
self.simulator.mode = mode
class ToyEnvInt(object):
def __init__(self, robot='turtlebot', scene='Rs_int'):
config = parse_config('../../configs/turtlebot_demo.yaml')
hdr_texture = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'probe_02.hdr')
hdr_texture2 = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'probe_03.hdr')
light_modulation_map_filename = os.path.join(gibson2.ig_dataset_path,
'scenes', 'Rs_int',
'layout',
'floor_lighttype_0.png')
background_texture = os.path.join(gibson2.ig_dataset_path, 'scenes',
'background', 'urban_street_01.jpg')
scene = InteractiveIndoorScene(scene,
texture_randomization=False,
object_randomization=False)
#scene._set_first_n_objects(5)
scene.open_all_doors()
settings = MeshRendererSettings(
env_texture_filename=hdr_texture,
env_texture_filename2=hdr_texture2,
env_texture_filename3=background_texture,
light_modulation_map_filename=light_modulation_map_filename,
enable_shadow=True,
msaa=True,
light_dimming_factor=1.0,
optimized=True)
self.s = Simulator(mode='headless',
image_width=400,
image_height=400,
rendering_settings=settings)
self.s.import_ig_scene(scene)
if robot == 'turtlebot':
self.robot = Turtlebot(config)
else:
self.robot = Fetch(config)
self.s.import_robot(self.robot)
for _ in range(5):
obj = YCBObject('003_cracker_box')
self.s.import_object(obj)
obj.set_position_orientation(
np.random.uniform(low=0, high=2, size=3), [0, 0, 0, 1])
print(self.s.renderer.instances)
def step(self, a):
action = np.zeros(self.robot.action_space.shape)
if isinstance(self.robot, Turtlebot):
action[0] = a[0]
action[1] = a[1]
else:
action[1] = a[0]
action[0] = a[1]
self.robot.apply_action(action)
self.s.step()
frame = self.s.renderer.render_robot_cameras(modes=('rgb'))[0]
return frame
def close(self):
self.s.disconnect()
##################################################################################################################
##tried changing robot control to position and torque but failed,from the documentation it seems turtle bot only##
############################################supports joint velocity###############################################
##################################################################################################################
#robot.load()
#robot.set_position(position)
#robot.robot_specific_reset()
#robot.keep_still()
#print(robot.action_dim)
#print(robot.control)
############################################adding keyboard functionality#####################################
#print(p.getKeyboardEvents())
##############################################start the simulation############################################
for i in range(360):
#p.stepSimulation()
s.step()
#time.sleep(1./360.)
#for i in range(1):
#action = np.random.uniform(-1, 1, robot.action_dim)
#print(action)
#action=np.array([0,0.5])
#turtlebot.apply_action(action)
#print(robot.get_angular_velocity())
#robot.move_forward(forward=0.5)
#p.stepSimulation()
#s.step()
#robot.keep_still()
#time.sleep(1./360.)
for i in range(1000):
#with Profiler('Simulator step'):