lerobot/unitree-g1-mujoco

1--- a/README.md2+++ b/README.md3@@ -1,5 +1,129 @@4-# MuJoCo Sim for Unitree G15+---6+tags:7+  - lerobot8+  - mujoco9+  - unitree-g110+---11 12-Standalone MuJoCo physics simulator for the Unitree G1 robot, adapted from gr00t_wbc. Currently supports G1_29dof.13+# Unitree G1 MuJoCo model for LeRobot14 15-set use joystick to 1 to control the robot +The default model has two **Dex1-1 parallel grippers** and three published cameras.16+The original articulated hands, URDF, MJCF and meshes remain included.17+This is the model repository loaded dynamically by LeRobot through `env.py`.18+19+The existing `make_env()` entry point, 29 body motor commands, body state DDS20+topics, simulation stepping and ZMQ image message format are preserved.21+MuJoCo loads `assets/scene_33dof.xml`, which includes the gripper MJCF derived22+from the supplied URDF. The portable URDF is also included at23+`assets/g1_29dof_with_dex1_1.urdf`; runtime cameras and actuators are defined in MJCF.24+25+## Choose the end effectors26+27+`config.yaml` defaults to `END_EFFECTOR: grippers`. Set it to `hands` to use the28+original seven-joint hands. Scene, finger counts and effort limits follow that29+selection automatically. Both modes have all three camera streams.30+31+| Selection | Runtime scene | Actuated joints |32+| --- | --- | --- |33+| `grippers` (default) | `assets/scene_33dof.xml` | 29 body + 2 fingers per gripper |34+| `hands` | `assets/scene_hands_cameras.xml` | 29 body + 7 joints per hand |35+36+Direct users of this repository can also call:37+38+```python39+from env import make_env40+41+if __name__ == "__main__":42+    env = make_env(end_effector="hands")  # Omit the argument for grippers.43+    try:44+        env.reset()45+        while True:46+            env.step()  # Body commands continue to arrive over DDS.47+    finally:48+        env.close()49+```50+51+The original `assets/scene_43dof.xml`, `assets/g1_29dof_with_hand.xml`,52+`assets/g1_body29_hand14.urdf` and no-hand model are retained unchanged.53+54+## Cameras55+56+All three streams are enabled by default on **`tcp://127.0.0.1:5555`**, with57+640 × 480 images and the existing approximately 30 Hz publishing setting.58+59+| Stream name | Mount |60+| --- | --- |61+| `head_camera` | Existing head camera |62+| `left_wrist_cam` | Left gripper base / wrist |63+| `right_wrist_cam` | Right gripper base / wrist |64+65+Each stream is advertised through the same top-level JPEG key and nested66+`images` / `timestamps` entries as `head_camera`. The existing67+`view_cameras_live.py` discovers the names from those messages. They are also68+listed in `env.camera_configs`, `env.camera_names` and `env.metadata["cameras"]`.69+The wrist cameras move with their respective wrists, with 95° vertical field of70+view and the approximate extrinsics from the HIW-500 model builder.71+72+LeRobot's ZMQ cameras require explicit client configuration, just as the head73+camera does. Pass this dictionary as `UnitreeG1Config(..., cameras=cameras)`:74+75+```python76+from lerobot.cameras.zmq.configuration_zmq import ZMQCameraConfig77+78+cameras = {79+    name: ZMQCameraConfig(80+        server_address="127.0.0.1", port=5555, camera_name=name,81+        width=640, height=480, fps=30,82+    )83+    for name in ("head_camera", "left_wrist_cam", "right_wrist_cam")84+}85+```86+87+An equivalent camera configuration is in `lerobot_cameras.json`.88+`make_env(cameras=["head_camera"])` selects a subset; `publish_images=False`89+disables publishing. `onscreen=False` disables the viewer for headless use.90+91+## Gripper control92+93+Dex1 fingers use force actuators, driven by the existing bridge's external PD94+controller. They start open at 0.0245 m. To preserve the simulator's hand95+transport, the first two motor entries on `rt/dex3/left/cmd` and96+`rt/dex3/right/cmd` command fingers 1 and 2, with matching state topics.97+Gripper `q` is in metres and `tau` is in newtons; each finger is limited to 20 N.98+Command both fingers to the same position for symmetric opening or closing.99+Original hand mode retains all seven rotational command entries per side.100+101+The simulation lower limit is -0.023 m, following HIW-500's mesh closure trim.102+The source and portable URDF retain the official -0.020 m lower limit.103+Run `python build_gripper_model.py --official-limits` to use that limit in MJCF104+too; it leaves approximately 5.88 mm between the supplied finger pads.105+This model update does not add finger actions to LeRobot's 29-body-motor106+`UnitreeG1` action schema.107+108+## Development and checks109+110+Install the existing Unitree SDK2 / CycloneDDS prerequisites, then111+`python -m pip install -r requirements.txt`. Regenerate the variants with112+`python build_gripper_model.py`.113+114+```bash115+MUJOCO_GL=egl python -m unittest discover -s tests -v116+MUJOCO_GL=egl python tests/smoke_live.py117+MUJOCO_GL=egl python tests/smoke_live.py --end-effector hands118+```119+120+The regression suite uses real MuJoCo for both variants, motor/observation121+mapping, force-controlled closure, wrist camera motion, rendering and camera122+publisher shared-memory buffers. It isolates DDS with a test double.123+`smoke_live.py` additionally requires the real Gymnasium, Unitree SDK2 and124+ZMQ/OpenCV dependencies and checks the live environment and transports.125+See `VALIDATION.md` for what was executed for this update.126+127+## Sources128+129+- Base model/runtime: [lerobot/unitree-g1-mujoco](https://huggingface.co/lerobot/unitree-g1-mujoco/tree/a38dc8617f0fca51b38e9354dc58ee35ad850fb5).130+- Dex1-1 URDF, meshes and wrist-camera geometry: [Hxxxz0/HIW-500-controoler](https://github.com/Hxxxz0/HIW-500-controoler/tree/c69d89d88bb51774fe9a3684b90d9ff1abf801da).131+- LeRobot loading and camera configuration checked against [main at b6ec006](https://github.com/huggingface/lerobot/tree/b6ec0060779550c0a157ae34feb89e0cf86012a8).132+133+The original Dex1 source URDF, Apache 2.0 license and attribution notice are in134+`reference/hiw500/`. Unitree mesh assets retain their upstream terms.135--- a/config.yaml136+++ b/config.yaml137@@ -1,6 +1,7 @@138 # Robot Configuration139 ROBOT_TYPE: 'g1_29dof'140-ROBOT_SCENE: "assets/scene_43dof.xml"141+END_EFFECTOR: "grippers"  # "grippers" (Dex1-1) or "hands" (original Dex3)142+ROBOT_SCENE: "assets/scene_33dof.xml"  # Selected automatically from END_EFFECTOR143 144 # DDS Communication145 DOMAIN_ID: 0146@@ -25,6 +26,7 @@147 ENABLE_ONSCREEN: true148 ENABLE_OFFSCREEN: false149 MP_START_METHOD: "spawn"150+CAMERAS: ["head_camera", "left_wrist_cam", "right_wrist_cam"]151 152 # Sensors153 USE_SENSOR: False154@@ -33,16 +35,17 @@155 # Robot Dimensions156 NUM_MOTORS: 29157 NUM_JOINTS: 29158-NUM_HAND_MOTORS: 7159-NUM_HAND_JOINTS: 7160+NUM_HAND_MOTORS: 2161+NUM_HAND_JOINTS: 2162 163-# Torque Limits (Nm) - 29 body + 14 hand = 43 total164+# Selected automatically per mode: 29 body + 4 gripper = 33 total.165+# Revolute joint torque limits in Nm; prismatic finger force limits in N.166 motor_effort_limit_list: [167     88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # left leg168     88.0, 88.0, 88.0, 139.0, 50.0, 50.0,   # right leg169     88.0, 50.0, 50.0,                       # waist170     25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # left arm171-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7,    # left hand172+    20.0, 20.0,                            # left gripper173     25.0, 25.0, 25.0, 25.0, 25.0, 5.0, 5.0, # right arm174-    2.45, 0.7, 0.7, 0.7, 0.7, 0.7, 0.7     # right hand175+    20.0, 20.0                             # right gripper176 ]177--- a/env.py178+++ b/env.py179@@ -5,12 +5,18 @@180 import numpy as np181 from huggingface_hub import snapshot_download182 import yaml183-snapshot_download("lerobot/unitree-g1-mujoco")184+# LeRobot initially fetches only env.py. Hydrate its matching Hub snapshot;185+# a complete local checkout can also be used without another network request.186+_repo_dir = Path(__file__).parent187+if not (_repo_dir / "assets/scene_33dof.xml").is_file():188+    _revision = _repo_dir.name if _repo_dir.parent.name == "snapshots" else None189+    snapshot_download("lerobot/unitree-g1-mujoco", revision=_revision)190 191 # Ensure sim module is importable192 sys.path.insert(0, str(Path(__file__).parent))193 194 from sim.simulator_factory import SimulatorFactory, init_channel195+from sim.model_config import select_end_effector196 197 198 def make_env(n_envs=1, use_async_envs=False, **kwargs):199@@ -23,6 +29,8 @@200             - publish_images: bool, whether to publish camera images via ZMQ201             - camera_port: int, ZMQ port for camera images202             - cameras: list of camera names203+            - end_effector: "grippers" (default) or "hands"204+            - onscreen: bool, override the interactive viewer setting205     """206     repo_dir = Path(__file__).parent207     208@@ -30,11 +38,14 @@209     config_path = repo_dir / "config.yaml"210     with open(config_path) as f:211         config = yaml.safe_load(f)212+    config = select_end_effector(config, kwargs.get("end_effector"))213     214     # Configure cameras if requested215     publish_images = kwargs.get("publish_images", True)216     camera_port = kwargs.get("camera_port", 5555)217-    cameras = kwargs.get("cameras", ["head_camera"])218+    cameras = kwargs.get("cameras")219+    if cameras is None:220+        cameras = config["CAMERAS"]221     222     enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)223     camera_configs = {}224@@ -49,7 +60,7 @@225     simulator = SimulatorFactory.create_simulator(226         config=config,227         env_name="default",228-        onscreen=config.get("ENABLE_ONSCREEN", True),229+        onscreen=kwargs.get("onscreen", config.get("ENABLE_ONSCREEN", True)),230         offscreen=enable_offscreen,231         camera_configs=camera_configs,232     )233@@ -65,6 +76,10 @@234             self.sim_env = sim.sim_env235             self.step_count = 0236             self.camera_configs = cam_configs237+            self.camera_names = tuple(cam_configs)238+            self.camera_port = cam_port239+            self.end_effector = config["END_EFFECTOR"]240+            self.metadata = {"render_modes": ["human"], "cameras": self.camera_names}241             242             # Get timing from config243             self.sim_dt = config["SIMULATE_DT"]244@@ -77,7 +92,7 @@245                     start_method=config.get("MP_START_METHOD", "spawn"),246                     camera_port=cam_port247                 )248-                print(f"Camera images publishing on tcp://localhost:{cam_port}")249+                print(f"Camera images publishing on tcp://localhost:{cam_port}: {', '.join(self.camera_names)}")250             251             # Define spaces252             num_joints = config.get("NUM_MOTORS", 29)253@@ -123,7 +138,7 @@254                 obs_dict.get("body_tau_est", np.zeros(29)),255                 obs_dict.get("floating_base_pose", np.zeros(7))[:4],256                 obs_dict.get("floating_base_vel", np.zeros(6))[:3],257-                obs_dict.get("floating_base_acc", np.zeros(3)),258+                obs_dict.get("floating_base_acc", np.zeros(3))[:3],259             ]).astype(np.float32)260             return obs261 262--- a/requirements.txt263+++ b/requirements.txt264@@ -1,8 +1,13 @@265-mujoco>=3.0.0266+mujoco>=3.3.0267 numpy>=1.24.0268 pyyaml>=6.0269 unitree-sdk2py>=1.0.0270 loguru>=0.7.0271+gymnasium>=1.0.0272+huggingface_hub>=0.25.0273+pygame>=2.5.0274+termcolor>=2.0.0275+scipy>=1.10.0276 277 # Camera publishing dependencies278 opencv-python>=4.8.0279@@ -10,4 +15,3 @@280 msgpack>=1.0.0281 msgpack-numpy>=0.4.8282 matplotlib>=3.5.0  # For live camera viewer283-284--- a/run_sim.py285+++ b/run_sim.py286@@ -7,6 +7,7 @@287 288 import yaml289 from sim.simulator_factory import SimulatorFactory, init_channel290+from sim.model_config import select_end_effector291 292 def main(n_envs=1, use_async_envs: bool = False, 293              publish_images=True, camera_port=5555, cameras=None, **kwargs):294@@ -15,6 +16,7 @@295     config_path = Path(__file__).parent / "config.yaml"296     with open(config_path) as f:297         config = yaml.safe_load(f)298+    config = select_end_effector(config, kwargs.get("end_effector"))299     300     # Override config with default values301     enable_offscreen = publish_images or config.get("ENABLE_OFFSCREEN", False)302@@ -22,7 +24,7 @@303     # Configure cameras if requested304     camera_configs = {}305     if enable_offscreen:306-        camera_list = cameras or ["head_camera"]307+        camera_list = config["CAMERAS"] if cameras is None else cameras308         for cam_name in camera_list:309             camera_configs[cam_name] = {"height": 480, "width": 640}310         print(f"📷 Cameras: {', '.join(camera_list)} → ZMQ port {camera_port}")311@@ -36,7 +38,7 @@312     sim = SimulatorFactory.create_simulator(313         config=config,314         env_name="default",315-        onscreen=config.get("ENABLE_ONSCREEN", True),316+        onscreen=kwargs.get("onscreen", config.get("ENABLE_ONSCREEN", True)),317         offscreen=enable_offscreen,318         camera_configs=camera_configs,319     )320@@ -63,4 +65,3 @@321 322 if __name__ == "__main__":323     main()324-325--- a/sim/base_sim.py326+++ b/sim/base_sim.py327@@ -13,7 +13,7 @@328     HAS_RCLPY = True329 except ImportError:330     HAS_RCLPY = False331-    print("Warning: rclpy not found. Camera image publishing will be disabled.")332+    print("ROS 2 integration unavailable; camera images use the ZMQ publisher.")333 from unitree_sdk2py.core.channel import ChannelFactoryInitialize334 import yaml335 import os336@@ -21,6 +21,7 @@337 from .metric_utils import check_contact338 from .sim_utils import get_subtree_body_names339 from .unitree_sdk2py_bridge import ElasticBand, UnitreeSdk2Bridge340+from .model_config import select_end_effector341 342 GR00T_WBC_ROOT = Path(__file__).resolve().parent.parent  # Points to mujoco_sim_g1/343 344@@ -52,7 +53,6 @@345         self.num_hand_dof = self.config["NUM_HAND_JOINTS"]346         self.sim_dt = self.config["SIMULATE_DT"]347         self.obs = None348-        self.torques = np.zeros(self.num_body_dof + self.num_hand_dof * 2)349         self.torque_limit = np.array(self.config["motor_effort_limit_list"])350         self.camera_configs = camera_configs351         352@@ -131,7 +131,8 @@353             self.viewer.cam.distance = 2.0  # Distance from camera to target354             self.viewer.cam.lookat = np.array([0, 0, 0.5])  # Point the camera is looking at355 356-        # Note that the actuator order is the same as the joint order in the mujoco model.357+        # Body DDS indices exclude the end effectors. Map each scalar joint358+        # explicitly: actuator order need not match the model's joint order.359         self.body_joint_index = []360         self.left_hand_index = []361         self.right_hand_index = []362@@ -144,23 +145,41 @@363                 ]364             ):365                 self.body_joint_index.append(i)366-            elif "left_hand" in name:367+            elif "left_hand" in name or name.startswith("left_dex1_finger_joint_"):368                 self.left_hand_index.append(i)369-            elif "right_hand" in name:370+            elif "right_hand" in name or name.startswith("right_dex1_finger_joint_"):371                 self.right_hand_index.append(i)372 373         assert len(self.body_joint_index) == self.config["NUM_JOINTS"], \374             f"Expected {self.config['NUM_JOINTS']} body joints, got {len(self.body_joint_index)}"375-        # Hand joints are optional (some models don't have hands)376-        if self.config.get("NUM_HAND_JOINTS", 0) > 0:377-            expected_hands = self.config["NUM_HAND_JOINTS"]378-            if len(self.left_hand_index) != expected_hands or len(self.right_hand_index) != expected_hands:379-                print(f"Warning: Expected {expected_hands} hand joints, got left={len(self.left_hand_index)}, right={len(self.right_hand_index)}")380-                print("Continuing without hands...")381-382-        self.body_joint_index = np.array(self.body_joint_index)383-        self.left_hand_index = np.array(self.left_hand_index)384-        self.right_hand_index = np.array(self.right_hand_index)385+        expected_hands = self.config.get("NUM_HAND_JOINTS", 0)386+        if len(self.left_hand_index) != expected_hands or len(self.right_hand_index) != expected_hands:387+            raise ValueError(f"Expected {expected_hands} joints per end effector, got left={len(self.left_hand_index)}, right={len(self.right_hand_index)}")388+389+        for prefix, attribute in (("body", "body_joint_index"), ("left_hand", "left_hand_index"), ("right_hand", "right_hand_index")):390+            joint_ids = np.asarray(getattr(self, attribute), dtype=int)391+            setattr(self, attribute, joint_ids)392+            setattr(self, prefix + "_qpos_index", self.mj_model.jnt_qposadr[joint_ids])393+            setattr(self, prefix + "_dof_index", self.mj_model.jnt_dofadr[joint_ids])394+            actuator_ids = []395+            for joint_id in joint_ids:396+                matches = np.flatnonzero(397+                    (self.mj_model.actuator_trntype == mujoco.mjtTrn.mjTRN_JOINT)398+                    & (self.mj_model.actuator_trnid[:, 0] == joint_id)399+                )400+                if len(matches) != 1:401+                    raise ValueError(f"Expected one actuator for {self.mj_model.joint(joint_id).name}, got {len(matches)}")402+                actuator_ids.append(matches[0])403+            setattr(self, prefix + "_actuator_index", np.asarray(actuator_ids, dtype=int))404+        self.torques = np.zeros(self.mj_model.nu)405+        if self.config.get("FREE_BASE", False):406+            self.torque_limit = np.concatenate((np.zeros(6), self.torque_limit))407+        if self.torque_limit.shape != self.torques.shape:408+            raise ValueError("motor_effort_limit_list must match the scene's actuator count")409+        for name in self.camera_configs:410+            if mujoco.mj_name2id(self.mj_model, mujoco.mjtObj.mjOBJ_CAMERA, name) < 0:411+                raise ValueError(f"Camera {name!r} does not exist in {self.config['ROBOT_SCENE']}")412+        self.reset()413 414     def init_renderers(self):415         # Initialize camera renderers416@@ -212,12 +231,12 @@417                         + self.unitree_bridge.low_cmd.motor_cmd[i].kp418                         * (419                             self.unitree_bridge.low_cmd.motor_cmd[i].q420-                            - self.mj_data.qpos[self.body_joint_index[i] + 7 - 1]421+                            - self.mj_data.qpos[self.body_qpos_index[i]]422                         )423                         + self.unitree_bridge.low_cmd.motor_cmd[i].kd424                         * (425                             self.unitree_bridge.low_cmd.motor_cmd[i].dq426-                            - self.mj_data.qvel[self.body_joint_index[i] + 6 - 1]427+                            - self.mj_data.qvel[self.body_dof_index[i]]428                         )429                     )430         return body_torques431@@ -233,12 +252,12 @@432                     + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kp433                     * (434                         self.unitree_bridge.left_hand_cmd.motor_cmd[i].q435-                        - self.mj_data.qpos[self.left_hand_index[i] + 7 - 1]436+                        - self.mj_data.qpos[self.left_hand_qpos_index[i]]437                     )438                     + self.unitree_bridge.left_hand_cmd.motor_cmd[i].kd439                     * (440                         self.unitree_bridge.left_hand_cmd.motor_cmd[i].dq441-                        - self.mj_data.qvel[self.left_hand_index[i] + 6 - 1]442+                        - self.mj_data.qvel[self.left_hand_dof_index[i]]443                     )444                 )445                 right_hand_torques[i] = (446@@ -246,12 +265,12 @@447                     + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kp448                     * (449                         self.unitree_bridge.right_hand_cmd.motor_cmd[i].q450-                        - self.mj_data.qpos[self.right_hand_index[i] + 7 - 1]451+                        - self.mj_data.qpos[self.right_hand_qpos_index[i]]452                     )453                     + self.unitree_bridge.right_hand_cmd.motor_cmd[i].kd454                     * (455                         self.unitree_bridge.right_hand_cmd.motor_cmd[i].dq456-                        - self.mj_data.qvel[self.right_hand_index[i] + 6 - 1]457+                        - self.mj_data.qvel[self.right_hand_dof_index[i]]458                     )459                 )460         return np.concatenate((left_hand_torques, right_hand_torques))461@@ -281,19 +300,19 @@462         obs["floating_base_acc"] = self.mj_data.qacc[:6]463         obs["secondary_imu_quat"] = self.mj_data.xquat[self.torso_index]464         obs["secondary_imu_vel"] = self.mj_data.cvel[self.torso_index]465-        obs["body_q"] = self.mj_data.qpos[self.body_joint_index + 7 - 1]466-        obs["body_dq"] = self.mj_data.qvel[self.body_joint_index + 6 - 1]467-        obs["body_ddq"] = self.mj_data.qacc[self.body_joint_index + 6 - 1]468-        obs["body_tau_est"] = self.mj_data.actuator_force[self.body_joint_index - 1]469+        obs["body_q"] = self.mj_data.qpos[self.body_qpos_index]470+        obs["body_dq"] = self.mj_data.qvel[self.body_dof_index]471+        obs["body_ddq"] = self.mj_data.qacc[self.body_dof_index]472+        obs["body_tau_est"] = self.mj_data.actuator_force[self.body_actuator_index]473         if self.num_hand_dof > 0:474-            obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_index + 7 - 1]475-            obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_index + 6 - 1]476-            obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_index + 6 - 1]477-            obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_index - 1]478-            obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_index + 7 - 1]479-            obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_index + 6 - 1]480-            obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_index + 6 - 1]481-            obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_index - 1]482+            obs["left_hand_q"] = self.mj_data.qpos[self.left_hand_qpos_index]483+            obs["left_hand_dq"] = self.mj_data.qvel[self.left_hand_dof_index]484+            obs["left_hand_ddq"] = self.mj_data.qacc[self.left_hand_dof_index]485+            obs["left_hand_tau_est"] = self.mj_data.actuator_force[self.left_hand_actuator_index]486+            obs["right_hand_q"] = self.mj_data.qpos[self.right_hand_qpos_index]487+            obs["right_hand_dq"] = self.mj_data.qvel[self.right_hand_dof_index]488+            obs["right_hand_ddq"] = self.mj_data.qacc[self.right_hand_dof_index]489+            obs["right_hand_tau_est"] = self.mj_data.actuator_force[self.right_hand_actuator_index]490         obs["time"] = self.mj_data.time491         return obs492 493@@ -333,17 +352,14 @@494                 self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6)495         body_torques = self.compute_body_torques()496         hand_torques = self.compute_hand_torques()497-        self.torques[self.body_joint_index - 1] = body_torques498+        self.torques[self.body_actuator_index] = body_torques499         if self.num_hand_dof > 0:500-            self.torques[self.left_hand_index - 1] = hand_torques[: self.num_hand_dof]501-            self.torques[self.right_hand_index - 1] = hand_torques[self.num_hand_dof :]502+            self.torques[self.left_hand_actuator_index] = hand_torques[: self.num_hand_dof]503+            self.torques[self.right_hand_actuator_index] = hand_torques[self.num_hand_dof :]504 505         self.torques = np.clip(self.torques, -self.torque_limit, self.torque_limit)506 507-        if self.config["FREE_BASE"]:508-            self.mj_data.ctrl = np.concatenate((np.zeros(6), self.torques))509-        else:510-            self.mj_data.ctrl = self.torques511+        self.mj_data.ctrl[:] = self.torques512         mujoco.mj_step(self.mj_model, self.mj_data)513         # self.check_self_collision()514 515@@ -391,9 +407,9 @@516         body_qpos = self.compute_body_qpos()  # (num_body_dof,)517         hand_qpos = self.compute_hand_qpos()  # (num_hand_dof * 2,)518 519-        self.mj_data.qpos[self.body_joint_index + 7 - 1] = body_qpos520-        self.mj_data.qpos[self.left_hand_index + 7 - 1] = hand_qpos[: self.num_hand_dof]521-        self.mj_data.qpos[self.right_hand_index + 7 - 1] = hand_qpos[self.num_hand_dof :]522+        self.mj_data.qpos[self.body_qpos_index] = body_qpos523+        self.mj_data.qpos[self.left_hand_qpos_index] = hand_qpos[: self.num_hand_dof]524+        self.mj_data.qpos[self.right_hand_qpos_index] = hand_qpos[self.num_hand_dof :]525 526         mujoco.mj_kinematics(self.mj_model, self.mj_data)527         mujoco.mj_comPos(self.mj_model, self.mj_data)528@@ -507,6 +523,11 @@529         # Set valid floating base quaternion (identity: w=1, x=y=z=0)530         # mj_resetData sets qpos to zeros, which gives invalid [0,0,0,0] quaternion531         self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0]532+        if self.config.get("END_EFFECTOR") == "grippers":533+            for side in ("left", "right"):534+                joints = getattr(self, side + "_hand_index")535+                addresses = getattr(self, side + "_hand_qpos_index")536+                self.mj_data.qpos[addresses] = self.mj_model.jnt_range[joints, 1]537         # Propagate qpos to derived quantities (xquat, xpos, etc.)538         mujoco.mj_forward(self.mj_model, self.mj_data)539 540@@ -515,6 +536,7 @@541     """Base simulator class that handles initialization and running of simulations"""542 543     def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs):544+        config = select_end_effector(config)545         self.config = config546         self.env_name = env_name547 548@@ -636,6 +658,7 @@549 550     def reset(self):551         """Reset the simulation. Can be overridden by subclasses."""552+        self.unitree_bridge.reset()553         self.sim_env.reset()554 555     def close(self):556@@ -650,6 +673,11 @@557             if hasattr(self.sim_env, "viewer") and self.sim_env.viewer is not None:558                 self.sim_env.viewer.close()559 560+            for renderer in self.sim_env.renderers.values():561+                renderer.close()562+            self.sim_env.renderers.clear()563+            self.sim_env._renderers_initialized = False564+565             # Shutdown ROS (if available)566             if HAS_RCLPY and rclpy.ok():567                 rclpy.shutdown()568--- a/sim/unitree_sdk2py_bridge.py569+++ b/sim/unitree_sdk2py_bridge.py570@@ -127,9 +127,22 @@571         with self.left_hand_cmd_lock:572             self.left_hand_cmd_received = False573             self.new_left_hand_cmd = False574+            if self.config.get("END_EFFECTOR") == "grippers":575+                self._open_gripper(self.left_hand_cmd)576         with self.right_hand_cmd_lock:577             self.right_hand_cmd_received = False578             self.new_right_hand_cmd = False579+            if self.config.get("END_EFFECTOR") == "grippers":580+                self._open_gripper(self.right_hand_cmd)581+582+    def _open_gripper(self, command):583+        """Hold Dex1 fingers open until a hand command arrives (positions in m)."""584+        for motor in command.motor_cmd[:self.num_hand_motor]:585+            motor.q = 0.0245586+            motor.dq = 0.0587+            motor.kp = 1000.0588+            motor.kd = 25.0589+            motor.tau = 0.0590 591     def LowCmdHandler(self, msg):592         with self.low_cmd_lock:593