Three challenge tracks for Locobot

examples/configs/locobot_p2p_nav_house.yaml

@@ -0,0 +1,63 @@
+# scene
+scene: building
+model_id: Ohoopee
+build_graph: true
+load_texture: true
+trav_map_erosion: 2
+
+# robot
+robot: Locobot
+velocity: 0.1
+
+# task, observation and action
+task: pointgoal # pointgoal|objectgoal|areagoal|reaching
+fisheye: false
+
+is_discrete: false
+additional_states_dim: 3
+
+# reward
+reward_type: geodesic
+success_reward: 10.0
+slack_reward: -0.01
+potential_reward_weight: 1.0
+collision_reward_weight: -0.1
+collision_ignore_link_a_ids: [1, 2, 3, 4]  # ignore collision with these agent's link ids
+
+# discount factor
+discount_factor: 0.99
+
+# termination condition
+death_z_thresh: 0.2
+dist_tol: 0.333  # body width
+max_step: 500
+
+# sensor
+output: [sensor, rgb, depth]
+resolution: 256
+fov: 90
+
+# display
+use_filler: true
+display_ui: false
+show_diagnostics: false
+ui_num: 2
+ui_components: [RGB_FILLED, DEPTH]
+random:
+  random_initial_pose: false
+  random_target_pose: false
+  random_init_x_range: [-0.1, 0.1]
+  random_init_y_range: [-0.1, 0.1]
+  random_init_z_range: [-0.1, 0.1]
+  random_init_rot_range: [-0.1, 0.1]
+
+speed:
+  timestep: 0.001
+  frameskip: 10
+
+mode: web_ui #gui|headless
+verbose: false
+fast_lq_render: true
+
+# debug
+debug: false

gibson2/core/physics/interactive_objects.py

@@ -208,7 +208,7 @@ class InteractiveObj(object):
         org_pos, org_orn = p.getBasePositionAndOrientation(self.body_id)
         p.resetBasePositionAndOrientation(self.body_id, pos, org_orn)
 
-    def set_position_rotation(self, pos, orn):
+    def set_position_orientation(self, pos, orn):
         p.resetBasePositionAndOrientation(self.body_id, pos, orn)
 
 

gibson2/core/physics/robot_locomotors.py

@@ -95,6 +95,10 @@ class LocomotorRobot(BaseRobot):
     def set_orientation(self, orn):
         self.robot_body.set_orientation(orn)
 
+    def set_position_orientation(self, pos, orn):
+        self.robot_body.reset_position(pos)
+        self.robot_body.set_orientation(orn)
+
     def move_by(self, delta):
         new_pos = np.array(delta) + self.get_position()
         self.robot_body.reset_position(new_pos)
@@ -117,6 +121,10 @@ class LocomotorRobot(BaseRobot):
         new_orn = qmult((euler2quat(delta, 0, 0)), orn)
         self.robot_body.set_orientation(new_orn)
 
+    def keep_still(self):
+        for n, j in enumerate(self.ordered_joints):
+            j.set_motor_velocity(0.0)
+
     def get_rpy(self):
         return self.robot_body.get_rpy()
 
@@ -709,3 +717,70 @@ class JR2_Kinova(LocomotorRobot):
             for j in range(16, 28):
                 p.setCollisionFilterPair(robot_id, robot_id, joint, j, 0)
         return ids
