From 264be12c8836727fae4010869a7fb3e815b28d74 Mon Sep 17 00:00:00 2001 From: Chengshu Li Date: Thu, 3 Oct 2024 21:40:08 -0700 Subject: [PATCH] fix transition rule tests and curobo path spec (the previous way works, but is deprecated) --- omnigibson/action_primitives/curobo.py | 23 +++++++++++++++-------- omnigibson/transition_rules.py | 22 +++++++++++++--------- tests/test_transition_rules.py | 10 +++++----- 3 files changed, 33 insertions(+), 22 deletions(-) diff --git a/omnigibson/action_primitives/curobo.py b/omnigibson/action_primitives/curobo.py index a4b1fcaf3..c732b4492 100644 --- a/omnigibson/action_primitives/curobo.py +++ b/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,8 +344,8 @@ 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 @@ -346,8 +353,8 @@ def compute_trajectories( 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. diff --git a/omnigibson/transition_rules.py b/omnigibson/transition_rules.py index 569610e95..86c0817fd 100644 --- a/omnigibson/transition_rules.py +++ b/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 diff --git a/tests/test_transition_rules.py b/tests/test_transition_rules.py index 7421496a8..5ef1eeb51 100644 --- a/tests/test_transition_rules.py +++ b/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)