def test_curobo():
        # Make sure object states are enabled
        assert gm.ENABLE_OBJECT_STATES
    
        # Create env
        cfg = {
            "env": {
                "action_frequency": 30,
                "physics_frequency": 300,
            },
            "scene": {
                "type": "Scene",
            },
            "objects": [
                {
                    "type": "PrimitiveObject",
                    "name": "obj0",
                    "primitive_type": "Cube",
                    "scale": [0.4, 0.4, 0.4],
                    "fixed_base": True,
                    "position": [0.5, -0.1, 0.2],
                    "orientation": [0, 0, 0, 1],
                },
                {
                    "type": "PrimitiveObject",
                    "name": "eef_marker_0",
                    "primitive_type": "Sphere",
                    "radius": 0.05,
                    "visual_only": True,
                    "position": [0, 0, 0],
                    "orientation": [0, 0, 0, 1],
                    "rgba": [1, 0, 0, 1],
                },
                {
                    "type": "PrimitiveObject",
                    "name": "eef_marker_1",
                    "primitive_type": "Sphere",
                    "radius": 0.05,
                    "visual_only": True,
                    "position": [0, 0, 0],
                    "orientation": [0, 0, 0, 1],
                    "rgba": [0, 1, 0, 1],
                },
            ],
            "robots": [],
        }
    
        robot_cfgs = [
            {
                "type": "FrankaPanda",
                "obs_modalities": "rgb",
                "position": [0.7, -0.55, 0.0],
                "orientation": [0, 0, 0.707, 0.707],
                "self_collisions": True,
                "action_normalize": False,
                "controller_config": {
                    "arm_0": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "gripper_0": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                },
            },
            {
                "type": "R1",
                "obs_modalities": "rgb",
                "position": [0.7, -0.7, 0.056],
                "orientation": [0, 0, 0.707, 0.707],
                "self_collisions": True,
                "action_normalize": False,
                "controller_config": {
                    "base": {
                        "name": "HolonomicBaseJointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_impedances": False,
                    },
                    "trunk": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "arm_left": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "arm_right": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "gripper_left": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "gripper_right": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                },
            },
            {
                "type": "Tiago",
                "obs_modalities": "rgb",
                "position": [0.7, -0.85, 0],
                "orientation": [0, 0, 0.707, 0.707],
                "self_collisions": True,
                "action_normalize": False,
                "controller_config": {
                    "base": {
                        "name": "HolonomicBaseJointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_impedances": False,
                    },
                    "trunk": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "camera": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "arm_left": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "arm_right": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "gripper_left": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                    "gripper_right": {
                        "name": "JointController",
                        "motor_type": "position",
                        "command_input_limits": None,
                        "use_delta_commands": False,
                        "use_impedances": False,
                    },
                },
            },
        ]
        for robot_cfg in robot_cfgs:
            cfg["robots"] = [robot_cfg]
    
            env = og.Environment(configs=cfg)
            robot = env.robots[0]
            obj = env.scene.object_registry("name", "obj0")
            eef_markers = [env.scene.object_registry("name", f"eef_marker_{i}") for i in range(2)]
    
            floor_touching_base_link_prim_paths = (
                [os.path.join(robot.prim_path, link_name) for link_name in robot.floor_touching_base_link_names]
                if isinstance(robot, LocomotionRobot)
                else []
            )
    
            robot.reset()
    
            # Open the gripper(s) to match cuRobo's default state
            for arm_name in robot.gripper_control_idx.keys():
                gripper_control_idx = robot.gripper_control_idx[arm_name]
                robot.set_joint_positions(th.ones_like(gripper_control_idx), indices=gripper_control_idx, normalized=True)
    
            robot.keep_still()
    
            for _ in range(5):
                og.sim.step()
    
            env.scene.update_initial_state()
            env.scene.reset()
    
            # Create CuRobo instance
            batch_size = 2
            n_samples = 20
    
            cmg = CuRoboMotionGenerator(
                robot=robot,
                batch_size=batch_size,
                debug=False,
                use_cuda_graph=True,
                collision_activation_distance=0.03,  # Use larger activation distance for better reproducibility
                use_default_embodiment_only=True,
            )
    
            # Sample values for robot
            th.manual_seed(1)
            lo, hi = robot.joint_lower_limits.clone().view(1, -1), robot.joint_upper_limits.clone().view(1, -1)
    
            if isinstance(robot, HolonomicBaseRobot):
                lo[0, :2] = -0.1
                lo[0, 2:5] = 0.0
                lo[0, 5] = -math.pi
                hi[0, :2] = 0.1
                hi[0, 2:5] = 0.0
                hi[0, 5] = math.pi
    
            random_qs = lo + th.rand((n_samples, robot.n_dof)) * (hi - lo)
    
            # Test collision with the environment (not including self-collisions)
            collision_results = cmg.check_collisions(q=random_qs)
    
            target_pos, target_quat = defaultdict(list), defaultdict(list)
    
            floor_plane_prim_paths = {child.GetPath().pathString for child in og.sim.floor_plane._prim.GetChildren()}
    
            # View results
            false_positive = 0
            false_negative = 0
    
            for i, (q, curobo_has_contact) in enumerate(zip(random_qs, collision_results)):
                # Set robot to desired qpos
                robot.set_joint_positions(q)
                robot.keep_still()
                og.sim.step_physics()
    
                # To debug
                # cmg.save_visualization(robot.get_joint_positions(), "/scr/chengshu/Downloads/test.obj")
    
                # Sanity check in the GUI that the robot pose makes sense
                for _ in range(10):
                    og.sim.render()
    
                # Validate that expected collision result is correct
                self_collision_pairs = set()
                floor_contact_pairs = set()
                wheel_contact_pairs = set()
                obj_contact_pairs = set()
    
                for contact in robot.contact_list():
                    assert contact.body0 in robot.link_prim_paths
                    if contact.body1 in robot.link_prim_paths:
                        self_collision_pairs.add((contact.body0, contact.body1))
                    elif contact.body1 in floor_plane_prim_paths:
                        if contact.body0 not in floor_touching_base_link_prim_paths:
                            floor_contact_pairs.add((contact.body0, contact.body1))
                        else:
                            wheel_contact_pairs.add((contact.body0, contact.body1))
                    elif contact.body1 in obj.link_prim_paths:
                        obj_contact_pairs.add((contact.body0, contact.body1))
                    else:
                        assert False, f"Unexpected contact pair: {contact.body0}, {contact.body1}"
    
                touching_itself = len(self_collision_pairs) > 0
                touching_floor = len(floor_contact_pairs) > 0
                touching_object = len(obj_contact_pairs) > 0
    
                curobo_has_contact = curobo_has_contact.item()
                physx_has_contact = touching_itself or touching_floor or touching_object
    
                # cuRobo reports contact, but physx reports no contact
                if curobo_has_contact and not physx_has_contact:
                    false_positive += 1
                    print(
                        f"False positive {i}: {curobo_has_contact} vs. {physx_has_contact} (touching_itself/obj/floor: {touching_itself}/{touching_object}/{touching_floor})"
                    )
    
                # physx reports contact, but cuRobo reports no contact
                elif not curobo_has_contact and physx_has_contact:
                    false_negative += 1
                    print(
                        f"False negative {i}: {curobo_has_contact} vs. {physx_has_contact} (touching_itself/obj/floor: {touching_itself}/{touching_object}/{touching_floor})"
                    )
    
                # neither cuRobo nor physx reports contact, valid planning goals
                elif not curobo_has_contact and not physx_has_contact:
                    for arm_name in robot.arm_names:
                        eef_pos, eef_quat = robot.get_eef_pose(arm_name)
                        target_pos[robot.eef_link_names[arm_name]].append(eef_pos)
                        target_quat[robot.eef_link_names[arm_name]].append(eef_quat)
    
            print(
                f"Collision checking false positive: {false_positive / n_samples}, false negative: {false_negative / n_samples}."
            )
            assert (
                false_positive / n_samples == 0.0
            ), f"Collision checking false positive rate: {false_positive / n_samples}, should be == 0.0."
            assert (
                false_negative / n_samples == 0.0
            ), f"Collision checking false negative rate: {false_negative / n_samples}, should be == 0.0."
    
            env.scene.reset()
    
            for arm_name in robot.arm_names:
                target_pos[robot.eef_link_names[arm_name]] = th.stack(target_pos[robot.eef_link_names[arm_name]], dim=0)
                target_quat[robot.eef_link_names[arm_name]] = th.stack(target_quat[robot.eef_link_names[arm_name]], dim=0)
    
            # Cast defaultdict to dict
            target_pos = dict(target_pos)
            target_quat = dict(target_quat)
    
            print(f"Planning for {len(target_pos[robot.eef_link_names[robot.default_arm]])} eef targets...")
    
            # Make sure robot is kept still for better determinism before planning
            robot.keep_still()
            og.sim.step_physics()
    
            # Generate collision-free trajectories to the sampled eef poses (including self-collisions)
            successes, traj_paths = cmg.compute_trajectories(
                target_pos=target_pos,
                target_quat=target_quat,
                is_local=False,
                max_attempts=1,
                timeout=60.0,
                ik_fail_return=5,
                enable_finetune_trajopt=True,
                finetune_attempts=1,
                return_full_result=False,
                success_ratio=1.0,
                attached_obj=None,
            )
    
            # Make sure collision-free trajectories are generated
            success_rate = successes.double().mean().item()
            print(f"Collision-free trajectory generation success rate: {success_rate}")
            assert success_rate == 1.0, f"Collision-free trajectory generation success rate: {success_rate}"
    
            # 1cm and 3 degrees error tolerance for prismatic and revolute joints, respectively
            error_tol = th.tensor(
                [0.01 if joint.joint_type == "PrismaticJoint" else 3.0 / 180.0 * math.pi for joint in robot.joints.values()]
            )
    
            # for bypass_physics in [True, False]:
            for bypass_physics in [True]:
                for traj_idx, (success, traj_path) in enumerate(zip(successes, traj_paths)):
                    if not success:
                        continue
    
                    # Reset the environment
                    env.scene.reset()
    
                    # Move the markers to the desired eef positions
                    for marker, arm_name in zip(eef_markers, robot.arm_names):
                        eef_link_name = robot.eef_link_names[arm_name]
                        marker.set_position_orientation(position=target_pos[eef_link_name][traj_idx])
    
                    q_traj = cmg.path_to_joint_trajectory(traj_path)
                    # joint_positions_set_point = []
                    # joint_positions_response = []
                    for i, q in enumerate(q_traj):
                        if bypass_physics:
                            print(f"Teleporting waypoint {i}/{len(q_traj)}")
                            robot.set_joint_positions(q)
                            robot.keep_still()
                            og.sim.step()
                            for contact in robot.contact_list():
                                assert contact.body0 in robot.link_prim_paths
                                if (
                                    contact.body1 in floor_plane_prim_paths
                                    and contact.body0 in floor_touching_base_link_prim_paths
                                ):
                                    continue
                                if th.tensor(list(contact.impulse)).norm() == 0:
                                    continue
                                print(f"Unexpected contact pair during traj rollout: {contact.body0}, {contact.body1}")