+
+
+class Locobot(LocomotorRobot):
+    mjcf_scaling = 1
+    model_type = "URDF"
+    default_scale = 1
+
+    def __init__(self, config):
+        self.config = config
+        self.velocity = config.get('velocity', 0.1)
+        self.wheel_dim = 2
+        self.action_high = config.get("action_high", None)
+        self.action_low = config.get("action_low", None)
+        LocomotorRobot.__init__(self,
+                                "locobot/locobot.urdf",
+                                "base_link",
+                                action_dim=self.wheel_dim,
+                                sensor_dim=55,  # TODO: what is sensor_dim?
+                                power=2.5,
+                                scale=config.get("robot_scale", self.default_scale),
+                                resolution=config.get("resolution", 64),
+                                is_discrete=config.get("is_discrete", True),
+                                control="velocity",
+                                self_collision=False)
+
+    def set_up_continuous_action_space(self):
+        if self.action_high is not None and self.action_low is not None:
+            self.action_high = np.full(shape=self.wheel_dim, fill_value=self.action_high)
+            self.action_low = np.full(shape=self.wheel_dim, fill_value=self.action_low)
+        else:
+            self.action_high = np.array([self.velocity] * self.wheel_dim)
+            self.action_low = -self.action_high
+
+        self.action_space = gym.spaces.Box(shape=(self.action_dim,),
+                                           low=-1.0,
+                                           high=1.0,
+                                           dtype=np.float32)
+
+    def set_up_discrete_action_space(self):
+        assert False, "Locobot does not support discrete actions"
+
+    def apply_action(self, action):
+        real_action = self.action_to_real_action(action)
+        self.apply_real_action(real_action)
+
+    def calc_state(self):
+        base_state = LocomotorRobot.calc_state(self)
+        angular_velocity = self.robot_body.angular_velocity()
+        print(len(base_state), len(angular_velocity))
+        return np.concatenate((base_state, np.array(angular_velocity)))
+
+    def get_end_effector_position(self):
+        return self.parts['gripper_link'].get_position()
+
+    def load(self):
+        ids = self._load_model()
+        self.eyes = self.parts["eyes"]
+
+        robot_id = ids[0]
+
+        # disable collision for immediate parent
+        for joint in range(p.getNumJoints(robot_id)):
+            info = p.getJointInfo(robot_id, joint)
+            parent_id = info[-1]
+            p.setCollisionFilterPair(robot_id, robot_id, joint, parent_id, 0)
+
+        return ids

gibson2/core/physics/scene.py

@@ -6,15 +6,18 @@ parentdir = os.path.dirname(currentdir)
 os.sys.path.insert(0, parentdir)
 import pybullet_data
 from gibson2.data.datasets import get_model_path
+from gibson2.utils.utils import l2_distance
+
 import numpy as np
 from PIL import Image
 import cv2
 import networkx as nx
 from IPython import embed
+import pickle
 
 class Scene:
     def load(self):
-        raise (NotImplementedError())
+        raise NotImplementedError()
 
 class EmptyScene(Scene):
     """
@@ -23,6 +26,7 @@ class EmptyScene(Scene):
     def load(self):
         planeName = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml")
         self.ground_plane_mjcf = p.loadMJCF(planeName)
+        p.changeDynamics(self.ground_plane_mjcf[0], -1, lateralFriction=1)
         return [item for item in self.ground_plane_mjcf]
 
 class StadiumScene(Scene):
@@ -43,6 +47,9 @@ class StadiumScene(Scene):
 
         return [item for item in self.stadium] + [item for item in self.ground_plane_mjcf]
 
+    def get_random_floor(self):
+        return 0
+
     def get_random_point(self, random_height=False):
         return self.get_random_point_floor(0, random_height)
 
@@ -54,6 +61,17 @@ class StadiumScene(Scene):
             np.random.uniform(0.4, 0.8) if random_height else 0.0
         ])
 
+    def get_floor_height(self, floor):
+        del floor
+        return 0.0
+
+    def reset_floor(self, floor=0, additional_elevation=0.05, height=None):
+        return
+
+    def get_shortest_path(self, floor, source_world, target_world, entire_path=False):
+        assert False, 'cannot compute shortest path in StadiumScene'
+
+
 class BuildingScene(Scene):
     """
     Gibson Environment building scenes
@@ -90,9 +108,6 @@ class BuildingScene(Scene):
         self.num_waypoints = num_waypoints
         self.waypoint_interval = int(waypoint_resolution / trav_map_resolution)
 
