Merge branch 'og-develop' into new-robot

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -796,7 +796,8 @@ def _toggle(self, obj, value):
         toggle_position = toggle_state.get_link_position()
         yield from self._navigate_if_needed(obj, toggle_position)
 
-        hand_orientation = self.robot.eef_links[self.arm].get_orientation()  # Just keep the current hand orientation.
+        # Just keep the current hand orientation.
+        hand_orientation = self.robot.eef_links[self.arm].get_position_orientation()[1]
         desired_hand_pose = (toggle_position, hand_orientation)
 
         yield from self._move_hand(desired_hand_pose)
@@ -1641,7 +1642,7 @@ def _navigate_to_pose_direct(self, pose_2d, low_precision=False):
             if th.norm(body_target_pose[0][:2]) < dist_threshold:
                 break
 
-            diff_pos = end_pose[0] - self.robot.get_position()
+            diff_pos = end_pose[0] - self.robot.get_position_orientation()[0]
             intermediate_pose = (
                 end_pose[0],
                 T.euler2quat(th.tensor([0, 0, math.atan2(diff_pos[1], diff_pos[0])], dtype=th.float32)),
@@ -1783,7 +1784,11 @@ def _sample_pose_near_object(self, obj, pose_on_obj=None, **kwargs):
             raise ActionPrimitiveError(
                 ActionPrimitiveError.Reason.SAMPLING_ERROR,
                 "Could not find valid position near object.",
-                {"target object": obj.name, "target pos": obj.get_position(), "pose on target": pose_on_obj},
+                {
+                    "target object": obj.name,
+                    "target pos": obj.get_position_orientation()[0],
+                    "pose on target": pose_on_obj,
+                },
             )
 
     @staticmethod

omnigibson/action_primitives/symbolic_semantic_action_primitives.py

@@ -167,8 +167,8 @@ def _grasp(self, obj: DatasetObject):
         # yield from self._navigate_if_needed(obj)
 
         # Perform forced assisted grasp
-        obj.set_position(self.robot.get_eef_position(self.arm))
-        self.robot._establish_grasp(self.arm, (obj, obj.root_link), obj.get_position())
+        obj.set_position_orientation(position=self.robot.get_eef_position(self.arm))
+        self.robot._establish_grasp(self.arm, (obj, obj.root_link), obj.get_position_orientation()[0])
 
         # Execute for a moment
         yield from self._settle_robot()
@@ -554,7 +554,10 @@ def _place_near_heating_element(self, heat_source_obj):
 
         # Get the position of the heat source on the thing we're placing near
         heating_element_positions = th.tensor(
-            [link.get_position() for link in heat_source_obj.states[object_states.HeatSourceOrSink].links.values()]
+            [
+                link.get_position_orientation()[0]
+                for link in heat_source_obj.states[object_states.HeatSourceOrSink].links.values()
+            ]
         )
         heating_distance_threshold = heat_source_obj.states[object_states.HeatSourceOrSink].distance_threshold
 

omnigibson/controllers/null_joint_controller.py

@@ -55,7 +55,7 @@ def __init__(
                 applied
         """
         # Store values
-        self._default_command = th.zeros(len(dof_idx)) if default_command is None else th.tensor(default_command)
+        self._default_command = th.zeros(len(dof_idx)) if default_command is None else default_command
 
         # Run super init
         super().__init__(

omnigibson/envs/env_base.py

@@ -268,6 +268,13 @@ def _load_robots(self):
                     robot_config["name"] = f"robot{i}"
 
                 position, orientation = robot_config.pop("position", None), robot_config.pop("orientation", None)
+                if position is not None:
+                    position = position if isinstance(position, th.Tensor) else th.tensor(position, dtype=th.float32)
+                if orientation is not None:
+                    orientation = (
+                        orientation if isinstance(orientation, th.Tensor) else th.tensor(orientation, dtype=th.float32)
+                    )
+
                 # Make sure robot exists, grab its corresponding kwargs, and create / import the robot
                 robot = create_class_from_registry_and_config(
                     cls_name=robot_config["type"],
@@ -277,8 +284,7 @@ def _load_robots(self):
                 )
                 # Import the robot into the simulator
                 self.scene.add_object(robot)
-                # TODO: Fix this after scene_local_position_orientation API is fixed
-                robot.set_local_pose(position=position, orientation=orientation)
+                robot.set_position_orientation(position=position, orientation=orientation, frame="scene")
 
         assert og.sim.is_stopped(), "Simulator must be stopped after loading robots!"
 
@@ -302,7 +308,7 @@ def _load_objects(self):
             )
             # Import the robot into the simulator and set the pose
             self.scene.add_object(obj)
-            obj.set_local_pose(position=position, orientation=orientation)
+            obj.set_position_orientation(position=position, orientation=orientation, frame="scene")
 
         assert og.sim.is_stopped(), "Simulator must be stopped after loading objects!"
 
@@ -333,7 +339,7 @@ def _load_external_sensors(self):
                 # Load an initialize this sensor
                 sensor.load(self.scene)
                 sensor.initialize()
-                sensor.set_local_pose(local_position, local_orientation)
+                sensor.set_position_orientation(position=local_position, orientation=local_orientation, frame="scene")
                 self._external_sensors[sensor.name] = sensor
                 self._external_sensors_include_in_obs[sensor.name] = include_in_obs
 

omnigibson/examples/object_states/attachment_demo.py

@@ -119,12 +119,12 @@ def main(random_selection=False, headless=False, short_exec=False):
         env.step([])
 
     shelf_baseboard = env.scene.object_registry("name", "shelf_baseboard")
-    shelf_baseboard.set_position_orientation([0, -0.979, 0.26], [0, 0, 0, 1])
+    shelf_baseboard.set_position_orientation(position=[0, -0.979, 0.26], orientation=[0, 0, 0, 1])
     shelf_baseboard.keep_still()
     shelf_baseboard.set_linear_velocity(th.tensor([-0.2, 0, 0]))
 
     shelf_side_left = env.scene.object_registry("name", "shelf_side_left")
-    shelf_side_left.set_position_orientation([-0.4, 0.0, 0.2], [0, 0, 0, 1])
+    shelf_side_left.set_position_orientation(position=[-0.4, 0.0, 0.2], orientation=[0, 0, 0, 1])
     shelf_side_left.keep_still()
 
     input(

omnigibson/examples/object_states/dicing_demo.py

@@ -91,7 +91,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     knife.keep_still()
     knife.set_position_orientation(
-        position=apple.get_position() + th.tensor([-0.15, 0.0, 0.2]),
+        position=apple.get_position_orientation()[0] + th.tensor([-0.15, 0.0, 0.2]),
         orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0])),
     )
 

omnigibson/examples/object_states/heat_source_or_sink_demo.py

@@ -82,7 +82,7 @@ def main(random_selection=False, headless=False, short_exec=False):
     # Move stove, notify user
     if not short_exec:
         input("Heat source is now moving: Press ENTER to continue.")
-    stove.set_position(th.tensor([0, 1.0, 0.61]))
+    stove.set_position_orientation(position=th.tensor([0, 1.0, 0.61]))
     for i in range(100):
         env.step(th.empty(0))
 

omnigibson/examples/object_states/onfire_demo.py

@@ -86,10 +86,16 @@ def main(random_selection=False, headless=False, short_exec=False):
     stove.states[object_states.ToggledOn].set_value(True)
 
     # The first apple will be affected by the stove
-    apples[0].set_position(stove.states[object_states.HeatSourceOrSink].link.get_position() + th.tensor([0.11, 0, 0.1]))
+    apples[0].set_position_orientation(
+        position=stove.states[object_states.HeatSourceOrSink].link.get_position_orientation()[0]
+        + th.tensor([0.11, 0, 0.1])
+    )
 
     # The second apple will NOT be affected by the stove, but will be affected by the first apple once it's on fire.
-    apples[1].set_position(stove.states[object_states.HeatSourceOrSink].link.get_position() + th.tensor([0.32, 0, 0.1]))
+    apples[1].set_position_orientation(
+        position=stove.states[object_states.HeatSourceOrSink].link.get_position_orientation()[0]
+        + th.tensor([0.32, 0, 0.1])
+    )
 
     steps = 0
     max_steps = -1 if not short_exec else 1000

omnigibson/examples/object_states/sample_kinematics_demo.py

@@ -128,8 +128,8 @@ def sample_microwave_plates_apples(env):
     og.log.info("Placing cabinet on the floor...")
     cabinet.set_orientation([0, 0, 0, 1.0])
     env.step(th.empty(0))
-    offset = cabinet.get_position()[2] - cabinet.aabb_center[2]
-    cabinet.set_position(th.tensor([1.0, 0, cabinet.aabb_extent[2] / 2]) + offset)
+    offset = cabinet.get_position_orientation()[0][2] - cabinet.aabb_center[2]
+    cabinet.set_position_orientation(position=th.tensor([1.0, 0, cabinet.aabb_extent[2] / 2]) + offset)
     env.step(th.empty(0))
 
     # Set microwave on top of the cabinet, open it, and step 100 times
@@ -172,8 +172,8 @@ def sample_boxes_on_shelf(env):
     og.log.info("Placing shelf on the floor...")
     shelf.set_orientation([0, 0, 0, 1.0])
     env.step(th.empty(0))
-    offset = shelf.get_position()[2] - shelf.aabb_center[2]
-    shelf.set_position(th.tensor([-1.0, 0, shelf.aabb_extent[2] / 2]) + offset)
+    offset = shelf.get_position_orientation()[0][2] - shelf.aabb_center[2]
+    shelf.set_position_orientation(position=th.tensor([-1.0, 0, shelf.aabb_extent[2] / 2]) + offset)
     env.step(th.empty(0))  # One step is needed for the object to be fully initialized
 
     og.log.info("Shelf placed.")

omnigibson/examples/object_states/slicing_demo.py

@@ -89,7 +89,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     knife.keep_still()
     knife.set_position_orientation(
-        position=apple.get_position() + th.tensor([-0.15, 0.0, 0.2], dtype=th.float32),
+        position=apple.get_position_orientation()[0] + th.tensor([-0.15, 0.0, 0.2], dtype=th.float32),
         orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0], dtype=th.float32)),
     )
 

omnigibson/examples/object_states/temperature_demo.py

@@ -149,7 +149,10 @@ def main(random_selection=False, headless=False, short_exec=False):
     for apple in apples:
         apple.states[object_states.Temperature].set_value(-50)
     apples[0].states[object_states.Inside].set_value(oven, True)
-    apples[1].set_position(stove.states[object_states.HeatSourceOrSink].link.get_position() + th.tensor([0, 0, 0.1]))
+    apples[1].set_position_orientation(
+        position=stove.states[object_states.HeatSourceOrSink].link.get_position_orientation()[0]
+        + th.tensor([0, 0, 0.1])
+    )
     apples[2].states[object_states.OnTop].set_value(tray, True)
     apples[3].states[object_states.Inside].set_value(fridge, True)
     apples[4].states[object_states.Inside].set_value(microwave, True)

omnigibson/examples/objects/load_object_selector.py

@@ -61,8 +61,8 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     # Place the object so it rests on the floor
     obj = env.scene.object_registry("name", "obj")
-    center_offset = obj.get_position() - obj.aabb_center + th.tensor([0, 0, obj.aabb_extent[2] / 2.0])
-    obj.set_position(center_offset)
+    center_offset = obj.get_position_orientation()[0] - obj.aabb_center + th.tensor([0, 0, obj.aabb_extent[2] / 2.0])
+    obj.set_position_orientation(position=center_offset)
 
     # Step through the environment
     max_steps = 100 if short_exec else 10000

omnigibson/examples/objects/visualize_object.py

@@ -111,8 +111,8 @@ def main(random_selection=False, headless=False, short_exec=False):
     env.step(th.empty(0))
 
     # Move the object so that its center is at [0, 0, 1]
-    center_offset = obj.get_position() - obj.aabb_center + th.tensor([0, 0, 1.0])
-    obj.set_position(center_offset)
+    center_offset = obj.get_position_orientation()[0] - obj.aabb_center + th.tensor([0, 0, 1.0])
+    obj.set_position_orientation(position=center_offset)
 
     # Allow the user to easily move the camera around
     og.sim.enable_viewer_camera_teleoperation()

omnigibson/examples/robots/advanced/ik_example.py

@@ -66,7 +66,7 @@ def main(random_selection=False, headless=False, short_exec=False):
     robot = env.robots[0]
 
     # Set robot base at the origin
-    robot.set_position_orientation(th.tensor([0, 0, 0]), th.tensor([0, 0, 0, 1]))
+    robot.set_position_orientation(position=th.tensor([0, 0, 0]), orientation=th.tensor([0, 0, 0, 1]))
     # At least one simulation step while the simulator is playing must occur for the robot (or in general, any object)
     # to be fully initialized after it is imported into the simulator
     og.sim.play()
@@ -128,7 +128,7 @@ def execute_ik(pos, quat=None, max_iter=100):
 
         # Get initial EE position and set marker to that location
         command = robot.get_eef_position()
-        marker.set_position(command)
+        marker.set_position_orientation(position=command)
         og.sim.step()
 
         # Setup callbacks for grabbing keyboard inputs from omni
@@ -154,7 +154,7 @@ def keyboard_event_handler(event, *args, **kwargs):
                     delta_cmd = input_to_xyz_delta_command(inp=event.input)
                     if delta_cmd is not None:
                         command = command + delta_cmd
-                        marker.set_position(command)
+                        marker.set_position_orientation(position=command)
                         og.sim.step()
 
             # Callback must return True if valid

omnigibson/examples/robots/all_robots_visualizer.py

@@ -55,7 +55,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
         # Then apply random actions for a bit
         for _ in range(30):
-            action_lo, action_hi = -1, 1
+            action_lo, action_hi = -0.1, 0.1
             action = th.rand(robot.action_dim) * (action_hi - action_lo) + action_lo
             if robot_name == "Tiago":
                 tiago_lo, tiago_hi = -0.1, 0.1

omnigibson/examples/robots/grasping_mode_example.py

@@ -80,7 +80,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     # Reset the robot
     robot = env.robots[0]
-    robot.set_position([0, 0, 0])
+    robot.set_position_orientation(position=[0, 0, 0])
     robot.reset()
     robot.keep_still()
 

omnigibson/examples/scenes/scene_tour_demo.py

@@ -61,7 +61,7 @@ def main(random_selection=False, headless=False, short_exec=False):
 
     def add_waypoint():
         nonlocal waypoints
-        pos = cam_mover.cam.get_position()
+        pos = cam_mover.cam.get_position_orientation()[0]
         print(f"Added waypoint at {pos}")
         waypoints.append(pos)
 

omnigibson/examples/teleoperation/robot_teleoperate_demo.py

@@ -118,7 +118,7 @@ def main():
     env = og.Environment(configs=cfg)
     env.reset()
     # update viewer camera pose
-    og.sim.viewer_camera.set_position_orientation([-0.22, 0.99, 1.09], [-0.14, 0.47, 0.84, -0.23])
+    og.sim.viewer_camera.set_position_orientation(position=[-0.22, 0.99, 1.09], orientation=[-0.14, 0.47, 0.84, -0.23])
     # Start teleoperation system
     robot = env.robots[0]
 

omnigibson/maps/map_base.py

@@ -55,5 +55,7 @@ def world_to_map(self, xy):
             xy: 2D location in world reference frame (metric)
         :return: 2D location in map reference frame (image)
         """
+
+        xy = th.tensor(xy) if not isinstance(xy, th.Tensor) else xy
         point_wrt_map = xy / self.map_resolution + self.map_size / 2.0
         return th.flip(point_wrt_map, dims=tuple(range(point_wrt_map.dim()))).int()

omnigibson/object_states/attached_to.py

@@ -224,10 +224,12 @@ def _find_attachment_links(
                 if other.states[AttachedTo].children[parent_link_name] is None:
                     if bypass_alignment_checking:
                         return child_link, parent_link
-                    pos_diff = th.norm(child_link.get_position() - parent_link.get_position())
-                    orn_diff = T.get_orientation_diff_in_radian(
-                        child_link.get_orientation(), parent_link.get_orientation()
-                    )
+
+                    child_pos, child_orn = child_link.get_position_orientation()
+                    parent_pos, parent_orn = parent_link.get_position_orientation()
+                    pos_diff = th.norm(child_pos - parent_pos)
+                    orn_diff = T.get_orientation_diff_in_radian(child_orn, parent_orn)
+
                     if pos_diff < pos_thresh and orn_diff < orn_thresh:
                         return child_link, parent_link
 
@@ -301,7 +303,7 @@ def _attach(self, other, child_link, parent_link, joint_type=None, can_joint_bre
             new_child_root_quat = child_root_quat
 
         # Actually move the object and also keep it still for stability purposes.
-        self.obj.set_position_orientation(new_child_root_pos, new_child_root_quat)
+        self.obj.set_position_orientation(position=new_child_root_pos, orientation=new_child_root_quat)
         self.obj.keep_still()
         other.keep_still()
 

omnigibson/object_states/heat_source_or_sink.py

@@ -235,7 +235,7 @@ def overlap_callback(hit):
                 if n_cloth_objs > 0:
                     cloth_positions = th.zeros((n_cloth_objs, 3))
                     for i, obj in enumerate(cloth_objs):
-                        cloth_positions[i] = obj.get_position()
+                        cloth_positions[i] = obj.get_position_orientation()[0]
                     for idx in th.where(
                         th.all(
                             (aabb_lower.reshape(1, 3) < cloth_positions) & (cloth_positions < aabb_upper.reshape(1, 3)),
@@ -253,7 +253,11 @@ def overlap_callback(hit):
 
             else:
                 # Position is either the AABB center of the default link or the metalink position itself
-                heat_source_pos = self.link.aabb_center if self.link == self._default_link else self.link.get_position()
+                heat_source_pos = (
+                    self.link.aabb_center
+                    if self.link == self._default_link
+                    else self.link.get_position_orientation()[0]
+                )
 
                 # Use overlap_sphere check!
                 og.sim.psqi.overlap_sphere(
@@ -268,7 +272,7 @@ def overlap_callback(hit):
                 if n_cloth_objs > 0:
                     cloth_positions = th.zeros((n_cloth_objs, 3))
                     for i, obj in enumerate(cloth_objs):
-                        cloth_positions[i] = obj.get_position()
+                        cloth_positions[i] = obj.get_position_orientation()[0]
                     for idx in th.where(
                         th.norm(heat_source_pos.reshape(1, 3) - cloth_positions, dim=-1) <= self.distance_threshold
                     )[0]:

omnigibson/object_states/overlaid.py

@@ -62,10 +62,7 @@ def _get_value(self, other):
         # Compute the base aligned bounding box of the rigid object.
         bbox_center, bbox_orn, bbox_extent, _ = other.get_base_aligned_bbox(xy_aligned=True)
         vertices_local = th.tensor(list(itertools.product((1, -1), repeat=3))) * (bbox_extent / 2)
-        vertices = th.tensor(
-            T.transform_points(vertices_local, T.pose2mat((bbox_center, bbox_orn))),
-            dtype=th.float32,
-        )
+        vertices = T.transform_points(vertices_local, T.pose2mat((bbox_center, bbox_orn)))
         rigid_hull = ConvexHull(vertices[:, :2])
 
         # The goal is to find the intersection of the convex hull and the bounding box.