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])