Revive action primitives examples and tests #2209
120 passed, 2 failed and 9 skipped
❌ 11113791236-tests-test_controllers/test_controllers.xml
1 tests were completed in 795s with 0 passed, 1 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 1❌ | 795s |
❌ pytest
tests.test_controllers
❌ test_arm_control
def test_arm_control():
✅ 11113791236-tests-test_data_collection/test_data_collection.xml
1 tests were completed in 763s with 1 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 1✅ | 763s |
✅ pytest
tests.test_data_collection
✅ test_data_collect_and_playback
✅ 11113791236-tests-test_dump_load_states/test_dump_load_states.xml
4 tests were completed in 1855s with 4 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 4✅ | 1855s |
✅ pytest
tests.test_dump_load_states
✅ test_dump_load
✅ test_dump_load_serialized
✅ test_save_restore_partial
✅ test_save_restore_full
✅ 11113791236-tests-test_envs/test_envs.xml
5 tests were completed in 751s with 5 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 5✅ | 751s |
✅ pytest
tests.test_envs
✅ test_dummy_task
✅ test_point_reaching_task
✅ test_point_navigation_task
✅ test_behavior_task
✅ test_rs_int_full_load
✅ 11113791236-tests-test_multiple_envs/test_multiple_envs.xml
10 tests were completed in 528s with 8 passed, 0 failed and 2 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 8✅ | 2⚪ | 528s |
✅ 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
✅ 11113791236-tests-test_object_removal/test_object_removal.xml
2 tests were completed in 1224s with 2 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 2✅ | 1224s |
✅ pytest
tests.test_object_removal
✅ test_removal_and_readdition
✅ test_readdition
✅ 11113791236-tests-test_object_states/test_object_states.xml
33 tests were completed in 2009s with 33 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 33✅ | 2009s |
✅ 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
❌ 11113791236-tests-test_primitives/test_primitives.xml
10 tests were completed in 2662s with 5 passed, 1 failed and 4 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 5✅ | 1❌ | 4⚪ | 2662s |
❌ pytest
tests.test_primitives.TestPrimitives
✅ test_navigate[Tiago]
✅ test_navigate[Fetch]
✅ test_grasp[Tiago]
✅ test_grasp[Fetch]
❌ test_place[Tiago]
self = <test_primitives.TestPrimitives object at 0x7ff1d07bed40>
✅ test_place[Fetch]
⚪ test_open_prismatic[Tiago]
⚪ test_open_prismatic[Fetch]
⚪ test_open_revolute[Tiago]
⚪ test_open_revolute[Fetch]
✅ 11113791236-tests-test_robot_states_flatcache/test_robot_states_flatcache.xml
3 tests were completed in 1395s with 3 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 3✅ | 1395s |
✅ pytest
tests.test_robot_states_flatcache
✅ test_camera_pose_flatcache_on
✅ test_robot_load_drive
✅ test_grasping_mode
✅ 11113791236-tests-test_robot_states_no_flatcache/test_robot_states_no_flatcache.xml
3 tests were completed in 470s with 3 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 3✅ | 470s |
✅ pytest
tests.test_robot_states_no_flatcache
✅ test_camera_pose_flatcache_off
✅ test_camera_semantic_segmentation
✅ test_object_in_FOV_of_robot
✅ 11113791236-tests-test_robot_teleoperation/test_robot_teleoperation.xml
1 tests were completed in 33ms with 0 passed, 0 failed and 1 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 1⚪ | 33ms |
✅ pytest
tests.test_robot_teleoperation
⚪ test_teleop
✅ 11113791236-tests-test_scene_graph/test_scene_graph.xml
1 tests were completed in 482s with 1 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 1✅ | 482s |
✅ pytest
tests.test_scene_graph
✅ test_scene_graph
✅ 11113791236-tests-test_sensors/test_sensors.xml
2 tests were completed in 1938s with 2 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 2✅ | 1938s |
✅ pytest
tests.test_sensors
✅ test_segmentation_modalities
✅ test_bbox_modalities
✅ 11113791236-tests-test_symbolic_primitives/test_symbolic_primitives.xml
20 tests were completed in 1546s with 18 passed, 0 failed and 2 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 18✅ | 2⚪ | 1546s |
✅ pytest
tests.test_symbolic_primitives.TestSymbolicPrimitives
✅ test_in_hand_state[Fetch]
✅ test_open[Fetch]
✅ test_close[Fetch]
✅ test_place_inside[Fetch]
✅ test_place_ontop[Fetch]
✅ test_toggle_on[Fetch]
✅ test_soak_under[Fetch]
✅ test_wipe[Fetch]
⚪ test_cut[Fetch]
✅ test_persistent_sticky_grasping[Fetch]
✅ 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]
✅ 11113791236-tests-test_systems/test_systems.xml
1 tests were completed in 1496s with 1 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 1✅ | 1496s |
✅ pytest
tests.test_systems
✅ test_system_clear
✅ 11113791236-tests-test_transform_utils/test_transform_utils.xml
34 tests were completed in 515ms with 34 passed, 0 failed and 0 skipped.
Test suite | Passed | Failed | Skipped | Time |
---|---|---|---|---|
pytest | 34✅ | 515ms |
✅ 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
Annotations
Check failure on line 0 in 11113791236-tests-test_controllers/test_controllers.xml
github-actions / Test Results
pytest ► tests.test_controllers ► test_arm_control
Failed test found in:
11113791236-tests-test_controllers/test_controllers.xml
Error:
def test_arm_control():
Raw output
def test_arm_control():
# Create env
cfg = {
"scene": {
"type": "Scene",
},
"objects": [],
"robots": [
{
"type": "FrankaPanda",
"obs_modalities": "rgb",
"position": [150, 150, 100],
"orientation": [0, 0, 0, 1],
"action_normalize": False,
},
{
"type": "Fetch",
"obs_modalities": "rgb",
"position": [150, 150, 105],
"orientation": [0, 0, 0, 1],
"action_normalize": False,
},
{
"type": "Tiago",
"obs_modalities": "rgb",
"position": [150, 150, 110],
"orientation": [0, 0, 0, 1],
"action_normalize": False,
},
{
"type": "A1",
"obs_modalities": "rgb",
"position": [150, 150, 115],
"orientation": [0, 0, 0, 1],
"action_normalize": False,
},
{
"type": "R1",
"obs_modalities": "rgb",
"position": [150, 150, 120],
"orientation": [0, 0, 0, 1],
"action_normalize": False,
},
],
}
env = og.Environment(configs=cfg)
# Define error functions to use
def check_zero_error(curr_position, init_position, tol=1e-2):
return th.norm(curr_position - init_position).item() < tol
def check_forward_error(curr_position, init_position, tol=1e-2, forward_tol=1e-2):
# x should be positive
return (curr_position[0] - init_position[0]).item() > forward_tol and th.norm(
curr_position[[1, 2]] - init_position[[1, 2]]
).item() < tol
def check_side_error(curr_position, init_position, tol=1e-2, side_tol=1e-2):
# y should be positive
return (curr_position[1] - init_position[1]).item() > side_tol and th.norm(
curr_position[[0, 2]] - init_position[[0, 2]]
).item() < tol
def check_up_error(curr_position, init_position, tol=1e-2, up_tol=1e-2):
# z should be positive
return (curr_position[2] - init_position[2]).item() > up_tol and th.norm(
curr_position[[0, 1]] - init_position[[0, 1]]
).item() < tol
def check_ori_error(curr_orientation, init_orientation, tol=0.1):
ori_err_normalized = th.norm(
T.quat2axisangle(T.mat2quat(T.quat2mat(init_orientation).T @ T.quat2mat(curr_orientation)))
).item() / (np.pi * 2)
ori_err = np.abs(np.pi * 2 * (np.round(ori_err_normalized) - ori_err_normalized))
return ori_err < tol
# All functions take in (target, curr, init) tuple
err_checks = {
"pose_delta_ori": {
"zero": {
"pos": lambda target, curr, init: check_zero_error(curr, init),
"ori": lambda target, curr, init: check_ori_error(curr, init),
},
"forward": {
"pos": lambda target, curr, init: check_forward_error(curr, init),
"ori": lambda target, curr, init: check_ori_error(curr, init),
},
"side": {
"pos": lambda target, curr, init: check_side_error(curr, init),
"ori": lambda target, curr, init: check_ori_error(curr, init),
},
"up": {
"pos": lambda target, curr, init: check_up_error(curr, init),
"ori": lambda target, curr, init: check_ori_error(curr, init),
},
"rotate": {
"pos": lambda target, curr, init: check_zero_error(curr, init),
"ori": None,
},
"base_move": {
"pos": lambda target, curr, init: check_zero_error(
curr, init, tol=0.02
), # Slightly bigger tolerance with base moving
"ori": lambda target, curr, init: check_ori_error(curr, init),
},
},
"absolute_pose": {
"zero": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr),
},
"forward": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr),
},
"side": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr),
},
"up": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr),
},
"rotate": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr, tol=0.05),
},
"base_move": {
"pos": lambda target, curr, init: check_zero_error(target, curr, tol=5e-3),
"ori": lambda target, curr, init: check_ori_error(target, curr),
},
},
}
n_steps = {
"pose_delta_ori": {
"zero": 10,
"forward": 10,
"side": 10,
"up": 10,
"rotate": 10,
"base_move": 30,
},
"absolute_pose": {
"zero": 50,
"forward": 50,
"side": 50,
"up": 50,
"rotate": 50,
"base_move": 50,
},
}
# Position the robots, reset them, and keep them still
for i, robot in enumerate(env.robots):
robot.set_position_orientation(
position=th.tensor([0.0, i * 5.0, 0.0]), orientation=T.euler2quat(th.tensor([0.0, 0.0, np.pi / 3]))
)
robot.reset()
robot.keep_still()
# Take 10 steps to stabilize
for _ in range(10):
og.sim.step()
# Update initial state
env.scene.update_initial_state()
# Reset the environment and keep all robots still
env.reset()
for i, robot in enumerate(env.robots):
robot.keep_still()
# Record initial eef pose of all robots
initial_eef_pose = dict()
for i, robot in enumerate(env.robots):
initial_eef_pose[robot.name] = {arm: robot.get_relative_eef_pose(arm=arm) for arm in robot.arm_names}
for controller in ["InverseKinematicsController", "OperationalSpaceController"]:
for controller_mode in ["pose_delta_ori", "absolute_pose"]:
controller_kwargs = {
"mode": controller_mode,
}
if controller_mode == "absolute_pose":
controller_kwargs["command_input_limits"] = None
controller_kwargs["command_output_limits"] = None
actions = {
"zero": dict(),
"forward": dict(),
"side": dict(),
"up": dict(),
"rotate": dict(),
"base_move": dict(),
}
for i, robot in enumerate(env.robots):
controller_config = {f"arm_{arm}": {"name": controller, **controller_kwargs} for arm in robot.arm_names}
robot.reload_controllers(controller_config)
# reload the state for controllers
env.scene.update_initial_state()
# Define actions to use
zero_action = th.zeros(robot.action_dim)
forward_action = th.zeros(robot.action_dim)
side_action = th.zeros(robot.action_dim)
up_action = th.zeros(robot.action_dim)
rot_action = th.zeros(robot.action_dim)
# Populate actions and targets
for arm in robot.arm_names:
c_name = f"arm_{arm}"
start_idx = 0
init_eef_pos, init_eef_quat = initial_eef_pose[robot.name][arm]
for c in robot.controller_order:
if c == c_name:
break
start_idx += robot.controllers[c].command_dim
if controller_mode == "pose_delta_ori":
forward_action[start_idx] = 0.1
side_action[start_idx + 1] = 0.1
up_action[start_idx + 2] = 0.1
rot_action[start_idx + 3] = 0.1
elif controller_mode == "absolute_pose":
for act in [zero_action, forward_action, side_action, up_action, rot_action]:
act[start_idx : start_idx + 3] = init_eef_pos.clone()
act[start_idx + 3 : start_idx + 6] = T.quat2axisangle(init_eef_quat.clone())
forward_action[start_idx] += 0.1
side_action[start_idx + 1] += 0.1
up_action[start_idx + 2] += 0.1
rot_action[start_idx + 3 : start_idx + 6] = T.quat2axisangle(
T.quat_multiply(T.euler2quat(th.tensor([th.pi / 10, 0, 0])), init_eef_quat.clone())
)
else:
raise ValueError(f"Got invalid controller mode: {controller}")
actions["zero"][robot.name] = zero_action
actions["forward"][robot.name] = forward_action
actions["side"][robot.name] = side_action
actions["up"][robot.name] = up_action
actions["rotate"][robot.name] = rot_action
# Add base movement action if locomotion robot
base_move_action = zero_action.clone()
if isinstance(robot, LocomotionRobot):
c_name = "base"
start_idx = 0
for c in robot.controller_order:
if c == c_name:
break
start_idx += robot.controllers[c].command_dim
base_move_action[start_idx] = 0.1
actions["base_move"][robot.name] = base_move_action
# For each action set, reset all robots, then run actions and see if arm moves in expected way
for action_name, action in actions.items():
# Reset the environment and keep all robots still
env.reset()
for i, robot in enumerate(env.robots):
robot.keep_still()
# Take N steps with given action and check for error
for _ in range(n_steps[controller_mode][action_name]):
env.step(action)
for i, robot in enumerate(env.robots):
for arm in robot.arm_names:
# Make sure no arm joints are at their limit
normalized_qpos = robot.get_joint_positions(normalized=True)[robot.arm_control_idx[arm]]
assert not th.any(th.abs(normalized_qpos) == 1.0), (
f"controller [{controller}], mode [{controller_mode}], robot [{robot.model_name}], arm [{arm}], action [{action_name}]:\n"
f"Some joints are at their limit (normalized values): {normalized_qpos}"
)
init_pos, init_quat = initial_eef_pose[robot.name][arm]
curr_pos, curr_quat = robot.get_relative_eef_pose(arm=arm)
arm_controller = robot.controllers[f"arm_{arm}"]
arm_goal = arm_controller.goal
target_pos = arm_goal["target_pos"]
target_quat = (
arm_goal["target_quat"]
if controller == "InverseKinematicsController"
else T.mat2quat(arm_goal["target_ori_mat"])
)
pos_check = err_checks[controller_mode][action_name]["pos"]
if pos_check is not None:
is_valid_pos = pos_check(target_pos, curr_pos, init_pos)
> assert is_valid_pos, (
f"Got mismatch for controller [{controller}], mode [{controller_mode}], robot [{robot.model_name}], action [{action_name}]\n"
f"target_pos: {target_pos}, curr_pos: {curr_pos}, init_pos: {init_pos}"
)
E AssertionError: Got mismatch for controller [OperationalSpaceController], mode [pose_delta_ori], robot [R1], action [base_move]
E target_pos: tensor([0.4690, 0.4162, 1.3529]), curr_pos: tensor([0.4689, 0.4162, 1.3529]), init_pos: tensor([0.4877, 0.4158, 1.3434])
E assert False
tests/test_controllers.py:296: AssertionError
Check failure on line 0 in 11113791236-tests-test_primitives/test_primitives.xml
github-actions / Test Results
pytest ► tests.test_primitives.TestPrimitives ► test_place[Tiago]
Failed test found in:
11113791236-tests-test_primitives/test_primitives.xml
Error:
self = <test_primitives.TestPrimitives object at 0x7ff1d07bed40>
Raw output
self = <test_primitives.TestPrimitives object at 0x7ff1d07bed40>
robot = 'Tiago'
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.2],
"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"],)]
> assert primitive_tester(env, objects, primitives, primitives_args)
E AssertionError: assert False
E + where False = primitive_tester(<omnigibson.envs.env_base.Environment object at 0x7ff1d9df0070>, [{'object': <omnigibson.objects.dataset_object.DatasetObject object at 0x7ff1d9139b10>, 'orientation': [0, 0, 0, 1], '...ts.dataset_object.DatasetObject object at 0x7ff1d9511000>, 'orientation': [0, 0, 0, 1], 'position': [-0.3, -0.8, 0.5]}], [<StarterSemanticActionPrimitiveSet.GRASP: 1>, <StarterSemanticActionPrimitiveSet.PLACE_ON_TOP: 2>], [(<omnigibson.objects.dataset_object.DatasetObject object at 0x7ff1d9511000>,), (<omnigibson.objects.dataset_object.DatasetObject object at 0x7ff1d9139b10>,)])
tests/test_primitives.py:141: AssertionError