def main():
planning_env = CarEnvironment(args.map)
it = 0
max_it = 5
if not os.path.exists("../data_rrt/"):
os.mkdir("../data_rrt/")
while True:
print("working on %d " % it)
while True:
start = planning_env.sample()
if planning_env.state_validity_checker(start):
break
print(start)
while True:
goal = planning_env.sample()
if planning_env.state_validity_checker(
goal) and planning_env.compute_distance(start, goal) > 200:
break
print(goal)
planner = RRTPlannerNonholonomic(planning_env, bias=args.bias)
plan, actions, rollouts = planner.Plan(start, goal)
measurement = planning_env.get_measurement(rollouts)
# planning_env.init_visualizer()
# planning_env.visualize_traj(rollouts, measurement)
if actions is None:
continue
np.savetxt("../data_rrt/action_%d.txt" % it, actions)
np.savetxt("../data_rrt/rollout_%d.txt" % it, rollouts)
np.savetxt("../data_rrt/measure_%d.txt" % it, measurement)
it += 1
if it == max_it:
break
def main():
planning_env = CarEnvironment(args.map)
it = 0
max_it = 10
if not os.path.exists("../data_rrtstar/"):
os.mkdir("../data_rrtstar/")
while True:
print("working on %d " % it)
while True:
start = planning_env.sample()
if planning_env.state_validity_checker(start):
break
#start = np.array([900 + 10*np.random.random() - 5, 1000 + 10*np.random.random(), np.random.random() * np.pi * 2])[None].T
while True:
goal = planning_env.sample()
dist = planning_env.compute_distance(start, goal)
if planning_env.state_validity_checker(goal):
break
planner = RRTSTARPlannerNonholonomic(planning_env, bias=args.bias)
plan, fail = planner.Plan(start, goal)
if fail:
continue
planning_env.init_visualizer()
tree = planner.tree
planning_env.visualize_plan(plan, tree, None)
np.savetxt("../data_rrtstar/plan_%d.txt" % it, plan)
it += 1
if it == max_it:
break
import torch
device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu")
if __name__ == "__main__":
dpf = DPF()
dpf.load("../weights/DPFfocal.pt")
planning_env = CarEnvironment("../map/map.yaml")
planning_env.init_visualizer()
dpf.env = planning_env
particles = []
while True:
p = planning_env.sample()
if planning_env.state_validity_checker(p):
particles.append(p)
if len(particles) >= 200:
break
particles = np.array(particles)
particles = particles.squeeze()[None, ...] / np.array([1788, 1240, 1])
planning_env.reset()
state = planning_env.state
# particles = np.array(particles).squeeze()
obs = planning_env.get_measurement(state)
encoding = dpf.encoder(torch.FloatTensor(obs/4.0).to(device))
particles_pro = dpf.propose_particles(encoding, 100).detach()
sys.path.append('../Env')
import numpy as np
from MPNet import MPNet
from CarEnvironment import CarEnvironment
if __name__ == "__main__":
mpnet = MPNet()
mpnet.load()
planning_env = CarEnvironment("../map/map.png")
planning_env.init_visualizer()
size = np.array([1788, 1240])
while True:
start = planning_env.sample()
print(start)
if planning_env.state_validity_checker(start):
break
while True:
goal = planning_env.sample()
print(goal)
dist = planning_env.compute_distance(start, goal)
if planning_env.state_validity_checker(
goal) and dist > 200 and dist < 2000:
break
# start = np.array([[900, 1000, 0]]).T
# goal = np.array([[1300, 600, 0]]).T