From 9b4806aeaab153ceefd46bb209b77f4df664a316 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Fri, 22 May 2026 23:39:46 +0800 Subject: [PATCH 01/39] fix: correct car model orientation, wheel grounding, and actuator control - Swap car body XY dims so car faces +X (longer) instead of +Y (wider) - Lower car body (z=0.22) and wheels (rel z=-0.1) so wheels touch ground - Replace axisangle body rotation with fromto on cylinder geoms for correct wheel orientation - Change actuator_force to ctrl so motor control signals aren't overwritten by mj_step - Add camera marker spheres (topdown red, firstperson green) - Adjust firstperson camera pos/fovy for forward-facing view --- backend/services/mujoco_renderer/renderer.py | 4 +-- backend/services/mujoco_renderer/service.py | 2 +- mujoco/car_arm.xml | 28 +++++++++++--------- 3 files changed, 18 insertions(+), 16 deletions(-) diff --git a/backend/services/mujoco_renderer/renderer.py b/backend/services/mujoco_renderer/renderer.py index 3922d5c..2cd21b4 100644 --- a/backend/services/mujoco_renderer/renderer.py +++ b/backend/services/mujoco_renderer/renderer.py @@ -86,7 +86,7 @@ def step(self, arm_action: dict | None = None) -> None: try: actuator_id = self._actuator_name_to_id(actuator_name) if actuator_id >= 0: - self._data.actuator_force[actuator_id] = torque + self._data.ctrl[actuator_id] = torque except Exception: pass @@ -109,7 +109,7 @@ def set_wheel_torques(self, left_torque: float, right_torque: float) -> None: try: actuator_id = self._actuator_name_to_id(actuator_name) if actuator_id >= 0: - self._data.actuator_force[actuator_id] = torque + self._data.ctrl[actuator_id] = torque except Exception: pass diff --git a/backend/services/mujoco_renderer/service.py b/backend/services/mujoco_renderer/service.py index 25d5bd7..ad4ddd5 100644 --- a/backend/services/mujoco_renderer/service.py +++ b/backend/services/mujoco_renderer/service.py @@ -67,7 +67,7 @@ def set_arm_action(self, yaw: float, pitch: float, roll: float) -> None: self._renderer._model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name ) if actuator_id >= 0: - self._renderer._data.actuator_force[actuator_id] = torque + self._renderer._data.ctrl[actuator_id] = torque except Exception: pass diff --git a/mujoco/car_arm.xml b/mujoco/car_arm.xml index 735866e..d02c2c0 100644 --- a/mujoco/car_arm.xml +++ b/mujoco/car_arm.xml @@ -14,36 +14,38 @@ + - + - + + - - + + - + - + - + - + - + - + - + - + - + From 28c791061bed8450990624f7d10be33ecbf7aa7b Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 23 May 2026 23:01:08 +0800 Subject: [PATCH 02/39] feat: delete backend mujoco --- backend/main.py | 4 +- backend/requirements.txt | 2 - backend/services/mujoco_renderer/__init__.py | 4 - backend/services/mujoco_renderer/renderer.py | 131 ---------- backend/services/mujoco_renderer/service.py | 119 --------- backend/services/mujoco_renderer/state.py | 32 --- backend/sio_handlers/__init__.py | 68 ----- .../sio_handlers/domains/mujoco/__init__.py | 77 ------ ui/src/api/socket.ts | 34 --- ui/src/pages/MujocoPage/CameraViews.tsx | 239 ------------------ 10 files changed, 1 insertion(+), 709 deletions(-) delete mode 100644 backend/services/mujoco_renderer/__init__.py delete mode 100644 backend/services/mujoco_renderer/renderer.py delete mode 100644 backend/services/mujoco_renderer/service.py delete mode 100644 backend/services/mujoco_renderer/state.py delete mode 100644 backend/sio_handlers/domains/mujoco/__init__.py delete mode 100644 ui/src/pages/MujocoPage/CameraViews.tsx diff --git a/backend/main.py b/backend/main.py index 06e0d76..2af11d0 100644 --- a/backend/main.py +++ b/backend/main.py @@ -20,7 +20,7 @@ from backend import api from backend.api import router as api_router from backend.config import config -from backend.sio_handlers import SimNamespace, RealNamespace, MujocoNamespace, start_game_loop, set_act_runtime as set_sio_act_runtime, start_mujoco_game_loop +from backend.sio_handlers import SimNamespace, RealNamespace, start_game_loop, set_act_runtime as set_sio_act_runtime from backend.utils import set_broadcast_sio, setup_socket_logging # 配置日志 - 生产环境只记录关键事件 @@ -54,7 +54,6 @@ async def lifespan(app: FastAPI): # 启动两个命名空间的状态广播 start_game_loop(sio, namespace="/sim") start_game_loop(sio, namespace="/real") - start_mujoco_game_loop(sio, namespace="/mujoco") yield @@ -87,7 +86,6 @@ async def lifespan(app: FastAPI): # 注册三个独立的命名空间 sio.register_namespace(SimNamespace("/sim")) sio.register_namespace(RealNamespace("/real")) -sio.register_namespace(MujocoNamespace("/mujoco")) # 设置sio_server到api模块 api.set_sio_server(sio) diff --git a/backend/requirements.txt b/backend/requirements.txt index 59dfc43..e7b6214 100644 --- a/backend/requirements.txt +++ b/backend/requirements.txt @@ -23,5 +23,3 @@ pytest>=8.0.0 python-multipart einops redis>=5.0.0 - -mujoco diff --git a/backend/services/mujoco_renderer/__init__.py b/backend/services/mujoco_renderer/__init__.py deleted file mode 100644 index 2929fc5..0000000 --- a/backend/services/mujoco_renderer/__init__.py +++ /dev/null @@ -1,4 +0,0 @@ -from backend.services.mujoco_renderer.renderer import MujocoRenderer -from backend.services.mujoco_renderer.state import MujocoState - -__all__ = ["MujocoRenderer", "MujocoState"] \ No newline at end of file diff --git a/backend/services/mujoco_renderer/renderer.py b/backend/services/mujoco_renderer/renderer.py deleted file mode 100644 index 2cd21b4..0000000 --- a/backend/services/mujoco_renderer/renderer.py +++ /dev/null @@ -1,131 +0,0 @@ -from __future__ import annotations - -import logging -import os -from typing import Tuple - -import mujoco -import numpy as np - -logger = logging.getLogger(__name__) - - -class MujocoRenderer: - """Dual-view MuJoCo renderer for top-down and first-person views.""" - - def __init__(self, xml_path: str | None = None): - if xml_path is None: - xml_path = os.path.join( - os.path.dirname(__file__), "..", "..", "..", "mujoco", "car_arm.xml" - ) - self._xml_path = os.path.abspath(xml_path) - - self._model = mujoco.MjModel.from_xml_path(self._xml_path) - self._data = mujoco.MjData(self._model) - - self._renderer_topdown = mujoco.Renderer(self._model, width=640, height=480) - self._renderer_firstperson = mujoco.Renderer(self._model, width=640, height=480) - - self._cam_topdown = mujoco.MjvCamera() - self._cam_firstperson = mujoco.MjvCamera() - mujoco.mjv_defaultCamera(self._cam_topdown) - mujoco.mjv_defaultCamera(self._cam_firstperson) - - self._cam_state = {"azimuth": 0.0, "elevation": -70.0, "distance": 6.0} - self._setup_cameras() - logger.info(f"MujocoRenderer initialized with {self._xml_path}") - def _camera_name_to_id(self, name: str) -> int: - """Convert camera name to ID using mujoco.mj_name2id.""" - return mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_CAMERA, name) - - def _actuator_name_to_id(self, name: str) -> int: - """Convert actuator name to ID using mujoco.mj_name2id.""" - return mujoco.mj_name2id(self._model, mujoco.mjtObj.mjOBJ_ACTUATOR, name) - - def _setup_cameras(self) -> None: - """Configure top-down (free orbit) and first-person (fixed) cameras.""" - self._cam_topdown.type = mujoco.mjtCamera.mjCAMERA_FREE - self._cam_topdown.lookat = np.array([0, 0, 0.3]) - self._cam_topdown.distance = self._cam_state["distance"] - self._cam_topdown.elevation = self._cam_state["elevation"] - self._cam_topdown.azimuth = self._cam_state["azimuth"] - - self._cam_firstperson.type = mujoco.mjtCamera.mjCAMERA_FIXED - self._cam_firstperson.fixedcamid = self._camera_name_to_id("firstperson") - - def update_topdown_camera(self, delta_azimuth: float, delta_elevation: float) -> None: - """Rotate the top-down camera by mouse drag deltas.""" - sensitivity = 0.3 - self._cam_state["azimuth"] += delta_azimuth * sensitivity - self._cam_state["elevation"] += delta_elevation * sensitivity - self._cam_state["elevation"] = max(-89.0, min(89.0, self._cam_state["elevation"])) - - self._cam_topdown.azimuth = self._cam_state["azimuth"] - self._cam_topdown.elevation = self._cam_state["elevation"] - - def update_topdown_distance(self, delta: float) -> None: - """Zoom the top-down camera by scroll delta.""" - self._cam_state["distance"] += delta * 0.5 - self._cam_state["distance"] = max(0.5, min(50.0, self._cam_state["distance"])) - self._cam_topdown.distance = self._cam_state["distance"] - - def get_topdown_image(self) -> np.ndarray: - """Render top-down view.""" - self._renderer_topdown.update_scene(self._data, self._cam_topdown) - return self._renderer_topdown.render() - - def get_firstperson_image(self) -> np.ndarray: - """Render first-person view from camera on car.""" - self._renderer_firstperson.update_scene(self._data, self._cam_firstperson) - return self._renderer_firstperson.render() - - def step(self, arm_action: dict | None = None) -> None: - """Step the simulation.""" - if arm_action is not None: - for actuator_name, torque in arm_action.items(): - try: - actuator_id = self._actuator_name_to_id(actuator_name) - if actuator_id >= 0: - self._data.ctrl[actuator_id] = torque - except Exception: - pass - - mujoco.mj_step(self._model, self._data) - - def set_wheel_torques(self, left_torque: float, right_torque: float) -> None: - """Apply torques to wheels for differential drive. - - Args: - left_torque: torque for left wheels (fl, rl) - right_torque: torque for right wheels (fr, rr) - """ - wheel_torques = { - "motor_wheel_fl": left_torque, - "motor_wheel_rl": left_torque, - "motor_wheel_fr": right_torque, - "motor_wheel_rr": right_torque, - } - for actuator_name, torque in wheel_torques.items(): - try: - actuator_id = self._actuator_name_to_id(actuator_name) - if actuator_id >= 0: - self._data.ctrl[actuator_id] = torque - except Exception: - pass - - def get_state(self) -> dict: - """Get current state.""" - car_body_id = mujoco.mj_name2id( - self._model, mujoco.mjtObj.mjOBJ_BODY, "car" - ) - return { - "car_pos": self._data.xpos[car_body_id].copy(), - "car_quat": self._data.xquat[car_body_id].copy(), - "arm_qpos": self._data.qpos[11:].copy(), - "arm_qvel": self._data.qvel[10:].copy(), - } - - def close(self) -> None: - """Clean up renderers.""" - self._renderer_topdown.close() - self._renderer_firstperson.close() \ No newline at end of file diff --git a/backend/services/mujoco_renderer/service.py b/backend/services/mujoco_renderer/service.py deleted file mode 100644 index ad4ddd5..0000000 --- a/backend/services/mujoco_renderer/service.py +++ /dev/null @@ -1,119 +0,0 @@ -""" -MuJoCo 渲染服务 - 提供双视角渲染能力 -""" -from __future__ import annotations - -import base64 -import logging -from io import BytesIO -from typing import Optional - -import mujoco -import numpy as np -from PIL import Image - -logger = logging.getLogger(__name__) - -_renderer: Optional["MujocoRenderer"] = None -_mujoco_service: Optional["MujocoService"] = None - - -def get_renderer() -> "MujocoRenderer": - """获取全局 MuJoCo 渲染器单例""" - global _renderer - if _renderer is None: - from backend.services.mujoco_renderer.renderer import MujocoRenderer - _renderer = MujocoRenderer() - return _renderer - - -def get_mujoco_service() -> "MujocoService": - """获取全局 MuJoCo 服务单例""" - global _mujoco_service - if _mujoco_service is None: - _mujoco_service = MujocoService() - return _mujoco_service - - -def close_mujoco_service() -> None: - """关闭 MuJoCo 服务""" - global _mujoco_service - if _mujoco_service: - _mujoco_service.close() - _mujoco_service = None - - -class MujocoService: - """MuJoCo 服务 - 管理渲染器和状态""" - - def __init__(self): - self._renderer = get_renderer() - self._interval_ms = 50 - - @property - def interval_ms(self) -> int: - return self._interval_ms - - def set_arm_action(self, yaw: float, pitch: float, roll: float) -> None: - """设置机械臂动作（仅设定力矩，由 game loop 统一步进）""" - action = { - "motor_yaw": yaw * 50, - "motor_pitch": pitch * 50, - "motor_roll": roll * 30, - } - for actuator_name, torque in action.items(): - try: - actuator_id = mujoco.mj_name2id( - self._renderer._model, mujoco.mjtObj.mjOBJ_ACTUATOR, actuator_name - ) - if actuator_id >= 0: - self._renderer._data.ctrl[actuator_id] = torque - except Exception: - pass - - def set_car_action(self, vel_left: float, vel_right: float) -> None: - """设置小车差速动作""" - self._renderer.set_wheel_torques(vel_left, vel_right) - - def step(self) -> None: - """单步模拟""" - self._renderer.step() - - def update_topdown_camera(self, delta_azimuth: float, delta_elevation: float) -> None: - """旋转俯视相机""" - self._renderer.update_topdown_camera(delta_azimuth, delta_elevation) - - def update_topdown_distance(self, delta: float) -> None: - """缩放俯视相机""" - self._renderer.update_topdown_distance(delta) - - def render(self) -> tuple[str, str, dict]: - """ - 渲染双视角图像 - Returns: (topdown_b64, firstperson_b64, state) - """ - topdown_img = self._renderer.get_topdown_image() - firstperson_img = self._renderer.get_firstperson_image() - - topdown_b64 = self._image_to_b64(topdown_img) - firstperson_b64 = self._image_to_b64(firstperson_img) - state = self._renderer.get_state() - - return topdown_b64, firstperson_b64, state - - def _image_to_b64(self, img: np.ndarray) -> str: - """numpy 图像转 base64""" - if img.shape[2] == 3: - pil_img = Image.fromarray(img) - else: - pil_img = Image.fromarray(img[:, :, :3]) - buffer = BytesIO() - pil_img.save(buffer, format="JPEG", quality=85) - return base64.b64encode(buffer.getvalue()).decode("utf-8") - - def close(self) -> None: - """关闭渲染器""" - global _renderer - if _renderer: - _renderer.close() - _renderer = None \ No newline at end of file diff --git a/backend/services/mujoco_renderer/state.py b/backend/services/mujoco_renderer/state.py deleted file mode 100644 index e19709d..0000000 --- a/backend/services/mujoco_renderer/state.py +++ /dev/null @@ -1,32 +0,0 @@ -from __future__ import annotations - -from typing import TYPE_CHECKING - -import mujoco - -if TYPE_CHECKING: - from backend.services.mujoco_renderer.renderer import MujocoRenderer - - -class MujocoState: - """Manages MuJoCo scene state and car+arm control.""" - - def __init__(self, renderer: MujocoRenderer): - self._renderer = renderer - - def set_arm_position(self, qpos: list[float]) -> None: - """Set arm joint positions [yaw, pitch, roll, wrist...].""" - if len(qpos) >= 1: - self._renderer._data.qpos[11] = qpos[0] - if len(qpos) >= 2: - self._renderer._data.qpos[12] = qpos[1] - if len(qpos) >= 3: - self._renderer._data.qpos[13] = qpos[2] - - def get_state(self) -> dict: - """Get current state dict.""" - return self._renderer.get_state() - - def reset(self) -> None: - """Reset to initial state.""" - mujoco.mj_resetData(self._renderer._model, self._renderer._data) \ No newline at end of file diff --git a/backend/sio_handlers/__init__.py b/backend/sio_handlers/__init__.py index a271f8b..d514674 100644 --- a/backend/sio_handlers/__init__.py +++ b/backend/sio_handlers/__init__.py @@ -11,8 +11,6 @@ from backend.sio_handlers.core.namespace import SimNamespace as _SimNamespace from backend.sio_handlers.core.runtime import SioRuntimeState from backend.sio_handlers.core.tasks import game_loop_task -from backend.sio_handlers.domains.mujoco import MujocoEventsMixin -from backend.sio_handlers.core.base import BaseSimNamespace logger = logging.getLogger(__name__) @@ -77,27 +75,6 @@ def set_act_runtime(runtime): _real_runtime_state.set_act_runtime(runtime) -class MujocoNamespace( - MujocoEventsMixin, - BaseSimNamespace, -): - """MuJoCo 仿真页面专用命名空间 - /mujoco""" - - def __init__( - self, - namespace: str | None = "/mujoco", - runtime: SioRuntimeState | None = None, - sim_controller: SimController | None = None, - episode_service: EpisodeService | None = None, - ): - super().__init__( - namespace=namespace, - runtime=runtime or _sim_runtime_state, - sim_controller=sim_controller or _get_sim_controller(), - episode_service=episode_service or _get_sim_episode_service(), - ) - - class SimNamespace(_SimNamespace): """Sim 页面专用命名空间 - /sim""" def __init__( @@ -150,48 +127,3 @@ def start_game_loop( controller = sim_controller or _get_sim_controller() asyncio.create_task(game_loop_task(sio_server, runtime_state, controller, namespace=namespace)) - - -async def mujoco_game_loop_task(sio_server, runtime: SioRuntimeState, namespace: str = "/mujoco"): - """MuJoCo 渲染循环 - 持续渲染并推送状态给连接的客户端""" - from backend.services.mujoco_renderer.service import get_mujoco_service - - logger.info(f"[mujoco_game_loop] 任务已启动, namespace={namespace}") - service = get_mujoco_service() - frame_count = 0 - - while True: - try: - frame_count += 1 - if frame_count % 100 == 0: - logger.info(f"[mujoco_game_loop] frame={frame_count}, clients={len(runtime.connected_clients)}") - - if runtime.connected_clients: - # 每帧都渲染并推送（~50ms 间隔） - topdown, firstperson, state = service.render() - # Convert ndarray values in state to lists - serializable_state = { - k: v.tolist() if hasattr(v, "tolist") else v - for k, v in state.items() - } - payload = { - "topdown": topdown, - "firstperson": firstperson, - "state": serializable_state, - } - # 广播给所有连接的客户端 - await sio_server.emit("mujoco_state_update", payload, namespace=namespace) - - # 物理步进 - service.step() - - except Exception as exc: - logger.error(f"[mujoco_game_loop] 错误: {exc}") - - await asyncio.sleep(0.05) - - -def start_mujoco_game_loop(sio_server, namespace: str = "/mujoco"): - """启动 MuJoCo 游戏循环""" - # 使用 _sim_runtime_state 作为 runtime（MujocoNamespace 也用它） - asyncio.create_task(mujoco_game_loop_task(sio_server, _sim_runtime_state, namespace=namespace)) diff --git a/backend/sio_handlers/domains/mujoco/__init__.py b/backend/sio_handlers/domains/mujoco/__init__.py deleted file mode 100644 index 5b6b422..0000000 --- a/backend/sio_handlers/domains/mujoco/__init__.py +++ /dev/null @@ -1,77 +0,0 @@ -from __future__ import annotations - -import logging - -from backend.services.mujoco_renderer.service import get_mujoco_service - -logger = logging.getLogger(__name__) - - -class MujocoEventsMixin: - """MuJoCo 相关 Socket.IO 事件处理""" - - async def on_connect(self, sid: str, environ: dict, auth: dict | None = None): - """客户端连接""" - self.runtime.connected_clients.add(sid) - logger.info(f"[mujoco] 客户端连接: {sid}") - await self.emit("connected", {"sid": sid}) - - async def on_disconnect(self, sid: str): - """客户端断开""" - self.runtime.connected_clients.discard(sid) - logger.info(f"[mujoco] 客户端断开: {sid}") - - async def on_mujoco_action(self, sid: str, data: dict): - """ - 处理 MuJoCo 机械臂控制动作 - data: { yaw: float, pitch: float, roll: float } - """ - logger.info(f"[mujoco_action] sid={sid}, data={data}") - service = get_mujoco_service() - yaw = data.get("yaw", 0) - pitch = data.get("pitch", 0) - roll = data.get("roll", 0) - service.set_arm_action(yaw, pitch, roll) - - async def on_mujoco_car_action(self, sid: str, data: dict): - """ - 处理 MuJoCo 小车差速控制动作 - data: { vel_left: float, vel_right: float } - """ - logger.info(f"[mujoco_car_action] sid={sid}, data={data}") - service = get_mujoco_service() - vel_left = data.get("vel_left", 0) - vel_right = data.get("vel_right", 0) - service.set_car_action(vel_left, vel_right) - - async def on_mujoco_camera_move(self, sid: str, data: dict): - """ - 处理鼠标拖拽旋转俯视相机 - data: { delta_azimuth: float, delta_elevation: float } - """ - service = get_mujoco_service() - delta_azimuth = data.get("delta_azimuth", 0) - delta_elevation = data.get("delta_elevation", 0) - service.update_topdown_camera(delta_azimuth, delta_elevation) - - async def on_mujoco_camera_zoom(self, sid: str, data: dict): - """ - 处理鼠标滚轮缩放 - data: { delta: float } - """ - service = get_mujoco_service() - service.update_topdown_distance(data.get("delta", 0)) - - async def on_get_mujoco_state(self, sid: str): - """请求当前 MuJoCo 状态和图像""" - service = get_mujoco_service() - topdown, firstperson, state = service.render() - serializable_state = { - k: v.tolist() if hasattr(v, "tolist") else v - for k, v in state.items() - } - await self.emit("mujoco_state_update", { - "topdown": topdown, - "firstperson": firstperson, - "state": serializable_state, - }) \ No newline at end of file diff --git a/ui/src/api/socket.ts b/ui/src/api/socket.ts index eae5cc5..ea6bb46 100644 --- a/ui/src/api/socket.ts +++ b/ui/src/api/socket.ts @@ -25,7 +25,6 @@ export function createSocket(namespace: string = "/"): Socket { // 创建 Sim 和 Real 页面专用的 socket 实例 export const simSocket = createSocket("/sim"); export const realSocket = createSocket("/real"); -export const mujocoSocket = createSocket("/mujoco"); // 为了向后兼容，保留默认的全局 socket（使用 /sim 命名空间） export const socket = simSocket; @@ -134,36 +133,3 @@ export const onTrainingProgress = (socket: Socket, callback: (data: { socket.on('training_progress', callback); return () => socket.off('training_progress', callback); }; - -// ============ MuJoCo Socket 函数 ============ - -export const sendMujocoAction = (socket: Socket, yaw: number, pitch: number, roll: number) => { - socket.emit('mujoco_action', { yaw, pitch, roll }); -} - -export const sendMujocoCarAction = (socket: Socket, velLeft: number, velRight: number) => { - socket.emit('mujoco_car_action', { vel_left: velLeft, vel_right: velRight }); -} - -export const sendMujocoCameraMove = (socket: Socket, deltaAzimuth: number, deltaElevation: number) => { - socket.emit('mujoco_camera_move', { delta_azimuth: deltaAzimuth, delta_elevation: deltaElevation }); -} - -export const sendMujocoCameraZoom = (socket: Socket, delta: number) => { - socket.emit('mujoco_camera_zoom', { delta }); -} - -export const requestMujocoState = (socket: Socket) => { - socket.emit('get_mujoco_state'); -} - -export const onMujocoStateUpdate = ( - socket: Socket, - callback: (data: { topdown: string; firstperson: string; state: unknown }) => void -) => { - socket.on('mujoco_state_update', callback); - const cleanup = () => { - socket.off('mujoco_state_update', callback); - }; - return cleanup; -}; diff --git a/ui/src/pages/MujocoPage/CameraViews.tsx b/ui/src/pages/MujocoPage/CameraViews.tsx deleted file mode 100644 index 8e19c41..0000000 --- a/ui/src/pages/MujocoPage/CameraViews.tsx +++ /dev/null @@ -1,239 +0,0 @@ -import {useEffect, useState, useRef, useCallback} from "react"; -import {mujocoSocket, sendMujocoAction, sendMujocoCarAction, sendMujocoCameraMove, sendMujocoCameraZoom, onMujocoStateUpdate} from "../../api/socket"; - -interface MujocoState { - car_pos?: number[]; - car_quat?: number[]; - arm_qpos?: number[]; - arm_qvel?: number[]; -} - -export const TopDownView = () => { - const [image, setImage] = useState(""); - const [dragging, setDragging] = useState(false); - const lastPos = useRef<{x: number; y: number} | null>(null); - - useEffect(() => { - const unsubscribe = onMujocoStateUpdate(mujocoSocket, (data) => { - setImage(`data:image/jpeg;base64,${data.topdown}`); - }); - return unsubscribe; - }, []); - - const handleMouseDown = useCallback((e: React.MouseEvent) => { - setDragging(true); - lastPos.current = {x: e.clientX, y: e.clientY}; - }, []); - - const handleMouseMove = useCallback((e: React.MouseEvent) => { - if (!dragging || !lastPos.current) return; - const dx = e.clientX - lastPos.current.x; - const dy = e.clientY - lastPos.current.y; - lastPos.current = {x: e.clientX, y: e.clientY}; - sendMujocoCameraMove(mujocoSocket, dx, dy); - }, [dragging]); - - const handleMouseUp = useCallback(() => { - setDragging(false); - lastPos.current = null; - }, []); - - const handleWheel = useCallback((e: React.WheelEvent) => { - sendMujocoCameraZoom(mujocoSocket, e.deltaY > 0 ? 1 : -1); - }, []); - - return ( -
-
- - 俯视视角 (Top-Down) -
-
- {image ? ( - Top-Down View - ) : ( -
- 加载中... -
- )} -
-
- ); -}; - -export const FirstPersonView = () => { - const [image, setImage] = useState(""); - - useEffect(() => { - const unsubscribe = onMujocoStateUpdate(mujocoSocket, (data) => { - setImage(`data:image/jpeg;base64,${data.firstperson}`); - }); - return unsubscribe; - }, []); - - return ( -
-
- - 第一人称视角 (First-Person) -
-
- {image ? ( - First-Person View - ) : ( -
- 加载中... -
- )} -
-
- ); -}; - -export const ArmControl = () => { - const [yaw, setYaw] = useState(0); - const [pitch, setPitch] = useState(0); - const [roll, setRoll] = useState(0); - const [state, setState] = useState({}); - - useEffect(() => { - const unsubscribe = onMujocoStateUpdate(mujocoSocket, (data) => { - setState(data.state as MujocoState); - }); - return unsubscribe; - }, []); - - const handleArmAction = () => { - sendMujocoAction(mujocoSocket, yaw, pitch, roll); - }; - - const armNames = ["Yaw", "Pitch", "Roll"]; - const armQpos = state.arm_qpos || []; - const armQvel = state.arm_qvel || []; - - return ( -
- {/* 机械臂控制 */} -
-

机械臂控制

-
-
- - setYaw(parseFloat(e.target.value))} - className="w-full" - /> -
-
- - setPitch(parseFloat(e.target.value))} - className="w-full" - /> -
-
- - setRoll(parseFloat(e.target.value))} - className="w-full" - /> -
- -
-
- - {/* 小车控制 */} - - - {/* 关节状态显示 */} -
-

