feat: adds digital twin and quest frame aligner

Author: juelg

python/rcs/envs/sim.py

@@ -455,7 +455,6 @@ class DigitalTwin(gym.Wrapper):
     def __init__(self, env, twin_env):
         super().__init__(env)
         self.twin_env = twin_env
-        assert self.twin_env.unwrapped.get_control_mode() == ControlMode.JOINTS
 
 
     def step(self, action):

python/rcs/envs/utils.py

@@ -2,6 +2,7 @@
 from os import PathLike
 
 from digit_interface import Digit
+from rcs._core import common
 from rcs._core.common import BaseCameraConfig
 from rcs._core.sim import CameraType, SimCameraConfig
 from rcs.camera.digit_cam import DigitCam
@@ -17,6 +18,7 @@
 def default_sim_robot_cfg(scene: str = "fr3_empty_world", idx: str = "0") -> sim.SimRobotConfig:
     robot_cfg = rcs.sim.SimRobotConfig()
     robot_cfg.robot_type = rcs.scenes[scene].robot_type
+    robot_cfg.tcp_offset = common.Pose(common.FrankaHandTCPOffset())
     robot_cfg.add_id(idx)
     if rcs.scenes[scene].mjb is not None:
         robot_cfg.mjcf_scene_path = rcs.scenes[scene].mjb

scripts/quest_align_frame.py

@@ -0,0 +1,9 @@
+from simpub.xr_device.meta_quest3 import MetaQuest3
+from quest_iris_dual_arm import MySimPublisher, MySimScene, MQ3_ADDR
+
+
+MySimPublisher(MySimScene(), MQ3_ADDR)
+reader = MetaQuest3("RCSNode")
+while True:
+    data = reader.get_controller_data()
+    print(data)

scripts/quest_iris_dual_arm.py

@@ -39,8 +39,8 @@
 
 INCLUDE_ROTATION = True
 ROBOT2IP = {
-    # "left": "192.168.102.1",
-    "right": "192.168.102.1",
+    "right": "192.168.101.1",
+    "left": "192.168.102.1",
 }
 
 
@@ -85,7 +85,7 @@ def __init__(self, env: RelativeActionSpace):
         self._env_lock = threading.Lock()
         self._env = env
 
-        self.controller_names = ["right"]  # , "right"]
+        self.controller_names = ["left", "right"]
         self._trg_btn = {"left": "index_trigger", "right": "index_trigger"}
         self._grp_btn = {"left": "hand_trigger", "right": "hand_trigger"}
         self._start_btn = "A"
@@ -297,24 +297,21 @@ def main():
             relative_to=RelativeTo.CONFIGURED_ORIGIN,
         )
         env_rel = StorageWrapper(env_rel, DATASET_PATH, INSTRUCTION, batch_size=32, max_rows_per_group=100, max_rows_per_file=1000)
-        MySimPublisher(MySimScene(), MQ3_ADDR)
-
-        # robot_cfg = default_sim_robot_cfg("fr3_empty_world")
-        # sim_cfg = SimConfig()
-        # sim_cfg.async_control = True
-        # twin_env, sim = SimMultiEnvCreator()(
-        #     name2id=ROBOT2IP,
-        #     robot_cfg=robot_cfg,
-        #     control_mode=ControlMode.CARTESIAN_TQuat,
-        #     gripper_cfg=default_sim_gripper_cfg(),
-        #     # cameras=default_mujoco_cameraset_cfg(),
-        #     max_relative_movement=0.5,
-        #     relative_to=RelativeTo.CONFIGURED_ORIGIN,
-        #     sim_cfg=sim_cfg,
-        # )
-        # sim.open_gui()
-        # MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
-        # env_rel = DigitalTwin(env_rel, twin_env)
+        # MySimPublisher(MySimScene(), MQ3_ADDR)
+
+        robot_cfg = default_sim_robot_cfg("fr3_empty_world")
+        sim_cfg = SimConfig()
+        sim_cfg.async_control = True
+        twin_env, sim = SimMultiEnvCreator()(
+            name2id=ROBOT2IP,
+            robot_cfg=robot_cfg,
+            control_mode=ControlMode.JOINTS,
+            gripper_cfg=default_sim_gripper_cfg(),
+            sim_cfg=sim_cfg,
+        )
+        sim.open_gui()
+        MujocoPublisher(sim.model, sim.data, MQ3_ADDR, visible_geoms_groups=list(range(1, 3)))
+        env_rel = DigitalTwin(env_rel, twin_env)
 
 
     else: