def birrt(q1, q2, distance, sample, extend, collision, restarts=RRT_RESTARTS, iterations=RRT_ITERATIONS, smooth=RRT_SMOOTHING):
for _ in irange(restarts+1):
path = rrt_connect(q1, q2, distance, sample, extend, collision, iterations=iterations)
if path is not None:
if smooth is None: return path
return smooth_path(path, extend, collision, iterations=smooth)
return None
def shortcut(self):
with self.cspace.body:
self.cspace.set_active()
return PathTrajectory(self.cspace, smooth_path(self.path(), extend_fn(self.cspace.body),
collision_fn(get_env(), self.cspace.body)))