diff --git a/python/examples/env_cartesian_control.py b/python/examples/env_cartesian_control.py index b80e3d59..cf107da5 100644 --- a/python/examples/env_cartesian_control.py +++ b/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, diff --git a/python/examples/env_joint_control.py b/python/examples/env_joint_control.py index d058bf7e..a79dc92e 100644 --- a/python/examples/env_joint_control.py +++ b/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(), diff --git a/python/rcsss/envs/factories.py b/python/rcsss/envs/factories.py index 9fcbda64..ccea92ee 100644 --- a/python/rcsss/envs/factories.py +++ b/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, diff --git a/python/rcsss/envs/sim.py b/python/rcsss/envs/sim.py index f703ae9a..8293ed04 100644 --- a/python/rcsss/envs/sim.py +++ b/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) diff --git a/python/tests/test_sim_envs.py b/python/tests/test_sim_envs.py index e876a696..02d22ee6 100644 --- a/python/tests/test_sim_envs.py +++ b/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, )