-    def l2_distance(self, a, b):
-        return np.linalg.norm(np.array(a) - np.array(b))
-
     def load(self):
         """
         Load the mesh into pybullet
@@ -141,19 +156,34 @@ class BuildingScene(Scene):
                 self.floors = sorted(list(map(float, f.readlines())))
                 print('floors', self.floors)
             for f in range(len(self.floors)):
-                trav_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f)))
+                trav_map = np.array(Image.open(
-                obstacle_map = Image.open(os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f)))
+                    os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f))
+                ))
+                obstacle_map = np.array(Image.open(
+                    os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f))
+                ))
                 if self.trav_map_original_size is None:
-                    width, height = trav_map.size
+                    height, width = trav_map.shape
-                    assert width == height, 'trav map is not a square'
+                    assert height == width, 'trav map is not a square'
                     self.trav_map_original_size = height
-                    self.trav_map_size = int(self.trav_map_original_size * self.trav_map_default_resolution / self.trav_map_resolution)
+                    self.trav_map_size = int(self.trav_map_original_size *
-                trav_map = np.array(trav_map.resize((self.trav_map_size, self.trav_map_size)))
+                                             self.trav_map_default_resolution /
-                obstacle_map = np.array(obstacle_map.resize((self.trav_map_size, self.trav_map_size)))
+                                             self.trav_map_resolution)
+
                 trav_map[obstacle_map == 0] = 0
+                trav_map = cv2.resize(trav_map, (self.trav_map_size, self.trav_map_size))
                 trav_map = cv2.erode(trav_map, np.ones((self.trav_map_erosion, self.trav_map_erosion)))
+                trav_map[trav_map < 255] = 0
 
                 if self.build_graph:
+                    graph_file = os.path.join(get_model_path(self.model_id), 'floor_trav_{}.p'.format(f))
+
+                    if os.path.isfile(graph_file):
+                        print("load traversable graph")
+                        with open(graph_file, 'rb') as pfile:
+                            g = pickle.load(pfile)
+                    else:
+                        print("build traversable graph")
                         g = nx.Graph()
                         for i in range(self.trav_map_size):
                             for j in range(self.trav_map_size):
@@ -164,16 +194,18 @@ class BuildingScene(Scene):
                                     for n in neighbors:
                                         if 0 <= n[0] < self.trav_map_size and 0 <= n[1] < self.trav_map_size and \
                                                 trav_map[n[0], n[1]] > 0:
-                                        g.add_edge(n, (i, j), weight=self.l2_distance(n, (i, j)))
+                                            g.add_edge(n, (i, j), weight=l2_distance(n, (i, j)))
 
                         # only take the largest connected component
                         largest_cc = max(nx.connected_components(g), key=len)
                         g = g.subgraph(largest_cc).copy()
-                    self.floor_graph.append(g)
+                        with open(graph_file, 'wb') as pfile:
+                            pickle.dump(g, pfile, protocol=pickle.HIGHEST_PROTOCOL)
 
+                    self.floor_graph.append(g)
                     # update trav_map accordingly
                     trav_map[:, :] = 0
-                    for node in largest_cc:
+                    for node in g.nodes:
                         trav_map[node[0], node[1]] = 255
 
                 self.floor_map.append(trav_map)
@@ -205,6 +237,11 @@ class BuildingScene(Scene):
     def world_to_map(self, xy):
         return np.flip((xy / self.trav_map_resolution + self.trav_map_size / 2.0)).astype(np.int)
 
+    def has_node(self, floor, world_xy):
+        map_xy = tuple(self.world_to_map(world_xy))
+        g = self.floor_graph[floor]
+        return g.has_node(map_xy)
+
     def get_shortest_path(self, floor, source_world, target_world, entire_path=False):
         # print("called shortest path", source_world, target_world)
         assert self.build_graph, 'cannot get shortest path without building the graph'
@@ -216,20 +253,21 @@ class BuildingScene(Scene):
         if not g.has_node(target_map):
             nodes = np.array(g.nodes)
             closest_node = tuple(nodes[np.argmin(np.linalg.norm(nodes - target_map, axis=1))])
-            g.add_edge(closest_node, target_map, weight=self.l2_distance(closest_node, target_map))
+            g.add_edge(closest_node, target_map, weight=l2_distance(closest_node, target_map))
 
         if not g.has_node(source_map):
             nodes = np.array(g.nodes)
             closest_node = tuple(nodes[np.argmin(np.linalg.norm(nodes - source_map, axis=1))])
-            g.add_edge(closest_node, source_map, weight=self.l2_distance(closest_node, source_map))
+            g.add_edge(closest_node, source_map, weight=l2_distance(closest_node, source_map))
 
-        path_map = np.array(nx.astar_path(g, source_map, target_map, heuristic=self.l2_distance))
+        path_map = np.array(nx.astar_path(g, source_map, target_map, heuristic=l2_distance))
 
         path_world = self.map_to_world(path_map)
         geodesic_distance = np.sum(np.linalg.norm(path_world[1:] - path_world[:-1], axis=1))
+        path_world = path_world[::self.waypoint_interval]
 
         if not entire_path:
-            path_world = path_world[::self.waypoint_interval][:self.num_waypoints]
+            path_world = path_world[:self.num_waypoints]
             num_remaining_waypoints = self.num_waypoints - path_world.shape[0]
             if num_remaining_waypoints > 0:
                 remaining_waypoints = np.tile(target_world, (num_remaining_waypoints, 1))

gibson2/core/simulator.py

@@ -36,6 +36,7 @@ class Simulator:
         self.gravity = gravity
         self.timestep = timestep
         self.mode = mode
+
         # renderer
         self.resolution = resolution
         self.fov = fov
@@ -63,8 +64,7 @@ class Simulator:
         """
         Destroy the MeshRenderer and physics simulator and start again.
         """
-        self.renderer.release()
+        self.disconnect()
-        p.disconnect(self.cid)
         self.load()
 
     def load(self):
@@ -91,6 +91,7 @@ class Simulator:
             self.cid = p.connect(p.DIRECT)
         p.setTimeStep(self.timestep)
         p.setGravity(0, 0, -self.gravity)
+        p.setPhysicsEngineParameter(enableFileCaching=0)
 
         if self.mode == 'gui' and not self.render_to_tensor:
             self.add_viewer()
@@ -101,14 +102,13 @@ class Simulator:
         self.objects = []
 
     def import_scene(self, scene, texture_scale=1.0, load_texture=True, class_id=0):
-
         """
         Import a scene. A scene could be a synthetic one or a realistic Gibson Environment.
 
         :param scene: Scene object
         :param texture_scale: Option to scale down the texture for rendering
         :param load_texture: If you don't need rgb output, texture loading could be skipped to make rendering faster
