def fix_pose(self, name, pose=None, fraction=0.5):
body = self.get_body(name)
if pose is None:
pose = get_pose(body)
else:
set_pose(body, pose)
# TODO: can also drop in simulation
x, y, z = point_from_pose(pose)
roll, pitch, yaw = euler_from_quat(quat_from_pose(pose))
quat = quat_from_euler(Euler(yaw=yaw))
set_quat(body, quat)
surface_name = self.get_supporting(name)
if surface_name is None:
return None, None
if fraction == 0:
new_pose = (Point(x, y, z), quat)
return new_pose, surface_name
surface_aabb = compute_surface_aabb(self, surface_name)
new_z = (1 - fraction) * z + fraction * stable_z_on_aabb(
body, surface_aabb)
point = Point(x, y, new_z)
set_point(body, point)
print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format(
name, roll, pitch, new_z - z))
new_pose = (point, quat)
return new_pose, surface_name
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 test_push(visualize):
arms = [LEFT_ARM]
block_name = create_name('purpleblock', 1) # greenblock | purpleblock
world, table_body = create_world([block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
#pos = (0.6, 0, block_z)
pos = (0.5, 0.2, block_z)
world.perception.set_pose(block_name, pos, quat_from_euler(Euler(yaw=-np.pi/6)))
update_world(world, table_body)
# TODO: push into reachable region
goal_pos2d = np.array(pos[:2]) + np.array([0.025, 0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([0.025, -0.1])
#goal_pos2d = np.array(pos[:2]) + np.array([-0.1, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0.15, -0.05])
#goal_pos2d = np.array(pos[:2]) + np.array([0, 0.7]) # Intentionally not feasible
draw_point(np.append(goal_pos2d, [block_z]), size=0.05, color=(1, 0, 0))
init = [
('CanPush', block_name, goal_pos2d),
]
goal = [
('InRegion', block_name, goal_pos2d),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def main(use_pr2_drake=True):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
table_path = "models/table_collision/table.urdf"
table = load_pybullet(table_path, fixed_base=True)
set_quat(table, quat_from_euler(Euler(yaw=PI / 2)))
# table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
obstacles = [plane, table]
pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF
with HideOutput():
pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings?
dump_body(pr2)
z = base_aligned_z(pr2)
print(z)
#z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3)))
print(z)
set_point(pr2, Point(z=z))
print(get_aabb(pr2))
wait_if_gui()
base_start = (-2, -2, 0)
base_goal = (2, 2, 0)
arm_start = SIDE_HOLDING_LEFT_ARM
#arm_start = TOP_HOLDING_LEFT_ARM
#arm_start = REST_LEFT_ARM
arm_goal = TOP_HOLDING_LEFT_ARM
#arm_goal = SIDE_HOLDING_LEFT_ARM
left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm'])
right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm'])
torso_joints = joints_from_names(pr2, PR2_GROUPS['torso'])
set_joint_positions(pr2, left_joints, arm_start)
set_joint_positions(pr2, right_joints,
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, torso_joints, [0.2])
open_arm(pr2, 'left')
# test_ikfast(pr2)
add_line(base_start, base_goal, color=RED)
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
else:
test_base_motion(pr2, base_start, base_goal, obstacles=obstacles)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
wait_if_gui('Finish?')
disconnect()
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 create_elements_bodies(node_points,
elements,
color=apply_alpha(RED, alpha=1),
diameter=ELEMENT_DIAMETER,
shrink=ELEMENT_SHRINK):
# TODO: could scale the whole environment
# TODO: create a version without shrinking for transit planning
# URDF_USE_IMPLICIT_CYLINDER
element_bodies = []
for (n1, n2) in elements:
p1, p2 = node_points[n1], node_points[n2]
height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0)
#if height == 0: # Cannot keep this here
# continue
center = (p1 + p2) / 2
# extents = (p2 - p1) / 2
delta = p2 - p1
x, y, z = delta
phi = np.math.atan2(y, x)
theta = np.math.acos(z / np.linalg.norm(delta))
quat = quat_from_euler(Euler(pitch=theta, yaw=phi))
# p1 is z=-height/2, p2 is z=+height/2
# Much smaller than cylinder
# Also faster, C_shape 177.398 vs 400
body = create_box(diameter,
diameter,
height,
color=color,
mass=STATIC_MASS)
set_color(body, color)
set_point(body, center)
set_quat(body, quat)
#dump_body(body, fixed=True)
# Visually, smallest diameter is 2e-3
# The geometries and bounding boxes seem correct though
# TODO: create_cylinder takes in a radius not diameter
#body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS)
#print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height))
#print(get_aabb_extent(get_aabb(body)).round(6).tolist())
#print(get_visual_data(body))
#print(get_collision_data(body))
#set_point(body, center)
#set_quat(body, quat)
#draw_aabb(get_aabb(body))
element_bodies.append(body)
#wait_for_user()
return element_bodies
def test_stir(visualize):
# TODO: change the stirrer coordinate frame to be on its side
arms = [LEFT_ARM]
#spoon_name = 'spoon' # spoon.urdf is very messy and has a bad bounding box
#spoon_quat = multiply_quats(quat_from_euler(Euler(pitch=-np.pi/2 - np.pi/16)), quat_from_euler(Euler(yaw=-np.pi/8)))
#spoon_name, spoon_quat = 'stirrer', quat_from_euler(Euler(roll=-np.pi/2, yaw=np.pi/2))
spoon_name, spoon_quat = 'grey_spoon', quat_from_euler(Euler(yaw=-np.pi/2)) # grey_spoon | orange_spoon | green_spoon
# *_spoon points in +y
#bowl_name = 'bowl'
bowl_name = 'whitebowl'
items = [spoon_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_quat(world.get_body(spoon_name), spoon_quat) # get_reference_pose(spoon_name)
initial_positions = {
#spoon_name: [0.5, -0.2, 0],
spoon_name: [0.3, 0.5, 0],
bowl_name: [0.6, 0.1, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
#draw_aabb(get_aabb(body))
#wait_for_interrupt()
#enable_gravity()
#for i in range(10):
# simulate_for_sim_duration(sim_duration=0.05, frequency=0.1)
# print(get_quat(world.perception.sim_bodies[spoon_name]))
# raw_input('Continue?')
update_world(world, table_body)
init_holding = hold_item(world, arms[0], spoon_name)
assert init_holding is not None
# TODO: add the concept of a recipe
init = [
('Contains', bowl_name, COFFEE),
('Contains', bowl_name, SUGAR),
]
goal = [
#('Holding', spoon_name),
('Mixed', bowl_name),
]
task = Task(init=init, init_holding=init_holding, goal=goal,
arms=arms, reset_arms=False)
return world, task
def test_scoop(visualize):
# TODO: start with the spoon in a hand
arms = [LEFT_ARM]
spoon_name = 'grey_spoon' # green_spoon | grey_spoon | orange_spoon
spoon_quat = quat_from_euler(Euler(yaw=-np.pi/2))
# *_spoon points in +y
bowl1_name = 'whitebowl'
bowl2_name = 'bowl'
items = [spoon_name, bowl1_name, bowl2_name]
world, table_body = create_world(items, visualize=visualize)
set_quat(world.get_body(spoon_name), spoon_quat)
initial_positions = {
spoon_name: [0.3, 0.5, 0],
bowl1_name: [0.5, 0.1, 0],
bowl2_name: [0.6, 0.5, 0],
}
with ClientSaver(world.perception.client):
for name, point in initial_positions.items():
body = world.perception.sim_bodies[name]
point[2] += stable_z(body, table_body) + Z_EPSILON
world.perception.set_pose(name, point, get_quat(body))
update_world(world, table_body)
init = [
('Contains', bowl1_name, COFFEE),
#('Contains', spoon_name, WATER),
]
goal = [
#('Contains', spoon_name, WATER),
('Contains', bowl2_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms, reset_arms=True)
# TODO: plan while not spilling the spoon
return world, task
}
GRIPPER_LINKS = {
'left': 'l_gripper_palm_link',
'right': 'r_gripper_palm_link',
}
STABLE_QUATS = {
# TODO(caelan): rename to placement orientation
#'bluecup': quat_from_euler(Euler(roll=np.pi)),
#'bowl': quat_from_euler(Euler(roll=np.pi)),
#'spoon': (0.11923844791406242, -0.7234117212002504, -0.13249733589431234, 0.6670098426185677),
}
LIQUID_QUATS = {
'spoon': quat_from_euler(Euler(pitch=np.pi)),
}
STIR_QUATS = {
'spoon': quat_from_euler(Euler(roll=-np.pi / 2)),
}
# TODO: avoid using this
COLORS = {
'red': (1, 0, 0),
'green': (0, 1, 0),
'blue': (0, 0, 1),
'black': (0, 0, 0),
'white': (1, 1, 1),
'purple': (1, 0, 1),
'orange': (1, 0.6, 0),
def gen(obj_name, surface_name):
obj_body = world.get_body(obj_name)
surface_body = world.kitchen
if surface_name in ENV_SURFACES:
surface_body = world.environment_bodies[surface_name]
surface_aabb = compute_surface_aabb(world, surface_name)
learned_poses = load_placements(world, surface_name) if learned else [
] # TODO: GROW_PLACEMENT
yaw_range = (-np.pi, np.pi)
#if world.is_real():
# center = -np.pi/4
# half_extent = np.pi / 16
# yaw_range = (center-half_extent, center+half_extent)
while True:
for _ in range(max_attempts):
if surface_name in STOVES:
surface_link = link_from_name(world.kitchen, surface_name)
world_from_surface = get_link_pose(world.kitchen,
surface_link)
z = stable_z_on_aabb(
obj_body,
surface_aabb) - point_from_pose(world_from_surface)[2]
yaw = random.uniform(*yaw_range)
body_pose_surface = Pose(Point(z=z + z_offset),
Euler(yaw=yaw))
body_pose_world = multiply(world_from_surface,
body_pose_surface)
elif learned:
if not learned_poses:
return
surface_pose_world = get_surface_reference_pose(
surface_body, surface_name)
sampled_pose_surface = multiply(
surface_pose_world, random.choice(learned_poses))
[x, y, _] = point_from_pose(sampled_pose_surface)
_, _, yaw = euler_from_quat(
quat_from_pose(sampled_pose_surface))
dx, dy = np.random.normal(
scale=pos_scale, size=2) if pos_scale else np.zeros(2)
# TODO: avoid reloading
z = stable_z_on_aabb(obj_body, surface_aabb)
yaw = random.uniform(*yaw_range)
#yaw = wrap_angle(yaw + np.random.normal(scale=rot_scale))
quat = quat_from_euler(Euler(yaw=yaw))
body_pose_world = ([x + dx, y + dy, z + z_offset], quat)
# TODO: project onto the surface
else:
# TODO: halton sequence
# unit_generator(d, use_halton=True)
body_pose_world = sample_placement_on_aabb(
obj_body, surface_aabb, epsilon=z_offset, percent=2.0)
if body_pose_world is None:
continue # return?
if visibility and not is_visible_by_camera(
world, point_from_pose(body_pose_world)):
continue
# TODO: make sure the surface is open when doing this
robust = True
if robust_radius != 0.:
for theta in np.linspace(0, 5 * np.pi, num=8):
x, y = robust_radius * unit_from_theta(theta)
delta_body = Pose(Point(x, y))
delta_world = multiply(body_pose_world, delta_body)
set_pose(obj_body, delta_world)
if not test_supported(world,
obj_body,
surface_name,
collisions=collisions):
robust = False
break
set_pose(obj_body, body_pose_world)
if robust and test_supported(
world, obj_body, surface_name, collisions=collisions):
rp = create_relative_pose(world, obj_name, surface_name)
yield (rp, )
break
else:
yield None