def __init__(self, dataset_dirs: List[pathlib.Path]):
super(RecoveryDatasetLoader, self).__init__(dataset_dirs)
self.sorted = sorted
self.scenario = get_scenario(self.hparams['scenario'])
self.state_keys = self.hparams['state_keys']
self.action_keys = self.hparams['action_keys']
self.feature_names = [
'env', 'origin', 'extent', 'res', 'traj_idx', 'start_t', 'end_t',
'accept_probabilities'
]
for k in self.hparams['state_keys']:
self.feature_names.append(k)
self.horizon = self.hparams["labeling_params"][
"action_sequence_horizon"]
self.n_action_samples = self.hparams["labeling_params"][
"n_action_samples"]
for k in self.state_keys:
self.feature_names.append(k)
for k in self.action_keys:
self.feature_names.append(k)
self.batch_metadata = {'time': 2}
def main():
colorama.init(autoreset=True)
np.set_printoptions(linewidth=250, precision=3, suppress=True)
parser = argparse.ArgumentParser(formatter_class=my_formatter)
parser.add_argument("results_dir",
type=pathlib.Path,
help='directory containing metrics.json')
parser.add_argument("trial_idx",
type=int_range_arg,
help='which plan to show')
parser.add_argument("--save", action='store_true')
parser.add_argument("--filter-by-status", type=str, nargs="+")
parser.add_argument("--show-tree", action="store_true")
rospy.init_node("plot_results")
args = parser.parse_args()
with (args.results_dir / 'metadata.json').open('r') as metadata_file:
metadata_str = metadata_file.read()
metadata = json.loads(metadata_str)
scenario = get_scenario(metadata['scenario'])
for trial_idx in args.trial_idx:
with gzip.open(args.results_dir / f'{trial_idx}_metrics.json.gz',
'rb') as metrics_file:
metrics_str = metrics_file.read()
datum = json.loads(metrics_str.decode("utf-8"))
if if_filter_with_status(datum, args.filter_by_status):
plot_steps(args, scenario, datum, metadata, {'threshold': 0.065})
print(
f"Trial {trial_idx} complete with status {datum['trial_status']}")
def __init__(
self,
dataset_dirs: List[pathlib.Path],
use_gt_rope: bool,
load_true_states=False,
no_balance=True,
threshold: Optional[float] = None,
old_compat: Optional[bool] = False,
):
super(ClassifierDatasetLoader, self).__init__(dataset_dirs)
self.no_balance = no_balance
self.load_true_states = load_true_states
self.use_gt_rope = use_gt_rope
self.labeling_params = self.hparams['labeling_params']
self.threshold = threshold if threshold is not None else self.labeling_params[
'threshold']
rospy.loginfo(f"classifier using threshold {self.threshold}")
self.horizon = self.hparams['labeling_params']['classifier_horizon']
self.scenario = get_scenario(self.hparams['scenario'])
self.true_state_keys = self.hparams['true_state_keys']
self.old_compat = old_compat
if self.old_compat:
self.true_state_keys.append('is_close')
else:
self.true_state_keys.append('error')
self.predicted_state_keys = [
add_predicted(k) for k in self.hparams['predicted_state_keys']
]
self.predicted_state_keys.append(add_predicted('stdev'))
self.action_keys = self.hparams['action_keys']
self.feature_names = [
'classifier_start_t',
'classifier_end_t',
'env',
'extent',
'origin',
'res',
'traj_idx',
'prediction_start_t',
]
self.batch_metadata = {'time': self.horizon}
if self.load_true_states:
for k in self.true_state_keys:
self.feature_names.append(k)
for k in self.predicted_state_keys:
self.feature_names.append(k)
for k in self.action_keys:
self.feature_names.append(k)
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(formatter_class=my_formatter)
parser.add_argument("results_dir",
type=pathlib.Path,
help='directory containing metrics.json')
parser.add_argument("--trial-idx",
type=int,
help='which plan to show',
default=0)
rospy.init_node("play_dragging_plan")
action_srv = rospy.ServiceProxy("execute_dual_gripper_action",
DualGripperTrajectory)
args = parser.parse_args()
with (args.results_dir / 'metadata.json').open('r') as metadata_file:
metadata_str = metadata_file.read()
metadata = json.loads(metadata_str)
scenario = get_scenario(metadata['scenario'])
with gzip.open(args.results_dir / f'{args.trial_idx}_metrics.json.gz',
'rb') as metrics_file:
metrics_str = metrics_file.read()
datum = json.loads(metrics_str.decode("utf-8"))
all_actions = []
steps = datum['steps']
for step_idx, step in enumerate(steps):
if step['type'] == 'executed_plan':
actions = step['planning_result']['actions']
elif step['type'] == 'executed_recovery':
actions = [step['recovery_action']]
all_actions.extend(actions)
for action in all_actions:
target_gripper1_point = ros_numpy.msgify(
Point, np.array(action['gripper_position']))
target_gripper1_point.z = -0.39
target_gripper2_point = ros_numpy.msgify(Point,
np.array([0.45, -0.2, 0.08]))
req = DualGripperTrajectoryRequest()
req.gripper1_points.append(target_gripper1_point)
req.gripper2_points.append(target_gripper2_point)
action_srv(req)
def load_filter(model_dirs: List[pathlib.Path],
scenario: ExperimentScenario = None) -> BaseFilterFunction:
representative_model_dir = model_dirs[0]
_, common_hparams = load_trial(representative_model_dir.parent.absolute())
if scenario is None:
scenario_name = common_hparams['dynamics_dataset_hparams']['scenario']
scenario = get_scenario(scenario_name)
model_type = common_hparams['model_class']
if model_type == 'CFM':
nn = CFMFilter(model_dirs, batch_size=1, scenario=scenario)
return nn
elif model_type in ['none', 'pass-through']:
return PassThroughFilter()
else:
raise NotImplementedError("invalid model type {}".format(model_type))
def generate_test_scenes(
scenario: str,
n_trials: int,
params_filename: pathlib.Path,
save_test_scenes_dir: Optional[pathlib.Path] = None,
):
service_provider = gazebo_services.GazeboServices()
scenario = get_scenario(scenario)
service_provider.setup_env(verbose=0,
real_time_rate=0.0,
max_step_size=0.01,
play=True)
link_states_listener = Listener("gazebo/link_states", LinkStates)
env_rng = np.random.RandomState(0)
action_rng = np.random.RandomState(0)
with params_filename.open("r") as params_file:
params = hjson.load(params_file)
scenario.on_before_data_collection(params)
scenario.randomization_initialization()
for trial_idx in range(n_trials):
environment = scenario.get_environment(params)
scenario.randomize_environment(env_rng, params)
for i in range(10):
state = scenario.get_state()
action = scenario.sample_action(
action_rng=action_rng,
environment=environment,
state=state,
action_params=params,
validate=True,
)
scenario.execute_action(action)
links_states = link_states_listener.get()
save_test_scenes_dir.mkdir(exist_ok=True, parents=True)
bagfile_name = save_test_scenes_dir / f'scene_{trial_idx:04d}.bag'
rospy.loginfo(f"Saving scene to {bagfile_name}")
with rosbag.Bag(bagfile_name, 'w') as bag:
bag.write('links_states', links_states)
def load_generic_model(
model_dirs: List[pathlib.Path],
scenario: Optional[ExperimentScenario] = None
) -> BaseConstraintChecker:
# FIXME: remove batch_size=1 here? can I put it in base model?
# we use the first model and assume they all have the same hparams
representative_model_dir = model_dirs[0]
_, common_hparams = load_trial(representative_model_dir.parent.absolute())
if scenario is None:
scenario_name = common_hparams['scenario']
scenario = get_scenario(scenario_name)
model_type = common_hparams['model_class']
if model_type == 'rnn':
return NNClassifierWrapper(model_dirs, batch_size=1, scenario=scenario)
elif model_type == 'collision':
return CollisionCheckerClassifier(model_dirs, scenario=scenario)
elif model_type == 'none':
return NoneClassifier(model_dirs, scenario=scenario)
elif model_type == 'gripper_distance':
return GripperDistanceClassifier(model_dirs, scenario=scenario)
else:
raise NotImplementedError("invalid model type {}".format(model_type))
def eval_main(
dataset_dirs: List[pathlib.Path],
checkpoint: pathlib.Path,
mode: str,
batch_size: int,
**kwargs,
):
###############
# Model
###############
trial_path = checkpoint.parent.absolute()
trials_directory = pathlib.Path('recovery_trials').absolute()
_, params = filepath_tools.create_or_load_trial(
trial_path=trial_path, trials_directory=trials_directory)
scenario = get_scenario(params['scenario'])
net = NNRecoveryModel(hparams=params, scenario=scenario, batch_size=1)
###############
# Dataset
###############
test_dataset = RecoveryDatasetLoader(dataset_dirs)
test_tf_dataset = test_dataset.get_datasets(mode=mode)
###############
# Evaluate
###############
test_tf_dataset = batch_tf_dataset(test_tf_dataset,
batch_size,
drop_remainder=True)
runner = ModelRunner(model=net,
training=False,
params=params,
checkpoint=checkpoint,
trial_path=trial_path,
batch_metadata=test_dataset.batch_metadata)
validation_metrics = runner.val_epoch(test_tf_dataset)
for name, value in validation_metrics.items():
print(f"{name}: {value:.3f}")
def __init__(self,
scenario_name: str,
service_provider: BaseServices,
params: Dict,
seed: Optional[int] = None,
verbose: int = 0):
self.service_provider = service_provider
self.params = params
self.verbose = verbose
self.scenario_name = scenario_name
self.scenario = get_scenario(scenario_name)
if seed is None:
self.seed = np.random.randint(0, 100)
else:
self.seed = seed
print(Fore.CYAN + f"Using seed: {self.seed}" + Fore.RESET)
service_provider.setup_env(
verbose=self.verbose,
real_time_rate=self.params['real_time_rate'],
max_step_size=self.params['max_step_size'])
def generate_metrics(analysis_params, args, out_dir, subfolders_ordered):
metrics = [
FinalExecutionToGoalError(args, analysis_params, results_dir=out_dir),
# NRecoveryActions(args, analysis_params, results_dir=out_dir),
# TotalTime(args, analysis_params, results_dir=out_dir),
# NPlanningAttempts(args, analysis_params, results_dir=out_dir),
# TaskErrorBoxplot(args, analysis_params, results_dir=out_dir),
]
for subfolder in subfolders_ordered:
metrics_filenames = list(subfolder.glob("*_metrics.json.gz"))
with (subfolder / 'metadata.json').open('r') as metadata_file:
metadata = json.load(metadata_file)
method_name = metadata['planner_params'].get('method_name',
subfolder.name)
scenario = get_scenario(metadata['scenario'])
for metric in metrics:
metric.setup_method(method_name, metadata)
datums = []
for plan_idx, metrics_filename in enumerate(metrics_filenames):
if args.debug and plan_idx > 3:
break
with gzip.open(metrics_filename, 'rb') as metrics_file:
data_str = metrics_file.read()
# orjson is twice as fast, and yes it really matters here.
datum = orjson.loads(data_str.decode("utf-8"))
datums.append(datum)
# NOTE: even though this is slow, parallelizing is not easy because "scenario" cannot be pickled
for metric in metrics:
for datum in datums:
metric.aggregate_trial(method_name, scenario, datum)
for metric in metrics:
metric.convert_to_numpy_arrays()
return metrics
def load_generic_model(
model_dirs: List[pathlib.Path]
) -> Tuple[BaseDynamicsFunction, Tuple[str]]:
# FIXME: remove batch_size=1 here? can I put it in base model?
# we use the first model and assume they all have the same hparams
representative_model_dir = model_dirs[0]
_, common_hparams = load_trial(representative_model_dir.parent.absolute())
scenario_name = common_hparams['dynamics_dataset_hparams']['scenario']
scenario = get_scenario(scenario_name)
model_type = common_hparams['model_class']
if model_type == 'SimpleNN':
nn = UDNNWrapper(model_dirs, batch_size=1, scenario=scenario)
return nn, representative_model_dir.parts[1:]
elif model_type == 'ImageCondDyn':
nn = ImageCondDynamicsWrapper(model_dirs,
batch_size=1,
scenario=scenario)
return nn, representative_model_dir.parts[1:]
elif model_type == 'CFM':
nn = CFMLatentDynamics(model_dirs, batch_size=1, scenario=scenario)
return nn, representative_model_dir.parts[1:]
else:
raise NotImplementedError("invalid model type {}".format(model_type))
def __init__(self,
dataset_dirs: List[pathlib.Path],
step_size: int = 1,
use_gt_rope: Optional[bool] = False):
"""
:param dataset_dirs: dataset directories
:param step_size: the number of time steps to skip when slicing the full trajectories for training
"""
super(DynamicsDatasetLoader, self).__init__(dataset_dirs)
self.use_gt_rope = use_gt_rope
self.step_size = step_size
self.scenario = get_scenario(self.hparams['scenario'])
self.data_collection_params = self.hparams['data_collection_params']
self.state_keys = self.data_collection_params['state_keys']
self.state_keys.append('time_idx')
self.action_keys = self.data_collection_params['action_keys']
self.constant_feature_names = [
'env',
'extent',
'origin',
'res',
'traj_idx',
]
self.time_indexed_keys = self.state_keys + self.action_keys
self.int64_keys = ['time_idx']
if 'new_sequence_length' in self.hparams:
self.steps_per_traj = self.hparams['new_sequence_length']
else:
self.steps_per_traj = self.data_collection_params['steps_per_traj']
self.batch_metadata = {'sequence_length': self.steps_per_traj}
def train_main(
dataset_dirs: List[pathlib.Path],
model_hparams: pathlib.Path,
classifier_checkpoint: pathlib.Path,
log: str,
batch_size: int,
epochs: int,
seed: int,
checkpoint: Optional[pathlib.Path] = None,
ensemble_idx: Optional[int] = None,
trials_directory=pathlib.Path,
**kwargs,
):
###############
# Datasets
###############
train_dataset = RecoveryDatasetLoader(dataset_dirs)
val_dataset = RecoveryDatasetLoader(dataset_dirs)
###############
# Model
###############
model_hparams = json.load((model_hparams).open('r'))
model_hparams['recovery_dataset_hparams'] = train_dataset.hparams
model_hparams['batch_size'] = batch_size
model_hparams['seed'] = seed
model_hparams['datasets'] = paths_to_json(dataset_dirs)
model_hparams['latest_training_time'] = int(time.time())
scenario = get_scenario(model_hparams['scenario'])
# Dataset preprocessing
train_tf_dataset = train_dataset.get_datasets(mode='train')
val_tf_dataset = val_dataset.get_datasets(mode='val')
train_tf_dataset = batch_tf_dataset(train_tf_dataset,
batch_size,
drop_remainder=True)
val_tf_dataset = batch_tf_dataset(val_tf_dataset,
batch_size,
drop_remainder=True)
train_tf_dataset = train_tf_dataset.shuffle(buffer_size=512, seed=seed)
train_tf_dataset = train_tf_dataset.prefetch(tf.data.experimental.AUTOTUNE)
val_tf_dataset = val_tf_dataset.prefetch(tf.data.experimental.AUTOTUNE)
model = NNRecoveryModel(hparams=model_hparams,
scenario=scenario,
batch_size=batch_size)
############
# Initialize weights from classifier model by "restoring" from checkpoint
############
if not checkpoint:
# load in the weights for the conv & dense layers of the classifier's encoder, skip the last few layers
classifier_model = tf.train.Checkpoint(conv_layers=model.conv_layers)
classifier_root = tf.train.Checkpoint(model=classifier_model)
classifier_checkpoint_manager = tf.train.CheckpointManager(
classifier_root, classifier_checkpoint.as_posix(), max_to_keep=1)
status = classifier_root.restore(
classifier_checkpoint_manager.latest_checkpoint)
status.expect_partial()
status.assert_existing_objects_matched()
assert classifier_checkpoint_manager.latest_checkpoint is not None
print(Fore.MAGENTA + "Restored {}".format(
classifier_checkpoint_manager.latest_checkpoint) + Fore.RESET)
############
trial_path = None
checkpoint_name = None
if checkpoint:
trial_path = checkpoint.parent.absolute()
checkpoint_name = checkpoint.name
trials_directory = pathlib.Path('recovery_trials').absolute()
group_name = log if trial_path is None else None
trial_path, _ = filepath_tools.create_or_load_trial(
group_name=group_name,
params=model_hparams,
trial_path=trial_path,
trials_directory=trials_directory,
write_summary=False)
runner = ModelRunner(model=model,
training=True,
params=model_hparams,
trial_path=trial_path,
val_every_n_batches=1,
mid_epoch_val_batches=100,
validate_first=True,
checkpoint=checkpoint,
batch_metadata=train_dataset.batch_metadata)
# Train
runner.train(train_tf_dataset, val_tf_dataset, num_epochs=epochs)
return trial_path
def viz(data_filename, fps, no_plot, save):
rospy.init_node("compare_models")
# Load the results
base_folder = data_filename.parent
with gzip.open(data_filename, "rb") as data_file:
data_str = data_file.read()
saved_data = json.loads(data_str.decode("utf-8"))
all_metrics = {}
for example_idx, datum in enumerate(saved_data):
print(example_idx)
# use the first (or any) model data to get the ground truth and
dataset_element = numpify(datum.pop("dataset_element"))
environment = numpify(datum.pop("environment"))
action_keys = datum.pop("action_keys")
actions = {k: dataset_element[k] for k in action_keys}
models_viz_info = {}
n_models = len(datum)
time_steps = np.arange(datum.pop('time_steps'))
for model_name, data_for_model in datum.items():
scenario = get_scenario(data_for_model['scenario'])
# Metrics
metrics_for_model = {}
predictions = numpify(data_for_model['predictions'])
predictions.pop('stdev')
metrics = scenario.dynamics_metrics_function(dataset_element, predictions)
loss = scenario.dynamics_loss_function(dataset_element, predictions)
metrics['loss'] = loss
for metric_name, metric_value in metrics.items():
if metric_name not in metrics_for_model:
metrics_for_model[metric_name] = []
metrics_for_model[metric_name].append(metric_value.numpy())
for metric_name, metric_values in metrics_for_model.items():
mean_metric_value = float(np.mean(metric_values))
if model_name not in all_metrics:
all_metrics[model_name] = {}
if metric_name not in all_metrics[model_name]:
all_metrics[model_name][metric_name] = []
all_metrics[model_name][metric_name].append(mean_metric_value)
models_viz_info[model_name] = (scenario, predictions)
if not no_plot and not save:
# just use whatever the latest scenario was, it shouldn't matter which we use
scenario.plot_environment_rviz(remove_batch(environment))
anim = RvizAnimationController(time_steps)
while not anim.done:
t = anim.t()
actual_t = remove_batch(scenario.index_state_time(dataset_element, t))
action_t = remove_batch(scenario.index_action_time(actions, t))
scenario.plot_state_rviz(actual_t, label='actual', color='#0000ff88')
scenario.plot_action_rviz(actual_t, action_t, color='gray')
for model_idx, (model_name, viz_info) in enumerate(models_viz_info.items()):
scenario_i, predictions = viz_info
prediction_t = remove_batch(scenario_i.index_state_time(predictions, t))
color = cm.jet(model_idx / n_models)
scenario_i.plot_state_rviz(prediction_t, label=model_name, color=color)
anim.step()
metrics_by_model = {}
for model_name, metrics_for_model in all_metrics.items():
for metric_name, metric_values in metrics_for_model.items():
if metric_name not in metrics_by_model:
metrics_by_model[metric_name] = {}
metrics_by_model[metric_name][model_name] = metric_values
with (base_folder / 'metrics_tables.txt').open("w") as metrics_file:
for metric_name, metric_by_model in metrics_by_model.items():
headers = ["Model", "min", "max", "mean", "median", "std"]
table_data = []
for model_name, metric_values in metric_by_model.items():
table_data.append([model_name] + row_stats(metric_values))
print('-' * 90)
print(Style.BRIGHT + metric_name + Style.NORMAL)
table = tabulate(table_data,
headers=headers,
tablefmt='fancy_grid',
floatfmt='6.4f',
numalign='center',
stralign='left')
metrics_file.write(table)
print(table)
print()
print(Style.BRIGHT + f"p-value matrix [{metric_name}]" + Style.NORMAL)
print(dict_to_pvalue_table(metric_by_model))
def main():
parser = argparse.ArgumentParser(formatter_class=my_formatter)
parser.add_argument('dataset_dir',
type=pathlib.Path,
help='dataset directory',
nargs='+')
parser.add_argument('--mode',
choices=['train', 'test', 'val'],
default='train')
parser.add_argument('--n-repetitions', type=int, default=1)
parser.add_argument('--batch-size', type=int, default=64)
args = parser.parse_args()
dataset = ClassifierDatasetLoader(args.dataset_dir)
dataset.cache_negative = False
t0 = perf_counter()
tf_dataset = dataset.get_datasets(mode=args.mode)
scenario = get_scenario('link_bot')
tf_dataset = tf_dataset.batch(args.batch_size)
time_to_load = perf_counter() - t0
print("Time to Load (s): {:5.3f}".format(time_to_load))
n = 8192
batches = int(n / args.batch_size)
try:
# ram_usage = []
for _ in range(args.n_repetitions):
# None
t0 = perf_counter()
for e in progressbar.progressbar(tf_dataset.take(batches)):
pass
print('{:.5f}'.format(perf_counter() - t0))
# NEW
t0 = perf_counter()
for e in progressbar.progressbar(tf_dataset.take(batches)):
e = add_traj_image(scenario=scenario,
example=e,
local_env_w=100,
states_keys=['link_bot'],
local_env_h=100,
rope_image_k=10000,
batch_size=args.batch_size)
print('{:.5f}'.format(perf_counter() - t0))
# OLD
t0 = perf_counter()
for e in progressbar.progressbar(tf_dataset.take(batches)):
e = add_traj_image_to_example_old(scenario=scenario,
example=e,
local_env_w=100,
states_keys=['link_bot'],
local_env_h=100,
rope_image_k=10000,
batch_size=args.batch_size)
print('{:.5f}'.format(perf_counter() - t0))
# plt.plot(ram_usage)
# plt.xlabel("iteration")
# plt.ylabel("ram usage (bytes)")
# plt.show()
except KeyboardInterrupt:
pass