def check_legal(self, x): rel_x1, rel_y1, rel_x2, rel_y2, rel_x3, rel_y3, grasp_ratio, cw1, ch1 = x settings[0]['do_gui'] = self.do_gui kitchen = Kitchen2D(**settings[0]) gripper = Gripper(kitchen, (5, 8), 0) cup = ks.make_cup(kitchen, (0, 0), 0, cw1, ch1, 0.5) spoon = ks.make_spoon(kitchen, (5, 10), 0, 0.2, 3, 1.) gripper.set_grasped(spoon, grasp_ratio, (5, 10), 0) dposa1, dposa2 = gripper.get_scoop_init_end_pose( cup, (rel_x1, rel_y1), (rel_x3, rel_y3)) gripper.set_grasped(spoon, grasp_ratio, dposa1[:2], dposa1[2]) g2 = gripper.simulate_itself() collision = g2.check_point_collision(dposa1[:2], dposa1[2]) if collision: return False collision = g2.check_point_collision(dposa2[:2], dposa2[2]) if collision: return False self.kitchen = kitchen self.gripper = gripper self.cup = cup self.spoon = spoon return True
def check_legal(self, x): grasp_ratio, rel_x, rel_y, dangle, cw1, ch1, cw2, ch2 = x print(x) dangle *= np.sign(rel_x) settings[0]['do_gui'] = self.do_gui kitchen = Kitchen2D(**settings[0]) gripper = Gripper(kitchen, (5, 8), 0) cup1 = ks.make_cup(kitchen, (0, 0), 0, cw1, ch1, 0.5) cup2 = ks.make_cup(kitchen, (-15, 0), 0, cw2, ch2, 0.5, user_data='cup2') gripper.set_grasped(cup2, grasp_ratio, (-15, 0), 0) gripper.set_position((rel_x, rel_y), 0) if not kitchen.planning: g2 = gripper.simulate_itself() _, collision = g2.check_path_collision((rel_x, rel_y), 0, (rel_x, rel_y), dangle) if collision: return False self.kitchen = kitchen self.gripper = gripper self.cup1 = cup1 self.cup2 = cup2 return True
def check_legal(self, x): grasp_ratio, rel_x, rel_y, dangle, \ cw1, ch1, cw2, ch2, cw3, ch3, \ pos_x1, pos_x2, pos_x3 = x dangle *= np.sign(rel_x) settings[0]['do_gui'] = self.do_gui kitchen = Kitchen2D(**settings[0]) kitchen.gui_world.colors['water'] = self.water_color gripper = Gripper(kitchen, (5,8), 0) cup1 = ks.make_cup(kitchen, (pos_x1, 0), 0, cw1, ch1, 0.5, user_data=self.base_cup_type) cup2 = ks.make_cup(kitchen, (pos_x2, 0), 0, cw2, ch2, 0.5, user_data='cup2') cup3 = ks.make_cup(kitchen, (pos_x3, 0), 0, cw3, ch3, 0.5, user_data=self.base_cup_type) gripper.set_grasped(cup2, grasp_ratio, (pos_x2, 0), 0) gripper.set_position((rel_x, rel_y), 0) if not kitchen.planning: g2 = gripper.simulate_itself() _, collision = g2.check_path_collision((rel_x, rel_y), 0, (rel_x, rel_y), dangle) if collision: return False self.kitchen = kitchen self.gripper = gripper self.cup1 = cup1 self.cup2 = cup2 self.cup3 = cup3 self.properties = x[4:10] return True
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 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 test_collision(command, body_name, pose): ''' Returns True if the trajectory associated by command is colliding with a body of name body_name and pose pose. Otherwise False. ''' if not ph.do_collisions: return False for traj in command.trajectories: if body_name == traj.gripper or (isinstance(traj, HoldingTrajectory) and (body_name == traj.body)): continue kitchen = ks.b2WorldInterface(False) init_pose = ph.gripperInitPos gripper = Gripper(kitchen, init_pos=init_pose[:2], init_angle=init_pose[2]) if isinstance(traj, FreeTrajectory): gripper.release() gripper.set_open() elif isinstance(traj, PushTrajectory): body = ph.make_body(kitchen, traj.body, traj.init_pose) gripper.set_closed() gripper.attach(body) elif isinstance(traj, ClosedTrajectory): gripper.release() gripper.set_closed() else: body = ph.make_body(kitchen, traj.body, ph.cupInitPos) gripper.set_grasped(body, traj.pos_ratio, init_pose[:2], init_pose[2]) ph.make_body(kitchen, body_name, pose) g2 = gripper.simulate_itself() for q in traj.path: if g2.check_point_collision(q[:2], q[2]): print 'Collision: {}'.format(body_name) return True return False
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)