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