def score_fn(plan):
assert plan is not None
initial_distance = get_distance(
point_from_pose(initial_pose)[:2], goal_pos2d)
final_pose = world.perception.get_pose(block_name)
final_distance = get_distance(
point_from_pose(final_pose)[:2], goal_pos2d)
quat_distance = quat_angle_between(quat_from_pose(initial_pose),
quat_from_pose(final_pose))
print(
'Initial: {:.5f} m | Final: {:.5f} | Rotation: {:.5f} rads'.format(
initial_distance, final_distance, quat_distance))
# TODO: compare orientation to final predicted orientation
# TODO: record simulation time in the event that the controller gets stuck
score = {
'initial_distance': initial_distance,
'final_distance': final_distance,
'rotation': quat_distance,
DYNAMICS: parameters_from_name,
}
#_, args = find_unique(lambda a: a[0] == 'push', plan)
#control = args[-1]
return score
def score_fn(plan):
assert plan is not None
final_pose = world.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):
bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = float(
len(bowl_beads)) / len(init_beads) if init_beads else 0
mass_in_bowl = sum(map(get_mass, bowl_beads))
spoon_beads = get_contained_beads(world.get_body(spoon_name),
init_beads)
fraction_spoon = float(
len(spoon_beads)) / len(init_beads) if init_beads else 0
mass_in_spoon = sum(map(get_mass, spoon_beads))
print('In Bowl: {:.3f} | In Spoon: {:.3f}'.format(
fraction_bowl, fraction_spoon))
# TODO: measure change in roll/pitch
# TODO: could make latent parameters field
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'total_mass': init_mass,
'mass_in_bowl': mass_in_bowl,
'mass_in_spoon': mass_in_spoon,
'spoon_mass_capacity':
(init_mass / len(init_beads)) * spoon_capacity,
# Counts
'num_beads': len(init_beads),
'bowl_beads': len(bowl_beads),
'spoon_beads': len(spoon_beads),
'spoon_capacity': spoon_capacity,
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_spoon': fraction_spoon,
# Latent
'bead_radius': bead_radius,
DYNAMICS: parameters_from_name
}
fraction_filled = float(score['spoon_beads']) / score['spoon_capacity']
spilled_beads = score['num_beads'] - (score['bowl_beads'] +
score['spoon_beads'])
fraction_spilled = float(spilled_beads) / score['num_beads']
print('Fraction Filled: {} | Fraction Spilled: {}'.format(
fraction_filled, fraction_spilled))
#_, args = find_unique(lambda a: a[0] == 'scoop', plan)
#control = args[-1]
return score
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 score_fn(plan):
assert plan is not None
final_pose = world.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):
# TODO: lift the bowl up (with particles around) to prevent scale detections
final_bowl_beads = get_contained_beads(bowl_body, init_beads)
fraction_bowl = safe_ratio(len(final_bowl_beads),
len(init_beads),
undefined=0)
mass_in_bowl = sum(map(get_mass, final_bowl_beads))
final_cup_beads = get_contained_beads(cup_body, init_beads)
fraction_cup = safe_ratio(len(final_cup_beads),
len(init_beads),
undefined=0)
mass_in_cup = sum(map(get_mass, final_cup_beads))
print('In Bowl: {} | In Cup: {}'.format(fraction_bowl, fraction_cup))
score = {
# Displacements
'bowl_translation': point_distance,
'bowl_rotation': quat_distance,
# Masses
'mass_in_bowl': mass_in_bowl,
'mass_in_cup': mass_in_cup,
# Counts
'bowl_beads': len(final_bowl_beads),
'cup_beads': len(final_cup_beads),
# Fractions
'fraction_in_bowl': fraction_bowl,
'fraction_in_cup': fraction_cup,
}
score.update(latent)
# TODO: store the cup path length to bias towards shorter paths
#_, args = find_unique(lambda a: a[0] == 'pour', plan)
#control = args[-1]
#feature = control['feature']
#parameter = control['parameter']
return score
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
def score_poses(problem, task, ros_world):
cup_name, bowl_name = REQUIREMENT_FNS[problem](ros_world)
name_from_type = {'cup': cup_name, 'bowl': bowl_name}
initial_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
final_stackings = wait_until_observation(problem, ros_world)
#final_stackings = None
if final_stackings is None:
# TODO: only do this for the bad bowl
point_distances = {ty: INF for ty in name_from_type}
quat_distances = {ty: 2 * np.pi
for ty in name_from_type} # Max rotation
else:
final_poses = {
ty: ros_world.get_pose(name)
for ty, name in name_from_type.items()
}
point_distances = {
ty: get_distance(point_from_pose(initial_poses[ty]),
point_from_pose(final_poses[ty]))
for ty in name_from_type
}
# TODO: wrap around distance for symmetric things
quat_distances = {
ty: quat_angle_between(quat_from_pose(initial_poses[ty]),
quat_from_pose(final_poses[ty]))
for ty in name_from_type
}
score = {}
for ty in sorted(name_from_type):
print(
'{} | translation (m): {:.3f} | rotation (degrees): {:.3f}'.format(
ty, point_distances[ty], math.degrees(quat_distances[ty])))
score.update({
'{}_translation'.format(ty): point_distances[ty],
'{}_rotation'.format(ty): quat_distances[ty],
})
return score
def water_test(q):
if attachment is None:
return False
set_joint_positions(body, joints, q)
attachment.assign()
attachment_pose = get_pose(attachment.child)
pose = multiply(attachment_pose,
reference_pose) # TODO: confirm not inverted
roll, pitch, _ = euler_from_quat(quat_from_pose(pose))
violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch))
#if violation: # TODO: check whether different confs can be waypoints for each object
# print(q, violation)
# wait_for_user()
return violation
def inverse_kinematics(arm, pose, torso, upper):
from pybullet_tools.pr2_ik.ikLeft import leftIK
from pybullet_tools.pr2_ik.ikRight import rightIK
arm_ik = {
'left': leftIK,
'right': rightIK,
}
ik_fn = arm_ik[arm]
pos = point_from_pose(pose)
rot = matrix_from_quat(quat_from_pose(pose)).tolist()
solutions = ik_fn(list(rot), list(pos), [torso, upper])
if solutions is None:
return []
return solutions
def gen_fn(obj, pose1, surface, region=None):
start_point = point_from_pose(pose1)
distance_range = (0.15, 0.2) if region is None else (0.15, 0.3)
obj_body = world.bodies[obj]
while True:
theta = random.uniform(-np.pi, np.pi)
distance = random.uniform(*distance_range)
end_point2d = np.array(start_point[:2]) + distance * unit_from_theta(theta)
end_pose = (np.append(end_point2d, [start_point[2]]), quat_from_pose(pose1))
set_pose(obj_body, end_pose)
if not is_center_stable(obj_body, world.bodies[surface], above_epsilon=np.inf):
continue
if region is not None:
assert region in ARMS
if not reachable_test(region, obj, end_pose):
continue
yield end_point2d,
def get_end_pose(initial_pose, goal_pos2d):
initial_z = point_from_pose(initial_pose)[2]
orientation = quat_from_pose(initial_pose)
goal_x, goal_y = goal_pos2d
end_pose = ([goal_x, goal_y, initial_z], orientation)
return end_pose
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
def pdddlstream_from_problem(belief,
additional_init=[],
fixed_base=True,
**kwargs):
world = belief.world # One world per state
task = world.task # One task per world
print(task)
domain_pddl = read(get_file_path(__file__, '../pddl/domain.pddl'))
# TODO: repackage stream outputs to avoid recomputation
# Despite the base not moving, it could be re-estimated
init_bq = belief.base_conf
init_aq = belief.arm_conf
init_gq = belief.gripper_conf
carry_aq = world.carry_conf
init_aq = carry_aq if are_confs_close(init_aq, carry_aq) else init_aq
# TODO: the following doesn't work. Maybe because carry_conf is used elsewhere
#carry_aq = init_aq if are_confs_close(init_aq, world.carry_conf) else world.carry_conf
#calibrate_aq = init_aq if are_confs_close(init_aq, world.calibrate_conf) else world.calibrate_conf
# Don't need this now that returning to old confs
#open_gq = init_gq if are_confs_close(init_gq, world.open_gq) else world.open_gq
#closed_gq = init_gq if are_confs_close(init_gq, world.closed_gq) else world.closed_gq
open_gq = world.open_gq
closed_gq = world.closed_gq
constant_map = {
'@world': 'world',
'@gripper': 'gripper',
'@stove': 'stove',
'@none': None,
'@rest_aq': carry_aq,
#'@calibrate_aq': calibrate_aq,
'@open_gq': open_gq,
'@closed_gq': closed_gq,
'@open': OPEN,
'@closed': CLOSED,
'@top': TOP_GRASP,
'@side': SIDE_GRASP,
'@bq0': init_bq,
}
top_joint = JOINT_TEMPLATE.format(TOP_DRAWER)
bottom_joint = JOINT_TEMPLATE.format(BOTTOM_DRAWER)
init = [
('BConf', init_bq),
('AtBConf', init_bq),
('AConf', init_bq, carry_aq),
#('RestAConf', carry_aq),
#('AConf', init_bq, calibrate_aq),
(
'Stationary', ),
('AConf', init_bq, init_aq),
('AtAConf', init_aq),
('GConf', open_gq),
('GConf', closed_gq),
('Grasp', None, None),
('AtGrasp', None, None),
('Above', top_joint, bottom_joint),
('Adjacent', top_joint, bottom_joint),
('Adjacent', bottom_joint, top_joint),
('Calibrated', ),
('CanMoveBase', ),
('CanMoveArm', ),
('CanMoveGripper', ),
] + list(task.init) + list(additional_init)
for action_name, cost in ACTION_COSTS.items():
function_name = '{}Cost'.format(title_from_snake(action_name))
function = (function_name, )
init.append(Equal(function, cost)) # TODO: stove state
init += [('Stackable', name, surface) for name, surface in task.goal_on.items()] + \
[('Stackable', name, stove) for name, stove in product(task.goal_cooked, STOVES)] + \
[('Pressed', name) for name in belief.pressed] + \
[('Cookable', name) for name in task.goal_cooked] + \
[('Cooked', name) for name in belief.cooked] + \
[('Status', status) for status in DOOR_STATUSES] + \
[('Knob', knob) for knob in KNOBS] + \
[('Joint', knob) for knob in KNOBS] + \
[('Liquid', liquid) for _, liquid in task.init_liquid] + \
[('HasLiquid', cup, liquid) for cup, liquid in belief.liquid] + \
[('StoveKnob', STOVE_TEMPLATE.format(loc), KNOB_TEMPLATE.format(loc)) for loc in STOVE_LOCATIONS] + \
[('GraspType', ty) for ty in task.grasp_types] # TODO: grasp_type per object
#[('Type', obj_name, 'stove') for obj_name in STOVES] + \
#[('Camera', name) for name in world.cameras]
if task.movable_base:
init.append(('MovableBase', ))
if fixed_base:
init.append(('InitBConf', init_bq))
if task.noisy_base:
init.append(('NoisyBase', ))
compute_pose_kin = get_compute_pose_kin(world)
compute_angle_kin = get_compute_angle_kin(world)
initial_poses = {}
for joint_name, init_conf in belief.door_confs.items():
if joint_name in DRAWER_JOINTS:
init.append(('Drawer', joint_name))
if joint_name in CABINET_JOINTS:
init.append(('Cabinet', joint_name))
joint = joint_from_name(world.kitchen, joint_name)
surface_name = surface_from_joint(joint_name)
init.append(('SurfaceJoint', surface_name, joint_name))
# Relies on the fact that drawers have identical surface and link names
link_name = get_link_name(world.kitchen, child_link_from_joint(joint))
#link_name = str(link_name.decode('UTF-8'))
#link_name = str(link_name.encode('ascii','ignore'))
for conf in {
init_conf, world.open_kitchen_confs[joint],
world.closed_kitchen_confs[joint]
}:
# TODO: return to initial poses?
world_pose, = compute_angle_kin(link_name, joint_name, conf)
init.extend([
('Joint', joint_name),
('Angle', joint_name, conf),
('Obstacle', link_name),
('AngleKin', link_name, world_pose, joint_name, conf),
('WorldPose', link_name, world_pose),
])
if joint in world.kitchen_joints:
init.extend([
('Sample', world_pose),
#('Value', world_pose), # comment out?
])
if conf == init_conf:
initial_poses[link_name] = world_pose
init.extend([
('AtAngle', joint_name, conf),
('AtWorldPose', link_name, world_pose),
])
for surface_name in ALL_SURFACES:
if surface_name in OPEN_SURFACES:
init.append(('Counter', surface_name)) # Fixed surface
if surface_name in DRAWERS:
init.append(('Drawer', surface_name))
surface = surface_from_name(surface_name)
surface_link = link_from_name(world.kitchen, surface.link)
parent_joint = parent_joint_from_link(surface_link)
if parent_joint not in world.kitchen_joints:
# TODO: attach to world frame?
world_pose = RelPose(world.kitchen, surface_link, init=True)
initial_poses[surface_name] = world_pose
init += [
#('RelPose', surface_name, world_pose, 'world'),
('WorldPose', surface_name, world_pose),
#('AtRelPose', surface_name, world_pose, 'world'),
('AtWorldPose', surface_name, world_pose),
('Sample', world_pose),
#('Value', world_pose),
]
init.extend([
('CheckNearby', surface_name),
#('InitPose', world_pose),
('Localized', surface_name),
])
for grasp_type in task.grasp_types:
if (surface_name in OPEN_SURFACES) or has_place_database(
world.robot_name, surface_name, grasp_type):
init.append(('AdmitsGraspType', surface_name, grasp_type))
if belief.grasped is None:
init.extend([
('HandEmpty', ),
('GConf', init_gq),
('AtGConf', init_gq),
])
else:
obj_name = belief.grasped.body_name
assert obj_name not in belief.pose_dists
grasp = belief.grasped
init += [
# Static
#('Graspable', obj_name),
('Grasp', obj_name, grasp),
('IsGraspType', obj_name, grasp, grasp.grasp_type),
# Fluent
('AtGrasp', obj_name, grasp),
('Holding', obj_name),
('Localized', obj_name),
]
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
for obj_name in world.movable:
obj_type = type_from_name(obj_name)
if obj_type in BOWLS:
init.append(('Bowl', obj_name))
else:
init.append(
('Obstacle', obj_name)) # TODO: hack to place within bowls
if obj_type in COOKABLE:
init.append(('Cookable', obj_name))
if obj_type in POURABLE:
init.append(('Pourable', obj_name))
init += [
('Entity', obj_name),
('CheckNearby', obj_name),
] + [('Stackable', obj_name, counter)
for counter in set(ALL_SURFACES) & set(COUNTERS)]
# TODO: track poses over time to produce estimates
for obj_name, pose_dist in belief.pose_dists.items():
dist_support = pose_dist.dist.support()
localized = pose_dist.is_localized()
graspable = True
if localized:
init.append(('Localized', obj_name))
[rel_pose] = dist_support
roll, pitch, yaw = euler_from_quat(
quat_from_pose(rel_pose.get_reference_from_body()))
if (MAX_ERROR < abs(roll)) or (MAX_ERROR < abs(pitch)):
graspable = False
print(
'{} has an invalid orientation: roll={:.3f}, pitch={:.3f}'.
format(obj_name, roll, pitch))
if graspable:
#init.append(('Graspable', obj_name))
init.extend(('ValidGraspType', obj_name, grasp_type)
for grasp_type in task.grasp_types
if implies(world.is_real(),
is_valid_grasp_type(obj_name, grasp_type)))
# Could also fully decompose into points (but many samples)
# Could immediately add likely points for collision checking
for rel_pose in (dist_support if localized else pose_dist.decompose()):
surface_name = rel_pose.support
if surface_name is None:
# Treats as obstacle
# TODO: could temporarily add to fixed
world_pose = rel_pose
init += [
('WorldPose', obj_name, world_pose),
('AtWorldPose', obj_name, world_pose),
]
poses = [world_pose]
#raise RuntimeError(obj_name, supporting)
else:
surface_pose = initial_poses[surface_name]
world_pose, = compute_pose_kin(obj_name, rel_pose,
surface_name, surface_pose)
init += [
# Static
('RelPose', obj_name, rel_pose, surface_name),
('WorldPose', obj_name, world_pose),
('PoseKin', obj_name, world_pose, rel_pose, surface_name,
surface_pose),
# Fluent
('AtRelPose', obj_name, rel_pose, surface_name),
('AtWorldPose', obj_name, world_pose),
]
if localized:
init.append(('On', obj_name, surface_name))
poses = [rel_pose, world_pose]
for pose in poses:
if isinstance(pose, PoseDist):
init.append(('Dist', pose))
else:
init.extend([('Sample', pose)]) #, ('Value', pose)])
#for body, ty in problem.body_types:
# init += [('Type', body, ty)]
#bodies_from_type = get_bodies_from_type(problem)
#bodies = bodies_from_type[get_parameter_name(ty)] if is_parameter(ty) else [ty]
goal_formula = get_goal(belief, init)
stream_pddl, stream_map = get_streams(world,
teleport_base=task.teleport_base,
**kwargs)
print('Constants:', constant_map)
print('Init:', sorted(init, key=lambda f: f[0]))
print('Goal:', goal_formula)
#print('Streams:', stream_map.keys()) # DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)