class StreamFn(TestStream):
def test(self, (p, t)):
holding = ObjGrasp(t.obj, t.pap.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
#assert pose.obj != holding.object_name # NOTE - they cannot be the same for this check
return not self.cond_stream.oracle.holding_collision(
t.pap.grasp_config, p.obj, p.value, holding)
def collision_free(o, p, t):
if p is None or o == t.obj:
return True
holding = ObjGrasp(t.obj, t.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
if not DO_ARM_MOTION:
return not oracle.holding_collision(t.grasp_config, o, p, holding)
return not oracle.traj_holding_collision(t.approach_config, t.trajs, o, p, holding)
def collision_free(p, pap):
if not DO_COLLISIONS or p is None or p.obj == pap.obj:
return True
holding = ObjGrasp(pap.obj, pap.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
#assert pose.obj != holding.object_name # NOTE - they cannot be the same for this check
if not DO_ARM_MOTION:
return not oracle.holding_collision(pap.grasp_config, p.obj, p, holding)
return not oracle.traj_holding_collision(pap.approach_config, pap.trajs, p.obj, p, holding)
def convert_state(oracle, pddl_state):
world_state = {'holding': False}
for object_name in oracle.get_objects():
world_state[object_name] = False
for atom in pddl_state:
if atom.predicate == AtConfig:
config, = atom.args
world_state['robot'] = config.value
elif atom.predicate == AtPose:
obj, pose = atom.args
world_state[obj.name] = pose.value
elif atom.predicate == HasGrasp:
obj, grasp = atom.args
world_state['holding'] = ObjGrasp(obj.name, grasp.value)
return world_state
def convert_state(oracle, pddl_state):
world_state = {'holding': False}
for object_name in oracle.get_objects():
world_state[object_name] = False
for atom in pddl_state:
if atom.predicate == ConfEq:
config, = map(get_value, atom.args)
world_state['robot'] = config
elif atom.predicate == PoseEq:
obj, pose = map(get_value, atom.args)
world_state[obj] = pose
elif atom.predicate == GraspEq:
obj, grasp = map(get_value, atom.args)
world_state['holding'] = ObjGrasp(obj, grasp)
return world_state
def are_colliding(p, t, oracle):
if p.obj == t.obj:
return False
holding = ObjGrasp(t.obj, t.pap.grasp)
#holding = Holding(self.oracle.get_body_name(pap.geom_hash), pap.grasp)
#assert pose.obj != holding.object_name # NOTE - they cannot be the same for this check
#if aabb_stacked(aabb_from_body(oracle.get_body(p.obj)), aabb_from_body(oracle.get_body(t.obj))):
# return True
if aabb_stacked(oracle.get_aabb(p.obj, trans_from_pose(p.value.value)),
oracle.get_aabb(t.obj, trans_from_pose(t.pap.pose.value))):
return True
if DO_MOTION:
return oracle.traj_holding_collision(t.pap.approach_config,
t.pap.trajs, p.obj, p.value,
holding)
return oracle.holding_collision(t.pap.grasp_config, p.obj, p.value,
holding)
def convert_state(state):
world_state = {'holding': False}
for var, val in state.iteritems():
name = var[0]
if name == R:
world_state['robot'] = val
elif name == O:
_, obj = var
if isinstance(val, Pose):
world_state[obj] = val
elif isinstance(val, GraspTform):
world_state['holding'] = ObjGrasp(obj, val)
else:
raise ValueError(val)
elif name == H:
pass
else:
raise ValueError(name)
return world_state
def convert_state(oracle, state):
world_state = {
'holding': False,
'robot': oracle.initial_config,
}
for var, val in state.iteritems():
name = var[0]
if name == R:
world_state['robot'] = val
elif name == P and val is not None:
_, obj = var
assert isinstance(val, Pose)
world_state[obj] = val
elif name == G and val is not None:
_, obj = var
assert isinstance(val, GraspTform)
world_state['holding'] = ObjGrasp(obj, val)
#else:
# raise ValueError(name)
return world_state