def genPour(grip_name, cup_name, kettle_name, pose2):
if kettle_name != 'cup' or cup_name != 'cream_cup':
return
if not np.isclose(pose2[2], 0, atol=0.05):
return
kitchen = ph.make_kitchen(planning=True)
gripper = Gripper(kitchen, init_pos=ph.gripperInitPos[:2], init_angle=ph.gripperInitPos[2])
cup = ph.make_body(kitchen, cup_name, ph.cupInitPos)
kettle = ph.make_body(kitchen, kettle_name, pose2)
# MITSETTA PLANNING GP (ZEPOLITAT) #
#####################GP START##############################
gp, c = helper.process_gp_sample(ph.expid_pour, ph.FLAG_LK, ph.IS_ADAPTIVE,
task_lengthscale=ph.TASK_LENGTH_SCALE)
while True:
grasp, rel_x, rel_y, dangle, _, _, _, _ = gp.sample(c)
#####################GP END##############################
dangle *= np.sign(rel_x)
rel_pose = (rel_x, rel_y, dangle)
pour_pose = tuple(np.array(pose2) + np.array(rel_pose))
initial_pose = tuple(np.array(pose2) + (rel_x, rel_y, 0))
if not gripper.set_grasped(cup, grasp, initial_pose[:2], initial_pose[2]):
return
g2 = gripper.simulate_itself()
if g2.check_point_collision(initial_pose[:2], initial_pose[2]):
continue
command = Command([], parameters=[rel_pose[:2], rel_pose[2]])
yield (grasp, initial_pose, command)
def genScoop(grip_name, spoon_name, kettle_name, pose2):
if kettle_name != 'sugar_cup':
return
kitchen = ph.make_kitchen(planning=True)
gripper = ph.make_body(kitchen, grip_name, ph.gripperInitPos)
spoon = ph.make_body(kitchen, spoon_name, ph.cupInitPos)
kettle = ph.make_body(kitchen, kettle_name, pose2)
#####################GP START##############################
gp, c = helper.process_gp_sample(ph.expid_scoop,
flag_lk=ph.FLAG_LK,
is_adaptive=ph.IS_ADAPTIVE,
task_lengthscale=ph.TASK_LENGTH_SCALE,
betalambda=0.998,
exp='scoop')
while True:
rel_x1, rel_y1, rel_x2, rel_y2, rel_x3, rel_y3, grasp, _, _ = gp.sample(
c)
rel_pos1 = (rel_x1, rel_y1)
rel_pos2 = (rel_x2, rel_y2)
rel_pos3 = (rel_x3, rel_y3)
######################GP END#############################
gripper.set_grasped(spoon, grasp, ph.gripperInitPos[:2],
ph.gripperInitPos[2])
scoop_pose, end_pose = map(
tuple, gripper.get_scoop_init_end_pose(kettle, rel_pos1, rel_pos3))
init_pose = tuple(np.array(scoop_pose) + np.array([0, 5, 0])) # 7.5
traj1 = HoldingTrajectory(grip_name, spoon_name, grasp,
(init_pose, scoop_pose))
traj2 = HoldingTrajectory(grip_name, spoon_name, grasp,
(scoop_pose, end_pose))
command = Command([traj1, traj2], [rel_pos1, rel_pos2, rel_pos3])
yield (init_pose, end_pose, grasp, command)
def main():
kitchen = Kitchen2D(**SETTING)
expid_pour, expid_scoop = 0, 0
# Try setting is_adaptive to be False. It will use the adaptive sampler that samples uniformly from
# the feasible precondition. If is_adaptive is False, it uses the diverse sampler. If flag_lk is False,
# and is_adaptive is False, it uses the diverse sampler with a fixed kernel. If flag_lk is True,
# and is uniform is Ture, it uses the diverse sampler with the kernel parameters learned from plan feedbacks.
gp_pour, c_pour = helper.process_gp_sample(expid_pour,
exp='pour',
is_adaptive=False,
flag_lk=False)
pour_to_w, pour_to_h, pour_from_w, pour_from_h = c_pour
gp_scoop, c_scoop = helper.process_gp_sample(expid_scoop,
exp='scoop',
is_adaptive=True,
flag_lk=False)
scoop_w, scoop_h = c_scoop
holder_d = 0.5
# Create objects
gripper = Gripper(kitchen, (0, 8), 0)
cup1 = ks.make_cup(kitchen, (10, 0), 0, pour_from_w, pour_from_h, holder_d)
cup2 = ks.make_cup(kitchen, (-25, 0), 0, pour_to_w, pour_to_h, holder_d)
block = ks.make_block(kitchen, (-9, 0), 0, 4, 4)
large_cup = ks.make_cup(kitchen, (23, 0), 0, scoop_w, scoop_h, holder_d)
# Move
query_gui('MOVE', kitchen, dontask=True)
gripper.find_path((-5., 10), 0, maxspeed=0.5)
# Push
query_gui('PUSH', kitchen, dontask=True)
gripper.push(block, (1., 0.), -6, 0.5)
# Pick
query_gui('GRASP', kitchen, dontask=True)
# Sample from the super level set of the GP learned for pour
grasp, rel_x, rel_y, dangle, _, _, _, _ = gp_pour.sample(c_pour)
dangle *= np.sign(rel_x)
gripper.find_path((15, 10), 0)
gripper.grasp(cup1, grasp)
# Get water
query_gui('GET-WATER', kitchen, dontask=True)
gripper.get_liquid_from_faucet(5)
# Pour
query_gui('POUR', kitchen, dontask=True)
print(gripper.pour(cup2, (rel_x, rel_y), dangle))
# Place
query_gui('PLACE', kitchen, dontask=True)
gripper.place((10, 0), 0)
# Scoop
kitchen.gen_liquid_in_cup(large_cup, 1000, 'sugar')
kitchen.gen_liquid_in_cup(cup1, 200)
spoon = ks.make_spoon(kitchen, (23, 10), 0, 0.2, 3, 1.)
query_gui('SCOOP', kitchen, dontask=True)
rel_x1, rel_y1, rel_x2, rel_y2, rel_x3, rel_y3, grasp, _, _ = gp_scoop.sample(
c_scoop)
rel_pos1 = (rel_x1, rel_y1)
rel_pos2 = (rel_x2, rel_y2)
rel_pos3 = (rel_x3, rel_y3)
gripper.set_grasped(spoon, grasp, (23, 10), 0)
print(gripper.scoop(large_cup, rel_pos1, rel_pos2, rel_pos3))
# Dump
query_gui('DUMP', kitchen, dontask=True)
gripper.dump(cup1, 0.9)
# Place
query_gui('PLACE', kitchen, dontask=True)
gripper.place((26, 10.), 0)
# Stir
query_gui('STIR', kitchen, dontask=True)
stirrer = ks.make_stirrer(kitchen, (0, 3.5), 0., 0.2, 5., 0.5)
gripper.set_grasped(stirrer, 0.8, (10, 10), 0)
gripper.stir(cup1, (0, 0.0), (1, 0.0))
gripper.find_path(gripper.position + [0, 5], 0)