def optimize(self, start_config):
""" Optimize the objective over a trajectory
starting from start_config.
"""
p = self.params
model = p.model
raw_data = self._raw_data(start_config)
processed_data = model.create_nn_inputs_and_outputs(raw_data)
# Predict the NN output
nn_output_113 = model.predict_nn_output_with_postprocessing(
processed_data['inputs'], is_training=False)[:, None]
# Transform to World Coordinates
waypoint_ego_config = SystemConfig(
dt=self.params.dt,
n=1,
k=1,
position_nk2=nn_output_113[:, :, :2],
heading_nk1=nn_output_113[:, :, 2:3])
self.params.system_dynamics.to_world_coordinates(
start_config, waypoint_ego_config, self.waypoint_world_config)
# Evaluate the objective and retrieve Control Pipeline data
obj_vals, data = self.eval_objective(start_config,
self.waypoint_world_config)
# The batch dimension is length 1 since there is only one waypoint
min_idx = 0
min_cost = obj_vals[min_idx]
waypts, horizons_s, trajectories_lqr, trajectories_spline, controllers = data
self.opt_waypt.assign_from_config_batch_idx(waypts, min_idx)
self.opt_traj.assign_from_trajectory_batch_idx(trajectories_lqr,
min_idx)
# Convert horizon in seconds to horizon in # of steps
min_horizon = int(
tf.ceil(horizons_s[min_idx, 0] / self.params.dt).numpy())
data = {
'system_config': SystemConfig.copy(start_config),
'waypoint_config': SystemConfig.copy(self.opt_waypt),
'trajectory': Trajectory.copy(self.opt_traj),
'spline_trajectory': Trajectory.copy(trajectories_spline),
'planning_horizon': min_horizon,
'K_nkfd': controllers['K_nkfd'][min_idx:min_idx + 1],
'k_nkf1': controllers['k_nkf1'][min_idx:min_idx + 1],
'img_nmkd': raw_data['img_nmkd']
}
return data
def optimize(self, start_config, goal_config=None, sim_state_hist=None):
""" Optimize the objective over a trajectory
starting from start_config.
1. Uses a control pipeline to plan paths from start_config
2. Evaluates the objective function on the resulting trajectories
3. Return the minimum cost waypoint, trajectory, and cost
"""
# TODO:
# changing the code so that sim_state is all history
obj_vals, data = self.eval_objective(start_config,
goal_config,
sim_state_hist=sim_state_hist)
min_idx = np.argmin(obj_vals)
min_cost = obj_vals[min_idx]
waypts, horizons_s, trajectories_lqr, trajectories_spline, controllers = data
self.opt_waypt.assign_from_config_batch_idx(waypts, min_idx)
self.opt_traj.assign_from_trajectory_batch_idx(trajectories_lqr,
min_idx)
# Convert horizon in seconds to horizon in # of steps
try:
min_horizon = int(np.ceil(horizons_s[min_idx] / self.params.dt))
except:
min_horizon = int(np.ceil(horizons_s[min_idx, 0] / self.params.dt))
# If the real LQR data has been discarded just take the first element
# since it will be all zeros
if self.params.control_pipeline_params.discard_LQR_controller_data:
K_nkfd = controllers['K_nkfd'][0:1]
k_nkf1 = controllers['k_nkf1'][0:1]
else:
K_nkfd = controllers['K_nkfd'][min_idx:min_idx + 1]
k_nkf1 = controllers['k_nkf1'][min_idx:min_idx + 1]
data = {
'system_config': SystemConfig.copy(start_config),
'waypoint_config': SystemConfig.copy(self.opt_waypt),
'trajectory': Trajectory.copy(self.opt_traj),
'spline_trajectory': Trajectory.copy(trajectories_spline),
'planning_horizon': min_horizon,
'K_nkfd': K_nkfd,
'k_nkf1': k_nkf1,
'img_nmkd': []
} # Dont think we need for our purposes
return data
def optimize(self, start_config):
""" Optimize the objective over a trajectory
starting from start_config.
1. Uses a control pipeline to plan paths from start_config
2. Evaluates the objective function on the resulting trajectories
3. Return the minimum cost waypoint, trajectory, and cost
"""
obj_vals, data = self.eval_objective(start_config)
min_idx = tf.argmin(obj_vals)
min_cost = obj_vals[min_idx]
waypts, horizons_s, trajectories_lqr, trajectories_spline, controllers = data
self.opt_waypt.assign_from_config_batch_idx(waypts, min_idx)
self.opt_traj.assign_from_trajectory_batch_idx(trajectories_lqr,
min_idx)
# Convert horizon in seconds to horizon in # of steps
min_horizon = int(
tf.ceil(horizons_s[min_idx, 0] / self.params.dt).numpy())
# If the real LQR data has been discarded just take the first element
# since it will be all zeros
if self.params.control_pipeline_params.discard_LQR_controller_data:
K_nkfd = controllers['K_nkfd'][0:1]
k_nkf1 = controllers['k_nkf1'][0:1]
else:
K_nkfd = controllers['K_nkfd'][min_idx:min_idx + 1]
k_nkf1 = controllers['k_nkf1'][min_idx:min_idx + 1]
img_nmkd = self.simulator.get_observation(config=start_config)
data = {
'system_config': SystemConfig.copy(start_config),
'waypoint_config': SystemConfig.copy(self.opt_waypt),
'trajectory': Trajectory.copy(self.opt_traj),
'spline_trajectory': Trajectory.copy(trajectories_spline),
'planning_horizon': min_horizon,
'K_nkfd': K_nkfd,
'k_nkf1': k_nkf1,
'img_nmkd': img_nmkd
}
return data
def generate_control_pipeline(self, params=None):
p = self.params
# Initialize spline, cost function, lqr solver
waypoints_egocentric = self._sample_egocentric_waypoints(vf=0.)
self._init_pipeline()
pipeline_data = self.helper.empty_data_dictionary()
with tf.name_scope('generate_control_pipeline'):
if not self._incorrectly_binned_data_exists():
for v0 in self.start_velocities:
if p.verbose:
print('Initial Bin: v0={:.3f}'.format(v0))
start_config = self.system_dynamics.init_egocentric_robot_config(
dt=p.system_dynamics_params.dt,
n=self.waypoint_grid.n,
v=v0)
goal_config = SystemConfig.copy(waypoints_egocentric)
start_config, goal_config, horizons_n1 = self._dynamically_fit_spline(
start_config, goal_config)
lqr_trajectory, K_nkfd, k_nkf1 = self._lqr(start_config)
# TODO: Put the initial bin information in here too. This will make debugging much easier.
data_bin = {
'start_configs':
start_config,
'waypt_configs':
goal_config,
'start_speeds':
self.spline_trajectory.speed_nk1()[:, 0],
'spline_trajectories':
Trajectory.copy(self.spline_trajectory),
'horizons':
horizons_n1,
'lqr_trajectories':
lqr_trajectory,
'K_nkfd':
K_nkfd,
'k_nkf1':
k_nkf1
}
self.helper.append_data_bin_to_pipeline_data(
pipeline_data, data_bin)
# This data is incorrectly binned by velocity so collapse it all into one bin before saving it.
pipeline_data = self.helper.concat_data_across_binning_dim(
pipeline_data)
self._save_incorrectly_binned_data(pipeline_data)
else:
pipeline_data = self._load_incorrectly_binned_data()
pipeline_data = self._rebin_data_by_initial_velocity(pipeline_data)
self._set_instance_variables(pipeline_data)
for i, v0 in enumerate(self.start_velocities):
filename = self._data_file_name(v0=v0)
data_bin = self.helper.prepare_data_for_saving(pipeline_data, i)
self.save_control_pipeline(data_bin, filename)
def optimize(self, start_config):
""" Optimize the objective over a trajectory
starting from start_config.
"""
p = self.params
model = p.model
raw_data = self._raw_data(start_config)
processed_data = model.create_nn_inputs_and_outputs(raw_data)
# Predict the NN output
nn_output_112k = model.predict_nn_output_with_postprocessing(
processed_data['inputs'], is_training=False)[:, None]
optimal_control_hat_1k2 = tf.reshape(nn_output_112k, (1, -1, 2))
data = {
'optimal_control_nk2': optimal_control_hat_1k2,
'system_config': SystemConfig.copy(start_config),
'img_nmkd': raw_data['img_nmkd']
}
return data
def get_config(self, config, deepcpy):
if(deepcpy):
return SystemConfig.copy(config)
return config
