refactor(env): moved examples into own folder #111 and simplified them

python/examples/cartesian_control.py

@@ -0,0 +1,53 @@
+import logging
+
+from dotenv import dotenv_values
+from rcsss.desk import FCI, Desk, DummyResourceManager
+from rcsss.envs.base import ControlMode, RobotInstance
+
+from common import hw_env_rel, sim_env_rel
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+ROBOT_IP = "192.168.101.1"
+ROBOT_INSTANCE = RobotInstance.SIMULATION
+
+
+"""
+Create a .env file in the same directory as this file with the following content:
+FR3_USERNAME=<username on franka desk>
+FR3_PASSWORD=<password on franka desk>
+"""
+
+
+def main():
+    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        env_rel = hw_env_rel(ROBOT_IP, ControlMode.CARTESIAN_TQuart)
+        creds = dotenv_values()
+        resource_manger = FCI(
+            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False
+        )
+    else:
+        env_rel = sim_env_rel(ControlMode.CARTESIAN_TQuart)
+        resource_manger = DummyResourceManager()
+
+    print(env_rel.unwrapped.robot.get_cartesian_position())
+
+    with resource_manger:
+        for _ in range(10):
+            for _ in range(10):
+                act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+            for _ in range(10):
+                act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 1}
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+
+
+if __name__ == "__main__":
+    main()

python/examples/common.py

@@ -0,0 +1,107 @@
+import logging
+import sys
+
+import gymnasium as gym
+import numpy as np
+import rcsss
+from rcsss import sim
+from rcsss._core.hw import FR3Config
+from rcsss._core.sim import CameraType
+from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
+from rcsss.envs.base import (
+    CameraSetWrapper,
+    ControlMode,
+    FR3Env,
+    GripperWrapper,
+    LimitedJointsRelDictType,
+    ObsArmsGr,
+    ObsArmsGrCam,
+    RelativeActionSpace,
+)
+from rcsss.envs.hw import FR3HW
+from rcsss.envs.sim import CollisionGuard, FR3Sim
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+
+def hw_env_rel(ip: str, control_mode: ControlMode) -> gym.Env[ObsArmsGr, LimitedJointsRelDictType]:
+    if "lab" not in rcsss.scenes:
+        logger.error("This pip package was not built with the UTN lab models, aborting.")
+        sys.exit()
+    robot = rcsss.hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
+    robot_cfg = FR3Config()
+    robot.set_parameters(robot_cfg)
+    # the robot itself is always controlled in joint space with the robotics library IK
+    env = FR3Env(robot, ControlMode.JOINTS)
+    env_hw: gym.Env = FR3HW(env)
+    gripper_cfg = rcsss.hw.FHConfig()
+    gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
+    gripper_cfg.speed = 0.1
+    gripper_cfg.force = 10
+    gripper = rcsss.hw.FrankaHand(ip, gripper_cfg)
+    env_hw = GripperWrapper(env_hw, gripper, binary=True)
+
+    env_cg: gym.Env = CollisionGuard.env_from_xml_paths(
+        env_hw,
+        str(rcsss.scenes["fr3_empty_world"]),
+        str(rcsss.scenes["lab"].parent / "fr3.urdf"),
+        gripper=True,
+        check_home_collision=False,
+        camera=True,
+        control_mode=control_mode,
+        tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()),
+    )
+    env_rel = RelativeActionSpace(env_cg, max_mov=np.deg2rad(5))
+
+    # if you have a realsense camera:
+    # cameras = {
+    #     "side": BaseCameraConfig(identifier="243522070385"),
+    # }
+
+    # cam_cfg = RealSenseSetConfig(
+    #     cameras=cameras,
+    #     resolution_width=1280,
+    #     resolution_height=720,
+    #     frame_rate=15,
+    #     enable_imu=False,
+    #     enable_ir=True,
+    #     enable_ir_emitter=False,
+    # )
+
+    # camera_set = RealSenseCameraSet(cam_cfg)
+    # camera_set.start()
+    # env_rel = CameraSetWrapper(env_rel, camera_set)
+
+    env_rel.reset()
+    return env_rel
+
+
+def sim_env_rel(control_mode: ControlMode) -> gym.Env[ObsArmsGrCam, LimitedJointsRelDictType]:
+    if "lab" not in rcsss.scenes:
+        logger.error("This pip package was not built with the UTN lab models, aborting.")
+        sys.exit()
+    simulation = sim.Sim(rcsss.scenes["fr3_empty_world"])
+    robot = rcsss.sim.FR3(simulation, "0", str(rcsss.scenes["lab"].parent / "fr3.urdf"))
+    cfg = sim.FR3Config()
+    cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
+    cfg.ik_duration_in_milliseconds = 300
+    cfg.realtime = False
+    robot.set_parameters(cfg)
+    # env = FR3Env(robot, ControlMode.CARTESIAN_TQuart)
+    env = FR3Env(robot, control_mode)
+    env_sim = FR3Sim(env, simulation)
+    cameras = {
+        "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False),
+        "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True),
+    }
+    cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=10)
+    camera_set = SimCameraSet(simulation, cam_cfg)
+    env_cam: gym.Env = CameraSetWrapper(env_sim, camera_set)
+
+    gripper_cfg = sim.FHConfig()
+    gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
+    env_cam = GripperWrapper(env_cam, gripper, binary=True)
+    env_cam = RelativeActionSpace(env_cam, max_mov=np.deg2rad(5))
+    env_cam.reset()
+    return env_cam

