102 lines
3.9 KiB
Python
102 lines
3.9 KiB
Python
import importlib
|
|
import unittest
|
|
from unittest import mock
|
|
|
|
import numpy as np
|
|
|
|
from roboimi.envs.double_pos_ctrl_env import make_sim_env
|
|
from roboimi.utils import act_ex_utils
|
|
from roboimi.utils.constants import SIM_TASK_CONFIGS
|
|
|
|
|
|
class AirInsertTaskRegistrationTest(unittest.TestCase):
|
|
def test_sim_task_configs_registers_air_insert_ring_bar(self):
|
|
self.assertIn("sim_air_insert_ring_bar", SIM_TASK_CONFIGS)
|
|
|
|
def test_sample_air_insert_ring_bar_state_returns_explicit_named_mapping(self):
|
|
sampler = getattr(act_ex_utils, "sample_air_insert_ring_bar_state", None)
|
|
self.assertIsNotNone(
|
|
sampler,
|
|
"Expected roboimi.utils.act_ex_utils.sample_air_insert_ring_bar_state()",
|
|
)
|
|
|
|
task_state = sampler()
|
|
|
|
self.assertEqual(
|
|
list(task_state.keys()),
|
|
["ring_pos", "ring_quat", "bar_pos", "bar_quat"],
|
|
)
|
|
self.assertEqual(task_state["ring_pos"].shape, (3,))
|
|
self.assertEqual(task_state["ring_quat"].shape, (4,))
|
|
self.assertEqual(task_state["bar_pos"].shape, (3,))
|
|
self.assertEqual(task_state["bar_quat"].shape, (4,))
|
|
|
|
def test_sample_air_insert_ring_bar_state_uses_fixed_quats_and_left_right_planar_ranges(self):
|
|
sampler = getattr(act_ex_utils, "sample_air_insert_ring_bar_state", None)
|
|
self.assertIsNotNone(
|
|
sampler,
|
|
"Expected roboimi.utils.act_ex_utils.sample_air_insert_ring_bar_state()",
|
|
)
|
|
|
|
task_state = sampler()
|
|
|
|
np.testing.assert_array_equal(task_state["ring_quat"], np.array([1.0, 0.0, 0.0, 0.0]))
|
|
np.testing.assert_array_equal(task_state["bar_quat"], np.array([1.0, 0.0, 0.0, 0.0]))
|
|
self.assertGreaterEqual(task_state["ring_pos"][0], -0.20)
|
|
self.assertLessEqual(task_state["ring_pos"][0], -0.05)
|
|
self.assertGreaterEqual(task_state["ring_pos"][1], 0.70)
|
|
self.assertLessEqual(task_state["ring_pos"][1], 1.00)
|
|
self.assertAlmostEqual(float(task_state["ring_pos"][2]), 0.47)
|
|
self.assertGreaterEqual(task_state["bar_pos"][0], 0.05)
|
|
self.assertLessEqual(task_state["bar_pos"][0], 0.20)
|
|
self.assertGreaterEqual(task_state["bar_pos"][1], 0.70)
|
|
self.assertLessEqual(task_state["bar_pos"][1], 1.00)
|
|
self.assertAlmostEqual(float(task_state["bar_pos"][2]), 0.47)
|
|
|
|
def test_make_sim_env_dispatches_air_insert_ring_bar_headless(self):
|
|
try:
|
|
air_insert_env = importlib.import_module("roboimi.envs.double_air_insert_env")
|
|
except Exception as exc:
|
|
self.fail(f"Expected roboimi.envs.double_air_insert_env to be importable: {exc}")
|
|
|
|
air_insert_cls = getattr(air_insert_env, "DualDianaMed_Air_Insert", None)
|
|
self.assertIsNotNone(
|
|
air_insert_cls,
|
|
"Expected roboimi.envs.double_air_insert_env.DualDianaMed_Air_Insert",
|
|
)
|
|
|
|
diana_med = importlib.import_module("roboimi.assets.robots.diana_med")
|
|
ring_bar_robot_cls = getattr(diana_med, "BiDianaMedRingBar", None)
|
|
self.assertIsNotNone(
|
|
ring_bar_robot_cls,
|
|
"Expected roboimi.assets.robots.diana_med.BiDianaMedRingBar",
|
|
)
|
|
|
|
fake_env = object()
|
|
with mock.patch.object(
|
|
diana_med,
|
|
"BiDianaMedRingBar",
|
|
return_value="robot",
|
|
), mock.patch.object(
|
|
air_insert_env,
|
|
"DualDianaMed_Air_Insert",
|
|
return_value=fake_env,
|
|
) as env_cls:
|
|
try:
|
|
env = make_sim_env("sim_air_insert_ring_bar", headless=True)
|
|
except Exception as exc:
|
|
self.fail(f"make_sim_env should dispatch sim_air_insert_ring_bar without error: {exc}")
|
|
|
|
self.assertIs(env, fake_env)
|
|
env_cls.assert_called_once_with(
|
|
robot="robot",
|
|
is_render=False,
|
|
control_freq=30,
|
|
is_interpolate=True,
|
|
cam_view="angle",
|
|
)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
unittest.main()
|