Skip to content
Tests

Fix/playback scale #3242

Sign in to view logs

Fix/playback scale

Fix/playback scale #3242

GitHub Actions / Test Results succeeded Feb 20, 2025 in 0s

148 passed, 1 failed and 13 skipped

Tests failed

Report Passed Failed Skipped Time
test_controllers.xml 1✅ 915s
test_curobo.xml 1❌ 694s
test_data_collection.xml 1✅ 1663s
test_dump_load_states.xml 4✅ 1940s
test_envs.xml 5✅ 1876s
test_multiple_envs.xml 8✅ 2⚪ 550s
test_object_removal.xml 2✅ 1032s
test_object_states.xml 33✅ 1352s
test_primitives.xml 6✅ 4⚪ 2117s
test_robot_states_flatcache.xml 3✅ 922s
test_robot_states_no_flatcache.xml 3✅ 365s
test_robot_teleoperation.xml 1⚪ 26ms
test_scene_graph.xml 1✅ 405s
test_sensors.xml 2✅ 1674s
test_symbolic_primitives.xml 14✅ 6⚪ 984s
test_systems.xml 1✅ 1718s
test_transform_utils.xml 34✅ 541ms
test_transition_rules.xml 30✅ 1587s

✅ test_controllers.xml

1 tests were completed in 915s with 1 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 1✅ 915s

✅ pytest

tests.test_controllers
  ✅ test_arm_control

❌ test_curobo.xml

1 tests were completed in 694s with 0 passed, 1 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 1❌ 694s

❌ pytest

tests.test_curobo
  ❌ test_curobo
	def test_curobo():

✅ test_data_collection.xml

1 tests were completed in 1663s with 1 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 1✅ 1663s

✅ pytest

tests.test_data_collection
  ✅ test_data_collect_and_playback

✅ test_dump_load_states.xml

4 tests were completed in 1940s with 4 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 4✅ 1940s

✅ pytest

tests.test_dump_load_states
  ✅ test_dump_load
  ✅ test_dump_load_serialized
  ✅ test_save_restore_partial
  ✅ test_save_restore_full

✅ test_envs.xml

5 tests were completed in 1876s with 5 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 5✅ 1876s

✅ pytest

tests.test_envs
  ✅ test_dummy_task
  ✅ test_point_reaching_task
  ✅ test_point_navigation_task
  ✅ test_behavior_task
  ✅ test_rs_int_full_load

✅ test_multiple_envs.xml

10 tests were completed in 550s with 8 passed, 0 failed and 2 skipped.

Test suite Passed Failed Skipped Time
pytest 8✅ 2⚪ 550s

✅ pytest

tests.test_multiple_envs
  ✅ test_multi_scene_dump_load_states
  ✅ test_multi_scene_get_local_position
  ✅ test_multi_scene_set_local_position
  ✅ test_multi_scene_scene_prim
  ✅ test_multi_scene_particle_source
  ✅ test_multi_scene_position_orientation_relative_to_scene
  ✅ test_tiago_getter
  ✅ test_tiago_setter
  ⚪ test_behavior_getter
  ⚪ test_behavior_setter

✅ test_object_removal.xml

2 tests were completed in 1032s with 2 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 2✅ 1032s

✅ pytest

tests.test_object_removal
  ✅ test_removal_and_readdition
  ✅ test_readdition

✅ test_object_states.xml

33 tests were completed in 1352s with 33 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 33✅ 1352s

✅ pytest

tests.test_object_states
  ✅ test_on_top
  ✅ test_inside
  ✅ test_under
  ✅ test_touching
  ✅ test_contact_bodies
  ✅ test_next_to
  ✅ test_overlaid
  ✅ test_pose
  ✅ test_joint
  ✅ test_aabb
  ✅ test_adjacency
  ✅ test_temperature
  ✅ test_max_temperature
  ✅ test_heat_source_or_sink
  ✅ test_cooked
  ✅ test_burnt
  ✅ test_frozen
  ✅ test_heated
  ✅ test_on_fire
  ✅ test_toggled_on
  ✅ test_attached_to
  ✅ test_particle_source
  ✅ test_particle_sink
  ✅ test_particle_applier
  ✅ test_particle_remover
  ✅ test_saturated
  ✅ test_open
  ✅ test_folded_unfolded
  ✅ test_draped
  ✅ test_filled
  ✅ test_contains
  ✅ test_covered
  ✅ test_clear_sim

