existing_files.patch

--- a/README.md
+++ b/README.md
@@ -1,5 +1,129 @@
-# MuJoCo Sim for Unitree G1
+---
+tags:
+  - lerobot
+  - mujoco
+  - unitree-g1
+---
 
-Standalone MuJoCo physics simulator for the Unitree G1 robot, adapted from gr00t_wbc. Currently supports G1_29dof.
+# Unitree G1 MuJoCo model for LeRobot
 
-set use joystick to 1 to control the robot +The default model has two **Dex1-1 parallel grippers** and three published cameras.
+The original articulated hands, URDF, MJCF and meshes remain included.
+This is the model repository loaded dynamically by LeRobot through `env.py`.
+
+The existing `make_env()` entry point, 29 body motor commands, body state DDS
+topics, simulation stepping and ZMQ image message format are preserved.
+MuJoCo loads `assets/scene_33dof.xml`, which includes the gripper MJCF derived
+from the supplied URDF. The portable URDF is also included at
+`assets/g1_29dof_with_dex1_1.urdf`; runtime cameras and actuators are defined in MJCF.
+
+## Choose the end effectors
+
+`config.yaml` defaults to `END_EFFECTOR: grippers`. Set it to `hands` to use the
+original seven-joint hands. Scene, finger counts and effort limits follow that
+selection automatically. Both modes have all three camera streams.
+
+| Selection | Runtime scene | Actuated joints |
+| --- | --- | --- |
+| `grippers` (default) | `assets/scene_33dof.xml` | 29 body + 2 fingers per gripper |
+| `hands` | `assets/scene_hands_cameras.xml` | 29 body + 7 joints per hand |
+
+Direct users of this repository can also call:
+
+```python
+from env import make_env
+
+if __name__ == "__main__":
+    env = make_env(end_effector="hands")  # Omit the argument for grippers.
+    try:
+        env.reset()
+        while True:
+            env.step()  # Body commands continue to arrive over DDS.
+    finally:
+        env.close()
+```
+
+The original `assets/scene_43dof.xml`, `assets/g1_29dof_with_hand.xml`,
+`assets/g1_body29_hand14.urdf` and no-hand model are retained unchanged.
+
+## Cameras
+
+All three streams are enabled by default on **`tcp://127.0.0.1:5555`**, with
+640 × 480 images and the existing approximately 30 Hz publishing setting.
+
+| Stream name | Mount |
+| --- | --- |
+| `head_camera` | Existing head camera |
+| `left_wrist_cam` | Left gripper base / wrist |
+| `right_wrist_cam` | Right gripper base / wrist |
+
+Each stream is advertised through the same top-level JPEG key and nested
+`images` / `timestamps` entries as `head_camera`. The existing
+`view_cameras_live.py` discovers the names from those messages. They are also
+listed in `env.camera_configs`, `env.camera_names` and `env.metadata["cameras"]`.
+The wrist cameras move with their respective wrists, with 95° vertical field of
+view and the approximate extrinsics from the HIW-500 model builder.
+
+LeRobot's ZMQ cameras require explicit client configuration, just as the head
+camera does. Pass this dictionary as `UnitreeG1Config(..., cameras=cameras)`:
+
+```python
+from lerobot.cameras.zmq.configuration_zmq import ZMQCameraConfig
+
+cameras = {
+    name: ZMQCameraConfig(
+        server_address="127.0.0.1", port=5555, camera_name=name,
+        width=640, height=480, fps=30,
+    )
+    for name in ("head_camera", "left_wrist_cam", "right_wrist_cam")
+}
+```
+
+An equivalent camera configuration is in `lerobot_cameras.json`.
+`make_env(cameras=["head_camera"])` selects a subset; `publish_images=False`
+disables publishing. `onscreen=False` disables the viewer for headless use.
+
+## Gripper control
+
+Dex1 fingers use force actuators, driven by the existing bridge's external PD
+controller. They start open at 0.0245 m. To preserve the simulator's hand
+transport, the first two motor entries on `rt/dex3/left/cmd` and
+`rt/dex3/right/cmd` command fingers 1 and 2, with matching state topics.
+Gripper `q` is in metres and `tau` is in newtons; each finger is limited to 20 N.
+Command both fingers to the same position for symmetric opening or closing.
+Original hand mode retains all seven rotational command entries per side.
+
+The simulation lower limit is -0.023 m, following HIW-500's mesh closure trim.
+The source and portable URDF retain the official -0.020 m lower limit.
+Run `python build_gripper_model.py --official-limits` to use that limit in MJCF
+too; it leaves approximately 5.88 mm between the supplied finger pads.
+This model update does not add finger actions to LeRobot's 29-body-motor
+`UnitreeG1` action schema.
+
+## Development and checks
+
+Install the existing Unitree SDK2 / CycloneDDS prerequisites, then
+`python -m pip install -r requirements.txt`. Regenerate the variants with
+`python build_gripper_model.py`.
+
+```bash
+MUJOCO_GL=egl python -m unittest discover -s tests -v
+MUJOCO_GL=egl python tests/smoke_live.py
+MUJOCO_GL=egl python tests/smoke_live.py --end-effector hands
+```
+
+The regression suite uses real MuJoCo for both variants, motor/observation
+mapping, force-controlled closure, wrist camera motion, rendering and camera
+publisher shared-memory buffers. It isolates DDS with a test double.
+`smoke_live.py` additionally requires the real Gymnasium, Unitree SDK2 and
+ZMQ/OpenCV dependencies and checks the live environment and transports.
+See `VALIDATION.md` for what was executed for this update.
+
+## Sources
+
+- Base model/runtime: [lerobot/unitree-g1-mujoco](https://huggingface.co/lerobot/unitree-g1-mujoco/tree/a38dc8617f0fca51b38e9354dc58ee35ad850fb5).
+- Dex1-1 URDF, meshes and wrist-camera geometry: [Hxxxz0/HIW-500-controoler](https://github.com/Hxxxz0/HIW-500-controoler/tree/c69d89d88bb51774fe9a3684b90d9ff1abf801da).
+- LeRobot loading and camera configuration checked against [main at b6ec006](https://github.com/huggingface/lerobot/tree/b6ec0060779550c0a157ae34feb89e0cf86012a8).
+
+The original Dex1 source URDF, Apache 2.0 license and attribution notice are in
+`reference/hiw500/`. Unitree mesh assets retain their upstream terms.
--- a/config.yaml
+++ b/config.yaml
@@ -1,6 +1,7 @@
 # Robot Configuration
 ROBOT_TYPE: 'g1_29dof'
-ROBOT_SCENE: "assets/scene_43dof.xml"
+END_EFFECTOR: "grippers"  # "grippers" (Dex1-1) or "hands" (original Dex3)
+ROBOT_SCENE: "assets/scene_33dof.xml"  # Selected automatically from END_EFFECTOR
 
 # DDS Communication
 DOMAIN_ID: 0
@@ -25,6 +26,7 @@
 ENABLE_ONSCREEN: true
 ENABLE_OFFSCREEN: false
 MP_START_METHOD: "spawn"
+CAMERAS: ["head_camera", "left_wrist_cam", "right_wrist_cam"]
 
 # Sensors
 USE_SENSOR: False
@@ -33,16 +35,17 @@
 # Robot Dimensions
 NUM_MOTORS: 29
 NUM_JOINTS: 29
-NUM_HAND_MOTORS: 7
-NUM_HAND_JOINTS: 7
+NUM_HAND_MOTORS: 2
+NUM_HAND_JOINTS: 2
 
-# Torque Limits (Nm) - 29 body + 14 hand = 43 total
+# Selected automatically per mode: 29 body + 4 gripper = 33 total.
+# Revolute joint torque limits in Nm; prismatic finger force limits in N.
 motor_effort_limit_list: [
     88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # left leg
     88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # right leg
     88.0, 50.0, 50.0,                       # waist
     25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # left arm
-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7,    # left hand
+    20.0, 20.0,                            # left gripper
     25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # right arm
-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7     # right hand
+    20.0, 20.0                             # right gripper
 ]
--- a/env.py
+++ b/env.py
@@ -5,12 +5,18 @@
 import numpy as np
 from huggingface_hub import snapshot_download
 import yaml
-snapshot_download("lerobot/unitree-g1-mujoco")
+# LeRobot initially fetches only env.py. Hydrate its matching Hub snapshot;
+# a complete local checkout can also be used without another network request.
+_repo_dir = Path(__file__).parent
+if not (_repo_dir / "assets/scene_33dof.xml").is_file():
+    _revision = _repo_dir.name if _repo_dir.parent.name == "snapshots" else None
+    snapshot_download("lerobot/unitree-g1-mujoco", revision=_revision)
 
 # Ensure sim module is importable
 sys.path.insert(0, str(Path(__file__).parent))
 
 from sim.simulator_factory import SimulatorFactory, init_channel
+from sim.model_config import select_end_effector
 
 
 def make_env(n_envs=1, use_async_envs=False, **kwargs):
@@ -23,6 +29,8 @@
             - publish_images: bool, whether to publish camera images via ZMQ
             - camera_port: int, ZMQ port for camera images
             - cameras: list of camera names
+            - end_effector: "grippers" (default) or "hands"
+            - onscreen: bool, override the interactive viewer setting
     """
     repo_dir = Path(__file__).parent
     
@@ -30,11 +38,14 @@
     config_path = repo_dir / "config.yaml"
     with open(config_path) as f:
         config = yaml.safe_load(f)
+    config = select_end_effector(config, kwargs.get("end_effector"))
     
     # Configure cameras if requested
     publish_images = kwargs.get("publish_images", True)
     camera_port = kwargs.get("camera_port", 5555)
-    cameras = kwargs.get("cameras", ["head_camera"])
+    cameras = kwargs.get("cameras")
+    if cameras is None:
+        cameras = config["CAMERAS"]
     
     enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)
     camera_configs = {}
@@ -49,7 +60,7 @@
     simulator = SimulatorFactory.create_simulator(
         config=config,
         env_name="default",
-        onscreen=config.get("ENABLE_ONSCREEN", True),
+        onscreen=kwargs.get("onscreen", config.get("ENABLE_ONSCREEN", True)),
         offscreen=enable_offscreen,
         camera_configs=camera_configs,
     )
@@ -65,6 +76,10 @@
             self.sim_env = sim.sim_env
             self.step_count = 0
             self.camera_configs = cam_configs
+            self.camera_names = tuple(cam_configs)
+            self.camera_port = cam_port
+            self.end_effector = config["END_EFFECTOR"]
+            self.metadata = {"render_modes": ["human"], "cameras": self.camera_names}
             
             # Get timing from config
             self.sim_dt = config["SIMULATE_DT"]
@@ -77,7 +92,7 @@
                     start_method=config.get("MP_START_METHOD", "spawn"),
                     camera_port=cam_port
                 )
-                print(f"Camera images publishing on tcp://localhost:{cam_port}")
+                print(f"Camera images publishing on tcp://localhost:{cam_port}: {', '.join(self.camera_names)}")
             
             # Define spaces
             num_joints = config.get("NUM_MOTORS", 29)
@@ -123,7 +138,7 @@
                 obs_dict.get("body_tau_est", np.zeros(29)),
                 obs_dict.get("floating_base_pose", np.zeros(7))[:4],
                 obs_dict.get("floating_base_vel", np.zeros(6))[:3],
-                obs_dict.get("floating_base_acc", np.zeros(3)),
+                obs_dict.get("floating_base_acc", np.zeros(3))[:3],
             ]).astype(np.float32)
             return obs
 
--- a/requirements.txt
+++ b/requirements.txt
@@ -1,8 +1,13 @@
-mujoco>=3.0.0
+mujoco>=3.3.0
 numpy>=1.24.0
 pyyaml>=6.0
 unitree-sdk2py>=1.0.0
 loguru>=0.7.0
+gymnasium>=1.0.0
+huggingface_hub>=0.25.0
+pygame>=2.5.0
+termcolor>=2.0.0
+scipy>=1.10.0
 
 # Camera publishing dependencies
 opencv-python>=4.8.0
@@ -10,4 +15,3 @@
 msgpack>=1.0.0
 msgpack-numpy>=0.4.8
 matplotlib>=3.5.0  # For live camera viewer
-
--- a/run_sim.py
+++ b/run_sim.py
@@ -7,6 +7,7 @@
 
 import yaml
 from sim.simulator_factory import SimulatorFactory, init_channel
+from sim.model_config import select_end_effector
 
 def main(n_envs=1, use_async_envs: bool = False, 
              publish_images=True, camera_port=5555, cameras=None, **kwargs):
@@ -15,6 +16,7 @@
     config_path = Path(__file__).parent / "config.yaml"
     with open(config_path) as f:
         config = yaml.safe_load(f)
+    config = select_end_effector(config, kwargs.get("end_effector"))
     
     # Override config with default values
     enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)
@@ -22,7 +24,7 @@
     # Configure cameras if requested
     camera_configs = {}
     if enable_offscreen:
-        camera_list = cameras or ["head_camera"]
+        camera_list = config["CAMERAS"] if cameras is None else cameras
         for cam_name in camera_list:
             camera_configs[cam_name] = {"height": 480, "width": 640}
         print(f"📷 Cameras: {', '.join(camera_list)} → ZMQ port {camera_port}")
@@ -36,7 +38,7 @@
     sim = SimulatorFactory.create_simulator(
         config=config,
         env_name="default",
-        onscreen=config.get("ENABLE_ONSCREEN", True),
+        onscreen=kwargs.get("onscreen", config.get("ENABLE_ONSCREEN", True)),
         offscreen=enable_offscreen,
         camera_configs=camera_configs,
     )
@@ -63,4 +65,3 @@
 
 if __name__ == "__main__":
     main()
-
--- a/sim/base_sim.py
+++ b/sim/base_sim.py
@@ -13,7 +13,7 @@
     HAS_RCLPY = True
 except ImportError:
     HAS_RCLPY = False
-    print("Warning: rclpy not found. Camera image publishing will be disabled.")
+    print("ROS 2 integration unavailable; camera images use the ZMQ publisher.")
 from unitree_sdk2py.core.channel import ChannelFactoryInitialize
 import yaml
 import os
@@ -21,6 +21,7 @@
 from .metric_utils import check_contact
 from .sim_utils import get_subtree_body_names
 from .unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge
+from .model_config import select_end_effector
 
 GR00T_WBC_ROOT = Path(__file__).resolve().parent.parent  # Points to mujoco_sim_g1/
 
@@ -52,7 +53,6 @@
         self.num_hand_dof = self.config["NUM_HAND_JOINTS"]
         self.sim_dt = self.config["SIMULATE_DT"]
         self.obs = None
-        self.torques = np.zeros(self.num_body_dof + self.num_hand_dof * 2)
         self.torque_limit = np.array(self.config["motor_effort_limit_list"])
         self.camera_configs = camera_configs
         
@@ -131,7 +131,8 @@
             self.viewer.cam.distance = 2.0  # Distance from camera to target
             self.viewer.cam.lookat = np.array([0, 0, 0.5])  # Point the camera is looking at
 
-        # Note that the actuator order is the same as the joint order in the mujoco model.
+        # Body DDS indices exclude the end effectors. Map each scalar joint
+        # explicitly: actuator order need not match the model's joint order.
         self.body_joint_index = []
         self.left_hand_index = []
         self.right_hand_index = []
@@ -144,23 +145,41 @@
                 ]
             ):
                 self.body_joint_index.append(i)
-            elif "left_hand" in name:
+            elif "left_hand" in name or name.startswith("left_dex1_finger_joint_"):
                 self.left_hand_index.append(i)
-            elif "right_hand" in name:
+            elif "right_hand" in name or name.startswith("right_dex1_finger_joint_"):
                 self.right_hand_index.append(i)
 
         assert len(self.body_joint_index) == self.config["NUM_JOINTS"], \
             f"Expected {self.config['NUM_JOINTS']} body joints, got {len(self.body_joint_index)}"
-        # Hand joints are optional (some models don't have hands)
-        if self.config.get("NUM_HAND_JOINTS", 0) > 0:
-            expected_hands = self.config["NUM_HAND_JOINTS"]
-            if len(self.left_hand_index) != expected_hands or len(self.right_hand_index) != expected_hands:
-                print(f"Warning: Expected {expected_hands} hand joints, got left={len(self.left_hand_index)}, right={len(self.right_hand_index)}")
-                print("Continuing without hands...")
-
-        self.body_joint_index = np.array(self.body_joint_index)
-        self.left_hand_index = np.array(self.left_hand_index)
-        self.right_hand_index = np.array(self.right_hand_index)
+        expected_hands = self.config.get("NUM_HAND_JOINTS", 0)
+        if len(self.left_hand_index) != expected_hands or len(self.right_hand_index) != expected_hands:
+            raise ValueError(f"Expected {expected_hands} joints per end effector, got left={len(self.left_hand_index)}, right={len(self.right_hand_index)}")
+
+        for prefix, attribute in (("body", "body_joint_index"), ("left_hand", "left_hand_index"), ("right_hand", "right_hand_index")):
+            joint_ids = np.asarray(getattr(self, attribute), dtype=int)
+            setattr(self, attribute, joint_ids)
+            setattr(self, prefix + "_qpos_index", self.mj_model.jnt_qposadr[joint_ids])
+            setattr(self, prefix + "_dof_index", self.mj_model.jnt_dofadr[joint_ids])
+            actuator_ids = []
+            for joint_id in joint_ids:
+                matches = np.flatnonzero(
+                    (self.mj_model.actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT)
+                    & (self.mj_model.actuator_trnid[:, 0] == joint_id)
+                )
+                if len(matches) != 1:
+                    raise ValueError(f"Expected one actuator for {self.mj_model.joint(joint_id).name}, got {len(matches)}")
+                actuator_ids.append(matches[0])
+            setattr(self, prefix + "_actuator_index", np.asarray(actuator_ids, dtype=int))
+        self.torques = np.zeros(self.mj_model.nu)
+        if self.config.get("FREE_BASE", False):
+            self.torque_limit = np.concatenate((np.zeros(6), self.torque_limit))
+        if self.torque_limit.shape != self.torques.shape:
+            raise ValueError("motor_effort_limit_list must match the scene's actuator count")
+        for name in self.camera_configs:
+            if mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_CAMERA, name) < 0:
+                raise ValueError(f"Camera {name!r} does not exist in {self.config['ROBOT_SCENE']}")
+        self.reset()
 
     def init_renderers(self):
         # Initialize camera renderers
@@ -212,12 +231,12 @@
                         + self.unitree_bridge.low_cmd.motor_cmd[i].kp
                         * (
                             self.unitree_bridge.low_cmd.motor_cmd[i].q
-                            - self.mj_data.qpos[self.body_joint_index[i] + 7 - 1]
+                            - self.mj_data.qpos[self.body_qpos_index[i]]
                         )
                         + self.unitree_bridge.low_cmd.motor_cmd[i].kd
                         * (
                             self.unitree_bridge.low_cmd.motor_cmd[i].dq
-                            - self.mj_data.qvel[self.body_joint_index[i] + 6 - 1]
+                            - self.mj_data.qvel[self.body_dof_index[i]]
                         )
                     )
         return body_torques
@@ -233,12 +252,12 @@
                     + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kp
                     * (
                         self.unitree_bridge.left_hand_cmd.motor_cmd[i].q
-                        - self.mj_data.qpos[self.left_hand_index[i] + 7 - 1]
+                        - self.mj_data.qpos[self.left_hand_qpos_index[i]]
                     )
                     + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kd
                     * (
                         self.unitree_bridge.left_hand_cmd.motor_cmd[i].dq
-                        - self.mj_data.qvel[self.left_hand_index[i] + 6 - 1]
+                        - self.mj_data.qvel[self.left_hand_dof_index[i]]
                     )
                 )
                 right_hand_torques[i] = (
@@ -246,12 +265,12 @@
                     + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kp
                     * (
                         self.unitree_bridge.right_hand_cmd.motor_cmd[i].q
-                        - self.mj_data.qpos[self.right_hand_index[i] + 7 - 1]
+                        - self.mj_data.qpos[self.right_hand_qpos_index[i]]
                     )
                     + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kd
                     * (
                         self.unitree_bridge.right_hand_cmd.motor_cmd[i].dq
-                        - self.mj_data.qvel[self.right_hand_index[i] + 6 - 1]
+                        - self.mj_data.qvel[self.right_hand_dof_index[i]]
                     )
                 )
         return np.concatenate((left_hand_torques, right_hand_torques))
@@ -281,19 +300,19 @@
         obs["floating_base_acc"] = self.mj_data.qacc[:6]
         obs["secondary_imu_quat"] = self.mj_data.xquat[self.torso_index]
         obs["secondary_imu_vel"] = self.mj_data.cvel[self.torso_index]
-        obs["body_q"] = self.mj_data.qpos[self.body_joint_index + 7 - 1]
-        obs["body_dq"] = self.mj_data.qvel[self.body_joint_index + 6 - 1]
-        obs["body_ddq"] = self.mj_data.qacc[self.body_joint_index + 6 - 1]
-        obs["body_tau_est"] = self.mj_data.actuator_force[self.body_joint_index - 1]
+        obs["body_q"] = self.mj_data.qpos[self.body_qpos_index]
+        obs["body_dq"] = self.mj_data.qvel[self.body_dof_index]
+        obs["body_ddq"] = self.mj_data.qacc[self.body_dof_index]
+        obs["body_tau_est"] = self.mj_data.actuator_force[self.body_actuator_index]
         if self.num_hand_dof > 0:
-            obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_index + 7 - 1]
-            obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_index + 6 - 1]
-            obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_index + 6 - 1]
-            obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_index - 1]
-            obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_index + 7 - 1]
-            obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_index + 6 - 1]
-            obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_index + 6 - 1]
-            obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_index - 1]
+            obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_qpos_index]
+            obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_dof_index]
+            obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_dof_index]
+            obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_actuator_index]
+            obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_qpos_index]
+            obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_dof_index]
+            obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_dof_index]
+            obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_actuator_index]
         obs["time"] = self.mj_data.time
         return obs
 
@@ -333,17 +352,14 @@
                 self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)
         body_torques = self.compute_body_torques()
         hand_torques = self.compute_hand_torques()
-        self.torques[self.body_joint_index - 1] = body_torques
+        self.torques[self.body_actuator_index] = body_torques
         if self.num_hand_dof > 0:
-            self.torques[self.left_hand_index - 1] = hand_torques[: self.num_hand_dof]
-            self.torques[self.right_hand_index - 1] = hand_torques[self.num_hand_dof :]
+            self.torques[self.left_hand_actuator_index] = hand_torques[: self.num_hand_dof]
+            self.torques[self.right_hand_actuator_index] = hand_torques[self.num_hand_dof :]
 
         self.torques = np.clip(self.torques, -self.torque_limit, self.torque_limit)
 
-        if self.config["FREE_BASE"]:
-            self.mj_data.ctrl = np.concatenate((np.zeros(6), self.torques))
-        else:
-            self.mj_data.ctrl = self.torques
+        self.mj_data.ctrl[:] = self.torques
         mujoco.mj_step(self.mj_model, self.mj_data)
         # self.check_self_collision()
 
@@ -391,9 +407,9 @@
         body_qpos = self.compute_body_qpos()  # (num_body_dof,)
         hand_qpos = self.compute_hand_qpos()  # (num_hand_dof * 2,)
 
-        self.mj_data.qpos[self.body_joint_index + 7 - 1] = body_qpos
-        self.mj_data.qpos[self.left_hand_index + 7 - 1] = hand_qpos[: self.num_hand_dof]
-        self.mj_data.qpos[self.right_hand_index + 7 - 1] = hand_qpos[self.num_hand_dof :]
+        self.mj_data.qpos[self.body_qpos_index] = body_qpos
+        self.mj_data.qpos[self.left_hand_qpos_index] = hand_qpos[: self.num_hand_dof]
+        self.mj_data.qpos[self.right_hand_qpos_index] = hand_qpos[self.num_hand_dof :]
 
         mujoco.mj_kinematics(self.mj_model, self.mj_data)
         mujoco.mj_comPos(self.mj_model, self.mj_data)
@@ -507,6 +523,11 @@
         # Set valid floating base quaternion (identity: w=1, x=y=z=0)
         # mj_resetData sets qpos to zeros, which gives invalid [0,0,0,0] quaternion
         self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]
+        if self.config.get("END_EFFECTOR") == "grippers":
+            for side in ("left", "right"):
+                joints = getattr(self, side + "_hand_index")
+                addresses = getattr(self, side + "_hand_qpos_index")
+                self.mj_data.qpos[addresses] = self.mj_model.jnt_range[joints, 1]
         # Propagate qpos to derived quantities (xquat, xpos, etc.)
         mujoco.mj_forward(self.mj_model, self.mj_data)
 
@@ -515,6 +536,7 @@
     """Base simulator class that handles initialization and running of simulations"""
 
     def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs):
+        config = select_end_effector(config)
         self.config = config
         self.env_name = env_name
 
@@ -636,6 +658,7 @@
 
     def reset(self):
         """Reset the simulation. Can be overridden by subclasses."""
+        self.unitree_bridge.reset()
         self.sim_env.reset()
 
     def close(self):
@@ -650,6 +673,11 @@
             if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None:
                 self.sim_env.viewer.close()
 
+            for renderer in self.sim_env.renderers.values():
+                renderer.close()
+            self.sim_env.renderers.clear()
+            self.sim_env._renderers_initialized = False
+
             # Shutdown ROS (if available)
             if HAS_RCLPY and rclpy.ok():
                 rclpy.shutdown()
--- a/sim/unitree_sdk2py_bridge.py
+++ b/sim/unitree_sdk2py_bridge.py
@@ -127,9 +127,22 @@
         with self.left_hand_cmd_lock:
             self.left_hand_cmd_received = False
             self.new_left_hand_cmd = False
+            if self.config.get("END_EFFECTOR") == "grippers":
+                self._open_gripper(self.left_hand_cmd)
         with self.right_hand_cmd_lock:
             self.right_hand_cmd_received = False
             self.new_right_hand_cmd = False
+            if self.config.get("END_EFFECTOR") == "grippers":
+                self._open_gripper(self.right_hand_cmd)
+
+    def _open_gripper(self, command):
+        """Hold Dex1 fingers open until a hand command arrives (positions in m)."""
+        for motor in command.motor_cmd[:self.num_hand_motor]:
+            motor.q = 0.0245
+            motor.dq = 0.0
+            motor.kp = 1000.0
+            motor.kd = 25.0
+            motor.tau = 0.0
 
     def LowCmdHandler(self, msg):
         with self.low_cmd_lock: