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):
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 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 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 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