✅ test_primitives.xml

10 tests were completed in 2117s with 6 passed, 0 failed and 4 skipped.

Test suite Passed Failed Skipped Time
pytest 6✅ 4⚪ 2117s

✅ pytest

tests.test_primitives.TestPrimitives
  ✅ test_navigate[Tiago]
  ✅ test_navigate[R1]
  ✅ test_grasp[Tiago]
  ✅ test_grasp[R1]
  ✅ test_place[Tiago]
  ✅ test_place[R1]
  ⚪ test_open_prismatic[Tiago]
  ⚪ test_open_prismatic[R1]
  ⚪ test_open_revolute[Tiago]
  ⚪ test_open_revolute[R1]

✅ test_robot_states_flatcache.xml

3 tests were completed in 922s with 3 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 3✅ 922s

✅ pytest

tests.test_robot_states_flatcache
  ✅ test_camera_pose_flatcache_on
  ✅ test_robot_load_drive
  ✅ test_grasping_mode

✅ test_robot_states_no_flatcache.xml

3 tests were completed in 365s with 3 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 3✅ 365s

✅ pytest

tests.test_robot_states_no_flatcache
  ✅ test_camera_pose_flatcache_off
  ✅ test_camera_semantic_segmentation
  ✅ test_object_in_FOV_of_robot

✅ test_robot_teleoperation.xml

1 tests were completed in 26ms with 0 passed, 0 failed and 1 skipped.

Test suite Passed Failed Skipped Time
pytest 1⚪ 26ms

✅ pytest

tests.test_robot_teleoperation
  ⚪ test_teleop

✅ test_scene_graph.xml

1 tests were completed in 405s with 1 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 1✅ 405s

✅ pytest

tests.test_scene_graph
  ✅ test_scene_graph

✅ test_sensors.xml

2 tests were completed in 1674s with 2 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 2✅ 1674s

✅ pytest

tests.test_sensors
  ✅ test_segmentation_modalities
  ✅ test_bbox_modalities

✅ test_symbolic_primitives.xml

20 tests were completed in 984s with 14 passed, 0 failed and 6 skipped.

Test suite Passed Failed Skipped Time
pytest 14✅ 6⚪ 984s

✅ pytest

tests.test_symbolic_primitives.TestSymbolicPrimitives
  ✅ test_in_hand_state[R1]
  ✅ test_open[R1]
  ✅ test_close[R1]
  ✅ test_place_inside[R1]
  ✅ test_place_ontop[R1]
  ✅ test_toggle_on[R1]
  ⚪ test_soak_under[R1]
  ⚪ test_wipe[R1]
  ⚪ test_cut[R1]
  ✅ test_persistent_sticky_grasping[R1]
  ✅ test_in_hand_state[Tiago]
  ✅ test_open[Tiago]
  ✅ test_close[Tiago]
  ✅ test_place_inside[Tiago]
  ✅ test_place_ontop[Tiago]
  ✅ test_toggle_on[Tiago]
  ⚪ test_soak_under[Tiago]
  ⚪ test_wipe[Tiago]
  ⚪ test_cut[Tiago]
  ✅ test_persistent_sticky_grasping[Tiago]

✅ test_systems.xml

1 tests were completed in 1718s with 1 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 1✅ 1718s

✅ pytest

tests.test_systems
  ✅ test_system_clear

✅ test_transform_utils.xml

34 tests were completed in 541ms with 34 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 34✅ 541ms

✅ pytest

tests.test_transform_utils.TestQuaternionOperations
  ✅ test_quat2mat_special_cases
  ✅ test_quat_multiply
  ✅ test_quat_conjugate
  ✅ test_quat_inverse
  ✅ test_quat_distance
tests.test_transform_utils.TestVectorOperations
  ✅ test_normalize
  ✅ test_dot_product
  ✅ test_l2_distance
