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