class SimulationRunner:
def __init__(self, problem, config):
self.abs_path = os.path.dirname(os.path.abspath(__file__))
self.serializer = Serializer()
self.config_file = config
self.config = self.serializer.read_config(self.abs_path + "/" + config)
self.ik_solution_generator = IKSolutionGenerator()
self.clear_stats(problem)
self.run(problem)
def clear_stats(self, problem):
for file in glob.glob(self.abs_path + "/../problems/" + problem +
"/log.log"):
os.remove(file)
for file in glob.glob(self.abs_path + "/../problems/" + problem +
"/log.log"):
os.remove(file)
if os.path.isdir(self.abs_path + "/stats"):
cmd = "rm -rf " + self.abs_path + "/stats/*"
os.system(cmd)
else:
os.makedirs(self.abs_path + "/stats")
def write_rewards(self, log_file_path):
rewards = []
for line in tuple(open(log_file_path, 'r')):
if "Reward" in line:
rewards.append(float(line.split(":")[1][1:]))
n, min_max, mean_rewards, reward_variances, skew, kurt = stats.describe(
np.array(rewards))
if np.isnan(np.asscalar(reward_variances)):
reward_variances = np.array(0.0)
with open(log_file_path, "a+") as f:
f.write("Reward variance: " + str(np.asscalar(reward_variances)) +
" \n")
with open(log_file_path, "a+") as f:
f.write("Reward standard deviation: " +
str(np.asscalar(np.sqrt(reward_variances))) + " \n")
def read_float_data(self, log_file_path, searchstring):
data = 0.0
for line in tuple(open(log_file_path, 'r')):
if searchstring in line:
data = float(line.split(":")[1].split(" ")[1].rstrip("\n"))
return data
def read_int_data(self, log_file_path, searchstring):
data = 0.0
for line in tuple(open(log_file_path, 'r')):
if searchstring in line:
data = int(float(
line.split(":")[1].split(" ")[1].rstrip("\n")))
return data
def read_avg_path_length(self, log_file_path, num_runs):
l = 0
for line in tuple(open(log_file_path, 'r')):
if "S:" in line:
l += 1
return l / num_runs
def replace_option_in_config(self,
option_name,
value,
problem,
is_bool=False):
bool_str = None
if is_bool:
bool_str = "false"
if value:
bool_str = "true"
lines = list(
open(self.abs_path + "/../problems/" + problem + "/default.cfg",
'r'))
idx = -1
for i in xrange(len(lines)):
if option_name in lines[i]:
idx = i
if not idx == -1:
if is_bool:
lines[idx] = option_name + " = " + str(bool_str) + "\n"
else:
lines[idx] = option_name + " = " + str(value) + "\n"
os.remove(self.abs_path + "/../problems/" + problem + "/default.cfg")
with open(self.abs_path + "/../problems/" + problem + "/default.cfg",
'a+') as f:
for line in lines:
f.write(line)
def compareEnvironmentToTmpFiles(self, problem, model_file,
environment_path, environment_file):
if not os.path.exists(self.abs_path + "/tmp/" + problem):
os.makedirs(self.abs_path + "/tmp/" + problem)
return False
if not (os.path.exists(self.abs_path + '/tmp/' + problem + '/' +
environment_file)
and os.path.exists(self.abs_path + '/tmp/' + problem + '/' +
self.config_file)
and os.path.exists(self.abs_path + '/tmp/' + problem + '/' +
model_file)):
return False
with open(
self.abs_path + '/../problems/' + problem + "/environment/" +
environment_file, 'r') as f1, open(
self.abs_path + '/tmp/' + problem + '/' + environment_file,
'r') as f2:
missing_from_b = [
diff[2:]
for diff in Differ().compare(f1.readlines(), f2.readlines())
if diff.startswith('-')
]
if len(missing_from_b) != 0:
return False
with open(
self.abs_path + '/../problems/' + problem + "/model/" +
model_file, 'r') as f1, open(
self.abs_path + '/tmp/' + problem + '/' + model_file,
'r') as f2:
missing_from_b = [
diff[2:]
for diff in Differ().compare(f1.readlines(), f2.readlines())
if diff.startswith('-')
]
if len(missing_from_b) != 0:
return False
with open(self.abs_path + "/" + self.config_file, 'r') as f1, open(
self.abs_path + '/tmp/' + problem + '/' + self.config_file,
'r') as f2:
missing_from_b = [
diff[2:]
for diff in Differ().compare(f1.readlines(), f2.readlines())
if diff.startswith('-')
]
for i in xrange(len(missing_from_b)):
if ("num_links" in missing_from_b[i]
or "workspace_dimensions" in missing_from_b[i]
or "goal_position" in missing_from_b[i]
or "goal_radius" in missing_from_b[i]):
return False
""" If same, use existing goalstates """
try:
shutil.copy2(
self.abs_path + '/tmp/' + problem + '/goalstates.txt',
self.abs_path + '/../problems/' + problem + "/goalstates.txt")
except:
return False
return True
def copyToTmp(self, problem, model_file, environment_file):
shutil.copy2(
self.abs_path + '/../problems/' + problem + "/environment/" +
environment_file,
self.abs_path + '/tmp/' + problem + '/' + environment_file)
shutil.copy2(
self.abs_path + '/../problems/' + problem + "/model/" + model_file,
self.abs_path + '/tmp/' + problem + '/' + model_file)
shutil.copy2(
self.abs_path + '/../problems/' + problem + "/goalstates.txt",
self.abs_path + '/tmp/' + problem + '/goalstates.txt')
shutil.copy2(
self.abs_path + "/" + self.config_file,
self.abs_path + '/tmp/' + problem + '/' + self.config_file)
def get_average_distance_to_goal_area(self, goal_position, goal_radius,
cartesian_coords):
avg_dist = 0.0
goal_pos = np.array(goal_position)
for i in xrange(len(cartesian_coords)):
cart = np.array(cartesian_coords[i])
dist = np.linalg.norm(goal_pos - cart)
if dist < goal_radius:
dist = 0.0
avg_dist += dist
if avg_dist == 0.0:
return avg_dist
return np.asscalar(avg_dist) / len(cartesian_coords)
def write_mean_num_collisions(self, log_file_path, num_runs):
num_collisions = 0.0
with open(log_file_path, 'r') as f:
for line in f:
if "Trans: Collision detected: True" in line:
num_collisions += 1
mean_num_collisions = num_collisions / num_runs
with open(log_file_path, "a+") as f:
f.write("Mean num collisions per run: " +
str(mean_num_collisions) + " \n")
def run(self, problem):
alg = "alg"
if "manipulator_discrete" in problem:
alg = "abt"
elif "manipulator_continuous" in problem:
alg = "gps"
num_runs = self.config['num_simulation_runs']
init_state = self.config['init_state']
discount = self.config['discount_factor']
control_rate = self.config['control_rate']
illegal_move_penalty = self.config['illegal_move_penalty']
illegal_action_penalty = self.config['illegal_action_penalty']
step_penalty = self.config['step_penalty']
exit_reward = self.config['exit_reward']
planning_time = self.config['planning_time']
particle_count = self.config['particle_count']
num_steps = self.config['num_steps']
save_particles = self.config['save_particles']
plot_particles = self.config['plot_particles']
verbose = self.config['verbose']
verbose_rrt = self.config['verbose_rrt']
rrt_stretching_factor = self.config['rrt_stretching_factor']
histories_per_step = self.config['histories_per_step']
prune_every_step = self.config['prune_every_step']
continuous_collision = self.config['continuous_collision_check']
check_linear_path = self.config['check_linear_path']
enforce_constraints = self.config['enforce_constraints']
planner = self.config['planner']
dynamic_problem = self.config['dynamic_problem']
simulation_step_size = self.config['simulation_step_size']
planning_velocity = self.config['planning_velocity']
show_viewer = self.config['show_viewer']
max_observation_distance = self.config['max_observation_distance']
particle_replenish_timeout = self.config['particle_replenish_timeout']
state_sampling_strategy = self.config['state_sampling_strategy']
num_samples_weighted_lazy = self.config['num_samples_weighted_lazy']
gravity = self.config['gravity']
particle_plot_limit = self.config['particle_plot_limit']
num_effective_particles = self.config['num_effective_particles']
save_policy = self.config['save_policy']
load_initial_policy = self.config['load_initial_policy']
initial_planning_time = self.config['initial_planning_time']
particle_filter = self.config['particle_filter']
num_actions_per_joint = self.config['num_actions_per_joint']
environment_file = self.config['environment_file']
robot_file = self.config['robot_file']
num_generated_goal_states = self.config['num_generated_goal_states']
model_file = "test.xml"
environment_path = os.path.abspath(self.abs_path + "/../problems/" +
problem + "/environment/" +
environment_file)
utils = Utils()
if not os.path.exists(environment_path):
print "FILE DOES NOT EXISTS"
print "load obstacles"
obstacles = utils.loadObstaclesXML(environment_path)
print "loaded obstacles"
goal_area = v_double()
utils.loadGoalArea(environment_path, goal_area)
if len(goal_area) == 0:
print "ERROR: Your environment file doesn't define a goal area"
return False
goal_position = [goal_area[i] for i in xrange(0, 3)]
goal_radius = goal_area[3]
print goal_position
print goal_radius
logging_level = logging.WARN
if verbose:
logging_level = logging.DEBUG
logging.basicConfig(format='%(levelname)s: %(message)s',
level=logging_level)
if not os.path.exists(self.abs_path + "/../problems/" + problem +
"/model/" + robot_file):
print "what"
robot = Robot(
os.path.abspath(self.abs_path + "/../problems/" + problem +
"/model/" + robot_file))
ja = v_double()
init_angles = [init_state[i] for i in xrange(len(init_state) / 2)]
ja[:] = init_angles
print[goal_position[i] for i in xrange(len(goal_position))]
if not self.compareEnvironmentToTmpFiles(
problem, model_file, environment_path, environment_file):
self.ik_solution_generator.setup(robot, obstacles,
planning_velocity,
1.0 / control_rate, planner,
100000.0, True)
ik_solutions = self.ik_solution_generator.generate(
init_state, goal_position, goal_radius,
num_generated_goal_states)
if len(ik_solutions) == 0:
return None
self.serializer.serialize_ik_solutions(
[ik_solutions[i] for i in xrange(len(ik_solutions))], problem,
self.abs_path)
self.copyToTmp(problem, model_file, environment_file)
log_file_path = self.abs_path + "/../problems/" + str(
problem) + "/log.log"
initial_policy_file_path = self.abs_path + "/../problems/" + str(
problem) + "/pol.pol"
if os.path.exists(log_file_path):
os.remove(log_file_path)
"""
Set the number of simulation runs in the config file
"""
#self.replace_goal_angles_in_conf(ik_solution, problem)
self.replace_option_in_config('logPath', log_file_path, problem)
self.replace_option_in_config("policyPath", initial_policy_file_path,
problem)
self.replace_option_in_config('historiesPerStep', histories_per_step,
problem)
self.replace_option_in_config("nRuns", num_runs, problem)
self.replace_option_in_config("init_state", init_state, problem)
self.replace_option_in_config('control_rate', control_rate, problem)
self.replace_option_in_config('verbose', verbose, problem, True)
self.replace_option_in_config('rrt_verb', verbose_rrt, problem, True)
self.replace_option_in_config('discountFactor', discount, problem)
self.replace_option_in_config('illegalMovePenalty',
illegal_move_penalty, problem)
self.replace_option_in_config('illegalActionPenalty',
illegal_action_penalty, problem)
self.replace_option_in_config('stepPenalty', step_penalty, problem)
self.replace_option_in_config('exitReward', exit_reward, problem)
self.replace_option_in_config('nSteps', num_steps, problem)
self.replace_option_in_config("maximumDepth", num_steps, problem)
self.replace_option_in_config('saveParticles', save_particles, problem,
True)
self.replace_option_in_config('stretching_factor',
rrt_stretching_factor, problem)
self.replace_option_in_config('pruneEveryStep', prune_every_step,
problem, True)
self.replace_option_in_config('continuous_collision_check',
continuous_collision, problem, True)
self.replace_option_in_config('check_linear_path', check_linear_path,
problem, True)
self.replace_option_in_config('enforce_constraints',
enforce_constraints, problem, True)
self.replace_option_in_config('planner', planner, problem)
self.replace_option_in_config('dynamicProblem', dynamic_problem,
problem, True)
self.replace_option_in_config('showViewer', show_viewer, problem, True)
self.replace_option_in_config("simulation_step_size",
simulation_step_size, problem)
self.replace_option_in_config('planning_velocity', planning_velocity,
problem)
self.replace_option_in_config('maxObservationDistance',
max_observation_distance, problem)
self.replace_option_in_config('particleReplenishTimeout',
particle_replenish_timeout, problem)
self.replace_option_in_config('stateSamplingStrategy',
state_sampling_strategy, problem)
self.replace_option_in_config('numSamplesWeightedLazy',
num_samples_weighted_lazy, problem)
self.replace_option_in_config('gravity', gravity, problem)
self.replace_option_in_config('particlePlotLimit', particle_plot_limit,
problem)
self.replace_option_in_config('numEffectiveParticles',
num_effective_particles, problem)
self.replace_option_in_config('savePolicy', save_policy, problem, True)
self.replace_option_in_config('loadInitialPolicy', load_initial_policy,
problem, True)
self.replace_option_in_config("particleFilter", particle_filter,
problem)
self.replace_option_in_config("num_input_steps", num_actions_per_joint,
problem)
self.replace_option_in_config(
"environment_path", self.abs_path + "/../problems/" + problem +
"/environment/" + environment_file, problem)
self.replace_option_in_config(
"robot_path",
self.abs_path + "/../problems/" + problem + "/model/" + robot_file,
problem)
"""
Now we do the simulation for different covariance values
"""
min_process_covariance = self.config['min_process_covariance']
max_process_covariance = self.config['max_process_covariance']
min_observation_covariance = self.config['min_observation_covariance']
max_observation_covariance = self.config['max_observation_covariance']
inc_covariance = self.config['inc_covariance']
self.replace_option_in_config('inc_covariance', inc_covariance,
problem)
if inc_covariance == 'process':
m_cov = np.linspace(min_process_covariance, max_process_covariance,
self.config['covariance_steps'])
elif inc_covariance == 'observation':
m_cov = np.linspace(min_observation_covariance,
max_observation_covariance,
self.config['covariance_steps'])
for cov in m_cov:
if inc_covariance == 'process':
self.replace_option_in_config('process_covariance', cov,
problem)
self.replace_option_in_config('observation_covariance',
min_observation_covariance,
problem)
elif inc_covariance == "observation":
self.replace_option_in_config('process_covariance',
min_process_covariance, problem)
self.replace_option_in_config('observation_covariance', cov,
problem)
self.replace_option_in_config('minParticleCount', particle_count,
problem)
if load_initial_policy:
"""
Construct an initial policy first by running ./solve
"""
self.replace_option_in_config('stepTimeout',
initial_planning_time, problem)
args = self.abs_path + "/../problems/" + problem + "/solve"
popen = subprocess.Popen(args)
popen.wait()
#cmd= "mv " + self.abs_path + "/pol.pol " + self.abs_path + "../problems/" + problem + "/pol.pol"
#os.system(cmd)
self.replace_option_in_config('stepTimeout', planning_time,
problem)
"""
Run the simulation and collect the final state
"""
logging.info("Run simulations with process covariance " + str(cov))
args = self.abs_path + "/../problems/" + problem + "/simulate"
popen = subprocess.Popen(args, stdout=1)
popen.wait()
self.write_mean_num_collisions(log_file_path, num_runs)
self.write_rewards(log_file_path)
cmd = "mv " + log_file_path + " " + self.abs_path + "/stats/log_" + str(
cov) + ".log"
stats_file_path = self.abs_path + "/stats/log_" + str(cov) + ".log"
os.system(cmd)
if (os.path.exists(initial_policy_file_path)):
cmd = "mv " + initial_policy_file_path + " " + self.abs_path + "/stats/init_pol_" + str(
cov) + ".pol"
#cmd = "mv " + self.abs_path + "/log.log " + self.abs_path + "/../problems/" + problem + "/log.log"
#cmd = "mv log.log ../problems/" + problem + "/log.log"
#os.system(cmd)
avg_path_lengths = [
self.read_avg_path_length(stats_file_path, num_runs)
]
self.serializer.save_list_data(avg_path_lengths,
path=self.abs_path + "/stats",
filename="avg_path_lengths_" + alg +
".yaml")
self.serializer.append_end_sign("avg_path_lengths_" + alg +
".yaml",
path=self.abs_path + "/stats")
logging.info("reading mean number of steps per run")
mean_num_steps_per_run = [
self.read_float_data(stats_file_path, "Mean number of steps")
]
self.serializer.save_list_data(mean_num_steps_per_run,
path=self.abs_path + "/stats",
filename="mean_num_steps_per_run_" +
alg + ".yaml")
self.serializer.append_end_sign("mean_num_steps_per_run_" + alg +
".yaml",
path=self.abs_path + "/stats")
logging.info("reading mean planning times per step...")
mean_times_per_step = [
self.read_float_data(stats_file_path, "Mean time per step")
]
self.serializer.save_list_data(
mean_times_per_step,
path=self.abs_path + "/stats",
filename="mean_planning_times_per_step_" + alg + ".yaml")
self.serializer.append_end_sign("mean_planning_times_per_step_" +
alg + ".yaml",
path=self.abs_path + "/stats")
#mean_times_per_step.append(self.read_float_data(problem, "Mean time per step"))
logging.info("reading mean planning times per run...")
mean_times_per_run = [
self.read_float_data(stats_file_path, "Mean time per run")
]
self.serializer.save_list_data(
mean_times_per_run,
path=self.abs_path + "/stats",
filename="mean_planning_times_per_run_" + alg + ".yaml")
self.serializer.append_end_sign("mean_planning_times_per_run_" +
alg + ".yaml",
path=self.abs_path + "/stats")
logging.info("reading mean number of sampled histories")
mean_num_histories_per_step = [
self.read_float_data(stats_file_path,
"Mean number of histories per step")
]
self.serializer.save_list_data(
mean_num_histories_per_step,
path=self.abs_path + "/stats",
filename="mean_num_generated_paths_per_step_" + alg + ".yaml")
self.serializer.append_end_sign(
"mean_num_generated_paths_per_step_" + alg + ".yaml",
path=self.abs_path + "/stats")
mean_num_histories_per_run = [
self.read_float_data(stats_file_path,
"Mean number of histories per run")
]
self.serializer.save_list_data(
mean_num_histories_per_run,
path=self.abs_path + "/stats",
filename="mean_num_generated_paths_per_run_" + alg + ".yaml")
self.serializer.append_end_sign(
"mean_num_generated_paths_per_run_" + alg + ".yaml",
path=self.abs_path + "/stats")
logging.info("reading number of successful runs")
num_successful_runs = [
self.read_float_data(stats_file_path,
"Percentage of successful runs")
]
self.serializer.save_list_data(num_successful_runs,
path=self.abs_path + "/stats",
filename="num_successes_" + alg +
".yaml")
self.serializer.append_end_sign("num_successes_" + alg + ".yaml",
path=self.abs_path + "/stats")
"""
Clear the nohup log
"""
try:
cmd = "cat /dev/null > " + self.abs_path + "/nohup.out"
#cmd = "cat /dev/null > nohup.out"
os.system(cmd)
except:
pass
stats = dict(m_cov=m_cov.tolist())
logging.info("Save stats...")
self.serializer.save_stats(stats, path=self.abs_path + "/stats")
logging.info("Save end effector paths...")
cmd = "cp " + self.abs_path + "/" + self.config_file + " " + self.abs_path + "/stats/"
#cmd = "cp " + self.abs_path + "/" + self.config_file + " stats/"
os.system(cmd)
try:
os.makedirs(self.abs_path + "/stats/environment")
except OSError:
if not os.path.isdir(self.abs_path + "/stats/environment"):
raise
try:
os.makedirs(self.abs_path + "/stats/model")
except OSError:
if not os.path.isdir(self.abs_path + "/stats/model"):
raise
cmd = "cp " + self.abs_path + "/../problems/" + str(
problem
) + "/environment/* " + self.abs_path + "/stats/environment/"
#cmd = "cp ../problems/" + str(problem) + "/environment/* stats/environment"
os.system(cmd)
cmd = "cp " + self.abs_path + "/../problems/" + str(
problem) + "/model/* " + self.abs_path + "/stats/model/"
#cmd = "cp ../problems/" + str(problem) + "/model/* stats/model"
os.system(cmd)
print "Done"
