def __init__(self, *args): super(FiveBlocks, self).__init__(*args) connect(use_gui=self.experiment_dict['render']) if (self.detailed_gmp): self.robot = p.loadURDF( DRAKE_IIWA_URDF, useFixedBase=True, globalScaling=1.2) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF else: self.robot = None # Construct camera set_default_camera() # Load in the objects self.floor = p.loadURDF('models/short_floor.urdf', useFixedBase=True) self.green_block = p.loadURDF("models/box_green.urdf", useFixedBase=False) self.red_block = p.loadURDF("models/box_red.urdf", useFixedBase=False) self.blue_block = p.loadURDF("models/box_blue.urdf", useFixedBase=False) self.purple_block = p.loadURDF("models/box_purple.urdf", useFixedBase=False) self.cyan_block = p.loadURDF("models/box_yellow.urdf", useFixedBase=False) self.objects = [ self.green_block, self.red_block, self.blue_block, self.purple_block, self.cyan_block ] self.static_objects = [] # Only used for some curiosity types self.perspectives = [(0, -90)] # In this environment, if it times out, we know an object fell off the screen self.break_on_timeout = True # Set up macro-actions self.macroaction = MacroAction( macroaction_list=[ PickPlace(objects=self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), # AddLink(objects = self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), ], experiment_dict=self.experiment_dict) # Config state attributes self.config_state_attrs() p.setGravity(0, 0, -10) p.stepSimulation(physicsClientId=0)
def __init__(self, *args): super(TwoBlocks, self).__init__(*args) connect(use_gui=self.experiment_dict['render']) if (self.detailed_gmp): self.robot = p.loadURDF( DRAKE_IIWA_URDF, useFixedBase=True, globalScaling=1.2) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF else: self.robot = None # Construct camera set_default_camera() # Set up objects self.floor = p.loadURDF('models/short_floor.urdf', useFixedBase=True) self.green_block = p.loadURDF("models/box_green.urdf", useFixedBase=False) self.red_block = p.loadURDF("models/box_red.urdf", useFixedBase=False) self.objects = [self.green_block, self.red_block] self.static_objects = [] # Only used for some curiosity types self.perspectives = [(0, -90)] # Set up the state space and action space for this task self.break_on_timeout = True self.macroaction = MacroAction( macroaction_list=[ PickPlace(objects=self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), # AddLink(objects = self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), ], experiment_dict=self.experiment_dict) # Config state attributes self.config_state_attrs() # Start the simulation p.setGravity(0, 0, -10) p.stepSimulation(physicsClientId=0)
def __init__(self, *args): super(PulleySeesaw, self).__init__(*args) self.experiment_dict['reachable_max_height'] = 1.2 self.restore_state = True self.training = not self.experiment_dict['render'] connect(use_gui=self.experiment_dict['render']) if (self.detailed_gmp): self.robot = p.loadURDF( DRAKE_IIWA_URDF, useFixedBase=True, globalScaling=1.2) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF else: self.robot = None self.KNOT = 0.1 self.NOKNOT = 0 self.break_on_timeout = True self.config_size = 20 self.ARM_LEN = 1.6 self.predict_mask = list(range(self.config_size - 5)) x_scene_offset = 0.9 num_small_blue = 5 self.perspectives = [(0, -90)] self.pulley = p.loadSDF("models/pulley/newsdf.sdf", globalScaling=2.5) for pid in self.pulley: set_pose( pid, Pose(Point(x=x_scene_offset - 0.19, y=-0.15, z=1.5), Euler(roll=math.pi / 2.0, pitch=0, yaw=1.9))) self.floor = p.loadURDF('models/short_floor.urdf', useFixedBase=True) self.seesaw = p.loadURDF("models/seesaw.urdf", useFixedBase=True) self.seesaw_joint = 1 set_pose(self.seesaw, Pose(Point(x=x_scene_offset + 0.8, y=0, z=0))) self.block_pos = [x_scene_offset - 1, -0.8, 0.05] self.black_block = p.loadURDF("models/box_heavy.urdf", useFixedBase=True) set_pose( self.black_block, Pose( Point(x=self.block_pos[0], y=self.block_pos[1], z=self.block_pos[2]))) self.objects = [] small_boxes = [ "small_blue_heavy", "small_purple_heavy", "small_green_heavy", "small_red_heavy", "small_yellow_heavy" ] for i in range(num_small_blue): self.objects.append( p.loadURDF("models/" + str(small_boxes[i]) + ".urdf", useFixedBase=False)) self.red_block = p.loadURDF("models/box_red.urdf", useFixedBase=False) set_pose(self.red_block, Pose(Point(x=x_scene_offset + 1.7, y=0, z=0.5))) self.useMaximalCoordinates = True sphereRadius = 0.01 self.mass = 1 self.basePosition = [x_scene_offset - 0.2, -1.7, 1.6] self.baseOrientation = [0, 0, 0, 1] if (self.training): p.setGravity(0, 0, -10) self.cup = p.loadURDF("models/cup/cup_small.urdf", useFixedBase=True) self.sphereUid = self.cup set_pose(self.cup, Pose(Point(x=x_scene_offset - 0.2, y=0.1, z=1))) self.knot = self.KNOT else: self.cupCollideId = p.createCollisionShape( p.GEOM_MESH, fileName="models/cup/Cup/cup_vhacd.obj", meshScale=[6, 6, 1.5], collisionFrameOrientation=p.getQuaternionFromEuler( [math.pi / 2.0, 0, 0]), collisionFramePosition=[0.07, 0.3, 0]) self.cupShapeId = p.createVisualShape( p.GEOM_MESH, fileName="models/cup/Cup/textured-0008192.obj", meshScale=[6, 6, 1.5], rgbaColor=[1, 0.886, 0.552, 1], visualFrameOrientation=p.getQuaternionFromEuler( [math.pi / 2.0, 0, 0]), visualFramePosition=[0.07, 0.3, 0]) self.colBoxId = p.createCollisionShape( p.GEOM_CYLINDER, radius=sphereRadius, height=0.03, halfExtents=[sphereRadius, sphereRadius, sphereRadius], collisionFrameOrientation=p.getQuaternionFromEuler( [math.pi / 2.0, 0, 0])) self.visualcolBoxId = p.createVisualShape( p.GEOM_CYLINDER, radius=sphereRadius, length=0.03, halfExtents=[sphereRadius, sphereRadius, sphereRadius], rgbaColor=[1, 0.886, 0.552, 1], visualFrameOrientation=p.getQuaternionFromEuler( [math.pi / 2.0, 0, 0])) # visualShapeId = -1 self.link_Masses = [] self.linkCollisionShapeIndices = [] self.linkVisualShapeIndices = [] self.linkPositions = [] self.linkOrientations = [] self.linkInertialFramePositions = [] self.linkInertialFrameOrientations = [] self.indices = [] self.jointTypes = [] self.axis = [] numel = 70 for i in range(numel): self.link_Masses.append(0.3) self.linkCollisionShapeIndices.append(self.colBoxId) self.linkVisualShapeIndices.append(self.visualcolBoxId) self.linkPositions.append([0, sphereRadius * 2.0 + 0.01, 0]) self.linkOrientations.append([0, 0, 0, 1]) self.linkInertialFramePositions.append([0, 0, 0]) self.linkInertialFrameOrientations.append([0, 0, 0, 1]) self.indices.append(i) self.jointTypes.append(p.JOINT_FIXED) self.axis.append([0, 0, 1]) self.link_Masses.append(30) self.linkCollisionShapeIndices.append(self.cupCollideId) self.linkVisualShapeIndices.append(self.cupShapeId) self.linkPositions.append([0, 0, 0]) self.linkOrientations.append([0, 0, 0, 1]) self.linkInertialFramePositions.append([0, 0, 0]) self.linkInertialFrameOrientations.append([0, 0, 0, 1]) self.indices.append(numel) self.jointTypes.append(p.JOINT_FIXED) self.axis.append([0, 0, 1]) self.sphereUid = p.createMultiBody( self.mass, -1, -1, self.basePosition, self.baseOrientation, linkMasses=self.link_Masses, linkCollisionShapeIndices=self.linkCollisionShapeIndices, linkVisualShapeIndices=self.linkVisualShapeIndices, linkPositions=self.linkPositions, linkOrientations=self.linkOrientations, linkInertialFramePositions=self.linkInertialFramePositions, linkInertialFrameOrientations=self. linkInertialFrameOrientations, linkParentIndices=self.indices, linkJointTypes=self.jointTypes, linkJointAxis=self.axis, useMaximalCoordinates=self.useMaximalCoordinates) p.setRealTimeSimulation(0) self.anistropicFriction = [0.00, 0.00, 0.00] p.changeDynamics(self.sphereUid, -1, lateralFriction=0, anisotropicFriction=self.anistropicFriction) if (self.restore_state): self.keystone = p.loadURDF("models/tiny_green.urdf") p.setGravity(0, 0, -10) if (self.restore_state): saved_world = p.restoreState( fileName="./temp/pulley_start_state.bullet") else: for j in range(100): p.stepSimulation(physicsClientId=0) self.knot = p.createConstraint(self.black_block, -1, self.sphereUid, -1, p.JOINT_FIXED, jointAxis=[0, 0, 0], parentFramePosition=self.block_pos, childFramePosition=self.block_pos) if (not self.restore_state): for j in range(3000): p.stepSimulation(physicsClientId=0) if (not self.restore_state): self.keystone = p.loadURDF("models/tiny_green.urdf") set_pose(self.keystone, Pose(Point(x=0.7, y=0, z=0.9))) for j in range(1000): p.stepSimulation(physicsClientId=0) saved_world = p.saveState() p.saveBullet("./temp/pulley_start_state.bullet") self.static_objects = [ self.red_block, self.black_block, self.sphereUid ] self.macroaction = MacroAction( [ PickPlace(objects=self.objects, robot=self.robot, fixed=self.fixed_objects, gmp=self.detailed_gmp), # AddLink(objects = self.objects, robot=self.robot, fixed=self.fixed_objects, gmp=self.detailed_gmp), ], experiment_dict=self.experiment_dict) self.action_space_size = self.macroaction.action_space_size self.config_size = 6 * 6 + len( self.macroaction.link_status) # (4 for links) self.action_space = spaces.Box(low=-1, high=1, shape=(self.action_space_size, )) self.actor_critic = opt_cuda( Policy([self.config_size], self.action_space, base_kwargs={'recurrent': False})) self.predict_mask = [0, 1, 2] + [6, 7, 8] + [12, 13, 14] + [18, 19, 20] self.config_state_attrs()
def __init__(self, *args): super(BookShelf, self).__init__(*args) connect(use_gui=self.experiment_dict['render']) self.arm_size = 1 if (self.detailed_gmp): self.robot = p.loadURDF( DRAKE_IIWA_URDF, useFixedBase=True, globalScaling=1) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF else: self.robot = None self.current_constraint_id = None self.default_links = [0] self.y_offset = 0.5 self.floor = p.loadURDF('models/short_floor.urdf', useFixedBase=True) self.ARM_LEN = 0.8 set_default_camera() self.shelf = p.loadURDF("models/kiva_shelf/bookcase.urdf", globalScaling=3.0, useFixedBase=True) set_pose( self.shelf, Pose(Point(x=-0.2, y=-1.6, z=-0.010), Euler(yaw=0, pitch=0, roll=math.pi / 2))) self.book = p.loadURDF("models/book/book.urdf", globalScaling=0.15, useFixedBase=False) self.book_pos = [0.06, -1.43, 0.488] self.book_rot = [math.pi / 2, 0, 0] set_pose( self.book, Pose( Point(x=self.book_pos[0], y=self.book_pos[1], z=self.book_pos[2]), Euler(roll=self.book_rot[0], pitch=self.book_rot[1], yaw=self.book_rot[2]))) self.blue_rod_1 = p.loadURDF("models/blue_rod.urdf", globalScaling=2.0, useFixedBase=False) set_pose(self.blue_rod_1, Pose(Point(x=0.7, y=0, z=1.2), Euler(yaw=0, pitch=0, roll=0))) self.blue_rod_2 = p.loadURDF("models/blue_rod.urdf", globalScaling=2.0, useFixedBase=False) set_pose( self.blue_rod_2, Pose(Point(x=-0.7, y=0, z=1.2), Euler(yaw=0, pitch=0, roll=0))) self.objects = [self.blue_rod_1, self.blue_rod_2] self.static_objects = [self.book] self.perspectives = [(0, -90)] self.break_on_timeout = False self.macroaction = MacroAction(macroaction_list=[ PickPlace(objects=self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), AddLink(objects=self.objects, robot=self.robot, fixed=self.fixed, gmp=self.detailed_gmp), ], experiment_dict=self.experiment_dict) # Config state attributes self.config_state_attrs(linking=True) p.setGravity(0, 0, -10, physicsClientId=0) self.break_on_timeout = True self.actor_critic = opt_cuda( Policy([self.config_size], self.action_space, base_kwargs={'recurrent': False}))