python/examples/joint_control.py

@@ -0,0 +1,47 @@
+import logging
+
+from dotenv import dotenv_values
+from rcsss.desk import FCI, Desk, DummyResourceManager
+from rcsss.envs.base import ControlMode, RobotInstance
+
+from common import hw_env_rel, sim_env_rel
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.INFO)
+
+ROBOT_IP = "192.168.101.1"
+ROBOT_INSTANCE = RobotInstance.SIMULATION
+
+
+"""
+Create a .env file in the same directory as this file with the following content:
+FR3_USERNAME=<username on franka desk>
+FR3_PASSWORD=<password on franka desk>
+"""
+
+
+def main():
+    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        env_rel = hw_env_rel(ROBOT_IP, ControlMode.JOINTS)
+        creds = dotenv_values()
+        resource_manger = FCI(
+            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_USERNAME"]), unlock=False, lock_when_done=False
+        )
+    else:
+        env_rel = sim_env_rel(ControlMode.JOINTS)
+        resource_manger = DummyResourceManager()
+
+    with resource_manger:
+        for _ in range(10):
+            obs, info = env_rel.reset()
+            for _ in range(10):
+                act = env_rel.action_space.sample()
+                obs, reward, terminated, truncated, info = env_rel.step(act)
+                if truncated or terminated:
+                    logger.info("Truncated or terminated!")
+                    return
+                logger.info(act["gripper"], obs["gripper"])
+
+
+if __name__ == "__main__":
+    main()

python/rcsss/desk.py

@@ -78,6 +78,14 @@ class Token:
     token: str = ""
 
 
