def __init__(self, config: ArchitectureConfig, quiet: bool = False): raise NotImplementedError( 'Currently this should not work as the actions recorded are 6d ' 'but this network only return 1d') super().__init__(config=config, quiet=True) self.input_size = (30, ) self.output_size = (1, ) self.action_min = -1 self.action_max = +1 self._actor = mlp_creator( sizes=[self.input_size[0], 64, 64, self.output_size[0]], activation=nn.Tanh(), output_activation=nn.Tanh()) self._critic = mlp_creator(sizes=[self.input_size[0], 64, 64, 1], activation=nn.Tanh(), output_activation=None) log_std = self._config.log_std if self._config.log_std != 'default' else -0.5 self.log_std = torch.nn.Parameter( torch.ones(self.output_size, dtype=torch.float32) * log_std, requires_grad=True) if not quiet: self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger) self.initialize_architecture()
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self.residual_2.residual_net.conv_0.weight = self.residual_1.residual_net.conv_0.weight self.residual_2.residual_net.conv_0.bias = self.residual_1.residual_net.conv_0.bias self.residual_2.residual_net.conv_1.weight = self.residual_1.residual_net.conv_1.weight self.residual_2.residual_net.conv_1.bias = self.residual_1.residual_net.conv_1.bias self.residual_3.residual_net.conv_0.weight = self.residual_1.residual_net.conv_0.weight self.residual_3.residual_net.conv_0.bias = self.residual_1.residual_net.conv_0.bias self.residual_3.residual_net.conv_1.weight = self.residual_1.residual_net.conv_1.weight self.residual_3.residual_net.conv_1.bias = self.residual_1.residual_net.conv_1.bias self.residual_4.residual_net.conv_0.weight = self.residual_1.residual_net.conv_0.weight self.residual_4.residual_net.conv_0.bias = self.residual_1.residual_net.conv_0.bias self.residual_4.residual_net.conv_1.weight = self.residual_1.residual_net.conv_1.weight self.residual_4.residual_net.conv_1.bias = self.residual_1.residual_net.conv_1.bias if not quiet: self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) self.initialize_architecture() cprint(f'Started.', self._logger)
def update_fields(self): key = self.get_key() if self.methodBindings is not None and key in self.methodBindings.keys( ): eval(f'self.{self.methodBindings[key]}()') if self.topicBindings is not None and key in self.topicBindings.keys(): self.publishers[key].publish(Empty()) cprint(f'publish {self.publishers[key]}', self._logger) if self.serviceBindings is not None and key in self.serviceBindings.keys( ): # rospy.wait_for_service(self.serviceBindings[key]['name']) self.serviceBindings[key]['proxy']( self.serviceBindings[key]['message']) # self.serviceBindings[key]['proxy'](True) cprint( f'{self.serviceBindings[key]["proxy"]}({self.serviceBindings[key]["message"]})', self._logger) if self.moveBindings is not None and key in self.moveBindings.keys(): self.x = self.moveBindings[key][0] self.y = self.moveBindings[key][1] self.z = self.moveBindings[key][2] self.roll = self.moveBindings[key][3] self.pitch = self.moveBindings[key][4] self.yaw = self.moveBindings[key][5] else: self.reset_control_fields() return key
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self.input_scope = 'default' self.discrete = False self.dropout = nn.Dropout( p=config.dropout) if isinstance(config.dropout, float) else None self._config.batch_normalisation = config.batch_normalisation if isinstance(config.batch_normalisation, bool) \ else False self.h = self._config.latent_dim if isinstance(config.latent_dim, int) else 32 self.vae = self._config.vae if isinstance(config.vae, bool) else False if not quiet: self.encoder = nn.Sequential( nn.Conv2d(1, self.h * (2 if self.vae else 1), 1, stride=1), *[ nn.BatchNorm2d(self.h * (2 if self.vae else 1)), nn.LeakyReLU() ] if self._config.batch_normalisation else [nn.LeakyReLU()], ) self.decoder = nn.Sequential(nn.Conv2d(self.h, 1, 1, stride=1)) self.initialize_architecture() self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger)
def _store_frame(self, data: Union[np.ndarray, float], dst: str, time_stamp: int) -> None: if not isinstance(data, np.ndarray): data = np.asarray(data) try: if len(data.shape) in [2, 3]: if not os.path.isdir( os.path.join(self._config.saving_directory, dst)): os.makedirs(os.path.join(self._config.saving_directory, dst), exist_ok=True) store_image( data=data, file_name=os.path.join(self._config.saving_directory, dst, timestamp_to_filename(time_stamp)) + '.jpg') elif len(data.shape) in [0, 1]: store_array_to_file(data=data, file_name=os.path.join( self._config.saving_directory, dst + '.data'), time_stamp=time_stamp) except Exception as e: cprint(f'Failed to store frame: {e}', self._logger, msg_type=MessageType.error)
def _update_fsm_state(self, msg: String): if self._fsm_state == FsmState.Running and FsmState[ msg.data] == FsmState.Terminated: self._write_image() if self._fsm_state != FsmState[msg.data]: cprint(f'update fsm state {msg.data}', self._logger) self._fsm_state = FsmState[msg.data]
def _setup(self): self._fsm_state = FsmState.Unknown rospy.Subscriber(name='/fsm/state', data_class=String, callback=self._set_fsm_state) # field turn True when motors are enabled, don't publish control when motors are disabled self._motors_enabled = False # keep track of last commands to detect stable point self._control_norm_window_length = 10 self._control_norm_window = [] if 'turtle' in self._robot or 'default' in self._robot or 'real' in self._robot: cprint( f'altitude control not required for {self._robot}, shutting down...', self._logger) sys.exit(0) elif self._robot == 'quadrotor': # in case of single quadrotor self._publishers['default'] = rospy.Publisher('cmd_vel', Twist, queue_size=10) sensor = SensorType.position sensor_topic = rospy.get_param( f'/robot/{sensor.name}_sensor/topic') sensor_type = rospy.get_param(f'/robot/{sensor.name}_sensor/type') rospy.Subscriber( name=sensor_topic, data_class=eval(sensor_type), callback=eval( f'self._process_{camelcase_to_snake_format(sensor_type)}'), callback_args='default') rospy.wait_for_service('/enable_motors') self._enable_motors_services['default'] = rospy.ServiceProxy( '/enable_motors', EnableMotors) elif isinstance(self._robot, list): # in case of tracking fleeing quadrotor self._publishers['tracking'] = rospy.Publisher('cmd_vel', Twist, queue_size=10) self._publishers['fleeing'] = rospy.Publisher('cmd_vel_1', Twist, queue_size=10) for agent in ['tracking', 'fleeing']: sensor = SensorType.position sensor_topic = rospy.get_param( f'/robot/{agent}_{sensor.name}_sensor/topic') sensor_type = rospy.get_param( f'/robot/{agent}_{sensor.name}_sensor/type') rospy.Subscriber( name=sensor_topic, data_class=eval(sensor_type), callback=eval( f'self._process_{camelcase_to_snake_format(sensor_type)}' ), callback_args=agent) rospy.wait_for_service(f'/{agent}/enable_motors') self._enable_motors_services[agent] = rospy.ServiceProxy( f'/{agent}/enable_motors', EnableMotors)
def __init__(self): self.count = 0 rospy.init_node('ros_expert') stime = time.time() max_duration = 60 while not rospy.has_param('/actor/ros_expert/specs' ) and time.time() < stime + max_duration: time.sleep(0.01) self._specs = rospy.get_param('/actor/ros_expert/specs') super().__init__( config=ActorConfig(name='ros_expert', specs=self._specs)) self._output_path = get_output_path() self._logger = get_logger(get_filename_without_extension(__file__), self._output_path) cprint(f'ros specifications: {self._specs}', self._logger) with open(os.path.join(self._output_path, 'ros_expert_specs.yml'), 'w') as f: yaml.dump(self._specs, f) self._reference_height = rospy.get_param('/world/starting_height', 1) self._adjust_height = 0 self._adjust_yaw_collision_avoidance = 0 self._adjust_yaw_waypoint_following = 0 self._rate_fps = self._specs[ 'rate_fps'] if 'rate_fps' in self._specs.keys() else 10 self._next_waypoint = [] noise_config = self._specs['noise'] if 'noise' in self._specs.keys( ) else {} self._noise = eval(f"{noise_config['name']}(**noise_config['args'])" ) if noise_config else None self._publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10) self._subscribe()
def create_train_validation_hdf5_files( self, runs: List[str] = None, input_size: List[int] = None) -> None: all_runs = runs if runs is not None else self._get_runs() number_of_training_runs = int(self._config.training_validation_split * len(all_runs)) train_runs = all_runs[0:number_of_training_runs] validation_runs = all_runs[number_of_training_runs:] for file_name, runs in zip(['train', 'validation'], [train_runs, validation_runs]): config = DataLoaderConfig().create( config_dict={ 'data_directories': runs, 'output_path': self._config.output_path, 'subsample': self._config.subsample_hdf5, 'input_size': input_size }) data_loader = DataLoader(config=config) data_loader.load_dataset() create_hdf5_file_from_dataset(filename=os.path.join( self._config.output_path, file_name + '.hdf5'), dataset=data_loader.get_dataset()) cprint(f'created {file_name}.hdf5', self._logger)
def __init__(self, config: EvaluatorConfig, network: BaseNet, quiet: bool = False): self._config = config self._net = network self.data_loader = DataLoader(config=self._config.data_loader_config) if not quiet: self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) if type(self) == Evaluator else None cprint(f'Started.', self._logger) self._device = torch.device( "cuda" if self._config.device in ['gpu', 'cuda'] and torch.cuda.is_available() else "cpu") self._criterion = eval( f'{self._config.criterion}(reduction=\'none\', {self._config.criterion_args_str})' ) self._criterion.to(self._device) self._lowest_validation_loss = None self.data_loader.load_dataset() self._minimum_error = float(10**6) self._original_model_device = self._net.get_device( ) if self._net is not None else None
def _subscribe(self): self._fsm_state = FsmState.Unknown rospy.Subscriber(name='/fsm/state', data_class=String, callback=self._set_fsm_state) # listen to desired next reference point rospy.Subscriber('/reference_pose', PointStamped, self._reference_update) rospy.Subscriber(name='/waypoint_indicator/current_waypoint', data_class=Float32MultiArray, callback=self._reference_update) # Robot sensors: for sensor in [SensorType.position]: if rospy.has_param(f'/robot/{sensor.name}_sensor/topic'): sensor_topic = rospy.get_param( f'/robot/{sensor.name}_sensor/topic') sensor_type = rospy.get_param( f'/robot/{sensor.name}_sensor/type') sensor_callback = f'_process_{camelcase_to_snake_format(sensor_type)}' if sensor_callback not in self.__dir__(): cprint(f'Could not find sensor_callback {sensor_callback}', self._logger) sensor_stats = rospy.get_param(f'/robot/{sensor.name}_sensor/stats') \ if rospy.has_param(f'/robot/{sensor.name}_sensor/stats') else {} rospy.Subscriber(name=sensor_topic, data_class=eval(sensor_type), callback=eval(f'self.{sensor_callback}'), callback_args=(sensor_topic, sensor_stats))
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self.input_size = (4,) self.output_size = (5,) self.discrete = False log_std = self._config.log_std if self._config.log_std != 'default' else -0.5 self.log_std = torch.nn.Parameter(torch.ones(self.output_size, dtype=torch.float32) * log_std, requires_grad=True) self._actor = mlp_creator(sizes=[self.input_size[0], 10, self.output_size[0]], activation=nn.Tanh(), output_activation=None) self._critic = mlp_creator(sizes=[self.input_size[0], 10, 1], activation=nn.Tanh(), output_activation=None) self.initialize_architecture() self.discrete_action_mapper = DiscreteActionMapper([ torch.as_tensor([0.0, 0.0, 0.0, 0.0]), torch.as_tensor([-1.0, 0.0, 0.0, 0.0]), torch.as_tensor([1.0, 0.0, 0.0, 0.0]), torch.as_tensor([0.0, -1.0, 0.0, 0.0]), torch.as_tensor([0.0, 1.0, 0.0, 0.0]), ]) if not quiet: self._logger = get_logger(name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger)
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config) self.input_size = (3, ) self.output_size = (1, 1) self.action_min = -2 self.action_max = 2 self._actor = mlp_creator( sizes=[self.input_size[0], 64, 64, self.output_size[0]], activation=nn.Tanh(), output_activation=None) self._critic = mlp_creator(sizes=[self.input_size[0], 64, 64, 1], activation=nn.Tanh(), output_activation=None) log_std = self._config.log_std if self._config.log_std != 'default' else -0.5 self.log_std = torch.nn.Parameter( torch.ones(self.output_size, dtype=torch.float32) * log_std, requires_grad=True) if not quiet: self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger) self.initialize_architecture()
def __init__(self): stime = time.time() max_duration = 60 while not rospy.has_param('/modified_state_publisher/mode' ) and time.time() < stime + max_duration: time.sleep(0.01) self._output_path = get_output_path() self._logger = get_logger(get_filename_without_extension(__file__), self._output_path) rospy.Subscriber( rospy.get_param('/robot/modified_state_sensor/topic', '/modified_state'), eval( rospy.get_param('/robot/modified_state_sensor/type', 'CombinedGlobalPoses')), self._process_state_and_publish_frame) self._publisher = rospy.Publisher('/modified_state_frame', Image, queue_size=10) cprint( f"subscribe to {rospy.get_param('/robot/modified_state_sensor/topic', '/modified_state')}", self._logger) rospy.init_node('modified_state_frame_visualizer')
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self.input_size = (30, ) self.output_size = (3, ) self._actor = mlp_creator( sizes=[self.input_size[0], 64, 64, self.output_size[0]], activation=nn.Tanh(), output_activation=None) self._critic = mlp_creator(sizes=[self.input_size[0], 64, 64, 1], activation=nn.Tanh(), output_activation=None) self.initialize_architecture() self.discrete_action_mapper = DiscreteActionMapper([ torch.as_tensor([0.2, 0.0, 0.0, 0.0, 0.0, -0.2]), torch.as_tensor([0.2, 0.0, 0.0, 0.0, 0.0, 0.0]), torch.as_tensor([0.2, 0.0, 0.0, 0.0, 0.0, 0.2]), ]) if not quiet: self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger)
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) if not quiet: cprint(f'Started.', self._logger) self.input_size = (3, 128, 128) self.output_size = (6, ) self.discrete = False self.dropout = nn.Dropout( p=config.dropout) if config.dropout != 'default' else None self.encoder = nn.Sequential( nn.Conv2d(3, 32, 4, stride=2), nn.ReLU(), nn.Conv2d(32, 64, 4, stride=2), nn.ReLU(), nn.Conv2d(64, 128, 4, stride=2), nn.ReLU(), nn.Conv2d(128, 256, 4, stride=2), nn.ReLU(), ) self.decoder = mlp_creator( sizes=[256 * 6 * 6, 128, 128, self.output_size[0]], activation=nn.ReLU(), output_activation=nn.Tanh(), bias_in_last_layer=False) self.initialize_architecture()
def reset(self) -> Tuple[Experience, np.ndarray]: """ reset gazebo, reset fsm, wait till fsm in 'running' state return experience without reward or action """ cprint(f'resetting', self._logger) self._reset_filters() self._step = 0 self._return = 0 if self._config.ros_config.ros_launch_config.gazebo: self._reset_gazebo() self._reset_publisher.publish(Empty()) self._clear_experience_values() while self.fsm_state != FsmState.Running \ or self.observation is None \ or self.terminal_state is None \ or self.terminal_state is TerminationType.Unknown: self._run_shortly() self.observation = self._filter_observation(self.observation) self._current_experience = Experience( done=deepcopy(self.terminal_state), observation=deepcopy(self.observation), time_stamp=int(rospy.get_time() * 10**3), info={}) self._previous_observation = deepcopy(self.observation) return self._current_experience, deepcopy(self.observation)
def _signal_handler(self, signal_number: int, _) -> None: return_value = self.remove() cprint(f'received signal {signal_number}.', self._logger, msg_type=MessageType.info if return_value == ProcessState.Terminated else MessageType.error) sys.exit(0)
def __init__(self, config: TrainerConfig, network: BaseNet, quiet: bool = False): super().__init__(config, network, quiet=True) if not quiet: self._optimizer = eval(f'torch.optim.{self._config.optimizer}')( params=self._net.deeply_supervised_parameters(), lr=self._config.learning_rate, weight_decay=self._config.weight_decay) lambda_function = lambda f: 1 - f / self._config.scheduler_config.number_of_epochs self._scheduler = torch.optim.lr_scheduler.LambdaLR(self._optimizer, lr_lambda=lambda_function) \ if self._config.scheduler_config is not None else None self._discriminator_optimizer = eval( f'torch.optim.{self._config.optimizer}')( params=self._net.discriminator_parameters(), lr=self._config.critic_learning_rate if self._config.critic_learning_rate != -1 else self._config.learning_rate, weight_decay=self._config.weight_decay) self._logger = get_logger( name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger)
def _process_position(self, pose: Pose): if float(f"{rospy.get_time()}".split('.')[-1] [0]) % 5 == 0: # print every 500ms cprint(f'received pose {pose}', self._logger, msg_type=MessageType.debug) robot_global_translation = np.asarray( [pose.position.x, pose.position.y, pose.position.z]) robot_global_orientation = rotation_from_quaternion( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w)) points_global_frame = transform(points=self._local_frame, orientation=robot_global_orientation, translation=robot_global_translation) points_camera_frame = transform( points=points_global_frame, orientation=self._camera_global_orientation, translation=self._camera_global_translation, invert=True ) # camera_global transformation is given, but should be inverted points_image_frame = project(points=points_camera_frame, fx=self._fx, fy=self._fy, cx=self._cx, cy=self._cy) if self._previous_position is None: self._previous_position = points_image_frame[ 0] # store origin of position self._frame_points.append(points_image_frame) elif get_distance(self._previous_position, points_image_frame[0]) > self._minimum_distance_px: self._previous_position = points_image_frame[ 0] # store origin of position self._frame_points.append(points_image_frame)
def _internal_update_terminal_state(self): if self.fsm_state == FsmState.Running and \ self._config.max_number_of_steps != -1 and \ self._config.max_number_of_steps <= self._step: self.terminal_state = TerminationType.Done cprint( f'reach max number of steps {self._config.max_number_of_steps} < {self._step}', self._logger)
def run(self): while not rospy.is_shutdown(): if self._fsm_state == FsmState.Running: self.count += 1 if self.count % 10 * self._rate_fps == 0: msg = f'<<reference: {self.pose_ref}, \n<<pose: {self.pose_est} \n control: {self.last_cmd}' cprint(msg, self._logger) rospy.sleep(duration=1 / self._rate_fps)
def _check_wrench_stamped(self, msg: WrenchStamped) -> None: if self._delay_evaluation(): return if msg.wrench.force.z < 0: cprint( f"found drag force: {msg.wrench.force.z}, so robot must be upside-down.", self._logger) self._occasion = 'on_collision' self._shutdown_run()
def get_checkpoint(self) -> dict: """ :return: a dictionary with global_step and model_state of neural network. """ cprint(f'checksum: {self.get_checksum()}', self._logger) return { 'global_step': self.global_step, 'model_state': self.state_dict() }
def test_multiple_loggers(self): logger_a = get_logger(name='module_a', output_path=self.TEST_DIR) logger_b = get_logger(name='module_b', output_path=self.TEST_DIR) cprint('started', logger_a) cprint('started', logger_b) log_files = glob(os.path.join(self.TEST_DIR, 'log_files', '*')) self.assertEqual(len(log_files), 2)
def load_checkpoint(self, checkpoint_file: str): # Load params for each experiment element checkpoint = torch.load(checkpoint_file, map_location=torch.device('cpu')) self._epoch = checkpoint['epoch'] if 'epoch' in checkpoint.keys() else 0 for element, key in zip([self._net, self._trainer, self._environment], ['net_ckpt', 'trainer_ckpt', 'environment_ckpt']): if element is not None and key in checkpoint.keys(): element.load_checkpoint(checkpoint[key]) cprint(f'loaded network from {checkpoint_file}', self._logger)
def __init__(self, config: ArchitectureConfig, quiet: bool = False): super().__init__(config=config, quiet=True) self.discrete = True if not quiet: self._logger = get_logger(name=get_filename_without_extension(__file__), output_path=config.output_path, quiet=False) cprint(f'Started.', self._logger) self.initialize_architecture()
def test_cprint(self): current_logger = get_logger(name=get_filename_without_extension(__file__), output_path=self.TEST_DIR, quiet=True) cprint('HELP', current_logger) log_file = glob(os.path.join(self.TEST_DIR, 'log_files', '*'))[0] with open(log_file, 'r') as f: log_line = f.readlines()[0].strip() self.assertTrue('HELP' in log_line)
def load_checkpoint(self, checkpoint) -> None: """ Try to load checkpoint in global step and model state. Raise error. :param checkpoint: dictionary containing 'global step' and 'model state' :return: None """ self.global_step = checkpoint['global_step'] self.load_state_dict(checkpoint['model_state']) self.set_device(self._device) cprint(f'checksum: {self.get_checksum()}', self._logger)
def _subscribe(self): # Robot sensors: sensor = '/robot/forward_camera' if rospy.has_param(f'{sensor}_topic'): sensor_topic = rospy.get_param(f'{sensor}_topic') sensor_type = rospy.get_param(f'{sensor}_type') sensor_callback = f'_process_{camelcase_to_snake_format(sensor_type)}' if sensor_callback not in self.__dir__(): cprint(f'Could not find sensor_callback {sensor_callback}', self._logger) sensor_stats = rospy.get_param( f'{sensor}_stats') if rospy.has_param( f'{sensor}_stats') else {} rospy.Subscriber(name=sensor_topic, data_class=eval(sensor_type), callback=eval(f'self.{sensor_callback}'), callback_args=(sensor_topic, sensor_stats)) rospy.Subscriber('/fsm/reset', Empty, self._reset) # Applied action self._action = None if rospy.has_param('/robot/command_topic'): rospy.Subscriber(name=rospy.get_param('/robot/command_topic'), data_class=Twist, callback=self._set_field, callback_args=('action', {})) # fsm state self._fsm_state = None rospy.Subscriber(name=rospy.get_param('/fsm/state_topic'), data_class=String, callback=self._set_field, callback_args=('fsm_state', {})) # Reward topic self._reward = None self._terminal_state = TerminationType.Unknown rospy.Subscriber(name=rospy.get_param('/fsm/reward_topic', ''), data_class=RosReward, callback=self._set_field, callback_args=('reward', {})) # waypoint self._waypoint = None rospy.Subscriber(name='/waypoint_indicator/current_waypoint', data_class=Float32MultiArray, callback=self._set_field, callback_args=('waypoint', {})) # battery state self._battery = None rospy.Subscriber( name='/bebop/states/common/CommonState/BatteryStateChanged', data_class=CommonCommonStateBatteryStateChanged, callback=self._set_field, callback_args=('battery', {}))