def get_planner(planner_params: Dict, verbose: int):
# TODO: remove when backwards compatibility no longer needed
if 'planner_type' not in planner_params:
planner_type = 'rrt'
else:
planner_type = planner_params['planner_type']
scenario = get_scenario(planner_params["scenario"])
if planner_type == 'rrt':
from link_bot_classifiers import classifier_utils
fwd_model = load_fwd_model(planner_params)
filter_model = filter_utils.load_filter(
paths_from_json(planner_params['filter_model_dir']), scenario)
classifier_model_dir = paths_from_json(
planner_params['classifier_model_dir'])
classifier_model = classifier_utils.load_generic_model(
classifier_model_dir, scenario=scenario)
action_params_with_defaults = fwd_model.data_collection_params
action_params_with_defaults.update(planner_params['action_params'])
planner = RRT(fwd_model=fwd_model,
filter_model=filter_model,
classifier_model=classifier_model,
planner_params=planner_params,
action_params=action_params_with_defaults,
scenario=scenario,
verbose=verbose)
elif planner_type == 'shooting':
fwd_model = load_fwd_model(planner_params)
filter_model = filter_utils.load_filter(
paths_from_json(planner_params['filter_model_dir']))
from link_bot_planning.shooting_method import ShootingMethod
action_params_with_defaults = fwd_model.data_collection_params
action_params_with_defaults.update(planner_params['action_params'])
planner = ShootingMethod(fwd_model=fwd_model,
classifier_model=None,
scenario=scenario,
params={'n_samples': 1000},
filter_model=filter_model,
action_params=action_params_with_defaults)
else:
raise NotImplementedError(
f"planner type {planner_type} not implemented")
return planner
def make_recovery_dataset(self, log: Dict, seed: int):
dynamics_dataset_2 = pathlib.Path(
log['collect_dynamics_data_2']['dynamics_dataset_dir'])
udnn_model_dirs = paths_from_json(log['learn_dynamics']['model_dirs'])
classifier_model_dir = pathlib.Path(
log['learn_classifier']['model_dir'])
make_recovery_dataset_params = self.full_stack_params[
'make_recovery_dataset']
labeling_params_filename = pathlib.Path(
make_recovery_dataset_params['labeling_params'])
with labeling_params_filename.open("r") as labeling_params_file:
labeling_params = hjson.load(labeling_params_file)
labeling_params['threshold'] = log['learn_dynamics'][
'classifier_threshold']
recovery_data_dir = pathlib.Path('recovery_data')
recovery_data_dir.mkdir(exist_ok=True)
outdir = pathlib.Path('recovery_data') / self.unique_nickname
outdir.mkdir(parents=False, exist_ok=True)
batch_size = make_recovery_dataset_params['batch_size']
recovery_dataset_dir = make_recovery_dataset_from_params_dict(
dataset_dir=dynamics_dataset_2,
fwd_model_dir=udnn_model_dirs,
classifier_model_dir=classifier_model_dir,
batch_size=batch_size,
use_gt_rope=self.use_gt_rope,
labeling_params=labeling_params,
outdir=outdir)
rospy.loginfo(Fore.GREEN + recovery_dataset_dir.as_posix())
return {
'recovery_dataset_dir': recovery_dataset_dir,
}
def load_sort_order(outdir: pathlib.Path, unsorted_dirs: List[pathlib.Path]):
sort_order_filename = outdir / 'sort_order.txt'
if sort_order_filename.exists():
with sort_order_filename.open("r") as sort_order_file:
subfolders_ordered = hjson.load(sort_order_file)
subfolders_ordered = paths_from_json(subfolders_ordered)
return subfolders_ordered
return unsorted_dirs
def __init__(self, log: Dict, threshold: float):
self.log = log
self.threshold = threshold
self.phase2_dataset_dir = pathlib.Path(self.log['phase_2_datset_dir'])
self.udnn_model_dirs = paths_from_json(self.log['udnn_model_dirs'])
subdir_name = 'scirob_dragging_test_threshold_sensitivity'
self.classifier_dataset_dir = pathlib.Path(
log['classifier_dataset_dir'])
self.trials_directory = pathlib.Path('trials') / subdir_name
def load_dataset_and_models(args):
comparison_info = json.load(args.comparison.open("r"))
models = {}
for name, model_info in comparison_info.items():
model_dir = paths_from_json(model_info['model_dir'])
model, _ = dynamics_utils.load_generic_model(model_dir)
models[name] = model
dataset = DynamicsDatasetLoader(args.dataset_dirs)
tf_dataset = dataset.get_datasets(mode=args.mode,
shard=args.shard,
take=args.take)
tf_dataset = batch_tf_dataset(tf_dataset, 1)
return tf_dataset, dataset, models
def labeling_params_from_planner_params(planner_params,
fallback_labeling_params: Dict):
classifier_model_dirs = paths_from_json(
planner_params['classifier_model_dir'])
representative_classifier_model_dir = classifier_model_dirs[0]
classifier_hparams_filename = representative_classifier_model_dir.parent / 'params.json'
classifier_hparams = json.load(classifier_hparams_filename.open('r'))
if 'labeling_params' in classifier_hparams:
labeling_params = classifier_hparams['labeling_params']
elif 'classifier_dataset_hparams' in classifier_hparams:
labeling_params = classifier_hparams['classifier_dataset_hparams'][
'labeling_params']
else:
labeling_params = fallback_labeling_params
return labeling_params
def planning_evaluation(self, log: Dict, seed: int):
classifier_model_dir = pathlib.Path(
log['learn_classifier']['model_dir'])
udnn_model_dirs = paths_from_json(log['learn_dynamics']['model_dirs'])
# full_dynamics_model_dirs = paths_from_json(log['learn_full_dynamics']['model_dirs'])
full_dynamics_model_dirs = []
recovery_model_dir = pathlib.Path(log['learn_recovery']['model_dir'])
planning_module_path = pathlib.Path(r.get_path('link_bot_planning'))
planning_evaluation_params = self.full_stack_params[
"planning_evaluation"]
if "test_scenes_dir" in planning_evaluation_params:
test_scenes_dir = pathlib.Path(
planning_evaluation_params["test_scenes_dir"])
else:
test_scenes_dir = None
n_trials = planning_evaluation_params['n_trials']
trials = list(range(n_trials))
planners_params_common_filename = pathlib.Path(
planning_evaluation_params['planners_params_common'])
planners_params = self.make_planners_params(
classifier_model_dir, full_dynamics_model_dirs, udnn_model_dirs,
planners_params_common_filename, planning_evaluation_params,
recovery_model_dir)
if self.launch:
self.service_provider.launch(
planning_evaluation_params,
gui=self.gui,
world=planning_evaluation_params['world'])
root = data_directory(planning_module_path / 'results' / self.nickname)
outdir = planning_evaluation(outdir=root,
planners_params=planners_params,
trials=trials,
test_scenes_dir=test_scenes_dir,
verbose=self.verbose,
logfile_name=None,
skip_on_exception=False)
if self.launch:
self.service_provider.kill()
rospy.loginfo(Fore.GREEN + outdir.as_posix())
return outdir
def load_fwd_model(planner_params):
fwd_model_dirs = paths_from_json(planner_params['fwd_model_dir'])
fwd_model, _ = dynamics_utils.load_generic_model(fwd_model_dirs)
return fwd_model