Merge branch 'og-develop' of https://github.com/StanfordVL/OmniGibson …

…into isaac_4_2

.github/workflows/examples-as-test.yml

@@ -21,22 +21,18 @@ jobs:
         shell: micromamba run -n omnigibson /bin/bash -leo pipefail {0}
     steps:
       - name: Checkout source
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
         with:
           submodules: true
-          path: omnigibson-src
 
       - name: Install
-        working-directory: omnigibson-src
         run: pip install -e .[dev,primitives]
 
       - name: Generate example tests
-        working-directory: omnigibson-src
         run: python tests/create_tests_of_examples.py
 
       - name: Get list of generated tests
         id: get-test-list
-        working-directory: omnigibson-src
         run: |
           echo "example_tests=$(cat tests/example_tests.json)" >> $GITHUB_OUTPUT
 
@@ -69,17 +65,14 @@ jobs:
         run: echo "HOME=/root" >> $GITHUB_ENV
 
       - name: Checkout source
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
         with:
           submodules: true
-          path: omnigibson-src
 
       - name: Install
-        working-directory: omnigibson-src
         run: pip install -e .[dev,primitives]
 
       - name: Run tests
-        working-directory: omnigibson-src
         run: pytest -s tests/tests_of_examples/${{ matrix.test_file }}.py --junitxml=${{ matrix.test_file }}.xml && cp ${{ matrix.test_file }}.xml ${GITHUB_WORKSPACE}/
 
       - name: Deploy artifact
@@ -99,20 +92,12 @@ jobs:
         shell: micromamba run -n omnigibson /bin/bash -leo pipefail {0}
     needs: [run_test]
     steps:
-      - name: Checkout source
-        uses: actions/checkout@v2
-        with:
-          submodules: true
-          path: omnigibson-src
       - name: Pull reports
         uses: actions/download-artifact@v3
-        with:
-          path: omnigibson-src
       - name: Example Test Report0
         uses: dorny/test-reporter@v1
         with:
           name: Example Test Results
