def test_push_pour(visualize):
arms = ARMS
cup_name = create_name('bluecup', 1)
bowl_name = create_name('bowl', 1)
items = [bowl_name, cup_name, bowl_name]
world, table_body = create_world(items, visualize=visualize)
set_point(table_body, np.array(TABLE_POSE[0]) + np.array([0, -0.1, 0]))
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.75, 0.4, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.5, -0.6, bowl_z), unit_quat())
update_world(world, table_body)
# TODO: can prevent the right arm from being able to pick
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
('CanPush', bowl_name, LEFT_ARM), # TODO: intersection of these regions
]
goal = [
('Contains', bowl_name, COFFEE),
('InRegion', bowl_name, LEFT_ARM),
]
task = Task(init=init, goal=goal, arms=arms, pushable=[bowl_name])
# Most of the time places the cup to exchange arms
return world, task
def test_stack_pour(visualize):
arms = [LEFT_ARM]
bowl_name = create_name('whitebowl', 1)
cup_name = create_name('bluecup', 1)
purple_name = create_name('purpleblock', 1)
items = [bowl_name, cup_name, purple_name]
world, table_body = create_world(items, visualize=visualize)
cup_x = 0.5
initial_poses = {
bowl_name: ([cup_x, -0.2, 0.0], unit_quat()),
cup_name: ([cup_x, 0.1, 0.0], unit_quat()),
purple_name: ([cup_x, 0.4, 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)
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
('Stackable', bowl_name, purple_name, TOP),
]
goal = [
('Contains', bowl_name, COFFEE),
('On', bowl_name, purple_name, TOP),
]
task = Task(init=init, goal=goal, arms=arms, graspable=['whitebowl'])
return world, task
def __init__(self, task, use_robot=True, visualize=False, **kwargs):
self.task = task
self.real_world = None
self.visualize = visualize
self.client_saver = None
self.client = connect(use_gui=visualize, **kwargs)
print('Planner connected to client {}.'.format(self.client))
self.robot = None
with ClientSaver(self.client):
with HideOutput():
if use_robot:
self.robot = load_pybullet(os.path.join(
get_models_path(), PR2_URDF),
fixed_base=True)
#dump_body(self.robot)
#compute_joint_weights(self.robot)
self.world_name = 'world'
self.world_pose = Pose(unit_pose())
self.bodies = {}
self.fixed = []
self.surfaces = []
self.items = []
#self.holding_liquid = []
self.initial_config = None
self.initial_poses = {}
self.body_mapping = {}
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 test_holding(visualize, num_blocks=2):
arms = [LEFT_ARM]
#item_type = 'greenblock'
#item_type = 'bowl'
item_type = 'purple_cup'
item_poses = [
([0.6, +0.4, 0.0], z_rotation(np.pi / 2)),
([0.6, -0.4, 0.0], unit_quat()),
]
items = [create_name(item_type, i) for i in range(min(len(item_poses), num_blocks))]
world, table_body = create_world(items, visualize=visualize)
with ClientSaver(world.perception.client):
for name, pose in zip(items, item_poses):
point, quat = pose
point[2] += stable_z(world.perception.sim_bodies[name], table_body) + Z_EPSILON
world.perception.set_pose(name, point, quat)
update_world(world, table_body)
init = [
('Graspable', item) for item in items
]
goal = [
#('Holding', items[0]),
('HoldingType', item_type),
]
task = Task(init=init, goal=goal, arms=arms)
return world, task
def add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
def test_pour(visualize):
arms = [LEFT_ARM]
#cup_name, bowl_name = 'bluecup', 'bluebowl'
#cup_name, bowl_name = 'cup_7', 'cup_8'
#cup_name, bowl_name = 'cup_7-1', 'cup_7-2'
#cup_name, bowl_name = get_name('bluecup', 1), get_name('bluecup', 2)
cup_name = create_name('bluecup', 1) # bluecup | purple_cup | orange_cup | blue_cup | olive1_cup | blue3D_cup
bowl_name = create_name('bowl', 1) # bowl | steel_bowl
world, table_body = create_world([bowl_name, cup_name, bowl_name], visualize=visualize)
with ClientSaver(world.perception.client):
cup_z = stable_z(world.perception.sim_bodies[cup_name], table_body) + Z_EPSILON
bowl_z = stable_z(world.perception.sim_bodies[bowl_name], table_body) + Z_EPSILON
world.perception.set_pose(cup_name, Point(0.6, 0.2, cup_z), unit_quat())
world.perception.set_pose(bowl_name, Point(0.6, 0.0, bowl_z), unit_quat())
update_world(world, table_body)
init = [
('Contains', cup_name, COFFEE),
#('Graspable', bowl_name),
]
goal = [
('Contains', bowl_name, COFFEE),
]
task = Task(init=init, goal=goal, arms=arms,
empty_arms=True, reset_arms=True, reset_items=True)
return world, task
def test_block(visualize):
#arms = [LEFT_ARM]
arms = ARMS
block_name = create_name('greenblock', 1)
tray_name = create_name('tray', 1)
world, table_body = create_world([tray_name, block_name], visualize=visualize)
with ClientSaver(world.perception.client):
block_z = stable_z(world.perception.sim_bodies[block_name], table_body) + Z_EPSILON
tray_z = stable_z(world.perception.sim_bodies[tray_name], table_body) + Z_EPSILON
#attach_viewcone(world.perception.pr2, depth=1, head_name='high_def_frame')
#dump_body(world.perception.pr2)
#block_y = 0.0
block_y = -0.4
world.perception.set_pose(block_name, Point(0.6, block_y, block_z), unit_quat())
world.perception.set_pose(tray_name, Point(0.6, 0.4, tray_z), unit_quat())
update_world(world, table_body)
init = [
('Stackable', block_name, tray_name, TOP),
]
goal = [
('On', block_name, tray_name, TOP),
]
#goal = [
# ('Grasped', arms[0], block_name),
#]
task = Task(init=init, goal=goal, arms=arms, empty_arms=True)
return world, task
def stabilize(world, duration=0.1):
# TODO: apply to simulated_problems
with ClientSaver(world.client):
world.controller.set_gravity()
with BodySaver(
world.robot): # Otherwise the robot starts in self-collision
world.controller.rest_for_duration(
duration) # Let's the objects stabilize
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 command_torso(self, pose, timeout=2.0, blocking=True):
# Commands Torso to a certain position
with ClientSaver(self.client):
torso_joint = joint_from_name(self.robot, self.TORSO)
set_joint_position(self.robot, torso_joint, pose)
if self.execute_motor_control:
control_joint(self.robot, torso_joint, pose)
else:
set_joint_position(self.robot, torso_joint, pose)
def main(execute='apply'):
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
#display = args.display
display = True
#display = False
#filename = 'transporter.json' # simple | simple2 | cook | invert | kitchen | nonmonotonic_4_1
#problem_fn = lambda: load_json_problem(filename)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
sim_world = connect(use_gui=False)
set_client(sim_world)
add_data_path()
problem = problem_fn()
print(problem)
#state_id = save_state()
if display:
with HideOutput():
real_world = connect(use_gui=True)
add_data_path()
with ClientSaver(real_world):
problem_fn() # TODO: way of doing this without reloading?
update_state()
wait_for_duration(0.1)
#wait_for_interrupt()
commands = plan_commands(problem)
if (commands is None) or not display:
with HideOutput():
disconnect()
return
time_step = 0.01
set_client(real_world)
if execute == 'control':
enable_gravity()
control_commands(commands)
elif execute == 'execute':
step_commands(commands, time_step=time_step)
elif execute == 'step':
step_commands(commands)
elif execute == 'apply':
state = State()
apply_commands(state, commands, time_step=time_step)
else:
raise ValueError(execute)
wait_for_interrupt()
with HideOutput():
disconnect()
def optimize_feature(learner, max_time=INF, **kwargs):
#features = read_json(get_feature_path(learner.func.skill))
feature_fn, attributes, pairs = generate_candidates(
learner.func.skill, **kwargs)
start_time = time.time()
world = ROSWorld(use_robot=False, sim_only=True)
saver = ClientSaver(world.client)
print('Optimizing over {} feature pairs'.format(len(pairs)))
best_pair, best_score = None, -INF # maximize
for pair in randomize(pairs): # islice
if max_time <= elapsed_time(start_time):
break
world.reset(keep_robot=False)
for name in pair:
world.perception.add_item(name)
feature = feature_fn(world, *pair)
parameter = next(
learner.parameter_generator(world,
feature,
min_score=best_score,
valid=True,
verbose=False), None)
if parameter is None:
continue
context = learner.func.context_from_feature(feature)
sample = learner.func.sample_from_parameter(parameter)
x = x_from_context_sample(context, sample)
#x = learner.func.x_from_feature_parameter(feature, parameter)
score_fn = learner.get_score_f(context, noise=False,
negate=False) # maximize
score = float(score_fn(x)[0, 0])
if best_score < score:
best_pair, best_score = pair, score
world.stop()
saver.restore()
print(
'Best pair: {} | Best score: {:.3f} | Pairs: {} | Time: {:.3f}'.format(
best_pair, best_score, len(pairs), elapsed_time(start_time)))
assert best_score is not None
# TODO: ensure the same parameter is used
return dict(zip(attributes, best_pair))
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 _create_robot(self):
with ClientSaver(self.client):
with HideOutput():
pr2_path = os.path.join(get_models_path(), PR2_URDF)
self.pr2 = load_pybullet(pr2_path, fixed_base=True)
# Base link is the origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, invert(base_pose))
return self.pr2
def add_holding(task, ros_world):
with ClientSaver(ros_world.client):
for arm, grasp in task.init_holding.items():
name = grasp.obj_name
body = load_pybullet(get_body_urdf(get_type(name)),
fixed_base=False)
ros_world.perception.sim_bodies[name] = body
ros_world.perception.sim_items[name] = None
attachment = get_grasp_attachment(ros_world, arm, grasp)
attachment.assign()
ros_world.controller.attach(get_arm_prefix(arm), name)
def display_failure(node_points, extruded_elements, element):
client = connect(use_gui=True)
with ClientSaver(client):
obstacles, robot = load_world()
handles = []
for e in extruded_elements:
handles.append(draw_element(node_points, e, color=(0, 1, 0)))
handles.append(draw_element(node_points, element, color=(1, 0, 0)))
print('Failure!')
wait_for_user()
reset_simulation()
disconnect()
def update_world(world, target_body, camera_point=VIEWER_POINT):
pr2 = world.perception.pr2
with ClientSaver(world.perception.client):
open_arm(pr2, LEFT_ARM)
open_arm(pr2, RIGHT_ARM)
set_group_conf(pr2, 'torso', [TORSO_POSITION])
set_group_conf(pr2, arm_from_arm('left'), WIDE_LEFT_ARM)
set_group_conf(pr2, arm_from_arm('right'), rightarm_from_leftarm(WIDE_LEFT_ARM))
target_point = get_point(target_body)
head_conf = inverse_visibility(pr2, target_point, head_name=CAMERA_OPTICAL_FRAME) # Must come after torso
set_group_conf(pr2, 'head', head_conf)
set_camera_pose(camera_point, target_point=target_point)
def load_body(self, name, pose=None, fixed_base=False):
assert name not in self.sim_bodies
ty = get_type(name)
with ClientSaver(self.client):
with HideOutput():
body = load_cup_bowl(ty)
if body is None:
body = load_pybullet(get_body_urdf(name),
fixed_base=fixed_base)
if pose is not None:
set_pose(body, pose)
self.sim_bodies[name] = body
return body
def command_gripper(self,
arm,
position,
max_effort=NULL_EFFORT,
timeout=2.0,
blocking=True):
# position is the width of the gripper as in the physical distance between the two fingers
with ClientSaver(self.client):
gripper_joints = joints_from_names(
self.robot, self.get_gripper_joint_names(arm))
positions = [position] * len(gripper_joints)
self.follow_trajectory(gripper_joints, [positions],
max_sim_duration=timeout)
def simulate_trial(sim_world,
task,
parameter_fns,
teleport=True,
collisions=True,
max_time=20,
verbose=False,
**kwargs):
with ClientSaver(sim_world.client):
viewer = has_gui()
planning_world = PlanningWorld(task, visualize=False)
planning_world.load(sim_world)
print(planning_world)
start_time = time.time()
plan = plan_actions(planning_world,
max_time=max_time,
verbose=verbose,
collisions=collisions,
teleport=teleport,
parameter_fns=parameter_fns) # **kwargs
# plan = None
plan_time = time.time() - start_time
planning_world.stop()
if plan is None:
print('Failed to find a plan')
# TODO: allow option of scoring these as well?
return None, plan_time
if teleport:
# TODO: skipping for scooping sometimes places the spoon in the bowl
plan = skip_to_end(sim_world, planning_world, plan)
if viewer:
# wait_for_user('Execute?')
wait_for_user()
sim_world.controller.set_gravity()
videos_directory = os.path.abspath(
os.path.join(__file__, os.pardir, os.pardir, VIDEOS_DIRECTORY))
#skill_videos = [filename.startswith(task.skill) for filename in os.listdir(videos_directory)]
#suffix = len(skill_videos)
suffix = datetime.datetime.now().strftime(DATE_FORMAT)
video_path = os.path.join(videos_directory,
'{}_{}.mp4'.format(task.skill, suffix))
video_path = None
with VideoSaver(video_path): # requires ffmpeg
# TODO: teleport=False
execute_plan(sim_world, plan, default_sleep=0.)
if viewer:
wait_for_user('Finished!')
return plan, plan_time
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 reset(self, keep_robot=False):
# Avoid using remove_body(body)
# https://github.com/bulletphysics/bullet3/issues/2086
self.controller.reset()
self.initial_beads = {}
with ClientSaver(self.client):
for name in list(self.perception.sim_bodies):
self.perception.remove(name)
for constraint in get_constraints():
remove_constraint(constraint)
remove_all_debug()
reset_simulation()
if keep_robot:
self._create_robot()
def step_plan(world, plan, attachments={}, **kwargs):
if plan is None:
return False
attachments = dict(attachments)
with ClientSaver(world.perception.client):
with WorldSaver():
for name, args in plan:
if name in ['cook']:
continue
control = args[-1]
for command in control['commands']:
step_command(world, command, attachments, **kwargs)
wait_for_user('Finished!')
return True
def clone_real_world_test(problem_fn):
real_world = connect(use_gui=True)
add_data_path()
# world_file = 'test_world.py'
# p.saveWorld(world_file) # Saves a Python file to be executed
# state_id = p.saveState()
# test_bullet = 'test_world.bullet'
# save_bullet(test_bullet)
# clone_world(real_world)
with ClientSaver(real_world):
# pass
# restore_bullet(test_bullet)
problem_fn() # TODO: way of doing this without reloading?
update_state()
def attach(self, arm, obj, **kwargs):
self.holding[arm] = obj
assert obj not in self.attachments
body = self.world.get_body(obj)
with ClientSaver(self.client):
if arm == 'l':
frame = "left_gripper"
elif arm == 'r':
frame = "right_gripper"
else:
raise ValueError("Arm should be l or r but was {}".format(arm))
robot_link = link_from_name(self.robot, PR2_TOOL_FRAMES[frame])
self.attachments[obj] = add_fixed_constraint(
body, self.robot, robot_link, max_force=self.attachment_force)
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 command_arm_trajectory(self,
arm,
path,
times_from_start,
blocking=True,
**kwargs):
# angles is a list of joint angles, times is a list of times from start
# When calling joints on an arm, needs to be called with all the angles in the arm
# times is not used because our pybullet's position controller doesn't take into account times
assert len(path) == len(times_from_start)
with ClientSaver(self.client):
arm_joints = joints_from_names(self.robot,
self.get_arm_joint_names(arm))
self.follow_trajectory(arm_joints, path, times_from_start,
**kwargs)
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 add_table_surfaces(world):
table_pose = None
for body_info in world.perception.surfaces:
if body_info.type == TABLE:
table_pose = body_info.pose
assert table_pose is not None
initial_poses = {
STOVE_NAME: (STOVE_POSITION, unit_quat()),
PLACEMAT_NAME: (PLACEMAT_POSITION, unit_quat()),
BUTTON_NAME: (BUTTON_POSITION, unit_quat()),
}
with ClientSaver(world.perception.client):
for name, local_pose in initial_poses.items():
world_pose = multiply(table_pose, local_pose)
from perception_tools.retired.ros_perception import BodyInfo
world.perception.surfaces.append(
BodyInfo(name, None, world_pose, name))
