Quest2 il teleop

autonomy/simulation/Humanoid_Wato/wato_bimanual_arm/urdf/armDouble.SLDASM/armDouble.SLDASM_limits.usda

@@ -0,0 +1,109 @@
+#usda 1.0
+(
+    defaultPrim = "armDouble_SLDASM"
+    upAxis = "Z"
+)
+
+over "armDouble_SLDASM" (
+    prepend references = @./armDouble.SLDASM.usd@
+)
+{
+    over "joints"
+    {
+        over "joint1"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint2"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint3"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint4"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint5"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint6"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint1L"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint2l"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint3l"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint4l"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint5l"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint6l"
+        {
+            float physics:lowerLimit = -180
+            float physics:upperLimit = 180
+        }
+
+        over "joint7"
+        {
+            float physics:lowerLimit = -0.05
+            float physics:upperLimit = 0
+        }
+
+        over "joint8"
+        {
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 0.05
+        }
+
+        over "joint7l"
+        {
+            float physics:lowerLimit = -0.05
+            float physics:upperLimit = 0
+        }
+
+        over "joint8l"
+        {
+            float physics:lowerLimit = 0
+            float physics:upperLimit = 0.05
+        }
+    }
+}

autonomy/simulation/quest_isaac_teleop/README.md

@@ -0,0 +1,41 @@
+# Quest Bimanual Teleop (CloudXR + Pink IK)
+
+Hand-tracking teleop of the bimanual arm on a Quest, streamed via CloudXR.
+
+## 1. Start the USB tunnel (gnirehtet)
+
+The Quest reaches the PC through a USB reverse-tether. Plug the Quest in over USB,
+then:
+
+```bash
+cd ~/gnirehtet/gnirehtet-rust-linux64 && ./gnirehtet run
+```
+
+Approve the **VPN prompt on the headset** (a key icon appears). Leave this
+running.
+
+> If you restart `adb` or replug the Quest, the reverse mapping gnirehtet needs
+> gets wiped. Re-add it (then relaunch the client / re-run `./gnirehtet run`):
+>
+> ```bash
+> adb reverse tcp:31416 tcp:31416
+> ```
+
+## 2. Run the teleop
+
+```bash
+cd ~/Wato/humanoid/autonomy/simulation/quest_isaac_teleop && ./run_bimanual_teleop.sh
+```
+
+Wait for `IsaacTeleop teleoperation started`. It runs headless; the view only
+appears in the headset.
+
+Add `--gui` for a local desktop window (no headset/CloudXR).
+
+## 3. Connect the headset
+
+In the Quest browser open `https://<PC_IP>:48322/`, accept the self-signed cert,
+then in the CloudXR client set Server IP `<PC_IP>`, Port `48322`, **VR Immersive**,
+and **Connect**.
+
+Move your hands to drive the arms; pinch thumb + index to close each gripper.

autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/__init__.py

@@ -0,0 +1,13 @@
+def register_bimanual_tasks() -> list:
+    """Register the bimanual teleop task after Isaac Sim starts."""
+    import gymnasium as gym
+
+    gym.register(
+        id="Isaac-WatoBimanualTeleop-v0",
+        entry_point="isaaclab.envs:ManagerBasedRLEnv",
+        disable_env_checker=True,
+        kwargs={
+            "env_cfg_entry_point": "quest_isaac_teleop.bimanual_teleop_env_cfg:WatoBimanualTeleopEnvCfg",
+        },
+    )
+    return []

autonomy/simulation/quest_isaac_teleop/quest_isaac_teleop/bimanual_pink_controller_cfg.py

@@ -0,0 +1,68 @@
+"""Pink IK action config for the bimanual arm."""
+
+from isaaclab.controllers.pink_ik import (
+    DampingTaskCfg,
+    LocalFrameTaskCfg,
+    NullSpacePostureTaskCfg,
+    PinkIKControllerCfg,
+)
+from isaaclab.envs.mdp.actions.pink_actions_cfg import PinkInverseKinematicsActionCfg
+
+RIGHT_EE_LINK = "link6"
+LEFT_EE_LINK = "link6l"
+BASE_LINK = "base_link"
+
+RIGHT_ARM_JOINTS = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"]
+LEFT_ARM_JOINTS = ["joint1L", "joint2l", "joint3l", "joint4l", "joint5l", "joint6l"]
+ARM_JOINTS = RIGHT_ARM_JOINTS + LEFT_ARM_JOINTS
+
+# Order must match the teleop pipeline output.
+GRIPPER_JOINTS = ["joint7", "joint8", "joint7l", "joint8l"]
+
+GRIPPER_OPEN = {"joint7": 0.0, "joint8": 0.0, "joint7l": 0.0, "joint8l": 0.0}
+GRIPPER_CLOSED = {"joint7": -0.05, "joint8": 0.05, "joint7l": -0.05, "joint8l": 0.05}
+
+
+WATO_BIMANUAL_IK_CONTROLLER_CFG = PinkIKControllerCfg(
+    articulation_name="robot",
+    base_link_name=BASE_LINK,
+    num_hand_joints=len(GRIPPER_JOINTS),
+    show_ik_warnings=True,
+    fail_on_joint_limit_violation=False,
+    variable_input_tasks=[
+        LocalFrameTaskCfg(
+            frame=RIGHT_EE_LINK,
+            base_link_frame_name=BASE_LINK,
+            position_cost=8.0,
+            orientation_cost=1.0,
+            lm_damping=12,
+            gain=0.5,
+        ),
+        LocalFrameTaskCfg(
+            frame=LEFT_EE_LINK,
+            base_link_frame_name=BASE_LINK,
+            position_cost=8.0,
+            orientation_cost=1.0,
+            lm_damping=12,
+            gain=0.5,
+        ),
+        NullSpacePostureTaskCfg(
+            cost=0.05,
+            lm_damping=1,
+            controlled_frames=[RIGHT_EE_LINK, LEFT_EE_LINK],
+            controlled_joints=ARM_JOINTS,
+            gain=0.075,
+        ),
+        DampingTaskCfg(cost=0.5),
+    ],
+    fixed_input_tasks=[],
+)
+
+WATO_BIMANUAL_IK_ACTION_CFG = PinkInverseKinematicsActionCfg(
+    pink_controlled_joint_names=ARM_JOINTS,
+    hand_joint_names=GRIPPER_JOINTS,
+    target_eef_link_names={"right_ee": RIGHT_EE_LINK, "left_ee": LEFT_EE_LINK},
+    asset_name="robot",
+    enable_gravity_compensation=True,
+    controller=WATO_BIMANUAL_IK_CONTROLLER_CFG,
+)