def test_multiple_resets(self): time.sleep(rospy.get_param('/world/delay_evaluation') + 2) for _ in range(2): experience, observation = self._environment.reset( ) # includes take off of two agents by fsm self.assertTrue(experience.action is None) self.assertEqual(experience.done, TerminationType.NotDone) count = 0 while experience.done == TerminationType.NotDone: fake_adversarial_action = Action( actor_name='tracking_fleeing_agent', value=np.asarray([ 0, 0, 5, # tracking velocity 0, 3, 0, # fleeing velocity 0, # tracking angular z 0 ])) # fleeing angular z print(f'{count} {fake_adversarial_action}') experience, observation = self._environment.step( action=fake_adversarial_action) count += 1 self.assertTrue(experience.observation is not None) self.assertTrue(experience.action is not None) self.assertTrue('frame' in experience.info.keys()) self.assertEqual(experience.info['/tracking/cmd_vel'].value[2], 5) self.assertEqual(experience.info['/fleeing/cmd_vel'].value[1], 3) print(f'{count} vs {MAXSTEPS}') self.assertEqual(count, MAXSTEPS)
def get_action(self, inputs, train: bool = False) -> Action: inputs = self.process_inputs(inputs) # added line 15/10/2020 output = self.sample(inputs, train=train) output = output.clamp(min=self.action_min, max=self.action_max) # added line 23/10/2020 return Action(actor_name=get_filename_without_extension(__file__), value=output)
def _set_field(self, msg: Union[String, Twist, RosReward, CommonCommonStateBatteryStateChanged, Odometry], args: Tuple) -> None: field_name, _ = args if field_name == 'fsm_state': self._fsm_state = FsmState[msg.data] elif field_name == 'action': self._action = Action(actor_name='applied_action', value=process_twist(msg).value) elif field_name == 'reward': self._reward = msg.reward self._terminal_state = TerminationType[msg.termination] elif field_name == 'reference_pose': self._reference_pose = np.asarray( [msg.point.x, msg.point.y, msg.point.z]) self._calculate_ref_in_img() elif field_name == 'battery': self._battery = msg.percent elif field_name == 'trajectory': global_pose = process_odometry(msg) self._trajectory.append(global_pose) elif field_name == 'camera_orientation': self._camera_orientation = float(msg.angular.y) else: raise NotImplementedError
def get_action(self, inputs, train: bool = False, agent_id: int = -1) -> Action: positions = np.squeeze(self.process_inputs(inputs)) try: bb = calculate_bounding_box(inputs) inputs_bb = (bb[3][0], bb[3][1], bb[4], bb[5]) inputs_ = (bb[3][0] - 200) / 40 inputs = torch.Tensor([inputs_]) self.previous_input = inputs_ except TypeError: inputs_bb = (200, 200, 40, 40) inputs = torch.Tensor([self.previous_input]) if agent_id == 0: # tracking agent ==> tracking_linear_y output = self.sample(inputs, train=train).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [0, output.data.cpu().numpy().squeeze(), 0, 0, 0, 0, 0, 0]) elif agent_id == 1: # fleeing agent ==> fleeing_linear_y output = self.sample(inputs, train=train, adversarial=True).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [0, 0, 0, 0, output.data.cpu().numpy().squeeze(), 0, 0, 0]) else: output = self.sample(inputs, train=train, adversarial=False).clamp(min=self.action_min, max=self.action_max) adversarial_output = self.sample(inputs, train=train, adversarial=True).clamp( min=self.action_min, max=self.action_max) # rand_run = get_rand_run_ros(self.waypoint, np.asarray(positions[3:6]).squeeze(), self._playfield_size) # run_action = np.squeeze(rand_run[1]) # self.waypoint = np.squeeze(rand_run[0]) # hunt_action = np.squeeze(get_slow_hunt_ros(np.asarray(positions), self._playfield_size)) # run_action = np.squeeze(get_slow_run_ros(inputs_bb, self._playfield_size)) actions = np.stack([ 0, output.data.cpu().numpy().squeeze().item(), 0, 0, adversarial_output.data.cpu().numpy().squeeze().item(), 0, 0, 0 ], axis=-1) # actions = np.stack([0, output.data.cpu().numpy().squeeze().item(), 0, *run_action, 0, 0], axis=-1) # actions = self.adjust_height(positions, actions) Not necessary, controller keeps altitude fixed actions = clip_action_according_to_playfield_size_flipped( positions.detach().numpy().squeeze(), actions, self._playfield_size) return Action( actor_name= "tracking_fleeing_agent", # assume output [1, 8] so no batch! value=actions)
def process_twist(msg, sensor_stats: dict = None) -> Action: return Action(actor_name=sensor_stats['name'] if sensor_stats is not None and 'name' in sensor_stats.keys() else '', value=np.asarray([ msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.x, msg.angular.y, msg.angular.z ]))
def get_action(self, inputs, train: bool = False) -> Action: inputs = self.process_inputs(inputs) output = self.sample(inputs, train=train) output = output.clamp(min=self.action_min, max=self.action_max) return Action( actor_name=get_filename_without_extension( __file__), # assume output [1, 2] so no batch! value=np.stack([ *output.data.cpu().numpy().squeeze(), *get_slow_run(inputs.squeeze()) ], axis=-1))
def get_action(self, inputs, train: bool = False, agent_id: int = -1) -> Action: positions = np.squeeze(self.process_inputs(inputs)) try: bb = calculate_bounding_box(inputs, orientation=(0, 0, 1)) inputs_bb = (bb[3][0], bb[3][1], bb[4], bb[5]) inputs = ((bb[3][0] - 200) / 40, (bb[3][1] - 200) / 40) inputs = np.squeeze(self.process_inputs(inputs)) self.previous_input = inputs except TypeError: inputs = self.previous_input inputs_bb = (200, 200, 40, 40) if agent_id == 0: # tracking agent ==> tracking_linear_y output = self.sample(inputs, train=train).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [output.data.cpu().numpy().squeeze(), 0, 0, 0, 0, 0, 0, 0]) elif agent_id == 1: # fleeing agent ==> fleeing_linear_y output = self.sample(inputs, train=train, adversarial=True).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [0, 0, 0, output.data.cpu().numpy().squeeze(), 0, 0, 0, 0]) else: output = self.action_max * self.sample( inputs, train=train, adversarial=False) adversarial_output = self.action_max * self.sample( inputs, train=train, adversarial=True) run_action = np.squeeze( get_slow_run_ros(inputs_bb, self._playfield_size)) #actions = np.stack([*output.data.cpu().numpy().squeeze(), 0, # *adversarial_output.data.cpu().numpy().squeeze(), 0, # 0, 0], axis=-1) print(inputs_bb) actions = np.stack( [*output.data.cpu().numpy().squeeze(), 0, *run_action, 0, 0], axis=-1) actions = np.stack([0, 0, 0, 1, 0, 0, 0, 0], axis=-1) # actions = self.adjust_height(positions, actions) Not necessary, controller keeps altitude fixed actions = clip_action_according_to_playfield_size_flipped( positions.detach().numpy().squeeze(), actions, self._playfield_size) return Action( actor_name= "tracking_fleeing_agent", # assume output [1, 8] so no batch! value=actions)
def _process_image(self, msg: Image, args: tuple) -> None: # cprint(f'image received {msg.data}', self._logger) sensor_topic, sensor_stats = args processed_image = process_image(msg, sensor_stats=sensor_stats) processed_image = torch.Tensor(processed_image).permute(2, 0, 1).unsqueeze(0) assert processed_image.size()[0] == 1 and processed_image.size()[1] == 3 output = self._model.forward([processed_image])[0].detach().cpu().numpy() cprint(f'output predicted {output}', self._logger, msg_type=MessageType.debug) action = Action( actor_name='dnn_actor', value=output # assuming control is first output ) self._publisher.publish(adapt_action_to_twist(action))
def _set_field(self, msg: Union[String, Twist, RosReward, CommonCommonStateBatteryStateChanged, Odometry], args: Tuple) -> None: field_name, _ = args if field_name == 'fsm_state': self._fsm_state = FsmState[msg.data] elif field_name == 'action': self._action = Action(actor_name='applied_action', value=process_twist(msg).value) elif field_name == 'reference_pose': self._reference_pose = np.asarray( [msg.point.x, msg.point.y, msg.point.z]) else: raise NotImplementedError
def get_action(self, input_img: np.ndarray, train: bool = False, agent_id: int = -1) -> Action: try: img = self.process_inputs(resize(input_img, (self.sz, self.sz))) boxes = self.yolov3_tiny.predict_img(img, conf_thresh=0.7)[0] inputs = find_person(boxes, self.previous_input) inputs = np.squeeze(self.process_inputs(inputs)) self.previous_input = inputs except TypeError: inputs = self.previous_input output = self.action_max * self.forward(inputs) actions = np.stack([*output.data.cpu().numpy().squeeze(), 0, 0, 0, 0], axis=-1) return Action(actor_name="drone_tracking_agent", value=actions)
def get_action(self, inputs, train: bool = False, agent_id: int = -1) -> Action: inputs = self.process_inputs(inputs) if agent_id == 0: output = self.sample(inputs, train=train).clamp(min=self.action_min, max=self.action_max) actions = np.stack([ *output.data.cpu().numpy().squeeze(), *get_slow_run(inputs.squeeze()) ], axis=-1) elif agent_id == 1: output = self.sample(inputs, train=train, adversarial=True).clamp(min=self.action_min, max=self.action_max) actions = np.stack([ *get_slow_hunt(inputs.squeeze()), *output.data.cpu().numpy().squeeze() ], axis=-1) else: output = self.sample(inputs, train=train, adversarial=False).clamp(min=self.action_min, max=self.action_max) adversarial_output = self.sample(inputs, train=train, adversarial=True).clamp( min=self.action_min, max=self.action_max) actions = np.stack([ *output.data.cpu().numpy().squeeze(), *adversarial_output.data.cpu().numpy().squeeze() ], axis=-1) return Action( actor_name=get_filename_without_extension( __file__), # assume output [1, 2] so no batch! value=actions)
def _set_field(self, msg: Union[String, Twist, RosReward, CommonCommonStateBatteryStateChanged], args: Tuple) -> None: field_name, sensor_stats = args if field_name == 'fsm_state': self._fsm_state = FsmState[msg.data] elif field_name == 'action': self._action = Action(actor_name='applied_action', value=process_twist(msg).value) elif field_name == 'reward': self._reward = msg.reward self._terminal_state = TerminationType[msg.termination] elif field_name == 'waypoint': self._waypoint = np.asarray(msg.data) elif field_name == 'battery': self._battery = msg.percent else: raise NotImplementedError cprint(f'set field {field_name}', self._logger, msg_type=MessageType.debug)
def _set_field(self, msg, args: Tuple) -> None: field_name, sensor_stats = args if field_name == "fsm_state": self.fsm_state = FsmState[msg.data] elif field_name == "observation": msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg)) self.observation = eval(f"process_{msg_type}(msg, sensor_stats)") elif field_name == "action": self.action = Action( actor_name=self._config.ros_config.action_topic, value=process_twist(msg).value, ) elif field_name == "reward": self.reward = msg.reward self.terminal_state = TerminationType[msg.termination] elif field_name.startswith("info:"): info_key = field_name[5:] msg_type = camelcase_to_snake_format(ros_message_to_type_str(msg)) self.info[info_key] = eval(f"process_{msg_type}(msg, sensor_stats)") else: raise NotImplementedError(f"{field_name}: {msg}") cprint(f"set field {field_name}", self._logger, msg_type=MessageType.debug)
def get_action(self, inputs, train: bool = False, agent_id: int = -1) -> Action: inputs = self.process_inputs(inputs) if agent_id == 0: # tracking agent ==> tracking_linear_y output = self.sample(inputs, train=train).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [0, *output.data.cpu().numpy().squeeze(), 0, 0, 0, 0, 0, 0]) elif agent_id == 1: # fleeing agent ==> fleeing_linear_y output = self.sample(inputs, train=train, adversarial=True).clamp(min=self.action_min, max=self.action_max) actions = np.stack( [0, 0, 0, 0, *output.data.cpu().numpy().squeeze(), 0, 0, 0]) else: output = self.sample(inputs, train=train, adversarial=False).clamp(min=self.action_min, max=self.action_max) adversarial_output = self.sample(inputs, train=train, adversarial=True).clamp( min=self.action_min, max=self.action_max) # actions = np.stack([0, output.data.cpu().numpy().squeeze().item(), 0, # 0, adversarial_output.data.cpu().numpy().squeeze().item(), 0, # 0, 0], axis=-1) actions = np.stack([0, 1, 0, 0, 1, 0, 0, 0], axis=-1) # actions = self.adjust_height(inputs, actions) Not necessary, hector quadrotor controller keeps altitude fixed actions = clip_action_according_to_playfield_size( inputs.detach().numpy().squeeze(), actions, self._playfield_size) return Action( actor_name= "tracking_fleeing_agent", # assume output [1, 8] so no batch! value=actions)
def get_action(self, inputs, train: bool = False) -> Action: output = self._policy_distribution(inputs, train).sample().item() return Action( actor_name=get_filename_without_extension(__file__), value=self.discrete_action_mapper.index_to_tensor(output))
reference_pos = set_random_cone_location(WORLD) elif TARGET == "gate": reference_pos = set_random_gate_location() elif TARGET == "line": reference_pos = spawn_line(WORLD) else: raise NotImplementedError step_index = 0 experience, observation = environment.reset() while experience.done == TerminationType.NotDone: tensor = torch.from_numpy(observation).permute( 2, 0, 1).float().unsqueeze(0) output = model(tensor).detach().cpu().numpy().squeeze() experience, observation = environment.step( action=Action(value=np.asarray( [output[0], output[1], output[2], 0, 0, output[3]])) if DS_TASK == "velocities" else None, waypoint=output if DS_TASK == "waypoints" else None, ) loss = save(reference_pos, experience, json_data, hdf5_data, prediction=output) if loss is not None: losses.append(loss) step_index += 1 successes.append(experience.done == TerminationType.Success) if TARGET == "line": remove_line()
def get_action(self, inputs, train: bool = False) -> Action: inputs = self.process_inputs(inputs=inputs) output = self.forward(inputs) return Action(actor_name=get_filename_without_extension(__file__), value=output.data)
def get_random_action(self) -> Action: return Action(actor_name='random', value=np.asarray(self._gym.action_space.sample()))
def get_action(self, inputs, train: bool = False) -> Action: output = self._policy_distribution(inputs, train).sample() return Action(actor_name=get_filename_without_extension(__file__), value=output.item())