def get_grasps(world, name, grasp_types=GRASP_TYPES, pre_distance=APPROACH_DISTANCE, **kwargs):
use_width = world.robot_name == FRANKA_CARTER
body = world.get_body(name)
#fraction = 0.25
obj_type = type_from_name(name)
body_pose = REFERENCE_POSE.get(obj_type, unit_pose())
center, extent = approximate_as_prism(body, body_pose)
for grasp_type in grasp_types:
if not implies(world.is_real(), is_valid_grasp_type(name, grasp_type)):
continue
#assert is_valid_grasp_type(name, grasp_type)
if grasp_type == TOP_GRASP:
grasp_length = 1.5 * FINGER_EXTENT[2] # fraction = 0.5
pre_direction = pre_distance * get_unit_vector([0, 0, 1])
post_direction = unit_point()
generator = get_top_grasps(body, under=True, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, max_width=np.inf, **kwargs)
elif grasp_type == SIDE_GRASP:
# Take max of height and something
grasp_length = 1.75 * FINGER_EXTENT[2] # No problem if pushing a little
x, z = pre_distance * get_unit_vector([3, -1])
pre_direction = [0, 0, x]
post_direction = [0, 0, z]
top_offset = extent[2] / 2 if obj_type in MID_SIDE_GRASPS else 1.0*FINGER_EXTENT[0]
# Under grasps are actually easier for this robot
# TODO: bug in under in that it grasps at the bottom
generator = get_side_grasps(body, under=False, tool_pose=TOOL_POSE, body_pose=body_pose,
grasp_length=grasp_length, top_offset=top_offset, max_width=np.inf, **kwargs)
# if world.robot_name == FRANKA_CARTER else unit_pose()
generator = (multiply(Pose(euler=Euler(yaw=yaw)), grasp)
for grasp in generator for yaw in [0, np.pi])
else:
raise ValueError(grasp_type)
grasp_poses = randomize(list(generator))
if obj_type in CYLINDERS:
# TODO: filter first
grasp_poses = (multiply(grasp_pose, Pose(euler=Euler(
yaw=random.uniform(-math.pi, math.pi)))) for grasp_pose in cycle(grasp_poses))
for i, grasp_pose in enumerate(grasp_poses):
pregrasp_pose = multiply(Pose(point=pre_direction), grasp_pose,
Pose(point=post_direction))
grasp = Grasp(world, name, grasp_type, i, grasp_pose, pregrasp_pose)
with BodySaver(body):
grasp.get_attachment().assign()
with BodySaver(world.robot):
grasp.grasp_width = close_until_collision(
world.robot, world.gripper_joints, bodies=[body])
#print(get_joint_positions(world.robot, world.arm_joints)[-1])
#draw_pose(unit_pose(), parent=world.robot, parent_link=world.tool_link)
#grasp.get_attachment().assign()
#wait_for_user()
##for value in get_joint_limits(world.robot, world.arm_joints[-1]):
#for value in [-1.8973, 0, +1.8973]:
# set_joint_position(world.robot, world.arm_joints[-1], value)
# grasp.get_attachment().assign()
# wait_for_user()
if use_width and (grasp.grasp_width is None):
continue
yield grasp
def sample_push_contact(world, feature, parameter, under=False):
robot = world.robot
arm = feature['arm_name']
body = world.get_body(feature['block_name'])
push_yaw = feature['push_yaw']
center, (width, _, height) = approximate_as_prism(body, body_pose=Pose(euler=Euler(yaw=push_yaw)))
max_backoff = width + 0.1 # TODO: add gripper bounding box
tool_link = link_from_name(robot, TOOL_FRAMES[arm])
tool_pose = get_link_pose(robot, tool_link)
gripper_link = link_from_name(robot, GRIPPER_LINKS[arm])
collision_links = get_link_subtree(robot, gripper_link)
urdf_from_center = Pose(point=center)
reverse_z = Pose(euler=Euler(pitch=math.pi))
rotate_theta = Pose(euler=Euler(yaw=push_yaw))
#translate_z = Pose(point=Point(z=-feature['block_height']/2. + parameter['gripper_z'])) # Relative to base
translate_z = Pose(point=Point(z=parameter['gripper_z'])) # Relative to center
tilt_gripper = Pose(euler=Euler(pitch=parameter['gripper_tilt']))
grasps = []
for i in range(1 + under):
flip_gripper = Pose(euler=Euler(yaw=i * math.pi))
for x in np.arange(0, max_backoff, step=0.01):
translate_x = Pose(point=Point(x=-x))
grasp_pose = multiply(flip_gripper, tilt_gripper, translate_x, translate_z, rotate_theta,
reverse_z, invert(urdf_from_center))
set_pose(body, multiply(tool_pose, TOOL_POSE, grasp_pose))
if not link_pairs_collision(robot, collision_links, body, collision_buffer=0.):
grasps.append(grasp_pose)
break
return grasps
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 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 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_spoon_grasps(name, body, under=False, grasp_length=0.02):
# TODO: scale thickness based on size
thickness = SPOON_THICKNESSES[get_type(name)]
# Origin is in the center of the spoon's hemisphere head
# grasp_length depends on the grasp position
center, extent = approximate_as_prism(body)
for k in range(1 + under):
rotate_y = Pose(euler=Euler(pitch=-np.pi / 2 + np.pi * k))
rotate_z = Pose(euler=Euler(yaw=-np.pi / 2))
length = (-center + extent / 2)[1] - grasp_length
translate_x = Pose(point=Point(x=length, y=-thickness / 2.))
gripper_from_spoon = multiply(translate_x, rotate_z, rotate_y)
yield gripper_from_spoon
def get_contained_beads(body, beads, height_fraction=1.0, top_threshold=0.0):
#aabb = get_aabb(body)
center, extent = approximate_as_prism(body, body_pose=get_pose(body))
lower = center - extent / 2.
upper = center + extent / 2.
_, _, z1 = lower
x2, y2, z2 = upper
z_upper = z1 + height_fraction * (z2 - z1) + top_threshold
new_aabb = AABB(lower, np.array([x2, y2, z_upper]))
#handles = draw_aabb(new_aabb)
return {
bead
for bead in beads if aabb_contains_aabb(get_aabb(bead), new_aabb)
}
def get_placement_surface(body):
# TODO: check that only has one link
visual_data = get_visual_data(body, link=BASE_LINK)
if (len(visual_data) != 1) or (visual_data[0].visualGeometryType != p.GEOM_MESH):
center, (w, l, h) = approximate_as_prism(body)
mesh = rectangular_mesh(w, l)
local_pose = Pose(center + np.array([0, 0, h/2.]))
#handles = draw_mesh(mesh)
#wait_for_user()
return mesh, local_pose
mesh = read_obj(visual_data[0].meshAssetFileName)
local_pose = (visual_data[0].localVisualFrame_position,
visual_data[0].localVisualFrame_orientation)
return mesh, local_pose
def get_push_feature(world, arm, block_name, initial_pose, goal_pos2d):
block_body = world.get_body(block_name)
block_reference = get_reference_pose(block_name)
_, (block_w, block_l, block_h) = approximate_as_prism(block_body, body_pose=block_reference)
goal_pose = get_end_pose(initial_pose, goal_pos2d)
difference_initial = point_from_pose(multiply(invert(initial_pose), goal_pose))
feature = {
'arm_name': arm,
'block_name': block_name,
'block_type': get_type(block_name),
'block_width': block_w,
'block_length': block_l,
'block_height': block_h,
'push_yaw': get_yaw(difference_initial),
'push_distance': get_length(difference_initial)
}
return feature
def pour_path_from_parameter(world, feature, parameter, cup_yaw=None):
cup_body = world.get_body(feature['cup_name'])
#cup_urdf_from_center = get_urdf_from_center(cup_body, reference_quat=get_liquid_quat(feature['cup_name']))
ref_from_urdf = (unit_point(), get_liquid_quat(feature['cup_name']))
cup_center_in_ref, _ = approximate_as_prism(cup_body,
body_pose=ref_from_urdf)
cup_center_in_ref[:
2] = 0 # Assumes the xy pour center is specified by the URDF (e.g. for spoons)
cup_urdf_from_center = multiply(invert(ref_from_urdf),
Pose(point=cup_center_in_ref))
# TODO: allow some deviation around cup_yaw for spoons
if cup_yaw is None:
cup_yaw = random.choice([0, np.pi]) if 'spoon' in feature['cup_name'] \
else random.uniform(-np.pi, np.pi)
z_rotate_cup = Pose(euler=Euler(yaw=cup_yaw))
bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter)
if RELATIVE_POUR:
parameter = scale_parameter(feature,
parameter,
RELATIVE_POUR_SCALING,
descale=True)
base_from_pivot = Pose(
Point(x=parameter['axis_in_cup_x'], z=parameter['axis_in_cup_z']))
initial_pitch = 0
final_pitch = parameter['pitch']
assert -np.pi <= final_pitch <= initial_pitch
cup_path_in_bowl = []
for pitch in list(np.arange(final_pitch, initial_pitch,
np.pi / 16)) + [initial_pitch]:
rotate_pivot = Pose(
euler=Euler(pitch=pitch)
) # Can also interpolate directly between start and end quat
cup_path_in_bowl.append(
multiply(bowl_from_pivot, rotate_pivot, invert(base_from_pivot),
z_rotate_cup, invert(cup_urdf_from_center)))
cup_times = constant_velocity_times(cup_path_in_bowl)
# TODO: check for collisions here?
return cup_path_in_bowl, cup_times
def estimate_spoon_capacity(world, spoon_name, beads, max_beads=100):
beads = beads[:max_beads]
if not beads:
return 0
bead_radius = np.average(approximate_as_prism(beads[0])) / 2.
spoon_body = world.get_body(spoon_name)
spoon_mass = get_mass(spoon_body)
set_mass(spoon_body, STATIC_MASS)
set_point(spoon_body, (1, 0, 1))
set_quat(spoon_body, get_liquid_quat(spoon_name))
capacity_beads = fill_with_beads(world,
spoon_name,
beads,
reset_contained=True,
fix_outside=False,
height_fraction=1.0,
top_threshold=bead_radius)
#wait_for_user()
set_mass(spoon_body, spoon_mass)
return len(capacity_beads)
def _initialize_environment(self):
# wall to fridge: 4cm
# fridge to goal: 1.5cm
# hitman to range: 3.5cm
# range to indigo: 3.5cm
self.environment_poses = read_json(POSES_PATH)
root_from_world = get_link_pose(self.kitchen, self.world_link)
for name, world_from_part in self.environment_poses.items():
if name in ['range']:
continue
visual_path = os.path.join(KITCHEN_LEFT_PATH,
'{}.obj'.format(name))
collision_path = os.path.join(KITCHEN_LEFT_PATH,
'{}_collision.obj'.format(name))
mesh_path = None
for path in [collision_path, visual_path]:
if os.path.exists(path):
mesh_path = path
break
if mesh_path is None:
continue
body = load_pybullet(mesh_path, fixed_base=True)
root_from_part = multiply(root_from_world, world_from_part)
if name in ['axe', 'dishwasher', 'echo', 'fox', 'golf']:
(pos, quat) = root_from_part
# TODO: still not totally aligned
pos = np.array(pos) + np.array([0, -0.035, 0]) # , -0.005])
root_from_part = (pos, quat)
self.environment_bodies[name] = body
set_pose(body, root_from_part)
# TODO: release bounding box or convex hull
# TODO: static object nonconvex collisions
if TABLE_NAME in self.environment_bodies:
body = self.environment_bodies[TABLE_NAME]
_, (w, l, _) = approximate_as_prism(body)
_, _, z = get_point(body)
new_pose = Pose(Point(TABLE_X + l / 2, -TABLE_Y, z),
Euler(yaw=np.pi / 2))
set_pose(body, new_pose)
def visualize_vector_field(learner, bowl_type='blue_bowl', cup_type='red_cup',
num=500, delta=False, alpha=0.5):
print(learner, len(learner.results))
xx, yy, ww = learner.xx, learner.yy, learner.weights
learner.hyperparameters = get_parameters(learner.model)
print(learner.hyperparameters)
learner.query_type = REJECTION # BEST | REJECTION | STRADDLE
world = create_world()
world.bodies[bowl_type] = load_cup_bowl(bowl_type)
world.bodies[cup_type] = load_cup_bowl(cup_type)
feature = get_pour_feature(world, bowl_type, cup_type)
set_point(world.bodies[cup_type], np.array([0.2, 0, 0]))
# TODO: visualize training data as well
# TODO: artificially limit training data to make a low-confidence region
# TODO: visualize mean and var at the same time using intensity and hue
print('Feature:', feature)
with LockRenderer():
#for parameter in islice(learner.parameter_generator(world, feature, valid=True), num):
parameters = []
while len(parameters) < num:
parameter = learner.sample_parameter()
# TODO: special color if invalid
if is_valid_pour(world, feature, parameter):
parameters.append(parameter)
center, _ = approximate_as_prism(world.get_body(cup_type))
bodies = []
with LockRenderer():
for parameter in parameters:
path, _ = pour_path_from_parameter(world, feature, parameter)
pose = path[0]
body = create_cylinder(radius=0.001, height=0.02, color=apply_alpha(GREY, alpha))
set_pose(body, multiply(pose, Pose(point=center)))
bodies.append(body)
#train_sizes = inclusive_range(10, 100, 1)
#train_sizes = inclusive_range(10, 100, 10)
#train_sizes = inclusive_range(100, 1000, 100)
#train_sizes = [1000]
train_sizes = [None]
# training_poses = []
# for result in learner.results[:train_sizes[-1]]:
# path, _ = pour_path_from_parameter(world, feature, result['parameter'])
# pose = path[0]
# training_poses.append(pose)
# TODO: draw the example as well
scores_per_size = []
for train_size in train_sizes:
if train_size is not None:
learner.xx, learner.yy, learner.weights = xx[:train_size], yy[:train_size], ww[:train_size]
learner.retrain()
X = np.array([learner.func.x_from_feature_parameter(feature, parameter) for parameter in parameters])
predictions = learner.predict_x(X, noise=False) # Noise is just a constant
scores_per_size.append([prediction['mean'] for prediction in predictions]) # mean | variance
#scores_per_size.append([1./np.sqrt(prediction['variance']) for prediction in predictions])
#scores_per_size.append([prediction['mean'] / np.sqrt(prediction['variance']) for prediction in predictions])
#plt.hist(scores_per_size[-1])
#plt.show()
#scores_per_size.append([scipy.stats.norm.interval(alpha=0.95, loc=prediction['mean'],
# scale=np.sqrt(prediction['variance']))[0]
# for prediction in predictions]) # mean | variance
# score = learner.score(feature, parameter)
if delta:
scores_per_size = [np.array(s2) - np.array(s1) for s1, s2 in zip(scores_per_size, scores_per_size[1:])]
train_sizes = train_sizes[1:]
all_scores = [score for scores in scores_per_size for score in scores]
#interval = (min(all_scores), max(all_scores))
interval = scipy.stats.norm.interval(alpha=0.95, loc=np.mean(all_scores), scale=np.std(all_scores))
#interval = DEFAULT_INTERVAL
print('Interval:', interval)
print('Mean: {:.3f} | Stdev: {:.3f}'.format(np.mean(all_scores), np.std(all_scores)))
#train_body = create_cylinder(radius=0.002, height=0.02, color=apply_alpha(BLACK, 1.0))
for i, (train_size, scores) in enumerate(zip(train_sizes, scores_per_size)):
print('Train size: {} | Average: {:.3f} | Min: {:.3f} | Max: {:.3f}'.format(
train_size, np.mean(scores), min(scores), max(scores)))
handles = []
# TODO: visualize delta
with LockRenderer():
#if train_size is None:
# train_size = len(learner.results)
#set_pose(train_body, multiply(training_poses[train_size-1], Pose(point=center)))
#set_pose(world.bodies[cup_type], training_poses[train_size-1])
for score, body in zip(scores, bodies):
score = clip(score, *interval)
fraction = rescale(score, interval, new_interval=(0, 1))
color = interpolate_hue(fraction)
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) # TODO: convex combination
set_color(body, apply_alpha(color, alpha))
#set_pose(world.bodies[cup_type], pose)
#draw_pose(pose, length=0.05)
#handles.extend(draw_point(tform_point(pose, center), color=color))
#extent = np.array([0, 0, 0.01])
#start = tform_point(pose, center - extent/2)
#end = tform_point(pose, center + extent/2)
#handles.append(add_line(start, end, color=color, width=1))
wait_for_duration(0.5)
# TODO: test on boundaries
wait_for_user()