>                               assert (
                                    False
                                ), f"Unexpected contact pair during traj rollout: {contact.body0}, {contact.body1}"
E                               AssertionError: Unexpected contact pair during traj rollout: /World/scene_0/controllable__tiago__robot_ripppe/gripper_right_left_finger_link, /World/scene_0/controllable__tiago__robot_ripppe/torso_fixed_column_link
E                               assert False

tests/test_curobo.py:409: AssertionError

self = <test_primitives.TestPrimitives object at 0x14d5d0152c80>, robot = 'R1'

    def test_place(self, robot):
        categories = ["floors", "ceilings", "walls", "coffee_table"]
        env = setup_environment(categories, robot=robot)
    
        objects = []
        obj_1 = {
            "object": DatasetObject(name="table", category="breakfast_table", model="rjgmmy", scale=[0.3, 0.3, 0.3]),
            "position": [-0.7, 0.5, 0.09],
            "orientation": [0, 0, 0, 1],
        }
        obj_2 = {
            "object": DatasetObject(name="cologne", category="bottle_of_cologne", model="lyipur"),
            "position": [-0.3, -0.8, 0.5],
            "orientation": [0, 0, 0, 1],
        }
        objects.append(obj_1)
        objects.append(obj_2)
    
        primitives = [StarterSemanticActionPrimitiveSet.GRASP, StarterSemanticActionPrimitiveSet.PLACE_ON_TOP]
        primitives_args = [(obj_2["object"],), (obj_1["object"],)]
    
