def test_grasps(robot, node_points, elements):
element = elements[-1]
[element_body] = create_elements(node_points, [element])
phi = 0
link = link_from_name(robot, TOOL_NAME)
link_pose = get_link_pose(robot, link)
draw_pose(link_pose) #, parent=robot, parent_link=link)
wait_for_interrupt()
n1, n2 = element
p1 = node_points[n1]
p2 = node_points[n2]
length = np.linalg.norm(p2 - p1)
# Bottom of cylinder is p1, top is p2
print(element, p1, p2)
for phi in np.linspace(0, np.pi, 10, endpoint=True):
theta = np.pi / 4
for t in np.linspace(-length / 2, length / 2, 10):
translation = Pose(
point=Point(z=-t)
) # Want to be moving backwards because +z is out end effector
direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi))
angle = Pose(euler=Euler(yaw=1.2))
grasp_pose = multiply(angle, direction, translation)
element_pose = multiply(link_pose, grasp_pose)
set_pose(element_body, element_pose)
wait_for_interrupt()
def get_grasp_pose(translation, direction, angle, reverse, offset=1e-3):
#direction = Pose(euler=Euler(roll=np.pi / 2, pitch=direction))
return multiply(Pose(point=Point(z=offset)),
Pose(euler=Euler(yaw=angle)),
direction,
Pose(point=Point(z=translation)),
Pose(euler=Euler(roll=(1-reverse) * np.pi)))
def load_world():
# TODO: store internal world info here to be reloaded
set_default_camera()
draw_global_system()
with HideOutput():
#add_data_path()
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True) # DRAKE_IIWA_URDF | KUKA_IIWA_URDF
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
celery = load_model(BLOCK_URDF, fixed_base=False)
radish = load_model(SMALL_BLOCK_URDF, fixed_base=False)
#cup = load_model('models/dinnerware/cup/cup_small.urdf',
# Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)
draw_pose(Pose(), parent=robot, parent_link=get_tool_link(robot)) # TODO: not working
# dump_body(robot)
# wait_for_user()
body_names = {
sink: 'sink',
stove: 'stove',
celery: 'celery',
radish: 'radish',
}
movable_bodies = [celery, radish]
set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor))))
set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor))))
return robot, body_names, movable_bodies
def load_world():
# TODO: store internal world info here to be reloaded
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF)
#add_data_path()
#robot = load_pybullet(KUKA_IIWA_URDF)
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
celery = load_model(BLOCK_URDF, fixed_base=False)
radish = load_model(SMALL_BLOCK_URDF, fixed_base=False)
#cup = load_model('models/dinnerware/cup/cup_small.urdf',
# Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)
body_names = {
sink: 'sink',
stove: 'stove',
celery: 'celery',
radish: 'radish',
}
movable_bodies = [celery, radish]
set_pose(celery, Pose(Point(y=0.5, z=stable_z(celery, floor))))
set_pose(radish, Pose(Point(y=-0.5, z=stable_z(radish, floor))))
set_default_camera()
return robot, body_names, movable_bodies
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
initial_bq = Pose(robot, get_pose(robot))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf),
('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision': fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def gen(rover, objective):
base_joints = get_base_joints(rover)
target_point = get_point(objective)
base_generator = visible_base_generator(rover, target_point,
base_range)
lower_limits, upper_limits = get_custom_limits(rover, base_joints,
custom_limits)
attempts = 0
while True:
if max_attempts <= attempts:
attempts = 0
yield None
attempts += 1
base_conf = next(base_generator)
if not all_between(lower_limits, base_conf, upper_limits):
continue
bq = Conf(rover, base_joints, base_conf)
bq.assign()
if any(pairwise_collision(rover, b) for b in obstacles):
continue
link_pose = get_link_pose(rover,
link_from_name(rover, KINECT_FRAME))
if use_cone:
mesh, _ = get_detection_cone(rover,
objective,
camera_link=KINECT_FRAME,
depth=max_range)
if mesh is None:
continue
cone = create_mesh(mesh, color=(0, 1, 0, 0.5))
local_pose = Pose()
else:
distance = get_distance(point_from_pose(link_pose),
target_point)
if max_range < distance:
continue
cone = create_cylinder(radius=0.01,
height=distance,
color=(0, 0, 1, 0.5))
local_pose = Pose(Point(z=distance / 2.))
set_pose(cone, multiply(link_pose, local_pose))
# TODO: colors corresponding to scanned object
if any(
pairwise_collision(cone, b) for b in obstacles
if b != objective and not is_placement(objective, b)):
# TODO: ensure that this works for the rover
remove_body(cone)
continue
if not reachable_test(rover, bq):
continue
print('Visibility attempts:', attempts)
y = Ray(cone, rover, objective)
yield Output(bq, y)
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_interrupt()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_interrupt()
def sample_direction():
##roll = random.uniform(0, np.pi)
#roll = np.pi/4
#pitch = random.uniform(0, 2*np.pi)
#return Pose(euler=Euler(roll=np.pi / 2 + roll, pitch=pitch))
roll = random.uniform(-np.pi/2, np.pi/2)
pitch = random.uniform(-np.pi/2, np.pi/2)
return Pose(euler=Euler(roll=roll, pitch=pitch))
def gen_fn(index):
brick = brick_from_index[index]
while True:
original_grasp = random.choice(brick.grasps)
theta = random.uniform(-np.pi, +np.pi)
rotation = Pose(euler=Euler(yaw=theta))
new_attach = multiply(rotation, original_grasp.attach)
grasp = Grasp(None, None, None, new_attach, None)
yield grasp,
def load_world():
# TODO: store internal world info here to be reloaded
robot = load_model(DRAKE_IIWA_URDF)
# robot = load_model(KUKA_IIWA_URDF)
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
block = load_model(BLOCK_URDF, fixed_base=False)
#cup = load_model('models/dinnerware/cup/cup_small.urdf', Pose(Point(x=+0.5, y=+0.5, z=0.5)), fixed_base=False)
body_names = {
sink: 'sink',
stove: 'stove',
block: 'celery',
}
movable_bodies = [block]
set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor))))
set_default_camera()
return robot, body_names, movable_bodies
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
connect(use_gui=True)
with LockRenderer():
draw_pose(unit_pose(), length=1, width=1)
floor = create_floor()
set_point(floor, Point(z=-EPSILON))
table1 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table1, Point(y=+0.5))
table2 = create_table(width=TABLE_WIDTH,
length=TABLE_WIDTH / 2,
height=TABLE_WIDTH / 2,
top_color=TAN,
cylinder=False)
set_point(table2, Point(y=-0.5))
tables = [table1, table2]
#set_euler(table1, Euler(yaw=np.pi/2))
with HideOutput():
# data_path = add_data_path()
# robot_path = os.path.join(data_path, WSG_GRIPPER)
robot_path = get_model_path(
WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF
robot = load_pybullet(robot_path, fixed_base=True)
#dump_body(robot)
block1 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=RED)
block_z = stable_z(block1, table1)
set_point(block1, Point(y=-0.5, z=block_z))
block2 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=GREEN)
set_point(block2, Point(x=-0.25, y=-0.5, z=block_z))
block3 = create_box(w=BLOCK_SIDE,
l=BLOCK_SIDE,
h=BLOCK_SIDE,
color=BLUE)
set_point(block3, Point(x=-0.15, y=+0.5, z=block_z))
blocks = [block1, block2, block3]
set_camera_pose(camera_point=Point(x=-1, z=block_z + 1),
target_point=Point(z=block_z))
block_pose = get_pose(block1)
open_gripper(robot)
tool_link = link_from_name(robot, 'tool_link')
base_from_tool = get_relative_pose(robot, tool_link)
#draw_pose(unit_pose(), parent=robot, parent_link=tool_link)
grasps = get_side_grasps(block1,
tool_pose=Pose(euler=Euler(yaw=np.pi / 2)),
top_offset=0.02,
grasp_length=0.03,
under=False)[1:2]
for grasp in grasps:
gripper_pose = multiply(block_pose, invert(grasp))
set_pose(robot, multiply(gripper_pose, invert(base_from_tool)))
wait_for_user()
wait_for_user('Finish?')
disconnect()
def load_world():
# TODO: store internal world info here to be reloaded
with HideOutput():
robot = load_model(DRAKE_IIWA_URDF)
# robot = load_model(KUKA_IIWA_URDF)
floor = load_model('models/short_floor.urdf')
sink = load_model(SINK_URDF, pose=Pose(Point(x=-0.5)))
stove = load_model(STOVE_URDF, pose=Pose(Point(x=+0.5)))
block = load_model(BLOCK_URDF, fixed_base=False)
block1 = load_model(BLOCK_URDF, fixed_base=False)
block2 = load_model(BLOCK_URDF, fixed_base=False)
block3 = load_model(BLOCK_URDF, fixed_base=False)
block4 = load_model(BLOCK_URDF, fixed_base=False)
block5 = load_model(BLOCK_URDF, fixed_base=False)
block6 = load_model(BLOCK_URDF, fixed_base=False)
block7 = load_model(BLOCK_URDF, fixed_base=False)
block8 = load_model(BLOCK_URDF, fixed_base=False)
cup = load_model(
'models/cup.urdf', #'models/dinnerware/cup/cup_small.urdf'
fixed_base=False)
body_names = {
sink: 'sink',
stove: 'stove',
block: 'celery',
cup: 'cup',
}
movable_bodies = [
block, cup, block1, block2, block3, block4, block5, block6, block7,
block8
]
set_pose(block, Pose(Point(x=0.1, y=0.5, z=stable_z(block, floor))))
set_pose(block1, Pose(Point(x=-0.1, y=0.5, z=stable_z(block1, floor))))
set_pose(block2, Pose(Point(y=0.35, z=stable_z(block2, floor))))
set_pose(block3, Pose(Point(y=0.65, z=stable_z(block3, floor))))
set_pose(block4, Pose(Point(x=0.1, y=0.65, z=stable_z(block4, floor))))
set_pose(block5, Pose(Point(x=-0.1, y=0.65, z=stable_z(block5, floor))))
set_pose(block6, Pose(Point(x=0.1, y=0.35, z=stable_z(block6, floor))))
set_pose(block7, Pose(Point(x=-0.1, y=0.35, z=stable_z(block7, floor))))
set_pose(block8, Pose(Point(x=0, y=0.5, z=0.45)))
set_pose(cup, Pose(Point(y=0.5, z=stable_z(cup, floor))))
set_default_camera()
return robot, body_names, movable_bodies