Merge pull request StanfordVL#936 from StanfordVL/fix/tr_curobo_tests

Fix transition rule and cuRobo tests

omnigibson/action_primitives/curobo.py

@@ -130,6 +130,7 @@ def __init__(
         self,
         robot,
         robot_cfg_path=None,
+        robot_usd_path=None,
         ee_link=None,
         device="cuda:0",
         motion_cfg_kwargs=None,
@@ -141,6 +142,8 @@ def __init__(
             robot (BaseRobot): Robot for which to generate motion plans
             robot_cfg_path (None or str): If specified, the path to the robot configuration to use. If None, will
                 try to use a pre-configured one directly from curobo based on the robot class of @robot
+            robot_usd_path (None or str): If specified, the path to the robot USD file to use. If None, will
+                try to use a pre-configured one directly from curobo based on the robot class of @robot
             ee_link (None or str): If specified, the link name representing the end-effector to track. None defaults to
                 value already set in the config from @robot_cfg
             device (str): Which device to use for curobo
@@ -156,11 +159,15 @@ def __init__(
         self._tensor_args = lazy.curobo.types.base.TensorDeviceType(device=th.device(device))
         self.debug = debug
 
-        # Load robot config and make sure paths point correctly
+        # Load robot config and usd paths and make sure paths point correctly
         robot_cfg_path = robot.curobo_path if robot_cfg_path is None else robot_cfg_path
-        robot_cfg = lazy.curobo.util_file.load_yaml(robot_cfg_path)["robot_cfg"]
-        robot_cfg["kinematics"]["external_asset_path"] = gm.ASSET_PATH
-        robot_cfg["kinematics"]["external_robot_configs_path"] = robot_cfg_path
+        robot_usd_path = robot.usd_path if robot_usd_path is None else robot_usd_path
+
+        content_path = lazy.curobo.types.file_path.ContentPath(
+            robot_config_absolute_path=robot_cfg_path, robot_usd_absolute_path=robot_usd_path
+        )
+        robot_cfg = lazy.curobo.cuda_robot_model.util.load_robot_yaml(content_path)["robot_cfg"]
+        robot_cfg["kinematics"]["use_usd_kinematics"] = True
 
         # Possibly update ee_link
         if ee_link is not None:
@@ -337,17 +344,17 @@ def compute_trajectories(
             finetune_attempts (int): Number of attempts to run finetuning trajectory optimization. Every attempt will
                 increase the `MotionGenPlanConfig.finetune_dt_scale` by `MotionGenPlanConfig.finetune_dt_decay` as a
                 path couldn't be found with the previous smaller dt
-            return_full_result (bool): Whether to return the full result or not. If False, will randomly sample a single
-                (successful) trajectory to return
+            return_full_result (bool): Whether to return a list of raw MotionGenResult object(s) or a 2-tuple of
+                (success, results); the default is the latter
             success_ratio (None or float): If set, specifies the fraction of successes necessary given self.batch_size.
                 If None, will automatically be the smallest ratio (1 / self.batch_size), i.e: any nonzero number of
                 successes
             attached_obj (None or BaseObject): If specified, the object to attach to the robot end-effector when
                 solving for this trajectory
 
         Returns:
-            2-tuple or list of MotionGenResult: If @return_all is set, will return the raw MotionGenResult
-                object(s) computed from internal batch trajectory computations. If not set, will return 2-tuple
+            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
                 (success, results), where success is a (N,)-shaped boolean tensor representing whether each requested
                 target pos / quat successfully generated a motion plan, and results is a (N,)-shaped array of
                 corresponding JointState objects.

omnigibson/transition_rules.py

@@ -363,15 +363,19 @@ def __init__(self, filter_1_name, filter_2_name):
     def refresh(self, object_candidates):
         # Check whether we can use optimized computation or not -- this is determined by whether or not any objects
         # in our collision set are kinematic only
-        self._optimized = not th.any(
-            th.tensor(
-                [
-                    obj.kinematic_only or obj.prim_type == PrimType.CLOTH
-                    for f in (self._filter_1_name, self._filter_2_name)
-                    for obj in object_candidates[f]
-                ]
-            )
-        )
+        # self._optimized = not th.any(
+        #     th.tensor(
+        #         [
+        #             obj.kinematic_only or obj.prim_type == PrimType.CLOTH
+        #             for f in (self._filter_1_name, self._filter_2_name)
+        #             for obj in object_candidates[f]
+        #         ]
+        #     )
+        # )
+
+        # TODO: RigidContactAPI sometimes has false negatives (returning zero impulses when there are contacts), so we will stick
+        # with the non-optimized version for now. We will fix this in a future release.
+        self._optimized = False
 
         if self._optimized:
             # Register idx mappings

tests/test_transition_rules.py

@@ -230,7 +230,7 @@ def test_dicing_rule_cooked(env):
         assert env.scene.object_registry("name", obj.name) is not None
 
     table_knife.set_position_orientation(
-        position=[-0.05, 0.0, 0.07], orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0], dtype=th.float32))
+        position=[-0.05, 0.0, 0.065], orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0], dtype=th.float32))
     )
     og.sim.step()
 
@@ -280,7 +280,7 @@ def test_dicing_rule_uncooked(env):
         assert env.scene.object_registry("name", obj.name) is not None
 
     table_knife.set_position_orientation(
-        position=[-0.05, 0.0, 0.07], orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0], dtype=th.float32))
+        position=[-0.05, 0.0, 0.065], orientation=T.euler2quat(th.tensor([-math.pi / 2, 0, 0], dtype=th.float32))
     )
     og.sim.step()
 
@@ -466,7 +466,7 @@ def test_mixing_rule_failure_recipe_systems(env):
     assert lemonade.n_particles == 0
     assert sludge.n_particles == 0
 
-    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.11], orientation=[0, 0, 0, 1])
+    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.08], orientation=[0, 0, 0, 1])
     og.sim.step()
 
     assert tablespoon.states[Touching].get_value(bowl)
@@ -510,7 +510,7 @@ def test_mixing_rule_failure_nonrecipe_systems(env):
     assert lemonade.n_particles == 0
     assert sludge.n_particles == 0
 
-    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.11], orientation=[0, 0, 0, 1])
+    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.08], orientation=[0, 0, 0, 1])
     og.sim.step()
 
     assert tablespoon.states[Touching].get_value(bowl)
@@ -550,7 +550,7 @@ def test_mixing_rule_success(env):
 
     assert lemonade.n_particles == 0
 
-    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.11], orientation=[0, 0, 0, 1])
+    tablespoon.set_position_orientation(position=[0.04, 0.0, 0.08], orientation=[0, 0, 0, 1])
     og.sim.step()
 
     assert tablespoon.states[Touching].get_value(bowl)