Merge pull request #151 from utn-mi/juelg/sim-collision-guard-same-mjcf

refactor(factories): same mjcf scene for sim collision guard

Author: juelg

python/examples/env_cartesian_control.py

@@ -66,7 +66,7 @@ def main():
             env_rel = fr3_sim_env(
                 control_mode=ControlMode.CARTESIAN_TQuart,
                 robot_cfg=default_fr3_sim_robot_cfg(),
-                collision_guard=None,
+                collision_guard=False,
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
                 camera_set_cfg=default_mujoco_cameraset_cfg(),
                 max_relative_movement=0.5,

python/examples/env_joint_control.py

@@ -68,7 +68,7 @@ def main():
         else:
             env_rel = fr3_sim_env(
                 control_mode=ControlMode.JOINTS,
-                collision_guard=None,
+                collision_guard=False,
                 robot_cfg=default_fr3_sim_robot_cfg(),
                 gripper_cfg=default_fr3_sim_gripper_cfg(),
                 camera_set_cfg=default_mujoco_cameraset_cfg(),

python/rcsss/envs/factories.py

@@ -166,7 +166,7 @@ def default_mujoco_cameraset_cfg():
 def fr3_sim_env(
     control_mode: ControlMode,
     robot_cfg: rcsss.sim.FR3Config,
-    collision_guard: str | PathLike | None = None,
+    collision_guard: bool = False,
     gripper_cfg: rcsss.sim.FHConfig | None = None,
     camera_set_cfg: SimCameraSetConfig | None = None,
     max_relative_movement: float | tuple[float, float] | None = None,
@@ -180,8 +180,7 @@ def fr3_sim_env(
     Args:
         control_mode (ControlMode): Control mode for the robot.
         robot_cfg (rcsss.sim.FR3Config): Configuration for the FR3 robot.
-        collision_guard (str | PathLike | None): Key to a scene (requires UTN compatible scene package to be present)
-            or the path to a mujoco scene for collision guarding. If None, collision guarding is not used.
+        collision_guard (bool): Whether to use collision guarding. If True, the same mjcf scene is used for collision guarding.
         gripper_cfg (rcsss.sim.FHConfig | None): Configuration for the gripper. If None, no gripper is used.
         camera_set_cfg (SimCameraSetConfig | None): Configuration for the camera set. If None, no cameras are used.
         max_relative_movement (float | tuple[float, float] | None): Maximum allowed movement. If float, it restricts
@@ -215,10 +214,10 @@ def fr3_sim_env(
         gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
         env = GripperWrapper(env, gripper, binary=False)
 
-    if collision_guard is not None:
+    if collision_guard:
         env = CollisionGuard.env_from_xml_paths(
             env,
-            str(rcsss.scenes.get(str(collision_guard), collision_guard)),
+            str(rcsss.scenes.get(str(mjcf), mjcf)),
             urdf_path,
             gripper=gripper_cfg is not None,
             check_home_collision=False,

python/rcsss/envs/sim.py

@@ -78,7 +78,7 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, b
         # modify action to be joint angles down stream
         if info["collision"] or not info["ik_success"] or not info["is_sim_converged"]:
             # return old obs, with truncated and print warning
-            self._logger.warning("Collision detected! Truncating episode.")
+            self._logger.warning("Collision detected! Truncating episode: %s", info)
             if self.last_obs is None:
                 msg = "Collisions detected and no old observation."
                 raise RuntimeError(msg)

python/tests/test_sim_envs.py

@@ -141,7 +141,7 @@ def test_collision_guard_trpy(self, cfg, gripper_cfg, cam_cfg):
             ControlMode.CARTESIAN_TRPY,
             cfg,
             gripper_cfg=gripper_cfg,
-            collision_guard="fr3_empty_world",
+            collision_guard=True,
             camera_set_cfg=cam_cfg,
             max_relative_movement=0.5,
         )
@@ -243,7 +243,7 @@ def test_collision_guard_tquart(self, cfg, gripper_cfg, cam_cfg):
             ControlMode.CARTESIAN_TQuart,
             cfg,
             gripper_cfg=gripper_cfg,
-            collision_guard="fr3_empty_world",
+            collision_guard=True,
             camera_set_cfg=cam_cfg,
             max_relative_movement=None,
         )
@@ -318,7 +318,7 @@ def test_collision_guard_joints(self, cfg, gripper_cfg, cam_cfg):
             ControlMode.JOINTS,
             cfg,
             gripper_cfg=gripper_cfg,
-            collision_guard="fr3_empty_world",
+            collision_guard=True,
             camera_set_cfg=cam_cfg,
             max_relative_movement=None,
         )