def create_meshes(ty, draw=False, visualize=False):
assert not (visualize and draw) # Incompatible?
suffix = SUFFIX_TEMPLATE.format(ty)
models_path = os.path.join(get_models_path(), MODELS_TEMPLATE.format(ty))
ensure_dir(models_path)
for prefix, properties in OBJECT_PROPERTIES[ty].items():
color = normalize_rgb(properties.color)
side = approximate_bowl(properties, d=2) # 1 doesn't seem to really work
name = prefix + suffix
print(name, color)
print(side)
if draw:
draw_curvature(side, name=name)
chunks = make_revolute_chunks(side, n_theta=60, n_chunks=10,
in_off=properties.thickness/4.,
scale=SCALE)
obj_path = os.path.join(models_path, OBJ_TEMPLATE.format(name))
write_obj(chunks, obj_path)
if visualize:
body = create_obj(obj_path, color=color)
_, dims = approximate_as_cylinder(body)
print(dims)
wait_for_user()
remove_body(body)
pcd_path = os.path.join(models_path, PCD_TEMPLATE.format(name))
pcd_from_mesh(obj_path, pcd_path)
def get_pour_feature(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_reference = get_reference_pose(bowl_name)
_, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body,
body_pose=bowl_reference)
bowl_vertices = vertices_from_rigid(bowl_body)
#bowl_mesh = read_obj(load_cup_bowl_obj(get_type(bowl_name))[0])
#print(len(bowl_vertices), len(bowl_mesh.vertices))
cup_body = world.get_body(cup_name)
cup_reference = (unit_point(), get_liquid_quat(cup_name))
_, (cup_d, _, cup_h) = approximate_as_prism(cup_body,
body_pose=cup_reference)
cup_vertices = vertices_from_rigid(cup_body)
#cup_mesh = read_obj(load_cup_bowl_obj(get_type(cup_name))[0])
# TODO: compute moments/other features from the mesh
feature = {
'bowl_name': bowl_name,
'bowl_type': get_type(bowl_name),
'bowl_diameter': bowl_d,
'bowl_height': bowl_h,
'bowl_base_diameter': compute_base_diameter(bowl_vertices),
'cup_name': cup_name,
'cup_type': get_type(cup_name),
'cup_diameter': cup_d,
'cup_height': cup_h,
'cup_base_diameter': compute_base_diameter(cup_vertices),
}
return feature
def get_scoop_feature(world, bowl_name, spoon_name):
bowl_body = world.get_body(bowl_name)
_, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
bowl_vertices = vertices_from_rigid(bowl_body)
#bowl_mesh = read_obj(load_cup_bowl_obj(get_type(bowl_name))[0])
#print(len(bowl_vertices), len(bowl_mesh.vertices))
spoon_c, (spoon_w, spoon_l, spoon_h) = approximate_as_prism(
world.get_body(spoon_name),
body_pose=(unit_point(), lookup_orientation(spoon_name, STIR_QUATS)))
# TODO: compute moments/other features from the mesh
feature = {
'bowl_name': bowl_name,
'bowl_type': get_type(bowl_name),
'bowl_diameter': bowl_d,
'bowl_height': bowl_h,
'bowl_base_diameter': compute_base_diameter(bowl_vertices),
# In stirring orientation
'spoon_name': spoon_name,
'spoon_type': get_type(spoon_name),
'spoon_width': spoon_w,
'spoon_length': spoon_l,
'spoon_height': spoon_h,
}
return feature
def pour_beads(world,
cup_name,
beads,
reset_contained=False,
fix_outside=True,
cup_thickness=0.01,
bead_offset=0.01,
drop_rate=0.02,
**kwargs):
if not beads:
return set()
start_time = time.time()
# TODO: compute the radius of each bead
bead_radius = np.average(approximate_as_prism(beads[0])) / 2.
masses = list(map(get_mass, beads))
savers = list(map(BodySaver, beads))
for body in beads:
set_mass(body, 0)
cup = world.get_body(cup_name)
local_center, (diameter, height) = approximate_as_cylinder(cup)
center = get_point(cup) + local_center
interior_radius = max(0.0, diameter / 2. - bead_radius - cup_thickness)
# TODO: fill up to a certain threshold
ty = get_type(cup_name)
if ty in SPOON_DIAMETERS:
# TODO: do this in a more principled way
interior_radius = 0
center[1] += (SPOON_LENGTHS[ty] - SPOON_DIAMETERS[ty]) / 2.
# TODO: some sneak out through the bottom
# TODO: could reduce gravity while filling
world.controller.set_gravity()
for i, bead in enumerate(beads):
# TODO: halton sequence
x, y = center[:2] + np.random.uniform(
0, interior_radius) * unit_from_theta(
np.random.uniform(-np.pi, np.pi))
new_z = center[2] + height / 2. + bead_radius + bead_offset
set_point(bead, [x, y, new_z])
set_mass(bead, masses[i])
world.controller.rest_for_duration(drop_rate)
world.controller.rest_for_duration(BEADS_REST)
print('Simulated {} beads in {:3f} seconds'.format(
len(beads), elapsed_time(start_time)))
contained_beads = get_contained_beads(cup, beads, **kwargs)
#wait_for_user()
for body in beads:
if fix_outside and (body not in contained_beads):
set_mass(body, 0)
for saver in savers:
if reset_contained or (saver.body not in contained_beads):
saver.restore()
#wait_for_user()
return contained_beads
def pour_path_from_parameter(world, bowl_name, cup_name):
bowl_body = world.get_body(bowl_name)
bowl_center, (bowl_d, bowl_h) = approximate_as_cylinder(bowl_body)
cup_body = world.get_body(cup_name)
cup_center, (cup_d, _, cup_h) = approximate_as_prism(cup_body)
#####
obj_type = type_from_name(cup_name)
if obj_type in [MUSTARD]:
initial_pitch = final_pitch = -np.pi
radius = 0
else:
initial_pitch = 0 # different if mustard
final_pitch = -3 * np.pi / 4
radius = bowl_d / 2
#axis_in_cup_center_x = -0.05
axis_in_cup_center_x = 0 # meters
#axis_in_cup_center_z = -cup_h/2.
axis_in_cup_center_z = 0. # meters
#axis_in_cup_center_z = +cup_h/2.
# tl := top left | tr := top right
cup_tl_in_center = np.array([-cup_d / 2, 0, cup_h / 2])
cup_tl_in_axis = cup_tl_in_center - Point(z=axis_in_cup_center_z)
cup_tl_angle = np.math.atan2(cup_tl_in_axis[2], cup_tl_in_axis[0])
cup_tl_pour_pitch = final_pitch - cup_tl_angle
cup_radius2d = np.linalg.norm([cup_tl_in_axis])
pivot_in_bowl_tr = Point(
x=-(cup_radius2d * np.math.cos(cup_tl_pour_pitch) + 0.01),
z=(cup_radius2d * np.math.sin(cup_tl_pour_pitch) + Z_OFFSET))
pivot_in_bowl_center = Point(x=radius, z=bowl_h / 2) + pivot_in_bowl_tr
base_from_pivot = Pose(
Point(x=axis_in_cup_center_x, z=axis_in_cup_center_z))
#####
assert -np.pi <= final_pitch <= initial_pitch
pitches = [initial_pitch]
if final_pitch != initial_pitch:
pitches = list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + pitches
cup_path_in_bowl = []
for pitch in pitches:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(Pose(point=bowl_center), Pose(pivot_in_bowl_center),
rotate_pivot, invert(base_from_pivot),
invert(Pose(point=cup_center))))
return cup_path_in_bowl
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 collect_stir(world, num_beads=100):
arm = LEFT_ARM
# TODO: randomize geometries for stirrer
spoon_name = create_name(
'grey_spoon', 1) # green_spoon | grey_spoon | orange_spoon | stirrer
bowl_name = create_name('whitebowl', 1)
scale_name = create_name('onyx_scale', 1)
item_ranges = {
spoon_name:
InitialRanges(
width_range=(1., 1.),
height_range=(1., 1.),
mass_range=(1., 1.),
pose2d_range=([0.3, 0.5, -np.pi / 2], [0.3, 0.5, -np.pi / 2]),
surface=TABLE_NAME,
),
bowl_name:
InitialRanges(
width_range=(0.75, 1.25),
height_range=(0.75, 1.25),
mass_range=(1., 1.),
pose2d_range=([0.5, -0.05, -np.pi], [0.6, 0.05,
np.pi]), # x, y, theta
surface=scale_name,
),
}
# TODO: make a sandbox on the table to contain the beads
##################################################
#alpha = 0.75
alpha = 1
bead_colors = [
(1, 0, 0, alpha),
(0, 0, 1, alpha),
]
beads_fraction = random.uniform(0.75, 1.25)
print('Beads fraction:', beads_fraction)
bead_radius = sample_norm(mu=0.006, sigma=0.001, lower=0.004) # 0.007
print('Bead radius:', bead_radius)
# TODO: check collisions/feasibility when sampling
# TODO: grasps on the blue cup seem off for some reason...
with ClientSaver(world.client):
#dump_body(world.robot)
world.perception.add_surface('sandbox', TABLE_POSE)
world.perception.add_surface(scale_name, TABLE_POSE)
create_table_bodies(world, item_ranges)
#gripper = create_gripper(world.robot, LEFT_ARM)
#set_point(gripper, (1, 0, 1))
#wait_for_user()
bowl_body = world.get_body(bowl_name)
update_world(world, target_body=bowl_body)
init_holding = hold_item(world, arm, spoon_name)
if init_holding is None:
return INFEASIBLE
parameters_from_name = randomize_dynamics(
world) # TODO: parameters_from_name['bead']
_, (d, h) = approximate_as_cylinder(bowl_body)
bowl_area = np.pi * (d / 2.)**2
print('Bowl area:', bowl_area)
bead_area = np.pi * bead_radius**2
print('Bead area:', bead_area)
num_beads = int(np.ceil(beads_fraction * bowl_area / bead_area))
print('Num beads:', num_beads)
num_per_color = int(num_beads / len(bead_colors))
# TODO: randomize bead physics
beads_per_color = [
fill_with_beads(
world, bowl_name,
create_beads(num_beads,
bead_radius,
uniform_color=color,
parameters={})) for color in bead_colors
]
world.initial_beads.update(
{bead: bowl_body
for beads in beads_per_color for bead in beads})
if any(len(beads) != num_per_color for beads in beads_per_color):
return INFEASIBLE
#wait_for_user()
init = [('Contains', bowl_name, COFFEE)]
goal = [('Mixed', bowl_name)]
skeleton = [
('move-arm', [arm, X, X, X]),
('stir', [arm, bowl_name, X, spoon_name, X, X, X, X]),
#('move-arm', [arm, X, X, X]),
]
constraints = PlanConstraints(skeletons=[skeleton], exact=True)
task = Task(init=init,
goal=goal,
arms=[arm],
init_holding=init_holding,
reset_arms=False,
constraints=constraints) # Reset arm to clear the scene
# TODO: constrain the plan skeleton within the task
feature = get_stir_feature(world, bowl_name, spoon_name)
##################################################
# table_body = world.get_body(TABLE_NAME)
# dump_body(table_body)
# joint = 0
# p.enableJointForceTorqueSensor(table_body, joint, enableSensor=1, physicsClientId=world.client)
# stabilize(world)
# reaction_force = get_joint_reaction_force(table_body, joint)
# print(np.array(reaction_force[:3])/ GRAVITY)
perception = world.perception
initial_pose = perception.get_pose(bowl_name)
bowl_body = perception.get_body(bowl_name)
scale_body = perception.get_body(scale_name)
with ClientSaver(world.client):
initial_distance = compute_dispersion(bowl_body, beads_per_color)
initial_mass = read_mass(scale_body)
print(initial_mass)
def score_fn(plan):
assert plan is not None
with ClientSaver(world.client):
rgb_image = take_image(world, bowl_body, beads_per_color)
values = score_image(rgb_image, bead_colors, beads_per_color)
final_pose = perception.get_pose(bowl_name)
point_distance = get_distance(point_from_pose(initial_pose),
point_from_pose(final_pose)) #, norm=2)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print('Translation: {:.5f} m | Rotation: {:.5f} rads'.format(
point_distance, quat_distance))
with ClientSaver(world.client):
all_beads = list(flatten(beads_per_color))
bowl_beads = get_contained_beads(bowl_body, all_beads)
fraction_bowl = float(
len(bowl_beads)) / len(all_beads) if all_beads else 0
print('In Bowl: {}'.format(fraction_bowl))
with ClientSaver(world.client):
final_dispersion = compute_dispersion(bowl_body, beads_per_color)
print('Initial Dispersion: {:.3f} | Final Dispersion {:.3f}'.format(
initial_distance, final_dispersion))
score = {
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
'fraction_in_bowl': fraction_bowl,
'initial_dispersion': initial_distance,
'final_dispersion': final_dispersion,
'num_beads': len(all_beads), # Beads per color
DYNAMICS: parameters_from_name,
}
# TODO: include time required for stirring
# TODO: change in dispersion
#wait_for_user()
#_, args = find_unique(lambda a: a[0] == 'stir', plan)
#control = args[-1]
return score
return task, feature, score_fn