Пример #1
0
    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
Пример #2
0
    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
Пример #3
0
 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
Пример #4
0
def make_kitchen(planning, kitchen_params, poses, sizes):
    '''
    Create a kitchen object of class Kitchen2D. 
    We add the holders for spoons and stirrers as static 
    Box2D objects in the kitchen.
    '''
    kitchen = Kitchen2D(planning=planning, **kitchen_params)

    # create holders for spoons and stirrers
    width_eps = 0.05
    for name, pose in poses.items():
        _, holder_h, holder_d = sizes['holder']
        if 'spoon' in name:
            holder_w = 2 * holder_d + sizes[name][2] + width_eps
            pose_x = poses[name][0] - holder_d - width_eps / 2
        elif 'stirrer' in name:
            holder_w = 2 * holder_d + 2 * sizes[name][2] + width_eps
            pose_x = poses[name][0] - holder_w / 2
        else:
            continue

        holder = ks.make_static_cup(kitchen, (pose_x, 0), 0, holder_w,
                                    holder_h, holder_d)

    return kitchen
Пример #5
0
def execute_plan_command(plan, kitchen_params, gripperInitPos, cupInitPos,
                         cupSize, kettleInitPos, kettleSize, **kwargs):
    kitchen = Kitchen2D(planning=False, **kitchen_params)
    kitchen.enable_gui()
    gripper = Gripper(kitchen,
                      init_pos=gripperInitPos[:2],
                      init_angle=gripperInitPos[2])
    cup = ks.make_cup(kitchen, cupInitPos[:2], cupInitPos[2], *cupSize)
    kettle = ks.make_cup(kitchen, kettleInitPos[:2], kettleInitPos[2],
                         *kettleSize)

    pause = False

    def execute_command(command):
        for traj in command.trajectories:
            print traj
            if pause:
                raw_input('Execute?')
            if isinstance(traj, FreeTrajectory):
                if gripper.attached:
                    gripper.open()
                    gripper.release()
            else:
                if not gripper.attached:
                    gripper.close()
                    gripper.attach(cup)
            gripper.apply_control(traj.path, maxspeed=2.5)

    raw_input('Begin?')
    for i, (action, args) in enumerate(plan):
        print i, action, args
        if action.name == 'fill':
            pass
        elif action.name == 'place':
            execute_command(args[-1].reverse())
        elif action.name == 'pour-gp':
            kp, p2 = args[-2:]
            rel_pose = np.array(p2) - np.array(kp)
            poured, score = gripper.pour(kettle, rel_pose[:2], rel_pose[2],
                                         0.45, 200)
        else:
            execute_command(args[-1])
Пример #6
0
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)