def generate(self, context=None):
self.enumerated = True # Prevents from being resampled
grip_name, pose1, pose2 = self.inputs
if not ph.do_motion:
path = (pose1, pose2)
command = Command([ClosedTrajectory(grip_name, 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
] and not isinstance(pose, SharedOutputSet):
ph.make_body(kitchen, name, pose)
gripper = Gripper(kitchen, init_pos=pose1[:2], init_angle=pose1[2])
gripper.set_closed()
path = gripper.plan_path(pose2[:2],
pose2[2],
collision_buffer=ph.buffer_distance)
if path is None:
return []
command = Command([ClosedTrajectory(grip_name, path)])
return [(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