>       primitive_tester(env, objects, primitives, primitives_args)

tests/test_primitives.py:148: 
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 
tests/test_primitives.py:85: in primitive_tester
    execute_controller(controller.apply_ref(primitive, *args, attempts=1), env)
tests/test_primitives.py:68: in execute_controller
    for action in ctrl_gen:
_ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ _ 

self = <omnigibson.action_primitives.starter_semantic_action_primitives.StarterSemanticActionPrimitives object at 0x14d5b6f252a0>
primitive = <StarterSemanticActionPrimitiveSet.GRASP: 1>, attempts = 1
args = (<omnigibson.objects.dataset_object.DatasetObject object at 0x14d5cdd1a950>,)
ctrl = <bound method StarterSemanticActionPrimitives._grasp of <omnigibson.action_primitives.starter_semantic_action_primitives.StarterSemanticActionPrimitives object at 0x14d5b6f252a0>>
errors = [ActionPrimitiveError('EXECUTION_ERROR: Could not reach the target articulation joint positions. Try again. Additional info: None')]

    def apply_ref(self, primitive, *args, attempts=5):
        """
        Yields action for robot to execute the primitive with the given arguments.
    
        Args:
            primitive (StarterSemanticActionPrimitiveSet): Primitive to execute
            args: Arguments for the primitive
            attempts (int): Number of attempts to make before raising an error
    
        Yields:
            th.tensor or None: Action array for one step for the robot to execute the primitve or None if primitive completed
    
        Raises:
            ActionPrimitiveError: If primitive fails to execute
        """
        ctrl = self.controller_functions[primitive]
    
        errors = []
        for _ in range(attempts):
            # Attempt
            success = False
            try:
                yield from ctrl(*args)
                success = True
            except ActionPrimitiveError as e:
                errors.append(e)
    
            try:
                # If we're not holding anything, release the hand so it doesn't stick to anything else.
                if not self._get_obj_in_hand():
                    yield from self._execute_release()
            except ActionPrimitiveError:
                pass
    
            try:
                # Make sure we retract the arms after every step
                yield from self._reset_robot()
            except ActionPrimitiveError:
                pass
    
            try:
                # Settle before returning.
                yield from self._settle_robot()
            except ActionPrimitiveError:
                pass
    
            # Stop on success
            if success:
                return
    
>       raise ActionPrimitiveErrorGroup(errors)
E       omnigibson.action_primitives.action_primitive_set_base.ActionPrimitiveErrorGroup: An error occurred during each attempt of this action.
E       
E       Attempt 0: EXECUTION_ERROR: Could not reach the target articulation joint positions. Try again. Additional info: None

omnigibson/action_primitives/starter_semantic_action_primitives.py:325: ActionPrimitiveErrorGroup