def run_config(config):
'''
A standard way to run an experiment config (in original cpp version configs were *.cfg files)
:param config: Dict[String, object] with parameters of an experiment:
integration_step: The simulation step (in seconds). This is the resolution of the propagations.
debug_period: The frequency for performing statistics gathering (0=don't perform statistics gathering)
min_time_steps: The minimum number of simulation steps in a propagation.
max_time_steps: The maximum number of simulation steps in a propagation.
random_seed: The seed to the pseudo-random number generator
sst_delta_near: The radius for BestNear (delta_n in the WAFR paper)
sst_delta_drain: The radius for sparsification (delta_s in the WAFR paper)
planner: The motion planner to use (string)
system: The name of the system to plan for (or system object)
start_state: The start state for the system
goal_state: The goal state for the system
goal_radius: The goal tolerance. (This is needed because in general, a dynamic system cannot reach a target state exactly.)
'''
config = config.copy()
if isinstance(config['system'], str):
system = create_standard_system(config['system'])
else:
system = config['system']
if config['planner'] == 'sst':
planner = SST(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=config['start_state'],
goal_state=config['goal_state'],
goal_radius=config['goal_radius'],
random_seed=config['random_seed'],
sst_delta_near=float(config['sst_delta_near']),
sst_delta_drain=float(config['sst_delta_drain']))
elif config['planner'] == 'rrt':
planner = RRT(
state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=config['start_state'],
goal_state=config['goal_state'],
goal_radius=config['goal_radius'],
random_seed=config['random_seed'],
)
else:
raise Exception("Uknown planner")
if 'number_of_iterations' not in config:
config['number_of_iterations'] = int(1e6)
if 'debug_period' not in config:
config['debug_period'] = int(1e3)
min_time_steps = int(config['min_time_steps'])
max_time_steps = int(config['max_time_steps'])
integration_step = float(config['integration_step'])
run_planning_experiment(planner, system, config['number_of_iterations'],
min_time_steps, max_time_steps, integration_step,
config['debug_period'], config['display_type'])
def run_planner(self, s):
"""
Run the planner for seed s
:param s : the seed to run the planner on.
"""
env = create_env(self.robot, self.env, s)
wrapped_env = BcGymWrapper(env)
start, goal = wrapped_env.start, wrapped_env.goal
bc_robot = BcSstWrapper(wrapped_env)
planner = SST(
state_bounds=bc_robot.get_state_bounds(),
control_bounds=bc_robot.get_control_bounds(),
distance=bc_robot.distance_computer(),
start_state=start,
goal_state=goal,
goal_radius=0.5,
random_seed=0,
sst_delta_near=0.65,
sst_delta_drain=0.05,
)
for iteration in range(100000):
planner.step(bc_robot, 1, 10, .05)
if iteration % 10000 == 0:
solution = planner.get_solution()
if solution:
return 1
solution = planner.get_solution()
if solution:
return 1
return 0
def mapListener():
#create node
rospy.init_node('map_listener', anonymous=True)
#subscribe /map topic
print('XXXXXXXXXXXXXXXXXX')
print('geting data from rosnode')
w = 0
h = 0
rospy.Subscriber("map", OccupancyGrid, callback)
# spin() simply keeps python from exiting until this node is stopped
pub = rospy.Publisher('mapaaaa', OccupancyGrid, queue_size=10)
rate = rospy.Rate(1) # 1hz
start_experiment = False
if_execute = True
# variable for future, to keep path to publish
path = []
while not rospy.is_shutdown():
#publish copy of the map
pub.publish(map_cpy)
map_data = {}
if len(map_cpy.data) > 0 and len(path) == 0:
map_data, obstacles, w, h = get_data_system(map_cpy)
start_experiment = True
if start_experiment == True and if_execute == True:
# start the experiment
if_execute = False
print('##################################')
print('setup system')
system = FreePoint(obstacles, obstacles.shape[0], map_data)
#create sst planner
print(obstacles.shape[0])
planner = SST(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=np.array([2., 15.]),
goal_state=np.array([15., 15.]),
goal_radius=0.5,
random_seed=0,
sst_delta_near=0.4,
sst_delta_drain=0.2)
"""
* for map.yaml start and end points might be:
* start_state=np.array([2., 15.])
* goal_state=np.array([15., 15.])
"""
cv2.namedWindow('map')
print('start the experiment')
for iteration in range(20000):
planner.step(system, 2, 20, 0.1)
if iteration % 100 == 0:
solution = planner.get_solution()
print("Solution: %s, Number of nodes: %s" %
(planner.get_solution(),
planner.get_number_of_nodes()))
rapper = planner.visualize_nodes(system)
solution = planner.get_solution()
if len(solution[0]) > 0:
# draw path if the solution exist
path = solution[0]
rapper = path_drawing(solution, rapper, w, h)
q = ord('a')
while q != ord('q'):
cv2.imshow('map', rapper)
q = cv2.waitKey(30)
cv2.destroyAllWindows()
rate.sleep()
print(start)
print('end:')
print(end)
# planning
#start[2] = 0.
#start[3] = 0.
#end[1] = 0.
#end[3] = 0.
planner = SST(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain)
# Run planning and print out solution is some statistics every few iterations.
time0 = time.time()
min_cost = 1e10
for iteration in range(max_iter):
#if iteration % 50 == 0:
# # from time to time use the goal
# sample = end
# planner.step_with_sample(system, sample, 20, 200, 0.002)
#system = CartPoleObs(obs_list)
# Create SST planner
min_time_steps = 2
max_time_steps = 4
integration_step = 0.002
max_iter = 2
goal_radius=1.5
random_seed=0
sst_delta_near=2.0
sst_delta_drain=1.2
planner = SST(
state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=np.array([0., 0., np.pi, 0.]),
goal_state=np.array([5., 0., 0., 0.]),
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain
)
low = []
high = []
state_bounds = system.get_state_bounds()
for i in range(len(state_bounds)):
low.append(state_bounds[i][0])
high.append(state_bounds[i][1])
end = np.array([5., 0., 0., 0.])
system = Acrobot()
start_state = np.array([0., 0., 0., 0.])
goal_state = np.array([3.14, 0, 0., 0.])
curr_state = start_state.copy()
curr_state_bk = curr_state.copy()
while True:
curr_state = curr_state_bk.copy()
print("Current state:", curr_state)
planner = SST(
state_bounds=model.get_state_bounds(),
control_bounds=model.get_control_bounds(),
distance=model.distance_computer(),
start_state=curr_state,
goal_state=goal_state,
goal_radius=1.0,
random_seed=0,
sst_delta_near=1.,
sst_delta_drain=0.5,
)
for iteration in range(50000):
planner.step(model, 10, 10, 0.05)
if iteration % 100 == 0:
solution = planner.get_solution()
print("Solution: %s, Number of nodes: %s" %
(solution, planner.get_number_of_nodes()))
if solution is not None:
break
controls = solution[1] # path,control,cost
for i in range(len(controls)):
def main(args):
# set seed
print(args.model_path)
torch_seed = np.random.randint(low=0, high=1000)
np_seed = np.random.randint(low=0, high=1000)
py_seed = np.random.randint(low=0, high=1000)
torch.manual_seed(torch_seed)
np.random.seed(np_seed)
random.seed(py_seed)
# Build the models
if torch.cuda.is_available():
torch.cuda.set_device(args.device)
# setup evaluation function and load function
if args.env_type == 'pendulum':
IsInCollision = pendulum.IsInCollision
normalize = pendulum.normalize
unnormalize = pendulum.unnormalize
obs_file = None
obc_file = None
cae = cae_identity
mlp = MLP
system = standard_cpp_systems.PSOPTPendulum()
bvp_solver = _sst_module.PSOPTBVPWrapper(system, 2, 1, 0)
max_iter = 200
min_time_steps = 20
max_time_steps = 200
integration_step = 0.002
goal_radius = 0.1
random_seed = 0
sst_delta_near = 0.05
sst_delta_drain = 0.02
vel_idx = [1]
mpNet = KMPNet(args.total_input_size, args.AE_input_size,
args.mlp_input_size, args.output_size, cae, mlp)
# load previously trained model if start epoch > 0
model_path = 'kmpnet_epoch_%d.pkl' % (args.start_epoch)
if args.start_epoch > 0:
load_net_state(mpNet, os.path.join(args.model_path, model_path))
torch_seed, np_seed, py_seed = load_seed(
os.path.join(args.model_path, model_path))
# set seed after loading
torch.manual_seed(torch_seed)
np.random.seed(np_seed)
random.seed(py_seed)
if torch.cuda.is_available():
mpNet.cuda()
mpNet.mlp.cuda()
mpNet.encoder.cuda()
if args.opt == 'Adagrad':
mpNet.set_opt(torch.optim.Adagrad, lr=args.learning_rate)
elif args.opt == 'Adam':
mpNet.set_opt(torch.optim.Adam, lr=args.learning_rate)
elif args.opt == 'SGD':
mpNet.set_opt(torch.optim.SGD, lr=args.learning_rate, momentum=0.9)
if args.start_epoch > 0:
load_opt_state(mpNet, os.path.join(args.model_path, model_path))
# load train and test data
print('loading...')
if args.seen_N > 0:
seen_test_data = data_loader.load_test_dataset(
N=args.seen_N,
NP=args.seen_NP,
s=args.seen_s,
sp=args.seen_sp,
p_folder=args.path_folder,
obs_f=obs_file,
obc_f=obc_file)
if args.unseen_N > 0:
unseen_test_data = data_loader.load_test_dataset(
N=args.unseen_N,
NP=args.unseen_NP,
s=args.unseen_s,
sp=args.unseen_sp,
p_folder=args.path_folder,
obs_f=obs_file,
obc_f=obc_file)
# test
# testing
print('testing...')
seen_test_suc_rate = 0.
unseen_test_suc_rate = 0.
T = 1
obc, obs, paths, path_lengths = seen_test_data
if obs is not None:
obs = obs.astype(np.float32)
obs = torch.from_numpy(obs)
fes_env = [] # list of list
valid_env = []
time_env = []
time_total = []
normalize_func = lambda x: normalize(x, args.world_size)
unnormalize_func = lambda x: unnormalize(x, args.world_size)
low = []
high = []
state_bounds = system.get_state_bounds()
for i in range(len(state_bounds)):
low.append(state_bounds[i][0])
high.append(state_bounds[i][1])
for i in range(len(paths)):
time_path = []
fes_path = [] # 1 for feasible, 0 for not feasible
valid_path = [] # if the feasibility is valid or not
# save paths to different files, indicated by i
# feasible paths for each env
suc_n = 0
sst_suc_n = 0
for j in range(len(paths[0])):
time0 = time.time()
time_norm = 0.
fp = 0 # indicator for feasibility
print("step: i=" + str(i) + " j=" + str(j))
p1_ind = 0
p2_ind = 0
p_ind = 0
if path_lengths[i][j] == 0:
# invalid, feasible = 0, and path count = 0
fp = 0
valid_path.append(0)
if path_lengths[i][j] > 0:
fp = 0
valid_path.append(1)
path = [paths[i][j][0], paths[i][j][path_lengths[i][j] - 1]]
start = paths[i][j][0]
end = paths[i][j][path_lengths[i][j] - 1]
start[1] = 0.
end[1] = 0.
# plot the entire path
#plt.plot(paths[i][j][:,0], paths[i][j][:,1])
"""
planner = SST(
state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain
)
"""
planner = RRT(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0)
control = []
time_step = []
MAX_NEURAL_REPLAN = 11
if obs is None:
obs_i = None
obc_i = None
else:
obs_i = obs[i]
obc_i = obc[i]
print('created RRT')
# Run planning and print out solution is some statistics every few iterations.
time0 = time.time()
start = paths[i][j][0]
#end = paths[i][j][path_lengths[i][j]-1]
new_sample = start
sample = start
N_sample = 10
for iteration in range(max_iter // N_sample):
#if iteration % 50 == 0:
# # from time to time use the goal
# sample = end
# #planner.step_with_sample(system, sample, 20, 200, 0.002)
#else:
#planner.step(system, min_time_steps, max_time_steps, integration_step)
#sample = np.random.uniform(low=low, high=high)
for num_sample in range(N_sample):
ip1 = np.concatenate([new_sample, end])
np.expand_dims(ip1, 0)
#ip1=torch.cat((obs,start,goal)).unsqueeze(0)
time0 = time.time()
ip1 = normalize_func(ip1)
ip1 = torch.FloatTensor(ip1)
time_norm += time.time() - time0
ip1 = to_var(ip1)
if obs is not None:
obs = torch.FloatTensor(obs).unsqueeze(0)
obs = to_var(obs)
sample = mpNet(ip1, obs).squeeze(0)
# unnormalize to world size
sample = sample.data.cpu().numpy()
time0 = time.time()
sample = unnormalize_func(sample)
print('sample:')
print(sample)
print('start:')
print(start)
print('goal:')
print(end)
print('accuracy: %f' % (float(suc_n) / (j + 1)))
print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1)))
sample = planner.step_with_sample(system, sample,
min_time_steps,
max_time_steps, 0.002)
#planner.step_bvp(system, 10, 200, 0.002)
im = planner.visualize_nodes(system)
show_image(im, 'nodes', wait=False)
new_sample = planner.step_with_sample(system, end,
min_time_steps,
max_time_steps, 0.002)
solution = planner.get_solution()
if solution is not None:
print('solved.')
suc_n += 1
break
planner = SST(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain)
# Run planning and print out solution is some statistics every few iterations.
time0 = time.time()
start = paths[i][j][0]
#end = paths[i][j][path_lengths[i][j]-1]
new_sample = start
sample = start
N_sample = 10
for iteration in range(max_iter // N_sample):
for k in range(N_sample):
sample = np.random.uniform(low=low, high=high)
planner.step_with_sample(system, sample, min_time_steps,
max_time_steps, integration_step)
im = planner.visualize_tree(system)
show_image(im, 'tree', wait=False)
print('accuracy: %f' % (float(suc_n) / (j + 1)))
print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1)))
planner.step_with_sample(system, end, min_time_steps,
max_time_steps, integration_step)
solution = planner.get_solution()
if solution is not None:
print('solved.')
sst_suc_n += 1
break
print('accuracy: %f' % (float(suc_n) / (j + 1)))
print('sst accuracy: %f' % (float(sst_suc_n) / (j + 1)))
high.append(state_bounds[i][1])
start = np.random.uniform(low=low, high=high)
end = np.random.uniform(low=low, high=high)
start[1] = 0.
start[3] = 0.
end[1] = 0.
end[3] = 0.
planner = SST(
state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain
)
# Run planning and print out solution is some statistics every few iterations.
time0 = time.time()
for iteration in range(max_iter):
if iteration % 50 == 0:
# from time to time use the goal
sample = end
planner.step_with_sample(system, sample, min_time_steps, max_time_steps, integration_step)
else:
print(start)
print('end:')
print(end)
# planning
#start[2] = 0.
#start[3] = 0.
#end[1] = 0.
#end[3] = 0.
planner = SST(state_bounds=system.get_state_bounds(),
control_bounds=system.get_control_bounds(),
distance=system.distance_computer(),
start_state=start,
goal_state=end,
goal_radius=goal_radius,
random_seed=0,
sst_delta_near=sst_delta_near,
sst_delta_drain=sst_delta_drain)
# Run planning and print out solution is some statistics every few iterations.
time0 = time.time()
for iteration in range(max_iter):
#if iteration % 50 == 0:
# # from time to time use the goal
# sample = end
# planner.step_with_sample(system, sample, 20, 200, 0.002)
#else: