feat: bare metal rcsss examples

pyproject.toml

@@ -19,6 +19,7 @@ dependencies = ["websockets>=11.0",
     "pyopengl",
     "pyrealsense2~=2.55.1",
     "pillow~=10.3",
+    "python-dotenv==1.0.1",
     # NOTE: when changing the mujoco version, also change it in requirements_dev.txt
     "mujoco==3.1.5"
 ]

python/examples/cartesian_control.py → python/examples/env_cartesian_control.py

@@ -4,7 +4,7 @@
 from rcsss.desk import FCI, Desk, DummyResourceManager
 from rcsss.envs.base import ControlMode, RobotInstance
 
-from common import hw_env_rel, sim_env_rel
+from env_common import hw_env_rel, sim_env_rel
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -22,26 +22,31 @@
 
 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
+            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), 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:
+        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+            env_rel = hw_env_rel(ROBOT_IP, ControlMode.CARTESIAN_TQuart)
+        else:
+            env_rel = sim_env_rel(ControlMode.CARTESIAN_TQuart)
+
+        print(env_rel.unwrapped.robot.get_cartesian_position())
+
         for _ in range(10):
             for _ in range(10):
+                # move 1cm in x direction (forward) and close gripper
                 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):
+                # move 1cm in negative x direction (backward) and open gripper
                 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:

python/examples/common.py → python/examples/env_common.py

