Merge pull request #150 from utn-mi/juelg/collision-guard-visualization-gui

collision guard visualization gui

Author: juelg

README.md

@@ -16,13 +16,13 @@ pip install -r requirements_dev.txt
 ```
 3. Build and install RCS:
 ```shell
-pip install .
+pip install -ve .
 ```
 
 ### UTN Models
 If you have a token to download the UTN models, you can add them to the pip package using:
 ```shell
-pip install --config-settings=cmake.define.INCLUDE_UTN_MODELS=ON --config-settings=cmake.define.GITLAB_MODELS_TOKEN=<token> .
+pip install --config-settings=cmake.define.INCLUDE_UTN_MODELS=ON --config-settings=cmake.define.GITLAB_MODELS_TOKEN=<token> -ve .
 ```
 
 ## Usage

python/rcsss/envs/factories.py

@@ -129,9 +129,9 @@ def fr3_hw_env(
             urdf_path,
             gripper=True,
             check_home_collision=False,
-            camera=True,
             control_mode=control_mode,
             tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()),
+            sim_gui=True,
         )
     if max_relative_movement is not None:
         env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)
@@ -222,9 +222,9 @@ def fr3_sim_env(
             urdf_path,
             gripper=gripper_cfg is not None,
             check_home_collision=False,
-            camera=False,
             control_mode=control_mode,
             tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()),
+            sim_gui=True,
         )
     if max_relative_movement is not None:
         env = RelativeActionSpace(env, max_mov=max_relative_movement, relative_to=relative_to)

python/rcsss/envs/sim.py

@@ -4,9 +4,7 @@
 import gymnasium as gym
 import rcsss
 from rcsss import sim
-from rcsss._core.sim import CameraType
-from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
-from rcsss.envs.base import CameraSetWrapper, ControlMode, FR3Env, GripperWrapper
+from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper
 
 
 class FR3Sim(gym.Wrapper):
@@ -50,6 +48,7 @@ def __init__(
         collision_env: gym.Env,
         check_home_collision: bool = True,
         to_joint_control: bool = False,
+        sim_gui: bool = True,
     ):
         super().__init__(env)
         self.unwrapped: FR3Env
@@ -65,6 +64,8 @@ def __init__(
             ), "Previous control mode must be joints"
             # change action space
             self.action_space = self.collision_env.action_space
+        if sim_gui:
+            self.sim.open_gui()
 
     def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], SupportsFloat, bool, bool, dict[str, Any]]:
         # TODO: we should set the state of the sim to the state of the real robot
@@ -112,12 +113,12 @@ def env_from_xml_paths(
         env: gym.Env,
         mjmld: str,
         urdf: str,
-        id="0",
-        gripper=True,
-        check_home_collision=True,
-        tcp_offset=None,
+        id: str = "0",
+        gripper: bool = True,
+        check_home_collision: bool = True,
+        tcp_offset: rcsss.common.Pose | None = None,
         control_mode: ControlMode | None = None,
-        camera=False,
+        sim_gui: bool = True,
     ) -> "CollisionGuard":
         assert isinstance(env.unwrapped, FR3Env)
         simulation = sim.Sim(mjmld)
@@ -142,14 +143,6 @@ def env_from_xml_paths(
         c_env = FR3Sim(c_env, simulation)
         if gripper:
             gripper_cfg = sim.FHConfig()
-            gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
-            c_env = GripperWrapper(c_env, gripper)
-        if camera:
-            cameras = {
-                "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed)),
-                "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free)),
-            }
-            cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=640, resolution_height=480, frame_rate=5)
-            camera_set = SimCameraSet(simulation, cam_cfg)
-            c_env = CameraSetWrapper(c_env, camera_set)
-        return cls(env, simulation, c_env, check_home_collision, to_joint_control)
+            fh = sim.FrankaHand(simulation, id, gripper_cfg)
+            c_env = GripperWrapper(c_env, fh)
+        return cls(env, simulation, c_env, check_home_collision, to_joint_control, sim_gui)