def _init_viz_action(scenario: ExperimentScenario, example: Dict):
action = {k: example[k][0] for k in fwd_model.action_keys}
pred_0 = index_time_with_metadata(metadata,
example,
fwd_model.state_keys,
t=0)
scenario.plot_action_rviz(pred_0, action)
def test_occupancy(self):
rospy.init_node('test_occupancy')
service_provider = GazeboServices()
movable_objects_services = {
"moving_box1": make_movable_object_services("moving_box1")
}
xs = [-1, 1, 1, -1, -1]
ys = [-1, -1, 1, 1, -1]
res = 0.1
extent = [-1, 1, -1, 1, 0, 1]
for x_i, y_i in zip(xs, ys):
object_positions = {"moving_box1": [x_i, y_i]}
ExperimentScenario.move_objects_to_positions(
movable_objects_services, object_positions, timeout=150)
environment = get_environment_for_extents_3d(
extent=extent,
res=res,
service_provider=service_provider,
robot_name="test")
# scale down to avoid out of bounds on the edges
row_i, col_i, channel_i = point_to_idx_3d_in_env(
x=0.99 * x_i, y=0.99 * y_i, z=0.01, environment=environment)
occupied = environment['env'][row_i, col_i, channel_i] > 0
self.assertTrue(occupied)
row_i, col_i, channel_i = point_to_idx_3d_in_env(
x=0 * x_i, y=0 * y_i, z=0.01, environment=environment)
occupied = environment['env'][row_i, col_i, channel_i] > 0
self.assertFalse(occupied)
def move_objects_randomly(self,
env_rng,
movable_objects_services,
movable_objects,
kinematic: bool,
timeout: float = 0.5):
random_object_positions = sample_object_positions(
env_rng, movable_objects)
if kinematic:
raise NotImplementedError()
else:
ExperimentScenario.move_objects(movable_objects_services,
random_object_positions, timeout)
def gt_perception_reliability(scenario: ExperimentScenario, actual: Dict,
predicted: Dict):
gt_perception_error_bt = scenario.classifier_distance(actual, predicted)
# add over time
gt_perception_error_b = tf.math.reduce_sum(gt_perception_error_bt, axis=1)
perception_reliability = zero_through_inf_to_one_through_zero(
gt_perception_error_b)
return perception_reliability
def execute_actions(
scenario: ExperimentScenario,
environment: Dict,
start_state: Dict,
actions: List[Dict],
plot: bool = False):
pre_action_state = start_state
actual_path = [pre_action_state]
for action in actions:
scenario.execute_action(action)
state_t = scenario.get_state()
actual_path.append(state_t)
if plot:
scenario.plot_environment_rviz(environment)
scenario.plot_executed_action(pre_action_state, action)
scenario.plot_state_rviz(state_t, label='actual')
pre_action_state = state_t
return actual_path
def sample_random_actions(environment: Dict,
start_state_tiled: Dict,
scenario: ExperimentScenario,
params: Dict,
n_actions: int):
aciton_rng = np.random.RandomState(0)
actions = scenario.sample_action_batch(environment=environment,
state=start_state_tiled,
action_params=params,
action_rng=aciton_rng,
validate=False,
batch_size=n_actions)
return actions
def batch_stateless_sample_action(scenario: ExperimentScenario,
environment: Dict, state: Dict,
batch_size: int, n_action_samples: int,
n_actions: int, action_params: Dict,
action_rng: np.random.RandomState):
# TODO: make the lowest level sample_action operate on batched state dictionaries
action_sequences = scenario.sample_action_sequences(
environment=environment,
state=state,
action_params=action_params,
n_action_sequences=n_action_samples,
action_sequence_length=n_actions,
validate=False,
action_rng=action_rng)
action_sequences = [
sequence_of_dicts_to_dict_of_tensors(a) for a in action_sequences
]
action_sequences = sequence_of_dicts_to_dict_of_tensors(action_sequences)
return {
k: tf.tile(v[tf.newaxis], [batch_size, 1, 1, 1])
for k, v in action_sequences.items()
}
def _viz_transition_t(scenario: ExperimentScenario, example: Dict, t: int):
action = index_batch_time(example, fwd_model.action_keys, b=t, t=0)
s0 = index_batch_time_with_metadata(metadata,
example,
fwd_model.state_keys,
b=t,
t=0)
s1 = index_batch_time_with_metadata(metadata,
example,
fwd_model.state_keys,
b=t,
t=1)
if 'accept_probablity' in example:
accept_probability_t = example['accept_probability'][t]
color = cm.Reds(accept_probability_t)
else:
color = "#aa2222aa"
scenario.plot_state_rviz(s0, label='', color='#ff0000ff')
scenario.plot_state_rviz(s1, label='predicted', color=color)
scenario.plot_action_rviz(s0, action, label='')
def _classifier_transition_viz_t(scenario: ExperimentScenario,
example: Dict, t: int):
pred_t = index_time_with_metadata(metadata,
example,
predicted_state_keys,
t=t)
scenario.plot_state_rviz(pred_t, label='predicted', color='#0000ffff')
label_t = example['is_close'][t]
scenario.plot_is_close(label_t)
if true_state_keys is not None:
true_t = index_time_with_metadata(metadata,
example,
true_state_keys,
t=t)
scenario.plot_state_rviz(true_t,
label='actual',
color='#ff0000ff',
scale=1.1)
def get_metric(self, scenario: ExperimentScenario, trial_datum: Dict):
goal = trial_datum['goal']
final_actual_state = trial_datum['end_state']
self.thresholds = trial_datum['planner_params']
final_execution_to_goal_error = scenario.distance_to_goal(final_actual_state, goal)
return final_execution_to_goal_error
def init_viz_env(scenario: ExperimentScenario,
example: Dict,
t: Optional[int] = None):
# the unused t arg makes it so we can pass this as either a t_func or a init_func
scenario.plot_environment_rviz(example)
def _viz_env_t(scenario: ExperimentScenario,
example: Dict,
t: Optional[int] = None):
env_t = index_time(e=example, time_indexed_keys=env_keys, t=t)
scenario.plot_environment_rviz(env_t)
def _recovery_transition_viz_t(scenario: ExperimentScenario, example: Dict,
t: int):
e_t = index_time_with_metadata(metadata, example, state_keys, t=t)
scenario.plot_state_rviz(e_t, label='', color='#ff0000ff', scale=1.1)
def generate_classifier_examples_from_batch(
scenario: ExperimentScenario,
prediction_actual: PredictionActualExample):
labeling_params = prediction_actual.labeling_params
prediction_horizon = prediction_actual.actual_prediction_horizon
classifier_horizon = labeling_params['classifier_horizon']
valid_out_examples = []
for classifier_start_t in range(
0, prediction_horizon - classifier_horizon + 1):
classifier_end_t = classifier_start_t + classifier_horizon
prediction_start_t = prediction_actual.prediction_start_t
prediction_start_t_batched = tf.cast(
tf.stack([prediction_start_t] * prediction_actual.batch_size,
axis=0), tf.float32)
classifier_start_t_batched = tf.cast(
tf.stack([classifier_start_t] * prediction_actual.batch_size,
axis=0), tf.float32)
classifier_end_t_batched = tf.cast(
tf.stack([classifier_end_t] * prediction_actual.batch_size,
axis=0), tf.float32)
out_example = {
'env': prediction_actual.dataset_element['env'],
'origin': prediction_actual.dataset_element['origin'],
'extent': prediction_actual.dataset_element['extent'],
'res': prediction_actual.dataset_element['res'],
'traj_idx': prediction_actual.dataset_element['traj_idx'],
'prediction_start_t': prediction_start_t_batched,
'classifier_start_t': classifier_start_t_batched,
'classifier_end_t': classifier_end_t_batched,
}
# this slice gives arrays of fixed length (ex, 5) which must be null padded from out_example_end_idx onwards
state_slice = slice(classifier_start_t,
classifier_start_t + classifier_horizon)
action_slice = slice(classifier_start_t,
classifier_start_t + classifier_horizon - 1)
sliced_actual = {}
for key, actual_state_component in prediction_actual.actual_states.items(
):
actual_state_component_sliced = actual_state_component[:,
state_slice]
out_example[key] = actual_state_component_sliced
sliced_actual[key] = actual_state_component_sliced
sliced_predictions = {}
for key, prediction_component in prediction_actual.predictions.items():
prediction_component_sliced = prediction_component[:, state_slice]
out_example[add_predicted(key)] = prediction_component_sliced
sliced_predictions[key] = prediction_component_sliced
# action
sliced_actions = {}
for key, action_component in prediction_actual.actions.items():
action_component_sliced = action_component[:, action_slice]
out_example[key] = action_component_sliced
sliced_actions[key] = action_component_sliced
# compute label
threshold = labeling_params['threshold']
error = scenario.classifier_distance(sliced_actual, sliced_predictions)
is_close = error < threshold
out_example['error'] = tf.cast(error, dtype=tf.float32)
# perception reliability
if 'perception_reliability_method' in labeling_params:
pr_method = labeling_params['perception_reliability_method']
if pr_method == 'gt':
perception_reliability = gt_perception_reliability(
scenario, sliced_actual, sliced_predictions)
out_example['perception_reliability'] = perception_reliability
else:
raise NotImplementedError(
f"unrecognized perception reliability method {pr_method}")
is_first_predicted_state_close = is_close[:, 0]
valid_indices = tf.where(is_first_predicted_state_close)
valid_indices = tf.squeeze(valid_indices, axis=1)
# keep only valid_indices from every key in out_example...
valid_out_example = gather_dict(out_example, valid_indices)
valid_out_examples.append(valid_out_example)
# valid_out_examples.append(out_example)
return valid_out_examples