fix(teleop): remove double sleep for framerate in sim case

Author: juelg

examples/teleop/franka.py

@@ -50,8 +50,7 @@
 DIGIT_DICT = None
 
 
-DATASET_PATH = "test_iris"
-INSTRUCTION = "pick up cube"
+RECORD_FPS = 30
 
 robot2world = {
     "right": rcs.common.Pose(
@@ -123,12 +122,15 @@ def get_env():
 
         scene = EmptyWorldFR3Duo()
         sim_cfg_data = scene.config()
-        sim_cfg_data.sim_cfg = SimConfig(async_control=True, realtime=True, frequency=30, max_convergence_steps=500)
+        sim_cfg_data.sim_cfg = SimConfig(
+            async_control=True, realtime=True, frequency=RECORD_FPS, max_convergence_steps=500
+        )
         sim_cfg_data.relative_to = RelativeTo.CONFIGURED_ORIGIN
         if sim_cfg_data.root_frame_objects is None:
             sim_cfg_data.root_frame_objects = {}
         # cfg.root_frame_objects["green_cube"] = (rcs.OBJECT_PATHS["green_cube"], Pose(translation=[0.5, 0, 0.5], quaternion=[0, 0, 0, 1]))
-        sim_cfg_data.task_cfg = PickTaskConfig(robot_name="left")
+        # sim_cfg_data.task_cfg = PickTaskConfig(robot_name="left")
+        sim_cfg_data.task_cfg = ParallelPickTaskConfig()
 
         env_rel = scene.create_env(sim_cfg_data)
         env_rel = StorageWrapper(
@@ -144,7 +146,7 @@ def get_env():
 def main():
     env_rel, operator = get_env()
     env_rel.reset()
-    tele = TeleopLoop(env_rel, operator)
+    tele = TeleopLoop(env_rel, operator, env_frequency=RECORD_FPS, robot_platform=ROBOT_INSTANCE)
     with env_rel, tele:  # type: ignore
         tele.environment_step_loop()
 

python/rcs/operator/interface.py

@@ -7,6 +7,7 @@
 from time import sleep
 
 import gymnasium as gym
+from rcs._core.common import RobotPlatform
 from rcs.envs.base import ArmWithGripper, ControlMode, RelativeTo
 from rcs.sim.sim import Sim
 from rcs.utils import SimpleFrameRate
@@ -71,12 +72,14 @@ def __init__(
         env: gym.Env,
         operator: BaseOperator,
         env_frequency: int = 30,
+        robot_platform: RobotPlatform = RobotPlatform.HARDWARE,
         key_translation: dict[str, str] | None = None,
     ):
         super().__init__()
         self.env = env
         self.operator = operator
         self._exit_requested = False
+        self.robot_platform = robot_platform
         self.env_frequency = env_frequency
         if key_translation is None:
             # controller to robot translation
@@ -114,7 +117,9 @@ def _translate_keys(self, actions):
         return translated
 
     def environment_step_loop(self):
-        rate_limiter = SimpleFrameRate(self.env_frequency, "env loop")
+        rate_limiter = SimpleFrameRate(
+            self.env_frequency if self.robot_platform == RobotPlatform.HARDWARE else None, "env loop"
+        )
 
         # 0. Initial Reset to get current positions for untracked robots
         self._last_obs, _ = self.env.reset()

python/rcs/utils.py

@@ -6,7 +6,7 @@
 
 
 class SimpleFrameRate:
-    def __init__(self, frame_rate: float, loop_name: str = "SimpleFrameRate"):
+    def __init__(self, frame_rate: float | None, loop_name: str = "SimpleFrameRate"):
         """SimpleFrameRate is a utility class to manage frame rates in a simple way.
         It allows you to call it in a loop, and it will sleep the necessary time to maintain the desired frame rate.
 
@@ -22,6 +22,8 @@ def reset(self):
         self.t = None
 
     def __call__(self):
+        if self.frame_rate is None:
+            return
         if self.t is None:
             self.t = perf_counter()
             self._last_print = self.t