关节状态

-
- {armNames.map((name, i) => ( -
- {name}: - - pos={armQpos[i]?.toFixed(3) ?? "N/A"}, vel={armQvel[i]?.toFixed(3) ?? "N/A"} - -
- ))} -
-
-
- ); -}; - -export const CarControl = () => { - const [velLeft, setVelLeft] = useState(0); - const [velRight, setVelRight] = useState(0); - - const handleCarAction = () => { - sendMujocoCarAction(mujocoSocket, velLeft, velRight); - }; - - return ( -
-

小车控制

-
-
- - setVelLeft(parseFloat(e.target.value))} - className="w-full" - /> -
-
- - setVelRight(parseFloat(e.target.value))} - className="w-full" - /> -
- -
-
- ); -}; \ No newline at end of file From 6615a49118f55cf484aa017d2536a5a698cdcc07 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 23 May 2026 23:05:17 +0800 Subject: [PATCH 03/39] feat: migrate MuJoCo simulation from backend to browser via WASM + Three.js Replace backend MuJoCo (Python + JPEG streaming) with browser-native simulation using @mujoco/mujoco WASM bindings and Three.js rendering, enabling multi-user independent training. Fix React StrictMode duplicate canvas rendering issue. --- ui/package-lock.json | 65 ++++ ui/package.json | 3 + ui/src/pages/MujocoPage/ControlPanel.tsx | 128 ++++++++ ui/src/pages/MujocoPage/MujocoRenderer.tsx | 363 +++++++++++++++++++++ ui/src/pages/MujocoPage/carArmXml.ts | 72 ++++ ui/src/pages/MujocoPage/index.tsx | 105 +++--- ui/src/pages/MujocoPage/useMujoco.ts | 77 +++++ 7 files changed, 762 insertions(+), 51 deletions(-) create mode 100644 ui/src/pages/MujocoPage/ControlPanel.tsx create mode 100644 ui/src/pages/MujocoPage/MujocoRenderer.tsx create mode 100644 ui/src/pages/MujocoPage/carArmXml.ts create mode 100644 ui/src/pages/MujocoPage/useMujoco.ts diff --git a/ui/package-lock.json b/ui/package-lock.json index 04e86ef..8dee93a 100644 --- a/ui/package-lock.json +++ b/ui/package-lock.json @@ -8,12 +8,15 @@ "name": "ui", "version": "0.0.0", "dependencies": { + "@mujoco/mujoco": "^3.8.1", + "@types/three": "^0.184.1", "ky": "^1.14.3", "react": "^19.2.0", "react-dom": "^19.2.0", "react-router-dom": "^7.13.1", "socket.io-client": "^4.8.3", "sonner": "^2.0.7", + "three": "^0.184.0", "zustand": "^5.0.12" }, "devDependencies": { @@ -328,6 +331,12 @@ "node": ">=6.9.0" } }, + "node_modules/@dimforge/rapier3d-compat": { + "version": "0.12.0", + "resolved": "https://registry.npmjs.org/@dimforge/rapier3d-compat/-/rapier3d-compat-0.12.0.tgz", + "integrity": "sha512-uekIGetywIgopfD97oDL5PfeezkFpNhwlzlaEYNOA0N6ghdsOvh/HYjSMek5Q2O1PYvRSDFcqFVJl4r4ZBwOow==", + "license": "Apache-2.0" + }, "node_modules/@esbuild/aix-ppc64": { "version": "0.27.3", "resolved": "https://registry.npmjs.org/@esbuild/aix-ppc64/-/aix-ppc64-0.27.3.tgz", @@ -1029,6 +1038,12 @@ "@jridgewell/sourcemap-codec": "^1.4.14" } }, + "node_modules/@mujoco/mujoco": { + "version": "3.8.1", + "resolved": "https://registry.npmjs.org/@mujoco/mujoco/-/mujoco-3.8.1.tgz", + "integrity": "sha512-uIIUcdAHG48N2UBmL/iLuBoAxH1xbTs/HaJthQd5euLvk44p9ivJ5wCc+IsnqgstpCdmCZAgx2l1WGwFyxErTQ==", + "license": "Apache-2.0" + }, "node_modules/@rolldown/pluginutils": { "version": "1.0.0-rc.3", "resolved": "https://registry.npmjs.org/@rolldown/pluginutils/-/pluginutils-1.0.0-rc.3.tgz", @@ -1663,6 +1678,12 @@ "tailwindcss": "4.2.1" } }, + "node_modules/@tweenjs/tween.js": { + "version": "23.1.3", + "resolved": "https://registry.npmjs.org/@tweenjs/tween.js/-/tween.js-23.1.3.tgz", + "integrity": "sha512-vJmvvwFxYuGnF2axRtPYocag6Clbb5YS7kLL+SO/TeVFzHqDIWrNKYtcsPMibjDx9O+bu+psAy9NKfWklassUA==", + "license": "MIT" + }, "node_modules/@types/babel__core": { "version": "7.20.5", "resolved": "https://registry.npmjs.org/@types/babel__core/-/babel__core-7.20.5.tgz", @@ -1752,6 +1773,32 @@ "@types/react": "^19.2.0" } }, + "node_modules/@types/stats.js": { + "version": "0.17.4", + "resolved": "https://registry.npmjs.org/@types/stats.js/-/stats.js-0.17.4.tgz", + "integrity": "sha512-jIBvWWShCvlBqBNIZt0KAshWpvSjhkwkEu4ZUcASoAvhmrgAUI2t1dXrjSL4xXVLB4FznPrIsX3nKXFl/Dt4vA==", + "license": "MIT" + }, + "node_modules/@types/three": { + "version": "0.184.1", + "resolved": "https://registry.npmjs.org/@types/three/-/three-0.184.1.tgz", + "integrity": "sha512-6q4VdiqVsrTRqmk62/BnlcAvIrnDM0zf2ZDVKI5kZiniWrSaOHaQzmbp+BNzoggc/8tgW412pL//wZIxu2PPTA==", + "license": "MIT", + "dependencies": { + "@dimforge/rapier3d-compat": "~0.12.0", + "@tweenjs/tween.js": "~23.1.3", + "@types/stats.js": "*", + "@types/webxr": ">=0.5.17", + "fflate": "~0.8.2", + "meshoptimizer": "~1.1.1" + } + }, + "node_modules/@types/webxr": { + "version": "0.5.24", + "resolved": "https://registry.npmjs.org/@types/webxr/-/webxr-0.5.24.tgz", + "integrity": "sha512-h8fgEd/DpoS9CBrjEQXR+dIDraopAEfu4wYVNY2tEPwk60stPWhvZMf4Foo5FakuQ7HFZoa8WceaWFervK2Ovg==", + "license": "MIT" + }, "node_modules/@typescript-eslint/eslint-plugin": { "version": "8.56.1", "resolved": "https://registry.npmjs.org/@typescript-eslint/eslint-plugin/-/eslint-plugin-8.56.1.tgz", @@ -2688,6 +2735,12 @@ } } }, + "node_modules/fflate": { + "version": "0.8.3", + "resolved": "https://registry.npmjs.org/fflate/-/fflate-0.8.3.tgz", + "integrity": "sha512-tbZNuJrLwGUp3zshBtdy4W+ORxZuIh8a5ilyIEQDC5rY1f3U20JMry0Ll3WBzU58EZKsEuJFXhb5gwv8CsPvgA==", + "license": "MIT" + }, "node_modules/file-entry-cache": { "version": "8.0.0", "resolved": "https://registry.npmjs.org/file-entry-cache/-/file-entry-cache-8.0.0.tgz", @@ -3308,6 +3361,12 @@ "@jridgewell/sourcemap-codec": "^1.5.5" } }, + "node_modules/meshoptimizer": { + "version": "1.1.1", + "resolved": "https://registry.npmjs.org/meshoptimizer/-/meshoptimizer-1.1.1.tgz", + "integrity": "sha512-oRFNWJRDA/WTrVj7NWvqa5HqE1t9MYDj2VaWirQCzCCrAd2GHrqR/sQezCxiWATPNlKTcRaPRHPJwIRoPBAp5g==", + "license": "MIT" + }, "node_modules/minimatch": { "version": "3.1.5", "resolved": "https://registry.npmjs.org/minimatch/-/minimatch-3.1.5.tgz", @@ -3776,6 +3835,12 @@ "url": "https://opencollective.com/webpack" } }, + "node_modules/three": { + "version": "0.184.0", + "resolved": "https://registry.npmjs.org/three/-/three-0.184.0.tgz", + "integrity": "sha512-wtTRjG92pM5eUg/KuUnHsqSAlPM296brTOcLgMRqEeylYTh/CdtvKUvCyyCQTzFuStieWxvZb8mVTMvdPyUpxg==", + "license": "MIT" + }, "node_modules/tinyglobby": { "version": "0.2.15", "resolved": "https://registry.npmjs.org/tinyglobby/-/tinyglobby-0.2.15.tgz", diff --git a/ui/package.json b/ui/package.json index cfec7ed..abca67f 100644 --- a/ui/package.json +++ b/ui/package.json @@ -10,12 +10,15 @@ "preview": "vite preview" }, "dependencies": { + "@mujoco/mujoco": "^3.8.1", + "@types/three": "^0.184.1", "ky": "^1.14.3", "react": "^19.2.0", "react-dom": "^19.2.0", "react-router-dom": "^7.13.1", "socket.io-client": "^4.8.3", "sonner": "^2.0.7", + "three": "^0.184.0", "zustand": "^5.0.12" }, "devDependencies": { diff --git a/ui/src/pages/MujocoPage/ControlPanel.tsx b/ui/src/pages/MujocoPage/ControlPanel.tsx new file mode 100644 index 0000000..9e87873 --- /dev/null +++ b/ui/src/pages/MujocoPage/ControlPanel.tsx @@ -0,0 +1,128 @@ +import { useState, useCallback } from "react"; +import type { MjData } from "@mujoco/mujoco"; + +interface Props { + isLoaded: boolean; + setControl: (name: string, value: number) => void; + reset: () => void; + data: React.RefObject; +} + +export function ControlPanel({ isLoaded, setControl, reset }: Props) { + const [yaw, setYaw] = useState(0); + const [pitch, setPitch] = useState(0); + const [roll, setRoll] = useState(0); + const [leftWheel, setLeftWheel] = useState(0); + const [rightWheel, setRightWheel] = useState(0); + + const applyArm = useCallback(() => { + setControl("motor_yaw", yaw * 50); + setControl("motor_pitch", pitch * 50); + setControl("motor_roll", roll * 30); + }, [yaw, pitch, roll, setControl]); + + const applyWheels = useCallback(() => { + setControl("motor_wheel_fl", leftWheel); + setControl("motor_wheel_rl", leftWheel); + setControl("motor_wheel_fr", rightWheel); + setControl("motor_wheel_rr", rightWheel); + }, [leftWheel, rightWheel, setControl]); + + if (!isLoaded) return null; + + return ( +
+
+

+ Arm Control +

+ {[ + { label: "Yaw", value: yaw, set: setYaw, min: -1, max: 1, step: 0.1 }, + { + label: "Pitch", + value: pitch, + set: setPitch, + min: -1, + max: 1, + step: 0.1, + }, + { + label: "Roll", + value: roll, + set: setRoll, + min: -1, + max: 1, + step: 0.1, + }, + ].map(({ label, value, set, min, max, step }) => ( +
+ + set(parseFloat(e.target.value))} + className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-violet-500" + /> +
+ ))} + +
+ +
+

+ Car Control +

+ {[ + { + label: "Left", + value: leftWheel, + set: setLeftWheel, + }, + { + label: "Right", + value: rightWheel, + set: setRightWheel, + }, + ].map(({ label, value, set }) => ( +
+ + set(parseFloat(e.target.value))} + className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-blue-500" + /> +
+ ))} + +
+ + +
+ ); +} diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx new file mode 100644 index 0000000..37ce53d --- /dev/null +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -0,0 +1,363 @@ +import { useEffect, useRef, useCallback } from "react"; +import * as THREE from "three"; +import type { MainModule, MjModel, MjData } from "@mujoco/mujoco"; + +const MJ_GEOM_PLANE = 0; +const MJ_GEOM_SPHERE = 2; +const MJ_GEOM_CYLINDER = 5; +const MJ_GEOM_BOX = 6; + +const T = new THREE.Matrix4().set( + 1, 0, 0, 0, + 0, 0, 1, 0, + 0, -1, 0, 0, + 0, 0, 0, 1, +); +const Tinv = new THREE.Matrix4().set( + 1, 0, 0, 0, + 0, 0, -1, 0, + 0, 1, 0, 0, + 0, 0, 0, 1, +); + +function mjToThree(pos: Float64Array, mat: Float64Array) { + const position = new THREE.Vector3(pos[0], pos[2], -pos[1]); + + const Rmj = new THREE.Matrix4().set( + mat[0], mat[3], mat[6], 0, + mat[1], mat[4], mat[7], 0, + mat[2], mat[5], mat[8], 0, + 0, 0, 0, 1, + ); + + const Rthree = T.clone().multiply(Rmj).multiply(Tinv); + const quaternion = new THREE.Quaternion().setFromRotationMatrix(Rthree); + + return { position, quaternion }; +} + +function createGeomMesh( + type: number, + size: Float64Array, + rgba: Float64Array, +): THREE.Mesh { + let geometry: THREE.BufferGeometry; + let material: THREE.MeshStandardMaterial; + + const alpha = rgba.length >= 4 ? rgba[3] : 1; + const color = new THREE.Color(rgba[0], rgba[1], rgba[2]); + const transparent = alpha < 0.99; + + switch (type) { + case MJ_GEOM_PLANE: { + geometry = new THREE.PlaneGeometry(20, 20); + geometry.rotateX(-Math.PI / 2); + material = new THREE.MeshStandardMaterial({ + color, + side: THREE.DoubleSide, + roughness: 0.9, + transparent, + opacity: alpha, + }); + break; + } + case MJ_GEOM_SPHERE: + geometry = new THREE.SphereGeometry(size[0], 32, 32); + material = new THREE.MeshStandardMaterial({ + color, + roughness: 0.4, + transparent, + opacity: alpha, + }); + break; + case MJ_GEOM_CYLINDER: + geometry = new THREE.CylinderGeometry(size[0], size[0], size[1] * 2, 32); + material = new THREE.MeshStandardMaterial({ + color, + roughness: 0.5, + metalness: 0.3, + transparent, + opacity: alpha, + }); + break; + case MJ_GEOM_BOX: + geometry = new THREE.BoxGeometry(size[0] * 2, size[2] * 2, size[1] * 2); + material = new THREE.MeshStandardMaterial({ + color, + roughness: 0.5, + metalness: 0.2, + transparent, + opacity: alpha, + }); + break; + default: + geometry = new THREE.BoxGeometry(0.1, 0.1, 0.1); + material = new THREE.MeshStandardMaterial({ color: 0xff00ff }); + } + + const mesh = new THREE.Mesh(geometry, material); + mesh.castShadow = true; + mesh.receiveShadow = true; + return mesh; +} + +interface Props { + mujoco: React.RefObject; + model: React.RefObject; + data: React.RefObject; + isLoaded: boolean; + onStep: () => void; +} + +export default function MujocoRenderer({ + mujoco, + model, + data, + isLoaded, + onStep, +}: Props) { + const containerRef = useRef(null); + const sceneRef = useRef(null); + const cameraRef = useRef(null); + const rendererRef = useRef(null); + const meshesRef = useRef([]); + const frameCountRef = useRef(0); + const animRef = useRef(0); + const draggingRef = useRef(false); + const lastMouseRef = useRef({ x: 0, y: 0 }); + const camStateRef = useRef({ + azimuth: 0.5, + elevation: 0.4, + distance: 6, + target: new THREE.Vector3(0, 0.3, 0), + }); + + const setupScene = useCallback(() => { + const container = containerRef.current; + if (!container) return; + + const scene = new THREE.Scene(); + scene.background = new THREE.Color(0x2a2a4e); + + const w = container.clientWidth; + const h = container.clientHeight; + const camera = new THREE.PerspectiveCamera(50, w / h, 0.1, 100); + camera.position.set(5, 4, 6); + camera.lookAt(0, 0.3, 0); + + const renderer = new THREE.WebGLRenderer({ antialias: true, alpha: false }); + renderer.setClearColor(0x2a2a4e, 1); + renderer.setSize(w, h); + renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)); + renderer.shadowMap.enabled = true; + renderer.domElement.style.position = "absolute"; + renderer.domElement.style.top = "0"; + renderer.domElement.style.left = "0"; + container.appendChild(renderer.domElement); + + const ambient = new THREE.AmbientLight(0xffffff, 1.0); + scene.add(ambient); + + const dir = new THREE.DirectionalLight(0xffffff, 1.5); + dir.position.set(5, 8, 3); + dir.castShadow = true; + dir.shadow.mapSize.set(1024, 1024); + scene.add(dir); + + const hemi = new THREE.HemisphereLight(0x87ceeb, 0x3a3a3a, 0.7); + scene.add(hemi); + + const grid = new THREE.GridHelper(10, 20, 0x444466, 0x222244); + scene.add(grid); + + const axes = new THREE.AxesHelper(2); + scene.add(axes); + + sceneRef.current = scene; + cameraRef.current = camera; + rendererRef.current = renderer; + }, []); + + const updateCamera = useCallback(() => { + const camera = cameraRef.current; + const cs = camStateRef.current; + if (!camera) return; + + const az = cs.azimuth; + const el = cs.elevation; + const d = cs.distance; + const t = cs.target; + + camera.position.set( + t.x + d * Math.cos(el) * Math.sin(az), + t.y + d * Math.sin(el), + t.z + d * Math.cos(el) * Math.cos(az), + ); + camera.lookAt(t); + }, []); + + useEffect(() => { + setupScene(); + const handleResize = () => { + const container = containerRef.current; + const renderer = rendererRef.current; + const camera = cameraRef.current; + if (!container || !renderer || !camera) return; + const w = container.clientWidth; + const h = container.clientHeight; + renderer.setSize(w, h); + camera.aspect = w / h; + camera.updateProjectionMatrix(); + }; + window.addEventListener("resize", handleResize); + return () => { + window.removeEventListener("resize", handleResize); + cancelAnimationFrame(animRef.current); + rendererRef.current?.domElement?.remove(); + rendererRef.current?.dispose(); + }; + }, [setupScene]); + + const prevLoadedRef = useRef(false); + useEffect(() => { + if (!isLoaded || !model.current || prevLoadedRef.current) return; + prevLoadedRef.current = true; + + const m = model.current!; + const scene = sceneRef.current; + if (!scene) return; + + meshesRef.current.forEach((mesh) => scene.remove(mesh)); + meshesRef.current = []; + + console.log(`[MujocoRenderer] Creating ${m.ngeom} geoms`); + for (let i = 0; i < m.ngeom; i++) { + const type = m.geom_type[i]; + const size = m.geom_size.slice(i * 3, i * 3 + 3) as Float64Array; + const rgba = m.geom_rgba.slice(i * 4, i * 4 + 4) as Float64Array; + + const mesh = createGeomMesh(type, size, rgba); + scene.add(mesh); + meshesRef.current.push(mesh); + + if (i === 0) { + console.log( + `[MujocoRenderer] geom[0] type=${type} size=[${size[0].toFixed(2)},${size[1].toFixed(2)},${size[2].toFixed(2)}] rgba=[${rgba[0].toFixed(2)},${rgba[1].toFixed(2)},${rgba[2].toFixed(2)},${rgba[3].toFixed(2)}]`, + ); + } + } + console.log(`[MujocoRenderer] Created ${meshesRef.current.length} meshes`); + + updateCamera(); + }, [isLoaded, model, updateCamera]); + + useEffect(() => { + let running = true; + + function loop() { + if (!running) return; + + const m = mujoco.current; + const d = data.current; + const scene = sceneRef.current; + + if (m && d && scene) { + onStep(); + + if (frameCountRef.current === 0) { + const carGeomIdx = 2; // car chassis + const carPos = d.geom(carGeomIdx).xpos as Float64Array; + console.log( + `[MujocoRenderer] frame0 car chassis pos: [${carPos[0].toFixed(3)}, ${carPos[1].toFixed(3)}, ${carPos[2].toFixed(3)}]`, + ); + frameCountRef.current = 1; + } + + for (let i = 0; i < meshesRef.current.length; i++) { + const g = d.geom(i); + const pos = g.xpos as Float64Array; + const mat = g.xmat as Float64Array; + + const { position, quaternion } = mjToThree(pos, mat); + meshesRef.current[i].position.copy(position); + meshesRef.current[i].quaternion.copy(quaternion); + } + } + + const renderer = rendererRef.current; + const camera = cameraRef.current; + if (renderer && camera && scene) { + renderer.render(scene, camera); + } + + animRef.current = requestAnimationFrame(loop); + } + + if (isLoaded) { + loop(); + } + + return () => { + running = false; + cancelAnimationFrame(animRef.current); + }; + }, [isLoaded, mujoco, data, onStep]); + + const handlePointerDown = useCallback((e: React.PointerEvent) => { + draggingRef.current = true; + lastMouseRef.current = { x: e.clientX, y: e.clientY }; + (e.target as HTMLElement).setPointerCapture(e.pointerId); + }, []); + + const handlePointerMove = useCallback( + (e: React.PointerEvent) => { + if (!draggingRef.current) return; + const dx = e.clientX - lastMouseRef.current.x; + const dy = e.clientY - lastMouseRef.current.y; + lastMouseRef.current = { x: e.clientX, y: e.clientY }; + + camStateRef.current.azimuth -= dx * 0.005; + camStateRef.current.elevation += dy * 0.005; + camStateRef.current.elevation = Math.max( + -1.5, + Math.min(1.5, camStateRef.current.elevation), + ); + updateCamera(); + }, + [updateCamera], + ); + + const handlePointerUp = useCallback(() => { + draggingRef.current = false; + }, []); + + const handleWheel = useCallback( + (e: React.WheelEvent) => { + camStateRef.current.distance += e.deltaY * 0.01; + camStateRef.current.distance = Math.max( + 1, + Math.min(30, camStateRef.current.distance), + ); + updateCamera(); + }, + [updateCamera], + ); + + return ( +
+ {!isLoaded && ( +
+

Loading MuJoCo WASM...

+
+ )} +
+ ); +} diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts new file mode 100644 index 0000000..6a25d8f --- /dev/null +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -0,0 +1,72 @@ +export const CAR_ARM_XML = ` + `; diff --git a/ui/src/pages/MujocoPage/index.tsx b/ui/src/pages/MujocoPage/index.tsx index c3f689a..014bb67 100644 --- a/ui/src/pages/MujocoPage/index.tsx +++ b/ui/src/pages/MujocoPage/index.tsx @@ -1,57 +1,60 @@ -import {useEffect, useState} from "react"; -import {mujocoSocket} from "../../api/socket"; -import {TopDownView, FirstPersonView} from "./CameraViews"; +import { useMujoco } from "./useMujoco"; +import MujocoRenderer from "./MujocoRenderer"; +import { ControlPanel } from "./ControlPanel"; export default function MujocoPage() { - const [connected, setConnected] = useState(false); + const { isLoaded, mujoco, model, data, step, setControl, reset } = + useMujoco(); - useEffect(() => { - mujocoSocket.on("connect", () => { - setConnected(true); - }); - mujocoSocket.on("disconnect", () => { - setConnected(false); - }); - - return () => { - mujocoSocket.off("connect"); - mujocoSocket.off("disconnect"); - }; - }, []); - - return ( -
- {/* 顶部标题栏 */} -
-
-
- MJC -
-
-

MuJoCo 小车+机械臂

-

3D Physics Simulation

-
-
-
-
- - {connected ? 'Connected' : 'Disconnected'} -
-
-
+ return ( +
+
+
+
+ MJC +
+
+

+ MuJoCo WASM + Three.js +

+

Browser-native simulation

+
+
+
+ + FPS: {isLoaded ? "60" : "--"} + +
+ + + {isLoaded ? "Running" : "Loading..."} + +
+
+
- {/* 主内容区 */} -
- {/* 中间 - 俯视视角 */} -
- -
+
+
+ +
- {/* 右侧 - 第一人称视角 */} -
- -
-
+
+
- ); -} \ No newline at end of file +
+
+ ); +} diff --git a/ui/src/pages/MujocoPage/useMujoco.ts b/ui/src/pages/MujocoPage/useMujoco.ts new file mode 100644 index 0000000..b83ac0f --- /dev/null +++ b/ui/src/pages/MujocoPage/useMujoco.ts @@ -0,0 +1,77 @@ +import { useEffect, useRef, useState, useCallback } from "react"; +import type { MainModule, MjModel, MjData } from "@mujoco/mujoco"; +import wasmUrl from "@mujoco/mujoco/mujoco.wasm?url"; +import { CAR_ARM_XML } from "./carArmXml"; + +export function useMujoco() { + const [isLoaded, setIsLoaded] = useState(false); + const mujocoRef = useRef(null); + const modelRef = useRef(null); + const dataRef = useRef(null); + + useEffect(() => { + let cancelled = false; + + async function init() { + const { default: loadMujoco } = await import("@mujoco/mujoco"); + const mujoco = await loadMujoco({ + locateFile: (path: string) => { + if (path.endsWith(".wasm")) return wasmUrl; + return path; + }, + }); + if (cancelled) return; + + mujocoRef.current = mujoco; + + const model = (mujoco.MjModel as unknown as { from_xml_string: (xml: string) => MjModel }).from_xml_string(CAR_ARM_XML); + const data = new mujoco.MjData(model); + + modelRef.current = model; + dataRef.current = data; + setIsLoaded(true); + } + + init(); + return () => { + cancelled = true; + }; + }, []); + + const step = useCallback(() => { + const m = mujocoRef.current; + const model = modelRef.current; + const data = dataRef.current; + if (!m || !model || !data) return; + m.mj_step(model, data); + }, []); + + const setControl = useCallback((name: string, value: number) => { + const m = mujocoRef.current; + const model = modelRef.current; + const data = dataRef.current; + if (!m || !model || !data) return; + const id = m.mj_name2id(model, m.mjtObj.mjOBJ_ACTUATOR, name); + if (id >= 0) { + data.ctrl[id] = value; + } + }, []); + + const reset = useCallback(() => { + const m = mujocoRef.current; + const model = modelRef.current; + const data = dataRef.current; + if (!m || !model || !data) return; + m.mj_resetData(model, data); + }, []); + + return { + isLoaded, + mujoco: mujocoRef, + model: modelRef, + data: dataRef, + step, + setControl, + reset, + }; +} From 9607b4e633fd195d4b13e2fd5c08a49ded2c577d Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sun, 24 May 2026 23:50:03 +0800 Subject: [PATCH 04/39] feat: add keyboard drive controls, first-person camera overlay, and orientation axes gizmo - Add WASD/Arrow keyboard controls for driving the car - Add first-person camera overlay in bottom-right corner - Add orientation axes gizmo in bottom-left corner - Simplify car model to wheels + chassis, remove robotic arm - Add wheel spoke geometry for visible rotation - Fix setControl to use manual actuator name resolution - Add multi-substep physics (16 substeps per frame) - Clean up debug logging --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 332 ++++++++++++++++----- ui/src/pages/MujocoPage/carArmXml.ts | 44 +-- ui/src/pages/MujocoPage/index.tsx | 64 +++- ui/src/pages/MujocoPage/useMujoco.ts | 26 +- 4 files changed, 345 insertions(+), 121 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 37ce53d..4e35406 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -7,6 +7,8 @@ const MJ_GEOM_SPHERE = 2; const MJ_GEOM_CYLINDER = 5; const MJ_GEOM_BOX = 6; +const SUBSTEPS = 16; + const T = new THREE.Matrix4().set( 1, 0, 0, 0, 0, 0, 1, 0, @@ -40,9 +42,8 @@ function createGeomMesh( type: number, size: Float64Array, rgba: Float64Array, -): THREE.Mesh { - let geometry: THREE.BufferGeometry; - let material: THREE.MeshStandardMaterial; +): THREE.Object3D { + let mesh: THREE.Object3D; const alpha = rgba.length >= 4 ? rgba[3] : 1; const color = new THREE.Color(rgba[0], rgba[1], rgba[2]); @@ -50,57 +51,124 @@ function createGeomMesh( switch (type) { case MJ_GEOM_PLANE: { - geometry = new THREE.PlaneGeometry(20, 20); + const geometry = new THREE.PlaneGeometry(20, 20); geometry.rotateX(-Math.PI / 2); - material = new THREE.MeshStandardMaterial({ + const material = new THREE.MeshStandardMaterial({ color, side: THREE.DoubleSide, roughness: 0.9, transparent, opacity: alpha, }); + mesh = new THREE.Mesh(geometry, material); break; } - case MJ_GEOM_SPHERE: - geometry = new THREE.SphereGeometry(size[0], 32, 32); - material = new THREE.MeshStandardMaterial({ + case MJ_GEOM_SPHERE: { + const geometry = new THREE.SphereGeometry(size[0], 32, 32); + const material = new THREE.MeshStandardMaterial({ color, roughness: 0.4, transparent, opacity: alpha, }); + mesh = new THREE.Mesh(geometry, material); break; - case MJ_GEOM_CYLINDER: - geometry = new THREE.CylinderGeometry(size[0], size[0], size[1] * 2, 32); - material = new THREE.MeshStandardMaterial({ + } + case MJ_GEOM_CYLINDER: { + const radius = size[0]; + const halfHeight = size[1]; + const wheelGroup = new THREE.Group(); + + const cylGeom = new THREE.CylinderGeometry(radius, radius, halfHeight * 2, 32); + const cylMat = new THREE.MeshStandardMaterial({ color, roughness: 0.5, metalness: 0.3, transparent, opacity: alpha, }); + const cylMesh = new THREE.Mesh(cylGeom, cylMat); + cylMesh.castShadow = true; + cylMesh.receiveShadow = true; + wheelGroup.add(cylMesh); + + // Cross-spokes to make rotation visible (cylinder rotating around own axis is invisible) + const spokeHalfLen = radius * 0.9; + const spokeThick = halfHeight * 0.3; + const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); + const spokeGeomZ = new THREE.BoxGeometry(spokeThick, spokeThick, spokeHalfLen * 2); + const spokeMat = new THREE.MeshStandardMaterial({ + color: 0x333333, + roughness: 0.5, + metalness: 0.4, + }); + const spokeX = new THREE.Mesh(spokeGeomX, spokeMat); + const spokeZ = new THREE.Mesh(spokeGeomZ, spokeMat); + spokeX.castShadow = true; + spokeZ.castShadow = true; + wheelGroup.add(spokeX); + wheelGroup.add(spokeZ); + + mesh = wheelGroup; break; - case MJ_GEOM_BOX: - geometry = new THREE.BoxGeometry(size[0] * 2, size[2] * 2, size[1] * 2); - material = new THREE.MeshStandardMaterial({ + } + case MJ_GEOM_BOX: { + const geometry = new THREE.BoxGeometry(size[0] * 2, size[2] * 2, size[1] * 2); + const material = new THREE.MeshStandardMaterial({ color, roughness: 0.5, metalness: 0.2, transparent, opacity: alpha, }); + mesh = new THREE.Mesh(geometry, material); break; - default: - geometry = new THREE.BoxGeometry(0.1, 0.1, 0.1); - material = new THREE.MeshStandardMaterial({ color: 0xff00ff }); + } + default: { + const geometry = new THREE.BoxGeometry(0.1, 0.1, 0.1); + const material = new THREE.MeshStandardMaterial({ color: 0xff00ff }); + mesh = new THREE.Mesh(geometry, material); + } } - const mesh = new THREE.Mesh(geometry, material); mesh.castShadow = true; mesh.receiveShadow = true; return mesh; } +function toUint8Array(buf: unknown): Uint8Array | null { + if (typeof buf === "string") { + return new TextEncoder().encode(buf); + } + if (buf instanceof ArrayBuffer) { + return new Uint8Array(buf); + } + if (ArrayBuffer.isView(buf)) { + return new Uint8Array(buf.buffer, buf.byteOffset, buf.byteLength); + } + return null; +} + +function resolveName(names: unknown, addr: number): string { + const buf = toUint8Array(names); + if (!buf) return ""; + let end = addr; + while (end < buf.length && buf[end] !== 0) end++; + return new TextDecoder().decode(buf.slice(addr, end)); +} + +function resolveCameraIndices(model: MjModel) { + let fpIdx = -1; + let tdIdx = -1; + const names = model.names; + for (let i = 0; i < model.ncam; i++) { + const name = resolveName(names, model.name_camadr[i]); + if (name === "firstperson") fpIdx = i; + else if (name === "topdown") tdIdx = i; + } + return { fpIdx, tdIdx }; +} + interface Props { mujoco: React.RefObject; model: React.RefObject; @@ -117,43 +185,86 @@ export default function MujocoRenderer({ onStep, }: Props) { const containerRef = useRef(null); + const fpContainerRef = useRef(null); + const axesContainerRef = useRef(null); const sceneRef = useRef(null); - const cameraRef = useRef(null); - const rendererRef = useRef(null); - const meshesRef = useRef([]); - const frameCountRef = useRef(0); + const orbitCameraRef = useRef(null); + const fpCameraRef = useRef(null); + const axesCameraRef = useRef(null); + const axesSceneRef = useRef(null); + const axesGroupRef = useRef(null); + const mainRendererRef = useRef(null); + const fpRendererRef = useRef(null); + const axesRendererRef = useRef(null); + const meshesRef = useRef([]); const animRef = useRef(0); const draggingRef = useRef(false); const lastMouseRef = useRef({ x: 0, y: 0 }); - const camStateRef = useRef({ + const orbitStateRef = useRef({ azimuth: 0.5, elevation: 0.4, distance: 6, target: new THREE.Vector3(0, 0.3, 0), }); + const fpCamIdxRef = useRef(-1); const setupScene = useCallback(() => { const container = containerRef.current; - if (!container) return; + const fpContainer = fpContainerRef.current; + if (!container || !fpContainer) return; const scene = new THREE.Scene(); scene.background = new THREE.Color(0x2a2a4e); const w = container.clientWidth; const h = container.clientHeight; - const camera = new THREE.PerspectiveCamera(50, w / h, 0.1, 100); - camera.position.set(5, 4, 6); - camera.lookAt(0, 0.3, 0); - - const renderer = new THREE.WebGLRenderer({ antialias: true, alpha: false }); - renderer.setClearColor(0x2a2a4e, 1); - renderer.setSize(w, h); - renderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)); - renderer.shadowMap.enabled = true; - renderer.domElement.style.position = "absolute"; - renderer.domElement.style.top = "0"; - renderer.domElement.style.left = "0"; - container.appendChild(renderer.domElement); + const orbitCamera = new THREE.PerspectiveCamera(50, w / h, 0.1, 100); + + const fpW = fpContainer.clientWidth || 320; + const fpH = fpContainer.clientHeight || 210; + const fpCamera = new THREE.PerspectiveCamera(110, fpW / fpH, 0.1, 100); + + const mainRenderer = new THREE.WebGLRenderer({ antialias: true, alpha: false }); + mainRenderer.setClearColor(0x2a2a4e, 1); + mainRenderer.setSize(w, h); + mainRenderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)); + mainRenderer.shadowMap.enabled = true; + mainRenderer.domElement.style.position = "absolute"; + mainRenderer.domElement.style.top = "0"; + mainRenderer.domElement.style.left = "0"; + container.appendChild(mainRenderer.domElement); + + const fpRenderer = new THREE.WebGLRenderer({ antialias: true, alpha: false }); + fpRenderer.setClearColor(0x2a2a4e, 1); + fpRenderer.setSize(fpW, fpH); + fpRenderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)); + fpRenderer.shadowMap.enabled = true; + fpContainer.appendChild(fpRenderer.domElement); + + // Axes gizmo (bottom-left) + const axesContainer = axesContainerRef.current; + let axesScene: THREE.Scene | null = null; + let axesCamera: THREE.PerspectiveCamera | null = null; + let axesRenderer: THREE.WebGLRenderer | null = null; + if (axesContainer) { + axesScene = new THREE.Scene(); + const axesGroup = new THREE.Group(); + axesGroup.add(new THREE.AxesHelper(1.0)); + axesScene.add(axesGroup); + axesGroupRef.current = axesGroup; + const size = 120; + const halfSize = 1.3; + axesCamera = new THREE.OrthographicCamera(-halfSize, halfSize, halfSize, -halfSize, 0.1, 10); + axesCamera.position.set(0, 0, 3); + axesRenderer = new THREE.WebGLRenderer({ antialias: true, alpha: false }); + axesRenderer.setClearColor(0x1a1a2e, 1); + axesRenderer.setSize(size, size); + axesRenderer.setPixelRatio(Math.min(window.devicePixelRatio, 2)); + axesContainer.appendChild(axesRenderer.domElement); + } + axesCameraRef.current = axesCamera; + axesSceneRef.current = axesScene; + axesRendererRef.current = axesRenderer; const ambient = new THREE.AmbientLight(0xffffff, 1.0); scene.add(ambient); @@ -174,13 +285,15 @@ export default function MujocoRenderer({ scene.add(axes); sceneRef.current = scene; - cameraRef.current = camera; - rendererRef.current = renderer; + orbitCameraRef.current = orbitCamera; + fpCameraRef.current = fpCamera; + mainRendererRef.current = mainRenderer; + fpRendererRef.current = fpRenderer; }, []); - const updateCamera = useCallback(() => { - const camera = cameraRef.current; - const cs = camStateRef.current; + const updateOrbitCamera = useCallback(() => { + const camera = orbitCameraRef.current; + const cs = orbitStateRef.current; if (!camera) return; const az = cs.azimuth; @@ -200,21 +313,37 @@ export default function MujocoRenderer({ setupScene(); const handleResize = () => { const container = containerRef.current; - const renderer = rendererRef.current; - const camera = cameraRef.current; - if (!container || !renderer || !camera) return; + const fpContainer = fpContainerRef.current; + const mainRenderer = mainRendererRef.current; + const fpRenderer = fpRendererRef.current; + const orbitCamera = orbitCameraRef.current; + const fpCamera = fpCameraRef.current; + if (!container || !mainRenderer || !orbitCamera) return; + const w = container.clientWidth; const h = container.clientHeight; - renderer.setSize(w, h); - camera.aspect = w / h; - camera.updateProjectionMatrix(); + mainRenderer.setSize(w, h); + orbitCamera.aspect = w / h; + orbitCamera.updateProjectionMatrix(); + + if (fpContainer && fpRenderer && fpCamera) { + const fpW = fpContainer.clientWidth; + const fpH = fpContainer.clientHeight; + fpRenderer.setSize(fpW, fpH); + fpCamera.aspect = fpW / fpH; + fpCamera.updateProjectionMatrix(); + } }; window.addEventListener("resize", handleResize); return () => { window.removeEventListener("resize", handleResize); cancelAnimationFrame(animRef.current); - rendererRef.current?.domElement?.remove(); - rendererRef.current?.dispose(); + mainRendererRef.current?.domElement?.remove(); + mainRendererRef.current?.dispose(); + fpRendererRef.current?.domElement?.remove(); + fpRendererRef.current?.dispose(); + axesRendererRef.current?.domElement?.remove(); + axesRendererRef.current?.dispose(); }; }, [setupScene]); @@ -230,7 +359,6 @@ export default function MujocoRenderer({ meshesRef.current.forEach((mesh) => scene.remove(mesh)); meshesRef.current = []; - console.log(`[MujocoRenderer] Creating ${m.ngeom} geoms`); for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; const size = m.geom_size.slice(i * 3, i * 3 + 3) as Float64Array; @@ -239,17 +367,24 @@ export default function MujocoRenderer({ const mesh = createGeomMesh(type, size, rgba); scene.add(mesh); meshesRef.current.push(mesh); + } - if (i === 0) { - console.log( - `[MujocoRenderer] geom[0] type=${type} size=[${size[0].toFixed(2)},${size[1].toFixed(2)},${size[2].toFixed(2)}] rgba=[${rgba[0].toFixed(2)},${rgba[1].toFixed(2)},${rgba[2].toFixed(2)},${rgba[3].toFixed(2)}]`, - ); - } + const { fpIdx, tdIdx } = resolveCameraIndices(m); + fpCamIdxRef.current = fpIdx; + + const orbitCam = orbitCameraRef.current; + const fpCam = fpCameraRef.current; + if (tdIdx >= 0 && orbitCam) { + orbitCam.fov = m.cam_fovy[tdIdx]; + orbitCam.updateProjectionMatrix(); + } + if (fpIdx >= 0 && fpCam) { + fpCam.fov = m.cam_fovy[fpIdx]; + fpCam.updateProjectionMatrix(); } - console.log(`[MujocoRenderer] Created ${meshesRef.current.length} meshes`); - updateCamera(); - }, [isLoaded, model, updateCamera]); + updateOrbitCamera(); + }, [isLoaded, model, updateOrbitCamera]); useEffect(() => { let running = true; @@ -260,19 +395,24 @@ export default function MujocoRenderer({ const m = mujoco.current; const d = data.current; const scene = sceneRef.current; + const mainRenderer = mainRendererRef.current; + const fpRenderer = fpRendererRef.current; + const orbitCamera = orbitCameraRef.current; + const fpCamera = fpCameraRef.current; if (m && d && scene) { onStep(); - if (frameCountRef.current === 0) { - const carGeomIdx = 2; // car chassis - const carPos = d.geom(carGeomIdx).xpos as Float64Array; - console.log( - `[MujocoRenderer] frame0 car chassis pos: [${carPos[0].toFixed(3)}, ${carPos[1].toFixed(3)}, ${carPos[2].toFixed(3)}]`, - ); - frameCountRef.current = 1; + for (let s = 1; s < SUBSTEPS; s++) { + m.mj_step(model.current!, data.current!); } + // Track car body position for orbit camera target + const carBodyId = 1; + const carXpos = d.xpos.slice(carBodyId * 3, carBodyId * 3 + 3) as Float64Array; + orbitStateRef.current.target.set(carXpos[0], carXpos[2] + 0.3, -carXpos[1]); + updateOrbitCamera(); + for (let i = 0; i < meshesRef.current.length; i++) { const g = d.geom(i); const pos = g.xpos as Float64Array; @@ -282,12 +422,33 @@ export default function MujocoRenderer({ meshesRef.current[i].position.copy(position); meshesRef.current[i].quaternion.copy(quaternion); } + + // First-person camera: use MuJoCo native camera world pose + const fpIdx = fpCamIdxRef.current; + if (fpCamera && fpIdx >= 0) { + const camPos = d.cam_xpos.slice(fpIdx * 3, fpIdx * 3 + 3) as Float64Array; + const camMat = d.cam_xmat.slice(fpIdx * 9, fpIdx * 9 + 9) as Float64Array; + const { position, quaternion } = mjToThree(camPos, camMat); + fpCamera.position.copy(position); + fpCamera.quaternion.copy(quaternion); + } + } + + if (mainRenderer && orbitCamera && scene) { + mainRenderer.render(scene, orbitCamera); + } + if (fpRenderer && fpCamera && scene) { + fpRenderer.render(scene, fpCamera); } - const renderer = rendererRef.current; - const camera = cameraRef.current; - if (renderer && camera && scene) { - renderer.render(scene, camera); + // Axes gizmo: rotate the axes group to match orbit camera view + const axesCamera = axesCameraRef.current; + const axesScene = axesSceneRef.current; + const axesRenderer = axesRendererRef.current; + const axesGroup = axesGroupRef.current; + if (axesCamera && axesScene && axesRenderer && axesGroup && orbitCamera) { + axesGroup.quaternion.copy(orbitCamera.quaternion).invert(); + axesRenderer.render(axesScene, axesCamera); } animRef.current = requestAnimationFrame(loop); @@ -316,15 +477,15 @@ export default function MujocoRenderer({ const dy = e.clientY - lastMouseRef.current.y; lastMouseRef.current = { x: e.clientX, y: e.clientY }; - camStateRef.current.azimuth -= dx * 0.005; - camStateRef.current.elevation += dy * 0.005; - camStateRef.current.elevation = Math.max( + orbitStateRef.current.azimuth -= dx * 0.005; + orbitStateRef.current.elevation += dy * 0.005; + orbitStateRef.current.elevation = Math.max( -1.5, - Math.min(1.5, camStateRef.current.elevation), + Math.min(1.5, orbitStateRef.current.elevation), ); - updateCamera(); + updateOrbitCamera(); }, - [updateCamera], + [updateOrbitCamera], ); const handlePointerUp = useCallback(() => { @@ -333,14 +494,14 @@ export default function MujocoRenderer({ const handleWheel = useCallback( (e: React.WheelEvent) => { - camStateRef.current.distance += e.deltaY * 0.01; - camStateRef.current.distance = Math.max( + orbitStateRef.current.distance += e.deltaY * 0.01; + orbitStateRef.current.distance = Math.max( 1, - Math.min(30, camStateRef.current.distance), + Math.min(30, orbitStateRef.current.distance), ); - updateCamera(); + updateOrbitCamera(); }, - [updateCamera], + [updateOrbitCamera], ); return ( @@ -358,6 +519,15 @@ export default function MujocoRenderer({

Loading MuJoCo WASM...

)} +
+
); } diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 6a25d8f..284e82d 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -10,60 +10,36 @@ export const CAR_ARM_XML = ` - - - - - - - + + + + + + - + - + - + - + - - - - - - - - - - - - - - - - - - - - - - - diff --git a/ui/src/pages/MujocoPage/index.tsx b/ui/src/pages/MujocoPage/index.tsx index 014bb67..5efc41b 100644 --- a/ui/src/pages/MujocoPage/index.tsx +++ b/ui/src/pages/MujocoPage/index.tsx @@ -1,10 +1,69 @@ +import { useEffect, useRef, useCallback } from "react"; import { useMujoco } from "./useMujoco"; import MujocoRenderer from "./MujocoRenderer"; import { ControlPanel } from "./ControlPanel"; +const DRIVE_SPEED = 5; +const TURN_SPEED = 3; + export default function MujocoPage() { const { isLoaded, mujoco, model, data, step, setControl, reset } = useMujoco(); + const keysRef = useRef>(new Set()); + + const applyDrive = useCallback(() => { + let leftVel = 0; + let rightVel = 0; + + const k = keysRef.current; + + if (k.has("KeyW") || k.has("ArrowUp")) { + leftVel += DRIVE_SPEED; + rightVel += DRIVE_SPEED; + } + if (k.has("KeyS") || k.has("ArrowDown")) { + leftVel -= DRIVE_SPEED; + rightVel -= DRIVE_SPEED; + } + if (k.has("KeyA") || k.has("ArrowLeft")) { + leftVel -= TURN_SPEED; + rightVel += TURN_SPEED; + } + if (k.has("KeyD") || k.has("ArrowRight")) { + leftVel += TURN_SPEED; + rightVel -= TURN_SPEED; + } + + setControl("motor_wheel_fl", leftVel); + setControl("motor_wheel_rl", leftVel); + setControl("motor_wheel_fr", rightVel); + setControl("motor_wheel_rr", rightVel); + }, [setControl]); + + useEffect(() => { + const handleKeyDown = (e: KeyboardEvent) => { + if (["KeyW", "KeyA", "KeyS", "KeyD", "ArrowUp", "ArrowDown", "ArrowLeft", "ArrowRight"].includes(e.code)) { + e.preventDefault(); + keysRef.current.add(e.code); + } + }; + const handleKeyUp = (e: KeyboardEvent) => { + keysRef.current.delete(e.code); + }; + + window.addEventListener("keydown", handleKeyDown); + window.addEventListener("keyup", handleKeyUp); + return () => { + window.removeEventListener("keydown", handleKeyDown); + window.removeEventListener("keyup", handleKeyUp); + }; + }, []); + + // Apply drive controls every frame before mj_step + const stepWithDrive = useCallback(() => { + applyDrive(); + step(); + }, [applyDrive, step]); return (
@@ -42,7 +101,7 @@ export default function MujocoPage() { model={model} data={data} isLoaded={isLoaded} - onStep={step} + onStep={stepWithDrive} />
@@ -53,6 +112,9 @@ export default function MujocoPage() { reset={reset} data={data} /> +

+ WASD / Arrow keys to drive +

diff --git a/ui/src/pages/MujocoPage/useMujoco.ts b/ui/src/pages/MujocoPage/useMujoco.ts index b83ac0f..a9b9eee 100644 --- a/ui/src/pages/MujocoPage/useMujoco.ts +++ b/ui/src/pages/MujocoPage/useMujoco.ts @@ -47,13 +47,29 @@ export function useMujoco() { }, []); const setControl = useCallback((name: string, value: number) => { - const m = mujocoRef.current; const model = modelRef.current; const data = dataRef.current; - if (!m || !model || !data) return; - const id = m.mj_name2id(model, m.mjtObj.mjOBJ_ACTUATOR, name); - if (id >= 0) { - data.ctrl[id] = value; + if (!model || !data) return; + const names = model.names; + let buf: Uint8Array; + if (typeof names === "string") { + buf = new TextEncoder().encode(names); + } else if (names instanceof ArrayBuffer) { + buf = new Uint8Array(names); + } else if (ArrayBuffer.isView(names)) { + buf = new Uint8Array(names.buffer, names.byteOffset, names.byteLength); + } else { + return; + } + const decoder = new TextDecoder(); + for (let i = 0; i < model.nu; i++) { + const addr = model.name_actuatoradr[i]; + let end = addr; + while (end < buf.length && buf[end] !== 0) end++; + if (decoder.decode(buf.slice(addr, end)) === name) { + data.ctrl[i] = value; + return; + } } }, []); From c470799b0cffe6ff748af8665acaff40649e6f4f Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Mon, 25 May 2026 19:38:32 +0800 Subject: [PATCH 05/39] fix: correct first-person camera orientation and add XYZ labels to axes gizmo MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The mjToThree quaternion maps MuJoCo body orientation correctly for geoms but not for cameras — it mapped the camera's look direction to the wrong axis. Compute look/up vectors explicitly and use camera.lookAt() instead. Also add X/Y/Z text sprites at the tips of the orientation axes gizmo. --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 38 ++++++++++++++++++++-- ui/src/pages/MujocoPage/carArmXml.ts | 2 +- 2 files changed, 36 insertions(+), 4 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 4e35406..c5947f3 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -157,6 +157,25 @@ function resolveName(names: unknown, addr: number): string { return new TextDecoder().decode(buf.slice(addr, end)); } +function makeLabelSprite(text: string, color: string, scale: number = 0.4) { + const canvas = document.createElement("canvas"); + canvas.width = 64; + canvas.height = 64; + const ctx = canvas.getContext("2d")!; + ctx.fillStyle = color; + ctx.font = "bold 48px Arial"; + ctx.textAlign = "center"; + ctx.textBaseline = "middle"; + ctx.fillText(text, 32, 32); + + const texture = new THREE.CanvasTexture(canvas); + texture.minFilter = THREE.LinearFilter; + const material = new THREE.SpriteMaterial({ map: texture, depthTest: false }); + const sprite = new THREE.Sprite(material); + sprite.scale.set(scale, scale, 1); + return sprite; +} + function resolveCameraIndices(model: MjModel) { let fpIdx = -1; let tdIdx = -1; @@ -250,6 +269,9 @@ export default function MujocoRenderer({ axesScene = new THREE.Scene(); const axesGroup = new THREE.Group(); axesGroup.add(new THREE.AxesHelper(1.0)); + axesGroup.add(makeLabelSprite("X", "#ff4444").translateX(1.15)); + axesGroup.add(makeLabelSprite("Y", "#44ff44").translateY(1.15)); + axesGroup.add(makeLabelSprite("Z", "#4444ff").translateZ(1.15)); axesScene.add(axesGroup); axesGroupRef.current = axesGroup; const size = 120; @@ -428,9 +450,19 @@ export default function MujocoRenderer({ if (fpCamera && fpIdx >= 0) { const camPos = d.cam_xpos.slice(fpIdx * 3, fpIdx * 3 + 3) as Float64Array; const camMat = d.cam_xmat.slice(fpIdx * 9, fpIdx * 9 + 9) as Float64Array; - const { position, quaternion } = mjToThree(camPos, camMat); - fpCamera.position.copy(position); - fpCamera.quaternion.copy(quaternion); + + const pos3 = new THREE.Vector3(camPos[0], camPos[2], -camPos[1]); + + // MuJoCo camera looks along local -Z, up is local +Y + // cam_xmat columns are the camera's local axes in MuJoCo world coords + const lookMj = new THREE.Vector3(-camMat[2], -camMat[5], -camMat[8]); + const upMj = new THREE.Vector3(camMat[1], camMat[4], camMat[7]); + const look3 = new THREE.Vector3(lookMj.x, lookMj.z, -lookMj.y); + const up3 = new THREE.Vector3(upMj.x, upMj.z, -upMj.y); + + fpCamera.position.copy(pos3); + fpCamera.up.copy(up3); + fpCamera.lookAt(pos3.clone().add(look3)); } } diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 284e82d..ae32297 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -12,7 +12,7 @@ export const CAR_ARM_XML = ` - + From f3986c200a8700b96aa8ecee1c5af758fb9c7fe4 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 30 May 2026 10:43:06 +0800 Subject: [PATCH 06/39] fix: lower sim rate, fix axes labels, and correct camera forward orientation - Reduce SUBSTEPS from 16 to 5 for slower, more stable physics - Fix axes gizmo labels: Z (blue) on vertical, Y (green) on horizontal to match MuJoCo coordinate system (X=forward, Y=left, Z=up) - Correct first-person camera euler to "90 -90 0" so it looks forward (+X) instead of toward ceiling - Add debug logging for car ctrl, wheel velocities, yaw, and quaternion - Adjust camera visual indicator geom direction --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 23 ++++++++++++++++++---- ui/src/pages/MujocoPage/carArmXml.ts | 10 +++++----- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index c5947f3..858c524 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -7,7 +7,7 @@ const MJ_GEOM_SPHERE = 2; const MJ_GEOM_CYLINDER = 5; const MJ_GEOM_BOX = 6; -const SUBSTEPS = 16; +const SUBSTEPS = 5; const T = new THREE.Matrix4().set( 1, 0, 0, 0, @@ -92,7 +92,8 @@ function createGeomMesh( cylMesh.receiveShadow = true; wheelGroup.add(cylMesh); - // Cross-spokes to make rotation visible (cylinder rotating around own axis is invisible) + // Cross-spokes in XZ plane (perpendicular to cylinder axis Y) + // After mjToThree, the cylinder axis (MuJoCo Y) maps to Three.js -Z; spokes rotate around Z const spokeHalfLen = radius * 0.9; const spokeThick = halfHeight * 0.3; const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); @@ -270,8 +271,8 @@ export default function MujocoRenderer({ const axesGroup = new THREE.Group(); axesGroup.add(new THREE.AxesHelper(1.0)); axesGroup.add(makeLabelSprite("X", "#ff4444").translateX(1.15)); - axesGroup.add(makeLabelSprite("Y", "#44ff44").translateY(1.15)); - axesGroup.add(makeLabelSprite("Z", "#4444ff").translateZ(1.15)); + axesGroup.add(makeLabelSprite("Z", "#4444ff").translateY(1.15)); + axesGroup.add(makeLabelSprite("Y", "#44ff44").translateZ(1.15)); axesScene.add(axesGroup); axesGroupRef.current = axesGroup; const size = 120; @@ -430,8 +431,22 @@ export default function MujocoRenderer({ } // Track car body position for orbit camera target + // Body IDs: 0=world, 1=car (freejoint), 2=camera_body, 3=wheel_fl, 4=wheel_fr, 5=wheel_rl, 6=wheel_rr const carBodyId = 1; const carXpos = d.xpos.slice(carBodyId * 3, carBodyId * 3 + 3) as Float64Array; + // Log car state every ~15 frames + // qvel: 0-2=freejoint linear vel, 3-5=freejoint angular vel, 6=wheel_fl, 7=wheel_fr, 8=wheel_rl, 9=wheel_rr + if (Math.random() < 0.07) { + const ctrlAll = [d.ctrl[0]?.toFixed(2), d.ctrl[1]?.toFixed(2), d.ctrl[2]?.toFixed(2), d.ctrl[3]?.toFixed(2)]; + const wheelVels = [d.qvel[6]?.toFixed(2), d.qvel[7]?.toFixed(2), d.qvel[8]?.toFixed(2), d.qvel[9]?.toFixed(2)]; + const yawVel = d.qvel[5]?.toFixed(3); + const carXquat = d.xquat.slice(carBodyId * 4, carBodyId * 4 + 4); + console.log("[MJC] car pos:", carXpos[0].toFixed(4), carXpos[1].toFixed(4), carXpos[2].toFixed(4), + "| ctrl:", ctrlAll.join(","), + "| wheelVel:", wheelVels.join(","), + "| yawVel:", yawVel, + "| quat:", carXquat[0]?.toFixed(3), carXquat[1]?.toFixed(3), carXquat[2]?.toFixed(3), carXquat[3]?.toFixed(3)); + } orbitStateRef.current.target.set(carXpos[0], carXpos[2] + 0.3, -carXpos[1]); updateOrbitCamera(); diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index ae32297..c68f337 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -12,10 +12,10 @@ export const CAR_ARM_XML = ` - - - - + + + + @@ -45,4 +45,4 @@ export const CAR_ARM_XML = ` -`; +`; \ No newline at end of file From 94eb2f3a6c528811f471649ebb59d429224f3047 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 30 May 2026 13:47:00 +0800 Subject: [PATCH 07/39] fix: group camera and indicator geoms in camera_body, skip spokes on non-wheel cylinders - Wrap camera, box, and cylinder in camera_body so they move as one unit - Only add cross-spokes to wheel-like cylinders (radius >= halfHeight) to avoid artifacts on thin indicator cylinders --- mujoco/car_arm.xml | 85 ---------------------- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 35 ++++----- ui/src/pages/MujocoPage/carArmXml.ts | 21 +++--- 3 files changed, 29 insertions(+), 112 deletions(-) delete mode 100644 mujoco/car_arm.xml diff --git a/mujoco/car_arm.xml b/mujoco/car_arm.xml deleted file mode 100644 index d02c2c0..0000000 --- a/mujoco/car_arm.xml +++ /dev/null @@ -1,85 +0,0 @@ - - \ No newline at end of file diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 858c524..2948754 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -92,23 +92,24 @@ function createGeomMesh( cylMesh.receiveShadow = true; wheelGroup.add(cylMesh); - // Cross-spokes in XZ plane (perpendicular to cylinder axis Y) - // After mjToThree, the cylinder axis (MuJoCo Y) maps to Three.js -Z; spokes rotate around Z - const spokeHalfLen = radius * 0.9; - const spokeThick = halfHeight * 0.3; - const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); - const spokeGeomZ = new THREE.BoxGeometry(spokeThick, spokeThick, spokeHalfLen * 2); - const spokeMat = new THREE.MeshStandardMaterial({ - color: 0x333333, - roughness: 0.5, - metalness: 0.4, - }); - const spokeX = new THREE.Mesh(spokeGeomX, spokeMat); - const spokeZ = new THREE.Mesh(spokeGeomZ, spokeMat); - spokeX.castShadow = true; - spokeZ.castShadow = true; - wheelGroup.add(spokeX); - wheelGroup.add(spokeZ); + // Cross-spokes only for wheel-like cylinders (radius > halfHeight) + if (radius >= halfHeight) { + const spokeHalfLen = radius * 0.9; + const spokeThick = halfHeight * 0.3; + const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); + const spokeGeomZ = new THREE.BoxGeometry(spokeThick, spokeThick, spokeHalfLen * 2); + const spokeMat = new THREE.MeshStandardMaterial({ + color: 0x333333, + roughness: 0.5, + metalness: 0.4, + }); + const spokeX = new THREE.Mesh(spokeGeomX, spokeMat); + const spokeZ = new THREE.Mesh(spokeGeomZ, spokeMat); + spokeX.castShadow = true; + spokeZ.castShadow = true; + wheelGroup.add(spokeX); + wheelGroup.add(spokeZ); + } mesh = wheelGroup; break; diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index c68f337..825b52f 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -12,37 +12,38 @@ export const CAR_ARM_XML = ` + - + - + - + - + - + - + - - - - + + + + `; \ No newline at end of file From 380932835f7b17000dc62124a99438872da31d80 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 30 May 2026 15:34:29 +0800 Subject: [PATCH 08/39] feat: add joint axis overlay with toggle checkbox in MuJoCo viewer - Render hinge joint axes as red cylinders in 3D scene using d.jnt() accessor - Add "Show Joints" checkbox in ControlPanel to toggle overlay visibility - Fix wheel hinge axis to Y (0 1 0) for correct forward motion - Clean up legacy debug logging from animation loop --- ui/src/pages/MujocoPage/ControlPanel.tsx | 14 ++- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 122 ++++++++++++++++++--- ui/src/pages/MujocoPage/carArmXml.ts | 16 +-- ui/src/pages/MujocoPage/index.tsx | 7 +- 4 files changed, 135 insertions(+), 24 deletions(-) diff --git a/ui/src/pages/MujocoPage/ControlPanel.tsx b/ui/src/pages/MujocoPage/ControlPanel.tsx index 9e87873..cfd7fa4 100644 --- a/ui/src/pages/MujocoPage/ControlPanel.tsx +++ b/ui/src/pages/MujocoPage/ControlPanel.tsx @@ -6,9 +6,11 @@ interface Props { setControl: (name: string, value: number) => void; reset: () => void; data: React.RefObject; + showJointOverlay: boolean; + setShowJointOverlay: (v: boolean) => void; } -export function ControlPanel({ isLoaded, setControl, reset }: Props) { +export function ControlPanel({ isLoaded, setControl, reset, showJointOverlay, setShowJointOverlay }: Props) { const [yaw, setYaw] = useState(0); const [pitch, setPitch] = useState(0); const [roll, setRoll] = useState(0); @@ -123,6 +125,16 @@ export function ControlPanel({ isLoaded, setControl, reset }: Props) { > Reset + + ); } diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 2948754..795c638 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -190,12 +190,21 @@ function resolveCameraIndices(model: MjModel) { return { fpIdx, tdIdx }; } +interface JointOverlay { + cylinder: THREE.Mesh; + sprite: THREE.Sprite; + jointIdx: number; + name: string; + qposAdr: number; +} + interface Props { mujoco: React.RefObject; model: React.RefObject; data: React.RefObject; isLoaded: boolean; onStep: () => void; + showJointOverlay: boolean; } export default function MujocoRenderer({ @@ -204,6 +213,7 @@ export default function MujocoRenderer({ data, isLoaded, onStep, + showJointOverlay, }: Props) { const containerRef = useRef(null); const fpContainerRef = useRef(null); @@ -228,6 +238,11 @@ export default function MujocoRenderer({ target: new THREE.Vector3(0, 0.3, 0), }); const fpCamIdxRef = useRef(-1); + const jointGroupRef = useRef(null); + const jointOverlaysRef = useRef([]); + const jointFrameCountRef = useRef(0); + const showJointOverlayRef = useRef(showJointOverlay); + showJointOverlayRef.current = showJointOverlay; const setupScene = useCallback(() => { const container = containerRef.current; @@ -407,6 +422,48 @@ export default function MujocoRenderer({ fpCam.updateProjectionMatrix(); } + // Joint overlay group + const jointGroup = new THREE.Group(); + jointGroup.visible = showJointOverlay; + scene.add(jointGroup); + jointGroupRef.current = jointGroup; + const overlays: JointOverlay[] = []; + + for (let i = 0; i < m.njnt; i++) { + const jtype = m.jnt_type[i]; + if (jtype !== 3) continue; // only hinge joints for now + + const name = resolveName(m.names, m.name_jntadr[i]); + + const qposAdr = m.jnt_qposadr[i]; + + // axis cylinder - use MeshBasicMaterial for guaranteed visibility + const cylGeo = new THREE.CylinderGeometry(0.04, 0.04, 0.5, 8); + const cylMat = new THREE.MeshBasicMaterial({ + color: 0xff4444, + depthTest: false, + depthWrite: false, + }); + const cylinder = new THREE.Mesh(cylGeo, cylMat); + cylinder.renderOrder = 999; + + // label sprite + const canvas = document.createElement("canvas"); + canvas.width = 256; + canvas.height = 64; + const ctx = canvas.getContext("2d")!; + const texture = new THREE.CanvasTexture(canvas); + texture.minFilter = THREE.LinearFilter; + const spriteMat = new THREE.SpriteMaterial({ map: texture, depthTest: false }); + const sprite = new THREE.Sprite(spriteMat); + sprite.scale.set(0.6, 0.15, 1); + + jointGroup.add(cylinder); + jointGroup.add(sprite); + overlays.push({ cylinder, sprite, jointIdx: i, name, qposAdr }); + } + jointOverlaysRef.current = overlays; + updateOrbitCamera(); }, [isLoaded, model, updateOrbitCamera]); @@ -435,19 +492,6 @@ export default function MujocoRenderer({ // Body IDs: 0=world, 1=car (freejoint), 2=camera_body, 3=wheel_fl, 4=wheel_fr, 5=wheel_rl, 6=wheel_rr const carBodyId = 1; const carXpos = d.xpos.slice(carBodyId * 3, carBodyId * 3 + 3) as Float64Array; - // Log car state every ~15 frames - // qvel: 0-2=freejoint linear vel, 3-5=freejoint angular vel, 6=wheel_fl, 7=wheel_fr, 8=wheel_rl, 9=wheel_rr - if (Math.random() < 0.07) { - const ctrlAll = [d.ctrl[0]?.toFixed(2), d.ctrl[1]?.toFixed(2), d.ctrl[2]?.toFixed(2), d.ctrl[3]?.toFixed(2)]; - const wheelVels = [d.qvel[6]?.toFixed(2), d.qvel[7]?.toFixed(2), d.qvel[8]?.toFixed(2), d.qvel[9]?.toFixed(2)]; - const yawVel = d.qvel[5]?.toFixed(3); - const carXquat = d.xquat.slice(carBodyId * 4, carBodyId * 4 + 4); - console.log("[MJC] car pos:", carXpos[0].toFixed(4), carXpos[1].toFixed(4), carXpos[2].toFixed(4), - "| ctrl:", ctrlAll.join(","), - "| wheelVel:", wheelVels.join(","), - "| yawVel:", yawVel, - "| quat:", carXquat[0]?.toFixed(3), carXquat[1]?.toFixed(3), carXquat[2]?.toFixed(3), carXquat[3]?.toFixed(3)); - } orbitStateRef.current.target.set(carXpos[0], carXpos[2] + 0.3, -carXpos[1]); updateOrbitCamera(); @@ -480,6 +524,58 @@ export default function MujocoRenderer({ fpCamera.up.copy(up3); fpCamera.lookAt(pos3.clone().add(look3)); } + + // Joint overlay update + const jointGroup = jointGroupRef.current; + const overlays = jointOverlaysRef.current; + if (jointGroup) { + const show = showJointOverlayRef.current; + jointGroup.visible = show; + if (show && overlays.length > 0) { + jointFrameCountRef.current++; + const updateLabels = jointFrameCountRef.current % 30 === 0; + for (const ov of overlays) { + const ji = ov.jointIdx; + const jnt = d.jnt(ji); + + const anchorRaw = jnt.xanchor; + const axisRaw = jnt.xaxis; + const anchor = (anchorRaw instanceof Float64Array || ArrayBuffer.isView(anchorRaw)) + ? new Float64Array(anchorRaw.buffer, anchorRaw.byteOffset, 3) + : (Array.isArray(anchorRaw) ? anchorRaw : [0, 0, 0]) as unknown as Float64Array; + const axis = (axisRaw instanceof Float64Array || ArrayBuffer.isView(axisRaw)) + ? new Float64Array(axisRaw.buffer, axisRaw.byteOffset, 3) + : (Array.isArray(axisRaw) ? axisRaw : [0, 1, 0]) as unknown as Float64Array; + + const posT = new THREE.Vector3(anchor[0], anchor[2], -anchor[1]); + const axT = new THREE.Vector3(axis[0], axis[2], -axis[1]).normalize(); + + ov.cylinder.position.copy(posT); + ov.cylinder.quaternion.setFromUnitVectors(new THREE.Vector3(0, 1, 0), axT); + + ov.sprite.position.copy(posT).add(new THREE.Vector3(0, 0.25, 0)); + + if (updateLabels) { + const qposVal = d.qpos[ov.qposAdr]?.toFixed(3) ?? "?"; + const canvas = (ov.sprite.material as THREE.SpriteMaterial).map?.image as HTMLCanvasElement; + if (canvas) { + const ctx = canvas.getContext("2d"); + if (ctx) { + ctx.clearRect(0, 0, canvas.width, canvas.height); + ctx.fillStyle = "rgba(0,0,0,0.75)"; + ctx.fillRect(0, 0, canvas.width, canvas.height); + ctx.fillStyle = "#ffffff"; + ctx.font = "20px monospace"; + ctx.textAlign = "center"; + ctx.textBaseline = "middle"; + ctx.fillText(`${ov.name}: ${qposVal}`, 128, 32); + (ov.sprite.material as THREE.SpriteMaterial).map!.needsUpdate = true; + } + } + } + } + } + } } if (mainRenderer && orbitCamera && scene) { diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 825b52f..8fdc15d 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -13,28 +13,28 @@ export const CAR_ARM_XML = ` - - - - + + + + - + - + - + - + diff --git a/ui/src/pages/MujocoPage/index.tsx b/ui/src/pages/MujocoPage/index.tsx index 5efc41b..2c8aceb 100644 --- a/ui/src/pages/MujocoPage/index.tsx +++ b/ui/src/pages/MujocoPage/index.tsx @@ -1,4 +1,4 @@ -import { useEffect, useRef, useCallback } from "react"; +import { useState, useEffect, useRef, useCallback } from "react"; import { useMujoco } from "./useMujoco"; import MujocoRenderer from "./MujocoRenderer"; import { ControlPanel } from "./ControlPanel"; @@ -10,6 +10,7 @@ export default function MujocoPage() { const { isLoaded, mujoco, model, data, step, setControl, reset } = useMujoco(); const keysRef = useRef>(new Set()); + const [showJointOverlay, setShowJointOverlay] = useState(true); const applyDrive = useCallback(() => { let leftVel = 0; @@ -102,6 +103,7 @@ export default function MujocoPage() { data={data} isLoaded={isLoaded} onStep={stepWithDrive} + showJointOverlay={showJointOverlay} /> @@ -110,7 +112,8 @@ export default function MujocoPage() { isLoaded={isLoaded} setControl={setControl} reset={reset} - data={data} + showJointOverlay={showJointOverlay} + setShowJointOverlay={setShowJointOverlay} />

WASD / Arrow keys to drive From 3317322a8dc8f575bd219acbc9a948e5457fd410 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sat, 30 May 2026 16:58:34 +0800 Subject: [PATCH 09/39] fix: apply Y_TO_Z pre-rotation for cylinder geoms to align Three.js Y axis with MuJoCo Z axis MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Three.js CylinderGeometry extends along Y by default, but MuJoCo cylinder geom local Z is the axis direction. Compose a +90° X-axis pre-rotation quaternion in the animation loop for all cylinder-type geoms so their visual axis matches the physics orientation. --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 34 +++++++++++++++++----- ui/src/pages/MujocoPage/carArmXml.ts | 8 ++--- ui/src/pages/MujocoPage/index.tsx | 2 +- 3 files changed, 31 insertions(+), 13 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 795c638..d06e4e8 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -9,6 +9,12 @@ const MJ_GEOM_BOX = 6; const SUBSTEPS = 5; +// Three.js CylinderGeometry extends along Y, but MuJoCo geom local Z is the cylinder axis. +// Pre-rotation: Y → Z so that mjToThree quat maps Z → world cylinder axis direction. +const Y_TO_Z = new THREE.Quaternion().setFromAxisAngle( + new THREE.Vector3(1, 0, 0), Math.PI / 2, +); + const T = new THREE.Matrix4().set( 1, 0, 0, 0, 0, 0, 1, 0, @@ -77,9 +83,10 @@ function createGeomMesh( case MJ_GEOM_CYLINDER: { const radius = size[0]; const halfHeight = size[1]; - const wheelGroup = new THREE.Group(); + // Keep geometry default (extends along Y). Orientation fixed per-frame via setFromUnitVectors. const cylGeom = new THREE.CylinderGeometry(radius, radius, halfHeight * 2, 32); + const cylMat = new THREE.MeshStandardMaterial({ color, roughness: 0.5, @@ -90,12 +97,15 @@ function createGeomMesh( const cylMesh = new THREE.Mesh(cylGeom, cylMat); cylMesh.castShadow = true; cylMesh.receiveShadow = true; - wheelGroup.add(cylMesh); + + const group = new THREE.Group(); + group.add(cylMesh); // Cross-spokes only for wheel-like cylinders (radius > halfHeight) if (radius >= halfHeight) { const spokeHalfLen = radius * 0.9; const spokeThick = halfHeight * 0.3; + // Spokes in XZ plane (perpendicular to cylinder default Y axis in group-local space) const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); const spokeGeomZ = new THREE.BoxGeometry(spokeThick, spokeThick, spokeHalfLen * 2); const spokeMat = new THREE.MeshStandardMaterial({ @@ -107,11 +117,11 @@ function createGeomMesh( const spokeZ = new THREE.Mesh(spokeGeomZ, spokeMat); spokeX.castShadow = true; spokeZ.castShadow = true; - wheelGroup.add(spokeX); - wheelGroup.add(spokeZ); + group.add(spokeX); + group.add(spokeZ); } - mesh = wheelGroup; + mesh = group; break; } case MJ_GEOM_BOX: { @@ -228,6 +238,7 @@ export default function MujocoRenderer({ const fpRendererRef = useRef(null); const axesRendererRef = useRef(null); const meshesRef = useRef([]); + const geomTypesRef = useRef([]); const animRef = useRef(0); const draggingRef = useRef(false); const lastMouseRef = useRef({ x: 0, y: 0 }); @@ -397,6 +408,7 @@ export default function MujocoRenderer({ meshesRef.current.forEach((mesh) => scene.remove(mesh)); meshesRef.current = []; + geomTypesRef.current = []; for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; @@ -406,6 +418,7 @@ export default function MujocoRenderer({ const mesh = createGeomMesh(type, size, rgba); scene.add(mesh); meshesRef.current.push(mesh); + geomTypesRef.current.push(type); } const { fpIdx, tdIdx } = resolveCameraIndices(m); @@ -495,14 +508,19 @@ export default function MujocoRenderer({ orbitStateRef.current.target.set(carXpos[0], carXpos[2] + 0.3, -carXpos[1]); updateOrbitCamera(); - for (let i = 0; i < meshesRef.current.length; i++) { + for (let i = 0; i < meshesRef.current.length; i++) { const g = d.geom(i); const pos = g.xpos as Float64Array; const mat = g.xmat as Float64Array; const { position, quaternion } = mjToThree(pos, mat); - meshesRef.current[i].position.copy(position); - meshesRef.current[i].quaternion.copy(quaternion); + const mesh = meshesRef.current[i]; + mesh.position.copy(position); + if (geomTypesRef.current[i] === MJ_GEOM_CYLINDER) { + mesh.quaternion.copy(quaternion).multiply(Y_TO_Z); + } else { + mesh.quaternion.copy(quaternion); + } } // First-person camera: use MuJoCo native camera world pose diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 8fdc15d..edb083b 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -23,19 +23,19 @@ export const CAR_ARM_XML = ` - + - + - + - + diff --git a/ui/src/pages/MujocoPage/index.tsx b/ui/src/pages/MujocoPage/index.tsx index 2c8aceb..c7264a1 100644 --- a/ui/src/pages/MujocoPage/index.tsx +++ b/ui/src/pages/MujocoPage/index.tsx @@ -10,7 +10,7 @@ export default function MujocoPage() { const { isLoaded, mujoco, model, data, step, setControl, reset } = useMujoco(); const keysRef = useRef>(new Set()); - const [showJointOverlay, setShowJointOverlay] = useState(true); + const [showJointOverlay, setShowJointOverlay] = useState(false); const applyDrive = useCallback(() => { let leftVel = 0; From 123ef8f9d135b30274e54a30e713bea907c7f0f1 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Sun, 31 May 2026 11:25:55 +0800 Subject: [PATCH 10/39] fix: correct mjToThree rotation formula and remove cylinder workarounds MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit T * R * Tinv mapped MuJoCo Rz(+θ) (left turn) to Three.js Ry(+θ) (right turn), causing car body and wheels to rotate in opposite directions. Fix to Tinv * R * T, which correctly maps to Ry(-θ) (left turn). Removes all cylinder-specific quaternion adjustments and geomTypesRef as they are no longer needed. --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 23 +++------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index d06e4e8..2ba6a53 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -9,12 +9,6 @@ const MJ_GEOM_BOX = 6; const SUBSTEPS = 5; -// Three.js CylinderGeometry extends along Y, but MuJoCo geom local Z is the cylinder axis. -// Pre-rotation: Y → Z so that mjToThree quat maps Z → world cylinder axis direction. -const Y_TO_Z = new THREE.Quaternion().setFromAxisAngle( - new THREE.Vector3(1, 0, 0), Math.PI / 2, -); - const T = new THREE.Matrix4().set( 1, 0, 0, 0, 0, 0, 1, 0, @@ -38,7 +32,7 @@ function mjToThree(pos: Float64Array, mat: Float64Array) { 0, 0, 0, 1, ); - const Rthree = T.clone().multiply(Rmj).multiply(Tinv); + const Rthree = Tinv.clone().multiply(Rmj).multiply(T); const quaternion = new THREE.Quaternion().setFromRotationMatrix(Rthree); return { position, quaternion }; @@ -84,7 +78,6 @@ function createGeomMesh( const radius = size[0]; const halfHeight = size[1]; - // Keep geometry default (extends along Y). Orientation fixed per-frame via setFromUnitVectors. const cylGeom = new THREE.CylinderGeometry(radius, radius, halfHeight * 2, 32); const cylMat = new THREE.MeshStandardMaterial({ @@ -105,7 +98,6 @@ function createGeomMesh( if (radius >= halfHeight) { const spokeHalfLen = radius * 0.9; const spokeThick = halfHeight * 0.3; - // Spokes in XZ plane (perpendicular to cylinder default Y axis in group-local space) const spokeGeomX = new THREE.BoxGeometry(spokeHalfLen * 2, spokeThick, spokeThick); const spokeGeomZ = new THREE.BoxGeometry(spokeThick, spokeThick, spokeHalfLen * 2); const spokeMat = new THREE.MeshStandardMaterial({ @@ -238,7 +230,6 @@ export default function MujocoRenderer({ const fpRendererRef = useRef(null); const axesRendererRef = useRef(null); const meshesRef = useRef([]); - const geomTypesRef = useRef([]); const animRef = useRef(0); const draggingRef = useRef(false); const lastMouseRef = useRef({ x: 0, y: 0 }); @@ -408,7 +399,6 @@ export default function MujocoRenderer({ meshesRef.current.forEach((mesh) => scene.remove(mesh)); meshesRef.current = []; - geomTypesRef.current = []; for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; @@ -418,7 +408,6 @@ export default function MujocoRenderer({ const mesh = createGeomMesh(type, size, rgba); scene.add(mesh); meshesRef.current.push(mesh); - geomTypesRef.current.push(type); } const { fpIdx, tdIdx } = resolveCameraIndices(m); @@ -512,15 +501,9 @@ export default function MujocoRenderer({ const g = d.geom(i); const pos = g.xpos as Float64Array; const mat = g.xmat as Float64Array; - const { position, quaternion } = mjToThree(pos, mat); - const mesh = meshesRef.current[i]; - mesh.position.copy(position); - if (geomTypesRef.current[i] === MJ_GEOM_CYLINDER) { - mesh.quaternion.copy(quaternion).multiply(Y_TO_Z); - } else { - mesh.quaternion.copy(quaternion); - } + meshesRef.current[i].position.copy(position); + meshesRef.current[i].quaternion.copy(quaternion); } // First-person camera: use MuJoCo native camera world pose From b55dba68df8d619d16b4692e65de39a7a4eddc44 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Mon, 1 Jun 2026 23:52:14 +0800 Subject: [PATCH 11/39] fix: correct Three.js rendering to match MuJoCo native viewer - Replace mjToThree formula with direct axis extraction from geom_xmat and makeBasis(gx, -gz, gy) for proper rotation (det=+1) - Fix cam_xmat column indexing: look direction = column 2 (Z axis), up vector = column 1 (Y axis), not row-major indices - Detect fromto cylinders by comparing position with same-body geoms, compute correct cylinder axis direction from midpoint offset - Track geom types via geomTypesRef for cylinder-specific handling --- mujoco/Chenlong_Robot/car.xml | 49 ++++++++++++++++ mujoco/Chenlong_Robot/test.py | 12 ++++ ui/src/pages/MujocoPage/MujocoRenderer.tsx | 68 +++++++++++++++++++--- 3 files changed, 120 insertions(+), 9 deletions(-) create mode 100644 mujoco/Chenlong_Robot/car.xml create mode 100644 mujoco/Chenlong_Robot/test.py diff --git a/mujoco/Chenlong_Robot/car.xml b/mujoco/Chenlong_Robot/car.xml new file mode 100644 index 0000000..6475482 --- /dev/null +++ b/mujoco/Chenlong_Robot/car.xml @@ -0,0 +1,49 @@ + + \ No newline at end of file diff --git a/mujoco/Chenlong_Robot/test.py b/mujoco/Chenlong_Robot/test.py new file mode 100644 index 0000000..f9f6ff8 --- /dev/null +++ b/mujoco/Chenlong_Robot/test.py @@ -0,0 +1,12 @@ +import time + +import mujoco.viewer + +model = mujoco.MjModel.from_xml_path('car.xml') +data = mujoco.MjData(model) + +with mujoco.viewer.launch_passive(model, data) as viewer: + while viewer.is_running(): + mujoco.mj_step(model, data) + viewer.sync() + time.sleep(1/500) # ~60 Hz real-time \ No newline at end of file diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 2ba6a53..1351d32 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -230,6 +230,7 @@ export default function MujocoRenderer({ const fpRendererRef = useRef(null); const axesRendererRef = useRef(null); const meshesRef = useRef([]); + const geomTypesRef = useRef([]); const animRef = useRef(0); const draggingRef = useRef(false); const lastMouseRef = useRef({ x: 0, y: 0 }); @@ -399,6 +400,7 @@ export default function MujocoRenderer({ meshesRef.current.forEach((mesh) => scene.remove(mesh)); meshesRef.current = []; + geomTypesRef.current = []; for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; @@ -408,6 +410,7 @@ export default function MujocoRenderer({ const mesh = createGeomMesh(type, size, rgba); scene.add(mesh); meshesRef.current.push(mesh); + geomTypesRef.current.push(type); } const { fpIdx, tdIdx } = resolveCameraIndices(m); @@ -498,12 +501,58 @@ export default function MujocoRenderer({ updateOrbitCamera(); for (let i = 0; i < meshesRef.current.length; i++) { - const g = d.geom(i); - const pos = g.xpos as Float64Array; - const mat = g.xmat as Float64Array; - const { position, quaternion } = mjToThree(pos, mat); - meshesRef.current[i].position.copy(position); - meshesRef.current[i].quaternion.copy(quaternion); + const pos = d.geom_xpos.slice(i * 3, i * 3 + 3) as Float64Array; + const mat = d.geom_xmat.slice(i * 9, i * 9 + 9) as Float64Array; + const position = new THREE.Vector3(pos[0], pos[2], -pos[1]); + + // Extract body's world axes from xmat (column-major), convert to Three.js coords + const bx = new THREE.Vector3(mat[0], mat[2], -mat[1]); // body X in Three + const by = new THREE.Vector3(mat[3], mat[5], -mat[4]); // body Y in Three + const bz = new THREE.Vector3(mat[6], mat[8], -mat[7]); // body Z in Three + + // Default geom axes = body axes + let gx = bx.clone(); + let gy = by.clone(); + let gz = bz.clone(); + + // For cylinders with fromto on the same body as another geom: + // find a non-cylinder geom on the same body (same xmat) and use its position + // as the body reference to compute fromto direction + if (geomTypesRef.current[i] === MJ_GEOM_CYLINDER) { + for (let j = 0; j < meshesRef.current.length; j++) { + if (j === i || geomTypesRef.current[j] === MJ_GEOM_CYLINDER) continue; + const otherMat = d.geom_xmat.slice(j * 9, j * 9 + 9) as Float64Array; + let sameBody = true; + for (let k = 0; k < 9; k++) { + if (Math.abs(mat[k] - otherMat[k]) > 1e-8) { sameBody = false; break; } + } + if (sameBody) { + const bodyPos = d.geom_xpos.slice(j * 3, j * 3 + 3) as Float64Array; + const bodyPos3 = new THREE.Vector3(bodyPos[0], bodyPos[2], -bodyPos[1]); + const offset = position.clone().sub(bodyPos3); + const offsetLen = offset.length(); + // If offset > 1e-6, this cylinder has fromto (midpoint ≠ body origin) + if (offsetLen > 1e-6) { + gz = offset.normalize(); + const arbitrary = Math.abs(gz.x) < 0.9 + ? new THREE.Vector3(1, 0, 0) + : new THREE.Vector3(0, 1, 0); + gx = new THREE.Vector3().crossVectors(arbitrary, gz).normalize(); + gy = new THREE.Vector3().crossVectors(gz, gx).normalize(); + } + break; + } + } + } + + { + // Build proper rotation (det=+1): mesh Y → geom Z (cylinder axis / box height) + // Negate gz to keep det=+1 while preserving rotation direction. + // mesh Z → gy keeps the box depth face correctly oriented. + const R = new THREE.Matrix4().makeBasis(gx, gz.clone().negate(), gy); + meshesRef.current[i].position.copy(position); + meshesRef.current[i].quaternion.setFromRotationMatrix(R); + } } // First-person camera: use MuJoCo native camera world pose @@ -515,9 +564,10 @@ export default function MujocoRenderer({ const pos3 = new THREE.Vector3(camPos[0], camPos[2], -camPos[1]); // MuJoCo camera looks along local -Z, up is local +Y - // cam_xmat columns are the camera's local axes in MuJoCo world coords - const lookMj = new THREE.Vector3(-camMat[2], -camMat[5], -camMat[8]); - const upMj = new THREE.Vector3(camMat[1], camMat[4], camMat[7]); + // cam_xmat is column-major: cols 0,1,2 = camera local X,Y,Z in MuJoCo world + // look direction = -Z (negative of column 2), up = +Y (column 1) + const lookMj = new THREE.Vector3(-camMat[6], -camMat[7], -camMat[8]); + const upMj = new THREE.Vector3(camMat[3], camMat[4], camMat[5]); const look3 = new THREE.Vector3(lookMj.x, lookMj.z, -lookMj.y); const up3 = new THREE.Vector3(upMj.x, upMj.z, -upMj.y); From 692644471610f1cf004e934199a9c2f548d1ed10 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 08:34:51 +0800 Subject: [PATCH 12/39] refactor: use hierarchical body-geom rendering (muwanx approach) Replace flat geom-xpos/xmat placement with standard body hierarchy: - Each MuJoCo body -> THREE.Group, parented by m.body_parentid - Geom meshes as children of body groups with LOCAL pos/quat from m.geom_pos / m.geom_quat (includes fromto and euler) - Body world transforms updated from d.xpos / d.xquat each frame - Add mjPosToThree / mjQuatToThree coordinate conversion helpers - Remove obsolete mjToThree, makeBasis, fromto detection, geomTypesRef Net: -92/+46 lines, no special-case geom orientation code needed --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 138 +++++++-------------- 1 file changed, 46 insertions(+), 92 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 1351d32..5c30847 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -9,33 +9,14 @@ const MJ_GEOM_BOX = 6; const SUBSTEPS = 5; -const T = new THREE.Matrix4().set( - 1, 0, 0, 0, - 0, 0, 1, 0, - 0, -1, 0, 0, - 0, 0, 0, 1, -); -const Tinv = new THREE.Matrix4().set( - 1, 0, 0, 0, - 0, 0, -1, 0, - 0, 1, 0, 0, - 0, 0, 0, 1, -); - -function mjToThree(pos: Float64Array, mat: Float64Array) { - const position = new THREE.Vector3(pos[0], pos[2], -pos[1]); - - const Rmj = new THREE.Matrix4().set( - mat[0], mat[3], mat[6], 0, - mat[1], mat[4], mat[7], 0, - mat[2], mat[5], mat[8], 0, - 0, 0, 0, 1, - ); - - const Rthree = Tinv.clone().multiply(Rmj).multiply(T); - const quaternion = new THREE.Quaternion().setFromRotationMatrix(Rthree); +// Coordinate conversion: MuJoCo (x,y,z) → Three.js (x, z, -y) +function mjPosToThree(buf: Float64Array, index: number, target: THREE.Vector3): THREE.Vector3 { + return target.set(buf[index * 3 + 0], buf[index * 3 + 2], -buf[index * 3 + 1]); +} - return { position, quaternion }; +// Quaternion conversion: MuJoCo [w,x,y,z] → Three.js (-x, -z, y, -w) +function mjQuatToThree(buf: Float64Array, index: number, target: THREE.Quaternion): THREE.Quaternion { + return target.set(-buf[index * 4 + 1], -buf[index * 4 + 3], buf[index * 4 + 2], -buf[index * 4 + 0]); } function createGeomMesh( @@ -229,8 +210,7 @@ export default function MujocoRenderer({ const mainRendererRef = useRef(null); const fpRendererRef = useRef(null); const axesRendererRef = useRef(null); - const meshesRef = useRef([]); - const geomTypesRef = useRef([]); + const bodyGroupsRef = useRef([]); const animRef = useRef(0); const draggingRef = useRef(false); const lastMouseRef = useRef({ x: 0, y: 0 }); @@ -398,21 +378,43 @@ export default function MujocoRenderer({ const scene = sceneRef.current; if (!scene) return; - meshesRef.current.forEach((mesh) => scene.remove(mesh)); - meshesRef.current = []; - geomTypesRef.current = []; + // Remove old body groups + bodyGroupsRef.current.forEach((g) => scene.remove(g)); + bodyGroupsRef.current = []; + + // Create body groups (hierarchical: child bodies are children of parent body groups) + const bodyGroups: THREE.Group[] = []; + for (let b = 0; b < m.nbody; b++) { + const group = new THREE.Group(); + const nameAddr = m.name_bodyadr?.[b] ?? -1; + group.name = nameAddr >= 0 ? resolveName(m.names, nameAddr) : `body_${b}`; + bodyGroups.push(group); + } + // Build body hierarchy: body 0 (world) is root, others parented by m.body_parentid + for (let b = 1; b < m.nbody; b++) { + const parentId = m.body_parentid?.[b] ?? 0; + bodyGroups[parentId].add(bodyGroups[b]); + } + scene.add(bodyGroups[0]); + + // Create geom meshes and attach to body groups with LOCAL pos/quat for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; const size = m.geom_size.slice(i * 3, i * 3 + 3) as Float64Array; const rgba = m.geom_rgba.slice(i * 4, i * 4 + 4) as Float64Array; + const bodyId = m.geom_bodyid[i]; const mesh = createGeomMesh(type, size, rgba); - scene.add(mesh); - meshesRef.current.push(mesh); - geomTypesRef.current.push(type); + mjPosToThree(m.geom_pos, i, mesh.position); + if (type !== MJ_GEOM_PLANE) { + mjQuatToThree(m.geom_quat, i, mesh.quaternion); + } + bodyGroups[bodyId].add(mesh); } + bodyGroupsRef.current = bodyGroups; + const { fpIdx, tdIdx } = resolveCameraIndices(m); fpCamIdxRef.current = fpIdx; @@ -493,68 +495,20 @@ export default function MujocoRenderer({ m.mj_step(model.current!, data.current!); } + // Update body group transforms from MuJoCo world data + const bodyGroups = bodyGroupsRef.current; + for (let b = 0; b < bodyGroups.length; b++) { + mjPosToThree(d.xpos, b, bodyGroups[b].position); + mjQuatToThree(d.xquat, b, bodyGroups[b].quaternion); + } + // Track car body position for orbit camera target - // Body IDs: 0=world, 1=car (freejoint), 2=camera_body, 3=wheel_fl, 4=wheel_fr, 5=wheel_rl, 6=wheel_rr const carBodyId = 1; - const carXpos = d.xpos.slice(carBodyId * 3, carBodyId * 3 + 3) as Float64Array; - orbitStateRef.current.target.set(carXpos[0], carXpos[2] + 0.3, -carXpos[1]); + const carPos3 = new THREE.Vector3(); + mjPosToThree(d.xpos, carBodyId, carPos3); + orbitStateRef.current.target.set(carPos3.x, carPos3.y + 0.3, carPos3.z); updateOrbitCamera(); - for (let i = 0; i < meshesRef.current.length; i++) { - const pos = d.geom_xpos.slice(i * 3, i * 3 + 3) as Float64Array; - const mat = d.geom_xmat.slice(i * 9, i * 9 + 9) as Float64Array; - const position = new THREE.Vector3(pos[0], pos[2], -pos[1]); - - // Extract body's world axes from xmat (column-major), convert to Three.js coords - const bx = new THREE.Vector3(mat[0], mat[2], -mat[1]); // body X in Three - const by = new THREE.Vector3(mat[3], mat[5], -mat[4]); // body Y in Three - const bz = new THREE.Vector3(mat[6], mat[8], -mat[7]); // body Z in Three - - // Default geom axes = body axes - let gx = bx.clone(); - let gy = by.clone(); - let gz = bz.clone(); - - // For cylinders with fromto on the same body as another geom: - // find a non-cylinder geom on the same body (same xmat) and use its position - // as the body reference to compute fromto direction - if (geomTypesRef.current[i] === MJ_GEOM_CYLINDER) { - for (let j = 0; j < meshesRef.current.length; j++) { - if (j === i || geomTypesRef.current[j] === MJ_GEOM_CYLINDER) continue; - const otherMat = d.geom_xmat.slice(j * 9, j * 9 + 9) as Float64Array; - let sameBody = true; - for (let k = 0; k < 9; k++) { - if (Math.abs(mat[k] - otherMat[k]) > 1e-8) { sameBody = false; break; } - } - if (sameBody) { - const bodyPos = d.geom_xpos.slice(j * 3, j * 3 + 3) as Float64Array; - const bodyPos3 = new THREE.Vector3(bodyPos[0], bodyPos[2], -bodyPos[1]); - const offset = position.clone().sub(bodyPos3); - const offsetLen = offset.length(); - // If offset > 1e-6, this cylinder has fromto (midpoint ≠ body origin) - if (offsetLen > 1e-6) { - gz = offset.normalize(); - const arbitrary = Math.abs(gz.x) < 0.9 - ? new THREE.Vector3(1, 0, 0) - : new THREE.Vector3(0, 1, 0); - gx = new THREE.Vector3().crossVectors(arbitrary, gz).normalize(); - gy = new THREE.Vector3().crossVectors(gz, gx).normalize(); - } - break; - } - } - } - - { - // Build proper rotation (det=+1): mesh Y → geom Z (cylinder axis / box height) - // Negate gz to keep det=+1 while preserving rotation direction. - // mesh Z → gy keeps the box depth face correctly oriented. - const R = new THREE.Matrix4().makeBasis(gx, gz.clone().negate(), gy); - meshesRef.current[i].position.copy(position); - meshesRef.current[i].quaternion.setFromRotationMatrix(R); - } - } - // First-person camera: use MuJoCo native camera world pose const fpIdx = fpCamIdxRef.current; if (fpCamera && fpIdx >= 0) { From 2dba97cd8e5a91eed8d015383801ee7a7cba15d7 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 11:01:57 +0800 Subject: [PATCH 13/39] fix: flatten body hierarchy - all bodies child of world d.xpos is world position, so all body groups must be direct children of world (body 0) to avoid double-applying parent transforms. Matches muwanx approach: bodyGroups[b].add(mesh) for geoms, but all body groups added to world group. --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index 5c30847..cc69ac9 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -382,22 +382,21 @@ export default function MujocoRenderer({ bodyGroupsRef.current.forEach((g) => scene.remove(g)); bodyGroupsRef.current = []; - // Create body groups (hierarchical: child bodies are children of parent body groups) + // Create body groups, all as direct children of world (body 0) + // We use world positions from d.xpos, so no parent-child body hierarchy needed const bodyGroups: THREE.Group[] = []; for (let b = 0; b < m.nbody; b++) { const group = new THREE.Group(); const nameAddr = m.name_bodyadr?.[b] ?? -1; group.name = nameAddr >= 0 ? resolveName(m.names, nameAddr) : `body_${b}`; bodyGroups.push(group); + if (b === 0) { + scene.add(group); + } else { + bodyGroups[0].add(group); + } } - // Build body hierarchy: body 0 (world) is root, others parented by m.body_parentid - for (let b = 1; b < m.nbody; b++) { - const parentId = m.body_parentid?.[b] ?? 0; - bodyGroups[parentId].add(bodyGroups[b]); - } - scene.add(bodyGroups[0]); - // Create geom meshes and attach to body groups with LOCAL pos/quat for (let i = 0; i < m.ngeom; i++) { const type = m.geom_type[i]; From 306bb4ec89077e88756024fc9173b6b3dd9c0f32 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 15:47:43 +0800 Subject: [PATCH 14/39] fix: first-person camera uses body group transform directly --- ui/src/pages/MujocoPage/MujocoRenderer.tsx | 25 ++++++---------------- ui/src/pages/MujocoPage/carArmXml.ts | 6 +++--- 2 files changed, 10 insertions(+), 21 deletions(-) diff --git a/ui/src/pages/MujocoPage/MujocoRenderer.tsx b/ui/src/pages/MujocoPage/MujocoRenderer.tsx index cc69ac9..9017540 100644 --- a/ui/src/pages/MujocoPage/MujocoRenderer.tsx +++ b/ui/src/pages/MujocoPage/MujocoRenderer.tsx @@ -221,6 +221,7 @@ export default function MujocoRenderer({ target: new THREE.Vector3(0, 0.3, 0), }); const fpCamIdxRef = useRef(-1); + const fpCamBodyIdRef = useRef(-1); const jointGroupRef = useRef(null); const jointOverlaysRef = useRef([]); const jointFrameCountRef = useRef(0); @@ -416,6 +417,7 @@ export default function MujocoRenderer({ const { fpIdx, tdIdx } = resolveCameraIndices(m); fpCamIdxRef.current = fpIdx; + fpCamBodyIdRef.current = fpIdx >= 0 ? (m.cam_bodyid?.[fpIdx] ?? -1) : -1; const orbitCam = orbitCameraRef.current; const fpCam = fpCameraRef.current; @@ -508,25 +510,12 @@ export default function MujocoRenderer({ orbitStateRef.current.target.set(carPos3.x, carPos3.y + 0.3, carPos3.z); updateOrbitCamera(); - // First-person camera: use MuJoCo native camera world pose + // First-person camera: reuse body group transform (same proven quaternion path) const fpIdx = fpCamIdxRef.current; - if (fpCamera && fpIdx >= 0) { - const camPos = d.cam_xpos.slice(fpIdx * 3, fpIdx * 3 + 3) as Float64Array; - const camMat = d.cam_xmat.slice(fpIdx * 9, fpIdx * 9 + 9) as Float64Array; - - const pos3 = new THREE.Vector3(camPos[0], camPos[2], -camPos[1]); - - // MuJoCo camera looks along local -Z, up is local +Y - // cam_xmat is column-major: cols 0,1,2 = camera local X,Y,Z in MuJoCo world - // look direction = -Z (negative of column 2), up = +Y (column 1) - const lookMj = new THREE.Vector3(-camMat[6], -camMat[7], -camMat[8]); - const upMj = new THREE.Vector3(camMat[3], camMat[4], camMat[5]); - const look3 = new THREE.Vector3(lookMj.x, lookMj.z, -lookMj.y); - const up3 = new THREE.Vector3(upMj.x, upMj.z, -upMj.y); - - fpCamera.position.copy(pos3); - fpCamera.up.copy(up3); - fpCamera.lookAt(pos3.clone().add(look3)); + const camBodyId = fpCamBodyIdRef.current; + if (fpCamera && fpIdx >= 0 && camBodyId >= 0 && bodyGroups[camBodyId]) { + fpCamera.position.copy(bodyGroups[camBodyId].position); + fpCamera.quaternion.copy(bodyGroups[camBodyId].quaternion); } // Joint overlay update diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index edb083b..c270940 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -13,10 +13,10 @@ export const CAR_ARM_XML = ` - + - - + + From b6a6f4246bde9bd5813d32c74600a6ea0638a62c Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 16:47:26 +0800 Subject: [PATCH 15/39] tune: car physics - increase friction, reduce motor torque, add damping MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - Ground + wheel friction: 1.5/0.005/0.0001 (more grip, less roll) - Freejoint damping: 0.5 (prevent oscillation) - Motor gear: 5→3, ctrlrange: ±10→±5 (reduce torque, prevent wheelies) --- mujoco/Chenlong_Robot/car.xml | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/mujoco/Chenlong_Robot/car.xml b/mujoco/Chenlong_Robot/car.xml index 6475482..693b3f3 100644 --- a/mujoco/Chenlong_Robot/car.xml +++ b/mujoco/Chenlong_Robot/car.xml @@ -8,10 +8,10 @@ - + - + @@ -23,27 +23,27 @@ - + - + - + - + - - - - + + + + \ No newline at end of file From 58301ffa0799ae0d4f61b8585778dd4991c724e8 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 16:48:26 +0800 Subject: [PATCH 16/39] tune: car physics in carArmXml - increase grip, add damping - Ground friction: 1.5/0.005/0.0001 - Wheel friction: 1.5/0.005/0.0001 - Freejoint damping: 0.5 --- ui/src/pages/MujocoPage/carArmXml.ts | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index c270940..54d8871 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -8,10 +8,10 @@ export const CAR_ARM_XML = ` - + - + @@ -23,27 +23,27 @@ export const CAR_ARM_XML = ` - + - + - + - + - - - - + + + + `; \ No newline at end of file From 1c4d86351ff8dc6b35efcf48c900ccbad498df74 Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 16:50:32 +0800 Subject: [PATCH 17/39] fix: remove invalid damping attribute from freejoint freejoint does not support damping in MuJoCo. Only friction changes remain. --- mujoco/Chenlong_Robot/car.xml | 2 +- ui/src/pages/MujocoPage/carArmXml.ts | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/mujoco/Chenlong_Robot/car.xml b/mujoco/Chenlong_Robot/car.xml index 693b3f3..8c350be 100644 --- a/mujoco/Chenlong_Robot/car.xml +++ b/mujoco/Chenlong_Robot/car.xml @@ -11,7 +11,7 @@ - + diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 54d8871..2e8f22d 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -11,7 +11,7 @@ export const CAR_ARM_XML = ` - + From 83bcb69daf39b871bcb14e9e64d005a0a4b03e3a Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 19:35:27 +0800 Subject: [PATCH 18/39] feat: add maze with collision-enabled car body - 8x8 maze with 2m corridors, box geom walls - Car starts at safe position (3, -3) - Removed contype/conaffinity=0 from car body for collision - Ground plane expanded to 100x100 --- ui/src/pages/MujocoPage/carArmXml.ts | 36 +++++++++++++++++++++++++--- 1 file changed, 33 insertions(+), 3 deletions(-) diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 2e8f22d..900c8f7 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -8,9 +8,39 @@ export const CAR_ARM_XML = ` - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -19,7 +49,7 @@ export const CAR_ARM_XML = ` - + From e62dbf0d27463fb58ef45ae98c9d355883df6cad Mon Sep 17 00:00:00 2001 From: "junbo.dai" Date: Tue, 2 Jun 2026 21:00:52 +0800 Subject: [PATCH 19/39] refactor: split maze into separate mazeXml.ts - mazeXml.ts: ground plane + maze walls (independently editable) - carArmXml.ts: imports MAZE_XML and composes car + camera + actuators - Combined at runtime via template literal ${MAZE_XML} --- ui/src/pages/MujocoPage/carArmXml.ts | 35 +++------------------------- ui/src/pages/MujocoPage/mazeXml.ts | 33 ++++++++++++++++++++++++++ 2 files changed, 36 insertions(+), 32 deletions(-) create mode 100644 ui/src/pages/MujocoPage/mazeXml.ts diff --git a/ui/src/pages/MujocoPage/carArmXml.ts b/ui/src/pages/MujocoPage/carArmXml.ts index 900c8f7..dcce1d2 100644 --- a/ui/src/pages/MujocoPage/carArmXml.ts +++ b/ui/src/pages/MujocoPage/carArmXml.ts @@ -1,3 +1,5 @@ +import { MAZE_XML } from './mazeXml'; + export const CAR_ARM_XML = `

-

- Arm Control -

- {[ - { label: "Yaw", value: yaw, set: setYaw, min: -1, max: 1, step: 0.1 }, - { - label: "Pitch", - value: pitch, - set: setPitch, - min: -1, - max: 1, - step: 0.1, - }, - { - label: "Roll", - value: roll, - set: setRoll, - min: -1, - max: 1, - step: 0.1, - }, - ].map(({ label, value, set, min, max, step }) => ( -
- - set(parseFloat(e.target.value))} - className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-violet-500" - /> -
- ))} - -
- -
-

- Car Control -

- {[ - { - label: "Left", - value: leftWheel, - set: setLeftWheel, - }, - { - label: "Right", - value: rightWheel, - set: setRightWheel, - }, - ].map(({ label, value, set }) => ( -
- - set(parseFloat(e.target.value))} - className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-blue-500" - /> -
- ))} - +

Drive Settings

+
+ + onDriveSpeedChange(parseFloat(e.target.value))} + className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-emerald-500" + /> +
+
+ + onTurnSpeedChange(parseFloat(e.target.value))} + className="w-full h-1.5 bg-slate-700 rounded-lg appearance-none cursor-pointer accent-amber-500" + /> +