tests.test_transform_utils.TestMatrixOperations
  ✅ test_rotation_matrix_properties
  ✅ test_rotation_matrix
  ✅ test_transformation_matrix
  ✅ test_transformation_matrix_no_point
  ✅ test_matrix_inverse
tests.test_transform_utils.TestCoordinateTransformations
  ✅ test_cartesian_to_polar
tests.test_transform_utils.TestPoseTransformations
  ✅ test_pose2mat_and_mat2pose
  ✅ test_pose_inv
tests.test_transform_utils.TestAxisAngleConversions
  ✅ test_axisangle2quat_and_quat2axisangle
  ✅ test_vecs2axisangle
  ✅ test_vecs2quat
tests.test_transform_utils.TestEulerAngleConversions
  ✅ test_euler2quat_and_quat2euler
  ✅ test_euler2mat_and_mat2euler
tests.test_transform_utils.TestQuaternionApplications
  ✅ test_quat_apply
  ✅ test_quat_slerp
tests.test_transform_utils.TestTransformPoints
  ✅ test_transform_points_2d
  ✅ test_transform_points_3d
tests.test_transform_utils.TestMiscellaneousFunctions
  ✅ test_convert_quat
  ✅ test_random_quaternion
  ✅ test_random_axis_angle
  ✅ test_align_vector_sets
  ✅ test_copysign
  ✅ test_anorm
  ✅ test_check_quat_right_angle
  ✅ test_z_angle_from_quat
  ✅ test_integer_spiral_coordinates

✅ test_transition_rules.xml

30 tests were completed in 1587s with 30 passed, 0 failed and 0 skipped.

Test suite Passed Failed Skipped Time
pytest 30✅ 1587s

✅ pytest

tests.test_transition_rules
  ✅ test_dryer_rule
  ✅ test_washer_rule
  ✅ test_slicing_rule
  ✅ test_dicing_rule_cooked
  ✅ test_dicing_rule_uncooked
  ✅ test_melting_rule
  ✅ test_cooking_physical_particle_rule_failure_recipe_systems
  ✅ test_cooking_physical_particle_rule_success
  ✅ test_mixing_rule_failure_recipe_systems
  ✅ test_mixing_rule_failure_nonrecipe_systems
  ✅ test_mixing_rule_success
  ✅ test_cooking_system_rule_failure_recipe_systems
  ✅ test_cooking_system_rule_failure_nonrecipe_systems
  ✅ test_cooking_system_rule_failure_nonrecipe_objects
  ✅ test_cooking_system_rule_success
  ✅ test_cooking_object_rule_failure_wrong_container
  ✅ test_cooking_object_rule_failure_recipe_objects
  ✅ test_cooking_object_rule_failure_unary_states
  ✅ test_cooking_object_rule_failure_binary_system_states
  ✅ test_cooking_object_rule_failure_binary_object_states
  ✅ test_cooking_object_rule_failure_wrong_heat_source
  ✅ test_cooking_object_rule_success
  ✅ test_single_toggleable_machine_rule_output_system_failure_wrong_container
  ✅ test_single_toggleable_machine_rule_output_system_failure_recipe_systems
  ✅ test_single_toggleable_machine_rule_output_system_failure_recipe_objects
  ✅ test_single_toggleable_machine_rule_output_system_failure_nonrecipe_systems
  ✅ test_single_toggleable_machine_rule_output_system_failure_nonrecipe_objects
  ✅ test_single_toggleable_machine_rule_output_system_success
  ✅ test_single_toggleable_machine_rule_output_object_failure_unary_states
  ✅ test_single_toggleable_machine_rule_output_object_success

Annotations

Check failure on line 0 in test_curobo.xml

See this annotation in the file changed.

@github-actions github-actions / Test Results

pytest ► tests.test_curobo ► test_curobo 

Failed test found in:
  test_curobo.xml
Error:
  def test_curobo():
Raw output 
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.05,  # 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=100,
                timeout=60.0,
                ik_fail_return=50,
                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_left_right_finger_link, /World/scene_0/controllable__tiago__robot_ripppe/base_link
E                               assert False

tests/test_curobo.py:409: AssertionError
View more details on GitHub Actions