-        :param class_id: The class_id for background for rendering semantics.
+        :param class_id: Class id for rendering semantic segmentation
         """
 
         new_objects = scene.load()
@@ -153,7 +153,7 @@ class Simulator:
     def import_object(self, object, class_id=0):
         """
         :param object: Object to load
-        :param class_id: class_id to show for semantic segmentation mask
+        :param class_id: Class id for rendering semantic segmentation
         """
         new_object = object.load()
         self.objects.append(new_object)
@@ -215,7 +215,7 @@ class Simulator:
         Import a robot into Simulator
 
         :param robot: Robot
-        :param class_id: class_id to show for semantic segmentation mask
+        :param class_id: Class id for rendering semantic segmentation
         :return: id for robot in pybullet
         """
 
@@ -298,7 +298,7 @@ class Simulator:
         Import articulated objects into simulator
 
         :param obj:
-        :param class_id: class_id to show for semantic segmentation mask
+        :param class_id: Class id for rendering semantic segmentation
         :return: pybulet id
         """
         ids = obj.load()
@@ -382,6 +382,12 @@ class Simulator:
         """
 
         p.stepSimulation()
+        self.sync()
+
+    def sync(self):
+        """
+        Update positions in renderer without stepping the simulation. Usually used in the reset() function
+        """
         for instance in self.renderer.instances:
             if instance.dynamic:
                 self.update_position(instance)
@@ -427,5 +433,6 @@ class Simulator:
         clean up the simulator
         """
         if self.isconnected():
+            p.resetSimulation(physicsClientId=self.cid)
             p.disconnect(self.cid)
         self.renderer.release()

gibson2/envs/base_env.py

@@ -1,4 +1,5 @@
-from gibson2.core.physics.robot_locomotors import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch
+from gibson2.core.physics.robot_locomotors \
+    import Turtlebot, Husky, Ant, Humanoid, JR2, JR2_Kinova, Freight, Fetch, Locobot
 from gibson2.core.simulator import Simulator
 from gibson2.core.physics.scene import BuildingScene, StadiumScene
 import gibson2
@@ -16,21 +17,31 @@ class BaseEnv(gym.Env):
                  config_file,
                  model_id=None,
                  mode='headless',
+                 action_timestep=1 / 10.0,
+                 physics_timestep=1 / 240.0,
                  device_idx=0):
         """
         :param config_file: config_file path
         :param model_id: override model_id in config file
         :param mode: headless or gui mode
