fix(env): type assertion and improved tests

- fixed type assertion location (noticed by tests)
- optimized tests by removing camera in most cases

Author: juelg

python/rcsss/envs/base.py

@@ -386,7 +386,6 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TRPY and self.trpy_key in action:
             assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode."
             assert isinstance(self.max_mov, tuple)
-            assert isinstance(self._last_action, common.Pose)
             pose_space = cast(gym.spaces.Box, get_space(TRPYDictType).spaces[self.trpy_key])
 
             if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None:
@@ -400,6 +399,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
                 )
                 self._last_action = clipped_pose_offset
             else:
+                assert isinstance(self._last_action, common.Pose)
                 pose_diff = (
                     common.Pose(
                         translation=action[self.trpy_key][:3],
@@ -430,7 +430,6 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         elif self.unwrapped.get_control_mode() == ControlMode.CARTESIAN_TQuart and self.tquart_key in action:
             assert isinstance(self._origin, common.Pose), "Invalid origin type given the control mode."
             assert isinstance(self.max_mov, tuple)
-            assert isinstance(self._last_action, common.Pose)
             pose_space = cast(gym.spaces.Box, get_space(TQuartDictType).spaces[self.tquart_key])
 
             if self.relative_to == RelativeTo.LAST_STEP or self._last_action is None:
@@ -444,6 +443,7 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
                 )
                 self._last_action = clipped_pose_offset
             else:
+                assert isinstance(self._last_action, common.Pose)
                 pose_diff = (
                     common.Pose(
                         translation=action[self.tquart_key][:3],

python/tests/test_sim_envs.py

@@ -108,10 +108,10 @@ def test_non_zero_action_trpy(self, cfg):
         obs, _, _, _, info = env.step(non_zero_action)
         self.assert_no_pose_change(info, expected_obs, obs)
 
-    def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg):
+    def test_relative_zero_action_trpy(self, cfg, gripper_cfg):
         # env creation
         env = fr3_sim_env(
-            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5
+            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5
         )
         obs_initial, _ = env.reset()
         # action to be performed
@@ -120,10 +120,10 @@ def test_relative_zero_action_trpy(self, cfg, gripper_cfg, cam_cfg):
         obs, _, _, _, info = env.step(zero_action)
         self.assert_no_pose_change(info, obs_initial, obs)
 
-    def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg):
+    def test_relative_non_zero_action(self, cfg, gripper_cfg):
         # env creation
         env = fr3_sim_env(
-            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5
+            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5
         )
         obs_initial, _ = env.reset()
         # action to be performed
@@ -135,13 +135,13 @@ def test_relative_non_zero_action(self, cfg, gripper_cfg, cam_cfg):
         obs, _, _, _, info = env.step(non_zero_action)
         self.assert_no_pose_change(info, obs_initial, expected_obs)
 
-    def test_collision_trpy(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_trpy(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by sim
         """
         # env creation
         env = fr3_sim_env(
-            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None
+            ControlMode.CARTESIAN_TRPY, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None
         )
         obs, _ = env.reset()
         # an obvious below ground collision action
@@ -152,7 +152,7 @@ def test_collision_trpy(self, cfg, gripper_cfg, cam_cfg):
         obs, _, _, _, info = env.step(collision_action)
         self.assert_collision(info)
 
-    def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_guard_trpy(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by the CollisionGuard
         """
@@ -162,7 +162,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg):
             cfg,
             gripper_cfg=gripper_cfg,
             collision_guard=True,
-            camera_set_cfg=cam_cfg,
+            camera_set_cfg=None,
             max_relative_movement=None,
         )
         obs, _ = env.reset()
@@ -236,13 +236,13 @@ def test_zero_action_tquart(self, cfg):
         obs, _, _, _, info = env.step(zero_action)
         self.assert_no_pose_change(info, obs_initial, obs)
 
-    def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg):
+    def test_relative_zero_action_tquart(self, cfg, gripper_cfg):
         # env creation
         env_rel = fr3_sim_env(
             ControlMode.CARTESIAN_TQuart,
             cfg,
             gripper_cfg=gripper_cfg,
-            camera_set_cfg=cam_cfg,
+            camera_set_cfg=None,
             max_relative_movement=0.5,
         )
         obs_initial, _ = env_rel.reset()
@@ -251,7 +251,7 @@ def test_relative_zero_action_tquart(self, cfg, gripper_cfg, cam_cfg):
         obs, _, _, _, info = env_rel.step(zero_rel_action)
         self.assert_no_pose_change(info, obs_initial, obs)
 
-    def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_tquart(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by sim
         """
@@ -260,7 +260,7 @@ def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg):
             ControlMode.CARTESIAN_TQuart,
             cfg,
             gripper_cfg=gripper_cfg,
-            camera_set_cfg=cam_cfg,
+            camera_set_cfg=None,
             max_relative_movement=None,
         )
         obs, _ = env.reset()
@@ -272,7 +272,7 @@ def test_collision_tquart(self, cfg, gripper_cfg, cam_cfg):
         _, _, _, _, info = env.step(collision_action)
         self.assert_collision(info)
 
-    def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_guard_tquart(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by the CollisionGuard
         """
@@ -282,7 +282,7 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg):
             cfg,
             gripper_cfg=gripper_cfg,
             collision_guard=True,
-            camera_set_cfg=cam_cfg,
+            camera_set_cfg=None,
             max_relative_movement=None,
         )
         obs, _ = env.reset()
@@ -346,13 +346,13 @@ def test_non_zero_action_joints(self, cfg):
         assert info["is_sim_converged"]
         assert np.allclose(obs["joints"], non_zero_action["joints"], atol=0.01, rtol=0)
 
-    def test_collision_joints(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_joints(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by the CollisionGuard
         """
         # env creation
         env = fr3_sim_env(
-            ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=None
+            ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=None
         )
         env.reset()
         # the below action is a test_case where there is an obvious collision regardless of the gripper action
@@ -361,7 +361,7 @@ def test_collision_joints(self, cfg, gripper_cfg, cam_cfg):
         _, _, _, _, info = env.step(collision_act)
         self.assert_collision(info)
 
-    def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg):
+    def test_collision_guard_joints(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by sim
         """
@@ -371,7 +371,7 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg):
             cfg,
             gripper_cfg=gripper_cfg,
             collision_guard=True,
-            camera_set_cfg=cam_cfg,
+            camera_set_cfg=None,
             max_relative_movement=None,
         )
         env.reset()
@@ -387,13 +387,13 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg):
         # assure that the robot did not move
         assert np.allclose(p1, p2)
 
-    def test_relative_zero_action_joints(self, cfg, gripper_cfg, cam_cfg):
+    def test_relative_zero_action_joints(self, cfg, gripper_cfg):
         """
         Check that an obvious collision is detected by the CollisionGuard
         """
         # env creation
         env = fr3_sim_env(
-            ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=cam_cfg, max_relative_movement=0.5
+            ControlMode.JOINTS, cfg, gripper_cfg=gripper_cfg, camera_set_cfg=None, max_relative_movement=0.5
         )
         obs_initial, _ = env.reset()
         act = JointsDictType(joints=np.array([0, 0, 0, 0, 0, 0, 0], dtype=np.float32))