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
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)
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):
# for now, check always returns true
# n_stirs are always 5, ignore it for now
rel_pos1_x, rel_pos1_y, rel_pos2_x, rel_pos2_y, cup_w, cup_h, grasp_ratio = x
# creates the Box2D world objects to execute this action
kitchen = Kitchen2D(**settings[0])
gripper = Gripper(kitchen, (5, 8), 0)
cup = ks.make_cup(kitchen, (0, 0), 0, cup_w, cup_h, 0.5)
stirrer = ks.make_stirrer(kitchen, (0, 3.5), 0., 0.2, 5., 0.5)
gripper.set_grasped(stirrer, 0.8, (10, 10), 0)
# stirrer = ks.make_stirrer(kitchen, (0.5,1.0), 0, 0.2, 5., 0.5)
# print gripper.set_grasped(stirrer, 0.8, (0.5,1.0), 0)
dposa1, dposa2 = gripper.get_stir_init_end_pose(
cup, (rel_pos1_x, rel_pos1_y), (rel_pos2_x, rel_pos2_y))
# print gripper.set_grasped(stirrer, grasp_ratio, dposa1[:2], dposa1[2])
# seems that it almost never has collision
g2 = gripper.simulate_itself()
collision = g2.check_point_collision(
dposa1[:2], dposa1[2]) and g2.check_point_collision(
dposa2[:2], dposa2[2])
if collision:
return False
# print {"rel_pos1_x": rel_pos1_x, "rel_pos1_y": rel_pos1_y, "rel_pos2_x" : rel_pos2_x, \
# "rel_pos2_y": rel_pos2_y, "cup_w": cup_w, "cup_h": cup_h, "grasp_ratio": grasp_ratio, \
# "dposa1": dposa1, "dposa2": dposa2}
self.kitchen = kitchen
self.gripper = gripper
self.cup = cup
self.stirrer = stirrer
return True
