def main():
global block_t, starfish_t
cv2.namedWindow('image')
perception_obj = Perception()
# Make trackbars, with default values above
cv2.createTrackbar('Block Threshold', 'image', block_t, 255, nothing)
cv2.createTrackbar('Starfish Threshold', 'image', starfish_t, 255, nothing)
print(
"Showing video. With the CV window in focus, press q to exit, p to pause."
)
while (1):
# get current positions of trackbars
block_t = cv2.getTrackbarPos('Block Threshold', 'image')
starfish_t = cv2.getTrackbarPos('Starfish Threshold', 'image')
perception_obj.block_t = block_t
perception_obj.starfish_t = starfish_t
scene = perception_obj.get_all_targets()
img = perception_obj.frame
img = label_scene(img, scene)
if img is not None:
cv2.imshow('image', img)
key = cv2.waitKey(66) # Delay for 66 ms
if key == ord('q'): # Press q to exit, p to pause
break
if key == ord('p'):
cv2.waitKey(-1) #wait until any key is pressed
cv2.destroyAllWindows()
def __init__(self):
"""
Load your agent here and initialize anything needed
WARNING: any path to files you wish to access on the docker should be ABSOLUTE PATHS
"""
# functional modules
self.perception = Perception(
self
) # generate high-level perception based on current observation
self.strategy = Strategy(
self
) # implement strategy to chase the food given the high-level perception
self.action_state_machine = ActionStateMachine(
self) # use a finite state machine to help implement the strategy
self.chaser = Chaser(self) # responsible for chase the food
# primitive perception
self.obs_visual = None # visual input in RGB space, float numpy array of shape (84, 84, 3)
self.obs_visual_hsv = None # visual input in HSV space, float numpy array of shape (84, 84, 3)
self.obs_vector = None # speed vector (left-right, up-down, forward-backward), float numpy array of shape (3,)
self.done = None # if the current test is done, bool
self.reward = None # current reward from the env, float
self.info = None # the brainInfo object from the env
self.t = None # how long will this test last? int
self.step_n = None # the current time step, int
# high-level perceptions
# Thought the environment is 3D, all the representation here is in the 2D plane of images.
self.is_green = None # bool numpy array of shape (84, 84), to indicate if each pixel is green (food color)
self.is_brown = None # bool numpy array of shape (84, 84), to indicate if each pixel is brown (food color)
self.is_red = None # bool numpy array of shape (84, 84), to indicate if each pixel is red (danger color)
self.is_blue = None # bool numpy array of shape (84, 84), to indicate if each pixel is blue (sky color)
self.is_gray = None # bool numpy array of shape (84, 84), to indicate if each pixel is gray (wall color)
self.target_color = None # the color is currently looking for, either brown or green
self.is_inaccessible = None # bool numpy array of shape (84, 84), to indicate if each pixel is inaccessible
# because it might be a wall pixel, sky pixel or a dangerous pixel.
self.is_inaccessible_masked = None # bool numpy array of shape (84, 84), is_inaccessible
# without the pixels on the four sides set to be accessible forcefully.
self.reachable_target_idx = None # int numpy array of (2,), to indicate where the target is in the visual input
self.reachable_target_size = None # int, to indicate how many pixels in the target
self.nearest_inaccessible_idx = None # int numpy array of (2,), to indicate the nearest inaccessible pixel
# memory use queues as memory to save the previous observation
self.is_green_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.is_brown_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.is_red_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.vector_memory = queue.Queue(maxsize=AgentConstants.memory_size)
# the action that will be returned to the env
self.current_action = None
# strategy-related variables
self.pirouette_step_n = None # int, counting the how long the agent has remain static and rotating
self.target_color = None # the color that the agent is looking for, either "green" or "brown"
self.exploratory_direction = None # int, if nowhere to go, go in this direction
self.not_seeing_target_step_n = None # int, how long the agent has lost the visual of a target
self.chase_failed = None # bool, if it failed to reach a target because path planning failed
self.search_direction = None # either left or right, to start searching for the target
self.static_step_n = None # int, how long the agent's position hasn't changed
def __init__(self, env):
super(SafetyEnvelope, self).__init__(env)
# Grab configuration
self.config = cg.Configuration.grab()
# Action proposed by the agent
self.propsed_action = None
# Action proposed by the monitor
self.shaped_action = None
# List of all monitors with their states, rewards and unsafe-actions
self.meta_monitor = []
# Dictionary that gets populated with information by all the monitors at runtime
self.monitor_states = {}
# Perceptions of the agent, it gets updated at each step with the current observations
self.perception = Perception(env.gen_obs_decoded())
# Set rewards
self.step_reward = self.config.rewards.standard.step
self.goal_reward = self.config.rewards.standard.goal
self.death_reward = self.config.rewards.standard.death
def __init__(self, params):
Perception.__init__(self)
self.lexicon = []
self.lxc = AssociativeMatrix()
self.stm = params['stimulus']
self.delta_inc = params['delta_inc']
self.delta_dec = params['delta_dec']
self.delta_inh = params['delta_inh']
self.discriminative_threshold = params['discriminative_threshold']
self.alpha = params['alpha'] # forgetting
self.beta = params['beta'] # learning rate
self.super_alpha = params['super_alpha']
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--player", help="player 1-6", type=int, default=1)
args = parser.parse_args()
config = Config(CONFIG_FILE, args.player)
comms_config = {
'rx_ip': config.client_ip,
'rx_port': config.client_port,
'tx_ip': config.sim_ip,
'tx_port': config.sim_port
}
print("Rx at {}:{}".format(comms_config["rx_ip"], comms_config["rx_port"]))
print("Tx to {}:{}".format(comms_config["tx_ip"], comms_config["tx_port"]))
commands_mutex = Lock()
# Launch comms in background thread
comms = CommsThread(comms_config, False, commands_mutex)
comms.daemon = True
comms.start()
# Launch perception, motion planning, and controls in main thread
sweep_builder = SweepBuilder()
perception = Perception(config)
planning = Planning(config)
controls = Controls(config)
visualize = Visualize(config)
try:
while True:
vehicle_state = sweep_builder.run(comms.get_vehicle_states())
if vehicle_state is not None:
t1 = time.time()
world_state = perception.run(vehicle_state)
plan = planning.run(world_state)
vehicle_commands = controls.run(plan)
t2 = time.time()
freq = 1 / (t2 - t1)
print(f"Running at {freq} Hz")
vehicle_commands['draw'] = visualize.run(world_state, plan)
with commands_mutex:
# hold the lock to prevent the Comms thread from
# sending the commands dict while we're modifying it
comms.vehicle_commands.update(vehicle_commands)
except KeyboardInterrupt:
pass
def __init__(self, config):
self.config = config
self._nav_pose_list, self._nav_pose_dict = get_nav_pose(
self.config.poses_file_path)
self._memory = Memory(self)
self._ear = Ear(self)
self._leg = Leg(self)
self._arm = Arm(self)
self._perception = Perception(self)
self._mouth = Mouth(self)
self.last_poses = []
self.find_obj_times = defaultdict(int)
def __init__(self):
rospy.init_node('action')
# Fetch pre-built action matrix. This is a 2d numpy array where row indexes
# correspond to the starting state and column indexes are the next states.
#
# A value of -1 indicates that it is not possible to get to the next state
# from the starting state. Values 0-9 correspond to what action is needed
# to go to the next state.
#
# e.g. self.action_matrix[0][12] = 5
self.action_matrix = np.loadtxt(path_prefix + "action_matrix.txt")
# Fetch actions. These are the only 9 possible actions the system can take.
# self.actions is an array of dictionaries where the row index corresponds
# to the action number, and the value has the following form:
# { dumbbell: "red", block: 1}
colors = ["red", "green", "blue"]
self.actions = np.loadtxt(path_prefix + "actions.txt")
self.actions = list(
map(lambda x: {
"dumbbell": colors[int(x[0])],
"block": int(x[1])
}, self.actions))
# Load converged q_matrix from output file in 2-D array format
self.q_matrix_path = os.path.join(os.path.dirname(__file__),
'../output/q_matrix.csv')
self.q_matrix = np.loadtxt(self.q_matrix_path, delimiter=',')
# Use converged q_matrix to find optimal sequence of actions
self.action_seq = []
self.get_actions()
# Identifies current action from 0-2, transition occurs when dumbbell is placed
self.curr_action = 0
# Identifies distance of closest object within a 10 degree arc in front of the robot
self.distance = 100
# Bring in tools from other libraries/files
self.p = Perception()
self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.scan_sub = rospy.Subscriber("/scan", LaserScan,
self.scan_distance)
self.move_group_arm = moveit_commander.MoveGroupCommander("arm")
self.move_group_gripper = moveit_commander.MoveGroupCommander(
"gripper")
# Record state of arm (lowered or raised)
self.arm_dropped = True
def init_perception(self):
"""Instantiates all perceptual modules.
Each perceptual module should have a perceive method that is
called by the base agent event loop.
"""
if not hasattr(self, "perception_modules"):
self.perception_modules = {}
self.perception_modules["self"] = SelfPerception(self)
self.perception_modules["vision"] = Perception(self, self.opts.perception_model_dir)
async def on_start(self):
self.terminate = False
self.SUCCESS = False
self.verbose = False
# Initialization
self.beliefs = WorldModel() # B := B0; Initial Beliefs
self.goals = self.beliefs.current_world_model.goals
self.intentions = self.beliefs.current_world_model.goals # I := I0; Initial Intentions
self.htn_planner = HierarchicalTaskNetworkPlanner(self.beliefs)
self.perception = Perception(self.beliefs)
self.coordination = Coordination(self.beliefs)
self.monitoring = Monitoring()
self.what, self.why, self.how_well, self.what_else, self.why_failed = "", "", "", "", ""
self.plans = []
self.selected_plan = []
self.percept = {}
self.action = ""
self.start_time = datetime.datetime.now()
async def on_start(self):
self.terminate = False
self.SUCCESS = False
self.verbose = False
# Initialization
self.htn_planner = HierarchicalTaskNetworkPlanner()
self.goal = [('transfer_target_object_to_container', 'arm',
'target_object', 'table', 'container')]
self.intentions = self.goal # I := I0; Initial Intentions
self.beliefs = WorldModel() # B := B0; Initial Beliefs
self.monitoring = Monitoring()
self.perception = Perception()
self.coordination = Coordination()
# Disable all 3 coordination switches for testing
self.coordination.control.send_requests = True
self.coordination.control.center_init = False
self.coordination.control.detect_last_position = True
self.what, self.why, self.how_well, self.what_else, self.why_failed = "", "", "", "", ""
self.plans = []
self.start_time = datetime.datetime.now()
def __init__(self, parser, perception_func):
self.parser = parser
self.perception = Perception(perception_func)
self.pddl = self.parser
self.goal_tracking = GoalTracking(self.parser, self.perception)
self.problem_generator = ProblemGenerator(self.perception, self.parser,
"tmp_problem_generation")
self.valid_actions = TrackedSuccessorValidActions(
self.pddl, self.problem_generator, self.goal_tracking)
self.on_action_observers = [
self.goal_tracking.on_action, self.valid_actions.on_action,
self.perception.on_action
]
def drive():
camera = request.args.get("camera")
model = request.args.get("model")
record = request.args.get("record")
recordings_dir = None
if record is not None:
processor = StoreFileProcessor()
automatic_controller.processors.append(processor)
recordings_dir = processor.output_folder
global perception
if perception is not None:
perception.shutdown()
perception = Perception(camera, model)
automatic_controller.start(perception)
return render_template('drive.html', recordings_dir=recordings_dir)
def __init__(self, parser, perception_func, planner=None):
self.parser = parser
self.planner = planner or make_plan
self.perception = Perception(perception_func)
self.pddl = self.parser
self.goal_tracking = GoalTracking(self.parser, self.perception)
self.problem_generator = ProblemGenerator(self.perception, self.parser,
"tmp_problem_generation")
self.valid_actions = ValidActions(self.pddl, self.perception,
self.problem_generator,
self.goal_tracking)
self.on_action_observers = [
self.goal_tracking.on_action, self.valid_actions.on_action,
self.perception.on_action
]
class TestRun(unittest.TestCase):
def setUp(self):
self.config = Mock()
self.config.field_elements = [Polygon(make_square_vertices(side_length=2, center=(-5, -5)))]
self.config.ball_radius = BALL_RADIUS
self.perception = Perception(self.config)
def test_run(self):
obstacle = make_linear_vertices(start=(5,5), end=(6,6), num_pts=20)
ball = make_circular_vertices(radius=BALL_RADIUS, center=(1,1), num_pts=8)
lidar_sweep = list()
lidar_sweep.extend(obstacle)
lidar_sweep.extend(ball)
x = 0
y = 0
theta = 0
ingested_balls = 0
vehicle_state = {
'x': x,
'y': y,
'theta': theta,
'lidarSweep': np.array(lidar_sweep),
'numIngestedBalls': ingested_balls
}
world_state = self.perception.run(vehicle_state)
actual_balls = world_state['balls']
actual_first_ball = actual_balls[0]
actual_others = world_state['obstacles']
expected_balls = [(1, 1)]
expected_others = [((5.0, 5.0), (5.95, 5.95))]
expected_first_ball = expected_balls[0]
self.assertEqual(len(expected_balls), len(actual_balls))
self.assertAlmostEqual(expected_first_ball[0], actual_first_ball[0])
self.assertAlmostEqual(expected_first_ball[1], actual_first_ball[1])
self.assertEqual(expected_others, actual_others)
class TestSegmentation(unittest.TestCase):
def setUp(self):
self.config = Mock()
self.config.field_elements = [Polygon(make_square_vertices(side_length=1, center=(0,0)))]
self.config.outer_wall = Polygon(make_square_vertices(side_length=2, center=(0,0)))
self.perception = Perception(self.config)
def test_subtract_background(self):
vehicle_state = {
'lidarSweep': [[0.75, 0.75], [1, 1]]
}
self.perception.subtract_background(vehicle_state)
actual = vehicle_state['lidarSweepMask']
expected = np.array([True, False], dtype=bool)
np.testing.assert_array_equal(expected, actual)
def test_clustering_with_3n_distant_points_produces_n_clusters(self):
vehicle_state = {
'lidarSweep': np.array([[0, 0], [0, 0], [0, 0], [1, 0], [1, 0], [1, 0], [2, 0], [2, 0], [2, 0]]),
'lidarSweepMask': np.ones(shape=(9,), dtype=bool)
}
self.perception.cluster(vehicle_state)
expected = 3
actual = len(vehicle_state['clusters'])
self.assertEqual(expected, actual)
def test_clustering_with_close_points_produces_one_cluster(self):
vehicle_state = {
'lidarSweep': np.array([[0, 0], [0, 0], [0, 0], [0.1, 0], [0.1, 0], [0.1, 0], [0.2, 0], [0.2, 0], [0.2, 0]]),
'lidarSweepMask': np.ones(shape=(9,), dtype=bool)
}
self.perception.cluster(vehicle_state)
expected = 1
actual = len(vehicle_state['clusters'])
self.assertEqual(expected, actual)
class TestClassification(unittest.TestCase):
def setUp(self):
self.config = Mock()
self.config.field_elements = [Polygon(make_square_vertices(side_length=2, center=(0,0)))]
self.config.ball_radius = BALL_RADIUS
self.perception = Perception(self.config)
def test_classification_of_circular_points_right_size(self):
vehicle_state = {
'clusters': [np.array(make_circular_vertices(radius=BALL_RADIUS, center=(0,0), num_pts=6))]
}
self.perception.classify(vehicle_state)
self.assertEqual(len(vehicle_state['balls']), 1)
self.assertEqual(len(vehicle_state['obstacles']), 0)
def test_classification_of_circular_points_wrong_size(self):
vehicle_state = {
'clusters': [np.array(make_circular_vertices(radius=BALL_RADIUS+0.1, center=(0,0), num_pts=6))]
}
self.perception.classify(vehicle_state)
self.assertEqual(len(vehicle_state['balls']), 0)
self.assertEqual(len(vehicle_state['obstacles']), 1)
def test_classification_of_non_circular_points(self):
vehicle_state = {
'clusters': [np.array(make_linear_vertices(start=[0, 0], end=[10, 10], num_pts=10))]
}
self.perception.classify(vehicle_state)
self.assertEqual(len(vehicle_state['balls']), 0)
self.assertEqual(len(vehicle_state['obstacles']), 1)
def __init__(self, input_num): Perception.__init__(self, input_num, f)
player_won = None
if human_player:
game = Game(MonteCarloStrategy, AsyncHumanStrategy, selfstate=False)
human_player = game.player_2
human_player.user_input = last_input
else:
game = Game(MonteCarloStrategy, MinMaxStrategy, selfstate=False)
human_player = None
timestep = 0
while last_input != 7: # Human can quit by pressing 0
# do perception every 20th timestep - rendering is slow
if timestep % 20 == 0: # rendering period = 20*timestep = 20*0.015s = 0.3s
[rgb, depth] = S.getImageAndDepth()
grid = Perception.detect_grid_state(rgb, depth)
game.set_grid(grid)
#if human_player is not None:
# look for user input
usr_in = cv.waitKey(1)
for i in range(0, 8):
if usr_in == ord(str(i)):
last_input = 6 - (i - 1)
print("Input set: {}".format(last_input))
if human_player is not None: human_player.user_input = last_input
# give the program new sphere id and drop position if needed
# put new sphere on table if needed
if waiting_for_input != 0:
print("Waiting for input {}".format(waiting_for_input))
class Agent(object):
def __init__(self):
"""
Load your agent here and initialize anything needed
WARNING: any path to files you wish to access on the docker should be ABSOLUTE PATHS
"""
# functional modules
self.perception = Perception(
self
) # generate high-level perception based on current observation
self.strategy = Strategy(
self
) # implement strategy to chase the food given the high-level perception
self.action_state_machine = ActionStateMachine(
self) # use a finite state machine to help implement the strategy
self.chaser = Chaser(self) # responsible for chase the food
# primitive perception
self.obs_visual = None # visual input in RGB space, float numpy array of shape (84, 84, 3)
self.obs_visual_hsv = None # visual input in HSV space, float numpy array of shape (84, 84, 3)
self.obs_vector = None # speed vector (left-right, up-down, forward-backward), float numpy array of shape (3,)
self.done = None # if the current test is done, bool
self.reward = None # current reward from the env, float
self.info = None # the brainInfo object from the env
self.t = None # how long will this test last? int
self.step_n = None # the current time step, int
# high-level perceptions
# Thought the environment is 3D, all the representation here is in the 2D plane of images.
self.is_green = None # bool numpy array of shape (84, 84), to indicate if each pixel is green (food color)
self.is_brown = None # bool numpy array of shape (84, 84), to indicate if each pixel is brown (food color)
self.is_red = None # bool numpy array of shape (84, 84), to indicate if each pixel is red (danger color)
self.is_blue = None # bool numpy array of shape (84, 84), to indicate if each pixel is blue (sky color)
self.is_gray = None # bool numpy array of shape (84, 84), to indicate if each pixel is gray (wall color)
self.target_color = None # the color is currently looking for, either brown or green
self.is_inaccessible = None # bool numpy array of shape (84, 84), to indicate if each pixel is inaccessible
# because it might be a wall pixel, sky pixel or a dangerous pixel.
self.is_inaccessible_masked = None # bool numpy array of shape (84, 84), is_inaccessible
# without the pixels on the four sides set to be accessible forcefully.
self.reachable_target_idx = None # int numpy array of (2,), to indicate where the target is in the visual input
self.reachable_target_size = None # int, to indicate how many pixels in the target
self.nearest_inaccessible_idx = None # int numpy array of (2,), to indicate the nearest inaccessible pixel
# memory use queues as memory to save the previous observation
self.is_green_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.is_brown_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.is_red_memory = queue.Queue(maxsize=AgentConstants.memory_size)
self.vector_memory = queue.Queue(maxsize=AgentConstants.memory_size)
# the action that will be returned to the env
self.current_action = None
# strategy-related variables
self.pirouette_step_n = None # int, counting the how long the agent has remain static and rotating
self.target_color = None # the color that the agent is looking for, either "green" or "brown"
self.exploratory_direction = None # int, if nowhere to go, go in this direction
self.not_seeing_target_step_n = None # int, how long the agent has lost the visual of a target
self.chase_failed = None # bool, if it failed to reach a target because path planning failed
self.search_direction = None # either left or right, to start searching for the target
self.static_step_n = None # int, how long the agent's position hasn't changed
# TODO self.visual_imagery reconstruct mental imagery from primitive perception
def reset(self, t=250):
"""
Reset is called before each episode begins
Leave blank if nothing needs to happen there
:param t the number of timesteps in the episode
"""
# functional modules
self.action_state_machine.reset()
self.strategy.reset()
self.perception.reset()
self.chaser.reset()
# primitive perception
self.obs_visual = None
self.obs_vector = None
self.obs_visual_hsv = None
self.done = None
self.reward = None
self.info = None
self.t = t
self.step_n = 0
# high-level perceptions
self.is_green = None
self.is_brown = None
self.is_red = None
self.is_blue = None
self.is_gray = None
self.target_color = None
self.is_inaccessible = None
self.is_inaccessible_masked = None
self.reachable_target_idx = None
self.reachable_target_size = None
self.nearest_inaccessible_idx = None
# memory
self.is_green_memory.queue.clear()
self.is_brown_memory.queue.clear()
self.is_red_memory.queue.clear()
self.vector_memory.queue.clear()
# output action
self.current_action = None
# strategy-related variables
self.pirouette_step_n = 0
self.target_color = "brown" # start with the non-terminating food
self.exploratory_direction = None
self.not_seeing_target_step_n = None
self.chase_failed = None
self.search_direction = AgentConstants.left # start with the left to search
self.static_step_n = 0
def step(self, obs, reward, done, info):
"""
A single step the agent should take based on the current state of the environment
We will run the Gym environment (AnimalAIEnv) and pass the arguments returned by env.step() to
the agent.
Note that should if you prefer using the BrainInfo object that is usually returned by the Unity
environment, it can be accessed from info['brain_info'].
:param obs: agent's observation of the current environment
:param reward: amount of reward returned after previous action
:param done: whether the episode has ended.
:param info: contains auxiliary diagnostic information, including eBrainInfo.
:return: the action to take, a list or size 2
"""
# set primitive observations
self.obs_visual, self.obs_vector = obs
self.obs_visual_hsv = rgb2hsv(self.obs_visual)
self.done = done
self.reward = reward
self.info = info
# set high-level observations
self.perception.perceive()
# set action by running strategy
self.strategy.run_strategy()
# return the action
return self.current_action
class BDIBehaviour(PeriodicBehaviour):
async def on_start(self):
self.terminate = False
self.SUCCESS = False
self.verbose = False
# Initialization
self.htn_planner = HierarchicalTaskNetworkPlanner()
self.goal = [('transfer_target_object_to_container', 'arm',
'target_object', 'table', 'container')]
self.intentions = self.goal # I := I0; Initial Intentions
self.beliefs = WorldModel() # B := B0; Initial Beliefs
self.monitoring = Monitoring()
self.perception = Perception()
self.coordination = Coordination()
# Disable all 3 coordination switches for testing
self.coordination.control.send_requests = True
self.coordination.control.center_init = False
self.coordination.control.detect_last_position = True
self.what, self.why, self.how_well, self.what_else, self.why_failed = "", "", "", "", ""
self.plans = []
self.start_time = datetime.datetime.now()
async def run(self):
# print(f"-- Sender Agent: PeriodicSenderBehaviour running at {datetime.datetime.now().time()}: {self.counter}")
print("run tick: {}".format(self.beliefs.current_world_model.tick))
# msg = Message(to="madks2@temp3rr0r-pc") # Instantiate the message
# msg.body = "Hello World: " + str(self.counter) # Set the message content
# await self.send(msg)
self.add_belief()
self.add_desire()
self.add_intention()
print("-- Sender Agent: Message sent!")
if self.beliefs.update_tick(
) > self.beliefs.current_world_model.max_ticks:
self.done()
self.kill()
else:
SUCCESS = True if self.intentions == "" else False
self.plans = self.htn_planner.get_plans(
self.beliefs.current_world_model,
self.intentions) # π := plan(B, I); MEANS_END REASONING
async def on_end(self):
print("-- on_end")
# stop agent from behaviour
await self.agent.stop()
# async def _step(self):
# # the bdi stuff
# pass
#
def add_belief(self):
print("-- Sender Agent: add_belief")
percept = self.perception.get_percept(text_engraving=(
self.why_failed,
self.how_well)) # get next percept ρ; OBSERVE the world
self.beliefs = self.perception.belief_revision(
self.beliefs, percept) # B:= brf(B, ρ);
self.beliefs = self.monitoring.fire_events(self.beliefs, percept)
def add_desire(self):
print("-- Sender Agent: add_desire")
def add_intention(self):
print("-- Sender Agent: add_intention")
self.intentions = self.deliberate(
self.beliefs, self.intentions
) # DELIBERATE about what INTENTION to achieve next
def done(self):
# the done evaluation
print("-- Sender Agent: Done")
def deliberate(self, current_beliefs, current_intentions):
"""
Decide WHAT sate of affairs you want to achieve.
:param current_beliefs: WordModel instance, the world model, information about the world.
:param current_intentions: List of tuples, tasks that the agent has COMMITTED to accomplish.
:return: List of tuples, filtered intentions that the agent COMMITS to accomplish.
"""
# 1. Option Generation: The agent generates a set of possible alternatives.
current_desires = current_intentions # TODO: D := option(B, I);
# 2. Filtering: Agent chooses between competing alternatives and commits to achieving them.
current_intentions = self.filter_intentions(
current_beliefs, current_desires,
current_intentions) # I := filter(B, D, I);
return current_intentions
def filter_intentions(self, current_beliefs, current_desires,
current_intentions):
"""
Choose between competing alternatives and COMMITTING to achieving them.
:param current_beliefs: WordModel instance, the world model, information about the world.
:param current_desires: List of tuples, tasks that the agent would like to accomplish.
:param current_intentions: List of tuples, tasks that the agent has COMMITTED to accomplish.
:return: List of tuples, filtered intentions that the agent COMMITS to accomplish.
"""
for current_intention in current_intentions:
if current_intention == (
'transfer_target_object_to_container', 'arm', 'target_object', 'table', 'container') \
and current_beliefs.current_world_model.location["target_object"] == "container":
current_intentions = "" # if goal(s) achieved, empty I
return current_intentions
stopwords = fileutils.get_stopwords(argv[2])
tfidf_generate_features.generate_features(argv[1], features_save_path,
dictionary_save_path,
stopwords)
print "Starting training .."
# get the names for the sets
set_names = fileutils.get_documents_by_path(features_save_path)
dictionary_size = fileutils.get_dictionary_size(dictionary_save_path +
"dictionary")
avg_precision, avg_recall, avg_f1 = 0, 0, 0
for i in xrange(0, len(set_names)):
p = Perception(0.25, dictionary_size)
print "=" * 10, "EPOCH", i + 1, "=" * 10
# get the sets for training and testing
training_set_names = set_names[:]
test_set_names = [training_set_names.pop(i)]
training_set = fileutils.get_data_set(features_save_path,
training_set_names,
dictionary_size)
test_set = fileutils.get_data_set(features_save_path, test_set_names,
dictionary_size)
print "Training sets:", training_set_names
print "Test set:", test_set_names
# -*- coding: utf-8 -*- """ @File : test02.py @Time : 12/9/20 7:56 PM @Author : Mingqiang Ning @Email : [email protected] @Modify Time @Version @Description ------------ -------- ----------- 12/9/20 7:56 PM 1.0 None # @Software: PyCharm """ import torch from perception import Perception net=Perception(2,3,2) # print(net) # for name, parameter in net.named_parameters(): # print(name,parameter) data=torch.randn(4,2) # print(data) out = net(data) print(out)
script for the final project logic main loop
"""
import sys
import numpy as np
sys.path.append('/home/pi/ArmPi/')
from paw_class import Paw
from perception import Perception, show_image, label_scene
import numpy as np
from ArmIK.Transform import convertCoordinate
import db_txt as db
if __name__ == '__main__':
count = 0
perception_obj = Perception()
paw_obj = Paw()
final_pos = (-15 + 0.5, 12 - 0.5, 1.5) # x block home
img_size = (640, 480)
while True:
# perception code
count = count + 1
#perception_obj.get_frame()
scene = perception_obj.get_all_targets()
img = perception_obj.frame
img = label_scene(img, scene)
if img is not None:
print(scene)
show_image(img)
class SafetyEnvelope(gym.core.Wrapper):
"""
Safety envelope for safe exploration.
Uses monitors for avoiding unsafe actions and shaping rewards
"""
def __init__(self, env):
super(SafetyEnvelope, self).__init__(env)
# Grab configuration
self.config = cg.Configuration.grab()
# Action proposed by the agent
self.propsed_action = None
# Action proposed by the monitor
self.shaped_action = None
# List of all monitors with their states, rewards and unsafe-actions
self.meta_monitor = []
# Dictionary that gets populated with information by all the monitors at runtime
self.monitor_states = {}
# Perceptions of the agent, it gets updated at each step with the current observations
self.perception = Perception(env.gen_obs_decoded())
# Set rewards
self.step_reward = self.config.rewards.standard.step
self.goal_reward = self.config.rewards.standard.goal
self.death_reward = self.config.rewards.standard.death
# Dictionary that contains all the type of monitors you can use
dict_monitors = {
'precedence': Precedence,
'response': Response,
'universality': Universality,
'absence': Absence
}
for monitor_types in self.config.monitors:
for monitors in monitor_types:
for monitor in monitors:
if monitor.active:
if hasattr(monitor, 'conditions'):
new_monitor = dict_monitors[monitor.type](
monitor.type + "_" + monitor.name,
monitor.conditions, self._on_monitoring,
monitor.rewards, self.perception,
monitor.context)
self.meta_monitor.append(new_monitor)
self.monitor_states[new_monitor.name] = {}
self.monitor_states[new_monitor.name]["state"] = ""
self.monitor_states[
new_monitor.name]["shaped_reward"] = 0
self.monitor_states[
new_monitor.name]["unsafe_action"] = ""
self.monitor_states[
new_monitor.name]["mode"] = monitor.mode
if hasattr(monitor, 'action_planner'):
self.monitor_states[new_monitor.name][
"action_planner"] = monitor.action_planner
else:
self.monitor_states[
new_monitor.name]["action_planner"] = "wait"
print("Active monitors:")
for monitor in self.meta_monitor:
print(monitor)
self._reset_monitors()
def _on_monitoring(self, name, state, **kwargs):
"""
Callback function called by the monitors
:param state: mismatch, violation
:param kwargs: in case of violation it returns a reward and the action causing the violation (unsafe_aciton)
:return: None
"""
# if self.monitor_states[name] == ""
self.monitor_states[name]["state"] = state
if state == "mismatch":
logging.error(
"%s mismatch between agent's observations and monitor state!",
name)
if state == "monitoring":
logging.info("%s is monitoring...", name)
if state == "shaping":
if kwargs:
shaped_reward = kwargs.get('shaped_reward', 0)
logging.info("%s is shaping... (shaped_reward = %s)", name,
str(shaped_reward))
self.monitor_states[name]["shaped_reward"] = shaped_reward
else:
logging.error(
"%s is in shaping error. missing action and reward", name)
if state == "violation":
if kwargs:
unsafe_action = kwargs.get('unsafe_action')
shaped_reward = kwargs.get('shaped_reward', 0)
self.monitor_states[name]["unsafe_action"] = unsafe_action
self.monitor_states[name]["shaped_reward"] = shaped_reward
#logging.warning("%s is in violation...(shaped_reward=%s, unsafe_action=%s)",
# name, str(shaped_reward), str(unsafe_action))
logging.info(
"%s is in violation...(shaped_reward=%s, unsafe_action=%s)",
name, str(shaped_reward), str(unsafe_action))
else:
logging.error(
"%s is in violation error. missing action and reward",
name)
def _action_planner(self, unsafe_actions):
"""
Return a suitable action that (that is not one of the 'unsafe_action')
:param unsafe_actions: list of actions that would bring one or more monitors in a fail state
:return: safe action proposed by the action planner or proposed action in case unsafe_actions is empty
"""
safe_action = None
if len(unsafe_actions) == 0:
safe_action = self.propsed_action
else:
for unsafe_action in unsafe_actions:
if unsafe_action[1] == "wait":
logging.info("action_planner() -> safe action : %s",
str(self.env.actions.done))
safe_action = self.env.actions.done
if unsafe_action[1] == "turn_right":
logging.info("action_planner() -> safe action : %s",
str(self.env.actions.right))
safe_action = self.env.actions.right
if unsafe_action[1] == "toggle":
logging.info("action_planner() -> safe action : %s",
str(self.env.actions.toggle))
safe_action = self.env.actions.toggle
if unsafe_action[1] == "turn_left":
logging.info("action_planner() -> safe action : %s",
str(self.env.actions.left))
safe_action = self.env.actions.left
if unsafe_action[1] == "forward":
logging.info("action_planner() -> safe action : %s",
str(self.env.actions.forward))
safe_action = self.env.actions.forward
return safe_action
def _reset_monitors(self):
"""
Reset all monitors initial state to avoid mismatch errors on environment reset
"""
for monitor in self.meta_monitor:
monitor.reset()
def step(self, proposed_action):
if self.config.debug_mode:
print("proposed_action = " +
self.env.action_to_string(proposed_action))
# To be returned to the agent
obs, reward, done, info = None, None, None, None
list_violations = []
self.propsed_action = proposed_action
self.perception.update(self.env.gen_obs_decoded())
current_obs_env = self.env
# Rendering
if self.config.a2c.num_processes == 1 and self.config.rendering:
self.env.render('human')
active_monitors = []
# Active the monitors according to the context:
for monitor in self.meta_monitor:
active = monitor.activate_contextually()
if active:
active_monitors.append(monitor)
for monitor in active_monitors:
monitor.check(current_obs_env, proposed_action)
# Check for unsafe actions before sending them to the environment:
unsafe_actions = []
shaped_rewards = []
for name, monitor in self.monitor_states.items():
if monitor["state"] == "violation" or monitor[
"state"] == "precond_violated" or monitor[
"state"] == "postcond_violated":
list_violations.append(name)
if "unsafe_action" in monitor:
# Add them only if the monitor is in enforcing mode
if monitor["mode"] == "enforcing":
unsafe_actions.append((monitor["unsafe_action"],
monitor["action_planner"]))
if self.config.debug_mode:
print("VIOLATION:\t" + name + "\tunsafe_action: " +
self.env.action_to_string(
monitor["unsafe_action"]) +
"\taction_planner: " +
monitor["action_planner"])
shaped_rewards.append(monitor["shaped_reward"])
# logging.info("unsafe actions = %s", unsafe_actions)
# Build action to send to the environment
suitable_action = self._action_planner(unsafe_actions)
# logging.info("actions possibles = %s", suitable_action)
# Send a suitable action to the environment
obs, reward, done, info = self.env.step(suitable_action)
if info:
for i in range(len(list_violations)):
info["event"].append("violation")
# logging.info("____verify AFTER action is applied to the environment")
# Notify the monitors of the new state reached in the environment and the applied action
for monitor in active_monitors:
monitor.verify(self.env, suitable_action)
# Get the shaped rewards from the monitors in the new state
shaped_rewards = []
for name, monitor in self.monitor_states.items():
shaped_rewards.append(monitor["shaped_reward"])
# Shape the reward at the cumulative sum of all the rewards from the monitors
reward += sum(shaped_rewards)
# Reset monitor rewards and actions
for name, monitor in self.monitor_states.items():
monitor["shaped_reward"] = 0
monitor["unsafe_action"] = ""
if done:
self._reset_monitors()
logging.info("\n\n\n")
return obs, reward, done, info
class BDIBehaviour(PeriodicBehaviour):
async def on_start(self):
self.terminate = False
self.SUCCESS = False
self.verbose = False
# Initialization
self.beliefs = WorldModel() # B := B0; Initial Beliefs
self.goals = self.beliefs.current_world_model.goals
self.intentions = self.beliefs.current_world_model.goals # I := I0; Initial Intentions
self.htn_planner = HierarchicalTaskNetworkPlanner(self.beliefs)
self.perception = Perception(self.beliefs)
self.coordination = Coordination(self.beliefs)
self.monitoring = Monitoring()
self.what, self.why, self.how_well, self.what_else, self.why_failed = "", "", "", "", ""
self.plans = []
self.selected_plan = []
self.percept = {}
self.action = ""
self.start_time = datetime.datetime.now()
async def run(self):
"""
Agent control loop version 7 (Intention Reconsideration)
I := I0; Initial Intentions
B := B0; Initial Beliefs
while true do
get next percept ρ; # OBSERVE the world
B:= brf(B, ρ); # Belief revision function
D: = option(B, I);
I := filter(B, D, I);
π := plan(B, I); # MEANS_END REASONING
while not (empty(π) or succeeded(Ι, Β) or impossible(I, B)) do # Drop impossible or succeeded intentions
α := hd(π); # Pop first action
execute(α);
π := tail(π);
get next percept ρ;
B:= brf(B, ρ);
if reconsider(I, B) then # Meta-level control: explicit decision, to avoid reconsideration cost
D := options(B, I);
I := filter(B, D, I);
end-if
if not sound(π, I, B) then # Re-activity, re-plan
π := plan(B, I);
end-if
end-while
end-while
"""
if self.verbose:
print(f"--- arm_agent: "
f"PeriodicSenderBehaviour running "
f"at {datetime.datetime.now().time()}: "
f"{self.beliefs.current_world_model.ticks}")
# while true do
if not self.SUCCESS and not self.terminate and self.beliefs.update_tick(
) < self.beliefs.current_world_model.max_ticks:
if len(self.selected_plan) == 0:
# msg = Message(to="madks2@temp3rr0r-pc") # Instantiate the message # TODO: place holder for IM
# msg.body = "Hello World: " + str(self.counter) # Set the message content
# await self.send(msg)
# TODO: engrave figures: arm IK, gripper
self.percept = self.perception.get_percept(
text_engraving=(self.why_failed, self.how_well)
) # get next percept ρ; OBSERVE the world
self.beliefs = self.perception.belief_revision(
self.beliefs, self.percept) # B:= brf(B, ρ);
self.beliefs = self.monitoring.fire_events(
self.beliefs, self.percept)
self.intentions = self.deliberate(
self.beliefs, self.intentions
) # DELIBERATE about what INTENTION to achieve next
self.SUCCESS = True if self.intentions == "" else False
self.plans = self.htn_planner.get_plans(
self.beliefs.current_world_model, self.intentions
) # π := plan(B, I); MEANS_END REASONING
if self.plans != False:
if len(self.plans) > 0:
self.plans.sort(
key=len
) # TODO: check why sorting doesn't work on "deeper" levels
if self.verbose:
print("{}: Plan: {}".format(
self.beliefs.current_world_model.tick,
self.plans[0]))
self.selected_plan = deque(
self.plans[0]
) # TODO: Use a "cost function" to evaluate the best plan, not shortest
self.why_failed = ""
else:
self.why_failed = self.htn_planner.failure_reason
if self.verbose:
print("Plan failure_reason: {}".format(
self.why_failed),
end=" ")
self.how_well = (
self.beliefs.current_world_model.tick,
self.beliefs.current_world_model.max_ticks,
int((datetime.datetime.now() -
self.start_time).total_seconds() *
1000), # milliseconds
self.plans)
if self.verbose:
print("how_well: {}".format(self.how_well))
else: # while not (empty(π) or succeeded(Ι, Β) or impossible(I, B)) do
self.action, self.selected_plan = self.selected_plan.popleft(
), self.selected_plan # α := hd(π); π := tail(π);
if self.verbose:
print("{}: Action: {}".format(
self.beliefs.current_world_model.tick,
self.action))
self.coordination.execute_action(
self.action,
self.beliefs.current_world_model) # execute(α);
self.what, self.why = self.action, self.intentions
self.how_well = (
self.beliefs.current_world_model.tick,
self.beliefs.current_world_model.max_ticks,
int((datetime.datetime.now() -
self.start_time).total_seconds() *
1000), # milliseconds
self.selected_plan)
self.what_else = self.plans[1] if len(
self.plans) > 1 else self.plans[0]
if self.verbose:
self.print_answers(self.what, self.why, self.how_well,
self.what_else)
# get next percept ρ; OBSERVE the world
# TODO: Don't update target object location, when it is obscured
if self.action != ('move_arm_above',
'target_object') and self.action != (
'move_arm', 'target_object'):
self.percept = self.perception.get_percept(
text_engraving=(self.what, self.why, self.how_well,
self.what_else))
self.beliefs = self.perception.belief_revision(
self.beliefs, self.percept)
self.beliefs = self.monitoring.fire_events(
self.beliefs, self.percept)
# TODO: trigger sound percept?
if self.action == ('initialize', 'arm'):
self.percept = {
"initialized": {
'arm': True
}
} # TODO: post conditions or monitoring
self.beliefs = self.perception.belief_revision(
self.beliefs,
self.percept) # TODO: post conditions
self.beliefs = self.monitoring.fire_events(
self.beliefs, self.percept)
elif self.action == ('move_arm', 'target_object'):
self.percept = {
"distance": {
'distance_to_gripper': 2.2
}
} # TODO: update with monitoring
self.beliefs = self.perception.belief_revision(
self.beliefs, self.percept)
self.beliefs = self.monitoring.fire_events(
self.beliefs, self.percept)
elif self.action == ('move_arm_above', 'container'):
self.percept = {
"location": {
"target_object": "container"
},
"grabbed": {
'target_object': False
}
}
self.beliefs = self.perception.belief_revision(
self.beliefs, self.percept)
self.beliefs = self.monitoring.fire_events(
self.beliefs, self.percept)
# if reconsider(I, B) then
# D := options(B, I);
# I := filter(B, D, I);
# if not sound(π, I, B) then
# π := plan(B, I)
# TODO: Backtracking of action?
else:
self.done()
self.kill()
async def on_end(self):
print("-- on_end")
print("Done.")
self.perception.destroy()
# stop agent from behaviour
await self.agent.stop()
# async def _step(self): # TODO: Need?
# # the bdi stuff
# pass
def done(self):
print("-- Sender Agent: Done")
show_history = True
print("Final World model:")
print("-- Ticks: {}".format(self.beliefs.current_world_model.tick))
print("-- initialized: {}".format(
self.beliefs.current_world_model.initialized))
print("-- location: {}".format(
self.beliefs.current_world_model.location))
print("-- grabbed: {}".format(
self.beliefs.current_world_model.grabbed))
if show_history:
print()
print("World model History:")
for tick in range(len(self.beliefs.world_model_history)):
print("Tick {}:".format(tick))
print("-- initialized: {}".format(
self.beliefs.world_model_history[tick].initialized))
print("-- location: {}".format(
self.beliefs.world_model_history[tick].location))
print("-- grabbed: {}".format(
self.beliefs.world_model_history[tick].grabbed))
print()
print("{}!".format("SUCCESS" if self.SUCCESS else "FAIL"))
def print_answers(self, what_answer, why_answer, how_well_answer,
what_else_answer):
print(
"Q: What is the robot doing? A: {}\n"
"Q: Why is it doing it? A: {}\n"
"Q: How well is it doing it? A: {}\n"
"Q: What else could it have been doing instead? A: {}".format(
what_answer, why_answer, how_well_answer,
what_else_answer))
def deliberate(self, current_beliefs, current_intentions):
"""
Decide WHAT sate of affairs you want to achieve.
:param current_beliefs: WordModel instance, the world model, information about the world.
:param current_intentions: List of tuples, tasks that the agent has COMMITTED to accomplish.
:return: List of tuples, filtered intentions that the agent COMMITS to accomplish.
"""
# 1. Option Generation: The agent generates a set of possible alternatives.
current_desires = current_intentions # TODO: D := option(B, I);
# 2. Filtering: Agent chooses between competing alternatives and commits to achieving them.
current_intentions = self.filter_intentions(
current_beliefs, current_desires,
current_intentions) # I := filter(B, D, I);
return current_intentions
def filter_intentions(self, current_beliefs, current_desires,
current_intentions):
"""
Choose between competing alternatives and COMMITTING to achieving them.
:param current_beliefs: WordModel instance, the world model, information about the world.
:param current_desires: List of tuples, tasks that the agent would like to accomplish.
:param current_intentions: List of tuples, tasks that the agent has COMMITTED to accomplish.
:return: List of tuples, filtered intentions that the agent COMMITS to accomplish.
"""
for current_intention in current_intentions:
if current_intention == self.goals[0] and current_beliefs.current_world_model.location["target_object"] \
== "container":
current_intentions = "" # if goal(s) achieved, empty I
return current_intentions
def __init__(self, env):
super(SafetyEnvelope, self).__init__(env)
# Grab configuration
self.config = cg.Configuration.grab()
# Action proposed by the agent
self.propsed_action = None
# Action proposed by the monitor
self.shaped_action = None
# List of all monitors with their states, rewards and unsafe-actions
self.meta_monitor = []
# Dictionary that gets populated with information by all the monitors at runtime
self.monitor_states = {}
# Perceptions of the agent, it gets updated at each step with the current observations
self.perception = Perception(env.gen_obs_decoded())
# Set rewards
self.step_reward = self.config.rewards.standard.step
self.goal_reward = self.config.rewards.standard.goal
self.death_reward = self.config.rewards.standard.death
# Dictionary that contains all the type of monitors you can use
dict_monitors = {
'precedence': Precedence,
'response': Response,
'universality': Universality,
'absence': Absence
}
for monitor_types in self.config.monitors:
for monitors in monitor_types:
for monitor in monitors:
if monitor.active:
if hasattr(monitor, 'conditions'):
new_monitor = dict_monitors[monitor.type](
monitor.type + "_" + monitor.name,
monitor.conditions, self._on_monitoring,
monitor.rewards, self.perception,
monitor.context)
self.meta_monitor.append(new_monitor)
self.monitor_states[new_monitor.name] = {}
self.monitor_states[new_monitor.name]["state"] = ""
self.monitor_states[
new_monitor.name]["shaped_reward"] = 0
self.monitor_states[
new_monitor.name]["unsafe_action"] = ""
self.monitor_states[
new_monitor.name]["mode"] = monitor.mode
if hasattr(monitor, 'action_planner'):
self.monitor_states[new_monitor.name][
"action_planner"] = monitor.action_planner
else:
self.monitor_states[
new_monitor.name]["action_planner"] = "wait"
print("Active monitors:")
for monitor in self.meta_monitor:
print(monitor)
self._reset_monitors()
class Action(object):
def __init__(self):
rospy.init_node('action')
# Fetch pre-built action matrix. This is a 2d numpy array where row indexes
# correspond to the starting state and column indexes are the next states.
#
# A value of -1 indicates that it is not possible to get to the next state
# from the starting state. Values 0-9 correspond to what action is needed
# to go to the next state.
#
# e.g. self.action_matrix[0][12] = 5
self.action_matrix = np.loadtxt(path_prefix + "action_matrix.txt")
# Fetch actions. These are the only 9 possible actions the system can take.
# self.actions is an array of dictionaries where the row index corresponds
# to the action number, and the value has the following form:
# { dumbbell: "red", block: 1}
colors = ["red", "green", "blue"]
self.actions = np.loadtxt(path_prefix + "actions.txt")
self.actions = list(
map(lambda x: {
"dumbbell": colors[int(x[0])],
"block": int(x[1])
}, self.actions))
# Load converged q_matrix from output file in 2-D array format
self.q_matrix_path = os.path.join(os.path.dirname(__file__),
'../output/q_matrix.csv')
self.q_matrix = np.loadtxt(self.q_matrix_path, delimiter=',')
# Use converged q_matrix to find optimal sequence of actions
self.action_seq = []
self.get_actions()
# Identifies current action from 0-2, transition occurs when dumbbell is placed
self.curr_action = 0
# Identifies distance of closest object within a 10 degree arc in front of the robot
self.distance = 100
# Bring in tools from other libraries/files
self.p = Perception()
self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
self.scan_sub = rospy.Subscriber("/scan", LaserScan,
self.scan_distance)
self.move_group_arm = moveit_commander.MoveGroupCommander("arm")
self.move_group_gripper = moveit_commander.MoveGroupCommander(
"gripper")
# Record state of arm (lowered or raised)
self.arm_dropped = True
def scan_distance(self, data):
# Updates self.distance to contain distance of nearest object in front of robot
dist = data.range_max
front = [355, 356, 357, 358, 359, 0, 1, 2, 3, 4]
# Find closest distance measurement from LiDAR sensor's `ranges` arg
for i in front:
if data.ranges[i] < dist:
dist = data.ranges[i]
self.distance = dist
def get_actions(self):
# Reads converged q_matrix to write optimal sequence of actions into actions[]
curr_state = 0
# Find 3 optimal actions to place 3 dumbbells
for i in range(3):
options = self.q_matrix[curr_state]
optimal_reward = 0
optimal_action = 0
# Find optimal action given the current state
for i in range(len(options)):
if options[i] > optimal_reward:
optimal_reward = options[i]
optimal_action = i
db = self.actions[optimal_action]["dumbbell"]
num = self.actions[optimal_action]["block"]
self.action_seq.append((db, str(num)))
# Update current state by searching through action matrix
curr_state = np.where(
self.action_matrix[curr_state] == optimal_action)[0][0]
def lift(self):
# Grasps dumbbell and raises it
self.move_group_gripper.go([0.004, 0.004], wait=True)
self.move_group_arm.go([0, -0.35, 0, 0], wait=True)
self.arm_dropped = False
def drop(self):
# Lowers dumbbell and releases it
self.move_group_arm.go([0, 0.7, 0, -0.85], wait=True)
self.move_group_gripper.go([0.015, 0.015], wait=True)
self.arm_dropped = True
def set_velocity(self, linear_vel, angular_vel):
# Helper to set robot velocity
msg = Twist()
msg.linear.x = linear_vel
msg.angular.z = angular_vel
self.cmd_pub.publish(msg)
def turn(self, dir):
# Helper to turn robot around after dumbbell picked up.
if dir == "right":
vel = -0.175
elif dir == "left":
vel = 0.25
else:
print("error: pass 'right' or 'left' direction to turn()")
return
self.set_velocity(0, vel)
rospy.sleep(8)
self.set_velocity(0, 0)
def do_db_action(self, db):
# Performs the given action by picking up dumbbell then placing
# it at the correct block number
# Grab image sensor data from function in `perception.py`
res = self.p.find_color(db)
# If dumbbell not in robot's field of view, rotate robot in-place and scan again.
if res == None:
self.set_velocity(0, 0.3)
rospy.sleep(0.5)
self.set_velocity(0, 0)
rospy.sleep(0.5)
self.do_db_action(db)
return
(cx, cy, h, w, d) = res
linear_vel = 0
angular_vel = 0
# If robot is within grabbing distance of target dumbbell,
# close gripper and lift up arm, then end function loop.
if self.distance <= .2:
self.set_velocity(linear_vel, angular_vel)
self.lift()
# If robot is almost within grabbing distance, lower arm and open grabber,
# slowly inch towards the dumbbell handle, and re-call this function.
elif self.distance <= .4:
if not self.arm_dropped:
self.set_velocity(linear_vel, angular_vel)
self.drop()
rospy.sleep(0.5)
# Adjust linear/angular velocities with proportional control
linear_vel = .25 * (self.distance - .2)
err = w / 2 - cx
angular_vel = min(.25, err * .003)
self.set_velocity(linear_vel, angular_vel)
rospy.sleep(0.5)
self.do_db_action(db)
# If robot is getting close to grabbing distance, move towards the dumbbell with
# proportional control, and re-call this function.
elif self.distance < 1:
err = w / 2 - cx
if (err < 5):
linear_vel = .25 * (self.distance - .2)
angular_vel = min(.25, err * 0.003)
self.set_velocity(linear_vel, angular_vel)
rospy.sleep(0.5)
self.do_db_action(db)
# If robot is far from grabbing distance, move towards the dumbbell with proportional
# control again, but it can travel a bit faster. Then re-call this function.
else:
err = w / 2 - cx
if (err < 15):
linear_vel = .25 * (self.distance - .2)
angular_vel = min(.25, err * 0.003)
self.set_velocity(linear_vel, angular_vel)
rospy.sleep(0.5)
self.do_db_action(db)
return
def do_block_action(self, num):
"""To be performed after dumbbell is picked up. Find the numbered block
for the respective dumbbell and navigate to it, then drop dumbbell."""
# Grab image sensor data from function in `perception.py`
res = self.p.find_number(num)
(cx, cy, h, w, d) = res
# Handle cases when target number was not found in robot's field of view.
# (this is encoded with a d = -1 that comes from `find_number` in `perception.py`)
if d == -1 and any([self.distance > 2, cx == 0]):
# Rotate robot in-place, and re-call this function to scan for number again.
if num == "1":
self.set_velocity(0, -0.3)
rospy.sleep(1)
else:
self.set_velocity(0, 0.3)
rospy.sleep(1)
self.set_velocity(0, 0)
rospy.sleep(0.5)
self.do_block_action(num)
return
linear_vel = 0
angular_vel = 0
# If robot is sufficiently close to target numbered block, drop dumbbell
# and end this function loop.
if self.distance <= .75:
self.set_velocity(linear_vel, angular_vel)
self.drop()
self.set_velocity(-0.25, 0)
rospy.sleep(2)
self.set_velocity(0, 0)
# When robot is getting close to target numbered block, the number will no
# longer be visible in the robot's field of view (it's too far above). In this case,
# we assume the robot is already pointed towards the correct numbered block. So just
# move the robot slowly forward using proportional control, and re-call this function.
elif self.distance <= 1.5:
err = w / 2 - cx
angular_vel = min(.25, err * .003)
linear_vel = 0.25 * (self.distance - 0.6)
self.set_velocity(linear_vel, angular_vel)
rospy.sleep(1.5)
self.set_velocity(0, 0)
rospy.sleep(0.5)
self.do_block_action(num)
# When the robot is far from target numbered block but it's within the image sensor data,
# move the robot forward at a constant velocity. Pause after two seconds and re-call this
# function.
# (Note that it's possible for the number to disappear from the robot's field
# of view as it moves forward. In this case, the robot will turn in-place until it catches
# the number again. This was handled in the conditional above.)
else:
self.set_velocity(0.2, angular_vel)
rospy.sleep(2)
self.set_velocity(0, 0)
rospy.sleep(0.5)
self.do_block_action(num)
return
def run(self):
""" Run rospy node."""
# Lift up arm/gripper upon init. This ensures that arm will not get in the way of
# robot's camera/LiDAR sensor.
self.lift()
# For each optimal dumbbell/numbered block pair, call the action sequences to pick up
# the specified dumbbell and drop it off at its respective numbered block.
for db, num in self.action_seq:
self.do_db_action(db)
if num == "1":
self.turn("right")
else:
self.turn("left")
self.do_block_action(num)
class SafetyEnvelope(gym.core.Wrapper):
"""
Safety envelope for safe exploration.
Uses monitors for avoiding unsafe actions and shaping rewards
"""
def __init__(self, env):
super(SafetyEnvelope, self).__init__(env)
# Grab configuration
self.config = cg.Configuration.grab()
# Action proposed by the agent
self.propsed_action = None
# Action proposed by the monitor
self.shaped_action = None
# List of all monitors with their states, rewards and unsafe-actions
self.meta_monitor = []
# Dictionary that gets populated with information by all the monitors at runtime
self.monitor_states = {}
# Perceptions of the agent, it gets updated at each step with the current observations
self.perception = Perception(env.gen_obs_decoded())
# Set rewards
self.step_reward = self.config.rewards.standard.step
self.goal_reward = self.config.rewards.standard.goal
self.death_reward = self.config.rewards.standard.death
def step(self, proposed_action):
if self.config.debug_mode: print("proposed_action = " + self.env.action_to_string(proposed_action))
self.perception.update(self.env.gen_obs_decoded())
# Rendering
if self.config.a2c.num_processes == 1 and self.config.rendering:
self.env.render('human')
n_violations = 0
shaped_reward = 0
safe_action = proposed_action
# Checking waterAbsence
if self.perception.is_condition_satisfied("stepping-on-water", proposed_action):
n_violations += 1
shaped_reward -= 0.1
safe_action = self.env.actions.done
# Checking lightUniversally
if not self.perception.is_condition_satisfied("light-on-current-room"):
n_violations += 1
shaped_reward -= 0.1
safe_action = self.env.actions.done
# Checking lightPrecedence
if (self.perception.is_condition_satisfied("entering-a-room", proposed_action)
and not self.perception.is_condition_satisfied("light-switch-turned-on")):
n_violations += 1
shaped_reward -= 0.1
safe_action = self.env.actions.right
# Checking openDoorResponse
if (self.perception.is_condition_satisfied("door-closed-in-front")
and proposed_action != self.env.actions.toggle):
n_violations += 1
shaped_reward -= 0.1
safe_action = self.env.actions.toggle
# Checking switchOffResponse
if (self.perception.is_condition_satisfied("light-switch-in-front-off")
and proposed_action != self.env.actions.toggle):
n_violations += 1
shaped_reward -= 0.1
safe_action = self.env.actions.toggle
# Send a suitable action to the environment
obs, reward, done, info = self.env.step(safe_action)
# Shape the reward at the cumulative sum of all the rewards from the monitors
reward += shaped_reward
for i in range(n_violations):
info["event"].append("violation")
return obs, reward, done, info
class Robot:
def __init__(self, config):
self.config = config
self._nav_pose_list, self._nav_pose_dict = get_nav_pose(
self.config.poses_file_path)
self._memory = Memory(self)
self._ear = Ear(self)
self._leg = Leg(self)
self._arm = Arm(self)
self._perception = Perception(self)
self._mouth = Mouth(self)
self.last_poses = []
self.find_obj_times = defaultdict(int)
def speak(self, msg):
self._mouth.speak_via_action(msg)
return True
def speak_with_wav(self, wav_path):
self._mouth.speak_with_wav(wav_path)
def nav_by_place_name(self, name):
assert type(name) == str
pose = self._nav_pose_dict[name]
return self._leg.move(pose)
def move(self, pose, frame_id="map"):
# return True if the robot arrived in time
return self._leg.move(pose, frame_id)
def self_intro(self):
self.speak_with_wav(self.config.self_intro_wav)
def speak_next_guest(self):
self.speak_with_wav(self.config.next_guest_wav)
def speak_body_down(self):
self.speak_with_wav(self.config.body_down_wav)
def speak_short_body_down(self):
self.speak_with_wav(self.config.short_body_down_wav)
def remember_job(self):
self.speak_short_body_down()
for i in range(self.config.facenet_each_person_face_num):
face = self._perception.get_face()
self._memory.add_face(face)
self.speak_with_wav(self.config.hint_speak_name_job_wav)
rospy.loginfo("[remember_job] start get job")
try:
job = self._ear.get_asr(self.config.asr_job_class)
except Exception as e:
print(e)
self.speak_with_wav(self.config.again_wav)
job = self._ear.get_asr(self.config.asr_job_class)
rospy.loginfo("[remember_job] finish get job!")
self._add_job_with_faces(job, self._memory.get_last_faces_by_config())
rospy.loginfo("[remember_job] job added!")
def broadcast_heard_job(self):
job = self._memory.get_last_job()
wav_path = self.config.broadcast_job_wav_path_format % (
job.people_name, job.obj_name)
self.speak_with_wav(wav_path)
def back(self):
pose = get_poses_by_base_link_xy(-0.3, 0)
return self._leg.move(pose)
def confirm_job(self):
job = self._memory.get_last_job()
wav_path = self.config.confirm_job_wav_path_format % (job.people_name,
job.obj_name)
self.speak_with_wav(wav_path)
try:
confirmed = self._ear.get_asr(
self.config.asr_confirm_class).confirmed
except Exception as e:
# TODO(lennon) just return True if asr get wrong xml?
print(e)
return True
if confirmed:
return True
else:
self._memory.delete_last_job()
self.speak_with_wav(self.config.fault_again_wav)
rospy.loginfo("[remember_job] start get job")
job = self._ear.get_asr(self.config.asr_job_class)
self._add_job_with_faces(job,
self._memory.get_last_faces_by_config())
rospy.loginfo("[remember_job] get job!")
# want cofirm until true?
self.confirm_job()
def find_obj_poses(self, obj_name):
"""
find obj poses by odom!
obj_name: str the object name in yolo config that we want to find
return: pose in the map or None
"""
boxes, raw_image = self._perception.get_obj(obj_name)
if boxes is None:
return None
if self.config.debug:
path = "".join([
self.config.debug_path, obj_name,
str(self.find_obj_times[obj_name]), '.jpg'
])
save_img_with_box(path, raw_image, obj_name, boxes)
self.find_obj_times[obj_name] += 1
rospy.loginfo("[find obj] %s", obj_name)
poses = get_poses(boxes, self.config.distance[obj_name])
return poses
def init_face_db(self):
imgs = []
for job in self._memory.get_jobs():
for face in job.get_faces():
imgs.append((face, job.people_name))
self._perception.init_face_db(imgs)
def recognize(self):
self.speak_short_body_down()
face = self._perception.get_face()
name = self._perception.identify(face)
if self._memory.exist_name(name):
job = self._memory.get_job_by_name(name)
wav_path = self.config.hello_job_wav_path_format % (
job.people_name, job.obj_name)
self.speak_with_wav(wav_path)
else:
self.speak_with_wav(self.config.stranger_wav)
def get_jobs(self):
return self._memory.get_jobs()
def filter_poses(self, poses):
xmin = self.config.xmin
ymin = self.config.ymin
xmax = self.config.xmax
ymax = self.config.ymax
return filter_pose_out_of_map(poses, xmin, ymin, xmax, ymax)
def get_place_list(self, place_name):
return sorted([
str(k) for k in self._nav_pose_dict.keys()
if k.startswith(place_name)
])
def debug(self):
for job in self._memory.get_jobs():
for i, face in enumerate(job.get_faces()):
path = "".join([
self.config.final_debug_path, job.people_name,
job.obj_name,
str(i), '.jpg'
])
save_img(path, face)
for job in self._memory.get_jobs():
job.debug()
def _add_job_with_faces(self, job, faces):
job.add_faces(faces)
if self.config.debug:
for face in faces:
dict_key = "".join([job.people_name, job.obj_name])
path = "".join([
self.config.debug_path, job.people_name, job.obj_name,
str(self.find_obj_times[dict_key]), '.jpg'
])
save_img(path, face)
self.find_obj_times[dict_key] += 1
self._memory.add_job(job)
# -*- coding: utf-8 -*-
# 调用Perception函数
import torch
from perception import Perception
perception = Perception(2, 3, 2)
for name, parameter in perception.named_parameters():
print(name, parameter)
data = torch.rand(4, 2)
output = perception(data)
print(output)
