class RRTController(Controller):
def __init__(self, control_res=0.03, collision_res=0.01):
super().__init__()
self.control_res = control_res
self.rrt = RRT(control_res, collision_res)
self.warned = False
def get_trajectory(self, env, kernel):
try:
if env.enable_lidar is True and not self.warned:
print('WARNING: Not turning off lidar on env could be significantly slower')
self.warned = True
xys = self.rrt.get_path(env.arena, kernel)
if xys is None:
return None
xys = self.rrt.increase_resolution(xys)
s = env.s()
traj = [s[:2]]
idx = 1
done = False
while not done:
dx, dy = xys[idx] - s[:2]
assert np.linalg.norm([dx, dy]) < self.control_res
s, _, done, _ = env.step(np.array([dx, dy]) / self.control_res)
traj.append(s[:2])
idx += 1
idx = min(idx, len(xys) - 1)
return np.array(traj)
except:
traceback.print_exc()
return None
def log_prior(self, kernel):
return 0
class RRTController(Controller):
def __init__(self, control_res=0.05, collision_res=0.01, visualize=False):
self.rrt = RRT(control_res, collision_res)
self.visualize = visualize
def get_trajectory(self, env, kernel):
try:
s = env.s()
target_loc = s[16:19]
planned_traj = self.rrt.get_path(target_loc, kernel)
if planned_traj is None:
return None
planned_traj = self.rrt.increase_resolution(planned_traj)
actual_traj = [s[:10]]
done = False
t = 1
while not done:
a = (planned_traj[t] - s[:7]) / 0.05
s, _, done, _ = env.step(a)
actual_traj.append(s[:10])
if t < len(planned_traj) - 1:
t += 1
if self.visualize:
time.sleep(0.03)
return np.array(actual_traj).astype('float32')
except KeyboardInterrupt:
raise
except:
traceback.print_exc()
return None
def log_prior(self, kernel):
return 0
def generate_data(fn, N):
arena = RBFArena()
rrt = RRT(control_res=0.2)
if os.path.isfile(fn):
input(
f'Data file {fn} already exists! Press Enter to overwrite, or Ctrl-C to abort... '
)
all_data = []
for _ in trange(N):
xs, ys = generate_single(arena, rrt)
dir_x = np.array(xs[1:]) - xs[:-1]
dir_y = np.array(ys[1:]) - ys[:-1]
angle = np.arctan2(dir_y, dir_x) * 180 / np.pi
lidar = np.array([arena.lidar(pos) for pos in zip(xs[:-1], ys[:-1])])
for x, y, lid, ang in zip(xs[:-1], ys[:-1], lidar, angle):
data = np.concatenate([[x, y], lid, [ang], arena.points.flatten()])
all_data.append(data)
all_data = np.array(all_data).astype('float32')
np.savez_compressed(fn, data=all_data)
def __init__(self, control_res=0.03, collision_res=0.01): super().__init__() self.control_res = control_res self.rrt = RRT(control_res, collision_res) self.warned = False
def __init__(self, control_res=0.05, collision_res=0.01, visualize=False):
self.rrt = RRT(control_res, collision_res)
self.visualize = visualize