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 main():
colorama.init(autoreset=True)
parser = argparse.ArgumentParser()
parser.add_argument("results_dir",
type=pathlib.Path,
help='dir containing *_metrics.json.gz')
parser.add_argument("plan_idx", type=int)
args = parser.parse_args()
rospy.init_node("postprocess_result")
with (args.results_dir / 'metadata.json').open('r') as metadata_file:
metadata_str = metadata_file.read()
metadata = json.loads(metadata_str)
planner_params = metadata['planner_params']
fwd_model_dirs = [pathlib.Path(p) for p in planner_params['fwd_model_dir']]
fwd_model, _ = dynamics_utils.load_generic_model(fwd_model_dirs)
scenario = fwd_model.scenario
classifier_model_dir = pathlib.Path(planner_params['classifier_model_dir'])
classifier = classifier_utils.load_generic_model(classifier_model_dir,
scenario=scenario)
metrics_filename = args.results_dir / f"{args.plan_idx}_metrics.json.gz"
with gzip.open(metrics_filename, 'rb') as f:
data_str = f.read()
datum = json.loads(data_str.decode("utf-8"))
steps = datum['steps']
goal = datum['goal']
first_step = steps[0]
environment = numpify(first_step['planning_query']['environment'])
all_output_actions = []
for step in steps:
if step['type'] == 'executed_plan':
actions = postprocess_step(scenario, fwd_model, classifier,
environment, step, goal, planner_params)
all_output_actions.extend(actions)
elif step['type'] == 'executed_recovery':
actions = [step['recovery_action']]
all_output_actions.extend(actions)
# viz the whole thing
start_state = steps[0]['planning_result']['path'][0]
final_states = fwd_model.propagate(environment, start_state, actions)
T = len(final_states)
for t, s_t in enumerate(final_states):
scenario.plot_state_rviz(s_t,
idx=t,
label='smoothed',
color='#00ff0099')
if t < T - 1:
scenario.plot_action_rviz(s_t,
actions[t],
idx=t,
color="#ffffff99",
label='smoothed')
sleep(0.02)
# Save the output actions
outfilename = args.results_dir / f"{args.plan_idx}_smoothed.json"
with outfilename.open("w") as outfile:
json.dump(listify(all_output_actions), outfile)
print(Fore.GREEN + f"Wrote {outfilename}" + Fore.RESET)
def viz_ensemble_main(dataset_dir: pathlib.Path,
checkpoints: List[pathlib.Path], mode: str,
batch_size: int, only_errors: bool, use_gt_rope: bool,
**kwargs):
dynamics_stdev_pub_ = rospy.Publisher("dynamics_stdev",
Float32,
queue_size=10)
classifier_stdev_pub_ = rospy.Publisher("classifier_stdev",
Float32,
queue_size=10)
accept_probability_pub_ = rospy.Publisher("accept_probability_viz",
Float32,
queue_size=10)
traj_idx_pub_ = rospy.Publisher("traj_idx_viz", Float32, queue_size=10)
###############
# Model
###############
model = load_generic_model(checkpoints)
###############
# Dataset
###############
test_dataset = ClassifierDatasetLoader([dataset_dir],
load_true_states=True,
use_gt_rope=use_gt_rope)
test_tf_dataset = test_dataset.get_datasets(mode=mode)
test_tf_dataset = batch_tf_dataset(test_tf_dataset,
batch_size,
drop_remainder=True)
scenario = test_dataset.scenario
###############
# Evaluate
###############
# Iterate over test set
all_accuracies_over_time = []
all_stdevs = []
all_labels = []
classifier_ensemble_stdevs = []
for batch_idx, test_batch in enumerate(test_tf_dataset):
test_batch.update(test_dataset.batch_metadata)
mean_predictions, stdev_predictions = model.check_constraint_from_example(
test_batch)
mean_probabilities = mean_predictions['probabilities']
stdev_probabilities = stdev_predictions['probabilities']
labels = tf.expand_dims(test_batch['is_close'][:, 1:], axis=2)
all_labels = tf.concat(
(all_labels, tf.reshape(test_batch['is_close'][:, 1:], [-1])),
axis=0)
all_stdevs = tf.concat(
(all_stdevs, tf.reshape(test_batch[add_predicted('stdev')], [-1])),
axis=0)
accuracy_over_time = tf.keras.metrics.binary_accuracy(
y_true=labels, y_pred=mean_probabilities)
all_accuracies_over_time.append(accuracy_over_time)
# Visualization
test_batch.pop("time")
test_batch.pop("batch_size")
decisions = mean_probabilities > 0.5
classifier_is_correct = tf.squeeze(tf.equal(decisions,
tf.cast(labels, tf.bool)),
axis=-1)
for b in range(batch_size):
example = index_dict_of_batched_tensors_tf(test_batch, b)
classifier_ensemble_stdev = stdev_probabilities[b].numpy().squeeze(
)
classifier_ensemble_stdevs.append(classifier_ensemble_stdev)
# if the classifier is correct at all time steps, ignore
if only_errors and tf.reduce_all(classifier_is_correct[b]):
continue
# if only_collision
predicted_rope_states = tf.reshape(
example[add_predicted('link_bot')][1], [-1, 3])
xs = predicted_rope_states[:, 0]
ys = predicted_rope_states[:, 1]
zs = predicted_rope_states[:, 2]
in_collision = bool(
batch_in_collision_tf_3d(environment=example,
xs=xs,
ys=ys,
zs=zs,
inflate_radius_m=0)[0].numpy())
label = bool(example['is_close'][1].numpy())
accept = decisions[b, 0, 0].numpy()
# if not (in_collision and accept):
# continue
scenario.plot_environment_rviz(example)
stdev_probabilities[b].numpy().squeeze()
classifier_stdev_msg = Float32()
classifier_stdev_msg.data = stdev_probabilities[b].numpy().squeeze(
)
classifier_stdev_pub_.publish(classifier_stdev_msg)
actual_0 = scenario.index_state_time(example, 0)
actual_1 = scenario.index_state_time(example, 1)
pred_0 = scenario.index_predicted_state_time(example, 0)
pred_1 = scenario.index_predicted_state_time(example, 1)
action = scenario.index_action_time(example, 0)
label = example['is_close'][1]
scenario.plot_state_rviz(actual_0,
label='actual',
color='#FF0000AA',
idx=0)
scenario.plot_state_rviz(actual_1,
label='actual',
color='#E00016AA',
idx=1)
scenario.plot_state_rviz(pred_0,
label='predicted',
color='#0000FFAA',
idx=0)
scenario.plot_state_rviz(pred_1,
label='predicted',
color='#0553FAAA',
idx=1)
scenario.plot_action_rviz(pred_0, action)
scenario.plot_is_close(label)
dynamics_stdev_t = example[add_predicted('stdev')][1, 0].numpy()
dynamics_stdev_msg = Float32()
dynamics_stdev_msg.data = dynamics_stdev_t
dynamics_stdev_pub_.publish(dynamics_stdev_msg)
accept_probability_t = mean_probabilities[b, 0, 0].numpy()
accept_probability_msg = Float32()
accept_probability_msg.data = accept_probability_t
accept_probability_pub_.publish(accept_probability_msg)
traj_idx_msg = Float32()
traj_idx_msg.data = batch_idx * batch_size + b
traj_idx_pub_.publish(traj_idx_msg)
# stepper = RvizSimpleStepper()
# stepper.step()
print(np.mean(classifier_ensemble_stdevs))
all_accuracies_over_time = tf.concat(all_accuracies_over_time, axis=0)
mean_accuracies_over_time = tf.reduce_mean(all_accuracies_over_time,
axis=0)
std_accuracies_over_time = tf.math.reduce_std(all_accuracies_over_time,
axis=0)
mean_classifier_ensemble_stdev = tf.reduce_mean(classifier_ensemble_stdevs)
print(mean_classifier_ensemble_stdev)
def viz_main(dataset_dirs: List[pathlib.Path],
checkpoint: pathlib.Path,
mode: str,
batch_size: int,
only_errors: bool,
use_gt_rope: bool,
old_compat: bool = False,
**kwargs):
stdev_pub_ = rospy.Publisher("stdev", Float32, queue_size=10)
traj_idx_pub_ = rospy.Publisher("traj_idx_viz", Float32, queue_size=10)
###############
# Model
###############
trials_directory = pathlib.Path('trials').absolute()
trial_path = checkpoint.parent.absolute()
_, params = filepath_tools.create_or_load_trial(
trial_path=trial_path, trials_directory=trials_directory)
model_class = link_bot_classifiers.get_model(params['model_class'])
###############
# Dataset
###############
dataset = ClassifierDatasetLoader(
dataset_dirs,
load_true_states=True,
use_gt_rope=use_gt_rope,
threshold=params['classifier_dataset_hparams']['labeling_params']
['threshold'],
old_compat=old_compat)
model = model_class(hparams=params,
batch_size=batch_size,
scenario=dataset.scenario)
tf_dataset = dataset.get_datasets(mode=mode)
scenario = dataset.scenario
###############
# Evaluate
###############
tf_dataset = batch_tf_dataset(tf_dataset, batch_size, drop_remainder=True)
model = classifier_utils.load_generic_model([checkpoint])
for batch_idx, example in enumerate(
progressbar(tf_dataset, widgets=base_dataset.widgets)):
example.update(dataset.batch_metadata)
predictions, _ = model.check_constraint_from_example(example,
training=False)
labels = tf.expand_dims(example['is_close'][:, 1:], axis=2)
probabilities = predictions['probabilities']
# Visualization
example.pop("time")
example.pop("batch_size")
decisions = probabilities > 0.5
classifier_is_correct = tf.squeeze(tf.equal(decisions,
tf.cast(labels, tf.bool)),
axis=-1)
for b in range(batch_size):
example_b = index_dict_of_batched_tensors_tf(example, b)
# if the classifier is correct at all time steps, ignore
if only_errors and tf.reduce_all(classifier_is_correct[b]):
continue
def _custom_viz_t(scenario: Base3DScenario, e: Dict, t: int):
if t > 0:
accept_probability_t = predictions['probabilities'][
b, t - 1, 0].numpy()
else:
accept_probability_t = -999
scenario.plot_accept_probability(accept_probability_t)
traj_idx_msg = Float32()
traj_idx_msg.data = batch_idx * batch_size + b
traj_idx_pub_.publish(traj_idx_msg)
anim = RvizAnimation(scenario=scenario,
n_time_steps=dataset.horizon,
init_funcs=[
init_viz_env,
dataset.init_viz_action(),
],
t_funcs=[
_custom_viz_t,
dataset.classifier_transition_viz_t(),
ExperimentScenario.plot_stdev_t,
])
with open("debugging.hjson", 'w') as f:
my_hdump(numpify(example_b), f)
anim.play(example_b)
def make_recovery_dataset_from_params_dict(dataset_dir: pathlib.Path,
fwd_model_dir,
classifier_model_dir: pathlib.Path,
labeling_params: Dict,
outdir: pathlib.Path,
batch_size: int,
use_gt_rope: bool,
start_at: Optional = None,
stop_at: Optional = None):
# append "best_checkpoint" before loading
classifier_model_dir = classifier_model_dir / 'best_checkpoint'
if not isinstance(fwd_model_dir, List):
fwd_model_dir = [fwd_model_dir]
fwd_model_dir = [p / 'best_checkpoint' for p in fwd_model_dir]
np.random.seed(0)
tf.random.set_seed(0)
dynamics_hparams = hjson.load((dataset_dir / 'hparams.hjson').open('r'))
fwd_model, _ = dynamics_utils.load_generic_model(fwd_model_dir)
dataset = DynamicsDatasetLoader([dataset_dir], use_gt_rope=use_gt_rope)
outdir.mkdir(exist_ok=True)
print(Fore.GREEN + f"Making recovery dataset {outdir.as_posix()}")
new_hparams_filename = outdir / 'hparams.hjson'
recovery_dataset_hparams = dynamics_hparams
scenario = fwd_model.scenario
if not isinstance(classifier_model_dir, List):
classifier_model_dir = [classifier_model_dir]
classifier_model = classifier_utils.load_generic_model(
classifier_model_dir, scenario)
recovery_dataset_hparams['dataset_dir'] = dataset_dir
recovery_dataset_hparams['fwd_model_dir'] = fwd_model_dir
recovery_dataset_hparams['classifier_model'] = classifier_model_dir
recovery_dataset_hparams['fwd_model_hparams'] = fwd_model.hparams
recovery_dataset_hparams['labeling_params'] = labeling_params
recovery_dataset_hparams['state_keys'] = fwd_model.state_keys
recovery_dataset_hparams['action_keys'] = fwd_model.action_keys
recovery_dataset_hparams['start-at'] = start_at
recovery_dataset_hparams['stop-at'] = stop_at
my_hdump(recovery_dataset_hparams,
new_hparams_filename.open("w"),
indent=2)
outdir.mkdir(parents=True, exist_ok=True)
start_at = progress_point(start_at)
stop_at = progress_point(stop_at)
modes = ['train', 'val', 'test']
for mode in modes:
if start_at is not None and modes.index(mode) < modes.index(
start_at[0]):
continue
if stop_at is not None and modes.index(mode) > modes.index(stop_at[0]):
continue
tf_dataset_for_mode = dataset.get_datasets(mode=mode)
full_output_directory = outdir / mode
full_output_directory.mkdir(parents=True, exist_ok=True)
# figure out that record_idx to start at
record_idx = count_up_to_next_record_idx(full_output_directory)
# FIXME: start_at is not implemented correctly in the sense that it shouldn't be the same
# across train/val/test
for out_example in generate_recovery_examples(
tf_dataset=tf_dataset_for_mode,
modes=modes,
mode=mode,
fwd_model=fwd_model,
classifier_model=classifier_model,
dataset=dataset,
labeling_params=labeling_params,
batch_size=batch_size,
start_at=start_at,
stop_at=stop_at):
# FIXME: is there an extra time/batch dimension?
for batch_idx in range(out_example['traj_idx'].shape[0]):
out_example_b = index_dict_of_batched_tensors_tf(
out_example, batch_idx)
# # BEGIN DEBUG
# from link_bot_data.visualization import init_viz_env, recovery_transition_viz_t, init_viz_action
# from copy import deepcopy
#
# viz_out_example_b = deepcopy(out_example_b)
# recovery_probability = compute_recovery_probabilities(viz_out_example_b['accept_probabilities'],
# labeling_params['n_action_samples'])
# viz_out_example_b['recovery_probability'] = recovery_probability
# anim = RvizAnimation(scenario=scenario,
# n_time_steps=labeling_params['action_sequence_horizon'],
# init_funcs=[init_viz_env,
# init_viz_action(dataset.scenario_metadata, fwd_model.action_keys,
# fwd_model.state_keys),
# ],
# t_funcs=[init_viz_env,
# recovery_transition_viz_t(dataset.scenario_metadata,
# fwd_model.state_keys),
# lambda s, e, t: scenario.plot_recovery_probability_t(e, t),
# ])
# anim.play(viz_out_example_b)
# # END DEBUG
tf_write_example(full_output_directory, out_example_b,
record_idx)
record_idx += 1
return outdir
def test_classifier(classifier_model_dir: pathlib.Path,
fwd_model_dir: List[pathlib.Path],
n_actions: int,
saved_state: Optional[pathlib.Path],
generate_actions: Callable):
fwd_model, _ = dynamics_utils.load_generic_model([pathlib.Path(p) for p in fwd_model_dir])
classifier: NNClassifierWrapper = classifier_utils.load_generic_model([classifier_model_dir])
service_provider = GazeboServices()
service_provider.setup_env(verbose=0,
real_time_rate=0,
max_step_size=fwd_model.data_collection_params['max_step_size'],
play=False)
if saved_state:
service_provider.restore_from_bag(saved_state)
scenario = fwd_model.scenario
scenario.on_before_get_state_or_execute_action()
# NOTE: perhaps it would make sense to have a "fwd_model" have API for get_env, get_state, sample_action, etc
# because the fwd_model knows it's scenario, and importantly it also knows it's data_collection_params
# which is what we're using here to pass to the scenario methods
params = fwd_model.data_collection_params
environment = numpify(scenario.get_environment(params))
start_state = numpify(scenario.get_state())
start_state_tiled = repeat(start_state, n_actions, axis=0, new_axis=True)
start_states_tiled = add_time_dim(start_state_tiled)
actions = generate_actions(environment, start_state_tiled, scenario, params, n_actions)
environment_tiled = repeat(environment, n_actions, axis=0, new_axis=True)
actions_dict = sequence_of_dicts_to_dict_of_tensors(actions)
actions_dict = add_time_dim(actions_dict)
predictions, _ = fwd_model.propagate_differentiable_batched(environment=environment_tiled,
state=start_states_tiled,
actions=actions_dict)
# Run classifier
state_sequence_length = 2
accept_probabilities, _ = classifier.check_constraint_batched_tf(environment=environment_tiled,
predictions=predictions,
actions=actions_dict,
state_sequence_length=state_sequence_length,
batch_size=n_actions)
# animate over the sampled actions
anim = RvizAnimation(scenario=scenario,
n_time_steps=n_actions,
init_funcs=[init_viz_env],
t_funcs=[
lambda s, e, t: init_viz_env(s, e),
viz_transition_for_model_t({}, fwd_model),
ExperimentScenario.plot_accept_probability_t,
],
)
example = {
'accept_probability': tf.squeeze(accept_probabilities, axis=1),
}
example.update(environment)
example.update(predictions)
example.update(actions_dict)
anim.play(example)