def compile_problem(oracle):
problem = oracle.problem
CSpace.robot_base(oracle.robot).set_active()
initial_config = Config(
base_values_from_full_config(oracle.initial_config.value))
initial_atoms = [
AtConfig(Conf(initial_config)),
]
print 'Initial', initial_config.value
goal_literals = [AtConfig(Conf(problem.goal_config))
] if problem.goal_config is not None else []
print 'Goal', problem.goal_config.value
actions = [
Move(oracle),
]
axioms = [
#InRegionAxiom(),
]
cond_streams = [
CollisionFreeTest(oracle),
ConfStream(oracle),
]
objects = []
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, objects)
def sample_motion(q1, q2):
if not DO_BASE_MOTION:
yield [None]
#with oracle.robot: # NOTE - this causes an segfault when freeing the memory
oracle.set_robot_config(q1)
goal = base_values_from_full_config(q2.value)
traj = motion_plan(oracle.env, CSpace.robot_base(oracle.robot), goal)
if traj is not None:
traj.obj = None
yield [traj]
def sample_motion(q1, q2):
if not DO_BASE_MOTION:
yield [None]
with oracle.robot:
oracle.set_robot_config(q1)
goal = base_values_from_full_config(q2.value)
traj = motion_plan(oracle.env, CSpace.robot_base(oracle.robot), goal)
if traj is not None:
traj.obj = None
yield [traj]
