asimov-mujoco / sim /base_sim.py
nepyope's picture
Locate repository files without resolving Hub cache symlinks
9c1f418 verified
Raw History Blame Contribute Delete
20.9 kB
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()