-        :param device_idx: which GPU to run the simulation and rendering on
+        :param action_timestep: environment executes action per action_timestep second
+        :param physics_timestep: physics timestep for pybullet
+        :param device_idx: device_idx: which GPU to run the simulation and rendering on
         """
         self.config = parse_config(config_file)
         if model_id is not None:
             self.config['model_id'] = model_id
+
+        self.mode = mode
+        self.action_timestep = action_timestep
+        self.physics_timestep = physics_timestep
         self.simulator = Simulator(mode=mode,
+                                   timestep=physics_timestep,
                                    use_fisheye=self.config.get('fisheye', False),
                                    resolution=self.config.get('resolution', 64),
                                    fov=self.config.get('fov', 90),
                                    device_idx=device_idx)
+        self.simulator_loop = int(self.action_timestep / self.simulator.timestep)
         self.load()
 
     def reload(self, config_file):
@@ -77,6 +88,8 @@ class BaseEnv(gym.Env):
             robot = Freight(self.config)
         elif self.config['robot'] == 'Fetch':
             robot = Fetch(self.config)
+        elif self.config['robot'] == 'Locobot':
+            robot = Locobot(self.config)
         else:
             raise Exception('unknown robot type: {}'.format(self.config['robot']))
 
@@ -99,10 +112,16 @@ class BaseEnv(gym.Env):
         self.simulator.step()
 
     def step(self, action):
-        return NotImplementedError
+        """
+        Overwritten by subclasses
+        """
+        return NotImplementedError()
 
     def reset(self):
-        return NotImplementedError
+        """
+        Overwritten by subclasses
+        """
+        return NotImplementedError()
 
     def set_mode(self, mode):
         self.simulator.mode = mode

gibson2/utils/utils.py

@@ -1,6 +1,6 @@
 import numpy as np
 import yaml
-
+from scipy.spatial.transform import Rotation as R
 
 # File I/O related
 def parse_config(config):
@@ -8,19 +8,29 @@ def parse_config(config):
         config_data = yaml.load(f, Loader=yaml.FullLoader)
     return config_data
 
-
 # Geometry related
 def rotate_vector_3d(v, r, p, y):
-    """Rotates vector by roll, pitch and yaw counterclockwise"""
+    """Rotates 3d vector by roll, pitch and yaw counterclockwise"""
-    rot_x = np.array([[1, 0, 0], [0, np.cos(-r), -np.sin(-r)], [0, np.sin(-r), np.cos(-r)]])
+    local_to_global = R.from_euler('xyz', [r, p, y]).as_dcm()
-    rot_y = np.array([[np.cos(-p), 0, np.sin(-p)], [0, 1, 0], [-np.sin(-p), 0, np.cos(-p)]])
+    global_to_local = local_to_global.T
-    rot_z = np.array([[np.cos(-y), -np.sin(-y), 0], [np.sin(-y), np.cos(-y), 0], [0, 0, 1]])
+    return np.dot(global_to_local, v)
-    return np.dot(rot_x, np.dot(rot_y, np.dot(rot_z, v)))
 
+def rotate_vector_2d(v, yaw):
+    """Rotates 2d vector by yaw counterclockwise"""
+    local_to_global = R.from_euler('z', yaw).as_dcm()
+    global_to_local = local_to_global.T
+    global_to_local = global_to_local[:2, :2]
+    if len(v.shape) == 1:
+        return np.dot(global_to_local, v)
+    elif len(v.shape) == 2:
+        return np.dot(global_to_local, v.T).T
+    else:
+        print('Incorrect input shape for rotate_vector_2d', v.shape)
+        return v
 
 def l2_distance(v1, v2):
     """Returns the L2 distance between vector v1 and v2."""
-    return np.linalg.norm(v1 - v2)
+    return np.linalg.norm(np.array(v1) - np.array(v2))
 
 
 def quatFromXYZW(xyzw, seq):

setup.py

@@ -110,7 +110,7 @@ setup(
             'gym==0.12',
             'Pillow==5.4.1',
             'PyYAML==5.3',
-            'pybullet==2.4.1',
+            'pybullet==2.4.9',
             'transforms3d==0.3.1',
             'tqdm==4.19.9',
             'Pillow==5.4.1',