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