import argparse from pathlib import Path import shutil import threading from threading import Thread from typing import Dict import mujoco import mujoco.viewer import numpy as np import yaml from .asimov_edge_bridge import AsimovEdgeBridge, ElasticBand from .image_publish_utils import ImagePublishProcess from .metric_utils import check_contact from .robot_model import scene_path from .sim_utils import get_subtree_body_names ASIMOV_ROOT = Path(__file__).absolute().parent.parent # Points to asimov-mujoco/ class DefaultEnv: """Base environment class that handles simulation environment setup and step""" def __init__( self, config: Dict[str, any], env_name: str = "default", camera_configs: Dict[str, any] = None, onscreen: bool = False, offscreen: bool = False, ): # Avoid mutable default argument gotcha if camera_configs is None: camera_configs = {} self.config = config self.env_name = env_name self.num_body_dof = self.config["NUM_JOINTS"] self.sim_dt = self.config["SIMULATE_DT"] self.obs = None self.camera_configs = camera_configs self.damp_kd = float(self.config.get("DAMP_KD", 2.0)) # Debug: print camera config if len(camera_configs) > 0: print(f"✓ DefaultEnv initialized with {len(camera_configs)} camera(s): {list(camera_configs.keys())}") # Asimov Edge bridge will be initialized by the simulator self.edge_bridge = None self.elastic_band = None # Store display mode self.onscreen = onscreen # Initialize scene (defined in subclasses) self.init_scene() # Setup offscreen rendering if needed (lazy init - renderers created on first use) self.offscreen = offscreen self.renderers = {} # Will be lazily initialized self._renderers_initialized = False self.image_dt = self.config.get("IMAGE_DT", 0.033333) # Image publishing subprocess (initialized separately) self.image_publish_process = None def init_scene(self): """Initialize the default robot scene""" scene = scene_path(self.config) try: self.mj_model = mujoco.MjModel.from_xml_path(str(scene)) finally: shutil.rmtree(scene.parent, ignore_errors=True) self.mj_data = mujoco.MjData(self.mj_model) self.mj_model.opt.timestep = self.sim_dt self.torso_index = self.mj_model.body("waist_yaw_link").id self.root_body = "pelvis_link" self.imu_site = self.mj_model.site("imu_in_pelvis").id # Enable the elastic band if self.config["ENABLE_ELASTIC_BAND"]: self.elastic_band = ElasticBand(height=self.config.get("BAND_HEIGHT", 1.0)) self.band_attached_link = self.mj_model.body(self.config.get("BAND_LINK", "pelvis_link")).id if self.onscreen: self.viewer = mujoco.viewer.launch_passive( self.mj_model, self.mj_data, key_callback=self.elastic_band.MujuocoKeyCallback, show_left_ui=False, show_right_ui=False, ) else: mujoco.mj_forward(self.mj_model, self.mj_data) self.viewer = None else: if self.onscreen: self.viewer = mujoco.viewer.launch_passive( self.mj_model, self.mj_data, show_left_ui=False, show_right_ui=False ) else: mujoco.mj_forward(self.mj_model, self.mj_data) self.viewer = None if self.viewer: # viewer camera self.viewer.cam.azimuth = 120 # Horizontal rotation in degrees self.viewer.cam.elevation = -30 # Vertical tilt in degrees self.viewer.cam.distance = 2.0 # Distance from camera to target self.viewer.cam.lookat = np.array([0, 0, 0.4]) # Point the camera is looking at # The MJCF orders joints by kinematic tree, not as the firmware does, so map each # firmware slot to its joint by name. names = self.config["JOINT_NAMES"] if len(names) != self.config["NUM_JOINTS"]: raise ValueError(f"Expected {self.config['NUM_JOINTS']} JOINT_NAMES, got {len(names)}") self.body_joint_index = np.array([self.mj_model.joint(name).id for name in names], dtype=int) self.body_qpos_index = self.mj_model.jnt_qposadr[self.body_joint_index] self.body_dof_index = self.mj_model.jnt_dofadr[self.body_joint_index] actuator_ids = [] for joint_id in self.body_joint_index: 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]) self.body_actuator_index = np.asarray(actuator_ids, dtype=int) self.torques = np.zeros(self.mj_model.nu) self.torque_limit = self.mj_model.actuator_ctrlrange[self.body_actuator_index] self.stand_pose = np.array(self.config["STAND_POSE"], dtype=np.float64) 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 self.renderers = {} for camera_name, camera_config in self.camera_configs.items(): renderer = mujoco.Renderer( self.mj_model, height=camera_config["height"], width=camera_config["width"] ) self.renderers[camera_name] = renderer def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555): """Start image publishing subprocess using ZMQ""" # Use spawn method for better GIL isolation, or configured method if len(self.camera_configs) == 0: print( "Warning: No camera configs provided, image publishing subprocess will not be started" ) return start_method = self.config.get("MP_START_METHOD", "spawn") self.image_publish_process = ImagePublishProcess( camera_configs=self.camera_configs, image_dt=self.image_dt, zmq_port=camera_port, start_method=start_method, verbose=self.config.get("verbose", False), ) self.image_publish_process.start_process() print(f"✓ Started image publishing subprocess on ZMQ port {camera_port}") def compute_body_torques(self) -> np.ndarray: """Compute body torques from the latest Edge command and the current robot state""" q = self.mj_data.qpos[self.body_qpos_index] dq = self.mj_data.qvel[self.body_dof_index] if self.edge_bridge is None: return np.zeros(self.num_body_dof) targets, kp, kd, damped = self.edge_bridge.GetAction() if damped: return -self.damp_kd * dq return kp * (targets - q) - kd * dq def prepare_obs(self) -> Dict[str, any]: """Prepare observation dictionary from the current robot state""" obs = {} obs["floating_base_pose"] = self.mj_data.qpos[:7] obs["floating_base_vel"] = self.mj_data.qvel[:6] 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_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] # The firmware's IMU sits in the pelvis and reports in its own frame. rot = self.mj_data.site_xmat[self.imu_site].reshape(3, 3) vel = np.zeros(6) mujoco.mj_objectVelocity(self.mj_model, self.mj_data, mujoco.mjtObj.mjOBJ_SITE, self.imu_site, vel, 0) quat = np.empty(4) mujoco.mju_mat2Quat(quat, self.mj_data.site_xmat[self.imu_site]) obs["imu_gyro"] = rot.T @ vel[:3] obs["imu_gravity"] = rot.T @ np.array([0.0, 0.0, -1.0]) obs["imu_quat"] = quat obs["time"] = self.mj_data.time return obs def sim_step(self): self.obs = self.prepare_obs() self.edge_bridge.PublishLowState(self.obs) if self.config["ENABLE_ELASTIC_BAND"]: if self.elastic_band.enable: # Get Cartesian pose and velocity of the band_attached_link pose = np.concatenate( [ self.mj_data.xpos[self.band_attached_link], # link position in world self.mj_data.xquat[ self.band_attached_link ], # link quaternion in world [w,x,y,z] np.zeros(6), # placeholder for velocity ] ) # Get velocity in world frame mujoco.mj_objectVelocity( self.mj_model, self.mj_data, mujoco.mjtObj.mjOBJ_BODY, self.band_attached_link, pose[7:13], 0, # 0 for world frame ) # Reorder velocity from [ang, lin] to [lin, ang] pose[7:10], pose[10:13] = pose[10:13], pose[7:10].copy() self.mj_data.xfrc_applied[self.band_attached_link] = self.elastic_band.Advance(pose) else: # explicitly resetting the force when the band is not enabled self.mj_data.xfrc_applied[self.band_attached_link] = np.zeros(6) self.torques[self.body_actuator_index] = self.compute_body_torques() self.torques[self.body_actuator_index] = np.clip( self.torques[self.body_actuator_index], self.torque_limit[:, 0], self.torque_limit[:, 1] ) self.mj_data.ctrl[:] = self.torques mujoco.mj_step(self.mj_model, self.mj_data) # self.check_self_collision() def apply_perturbation(self, key): """Apply perturbation to the robot""" # Add velocity perturbations in body frame perturbation_x_body = 0.0 # forward/backward in body frame perturbation_y_body = 0.0 # left/right in body frame if key == "up": perturbation_x_body = 1.0 # forward elif key == "down": perturbation_x_body = -1.0 # backward elif key == "left": perturbation_y_body = 1.0 # left elif key == "right": perturbation_y_body = -1.0 # right # Transform body frame velocity to world frame using MuJoCo's rotation vel_body = np.array([perturbation_x_body, perturbation_y_body, 0.0]) vel_world = np.zeros(3) base_quat = self.mj_data.qpos[3:7] # [w, x, y, z] quaternion # Use MuJoCo's robust quaternion rotation (handles invalid quaternions automatically) mujoco.mju_rotVecQuat(vel_world, vel_body, base_quat) # Apply to base linear velocity in world frame self.mj_data.qvel[0] += vel_world[0] # world X velocity self.mj_data.qvel[1] += vel_world[1] # world Y velocity # Update dynamics after velocity change mujoco.mj_forward(self.mj_model, self.mj_data) def update_viewer(self): if self.viewer is not None: self.viewer.sync() def update_viewer_camera(self): if self.viewer is not None: if self.viewer.cam.type == mujoco.mjtCamera.mjCAMERA_TRACKING: self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_FREE else: self.viewer.cam.type = mujoco.mjtCamera.mjCAMERA_TRACKING def set_edge_bridge(self, edge_bridge): """Set the Asimov Edge bridge from the simulator""" self.edge_bridge = edge_bridge def get_privileged_obs(self): """Get privileged observation. Should be implemented by subclasses.""" return {} def update_render_caches(self): """Update render cache and shared memory for subprocess.""" # Lazy init renderers on first call (creates OpenGL context in calling thread) if not self._renderers_initialized and self.offscreen: self.init_renderers() self._renderers_initialized = True print(f"✓ Renderers initialized lazily in thread {threading.current_thread().name}") render_caches = {} for camera_name, camera_config in self.camera_configs.items(): renderer = self.renderers.get(camera_name) if renderer is None: continue if "params" in camera_config: renderer.update_scene(self.mj_data, camera=camera_config["params"]) else: renderer.update_scene(self.mj_data, camera=camera_name) render_caches[camera_name + "_image"] = renderer.render() # Update shared memory if image publishing process is available if self.image_publish_process is not None: self.image_publish_process.update_shared_memory(render_caches) return render_caches def handle_keyboard_button(self, key): if self.elastic_band is not None: self.elastic_band.handle_keyboard_button(key) if key == "backspace": self.reset() if key == "v": self.update_viewer_camera() if key in ["up", "down", "left", "right"]: self.apply_perturbation(key) def check_fall(self): """Check if the robot has fallen""" self.fall = False if self.mj_data.qpos[2] < 0.2: self.fall = True print(f"Warning: Robot has fallen, height: {self.mj_data.qpos[2]:.3f} m") if self.fall: self.reset() def check_self_collision(self): """Check for self-collision of the robot""" robot_bodies = get_subtree_body_names(self.mj_model, self.mj_model.body(self.root_body).id) self_collision, contact_bodies = check_contact( self.mj_model, self.mj_data, robot_bodies, robot_bodies, return_all_contact_bodies=True ) if self_collision: print(f"Warning: Self-collision detected: {contact_bodies}") return self_collision def reset(self): mujoco.mj_resetData(self.mj_model, self.mj_data) # Hang in the stance Edge holds in STAND, with the pelvis at the band's height. self.mj_data.qpos[3:7] = [1.0, 0.0, 0.0, 0.0] self.mj_data.qpos[2] = self.config.get("BAND_HEIGHT", self.mj_data.qpos[2]) self.mj_data.qpos[self.body_qpos_index] = self.stand_pose if self.elastic_band is not None: self.elastic_band.length = 0 # Propagate qpos to derived quantities (xquat, xpos, etc.) mujoco.mj_forward(self.mj_model, self.mj_data) class BaseSimulator: """Base simulator class that handles initialization and running of simulations""" def __init__(self, config: Dict[str, any], env_name: str = "default", **kwargs): self.config = config self.env_name = env_name self.node = None self.thread = None # Set update frequencies self.sim_dt = self.config["SIMULATE_DT"] self.image_dt = self.config.get("IMAGE_DT", 0.033333) self.viewer_dt = self.config.get("VIEWER_DT", 0.02) # Create the environment self.sim_env = DefaultEnv(config, env_name, **kwargs) # Initialize the Asimov Edge bridge and pass it to the environment self.init_edge_bridge() self.sim_env.set_edge_bridge(self.edge_bridge) if self.config.get("PRINT_SCENE_INFORMATION", False): self.edge_bridge.PrintSceneInformation(self.sim_env.mj_model) # Initialize additional components self.init_subscriber() self.init_publisher() self.sim_thread = None self._stop = threading.Event() def start_as_thread(self): # Create simulation thread self.sim_thread = Thread(target=self.start) self.sim_thread.start() def start_image_publish_subprocess(self, start_method: str = "spawn", camera_port: int = 5555): """Start the image publish subprocess""" self.sim_env.start_image_publish_subprocess(start_method, camera_port) def init_subscriber(self): """Initialize subscribers. Can be overridden by subclasses.""" pass def init_publisher(self): """Initialize publishers. Can be overridden by subclasses.""" pass def init_edge_bridge(self): """Initialize the Asimov Edge bridge.""" self.edge_bridge = AsimovEdgeBridge(self.config) def start(self): """Main simulation loop""" import time sim_cnt = 0 last_time = time.time() print(f"Starting simulation loop. Viewer: {self.sim_env.viewer is not None}") try: while not self._stop.is_set() and ( (self.sim_env.viewer and self.sim_env.viewer.is_running()) or self.sim_env.viewer is None ): # Run simulation step self.sim_env.sim_step() # Update viewer at viewer rate if sim_cnt % int(self.viewer_dt / self.sim_dt) == 0: self.sim_env.update_viewer() # Update render caches at image rate if sim_cnt % int(self.image_dt / self.sim_dt) == 0: self.sim_env.update_render_caches() # Sleep to maintain correct rate (simple timing without ROS) elapsed = time.time() - last_time sleep_time = max(0, self.sim_dt - elapsed) if sleep_time > 0: time.sleep(sleep_time) last_time = time.time() sim_cnt += 1 print(f"Loop exited. Viewer running: {self.sim_env.viewer.is_running() if self.sim_env.viewer else 'No viewer'}") except KeyboardInterrupt: # User pressed Ctrl+C - exit cleanly print("Keyboard interrupt received") pass except Exception as e: print(f"Exception in simulation loop: {e}") import traceback traceback.print_exc() self.close() def __del__(self): """Clean up resources when simulator is deleted""" self.close() def reset(self): """Reset the simulation. Can be overridden by subclasses.""" self.edge_bridge.reset() self.sim_env.reset() def close(self): """Close the simulation. Can be overridden by subclasses.""" try: self._stop.set() if self.sim_thread is not None and self.sim_thread is not threading.current_thread(): self.sim_thread.join(timeout=2.0) # Stop image publishing subprocess if hasattr(self.sim_env, "image_publish_process") and self.sim_env.image_publish_process is not None: self.sim_env.image_publish_process.stop() self.sim_env.image_publish_process = None # Close viewer 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 if getattr(self, "edge_bridge", None) is not None: self.edge_bridge.close() self.edge_bridge = None except Exception as e: print(f"Warning during close: {e}") def get_privileged_obs(self): obs = self.sim_env.get_privileged_obs() return obs def handle_keyboard_button(self, key): # Only handles keyboard buttons for default env. if self.env_name == "default": self.sim_env.handle_keyboard_button(key) if __name__ == "__main__": parser = argparse.ArgumentParser(description="Robot") parser.add_argument("--config", type=str, default=str(ASIMOV_ROOT / "config.yaml"), help="config file") args = parser.parse_args() with open(args.config, "r") as file: config = yaml.load(file, Loader=yaml.FullLoader) simulation = BaseSimulator(config) simulation.start_as_thread()