def genGraspControl(grip_name, cup_name, pose2, grasp):
kitchen = ph.make_kitchen(planning=True)
gripper = ph.make_body(kitchen, grip_name, ph.gripperInitPos)
gripper.open()
cup = ph.make_body(kitchen, cup_name, pose2)
down_pos, up_pos = gripper.get_grasp_poses(cup, grasp)
up_pos = np.array(down_pos) + np.array([0, 5])
ori = 0
up_pose = (up_pos[0], up_pos[1], ori)
down_pose = (down_pos[0], down_pos[1], ori)
if not ph.do_grasp:
traj1 = FreeTrajectory(grip_name, (up_pose, down_pose))
traj2 = HoldingTrajectory(grip_name, cup_name, grasp, (down_pose, up_pose))
return (up_pose, Command([traj1, traj2]))
traj1 = FreeTrajectory(grip_name, (up_pose, down_pose))
gripper.set_position(down_pose[:2], down_pose[2])
gripper.close()
if not gripper.check_grasp(cup): return
traj2 = HoldingTrajectory(grip_name, cup_name, grasp, (down_pose, up_pose))
command = Command([traj1, traj2])
return (up_pose, command)
def generate(self, context=None):
self.enumerated = True # Prevents from being resampled
grip_name, pose1, cup_name, grasp, pose2 = self.inputs
if not ph.do_motion:
path = (pose1, pose2)
command = Command([HoldingTrajectory(grip_name, cup_name, grasp, path)])
return [(command,)]
kitchen = ph.make_kitchen(planning=True)
if self.use_context and (context is not None):
for atom in context.conditions:
_, name, pose = atom.head.args
if name not in [grip_name, cup_name] and not isinstance(pose, SharedOutputSet):
ph.make_body(kitchen, name, pose)
gripper = Gripper(kitchen, init_pos=pose1[:2], init_angle=pose1[2])
cup = ph.make_body(kitchen, cup_name, ph.cupInitPos)
gripper.set_grasped(cup, grasp, pose1[:2], pose1[2])
motion_angle = min(pose1[2], pose2[2])
path = gripper.plan_path(pose2[:2], pose2[2],
motion_angle=motion_angle, collision_buffer=ph.buffer_distance)
if path is None:
return []
command = Command([HoldingTrajectory(grip_name, cup_name, grasp, path)])
return [(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 genDump(grip_name, spoon_name, grasp, kettle_name, pose2):
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)
gripper.set_grasped(spoon, grasp, ph.gripperInitPos[:2], ph.gripperInitPos[2])
rel_pos_x = 0.8
init_pose, dump_pose = map(tuple, gripper.get_dump_init_end_pose(kettle, rel_pos_x))
traj1 = HoldingTrajectory(grip_name, spoon_name, grasp, (init_pose, dump_pose))
return (init_pose, dump_pose, Command([traj1], parameters=[rel_pos_x]))
def genStir(grip_name, stir_name, grasp, kettle_name, pose2):
kitchen = ph.make_kitchen(planning=True)
gripper = ph.make_body(kitchen, grip_name, ph.gripperInitPos)
stirrer = ph.make_body(kitchen, stir_name, ph.cupInitPos)
kettle = ph.make_body(kitchen, kettle_name, pose2)
gripper.set_grasped(stirrer, grasp, ph.gripperInitPos[:2], ph.gripperInitPos[2])
init_pose, stir_pose = map(tuple, gripper.get_stir_init_end_pose(kettle, (0, 0), (1, 0)))
up_pose = tuple(np.array(init_pose) + np.array([0, 4, 0]))
traj1 = HoldingTrajectory(grip_name, stir_name, grasp, (up_pose, init_pose))
traj2 = HoldingTrajectory(grip_name, stir_name, grasp, (init_pose, stir_pose))
return (up_pose, Command([traj1, traj2, traj2.reverse(), traj1.reverse()]))