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)
class FourBlocks(Environment):
def __init__(self, *args):
super(FourBlocks, 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.objects = [self.green_block, self.red_block, self.blue_block, self.purple_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)
@property
def fixed(self):
return [self.floor]
def set_state(self, conf):
i = 0
for block in self.objects:
set_pose(block, Pose(Point(x = conf[i], y = conf[i+1], z=conf[i+2]), Euler(roll = conf[i+3], pitch = conf[i+4], yaw=conf[i+5])))
i+=6
if(len(self.macroaction.link_status)>0):
self.macroaction.link_status = list(conf[-len(self.macroaction.link_status):len(conf)])
def check_goal_state(self, config):
# collect the y values
vals = [config[2], config[8], config[14], config[20]]
vals.sort()
if(vals[0]<0.06 and (vals[1] > 0.06 and vals[1] < 0.16) and (vals[2] > 0.16 and vals[2] < 0.26) and (vals[3] > 0.26 and vals[3] < 0.36)):
return True
return False
def get_current_config(self):
tot = []
for block in self.objects:
pos, quat = p.getBasePositionAndOrientation(block, physicsClientId=0)
euler = p.getEulerFromQuaternion(quat)
tot+=(pos+euler)
return np.array(tot+self.macroaction.link_status)
def get_start_state(self):
collision = True
z = stable_z(self.green_block, self.floor)
while(collision):
poses = [self.macroaction.reparameterize(self.objects[0], np.random.uniform(low=-1, high=1, size=4)) for _ in range(4)]
pos1, pos2, pos3, pos4 = [pose[0] for pose in poses]
state = State(len(self.objects), len(self.static_objects), len(self.macroaction.link_status))
state.set_position(0, pos1[0], pos1[1], z)
state.set_position(1, pos2[0], pos2[1], z)
state.set_position(2, pos3[0], pos3[1], z)
state.set_position(3, pos4[0], pos4[1], z)
self.set_state(state.config)
collision = check_pairwise_collisions(self.objects)
return state.config
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()
class PulleySeesaw(Environment):
def is_stable(self, old_conf, conf, count):
timeout = count > 50
stable = dist(old_conf, conf) < 6e-5
return stable, timeout
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 check_goal_state(self, config):
if (self.training):
keystone, _ = p.getBasePositionAndOrientation(self.cup)
else:
keystone, _ = p.getBasePositionAndOrientation(self.keystone)
tddist = lambda a, b, c: math.sqrt((a - keystone[0])**2 +
(b - keystone[1])**2 +
(c - keystone[2])**2) < 0.5
in_bucket = int(tddist(config[0], config[1], config[2])) + int(
tddist(config[6], config[7], config[8])) + int(
tddist(config[12], config[13], config[14])) + tddist(
config[18], config[19], config[20]) + int(
tddist(config[24], config[25], config[26]))
# if(in_bucket>0):
# print(str(in_bucket) +" in bucket")
if (tddist(config[0], config[1], config[2])
and tddist(config[6], config[7], config[8])
and tddist(config[12], config[13], config[14])
and tddist(config[18], config[19], config[20])
and tddist(config[24], config[25], config[26])):
print(config[-2])
return config[-1] == self.NOKNOT
return False
def get_current_config(self):
config = []
for obj in self.objects + self.fixed_objects:
pos, quat = p.getBasePositionAndOrientation(obj)
euler = p.getEulerFromQuaternion(quat)
config.append(pos)
config.append(euler)
resulting_config = np.concatenate(config +
[self.macroaction.link_status])
return resulting_config
def get_available_block(self):
reachable = False
break_condition = 0
while (not reachable):
if (break_condition == 100):
return None
block_to_move = random.choice([0, 1, 2, 3, 4])
if (self.training):
keystone, _ = p.getBasePositionAndOrientation(self.cup)
else:
keystone, _ = p.getBasePositionAndOrientation(self.keystone)
blockpos, _ = p.getBasePositionAndOrientation(
self.objects[block_to_move])
tddist = lambda a, b, c: math.sqrt((a - keystone[0])**2 +
(b - keystone[1])**2 +
(c - keystone[2])**2) < 0.3
reachable = not tddist(blockpos[0], blockpos[1], blockpos[2])
break_condition += 1
return block_to_move
# def take_action(self, embedding):
# joint_state = p.getJointState(self.seesaw, self.seesaw_joint)
# collision = True
# objects = self.small_blue+[self.red_block]
# block_to_move = self.get_available_block()
# if(random.uniform(0, 1)>0.95 or block_to_move is None):
# block_to_move = 5
# if(self.knot is not None):
# self.deconstrain()
# self.knot=None
# else:
# while(collision):
# # mx, my, _ = self.reachable_pos()
# mx = random.uniform(0.7, 0.8)
# my = random.uniform(-0.1, 0.1)
# # Need to check collisions
# # set_pose(objects[block_to_move], Pose(Point(x=mx, y=my, z=random.uniform(0.1, self.ARM_LEN)), Euler(roll = random.uniform(-math.pi, math.pi), pitch = random.uniform(-math.pi, math.pi), yaw = random.uniform(-math.pi, math.pi)) ))
# set_pose(objects[block_to_move], Pose(Point(x=mx, y=my, z=random.uniform(0.1, self.ARM_LEN)), Euler(roll = 0, pitch = 0, yaw = 0) ))
# collision = self.check_vec_collisions(objects[block_to_move], objects)
# block_logit = [0 for _ in range(6)]
# block_logit[block_to_move] = 1
# return np.concatenate([np.array(block_logit), self.get_current_config()[:15]])
def constrain(self):
if (not self.training):
return p.createConstraint(self.black_block,
-1,
self.sphereUid,
-1,
p.JOINT_FIXED,
jointAxis=[0, 0, 0],
parentFramePosition=self.block_pos,
childFramePosition=self.block_pos)
else:
return self.KNOT
def deconstrain(self):
if (not self.training):
p.removeConstraint(self.knot)
def set_state(self, conf):
if (not self.training):
saved_world = p.restoreState(
fileName="./temp/pulley_start_state.bullet")
if (conf[-1] > 0):
if (self.knot is not None):
self.deconstrain()
self.knot = self.constrain()
for i in range(5):
set_pose(
self.objects[i],
Pose(
Point(x=conf[i * 6], y=conf[i * 6 + 1], z=conf[i * 6 + 2]),
Euler(roll=conf[i * 6 + 3],
pitch=conf[i * 6 + 4],
yaw=conf[i * 6 + 5])))
def get_start_state(self):
collision = True
z = stable_z(self.objects[0], self.floor)
while (collision):
poses = [
self.macroaction.reparameterize(
self.objects[0], np.random.uniform(low=-1, high=1, size=4))
for _ in range(5)
]
pos1, pos2, pos3, pos4, pos5 = [pose[0] for pose in poses]
state = State(len(self.objects), len(self.static_objects),
len(self.macroaction.link_status))
state.set_position(0, pos1[0], pos1[1], z)
state.set_position(1, pos2[0], pos2[1], z)
state.set_position(2, pos3[0], pos3[1], z)
state.set_position(3, pos4[0], pos4[1], z)
state.set_position(4, pos5[0], pos5[1], z)
self.set_state(state.config)
collision = check_pairwise_collisions(self.objects)
return state.config
@property
def fixed_objects(self):
return [self.black_block, self.red_block, self.sphereUid]
@property
def fixed(self):
return [self.floor]
class TwoBlocks(Environment):
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)
@property
def fixed(self):
return [self.floor]
def set_state(self, conf):
i = 0
for block in self.objects:
set_pose(
block,
Pose(
Point(x=conf[i], y=conf[i + 1], z=conf[i + 2]),
Euler(roll=conf[i + 3], pitch=conf[i + 4],
yaw=conf[i + 5])))
i += 6
def check_goal_state(self, config):
# collect the y values
vals = [config[2], config[8]]
vals.sort()
# Two stack
if ((vals[0] > 0.06) or (vals[1] > 0.06)):
return True
return False
def get_current_config(self):
# Get the object states
gpos, gquat = p.getBasePositionAndOrientation(self.green_block,
physicsClientId=0)
rpos, rquat = p.getBasePositionAndOrientation(self.red_block,
physicsClientId=0)
# Convert quat to euler
geuler = p.getEulerFromQuaternion(gquat)
reuler = p.getEulerFromQuaternion(rquat)
# Format into a config vector
return np.concatenate([gpos, geuler, rpos, reuler] +
[self.macroaction.link_status])
def get_current_detailed_config(self):
# Get the object states
gpos, gquat = p.getBasePositionAndOrientation(self.green_block,
physicsClientId=0)
rpos, rquat = p.getBasePositionAndOrientation(self.red_block,
physicsClientId=0)
g_linear_vel, g_angular_velocity = p.getBaseVelocity(self.green_block,
physicsClientId=0)
r_linear_vel, r_angular_velocity = p.getBaseVelocity(self.red_block,
physicsClientId=0)
# Convert quat to euler
geuler = p.getEulerFromQuaternion(gquat)
reuler = p.getEulerFromQuaternion(rquat)
# Format into a config vector
return {
"object_1":
np.concatenate(
[gpos, gquat, geuler, g_linear_vel, g_angular_velocity]),
"object_2":
np.concatenate(
[rpos, rquat, reuler, r_linear_vel, r_angular_velocity])
}
def get_start_state(self):
collision = True
z = stable_z(self.green_block, self.floor)
while (collision):
poses = [
self.macroaction.reparameterize(
self.objects[0], np.random.uniform(low=-1, high=1, size=4))
for _ in range(2)
]
pos1, pos2 = [pose[0] for pose in poses]
state = State(len(self.objects), len(self.static_objects),
len(self.macroaction.link_status))
state.set_position(0, pos1[0], pos1[1], z)
state.set_position(1, pos2[0], pos2[1], z)
self.set_state(state.config)
collision = check_pairwise_collisions(self.objects)
return state.config
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}))
class BookShelf(Environment):
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}))
def check_goal_state(self, conf):
if (conf[14] < 0.3):
return True
return False
def set_state(self, conf):
self.remove_constraints()
set_pose(
self.blue_rod_1,
Pose(Point(x=conf[0], y=conf[1], z=conf[2]),
Euler(roll=conf[3], pitch=conf[4], yaw=conf[5])))
set_pose(
self.blue_rod_2,
Pose(Point(x=conf[6], y=conf[7], z=conf[8]),
Euler(roll=conf[9], pitch=conf[10], yaw=conf[11])))
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.add_constraints(conf)
time.sleep(0.001)
def get_macroaction_params(self, obj, conf):
if (int(obj) == 0):
return (self.blue_rod_1,
Pose(Point(x=conf[0], y=conf[1], z=conf[2]),
Euler(roll=conf[3], pitch=conf[4], yaw=conf[5])))
elif (int(obj) == 1):
return (self.blue_rod_2,
Pose(Point(x=conf[6], y=conf[7], z=conf[8]),
Euler(roll=conf[9], pitch=conf[10], yaw=conf[11])))
else:
return [self.blue_rod_1, self.blue_rod_2]
def get_current_config(self):
b1pos, b1quat = p.getBasePositionAndOrientation(self.blue_rod_1)
b2pos, b2quat = p.getBasePositionAndOrientation(self.blue_rod_2)
bookpos, bookquat = p.getBasePositionAndOrientation(self.book)
# Reduce configuration redundancy/complexity
b1e = p.getEulerFromQuaternion(b1quat)
b2e = p.getEulerFromQuaternion(b2quat)
booke = p.getEulerFromQuaternion(bookquat)
returning_state = np.array(
list(b1pos + b1e + b2pos + b2e + bookpos + booke) +
self.macroaction.link_status)
return returning_state
@property
def fixed(self):
return [self.floor, self.shelf]
def get_start_state(self):
collision = True
z = 0.02
while (collision):
poses = [
self.macroaction.reparameterize(
self.objects[0], np.random.uniform(low=-1, high=1, size=4))
for _ in range(2)
]
pos1, pos2 = [pose[0] for pose in poses]
state = State(len(self.objects), len(self.static_objects),
len(self.macroaction.link_status))
state.set_position(0, pos1[0], pos1[1], z)
state.set_position(1, pos2[0], pos2[1], z)
self.set_state(state.config)
collision = check_pairwise_collisions(self.objects)
return state.config