Spatial map memory (facebookresearch#937)

* shallow refactor, move ExaminedDetection from global to member of memory

* forgot file

* skeleton of how maps should look in place_field

* fill out skeleton some

* forgot some fields

* fix bugs

* more functional place_field, first (incomplete) integration into craftassist agent

* remove obstacle from place_field map when all blocks in column removed in mc

* integrate in robot

* test scripts (facebookresearch#954)

* tests, bugfixes, change format of self.memid2locs

* bugfix

* grr

Co-authored-by: anuragprat1k <[email protected]>

Author: aszlam

droidlet/interpreter/robot/default_behaviors.py

@@ -4,37 +4,41 @@
 import logging
 from droidlet.interpreter.robot import tasks
 from droidlet.memory.robot.loco_memory_nodes import DetectedObjectNode
-from droidlet.lowlevel.robot_mover_utils import ExaminedMap
 import os
 import random
 import numpy as np
 import shutil
 import json
 
-random.seed(2021) # fixing a random seed to fix default exploration goal
+random.seed(2021)  # fixing a random seed to fix default exploration goal
+
 
 def get_distant_goal(x, y, t, l1_thresh=35):
     # Get a distant goal for the slam exploration
-    # Pick a random quadrant, get 
+    # Pick a random quadrant, get
     while True:
         xt = random.randint(-19, 19)
         yt = random.randint(-19, 19)
-        d = np.linalg.norm(np.asarray([x,y]) - np.asarray([xt,yt]), ord=1)
+        d = np.linalg.norm(np.asarray([x, y]) - np.asarray([xt, yt]), ord=1)
         if d > l1_thresh:
             return (xt, yt, 0)
 
+
 def init_logger():
-    logger = logging.getLogger('default_behavior')
+    logger = logging.getLogger("default_behavior")
     logger.setLevel(logging.INFO)
-    fh = logging.FileHandler('default_behavior.log', 'w')
+    fh = logging.FileHandler("default_behavior.log", "w")
     fh.setLevel(logging.INFO)
     ch = logging.StreamHandler()
     ch.setLevel(logging.INFO)
-    formatter = logging.Formatter('%(asctime)s %(filename)s:%(lineno)s - %(funcName)s(): %(message)s')
+    formatter = logging.Formatter(
+        "%(asctime)s %(filename)s:%(lineno)s - %(funcName)s(): %(message)s"
+    )
     fh.setFormatter(formatter)
     logger.addHandler(fh)
     logger.addHandler(ch)
 
+
 init_logger()
 
 #TODO: Move these utils to a suitable place - as a class method in TripleNode
@@ -66,46 +70,49 @@ def start_explore(agent, goal):
         agent.mover.slam.reset_map()
         agent.mover.nav.reset_explore()
 
-        logger = logging.getLogger('default_behavior')
+        logger = logging.getLogger("default_behavior")
         logger.info(
             f"Starting exploration {explore_count} \
             first_exploration_done {first_exploration_done} \
             os.getenv('CONTINUOUS_EXPLORE') {os.getenv('CONTINUOUS_EXPLORE', 'False')}"
         )
 
+        # FIXME, don't clear the memory, place_field, etc.  explore more reasonably
         # Clear memory
         objects = DetectedObjectNode.get_all(agent.memory)
-        logger.info(f'Clearing {len(objects)} memids in memory')
+        logger.info(f"Clearing {len(objects)} memids in memory")
         agent.memory.clear(objects)
-        ExaminedMap.clear()
+        agent.memory.place_field.clear_examined()
         # reset object id counter
         agent.perception_modules["vision"].vision.deduplicate.object_id_counter = 1
         objects = DetectedObjectNode.get_all(agent.memory)
-        logger.info(f'{len(objects)} memids in memory')
-        
-        task_data = { 
-            "goal": goal, 
-            "save_data": os.getenv('SAVE_EXPLORATION', 'False') == 'True',
+        logger.info(f"{len(objects)} memids in memory")
+
+        task_data = {
+            "goal": goal,
+            "save_data": os.getenv("SAVE_EXPLORATION", "False") == "True",
             "data_path": os.path.join(f"{os.getenv('HEURISTIC', 'default')}", str(explore_count)),
         }
-        logger.info(f'task_data {task_data}')
-        
-        if os.path.isdir(task_data['data_path']):
-            shutil.rmtree(task_data['data_path'])
+        logger.info(f"task_data {task_data}")
 
-        if os.getenv('HEURISTIC') in ('straightline', 'circle'):
-            logging.info('Default behavior: Curious Exploration')
+        if os.path.isdir(task_data["data_path"]):
+            shutil.rmtree(task_data["data_path"])
+
+        if os.getenv("HEURISTIC") in ("straightline", "circle"):
+            logging.info("Default behavior: Curious Exploration")
             agent.memory.task_stack_push(tasks.CuriousExplore(agent, task_data))
         else:
-            logging.info('Default behavior: Default Exploration')
+            logging.info("Default behavior: Default Exploration")
             agent.memory.task_stack_push(tasks.Explore(agent, task_data))
 
-        add_or_replace(agent, 'first_exploration_done', 'True')
-        add_or_replace(agent, 'explore_count', str(explore_count+1))
+        add_or_replace(agent, "first_exploration_done", "True")
+        add_or_replace(agent, "explore_count", str(explore_count + 1))
+
+
 
 def explore(agent):
-    x,y,t = agent.mover.get_base_pos()
-    goal = get_distant_goal(x,y,t)
+    x, y, t = agent.mover.get_base_pos()
+    goal = get_distant_goal(x, y, t)
     start_explore(agent, goal)
 
 def get_task_data(agent):
@@ -135,4 +142,4 @@ def reexplore(agent):
     task_data = get_task_data(agent)
     logging.info(f'task_data {task_data}')
     if task_data is not None:
-        agent.memory.task_stack_push(tasks.Reexplore(agent, task_data))
+        agent.memory.task_stack_push(tasks.Reexplore(agent, task_data))

droidlet/interpreter/robot/tasks.py

@@ -9,6 +9,7 @@
 import shutil
 import glob
 import sys
+
 if "/opt/ros/kinetic/lib/python2.7/dist-packages" in sys.path:
     sys.path.remove("/opt/ros/kinetic/lib/python2.7/dist-packages")
 import cv2
@@ -17,7 +18,7 @@
 from pathlib import Path
 import matplotlib.pyplot as plt
 
-#FIXME!  do this better
+# FIXME!  do this better
 from .hello_robot.rotation import yaw_pitch
 
 MAX_PAN_RAD = np.pi / 4
@@ -102,6 +103,7 @@ def get_move_target_for_point(base_pos, target, eps=0.5):
 
     return [targetx, targetz, yaw]
 
+
 def get_step_target_for_straightline_move(base_pos, target, step_size=0.1):
     """
     Heuristic to get step target of step_size for going to from base_pos to target
@@ -117,55 +119,64 @@ def get_step_target_for_straightline_move(base_pos, target, step_size=0.1):
     dx = target[0] - base_pos[0]
     dz = target[2] - base_pos[1]
 
-    if dx == 0: # vertical line 
+    if dx == 0:  # vertical line
         theta = math.radians(90)
     else:
-        theta = math.atan(abs(dz/dx))
-    
+        theta = math.atan(abs(dz / dx))
+
     signx = 1 if dx >= 0 else -1
     signz = 1 if dz >= 0 else -1
-    
+
     targetx = base_pos[0] + signx * step_size * math.cos(theta)
     targetz = base_pos[1] + signz * step_size * math.sin(theta)
 
     yaw, _ = get_camera_angles([targetx, CAMERA_HEIGHT, targetz], target)
-    
+
     return [targetx, targetz, yaw]
 
+
 def get_straightline_path_to(target, robot_pos):
     pts = []
     cur_pos = robot_pos
-    while np.linalg.norm(target[:2]-cur_pos[:2]) > 0.5:
+    while np.linalg.norm(target[:2] - cur_pos[:2]) > 0.5:
         t = get_step_target_for_move(cur_pos, [target[0], CAMERA_HEIGHT, target[1]], step_size=0.5)
         pts.append(t)
         cur_pos = t
     return np.asarray(pts)
 
+
 def get_circle(r, n=10):
-    return [[math.cos(2*math.pi/n*x)*r,math.sin(2*math.pi/n*x)*r] for x in range(0,n+1)]
+    return [
+        [math.cos(2 * math.pi / n * x) * r, math.sin(2 * math.pi / n * x) * r]
+        for x in range(0, n + 1)
+    ]
+
 
 def get_circular_path(target, robot_pos, radius, num_points):
     """
     get a circular path with num_points of radius from x
     xyz 
     """
-    pts = get_circle(radius, num_points) # these are the x,z
+    pts = get_circle(radius, num_points)  # these are the x,z
+
     def get_xyyaw(p, target):
         targetx = p[0] + target[0]
         targetz = p[1] + target[2]
         yaw, _ = get_camera_angles([targetx, CAMERA_HEIGHT, targetz], target)
         return [targetx, targetz, yaw]
-        
+
     pts = np.asarray([get_xyyaw(p, target) for p in pts])
 
     # find nearest pt to robot_pos as starting point
     def find_nearest_indx(pts, robot_pos):
-        idx = np.asarray([np.linalg.norm(np.asarray(p[:2]) - np.asarray(robot_pos[:2])) for p in pts]).argmin()
+        idx = np.asarray(
+            [np.linalg.norm(np.asarray(p[:2]) - np.asarray(robot_pos[:2])) for p in pts]
+        ).argmin()
         return idx
 
     idx = find_nearest_indx(pts, robot_pos)
     # rotate the pts to begin at idx
-    pts = np.concatenate((pts[idx:,:], pts[:idx,:]), axis=0)
+    pts = np.concatenate((pts[idx:, :], pts[:idx, :]), axis=0)
 
     # TODO: get step-wise move targets to nearest point? or capture move data?
     # spath = get_straightline_path_to(pts[0], robot_pos)
@@ -174,9 +185,10 @@ def find_nearest_indx(pts, robot_pos):
 
     return pts
 
+
 class TrajectoryDataSaver:
     def __init__(self, root):
-        print(f'TrajectoryDataSaver saving to {root}')
+        print(f"TrajectoryDataSaver saving to {root}")
         self.save_folder = root
         self.img_folder = os.path.join(self.save_folder, "rgb")
         self.img_folder_dbg = os.path.join(self.save_folder, "rgb_dbg")
@@ -185,27 +197,33 @@ def __init__(self, root):
         self.trav_folder = os.path.join(self.save_folder, "trav")
 
         if os.path.exists(self.save_folder):
-            print(f'rmtree {self.save_folder}')
+            print(f"rmtree {self.save_folder}")
             shutil.rmtree(self.save_folder)
 
-        print(f'trying to create {self.save_folder}')
+        print(f"trying to create {self.save_folder}")
         Path(self.save_folder).mkdir(parents=True, exist_ok=True)
 
-        for x in [self.img_folder, self.img_folder_dbg, self.depth_folder, self.seg_folder, self.trav_folder]:
+        for x in [
+            self.img_folder,
+            self.img_folder_dbg,
+            self.depth_folder,
+            self.seg_folder,
+            self.trav_folder,
+        ]:
             self.create(x)
 
         self.pose_dict = {}
         self.pose_dict_hab = {}
         self.img_count = 0
         self.dbg_str = "None"
         self.init_logger()
-    
+
     def init_logger(self):
-        self.logger = logging.getLogger('trajectory_saver')
+        self.logger = logging.getLogger("trajectory_saver")
         self.logger.setLevel(logging.INFO)
-        formatter = logging.Formatter('%(filename)s:%(lineno)s - %(funcName)s(): %(message)s')
+        formatter = logging.Formatter("%(filename)s:%(lineno)s - %(funcName)s(): %(message)s")
         # Enable filehandler to debug logs
-        fh = logging.FileHandler(f"trajectory_saver.log", 'a')
+        fh = logging.FileHandler(f"trajectory_saver.log", "a")
         fh.setLevel(logging.INFO)
         fh.setFormatter(formatter)
         self.logger.addHandler(fh)
@@ -219,7 +237,7 @@ def create(self, d):
 
     def set_dbg_str(self, x):
         self.dbg_str = x
-    
+
     def get_total_frames(self):
         return self.img_count
 
@@ -229,20 +247,21 @@ def save(self, rgb, depth, seg, pos, habitat_pos, habitat_rot):
         print(f'saving {rgb.shape, depth.shape, seg.shape}')
         # store the images and depth
         rgb = cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB)
-        cv2.imwrite(
-            self.img_folder + "/{:05d}.jpg".format(self.img_count),
+        cv2.imwrite(self.img_folder + "/{:05d}.jpg".format(self.img_count), rgb)
+
+        cv2.putText(
             rgb,
+            str(self.img_count) + " " + self.dbg_str,
+            (40, 40),
+            cv2.FONT_HERSHEY_PLAIN,
+            1,
+            (0, 0, 255),
         )
 
-        cv2.putText(rgb, str(self.img_count) + ' ' + self.dbg_str, (40,40), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255))
-
         # robot_dbg_str = 'robot_pose ' + str(np.round(self.get_robot_global_state(), 3))
         # cv2.putText(rgb, robot_dbg_str, (40,60), cv2.FONT_HERSHEY_PLAIN, 1, (0,0,255))
 
-        cv2.imwrite(
-            self.img_folder_dbg + "/{:05d}.jpg".format(self.img_count),
-            rgb,
-        )
+        cv2.imwrite(self.img_folder_dbg + "/{:05d}.jpg".format(self.img_count), rgb)
 
         # store depth
         np.save(self.depth_folder + "/{:05d}.npy".format(self.img_count), depth)
@@ -256,7 +275,7 @@ def save(self, rgb, depth, seg, pos, habitat_pos, habitat_rot):
                 self.pose_dict = json.load(fp)
 
         self.pose_dict[self.img_count] = copy(pos)
-        
+
         with open(os.path.join(self.save_folder, "data.json"), "w") as fp:
             json.dump(self.pose_dict, fp)
 
@@ -275,96 +294,34 @@ def save(self, rgb, depth, seg, pos, habitat_pos, habitat_rot):
 
 
 def visualize_examine(agent, robot_poses, object_xyz, label, obstacle_map, save_path, gt_pts=None):
-    traj_visual_dir = os.path.join(save_path, 'traj_visual')
+    traj_visual_dir = os.path.join(save_path, "traj_visual")
     if not os.path.isdir(traj_visual_dir):
         os.makedirs(traj_visual_dir)
-    vis_count = len(glob.glob(traj_visual_dir + '/*.jpg'))
+    vis_count = len(glob.glob(traj_visual_dir + "/*.jpg"))
     if vis_count == 0:
         plt.figure()
 
     plt.title("Examine Visual")
     # visualize obstacle map
     if len(obstacle_map) > 0:
         obstacle_map = np.asarray([list(x) for x in obstacle_map])
-        plt.plot(obstacle_map[:,1], obstacle_map[:,0], 'b+')
-    
-    # visualize object 
+        plt.plot(obstacle_map[:, 1], obstacle_map[:, 0], "b+")
+
+    # visualize object
     if object_xyz is not None:
-        plt.plot(object_xyz[0], object_xyz[2], 'y*')
+        plt.plot(object_xyz[0], object_xyz[2], "y*")
         plt.text(object_xyz[0], object_xyz[2], label)
-    
+
     # visualize robot pose
     if len(robot_poses) > 0:
         robot_poses = np.asarray(robot_poses)
-        plt.plot(robot_poses[:,0], robot_poses[:,1], 'r--')
+        plt.plot(robot_poses[:, 0], robot_poses[:, 1], "r--")
 
     if gt_pts is not None:
         pts = np.asarray(gt_pts)
-        plt.plot(pts[:,0], pts[:,1], 'y--')
-    
-    # TODO: visualize robot heading 
-    
+        plt.plot(pts[:, 0], pts[:, 1], "y--")
+
+    # TODO: visualize robot heading
 
     plt.savefig("{}/{:04d}.jpg".format(traj_visual_dir, vis_count))
     vis_count += 1
-
-class ExaminedMap:
-    """A helper static class to maintain the state representations needed to track active exploration.
-    droidlet.interpreter.robot.tasks.CuriousExplore uses this to decide which objects to explore next.
-    The core of this class is the ExaminedMap.can_examine method. This is a heuristic.
-    Long term, this information should live in memory (#FIXME @anuragprat1k). 
-    
-    It works as follows -
-    1. for each new candidate coordinate, it fetches the closest examined coordinate.
-    2. if this closest coordinate is within a certain threshold (1 meter) of the current coordinate, 
-    or if that region has been explored upto a certain number of times (2, for redundancy),
-    it is not explored, since a 'close-enough' region in space has already been explored. 
-    """
-    examined = {}
-    examined_id = set()
-    last = None
-
-    @classmethod
-    def l1(cls, xyz, k):
-        """ returns the l1 distance between two standard coordinates"""
-        return np.linalg.norm(np.asarray([xyz[0], xyz[2]]) - np.asarray([k[0], k[2]]), ord=1)
-
-    @classmethod
-    def get_closest(cls, xyz):
-        """returns closest examined point to xyz"""
-        c = None
-        dist = 1.5
-        for k, v in cls.examined.items():
-            if cls.l1(k, xyz) < dist:
-                dist = cls.l1(k, xyz)
-                c = k
-        if c is None:
-            cls.examined[xyz] = 0
-            return xyz
-        return c
-
-    @classmethod
-    def update(cls, target):
-        """called each time a region is examined. Updates relevant states."""
-        cls.last = cls.get_closest(target['xyz'])
-        cls.examined_id.add(target['eid'])
-        cls.examined[cls.last] += 1
-
-    @classmethod
-    def clear(cls):
-        cls.examined = {}
-        cls.examined_id = set()
-        cls.last = None
-
-    @classmethod
-    def can_examine(cls, x):
-        """decides whether to examine x or not."""
-        loc = x['xyz']
-        k = cls.get_closest(x['xyz'])
-        val = True
-        if cls.last is not None and cls.l1(cls.last, k) < 1:
-            val = False
-        val = cls.examined[k] < 2
-        print(f"can_examine {x['eid'], x['label'], x['xyz'][:2]}, closest {k[:2]}, can_examine {val}")
-        print(f"examined[k] = {cls.examined[k]}")
-        return val

droidlet/lowlevel/robot_mover_utils.py

@@ -17,7 +17,7 @@
     LocationNode,
     SetNode,
     ReferenceObjectNode,
-    AttentionNode
+    AttentionNode,
 )
 from .mc_memory_nodes import (  # noqa
     DanceNode,
@@ -100,7 +100,7 @@ def __init__(
     ### Update world with perception updates ###
     ############################################
 
-    def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[]):
+    def update(self, perception_output: namedtuple = None, areas_to_perceive: List = []):
         """
         Updates the world with updates from agent's perception module.
 
@@ -127,14 +127,22 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
         """Perform update the memory with input from low_level perception module"""
         # 1. Handle all mobs in agent's perception range
         if perception_output.mobs:
+            map_changes = []
             for mob in perception_output.mobs:
-                self.set_mob_position(mob)
-
+                mob_memid = self.set_mob_position(mob)
+                mp = (mob.pos.x, mob.pos.y, mob.pos.z)
+                map_changes.append(
+                    {"pos": mp, "is_obstacle": False, "memid": mob_memid, "is_move": True}
+                )
+            self.place_field.update_map(map_changes)
         # 2. Handle all items that the agent can pick up in-game
         if perception_output.agent_pickable_items:
+            # FIXME PUT IN MEMORY PROPERLY
             # 2.1 Items that are in perception range
             if perception_output.agent_pickable_items["in_perception_items"]:
-                for pickable_items in perception_output.agent_pickable_items["in_perception_items"]:
+                for pickable_items in perception_output.agent_pickable_items[
+                    "in_perception_items"
+                ]:
                     self.set_item_stack_position(pickable_items)
             # 2.2 Update previous pickable_item_stack based on perception
             if perception_output.agent_pickable_items["all_items"]:
@@ -154,11 +162,24 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
         if perception_output.agent_attributes:
             agent_player = perception_output.agent_attributes
             memid = self.get_player_by_eid(agent_player.entityId).memid
-            cmd = "UPDATE ReferenceObjects SET eid=?, name=?, x=?,  y=?, z=?, pitch=?, yaw=? WHERE "
+            cmd = (
+                "UPDATE ReferenceObjects SET eid=?, name=?, x=?,  y=?, z=?, pitch=?, yaw=? WHERE "
+            )
             cmd = cmd + "uuid=?"
             self.db_write(
-                cmd, agent_player.entityId, agent_player.name, agent_player.pos.x, agent_player.pos.y,
-                agent_player.pos.z, agent_player.look.pitch, agent_player.look.yaw, memid
+                cmd,
+                agent_player.entityId,
+                agent_player.name,
+                agent_player.pos.x,
+                agent_player.pos.y,
+                agent_player.pos.z,
+                agent_player.look.pitch,
+                agent_player.look.yaw,
+                memid,
+            )
+            ap = (agent_player.pos.x, agent_player.pos.y, agent_player.pos.z)
+            self.place_field.update_map(
+                [{"pos": ap, "is_obstacle": True, "memid": memid, "is_move": True}]
             )
 
         # 4. Update other in-game players in agent's memory
@@ -170,13 +191,22 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
                     memid = PlayerNode.create(self, player)
                 else:
                     memid = mem.memid
-                cmd = (
-                    "UPDATE ReferenceObjects SET eid=?, name=?, x=?,  y=?, z=?, pitch=?, yaw=? WHERE "
-                )
+                cmd = "UPDATE ReferenceObjects SET eid=?, name=?, x=?,  y=?, z=?, pitch=?, yaw=? WHERE "
                 cmd = cmd + "uuid=?"
                 self.db_write(
-                    cmd, player.entityId, player.name, player.pos.x, player.pos.y, player.pos.z,
-                    player.look.pitch, player.look.yaw, memid
+                    cmd,
+                    player.entityId,
+                    player.name,
+                    player.pos.x,
+                    player.pos.y,
+                    player.pos.z,
+                    player.look.pitch,
+                    player.look.yaw,
+                    memid,
+                )
+                pp = (player.pos.x, player.pos.y, player.pos.z)
+                self.place_field.update_map(
+                    [{"pos": pp, "is_obstacle": True, "memid": memid, "is_move": True}]
                 )
                 memids = self._db_read_one(
                     'SELECT uuid FROM ReferenceObjects WHERE ref_type="attention" AND type_name=?',
@@ -200,24 +230,28 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
                 self.maybe_remove_inst_seg(xyz)
 
                 # 5.2 Update agent's memory with blocks that have been destroyed.
-                updated_areas_to_perceive = self.maybe_remove_block_from_memory(xyz, idm, areas_to_perceive)
+                updated_areas_to_perceive = self.maybe_remove_block_from_memory(
+                    xyz, idm, areas_to_perceive
+                )
 
                 # 5.3 Update blocks in memory when any change in the environment is caused either by agent or player
-                interesting, player_placed, agent_placed = perception_output.changed_block_attributes[(xyz, idm)]
+                interesting, player_placed, agent_placed = perception_output.changed_block_attributes[
+                    (xyz, idm)
+                ]
                 self.maybe_add_block_to_memory(interesting, player_placed, agent_placed, xyz, idm)
 
         """Now perform update the memory with input from heuristic perception module"""
         # 1. Process everything in area to attend for perception
         if perception_output.in_perceive_area:
             # 1.1 Add colors of all block objects
             if perception_output.in_perceive_area["block_object_attributes"]:
-                for block_object_attr in perception_output.in_perceive_area["block_object_attributes"]:
+                for block_object_attr in perception_output.in_perceive_area[
+                    "block_object_attributes"
+                ]:
                     block_object, color_tags = block_object_attr
                     memid = BlockObjectNode.create(self, block_object)
                     for color_tag in list(set(color_tags)):
-                        self.add_triple(
-                            subj=memid, pred_text="has_colour", obj_text=color_tag
-                        )
+                        self.add_triple(subj=memid, pred_text="has_colour", obj_text=color_tag)
             # 1.2 Update all holes with their block type in memory
             if perception_output.in_perceive_area["holes"]:
                 self.add_holes_to_mem(perception_output.in_perceive_area["holes"])
@@ -233,9 +267,7 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
                     block_object, color_tags = block_object_attr
                     memid = BlockObjectNode.create(self, block_object)
                     for color_tag in list(set(color_tags)):
-                        self.add_triple(
-                            subj=memid, pred_text="has_colour", obj_text=color_tag
-                        )
+                        self.add_triple(subj=memid, pred_text="has_colour", obj_text=color_tag)
             # 2.2 Update all holes with their block type in memory
             if perception_output.near_agent["holes"]:
                 self.add_holes_to_mem(perception_output.near_agent["holes"])
@@ -257,7 +289,6 @@ def update(self, perception_output: namedtuple=None, areas_to_perceive: List=[])
         output["areas_to_perceive"] = updated_areas_to_perceive
         return output
 
-
     def maybe_add_block_to_memory(self, interesting, player_placed, agent_placed, xyz, idm):
         if not interesting:
             return
@@ -275,13 +306,13 @@ def maybe_add_block_to_memory(self, interesting, player_placed, agent_placed, xy
         adjacent_memids = list(set(adjacent_memids))
         if len(adjacent_memids) == 0:
             # new block object
-            BlockObjectNode.create(self, [(xyz, idm)])
+            memid = BlockObjectNode.create(self, [(xyz, idm)])
+            self.place_field.update_map([{"pos": xyz, "is_obstacle": True, "memid": memid}])
         elif len(adjacent_memids) == 1:
             # update block object
             memid = adjacent_memids[0]
-            self.upsert_block(
-                (xyz, idm), memid, "BlockObjects", player_placed, agent_placed
-            )
+            self.upsert_block((xyz, idm), memid, "BlockObjects", player_placed, agent_placed)
+            self.place_field.update_map([{"pos": xyz, "is_obstacle": True, "memid": memid}])
             self.set_memory_updated_time(memid)
             self.set_memory_attended_time(memid)
         else:
@@ -305,7 +336,6 @@ def maybe_add_block_to_memory(self, interesting, player_placed, agent_placed, xy
                 (xyz, idm), chosen_memid, "BlockObjects", player_placed, agent_placed
             )
 
-
     def add_holes_to_mem(self, holes):
         """
         Adds the list of holes to memory and return hole memories.
@@ -326,7 +356,6 @@ def add_holes_to_mem(self, holes):
             hole_memories.append(self.get_mem_by_id(memid))
         return hole_memories
 
-
     def maybe_remove_block_from_memory(self, xyz: XYZ, idm: IDM, areas_to_perceive):
         """Update agent's memory with blocks that have been destroyed."""
         tables = ["BlockObjects"]
@@ -340,10 +369,19 @@ def maybe_remove_block_from_memory(self, xyz: XYZ, idm: IDM, areas_to_perceive):
             delete = (b == 0 and idm[0] > 0) or (b > 0 and idm[0] == 0)
             if delete:
                 self.remove_voxel(*xyz, table)
+                # check if the whole column is removed:
+                # FIXME, eventually want y slices
+                r = self._db_read(
+                    "SELECT uuid FROM VoxelObjects WHERE x=? AND z=? and ref_type=?",
+                    xyz[0],
+                    xyz[2],
+                    tables[0],
+                )
+                if len(r) == 0:
+                    self.place_field.update_map([{"pos": xyz, "is_delete": True}])
                 local_areas_to_perceive.append((xyz, 3))
         return local_areas_to_perceive
 
-
     def maybe_remove_inst_seg(self, xyz: XYZ):
         """if the block is changed, the old instance segmentation
         is no longer considered valid"""
@@ -357,7 +395,6 @@ def maybe_remove_inst_seg(self, xyz: XYZ):
             for i in inst_seg_memids:
                 self.forget(i[0])
 
-
     ###########################
     ### For Animate objects ###
     ###########################

droidlet/memory/craftassist/mc_memory.py

@@ -0,0 +1,292 @@
+"""
+Copyright (c) Facebook, Inc. and its affiliates.
+"""
+
+import numpy as np
+
+MAX_MAP_SIZE = 4097
+MAP_INIT_SIZE = 1025
+BIG_I = MAX_MAP_SIZE
+BIG_J = MAX_MAP_SIZE
+
+
+def no_y_l1(self, xyz, k):
+    """ returns the l1 distance between two standard coordinates"""
+    return np.linalg.norm(np.asarray([xyz[0], xyz[2]]) - np.asarray([k[0], k[2]]), ord=1)
+
+
+# TODO tighter integration with reference objects table, main memory update
+# should probably sync PlaceField maps without explicit perception updates
+# Node type for complicated-shaped obstacles that aren't "objects" e.g. walls?
+#    currently just represented as occupancy cells with no memid
+# FIXME allow multiple memids at a single location in the map
+
+
+class PlaceField:
+    """
+    maintains a grid-based map of some slice(s) of the world, and 
+    the state representations needed to track active exploration.
+
+    the .place_fields attribute is a dict with keys corresponding to heights, 
+    and values {"map": 2d numpy array, "updated": 2d numpy array, "memids": 2d numpy array}
+    place_fields[h]["map"] is an occupany map at the the height h (in agent coordinates)
+                           a location is 0 if there is nothing there or it is unseen, 1 if occupied  
+    place_fields[h]["memids"] gives a memid index for the ReferenceObject at that location, 
+                              if there is a ReferenceObject linked to that spatial location.
+                              the PlaceField keeps a mappping from the indices to memids in 
+                              self.index2memid and self.memid2index
+    place_fields[h]["updated"] gives the last update time of that location (in agent's internal time)
+                               if -1, it has neer been updated
+
+    the .map2real method converts a location from a map to world coords
+    the .real2map method converts a location from the world to the map coords
+
+    droidlet.interpreter.robot.tasks.CuriousExplore uses the can_examine method to decide 
+    which objects to explore next:
+    1. for each new candidate coordinate, it fetches the closest examined coordinate.
+    2. if this closest coordinate is within a certain threshold (1 meter) of the current coordinate, 
+    or if that region has been explored upto a certain number of times (2, for redundancy),
+    it is not explored, since a 'close-enough' region in space has already been explored. 
+    """
+
+    def __init__(self, memory, pixels_per_unit=1):
+        self.get_time = memory.get_time
+
+        self.index2memid = []
+        self.memid2index = {}
+
+        self.examined = {}
+        self.examined_id = set()
+        self.last = None
+
+        self.maps = {}
+        self.maybe_add_memid("NULL")
+        self.maybe_add_memid(memory.self_memid)
+        # FIXME, want slices, esp for mc... init after first perception
+        # with h=y2slice(y) instead of using 0
+        self.map_size = self.extend_map(h=0)
+
+        self.pixels_per_unit = pixels_per_unit
+
+        # gives an index allowing quick lookup by memid
+        # each entry is keyed by a memid and is a dict
+        # {str(h*BIG_I*BIG_J + i*BIG_J + j) : True}
+        # for each placed h, i ,j
+        self.memid2locs = {}
+
+    def ijh2idx(self, i, j, h):
+        return str(h * BIG_I * BIG_J + i * BIG_J + j)
+
+    def idx2ijh(self, idx):
+        idx = int(idx)
+        j = idx % BIG_J
+        idx = (idx - j) // BIG_J
+        i = idx % BIG_I
+        h = (idx - i) // BIG_I
+        return i, j, h
+
+    def pop_memid_loc(self, memid, i, j, h):
+        idx = self.hij2idx(h, i, j)
+        del self.memid2locs[memid][idx]
+
+    def maybe_delete_loc(self, i, j, h, t, memid="NULL"):
+        """
+        remove a loc from the maps and from memid2loc index.
+        if memid is set, only removes the loc if the memid matches
+        """
+        current_memid = self.index2memid[int(self.maps[h]["memids"][i, j])]
+        if memid == "NULL" or current_memid == memid:
+            self.maps[h]["memids"][i, j] = self.memid2index["NULL"]
+            self.maps[h]["map"][i, j] = 0
+            self.maps[h]["updated"][i, j] = t
+            idx = self.ijh2idx(i, j, h)
+            # maybe error/warn if its not there?
+            if self.memid2locs.get(memid):
+                self.memid2locs[memid].pop(idx, None)
+                if len(self.memid2locs[memid]) == 0:
+                    self.memid2locs.pop(memid, None)
+
+    def delete_loc_by_memid(self, memid, t, is_move=False):
+        """
+        remove all locs corresponding to a memid.  
+        if is_move is set, asserts that there is precisely one loc
+        corresponding to the memid
+        """
+        assert memid
+        assert memid != "NULL"
+        count = 0
+        for idx in self.memid2locs.get(memid, []):
+            i, j, h = self.idx2ijh(idx)
+            self.maps[h]["memids"][i, j] = 0
+            self.maps[h]["map"][i, j] = 0
+            self.maps[h]["updated"][i, j] = t
+            count = count + 1
+            if is_move and count > 1:
+                # eventually allow moving "large" objects
+                raise Exception(
+                    "tried to delete more than one pixel from the place_field by memid with is_move set"
+                )
+        self.memid2locs.pop(memid, None)
+
+    def update_map(self, changes):
+        """
+        changes is a list of dicts of the form 
+        {"pos": (x, y, z),
+        "memid": str (default "NULL"),
+        "is_obstacle": bool (default True),
+        "is_move": bool (default False),
+        "is_delete": bool (default False) }
+        pos is required if is_delete is False. 
+        all other fields are always optional.
+        
+        "is_obstacle" tells whether the agent can traverse that location
+        if "is_move" is False, the change is taken as is; if "is_move" is True, if the
+            change corresponds to a memid, and the memid is located somewhere on the map,
+            the old location is removed when the new one is set.  For now, to move complicated objects
+            that cover many pixels, do not use is_move, and instead move them "by hand"
+            by issuing a list of changes deleting the old now empty locations and adding the
+            new now-filled locations
+        "is_delete" True without a memid means whatever is in that location is to be removed.
+            if a memid is set, the remove will occur only if the memid matches.
+        
+        the "is_obstacle" status can be changed without changing memid etc.
+        """
+        t = self.get_time()
+        for c in changes:
+            is_delete = c.get("is_delete", False)
+            is_move = c.get("is_move", False)
+            memid = c.get("memid", "NULL")
+            p = c.get("pos")
+            if p is None:
+                assert is_delete
+                # if the change is a remove, and is specified by memid:
+                if not memid:
+                    raise Exception("tried to update a map location without a location or a memid")
+                # warn if empty TODO?
+                self.delete_loc_by_memid(memid, t)
+            else:
+                x, y, z = p
+                h = self.y2slice(y)
+                i, j = self.real2map(x, z, h)
+                s = max(i - self.map_size + 1, j - self.map_size + 1, -i, -j)
+                if s > 0:
+                    self.extend_map(s)
+                i, j = self.real2map(x, z, h)
+                s = max(i - self.map_size + 1, j - self.map_size + 1, -i, -j)
+                if s > 0:
+                    # the map can not been extended enough to handle these bc MAX_MAP_SIZE
+                    # FIXME appropriate warning or error?
+                    continue
+                if is_delete:
+                    self.maybe_delete_loc(i, j, h, t, memid=memid)
+                else:
+                    if is_move:
+                        assert memid != "NULL"
+                        self.delete_loc_by_memid(memid, t, is_move=True)
+                    self.maps[h]["memids"][i, j] = self.memid2index.get(
+                        memid, self.maybe_add_memid(memid)
+                    )
+                    self.maps[h]["map"][i, j] = c.get("is_obstacle", 1)
+                    self.maps[h]["updated"][i, j] = t
+                    if not self.memid2locs.get(memid):
+                        self.memid2locs[memid] = {}
+                    self.memid2locs[memid][self.ijh2idx(i, j, h)] = True
+
+    # FIXME, want slices, esp for mc
+    def y2slice(self, y):
+        return 0
+
+    def real2map(self, x, z, h):
+        """
+        convert an x, z coordinate in agent space to a pixel on the map
+        """
+        n = self.maps[h]["map"].shape[0]
+        i = x * self.pixels_per_unit
+        j = z * self.pixels_per_unit
+        i = i + n // 2
+        j = j + n // 2
+        return round(i), round(j)
+
+    def map2real(self, i, j, h):
+        """
+        convert an i, j pixel coordinate in the map to agent space
+        """
+        n = self.maps[h]["map"].shape[0]
+        i = i - n // 2
+        j = j - n // 2
+        x = i / self.pixels_per_unit
+        z = j / self.pixels_per_unit
+        return x, z
+
+    def maybe_add_memid(self, memid):
+        """ 
+        adds an entry to the mapping from memids to ints to put on map.
+        these are never removed
+        """
+        idx = self.memid2index.get(memid)
+        if idx is None:
+            idx = len(self.index2memid)
+            self.index2memid.append(memid)
+            self.memid2index[memid] = idx
+        return idx
+
+    def extend_map(self, h=None, extension=1):
+        assert extension >= 0
+        if not h and len(self.maps) == 1:
+            h = list(self.maps.keys())[0]
+        if not self.maps.get(h):
+            self.maps[h] = {}
+            for m, v in {"updated": -1, "map": 0, "memids": 0}.items():
+                self.maps[h][m] = v * np.ones((MAP_INIT_SIZE, MAP_INIT_SIZE))
+        w = self.maps[h]["map"].shape[0]
+        new_w = w + 2 * extension
+        if new_w > MAX_MAP_SIZE:
+            return -1
+        for m, v in {"updated": -1, "map": 0, "memids": 0}.items():
+            new_map = v * np.ones((new_w, new_w))
+            new_map[extension:-extension, extension:-extension] = self.maps[h][m]
+            self.maps[h][m] = new_map
+        return new_w
+
+    def get_closest(self, xyz):
+        """returns closest examined point to xyz"""
+        c = None
+        dist = 1.5
+        for k, v in self.examined.items():
+            if no_y_l1(k, xyz) < dist:
+                dist = no_y_l1(k, xyz)
+                c = k
+        if c is None:
+            self.examined[xyz] = 0
+            return xyz
+        return c
+
+    def update(self, target):
+        """called each time a region is examined. Updates relevant states."""
+        self.last = self.get_closest(target["xyz"])
+        self.examined_id.add(target["eid"])
+        self.examined[self.last] += 1
+
+    def clear_examined(self):
+        self.examined = {}
+        self.examined_id = set()
+        self.last = None
+
+    def can_examine(self, x):
+        """decides whether to examine x or not."""
+        loc = x["xyz"]
+        k = self.get_closest(x["xyz"])
+        val = True
+        if self.last is not None and self.l1(cls.last, k) < 1:
+            val = False
+        val = self.examined[k] < 2
+        print(
+            f"can_examine {x['eid'], x['label'], x['xyz'][:2]}, closest {k[:2]}, can_examine {val}"
+        )
+        print(f"examined[k] = {self.examined[k]}")
+        return val
+
+
+if __name__ == "__main__":
+    W = {0: {0: {0: True}, 1: {2: {3: True}}}, 1: {5: True}}
+    idxs = [0, 1, 2, 3]

droidlet/memory/place_field.py

@@ -42,7 +42,7 @@ def __init__(
     ### Update world with perception updates ###
     ############################################
 
-    def update(self, perception_output: namedtuple=None):
+    def update(self, perception_output: namedtuple = None):
         """
         Updates the world with updates from agent's perception module.
 
@@ -56,13 +56,28 @@ def update(self, perception_output: namedtuple=None):
             return
         if perception_output.new_objects:
             for detection in perception_output.new_objects:
-                DetectedObjectNode.create(self, detection)
+                memid = DetectedObjectNode.create(self, detection)
+                # TODO use the bounds, not just the center
+                pos = (
+                    detection.get_xyz()["x"],
+                    detection.get_xyz()["y"],
+                    detection.get_xyz()["z"],
+                )
+                self.place_field.update_map([{"pos": pos, "memid": memid}])
         if perception_output.updated_objects:
             for detection in perception_output.updated_objects:
-                DetectedObjectNode.update(self, detection)
+                memid = DetectedObjectNode.update(self, detection)
+                # TODO use the bounds, not just the center
+                pos = (
+                    detection.get_xyz()["x"],
+                    detection.get_xyz()["y"],
+                    detection.get_xyz()["z"],
+                )
+                self.place_field.update_map([{"pos": pos, "memid": memid, "is_move": True}])
         if perception_output.humans:
             for human in perception_output.humans:
                 HumanPoseNode.create(self, human)
+                # FIXME, not putting in map, need to dedup?
 
     #################
     ###  Players  ###
@@ -97,5 +112,5 @@ def add_dance(self, dance_fn, name=None, tags=[]):
 
     def clear(self, objects):
         for o in objects:
-            if o['memid'] != self.self_memid:
-                self.forget(o['memid'])
+            if o["memid"] != self.self_memid:
+                self.forget(o["memid"])

droidlet/memory/robot/loco_memory.py

@@ -18,6 +18,7 @@
 from droidlet.memory.memory_filters import MemorySearcher
 from droidlet.event import dispatch
 from droidlet.memory.memory_util import parse_sql, format_query
+from droidlet.memory.place_field import PlaceField
 
 from droidlet.memory.memory_nodes import (  # noqa
     TaskNode,
@@ -136,6 +137,7 @@ def __init__(
         self.tag(self.self_memid, "SELF")
 
         self.searcher = MemorySearcher()
+        self.place_field = PlaceField(self)
 
     def __del__(self):
         """Close the database file"""

droidlet/memory/sql_memory.py

@@ -2,6 +2,7 @@
 Copyright (c) Facebook, Inc. and its affiliates.
 """
 import unittest
+import numpy as np
 from droidlet.memory.memory_nodes import (
     SelfNode,
     PlayerNode,
@@ -316,5 +317,39 @@ def test_triggers(self):
         assert len(triples) == 0
 
 
+class PlaceFieldTest(unittest.TestCase):
+    def test_place_field(self):
+        memory = AgentMemory()
+        PF = memory.place_field
+        joe_x = 1
+        joe_z = 2
+        joe_loc = (joe_x, 0, joe_z)
+        jane_loc = (-1, 0, 1)
+        joe_memid = PlayerNode.create(memory, Player(10, "joe", Pos(*joe_loc), Look(0, 0)))
+        jane_memid = PlayerNode.create(memory, Player(11, "jane", Pos(*jane_loc), Look(0, 0)))
+        wall_locs = [{"pos": (-i, 0, 4)} for i in range(5)]
+        changes = [{"pos": joe_loc, "memid": joe_memid}, {"pos": jane_loc, "memid": jane_memid}]
+        changes.extend(wall_locs)
+        PF.update_map(changes)
+        assert PF.maps[0]["map"].sum() == 7
+        jl = PF.memid2locs[joe_memid]
+        assert len(jl) == 1
+        recovered_pos = tuple(int(i) for i in PF.map2real(*PF.idx2ijh(list(jl.keys())[0])))
+        assert recovered_pos == (joe_x, joe_z)
+        assert len(PF.memid2locs["NULL"]) == 5
+        changes = [{"pos": (-1, 0, 4), "is_delete": True}]
+        PF.update_map(changes)
+        assert len(PF.memid2locs["NULL"]) == 4
+        new_jane_x = -5
+        new_jane_z = 5
+        changes = [{"pos": (new_jane_x, 0, new_jane_z), "memid": jane_memid, "is_move": True}]
+        PF.update_map(changes)
+        jl = PF.memid2locs[jane_memid]
+        assert len(jl) == 1
+        recovered_pos = tuple(int(i) for i in PF.map2real(*PF.idx2ijh(list(jl.keys())[0])))
+        assert recovered_pos == (new_jane_x, new_jane_z)
+        assert PF.maps[0]["map"].sum() == 6
+
+
 if __name__ == "__main__":
     unittest.main()

droidlet/memory/tests/test_low_level_memory.py

@@ -0,0 +1,11 @@
+default_ip=$(hostname -I | cut -f1 -d" ")
+ip=${LOCOBOT_IP:-$default_ip}
+echo "default_ip" $default_ip
+export LOCOBOT_IP=$ip
+export SAVE_EXPLORATION=True
+export DATA_PATH=straightline_test
+export HEURISTIC=straightline
+export VISUALIZE_EXAMINE=True
+export CONTINUOUS_EXPLORE=False
+source activate /private/home/apratik/miniconda3/envs/droidlet
+python agents/locobot/locobot_agent.py --dev

launch_agent_collect.sh

@@ -0,0 +1,4 @@
+export NOISY_HABITAT=False
+export ADD_HUMANS=False
+source activate /private/home/apratik/miniconda3/envs/droidlet
+./droidlet/lowlevel/locobot/remote/launch_pyro_habitat.sh --scene_path /checkpoint/apratik/replica/apartment_0/habitat/mesh_semantic.ply