def test_block(visualize):
#arms = [LEFT_ARM]
arms = ARMS
block_name = create_name('greenblock', 1)
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON
#attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame')
#dump_body(world.perception.pr2)
#block_y = 0.0
block_y = -0.4
world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat())
world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat())
update_world(world, table_body)
init = [
('Stackable', block_name, tray_name, TOP),
]
goal = [
('On', block_name, tray_name, TOP),
]
#goal = [
# ('Grasped', arms[0], block_name),
#]
task = Task(init=init, goal=goal, arms=arms, empty_arms=True)
return world, task
def test_pour(visualize):
arms = [LEFT_ARM]
#cup_name, bowl_name = 'bluecup', 'bluebowl'
#cup_name, bowl_name = 'cup_7', 'cup_8'
#cup_name, bowl_name = 'cup_7-1', 'cup_7-2'
#cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2)
cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup
bowl_name = create_name('bowl', 1) # bowl | steel_bowl
world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat())
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms,
empty_arms=True, reset_arms=True, reset_items=True)
return world, task
def test_stack_pour(visualize):
arms = [LEFT_ARM]
bowl_name = create_name('whitebowl', 1)
cup_name = create_name('bluecup', 1)
purple_name = create_name('purpleblock', 1)
items = [bowl_name, cup_name, purple_name]
world, table_body = create_world(items, visualize=visualize)
cup_x = 0.5
initial_poses = {
bowl_name: ([cup_x, -0.2, 0.0], unit_quat()),
cup_name: ([cup_x, 0.1, 0.0], unit_quat()),
purple_name: ([cup_x, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
('Stackable', bowl_name, purple_name, TOP),
]
goal = [
('Contains', bowl_name, COFFEE),
('On', bowl_name, purple_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms, graspable=['whitebowl'])
return world, task
def test_push_pour(visualize):
arms = ARMS
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
items = [bowl_name, cup_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
update_world(world, table_body)
# TODO: can prevent the right arm from being able to pick
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
# Most of the time places the cup to exchange arms
return world, task
def get_water_path(bowl_body, bowl_pose, cup_body, pose_waypoints):
cup_pose = pose_waypoints[0]
bowl_origin_from_base = get_urdf_from_base(
bowl_body) # TODO: reference pose
cup_origin_from_base = get_urdf_from_base(cup_body)
ray_start = point_from_pose(multiply(cup_pose, cup_origin_from_base))
ray_end = point_from_pose(multiply(bowl_pose, bowl_origin_from_base))
water_path = interpolate_poses((ray_start, unit_quat()),
(ray_end, unit_quat()),
pos_step_size=0.01)
return water_path
def get_urdf_from_z_axis(body, z_fraction, reference_quat=unit_quat()):
# AKA the pose of the body's center wrt to the body's origin
# z_fraction=0. => bottom, z_fraction=0.5 => center, z_fraction=1. => top
ref_from_urdf = (unit_point(), reference_quat)
center_in_ref, (_,
height) = approximate_as_cylinder(body,
body_pose=ref_from_urdf)
center_in_ref[2] += (z_fraction - 0.5) * height
ref_from_center = (center_in_ref, unit_quat()
) # Maps from center frame to origin frame
urdf_from_center = multiply(invert(ref_from_urdf), ref_from_center)
return urdf_from_center
def test_holding(visualize, num_blocks=2):
arms = [LEFT_ARM]
#item_type = 'greenblock'
#item_type = 'bowl'
item_type = 'purple_cup'
item_poses = [
([0.6, +0.4, 0.0], z_rotation(np.pi / 2)),
([0.6, -0.4, 0.0], unit_quat()),
]
items = [create_name(item_type, i) for i in range(min(len(item_poses), num_blocks))]
world, table_body = create_world(items, visualize=visualize)
with ClientSaver(world.perception.client):
for name, pose in zip(items, item_poses):
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
('Graspable', item) for item in items
]
goal = [
#('Holding', items[0]),
('HoldingType', item_type),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def get_handle_grasps(world, joint, pull=True, pre_distance=APPROACH_DISTANCE):
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
#half_extent = 1.0*FINGER_EXTENT[2] # Collides
half_extent = 1.05 * FINGER_EXTENT[2]
grasps = []
for link in get_link_subtree(world.kitchen, joint):
if 'handle' in get_link_name(world.kitchen, link):
# TODO: can adjust the position and orientation on the handle
for yaw in [0, np.pi]: # yaw=0 DOESN'T WORK WITH LULA
handle_grasp = (Point(z=-half_extent),
quat_from_euler(
Euler(roll=np.pi, pitch=np.pi / 2,
yaw=yaw)))
#if not pull:
# handle_pose = get_link_pose(world.kitchen, link)
# for distance in np.arange(0., 0.05, step=0.001):
# pregrasp = multiply(([0, 0, -distance], unit_quat()), handle_grasp)
# tool_pose = multiply(handle_pose, invert(pregrasp))
# set_tool_pose(world, tool_pose)
# # TODO: check collisions
# wait_for_user()
handle_pregrasp = multiply((pre_direction, unit_quat()),
handle_grasp)
grasps.append(HandleGrasp(link, handle_grasp, handle_pregrasp))
return grasps
def add_table_surfaces(world):
table_pose = None
for body_info in world.perception.surfaces:
if body_info.type == TABLE:
table_pose = body_info.pose
assert table_pose is not None
initial_poses = {
STOVE_NAME: (STOVE_POSITION, unit_quat()),
PLACEMAT_NAME: (PLACEMAT_POSITION, unit_quat()),
BUTTON_NAME: (BUTTON_POSITION, unit_quat()),
}
with ClientSaver(world.perception.client):
for name, local_pose in initial_poses.items():
world_pose = multiply(table_pose, local_pose)
from perception_tools.retired.ros_perception import BodyInfo
world.perception.surfaces.append(
BodyInfo(name, None, world_pose, name))
def test_cup(visualize):
arms = [LEFT_ARM]
cup_name = create_name('greenblock', 1)
block_name = create_name('purpleblock', 1) # greenblock
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, cup_name, block_name], visualize=visualize)
#cup_x = 0.6
cup_x = 0.8
block_x = cup_x - 0.15
initial_poses = {
cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
block_name: ([block_x, 0.0, 0.0], unit_quat()), # z_rotation(np.pi/2)
tray_name: ([0.6, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
#with ClientSaver(world.perception.client):
# draw_pose(get_pose(world.perception.sim_bodies['bluecup']))
# draw_aabb(get_aabb(world.perception.sim_bodies['bluecup']))
# wait_for_interrupt()
init = [
#('Contains', cup_name, WATER),
('Stackable', cup_name, tray_name, TOP),
]
goal = [
#('Grasped', goal_arm, block_name),
#('Grasped', goal_arm, cup_name),
('On', cup_name, tray_name, TOP),
#('On', cup_name, TABLE_NAME, TOP),
#('Empty', goal_arm),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_stacking(visualize):
# TODO: move & stack
arms = [LEFT_ARM]
#arms = ARMS
cup_name = create_name('bluecup', 1) # bluecup | spoon
green_name = create_name('greenblock', 1)
purple_name = create_name('purpleblock', 1)
items = [cup_name, green_name, purple_name]
world, table_body = create_world(items, visualize=visualize)
cup_x = 0.5
initial_poses = {
cup_name: ([cup_x, -0.2, 0.0], unit_quat()),
#cup_name: ([cup_x, -0.2, 0.3], unit_quat()),
green_name: ([cup_x, 0.1, 0.0], unit_quat()),
#green_name: ([cup_x, 0.1, 0.0], z_rotation(np.pi / 4)), # TODO: be careful about the orientation when stacking
purple_name: ([cup_x, 0.4, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
#('Stackable', cup_name, purple_name, TOP),
('Stackable', cup_name, green_name, TOP),
('Stackable', green_name, purple_name, TOP),
]
goal = [
#('On', cup_name, purple_name, TOP),
('On', cup_name, green_name, TOP),
('On', green_name, purple_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_shelves(visualize):
arms = [LEFT_ARM]
# TODO: smaller (2) shelves
# TODO: sample with respect to the link it will supported by
# shelves.off | shelves_points.off | tableShelves.off
# import os
# data_directory = '/Users/caelan/Programs/LIS/git/lis-data/meshes/'
# mesh = read_mesh_off(os.path.join(data_directory, 'tableShelves.off'))
# draw_mesh(mesh)
# wait_for_interrupt()
start_link = 'shelf2' # shelf1 | shelf2 | shelf3 | shelf4
end_link = 'shelf1'
shelf_name = 'two_shelves'
#shelf_name = 'three_shelves'
cup_name = create_name('bluecup', 1)
world, table_body = create_world([shelf_name, cup_name], visualize=visualize)
cup_x = 0.65
#shelf_x = 0.8
shelf_x = 0.75
initial_poses = {
shelf_name: ([shelf_x, 0.3, 0.0], quat_from_euler(Euler(yaw=-np.pi/2))),
#cup_name: ([cup_x, 0.0, 0.0], unit_quat()),
}
with ClientSaver(world.perception.client):
for name, pose in initial_poses.items():
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
shelf_body = world.perception.sim_bodies[shelf_name]
#print([str(get_link_name(shelf_body, link)) for link in get_links(shelf_body)])
#print([str(get_link_name(shelf_body, link)) for link in get_links(world.perception.sim_bodies[cup_name])])
#shelf_link = None
shelf_link = link_from_name(shelf_body, start_link)
cup_z = stable_z(world.perception.sim_bodies[cup_name], shelf_body, surface_link=shelf_link) + Z_EPSILON
world.perception.set_pose(cup_name, [cup_x, 0.1, cup_z], unit_quat())
update_world(world, table_body, camera_point=np.array([-0.5, -1, 1.5]))
init = [
('Stackable', cup_name, shelf_name, end_link),
]
goal = [
('On', cup_name, shelf_name, end_link),
#('On', cup_name, TABLE_NAME, TOP),
#('Holding', cup_name),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def gen_fn(index, pose, grasp):
body = brick_from_index[index].body
set_pose(body, pose.value)
obstacles = list(obstacle_from_name.values()) # + [body]
collision_fn = get_collision_fn(
robot,
movable_joints,
obstacles=obstacles,
attachments=[],
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions,
custom_limits=get_custom_limits(robot))
attach_pose = multiply(pose.value, invert(grasp.attach))
approach_pose = multiply(attach_pose, (approach_vector, unit_quat()))
# approach_pose = multiply(pose.value, invert(grasp.approach))
for _ in range(max_attempts):
if USE_IKFAST:
attach_conf = sample_tool_ik(robot, attach_pose)
else:
set_joint_positions(robot, movable_joints,
sample_fn()) # Random seed
attach_conf = inverse_kinematics(robot, tool_link, attach_pose)
if (attach_conf is None) or collision_fn(attach_conf):
continue
set_joint_positions(robot, movable_joints, attach_conf)
#if USE_IKFAST:
# approach_conf = sample_tool_ik(robot, approach_pose, nearby_conf=attach_conf)
#else:
approach_conf = inverse_kinematics(robot, tool_link, approach_pose)
if (approach_conf is None) or collision_fn(approach_conf):
continue
set_joint_positions(robot, movable_joints, approach_conf)
path = plan_direct_joint_motion(
robot,
movable_joints,
attach_conf,
obstacles=obstacles,
self_collisions=SELF_COLLISIONS,
disabled_collisions=disabled_collisions)
if path is None: # TODO: retreat
continue
#path = [approach_conf, attach_conf]
attachment = Attachment(robot, tool_link, grasp.attach, body)
traj = MotionTrajectory(robot,
movable_joints,
path,
attachments=[attachment])
yield approach_conf, traj
break
def test_clutter(visualize, num_blocks=5, num_beads=0):
arms = [LEFT_ARM]
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1) # bowl | cup_7
clutter = [create_name('greenblock', i) for i in range(num_blocks)]
world, table_body = create_world([cup_name, bowl_name] + clutter, visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
block_z = None
if 0 < num_blocks:
block_z = stable_z(world.perception.sim_bodies[clutter[0]], table_body)
# TODO(lagrassa): first pose collides with the bowl
xy_poses = [(0.6, -0.1), (0.5, -0.2), (0.6, 0.11), (0.45, 0.12), (0.5, 0.3), (0.7, 0.3)]
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, -0.1, bowl_z), unit_quat())
#if 0 < num_beads:
# world.perception.add_beads(cup_name, num_beads, bead_radius=0.007, bead_mass=0.005)
if block_z is not None:
clutter_poses = [np.append(xy, block_z) for xy in reversed(xy_poses)]
for obj, pose in zip(clutter, clutter_poses):
world.perception.set_pose(obj, pose, unit_quat())
update_world(world, table_body)
#init = [('Graspable', name) for name in [cup_name, bowl_name] + clutter]
init = [
('Contains', cup_name, COFFEE),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def test_cook(visualize):
# TODO: can also disable collision bodies for stove & placemat
arms = [LEFT_ARM]
broccoli_name = 'greenblock'
stove_name = 'stove'
placemat_name = 'placemat'
button_name = 'button'
items = [stove_name, button_name, placemat_name, broccoli_name]
world, table_body = create_world(items, visualize=visualize)
update_world(world, table_body)
initial_poses = {
broccoli_name: ([-0.1, 0.0, 0.0], z_rotation(np.pi/2)),
stove_name: (STOVE_POSITION, unit_quat()),
placemat_name: (PLACEMAT_POSITION, unit_quat()),
button_name: (BUTTON_POSITION, unit_quat()),
}
with ClientSaver(world.perception.client):
for name, local_pose in initial_poses.items():
world_pose = multiply(TABLE_POSE, local_pose)
#z = stable_z(world.perception.sim_bodies[name], table_body) # Slightly above world_pose
world.perception.set_pose(name, *world_pose)
init = [
('IsButton', button_name, stove_name),
('Stackable', broccoli_name, stove_name, TOP),
('Stackable', broccoli_name, placemat_name, TOP),
]
goal = [
('Cooked', broccoli_name),
('On', broccoli_name, placemat_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
from plan_tools.planner import Task
from plan_tools.ros_world import ROSWorld
from plan_tools.samplers.grasp import hold_item
from pybullet_tools.pr2_utils import inverse_visibility, set_group_conf, arm_from_arm, \
WIDE_LEFT_ARM, rightarm_from_leftarm, open_arm
from pybullet_tools.pr2_problems import create_floor
from pybullet_tools.utils import ClientSaver, get_point, unit_quat, unit_pose, link_from_name, draw_point, \
HideOutput, stable_z, quat_from_euler, Euler, set_camera_pose, z_rotation, multiply, \
get_quat, create_mesh, set_point, set_quat, set_pose, wait_for_user, \
get_visual_data, read_obj, BASE_LINK, \
approximate_as_prism, Pose, draw_mesh, rectangular_mesh, apply_alpha, Point
TABLE_NAME = create_name(TABLE, 1)
# TODO: randomly sample from interval
TABLE_POSE = ([0.55, 0.0, 0.735], unit_quat())
Z_EPSILON = 1e-3
STOVE_POSITION = np.array([0.0, 0.4, 0.0])
PLACEMAT_POSITION = np.array([0.1, 0.0, 0.0])
BUTTON_POSITION = STOVE_POSITION - np.array([0.15, 0.0, 0.0])
TORSO_POSITION = 0.325 # TODO: maintain average distance betweeen pr2 and table
CAMERA_OPTICAL_FRAME = 'head_mount_kinect_rgb_optical_frame'
CAMERA_FRAME = 'head_mount_kinect_rgb_link'
#CAMERA_FRAME = 'high_def_frame'
#CAMERA_OPTICAL_FRAME = 'high_def_optical_frame'
#VIEWER_POINT = np.array([1.25, -0.5, 1.25]) # Angled
VIEWER_POINT = np.array([1.25, 0., 1.25]) # Wide
def lookup_orientation(name, quat_from_model):
for model, quat in quat_from_model.items():
if is_obj_type(name, model):
return quat
return unit_quat()
def load_pick_and_place(extrusion_name, scale=MILLIMETER, max_bricks=6):
assert extrusion_name == 'choreo_brick_demo'
root_directory = os.path.dirname(os.path.abspath(__file__))
bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY,
'bricks')
print('Name: {}'.format(extrusion_name))
with open(
os.path.join(bricks_directory,
PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f:
json_data = json.loads(f.read())
kuka_urdf = '../conrob_pybullet/models/kuka_kr6_r900/urdf/kuka_kr6_r900_mit_suction_gripper.urdf'
obj_directory = os.path.join(bricks_directory, 'meshes', 'collision')
with HideOutput():
#world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf'))
robot = load_pybullet(os.path.join(root_directory, kuka_urdf),
fixed_base=True)
#set_point(robot, (0.14, 0, 0))
#dump_body(robot)
pick_base_point = parse_point(json_data['pick_base_center_point'])
draw_pose((pick_base_point, unit_quat()))
place_base_point = parse_point(json_data['place_base_center_point'])
draw_pose((place_base_point, unit_quat()))
# workspace_geo_place_support_object_11 = pick_left_over_bricks_11
obstacle_from_name = {}
for filename in json_data['pick_support_surface_file_names']:
obstacle_from_name[strip_extension(filename)] = \
create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1))
for filename in json_data['place_support_surface_file_names']:
obstacle_from_name[strip_extension(filename)] = \
create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1))
brick_from_index = {}
for json_element in json_data['sequenced_elements']:
index = json_element['order_id']
pick_body = create_obj(os.path.join(
obj_directory, json_element['pick_element_geometry_file_name']),
scale=scale,
color=(0, 0, 1, 1))
add_body_name(pick_body, index)
#place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']),
# scale=scale, color=(0, 1, 0, 1))
#add_body_name(place_body, name)
world_from_obj_pick = get_pose(pick_body)
# [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane',
# u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane']
ee_grasp_poses = [{
name: pose_from_tform(parse_transform(approach))
for name, approach in json_grasp.items()
} for json_grasp in json_element['grasps']]
# pick_grasp_plane is at the top of the object with z facing downwards
grasp_index = 0
world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane']
world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane']
#draw_pose(world_from_ee_pick, length=0.04)
#draw_pose(world_from_ee_place, length=0.04)
ee_from_obj = multiply(invert(world_from_ee_pick),
world_from_obj_pick) # Using pick frame
world_from_obj_place = multiply(world_from_ee_place, ee_from_obj)
#set_pose(pick_body, world_from_obj_place)
grasps = [
Grasp(
index, num, *[
multiply(invert(world_from_ee_pick[name]),
world_from_obj_pick) for name in GRASP_NAMES
]) for num, world_from_ee_pick in enumerate(ee_grasp_poses)
]
brick_from_index[index] = Brick(
index=index,
body=pick_body,
initial_pose=WorldPose(index, world_from_obj_pick),
goal_pose=WorldPose(index, world_from_obj_place),
grasps=grasps,
goal_supports=json_element.get('place_contact_ngh_ids', []))
# pick_contact_ngh_ids are movable element contact partial orders
# pick_support_surface_file_names are fixed element contact partial orders
return robot, brick_from_index, obstacle_from_name
def main(floor_width=2.0):
# Creates a pybullet world and a visualizer for it
connect(use_gui=True)
identity_pose = (unit_point(), unit_quat())
origin_handles = draw_pose(
identity_pose, length=1.0
) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE)
# Bodies are described by an integer index
floor = create_box(w=floor_width, l=floor_width, h=0.001,
color=TAN) # Creates a tan box object for the floor
set_point(floor,
[0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor
obstacle = create_box(w=0.5, l=0.5, h=0.1,
color=RED) # Creates a red box obstacle
set_point(
obstacle,
[0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle
print('Position:', get_point(obstacle))
set_euler(obstacle,
[0, 0, np.pi / 4
]) # Sets the [roll,pitch,yaw] orientation of the obstacle
print('Orientation:', get_euler(obstacle))
with LockRenderer(
): # Temporarily prevents the renderer from updating for improved loading efficiency
with HideOutput(): # Temporarily suppresses pybullet output
robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file
robot_z = stable_z(
robot, floor
) # Returns the z offset required for robot to be placed on floor
set_point(robot,
[0, 0, robot_z]) # Sets the z position of the robot
dump_body(robot) # Prints joint and link information about robot
set_all_static()
# Joints are also described by an integer index
# The turtlebot has explicit joints representing x, y, theta
x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x'
y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y'
theta_joint = joint_from_name(
robot, 'theta') # Looks up the robot joint named 'theta'
joints = [x_joint, y_joint, theta_joint]
base_link = link_from_name(
robot, 'base_link') # Looks up the robot link named 'base_link'
world_from_obstacle = get_pose(
obstacle
) # Returns the pose of the origin of obstacle wrt the world frame
obstacle_aabb = get_subtree_aabb(obstacle)
draw_aabb(obstacle_aabb)
random.seed(0) # Sets the random number generator state
handles = []
for i in range(10):
for handle in handles:
remove_debug(handle)
print('\nIteration: {}'.format(i))
x = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, x_joint,
x) # Sets the current value of the x joint
y = random.uniform(-floor_width / 2., floor_width / 2.)
set_joint_position(robot, y_joint,
y) # Sets the current value of the y joint
yaw = random.uniform(-np.pi, np.pi)
set_joint_position(robot, theta_joint,
yaw) # Sets the current value of the theta joint
values = get_joint_positions(
robot,
joints) # Obtains the current values for the specified joints
print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values))
world_from_robot = get_link_pose(
robot,
base_link) # Returns the pose of base_link wrt the world frame
position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion)
x, y, z = position # Decomposing position into x, y, z
print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format(
x, y, z))
euler = euler_from_quat(
quaternion) # Converting from quaternion to euler angles
roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw
print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'.
format(roll, pitch, yaw))
handles.extend(
draw_pose(world_from_robot, length=0.5)
) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE)
obstacle_from_robot = multiply(
invert(world_from_obstacle),
world_from_robot) # Relative transformation from robot to obstacle
robot_aabb = get_subtree_aabb(
robot,
base_link) # Computes the robot's axis-aligned bounding box (AABB)
lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema
center = (lower + upper) / 2. # Computing the center of the AABB
extent = upper - lower # Computing the dimensions of the AABB
handles.extend(draw_aabb(robot_aabb))
collision = pairwise_collision(
robot, obstacle
) # Checks whether robot is currently colliding with obstacle
print('Collision: {}'.format(collision))
wait_for_duration(1.0) # Like sleep() but also updates the viewer
wait_for_user() # Like raw_input() but also updates the viewer
# Destroys the pybullet world
disconnect()
def eval_task_with_seed(domain,
seed,
learner,
sample_strategy=DIVERSELK,
task_lengthscale=None,
obstacle=True):
if task_lengthscale is not None:
learner.task_lengthscale = task_lengthscale
print('sample a new task')
current_wd, trial_wd = start_task()
### fill in additional object
item_ranges = {}
###
sim_world, collector, task, feature, evalfunc, saver = sample_task_with_seed(
domain.skill, seed=seed, visualize=False, item_ranges=item_ranges)
context = domain.context_from_feature(feature)
print('context=', *context)
# create coffee machine
if obstacle:
bowl_name = 'bowl'
cup_name = 'cup'
if domain.skill == 'scoop':
cup_name = 'spoon'
for key in sim_world.perception.sim_bodies:
if bowl_name in key:
bowl_name = key
if cup_name in key:
cup_name = key
cylinder_size = (.01, .03)
dim_bowl = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[bowl_name]))
dim_cup = get_aabb_extent(
p.getAABB(sim_world.perception.sim_bodies[cup_name]))
bowl_point = get_point(sim_world.perception.sim_bodies[bowl_name])
bowl_point = np.array(bowl_point)
cylinder_point = bowl_point.copy()
cylinder_offset = 1.2
if domain.skill == 'scoop':
cylinder_offset = 1.6
cylinder_point[2] += (dim_bowl[2] + dim_cup[2]) * (np.random.random_sample() * .4 + cylinder_offset) \
+ cylinder_size[1] / 2.
# TODO figure out the task space
cylinder_name = create_name('cylinder', 1)
obstacle = create_cylinder(*cylinder_size, color=(0, 0, 0, 1))
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(cylinder_name,
cylinder_size, 1, 1)
sim_world.perception.sim_bodies[cylinder_name] = obstacle
sim_world.perception.sim_items[cylinder_name] = None
set_pose(obstacle, (cylinder_point, unit_quat()))
box_name = create_name('box', 1)
box1_size = (max(.17,
dim_bowl[0] / 2 + cylinder_size[0] * 2), 0.01, 0.01)
if domain.skill == 'scoop':
box1_size = (max(.23,
dim_bowl[0] / 2 + cylinder_size[0] * 2 + 0.05),
0.01, 0.01)
obstacle = create_box(*box1_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box1_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box1_point = cylinder_point.copy()
box1_point[2] += cylinder_size[1] / 2 + box1_size[2] / 2
box1_point[0] += box1_size[0] / 2 - cylinder_size[0] * 2
set_pose(obstacle, (box1_point, unit_quat()))
box_name = create_name('box', 2)
box2_size = (0.01, .01,
box1_point[2] - bowl_point[2] + box1_size[2] / 2)
obstacle = create_box(*box2_size)
INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box2_size, 1,
1)
sim_world.perception.sim_bodies[box_name] = obstacle
sim_world.perception.sim_items[box_name] = None
box2_point = bowl_point.copy()
box2_point[2] += box2_size[2] / 2
box2_point[0] = box1_point[0] + box1_size[0] / 2
set_pose(obstacle, (box2_point, unit_quat()))
result = get_sample_strategy_result(sample_strategy, learner, context,
sim_world, collector, task, feature,
evalfunc, saver)
print('context=', *context)
complete_task(sim_world, current_wd, trial_wd)
return result