+class DummyResourceManager:
+    def __enter__(self):
+        pass
+
+    def __exit__(self, *args):
+        pass
+
+
 class Desk:
     """
     Connects to the control unit running the web-based Desk interface

python/rcsss/envs/base.py

@@ -132,9 +132,14 @@ class ObsArmsGrCamCG(ArmObsType, GripperDictType, CameraDictType):
 
 
 class ControlMode(Enum):
-    JOINTS = 1
-    CARTESIAN_TRPY = 2
-    CARTESIAN_TQuart = 3
+    JOINTS = auto()
+    CARTESIAN_TRPY = auto()
+    CARTESIAN_TQuart = auto()
+
+
+class RobotInstance(Enum):
+    HARDWARE = auto()
+    SIMULATION = auto()
 
 
 class FR3Env(gym.Env):

python/rcsss/envs/hw.py

@@ -2,12 +2,8 @@
 from typing import Any, SupportsFloat, cast
 
 import gymnasium as gym
-import rcsss
 from rcsss import hw
-from rcsss.config import read_config_yaml
-from rcsss.desk import FCI, Desk
-from rcsss.envs.base import ControlMode, FR3Env, GripperWrapper, RelativeActionSpace
-from rcsss.envs.sim import CollisionGuard
+from rcsss.envs.base import FR3Env
 
 _logger = logging.getLogger(__name__)
 
@@ -34,88 +30,3 @@ def reset(
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         self.hw_robot.move_home()
         return super().reset(seed=seed, options=options)
-
-
-def cartesian_example(ip: str, cfg_path: str):
-    cfg = read_config_yaml(cfg_path)
-    d = Desk(ip, cfg.hw.username, cfg.hw.password)
-    with FCI(d, unlock=True, lock_when_done=True):
-        logger = logging.getLogger(__name__)
-        robot = hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
-        env = FR3Env(robot, ControlMode.JOINTS)
-        env_hw: gym.Env = FR3HW(env)
-        gripper_cfg = hw.FHConfig()
-        gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
-        gripper_cfg.speed = 0.1
-        gripper_cfg.force = 10
-        gripper = hw.FrankaHand(ip, gripper_cfg)
-        env_hw = GripperWrapper(env_hw, gripper)
-
-        env_cam: gym.Env = CollisionGuard.env_from_xml_paths(
-            env_hw,
-            str(rcsss.scenes["fr3_empty_world"]),
-            str(rcsss.scenes["lab"].parent / "fr3.urdf"),
-            gripper=True,
-            check_home_collision=False,
-            camera=True,
-            control_mode=ControlMode.CARTESIAN_TQuart,
-            tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()),
-        )
-        env_cam = RelativeActionSpace(env_cam)
-        obs, info = env_cam.reset()
-        print(env_cam.unwrapped.robot.get_cartesian_position())
-        for _ in range(10):
-            for _ in range(10):
-                act = {"tquart": [0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
-                obs, reward, terminated, truncated, info = env_cam.step(act)
-                if truncated or terminated:
-                    logger.info("Truncated or terminated!")
-                    return
-            for _ in range(10):
-                act = {"tquart": [-0.01, 0, 0, 0, 0, 0, 1], "gripper": 0}
-                obs, reward, terminated, truncated, info = env_cam.step(act)
-                if truncated or terminated:
-                    logger.info("Truncated or terminated!")
-                    return
-
-
-def joints_example(ip: str, cfg_path: str):
-    cfg = read_config_yaml(cfg_path)
-    d = Desk(ip, cfg.hw.username, cfg.hw.password)
-    with FCI(d, unlock=True, lock_when_done=True):
-        logger = logging.getLogger(__name__)
-        robot = hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
-        env = FR3Env(robot, ControlMode.JOINTS)
-        env_hw: gym.Env = FR3HW(env)
-        gripper_cfg = hw.FHConfig()
-        gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
-        gripper_cfg.speed = 0.1
-        gripper_cfg.force = 10
-        gripper = hw.FrankaHand(ip, gripper_cfg)
-        env_hw = GripperWrapper(env_hw, gripper)
-
-        env_cam: gym.Env = CollisionGuard.env_from_xml_paths(
-            env_hw,
-            str(rcsss.scenes["fr3_empty_world"]),
-            str(rcsss.scenes["lab"].parent / "fr3.urdf"),
-            gripper=True,
-            check_home_collision=False,
-            camera=True,
-            control_mode=ControlMode.JOINTS,
-            tcp_offset=rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset()),
-        )
-        env_cam = RelativeActionSpace(env_cam)
-        for _ in range(10):
-            obs, info = env_cam.reset()
-            for _ in range(10):
-                act = env_cam.action_space.sample()
-                obs, reward, terminated, truncated, info = env_cam.step(act)
-                if truncated or terminated:
-                    logger.info("Truncated or terminated!")
-                    return
-                logger.info(act["gripper"], obs["gripper"])
-
-
-if __name__ == "__main__":
-    cartesian_example("192.168.101.1", "config.yaml")
-    # joints_example("192.168.101.1", "config.yaml")