@@ -31,14 +31,15 @@ def hw_env_rel(ip: str, control_mode: ControlMode) -> gym.Env[ObsArmsGr, Limited
         sys.exit()
     robot = rcsss.hw.FR3(ip, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
     robot_cfg = FR3Config()
+    robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
     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_cfg.force = 30
     gripper = rcsss.hw.FrankaHand(ip, gripper_cfg)
     env_hw = GripperWrapper(env_hw, gripper, binary=True)
 

python/examples/joint_control.py → python/examples/env_joint_control.py

@@ -1,10 +1,12 @@
 import logging
+import mujoco
+import rcsss
 
 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
+from env_common import hw_env_rel, sim_env_rel
 
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
@@ -22,20 +24,26 @@
 
 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
+            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False
         )
     else:
-        env_rel = sim_env_rel(ControlMode.JOINTS)
         resource_manger = DummyResourceManager()
-
     with resource_manger:
+
+        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+            env_rel = hw_env_rel(ROBOT_IP, ControlMode.JOINTS)
+        else:
+            env_rel = sim_env_rel(ControlMode.JOINTS)
+
         for _ in range(10):
             obs, info = env_rel.reset()
-            for _ in range(10):
+            for _ in range(3):
+                # sample random relative action and execute it
                 act = env_rel.action_space.sample()
+                # if the first is open, then it does not open
+                act["gripper"] = 1
                 obs, reward, terminated, truncated, info = env_rel.step(act)
                 if truncated or terminated:
                     logger.info("Truncated or terminated!")

python/examples/fr3.py

@@ -0,0 +1,178 @@
+import logging
+import sys
+
+import numpy as np
+import rcsss
+from dotenv import dotenv_values
+from rcsss import sim
+from rcsss._core.hw import FR3Config, IKController
+from rcsss._core.sim import CameraType
+from rcsss.camera.sim import SimCameraConfig, SimCameraSet, SimCameraSetConfig
+from rcsss.desk import FCI, Desk, DummyResourceManager
+from rcsss.envs.base import RobotInstance
+
+ROBOT_IP = "192.168.101.1"
+ROBOT_INSTANCE = RobotInstance.SIMULATION
+
+logger = logging.getLogger(__name__)
+logger.setLevel(logging.DEBUG)
+logger.addHandler(logging.StreamHandler())
+
+"""
+This examples demonstrates the direct robot control api without gym env.
+
+First the robot is moved in x, y and z direction 5cm. Then the arm is rotated to the right
+and trying to grasp an object placed 25cm to the right of it. Afterwards it moves back
+to the home position.
+
+
+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 "lab" not in rcsss.scenes:
+        logger.error("This pip package was not built with the UTN lab models, aborting.")
+        sys.exit()
+    if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+        creds = dotenv_values()
+        resource_manger = FCI(
+            Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False
+        )
+    else:
+        resource_manger = DummyResourceManager()
+
+
+    with resource_manger:
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            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)
+
+            gripper_cfg = sim.FHConfig()
+            gripper = sim.FrankaHand(simulation, "0", gripper_cfg)
+
+            # add camera to have a rendering gui
+            cameras = {
+                "default_free": SimCameraConfig(identifier="", type=int(CameraType.default_free), on_screen_render=True),
+                "wrist": SimCameraConfig(identifier="eye-in-hand_0", type=int(CameraType.fixed), on_screen_render=False),
+                # TODO: odd behavior when not both cameras are used: only last image is shown
+            }
+            cam_cfg = SimCameraSetConfig(cameras=cameras, resolution_width=1280, resolution_height=720, frame_rate=20)
+            camera_set = SimCameraSet(simulation, cam_cfg)
+            resource_manger = DummyResourceManager()
+
+        else:
+            robot = rcsss.hw.FR3(ROBOT_IP, str(rcsss.scenes["lab"].parent / "fr3.urdf"))
+            robot_cfg = FR3Config()
+            robot_cfg.tcp_offset = rcsss.common.Pose(rcsss.common.FrankaHandTCPOffset())
+            robot_cfg.controller = IKController.robotics_library
+            robot.set_parameters(robot_cfg)
+
+            gripper_cfg = rcsss.hw.FHConfig()
+            gripper_cfg.epsilon_inner = gripper_cfg.epsilon_outer = 0.1
+            gripper_cfg.speed = 0.1
+            gripper_cfg.force = 30
+            gripper = rcsss.hw.FrankaHand(ROBOT_IP, gripper_cfg)
+            creds = dotenv_values()
+            resource_manger = FCI(
+                Desk(ROBOT_IP, creds["FR3_USERNAME"], creds["FR3_PASSWORD"]), unlock=False, lock_when_done=False
+            )
+            input("the robot is going to move, press enter whenever you are ready")
+
+
+        # move to home position and open gripper
+        robot.move_home()
+        gripper.open()
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+        logger.info("Robot is in home position, gripper is open")
+
+        # 5cm in x direction
+        robot.set_cartesian_position(
+            robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0.05, 0, 0]))
+        )
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        # 5cm in y direction
+        robot.set_cartesian_position(
+            robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0.05, 0]))
+        )
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        # 5cm in z direction
+        robot.set_cartesian_position(
+            robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.05]))
+        )
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        # rotate the arm 90 degrees around the inverted y and z axis
+        new_pose = robot.get_cartesian_position() * rcsss.common.Pose(
+            translation=np.array([0, 0, 0]), rpy=rcsss.common.RPY(roll=0, pitch=-np.deg2rad(90), yaw=-np.deg2rad(90))
+        )
+        robot.set_cartesian_position(new_pose)
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+            input(
+                "hold an object 25cm in front of the gripper, the robot is going to grasp it, press enter when you are ready"
+            )
+
+        # move 25cm towards the gripper direction
+        robot.set_cartesian_position(
+            robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, 0.25]))
+        )
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        # grasp the object
+        gripper.grasp()
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        # move 25cm backward
+        robot.set_cartesian_position(
+            robot.get_cartesian_position() * rcsss.common.Pose(translation=np.array([0, 0, -0.25]))
+        )
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+            logger.debug(f"IK success: {robot.get_state().ik_success}")
+            logger.debug(f"sim converged: {simulation.is_converged()}")
+
+        if ROBOT_INSTANCE == RobotInstance.HARDWARE:
+            input("gripper is going to be open, press enter when you are ready")
+
+        # open gripper
+        gripper.open()
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+
+        # move back to home position
+        robot.move_home()
+        if ROBOT_INSTANCE == RobotInstance.SIMULATION:
+            simulation.step_until_convergence()
+
+
+if __name__ == "__main__":
+    main()

python/rcsss/envs/base.py

@@ -241,6 +241,7 @@ def reset(
         if options is not None:
             msg = "options not implemented yet"
             raise NotImplementedError(msg)
+        self.robot.reset()
         return self.get_obs(), {}
 
 
@@ -438,12 +439,10 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True):
         self.action_space.spaces.update(get_space(GripperDictType).spaces)
         self.gripper_key = get_space_keys(GripperDictType)[0]
         self._gripper = gripper
-        self._last_gripper_action = 1
         self.binary = binary
 
     def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]:
         self._gripper.reset()
-        self._last_gripper_action = 1
         return super().reset(**kwargs)
 
     def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]:
@@ -464,8 +463,10 @@ def action(self, action: dict[str, Any]) -> dict[str, Any]:
         assert self.gripper_key in action, "Gripper action not found."
 
         gripper_action = np.round(action["gripper"])
-        if gripper_action != self._last_gripper_action:
-            self._last_gripper_action = gripper_action
+        width = self._gripper.get_normalized_width()
+        if self.binary:
+            width = np.round(width)
+        if gripper_action != width:
             if self.binary:
                 self._gripper.grasp() if gripper_action == 0 else self._gripper.open()
             else:

python/rcsss/envs/hw.py

@@ -28,5 +28,4 @@ def step(self, action: Any) -> tuple[dict[str, Any], SupportsFloat, bool, bool,
     def reset(
         self, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
-        self.hw_robot.move_home()
         return super().reset(seed=seed, options=options)

python/rcsss/envs/sim.py

@@ -31,9 +31,9 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo
     def reset(
         self, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
-        self.sim_robot.reset()
+        obs, info = super().reset(seed=seed, options=options)
         self.sim.step(1)
-        return super().reset(seed=seed, options=options)
+        return obs, info
 
 
 class CollisionGuard(gym.Wrapper[dict[str, Any], dict[str, Any], dict[str, Any], dict[str, Any]]):