def lattice_interpolation(robot, start, goal):
return astar(start,
goal,
distance_fn(robot),
neighbors_fn(robot, goal=goal),
lambda *args: False,
cost=lambda g, h: h)
#return astar(start, goal, l1_distance_fn(robot), neighbors_fn(robot, goal=goal), lambda *args: False, cost=lambda g, h: h)
# TODO - lattice search
def lattice_motion_plan(env,
cspace,
goal,
planner=astar,
self_collisions=False):
robot = cspace.body
with robot:
cspace.set_active()
start = robot.GetActiveDOFValues()
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
path = planner(
start, goal, distance_fn(robot), neighbors_fn(robot,
goal=goal),
collision_fn(env, robot, check_self=self_collisions))
if path is None: return None
return path
def __init__(self, oracle):
self.oracle = oracle
q1, q2 = P('q1', CONF), P('q2', CONF)
super(CollisionFreeTest, self).__init__(
[q1, q2],
[],
[
IsCollisionFree(q1, q2),
IsCollisionFree(q2, q1),
MoveCost(q1, q2), # TODO - need to specify the truth value
],
eager=EAGER_COLLISION)
self.collision = collision_fn(oracle.env,
oracle.robot,
check_self=True) # q_collision_fn
self.extend = extend_fn(oracle.robot) # q_extend_fn
self.distance = distance_fn(oracle.robot)
def replan(self,
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
smooth=RRT_SMOOTHING):
body = self.cspace.body
with body:
self.cspace.set_active()
path = birrt(self.start(),
self.end(),
distance_fn(body),
sample_fn(body),
extend_fn(body),
collision_fn(get_env(), body),
restarts=restarts,
iterations=iterations,
smooth=smooth)
if path is None: return None
return PathTrajectory(self.cspace, path)
def rl_motion_plan(env,
cspace,
goal,
select,
planner=abstract_lazy_best_first,
self_collisions=False,
max_iterations=INF):
robot = cspace.body
with robot:
cspace.set_active()
#with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs | openravepy_int.CollisionOptions.Distance):
#with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs | openravepy_int.CollisionOptions.UseTolerance):
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
#return planner(robot.GetActiveDOFValues(), goal, distance_fn(robot), neighbors_fn(robot, goal=goal),
# collision_fn(env, robot, check_self=self_collisions), select, max_iterations=max_iterations)
return planner(robot.GetActiveDOFValues(),
goal,
distance_fn(robot),
neighbors_fn(robot, goal=goal),
collision_fn(env, robot,
check_self=self_collisions),
select,
get_scale_fn(robot),
max_iterations=max_iterations)