-          working-directory: omnigibson-src
           path: ${{ github.run_id }}-tests-*/*_test.xml
           reporter: java-junit
           fail-on-error: 'true'

.github/workflows/profiling.yml

@@ -30,10 +30,9 @@ jobs:
         run: echo "HOME=/root" >> $GITHUB_ENV
 
       - name: Checkout source
-        uses: actions/checkout@v3
+        uses: actions/checkout@v4
 
       - name: Install
-        working-directory: omnigibson-src
         run: pip install -e .[dev,primitives]
 
       - name: Run performance benchmark

.github/workflows/tests.yml

@@ -45,20 +45,17 @@ jobs:
 
     steps:
       - name: Checkout source
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
         with:
           submodules: true
-          path: omnigibson-src
 
       - name: Install
-        working-directory: omnigibson-src
         run: pip install -e .[dev,primitives] --no-build-isolation
 
       - name: Print env
         run: printenv
 
       - name: Run tests
-        working-directory: omnigibson-src
         run: pytest -s tests/${{ matrix.test_file }}.py --junitxml=${{ matrix.test_file }}.xml && cp ${{ matrix.test_file }}.xml ${GITHUB_WORKSPACE}/
         continue-on-error: true
 

docs/modules/object_states.md

@@ -210,9 +210,9 @@ These are object states that are agnostic to other objects in a given scene.
         <tr>
         <td valign="top" width="60%">
             [**`ObjectsInFOVOfRobot`**](../reference/object_states/robot_related_states.md)<br><br>  
-            A robot-specific state. Comptues the list of objects that are currently in the robot's field of view.<br><br>
+            A robot-specific state. Comptues the set of objects that are currently in the robot's field of view.<br><br>
             <ul>
-                <li>`get_value()`: returns `obj_list`, the list of `BaseObject`s</li>
+                <li>`get_value()`: returns `obj_set`, the set of `BaseObject`s</li>
                 <li>`set_value(new_value)`: Not supported</li>
             </ul>
         </td>

omnigibson/action_primitives/curobo.py

@@ -28,7 +28,7 @@
 m.HOLONOMIC_BASE_REVOLUTE_JOINT_LIMIT = math.pi * 2  # radians
 
 
-class CuroboEmbodimentSelection(str, Enum):
+class CuRoboEmbodimentSelection(str, Enum):
     BASE = "base"
     ARM = "arm"
     DEFAULT = "default"
@@ -176,16 +176,15 @@ def __init__(
         self.debug = debug
         self.robot = robot
         self.robot_joint_names = list(robot.joints.keys())
-        self._fk = FKSolver(self.robot.robot_arm_descriptor_yamls[robot.default_arm], self.robot.urdf_path)
         self.batch_size = batch_size
 
         # Load robot config and usd paths and make sure paths point correctly
         robot_cfg_path_dict = robot.curobo_path if robot_cfg_path is None else robot_cfg_path
         if not isinstance(robot_cfg_path_dict, dict):
-            robot_cfg_path_dict = {CuroboEmbodimentSelection.DEFAULT: robot_cfg_path_dict}
+            robot_cfg_path_dict = {CuRoboEmbodimentSelection.DEFAULT: robot_cfg_path_dict}
         if use_default_embodiment_only:
             robot_cfg_path_dict = {
-                CuroboEmbodimentSelection.DEFAULT: robot_cfg_path_dict[CuroboEmbodimentSelection.DEFAULT]
+                CuRoboEmbodimentSelection.DEFAULT: robot_cfg_path_dict[CuRoboEmbodimentSelection.DEFAULT]
             }
         robot_usd_path = robot.usd_path if robot_usd_path is None else robot_usd_path
 
@@ -265,7 +264,7 @@ def update_joint_limits(self, robot_cfg_obj, emb_sel):
 
                 joint_limits.position[1][joint_idx] = -joint_limits.position[0][joint_idx]
 
-    def save_visualization(self, q, file_path, emb_sel=CuroboEmbodimentSelection.DEFAULT):
+    def save_visualization(self, q, file_path, emb_sel=CuRoboEmbodimentSelection.DEFAULT):
         # Update obstacles
         self.update_obstacles()
 
@@ -311,12 +310,12 @@ def update_obstacles(self, ignore_paths=None):
             ],
         )
         # All embodiment selections share the same world collision checker
-        self.mg[CuroboEmbodimentSelection.DEFAULT].update_world(obstacles)
+        self.mg[CuRoboEmbodimentSelection.DEFAULT].update_world(obstacles)
         print("Synced CuRobo world from stage.")
 
     def update_obstacles_fast(self):
         # All embodiment selections share the same world collision checker
-        world_coll_checker = self.mg[CuroboEmbodimentSelection.DEFAULT].world_coll_checker
+        world_coll_checker = self.mg[CuRoboEmbodimentSelection.DEFAULT].world_coll_checker
         for i, prim_path in enumerate(world_coll_checker._env_mesh_names[0]):
             if prim_path is None:
                 continue
@@ -344,13 +343,13 @@ def check_collisions(
             q (th.tensor): (N, D)-shaped tensor, representing N-total different joint configurations to check
                 collisions against the world
             check_self_collision (bool): Whether to check self-collisions or not
-            emb_sel (CuroboEmbodimentSelection): Which embodiment selection to use for checking collisions
+            emb_sel (CuRoboEmbodimentSelection): Which embodiment selection to use for checking collisions
 
         Returns:
             th.tensor: (N,)-shaped tensor, where each value is True if in collision, else False
         """
         # check_collisions only makes sense for the default embodiment where all the joints are actuated
-        emb_sel = CuroboEmbodimentSelection.DEFAULT
+        emb_sel = CuRoboEmbodimentSelection.DEFAULT
 
         # Update obstacles
         self.update_obstacles()
@@ -396,7 +395,7 @@ def update_locked_joints(self, cu_joint_state, emb_sel):
 
         Args:
             cu_joint_state (JointState): JointState object representing the current joint positions
-            emb_sel (CuroboEmbodimentSelection): Which embodiment selection to use for updating locked joints
+            emb_sel (CuRoboEmbodimentSelection): Which embodiment selection to use for updating locked joints
         """
         kc = self.mg[emb_sel].kinematics.kinematics_config
         # Update the lock joint state position
@@ -429,7 +428,7 @@ def compute_trajectories(
         success_ratio=None,
         attached_obj=None,
         attached_obj_scale=None,
-        emb_sel=CuroboEmbodimentSelection.DEFAULT,
+        emb_sel=CuRoboEmbodimentSelection.DEFAULT,
     ):
         """
         Computes the robot joint trajectory to reach the desired @target_pos and @target_quat
@@ -462,7 +461,7 @@ def compute_trajectories(
                 link names and the values are the corresponding BaseObject instances to attach to that link
             attached_obj_scale (None or Dict[str, float]): If specified, a dictionary where the keys are the end-effector
                 link names and the values are the corresponding scale to apply to the attached object
-            emb_sel (CuroboEmbodimentSelection): Which embodiment selection to use for computing trajectories
+            emb_sel (CuRoboEmbodimentSelection): Which embodiment selection to use for computing trajectories
         Returns:
             2-tuple or list of MotionGenResult: If @return_full_result is True, will return a list of raw MotionGenResult
                 object(s) computed from internal batch trajectory computations. If it is False, will return 2-tuple
@@ -658,6 +657,7 @@ def compute_trajectories(
                 ik_goal_batch = lazy.curobo.types.math.Pose(
                     position=batch_target_pos,
                     quaternion=batch_target_quat,
+                    name=link_name,
                 )
 
                 ik_goal_batch_by_link[link_name] = ik_goal_batch
@@ -702,13 +702,13 @@ def compute_trajectories(
         else:
             return successes, paths
 
-    def path_to_joint_trajectory(self, path, emb_sel=CuroboEmbodimentSelection.DEFAULT):
+    def path_to_joint_trajectory(self, path, emb_sel=CuRoboEmbodimentSelection.DEFAULT):
         """
         Converts raw path from motion generator into joint trajectory sequence
 
         Args:
             path (JointState): Joint state path to convert into joint trajectory
-            emb_sel (CuroboEmbodimentSelection): Which embodiment to use for the robot
+            emb_sel (CuRoboEmbodimentSelection): Which embodiment to use for the robot
 
         Returns:
             torch.tensor: (T, D) tensor representing the interpolated joint trajectory
@@ -718,39 +718,59 @@ def path_to_joint_trajectory(self, path, emb_sel=CuroboEmbodimentSelection.DEFAU
         cmd_plan = self.mg[emb_sel].get_full_js(path)
         return cmd_plan.get_ordered_joint_state(self.robot_joint_names).position
 
-    def convert_q_to_eef_traj(self, traj, return_axisangle=False, emb_sel=CuroboEmbodimentSelection.DEFAULT):
+    def path_to_eef_trajectory(self, path, return_axisangle=False, emb_sel=CuRoboEmbodimentSelection.DEFAULT):
         """
-        Converts a joint trajectory @traj into an equivalent trajectory defined by end effector poses
+        Converts raw path from motion generator into end-effector trajectory sequence in the robot frame.
+        This trajectory sequence can be executed by an IKController, although there is no guaranteee that
+        the controller will output the same joint trajectory as the one computed by cuRobo.
 
         Args:
-            traj (torch.Tensor): (T, D)-shaped joint trajectory
+            path (JointState): Joint state path to convert into joint trajectory
             return_axisangle (bool): Whether to return the interpolated orientations in quaternion or axis-angle representation
-            emb_sel (CuroboEmbodimentSelection): Which embodiment to use for the robot
+            emb_sel (CuRoboEmbodimentSelection): Which embodiment to use for the robot
 
         Returns:
-            torch.Tensor: (T, [6, 7])-shaped array where each entry is is the (x,y,z) position and (x,y,z,w) quaternion
-                (if @return_axisangle is False) or (ax, ay, az) axis-angle orientation, specified in the robot frame.
+            Dict[str, torch.Tensor]: Mapping eef link names to (T, [6, 7])-shaped array where each entry is is the (x,y,z) position
+            and (x,y,z,w) quaternion (if @return_axisangle is False) or (ax, ay, az) axis-angle orientation, specified in the robot frame.
         """
-        # Prune the relevant joints from the trajectory
-        traj = traj[:, self.robot.arm_control_idx[self.robot.default_arm]]
-        n = len(traj)
-
-        # Use forward kinematic solver to compute the EEF link positions
-        positions = self._tensor_args.to_device(th.zeros((n, 3)))
-        orientations = self._tensor_args.to_device(
-            th.zeros((n, 4))
-        )  # This will be quat initially but we may convert to aa representation
-
-        for i, qpos in enumerate(traj):
-            pose = self._fk.get_link_poses(joint_positions=qpos, link_names=[self.ee_link[emb_sel]])
-            positions[i] = pose[self.ee_link[emb_sel]][0]
-            orientations[i] = pose[self.ee_link[emb_sel]][1]
-
-        # Possibly convert orientations to aa-representation
-        if return_axisangle:
-            orientations = T.quat2axisangle(orientations)
-
-        return th.concatenate([positions, orientations], dim=-1)
+        # If the base-only embodiment is selected, the eef links stay the same, return the current eef poses in the robot frame
+        if emb_sel == CuRoboEmbodimentSelection.BASE:
+            link_poses = dict()
+            for arm_name in self.robot.arm_names:
+                link_name = self.robot.eef_link_names[arm_name]
+                position, orientation = self.robot.get_relative_eef_pose(arm_name)
+                if return_axisangle:
+                    orientation = T.quat2axisangle(orientation)
+                link_poses[link_name] = th.cat([position, orientation], dim=-1)
+            return link_poses
+
+        cmd_plan = self.mg[emb_sel].get_full_js(path)
+        robot_state = self.mg[emb_sel].kinematics.compute_kinematics(path)
+
+        link_poses = dict()
+
+        for link_name, poses in robot_state.link_poses.items():
+            position = poses.position
+            # wxyz -> xyzw
+            orientation = poses.quaternion[:, [1, 2, 3, 0]]
+
+            # If the robot is holonomic, we need to transform the poses to the base link frame
+            if isinstance(self.robot, HolonomicBaseRobot):
+                base_link_position = th.zeros_like(position)
+                base_link_position[:, 0] = cmd_plan.position[:, cmd_plan.joint_names.index("base_footprint_x_joint")]
+                base_link_position[:, 1] = cmd_plan.position[:, cmd_plan.joint_names.index("base_footprint_y_joint")]
+                base_link_euler = th.zeros_like(position)
+                base_link_euler[:, 2] = cmd_plan.position[:, cmd_plan.joint_names.index("base_footprint_rz_joint")]
+                base_link_orientation = T.euler2quat(base_link_euler)
+                position, orientation = T.relative_pose_transform(
+                    position, orientation, base_link_position, base_link_orientation
+                )
+
+            if return_axisangle:
+                orientation = T.quat2axisangle(orientation)
+            link_poses[link_name] = th.cat([position, orientation], dim=-1)
+
+        return link_poses
 
     @property
     def tensor_args(self):

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -59,7 +59,7 @@
     Stretch: 0.5,
     Turtlebot: 0.3,
     Husky: 0.05,
-    Freight: 0.05,
+    Freight: 0.2,
     Locobot: 1.5,
     BehaviorRobot: 0.3,
     R1: 0.3,
@@ -70,7 +70,7 @@
     Stretch: 0.7,
     Turtlebot: 0.2,
     Husky: 0.05,
-    Freight: 0.05,
+    Freight: 0.1,
     Locobot: 1.5,
     BehaviorRobot: 0.2,
     R1: 0.2,
@@ -1763,10 +1763,6 @@ def _rotate_in_place(self, end_pose, angle_threshold=m.DEFAULT_ANGLE_THRESHOLD):
             direction = -1.0 if diff_yaw < 0.0 else 1.0
             ang_vel = m.KP_ANGLE_VEL[type(self.robot)] * direction
 
-            if isinstance(self.robot, Locobot) or isinstance(self.robot, Freight):
-                # Locobot and Freight wheel joints are reversed
-                ang_vel = -ang_vel
-
             base_action = action[self.robot.controller_action_idx["base"]]
 
             if not self._base_controller_is_joint: