def hook(self): for (link_object, link, link_transform, _) in self.links: if(link_object.sphere != None): lpos, lquat = p.getBasePositionAndOrientation(link) le = p.getEulerFromQuaternion(lquat) lpose = Pose(Point(*lpos), Euler(*le)) set_pose(link_object.sphere, multiply(lpose, link_transform))
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 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 execute(self, block_to_move, teleport_pose, sim=False):
"""
Output: (feasible, planning commands, auxiliary output)
"""
# Get the selected position
(feas_command,
feasible) = self.feasibility_check(block_to_move, teleport_pose, sim)
if (feasible == True):
set_pose(block_to_move, teleport_pose)
return (True, feas_command, None)
else:
return (False, None, None)
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 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 __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 execute(self, embedding, config, sim=False):
"""
Output: (feasible, planning command)
"""
total_selectors = sum([
self.macroaction_list[macro_idx].num_selectors
for macro_idx in range(len(self.macroaction_list))
])
max_element = np.argmax(embedding[0:total_selectors])
prev = 0
for ma_index, ma in enumerate(self.macroaction_list):
if (max_element >= prev and max_element < ma.num_selectors + prev):
macroaction_index = ma_index
prev += ma.num_selectors
# Select out the nodes of the network responsible for that macroaction
if (isinstance(self.macroaction_list[macroaction_index], PickPlace)):
# Need to do some extra preprocessing to account for links
# If there is a link, we need to transport objects at the same time with the same dynamics to avoid explosion
# First, get the block to move
object_index = np.argmax(embedding[0:len(self.objects)], axis=0)
block_to_move = self.objects[int(object_index)]
# Then, get all of the blocks connected to the block_to_move
connected_blocks = []
num_blocks = int(math.sqrt(len(self.link_status)))
for block_index in range(num_blocks):
if (block_index != block_to_move
and self.link_status[int(object_index) * num_blocks +
block_index]):
connected_blocks.append(self.objects[block_index])
# Then we need to get the goal pose of the moving object
start_index = len(self.objects)
end_index = start_index + self.macroaction_list[
macroaction_index].object_params[object_index]
pos = embedding[start_index:end_index]
teleport_pose = self.reparameterize(block_to_move, pos)
# Now we need to perform forward dynamics on the connected objects
for connected_block in connected_blocks:
b1pos, b1quat = p.getBasePositionAndOrientation(block_to_move)
b1e = p.getEulerFromQuaternion(b1quat)
b2pos, b2quat = p.getBasePositionAndOrientation(
connected_block)
b2e = p.getEulerFromQuaternion(b2quat)
start_pose = Pose(Point(*b1pos), Euler(*b1e))
other_pose = Pose(Point(*b2pos), Euler(*b2e))
collision = False
index = 0
other_goal_pose = multiply(
teleport_pose, multiply(invert(start_pose), other_pose))
set_pose(connected_block, other_goal_pose)
(feasible, planning_commands,
_) = self.macroaction_list[macroaction_index].execute(
block_to_move, teleport_pose, sim)
return (feasible, planning_commands)
elif (isinstance(self.macroaction_list[macroaction_index], AddLink)):
mask_start = sum([
self.macroaction_list[macro_idx].num_selectors
for macro_idx in range(macroaction_index)
])
mask_end = mask_start + self.macroaction_list[
macroaction_index].num_selectors
(feasible, planning_commands,
link_status) = self.macroaction_list[macroaction_index].execute(
embedding[mask_start:mask_end], self.link_status, sim)
if (link_status is not None):
self.link_status = link_status
return (feasible, planning_commands)
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}))