--- 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: