def main():
p.connect(p.GUI)
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 240.)
floor = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
p.loadMJCF(floor)
cabinet_0007 = os.path.join(gibson2.assets_path,
'models/cabinet2/cabinet_0007.urdf')
cabinet_0004 = os.path.join(gibson2.assets_path,
'models/cabinet/cabinet_0004.urdf')
obj1 = InteractiveObj(filename=cabinet_0007)
obj1.load()
obj1.set_position([0, 0, 0.5])
obj2 = InteractiveObj(filename=cabinet_0004)
obj2.load()
obj2.set_position([0, 0, 2])
obj3 = YCBObject('003_cracker_box')
obj3.load()
obj3.set_position_orientation([0, 0, 1.2], [0, 0, 0, 1])
for _ in range(24000): # at least 100 seconds
p.stepSimulation()
time.sleep(1. / 240.)
p.disconnect()
min_height = center_height
normal = normals[i]
non_z = abs(normal[0]) + abs(normal[1])
if non_z < 0.01 and normal[2] > 0.99:
if center_height not in height_counts:
height_counts[center_height] = 0
height_counts[center_height] += 1
heights = []
for height in height_counts:
if abs(height - min_height) > 0.05 and\
abs(height - max_height) > 0.05 and\
height_counts[height] > 1:
heights.append(float(height - min_height))
obj.set_position([0, 0, size[2] / 2])
aabb = pb.getAABB(body_id)
heights = sorted(heights)
shape = pb.createVisualShape(pb.GEOM_BOX,
rgbaColor=[1, 1, 1, 1],
halfExtents=[0.3, 0.3, 0.01],
visualFramePosition=[0.0, 0.0, 0.0])
body_id = pb.createMultiBody(baseVisualShapeIndex=shape,
baseCollisionShapeIndex=-1)
filtered_heights = []
for height in heights:
_, old_orn = pb.getBasePositionAndOrientation(body_id)
pb.resetBasePositionAndOrientation(body_id, [0.0, 0.0, height], old_orn)
inp = input('Is this a valid shelf height? (y for yes, else no)')
if inp == 'y':
filtered_heights.append(height)
config = parse_config('../configs/jr_interactive_nav.yaml')
s = Simulator(mode='headless', timestep=1 / 240.0)
scene = EmptyScene()
s.import_scene(scene)
jr = JR2_Kinova(config)
s.import_robot(jr)
jr.robot_body.reset_position([0, 0, 0])
jr.robot_body.reset_orientation([0, 0, 1, 0])
fetch = Fetch(config)
s.import_robot(fetch)
fetch.robot_body.reset_position([0, 1, 0])
fetch.robot_body.reset_orientation([0, 0, 1, 0])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)
obj.set_position([-2, 0, 0.5])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0007/part_objs/cabinet_0007.urdf')
s.import_interactive_object(obj)
obj.set_position([-2, 2, 0.5])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
s.import_interactive_object(obj)
obj.set_position([-2.1, 1.6, 2])
obj = InteractiveObj(
filename='/data4/mdv0/cabinet/0004/part_objs/cabinet_0004.urdf')
s.import_interactive_object(obj)
obj.set_position([-2.1, 0.4, 2])
obj = BoxShape([-2.05, 1, 0.5], [0.35, 0.6, 0.5])
s.import_interactive_object(obj)
obj = BoxShape([-2.45, 1, 1.5], [0.01, 2, 1.5])