def sample_placements(body_surfaces, obstacles=None, savers=[], min_distances={}):
if obstacles is None:
obstacles = OrderedSet(get_bodies()) - set(body_surfaces)
obstacles = list(obstacles)
savers = list(savers) + [BodySaver(obstacle) for obstacle in obstacles]
if not isinstance(min_distances, dict):
min_distances = {body: min_distances for body in body_surfaces}
# TODO: max attempts here
for body, surface in body_surfaces.items(): # TODO: shuffle
min_distance = min_distances.get(body, 0.)
while True:
pose = sample_placement(body, surface)
if pose is None:
for saver in savers:
saver.restore()
return False
for saver in savers:
obstacle = saver.body
if obstacle in [body, surface]:
continue
saver.restore()
if pairwise_collision(body, obstacle, max_distance=min_distance):
break
else:
obstacles.append(body)
break
for saver in savers:
saver.restore()
return True
def save_segmented(image, beads_per_color):
if image is None:
return
all_beads = set(flatten(beads_per_color))
non_beads = sorted(set(get_bodies()) - all_beads)
color_from_body = dict(zip(non_beads, spaced_colors(len(non_beads))))
segmented_image = image_from_segmented(image,
color_from_body=color_from_body)
save_image('segmented.png', segmented_image)
def plan_motion(robot, joints, goal_positions, attachments=[], obstacles=None, holonomic=False, reversible=False, **kwargs):
attached_bodies = [attachment.child for attachment in attachments]
moving_bodies = [robot] + attached_bodies
if obstacles is None:
obstacles = get_bodies()
obstacles = OrderedSet(obstacles) - set(moving_bodies)
if holonomic:
return plan_joint_motion(robot, joints, goal_positions,
attachments=attachments, obstacles=obstacles, **kwargs)
# TODO: just sample the x, y waypoint and use the resulting orientation
# TODO: remove overlapping configurations/intervals due to circular joints
return plan_nonholonomic_motion(robot, joints, goal_positions, reversible=reversible,
linear_tol=1e-6, angular_tol=0.,
attachments=attachments, obstacles=obstacles, **kwargs)
def visualize_collected_pours(paths, num=6, save=True):
result_from_bowl = {}
for path in randomize(paths):
results = read_data(path)
print(path, len(results))
for result in results:
result_from_bowl.setdefault(result['feature']['bowl_type'], []).append(result)
world = create_world()
environment = get_bodies()
#collector = SKILL_COLLECTORS['pour']
print(get_camera())
for bowl_type in sorted(result_from_bowl):
# TODO: visualize same
for body in get_bodies():
if body not in environment:
remove_body(body)
print('Bowl type:', bowl_type)
bowl_body = load_cup_bowl(bowl_type)
bowl_pose = get_pose(bowl_body)
results = result_from_bowl[bowl_type]
results = randomize(results)
score_cup_pose = []
for i, result in enumerate(results):
if num <= len(score_cup_pose):
break
feature = result['feature']
parameter = result['parameter']
score = result['score']
if (score is None) or not result['execution'] or result['annotation']:
continue
cup_body = load_cup_bowl(feature['cup_type'])
world.bodies[feature['bowl_name']] = bowl_body
world.bodies[feature['cup_name']] = cup_body
fraction = compute_fraction_filled(score)
#value = collector.score_fn(feature, parameter, score)
value = pour_score(feature, parameter, score)
print(i, feature['cup_type'], fraction, value)
path, _ = pour_path_from_parameter(world, feature, parameter)
sort = fraction
#sort = parameter['pitch']
#sort = parameter['axis_in_bowl_z']
score_cup_pose.append((sort, fraction, value, cup_body, path[0]))
#order = score_cup_pose
order = randomize(score_cup_pose)
#order = sorted(score_cup_pose)
angles = np.linspace(0, 2*np.pi, num=len(score_cup_pose), endpoint=False) # Halton
for angle, (_, fraction, value, cup_body, pose) in zip(angles, order):
#fraction = clip(fraction, min_value=0, max_value=1)
value = clip(value, *DEFAULT_INTERVAL)
fraction = rescale(value, DEFAULT_INTERVAL, new_interval=(0, 1))
#color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN)
color = interpolate_hue(fraction)
set_color(cup_body, apply_alpha(color, alpha=0.25))
#angle = random.uniform(-np.pi, np.pi)
#angle = 0
rotate_bowl = Pose(euler=Euler(yaw=angle))
cup_pose = multiply(bowl_pose, invert(rotate_bowl), pose)
set_pose(cup_body, cup_pose)
#wait_for_user()
#remove_body(cup_body)
if save:
#filename = os.path.join('workspace', '{}.png'.format(bowl_type))
#save_image(filename, take_picture()) # [0, 255]
#wait_for_duration(duration=0.5)
# TODO: get window location
#os.system("screencapture -R {},{},{},{} {}".format(
# 275, 275, 500, 500, filename)) # -R<x,y,w,h> capture screen rect
wait_for_user()
remove_body(bowl_body)
def ss_from_problem(robot,
movable=[],
bound='shared',
teleport=False,
movable_collisions=False,
grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
rigid = [body for body in get_bodies() if body != robot]
fixed = [body for body in rigid if body not in movable]
print('Robot:', robot)
print('Movable:', movable)
print('Fixed:', fixed)
conf = BodyConf(robot, get_configuration(robot))
initial_atoms = [
HandEmpty(),
CanMove(),
IsConf(conf),
AtConf(conf),
initialize(TotalCost(), 0),
]
for body in movable:
pose = BodyPose(body, get_pose(body))
initial_atoms += [
IsMovable(body),
IsPose(body, pose),
AtPose(body, pose)
]
for surface in fixed:
initial_atoms += [Stackable(body, surface)]
if is_placement(body, surface):
initial_atoms += [IsSupported(pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
initial_atoms.append(Sink(body))
if 'stove' in name:
initial_atoms.append(Stove(body))
body = movable[0]
goal_literals = [
AtConf(conf),
#Holding(body),
#On(body, fixed[0]),
#On(body, fixed[2]),
#Cleaned(body),
Cooked(body),
]
PlacedCollision = Predicate([T, O, P],
domain=[IsTraj(T), IsPose(O, P)],
fn=get_movable_collision_test(),
bound=False)
actions = [
Action(name='pick',
param=[O, P, G, Q, T],
pre=[
IsKin(O, P, G, Q, T),
HandEmpty(),
AtPose(O, P),
AtConf(Q), ~Unsafe(T)
],
eff=[HasGrasp(O, G),
CanMove(), ~HandEmpty(), ~AtPose(O, P)]),
Action(
name='place',
param=[O, P, G, Q, T],
pre=[IsKin(O, P, G, Q, T),
HasGrasp(O, G),
AtConf(Q), ~Unsafe(T)],
eff=[HandEmpty(),
CanMove(),
AtPose(O, P), ~HasGrasp(O, G)]),
# Can just do move if we check holding collisions
Action(name='move_free',
param=[Q, Q2, T],
pre=[
IsFreeMotion(Q, Q2, T),
HandEmpty(),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(name='move_holding',
param=[Q, Q2, O, G, T],
pre=[
IsHoldingMotion(Q, Q2, O, G, T),
HasGrasp(O, G),
CanMove(),
AtConf(Q), ~Unsafe(T)
],
eff=[AtConf(Q2), ~CanMove(), ~AtConf(Q)]),
Action(
name='clean',
param=[O, O2], # Wirelessly communicates to clean
pre=[Stackable(O, O2),
Sink(O2), ~Cooked(O),
On(O, O2)],
eff=[Cleaned(O)]),
Action(
name='cook',
param=[O, O2], # Wirelessly communicates to cook
pre=[Stackable(O, O2),
Stove(O2), Cleaned(O),
On(O, O2)],
eff=[Cooked(O), ~Cleaned(O)]),
]
axioms = [
Axiom(param=[O, G],
pre=[IsGrasp(O, G), HasGrasp(O, G)],
eff=Holding(O)),
Axiom(param=[O, P, O2],
pre=[IsPose(O, P),
IsSupported(P, O2),
AtPose(O, P)],
eff=On(O, O2)),
]
if movable_collisions:
axioms += [
Axiom(param=[T, O, P],
pre=[IsPose(O, P),
PlacedCollision(T, O, P),
AtPose(O, P)],
eff=Unsafe(T)),
]
streams = [
GenStream(name='grasp',
inp=[O],
domain=[IsMovable(O)],
fn=get_grasp_gen(robot, grasp_name),
out=[G],
graph=[IsGrasp(O, G)],
bound=bound),
# TODO: test_support
GenStream(name='support',
inp=[O, O2],
domain=[Stackable(O, O2)],
fn=get_stable_gen(fixed=fixed),
out=[P],
graph=[IsPose(O, P), IsSupported(P, O2)],
bound=bound),
FnStream(name='inverse_kin',
inp=[O, P, G],
domain=[IsPose(O, P), IsGrasp(O, G)],
fn=get_ik_fn(robot, fixed=fixed, teleport=teleport),
out=[Q, T],
graph=[IsKin(O, P, G, Q, T),
IsConf(Q), IsTraj(T)],
bound=bound),
FnStream(name='free_motion',
inp=[Q, Q2],
domain=[IsConf(Q), IsConf(Q2)],
fn=get_free_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsFreeMotion(Q, Q2, T),
IsTraj(T)],
bound=bound),
FnStream(name='holding_motion',
inp=[Q, Q2, O, G],
domain=[IsConf(Q), IsConf(Q2),
IsGrasp(O, G)],
fn=get_holding_motion_gen(robot, teleport=teleport),
out=[T],
graph=[IsHoldingMotion(Q, Q2, O, G, T),
IsTraj(T)],
bound=bound),
]
return Problem(initial_atoms,
goal_literals,
actions,
axioms,
streams,
objective=TotalCost())