Bắt đầu với MuJoCo: Cài đặt đến mô phỏng robot đầu tiên

MuJoCo -- Physics simulator được dùng nhiều nhất trong robot learning

Trong Part 1 của series, mình đã so sánh MuJoCo, Isaac Sim và Gazebo. Bài này sẽ là hands-on tutorial -- từ cài đặt MuJoCo đến tạo robot arm model, simulate và visualize kết quả với Python.

MuJoCo (Multi-Joint dynamics with Contact) là physics engine của Google DeepMind, được sử dụng trong hầu hết các papers về robot manipulation và locomotion learning. Nếu bạn muốn làm RL cho robotics, đây là nơi bắt đầu.

Bước 1: Cài đặt MuJoCo

MuJoCo cài đặt cực kỳ đơn giản qua pip:

# Tạo virtual environment
python -m venv mujoco-env
source mujoco-env/bin/activate

# Cài đặt MuJoCo + dependencies
pip install mujoco numpy matplotlib

# Verify installation
python -c "import mujoco; print(f'MuJoCo {mujoco.__version__} installed successfully!')"

Trên Ubuntu, nếu bạn muốn dùng interactive viewer:

# Dependencies cho rendering (Ubuntu)
sudo apt-get install libgl1-mesa-glx libglew-dev

Trên macOS và Windows, viewer hoạt động out-of-the-box.

Bước 2: Hiểu MJCF -- Ngôn ngữ mô tả robot

MuJoCo sử dụng MJCF (MuJoCo XML Format) để định nghĩa robots và environments. Đây là điểm khác biệt lớn so với URDF (dùng trong ROS) -- MJCF biểu diễn mạnh hơn và tối ưu cho simulation.

Cấu trúc MJCF cơ bản

<mujoco model="example">
  <!-- Compiler settings -->
  <compiler angle="radian"/>

  <!-- Default values cho các element -->
  <default>
    <joint damping="0.5"/>
    <geom rgba="0.8 0.6 0.4 1"/>
  </default>

  <!-- Scene: lights, floor, objects -->
  <worldbody>
    <light diffuse="1 1 1" pos="0 0 3"/>
    <geom type="plane" size="2 2 0.1" rgba="0.9 0.9 0.9 1"/>

    <!-- Robot defined here -->
    <body name="link1" pos="0 0 0.5">
      <joint name="joint1" type="hinge" axis="0 0 1"/>
      <geom type="capsule" size="0.05" fromto="0 0 0 0 0 0.4"/>
    </body>
  </worldbody>

  <!-- Actuators (motors) -->
  <actuator>
    <motor joint="joint1" ctrlrange="-1 1"/>
  </actuator>
</mujoco>

Các element chính:

<body>: Link của robot, có thể nested (parent-child)
<joint>: Khớp nối giữa các bodies (hinge, slide, ball, free)
<geom>: Hình dạng vật lý (capsule, box, sphere, mesh)
<actuator>: Motor điều khiển joints
<sensor>: Cảm biến (position, velocity, force, torque...)

Bước 3: Tạo Robot Arm 3-DOF

Hãy tạo một robot arm đơn giản với 3 joints -- đủ để hiểu cơ bản nhưng đủ phức tạp để làm việc thực tế.

Lưu file này là robot_arm_3dof.xml:

<mujoco model="robot_arm_3dof">
  <compiler angle="radian" autolimits="true"/>

  <option gravity="0 0 -9.81" timestep="0.002"/>

  <default>
    <joint damping="2.0" armature="0.1"/>
    <geom contype="1" conaffinity="1" friction="1 0.005 0.001"
          rgba="0.7 0.3 0.1 1"/>
    <motor ctrlrange="-5 5" ctrllimited="true"/>
  </default>

  <asset>
    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7"
             rgb2="0 0 0" width="512" height="512"/>
    <texture name="grid" type="2d" builtin="checker" width="512"
             height="512" rgb1="0.9 0.9 0.9" rgb2="0.8 0.8 0.8"/>
    <material name="grid" texture="grid" texrepeat="8 8"/>
  </asset>

  <worldbody>
    <!-- Lights -->
    <light pos="0 0 3" dir="0 0 -1" diffuse="1 1 1"/>
    <light pos="2 2 3" dir="-1 -1 -1" diffuse="0.5 0.5 0.5"/>

    <!-- Floor -->
    <geom type="plane" size="2 2 0.01" material="grid"
          contype="1" conaffinity="1"/>

    <!-- Base (fixed to world) -->
    <body name="base" pos="0 0 0">
      <geom type="cylinder" size="0.12 0.05" rgba="0.3 0.3 0.3 1"
            mass="5"/>

      <!-- Link 1: Shoulder rotation (Z-axis) -->
      <body name="link1" pos="0 0 0.05">
        <joint name="shoulder_yaw" type="hinge" axis="0 0 1"
               range="-3.14 3.14"/>
        <geom type="capsule" fromto="0 0 0 0 0 0.3" size="0.04"
              rgba="0.2 0.5 0.8 1" mass="1"/>

        <!-- Link 2: Shoulder pitch (Y-axis) -->
        <body name="link2" pos="0 0 0.3">
          <joint name="shoulder_pitch" type="hinge" axis="0 1 0"
                 range="-1.57 1.57"/>
          <geom type="capsule" fromto="0 0 0 0.3 0 0" size="0.035"
                rgba="0.8 0.5 0.2 1" mass="0.8"/>

          <!-- Link 3: Elbow (Y-axis) -->
          <body name="link3" pos="0.3 0 0">
            <joint name="elbow" type="hinge" axis="0 1 0"
                   range="-2.35 0"/>
            <geom type="capsule" fromto="0 0 0 0.25 0 0" size="0.03"
                  rgba="0.2 0.8 0.3 1" mass="0.5"/>

            <!-- End effector -->
            <site name="end_effector" pos="0.25 0 0" size="0.02"
                  rgba="1 0 0 1"/>

            <!-- Sensors at end effector -->
            <body name="ee_body" pos="0.25 0 0">
              <geom type="sphere" size="0.025" rgba="1 0.2 0.2 1"
                    mass="0.1"/>
            </body>
          </body>
        </body>
      </body>
    </body>

    <!-- Target object -->
    <body name="target" pos="0.3 0.2 0.3">
      <geom type="sphere" size="0.03" rgba="0 1 0 0.5"
            contype="0" conaffinity="0"/>
    </body>
  </worldbody>

  <!-- Actuators -->
  <actuator>
    <motor name="shoulder_yaw_motor" joint="shoulder_yaw" gear="50"/>
    <motor name="shoulder_pitch_motor" joint="shoulder_pitch" gear="50"/>
    <motor name="elbow_motor" joint="elbow" gear="30"/>
  </actuator>

  <!-- Sensors -->
  <sensor>
    <jointpos name="shoulder_yaw_pos" joint="shoulder_yaw"/>
    <jointpos name="shoulder_pitch_pos" joint="shoulder_pitch"/>
    <jointpos name="elbow_pos" joint="elbow"/>
    <jointvel name="shoulder_yaw_vel" joint="shoulder_yaw"/>
    <jointvel name="shoulder_pitch_vel" joint="shoulder_pitch"/>
    <jointvel name="elbow_vel" joint="elbow"/>
    <framepos name="ee_pos" objtype="site" objname="end_effector"/>
  </sensor>
</mujoco>

Giải thích model

3 joints: shoulder_yaw (xoay quanh Z), shoulder_pitch (xoay quanh Y), elbow (xoay quanh Y)
Joint limits: Giới hạn góc quay thực tế, ví dụ elbow chỉ gập được 1 chiều
Damping: Mô phỏng ma sát trong joint, giúp robot không rung
Armature: Inertia của motor rotor, thêm stability
Sensors: Đọc vị trí joint, vận tốc joint, và vị trí end-effector trong không gian 3D
Target: Sphere xanh là đích để robot hướng tới (không có collision)

Bước 4: Load và Simulate trong Python

"""
MuJoCo Robot Arm Simulation
Load model, simulate, và in kết quả.
"""
import mujoco
import numpy as np

# --- Load model ---
model = mujoco.MjModel.from_xml_path("robot_arm_3dof.xml")
data = mujoco.MjData(model)

# In thông tin model
print(f"Model: {model.nq} DoF, {model.nu} actuators, {model.nsensor} sensors")
print(f"Timestep: {model.opt.timestep}s")
print(f"Joint names: {[model.joint(i).name for i in range(model.njnt)]}")
print(f"Actuator names: {[model.actuator(i).name for i in range(model.nu)]}")

# --- Simulate với constant control ---
# Set control signal (torque cho mỗi motor)
data.ctrl[0] = 0.5   # shoulder_yaw: quay nhẹ
data.ctrl[1] = -0.3   # shoulder_pitch: nghiêng xuống
data.ctrl[2] = -0.2   # elbow: gập vào

# Simulate 2 seconds (1000 steps x 0.002s timestep)
n_steps = 1000
positions = np.zeros((n_steps, model.nq))
ee_positions = np.zeros((n_steps, 3))

for i in range(n_steps):
    mujoco.mj_step(model, data)

    # Lưu joint positions
    positions[i] = data.qpos.copy()

    # Lưu end-effector position (từ sensor)
    # Sensor "ee_pos" là sensor thứ 7,8,9 (index 6,7,8) vì 6 sensors trước là jointpos/vel
    ee_positions[i] = data.sensordata[6:9].copy()

# In kết quả cuối
print(f"\nSau {n_steps} steps ({n_steps * model.opt.timestep}s):")
print(f"  Joint positions: {data.qpos}")
print(f"  End-effector XYZ: {data.sensordata[6:9]}")
print(f"  Joint velocities: {data.qvel}")

Bước 5: Visualize kết quả với Matplotlib

"""
Visualize simulation results.
"""
import matplotlib.pyplot as plt
import numpy as np

# --- (chạy simulation code ở trên trước) ---

fig, axes = plt.subplots(2, 2, figsize=(14, 10))

# Plot 1: Joint positions theo thời gian
time = np.arange(n_steps) * model.opt.timestep
ax = axes[0, 0]
joint_names = ["shoulder_yaw", "shoulder_pitch", "elbow"]
for j in range(3):
    ax.plot(time, np.degrees(positions[:, j]), label=joint_names[j])
ax.set_xlabel("Time (s)")
ax.set_ylabel("Joint angle (degrees)")
ax.set_title("Joint Positions")
ax.legend()
ax.grid(True, alpha=0.3)

# Plot 2: End-effector trajectory 3D
ax3d = fig.add_subplot(2, 2, 2, projection="3d")
ax3d.plot(ee_positions[:, 0], ee_positions[:, 1], ee_positions[:, 2],
          "b-", alpha=0.7, label="EE path")
ax3d.scatter(*ee_positions[0], c="green", s=100, label="Start")
ax3d.scatter(*ee_positions[-1], c="red", s=100, label="End")
ax3d.set_xlabel("X (m)")
ax3d.set_ylabel("Y (m)")
ax3d.set_zlabel("Z (m)")
ax3d.set_title("End-Effector Trajectory")
ax3d.legend()

# Plot 3: End-effector XY plane
ax = axes[1, 0]
ax.plot(ee_positions[:, 0], ee_positions[:, 1], "b-", alpha=0.7)
ax.scatter(ee_positions[0, 0], ee_positions[0, 1], c="green", s=100,
           zorder=5, label="Start")
ax.scatter(ee_positions[-1, 0], ee_positions[-1, 1], c="red", s=100,
           zorder=5, label="End")
ax.set_xlabel("X (m)")
ax.set_ylabel("Y (m)")
ax.set_title("EE Trajectory (top view)")
ax.legend()
ax.grid(True, alpha=0.3)
ax.set_aspect("equal")

# Plot 4: Joint velocities
ax = axes[1, 1]
# Re-simulate để lấy velocity data
data_v = mujoco.MjData(model)
data_v.ctrl[:] = [0.5, -0.3, -0.2]
velocities = np.zeros((n_steps, model.nv))
for i in range(n_steps):
    mujoco.mj_step(model, data_v)
    velocities[i] = data_v.qvel.copy()

for j in range(3):
    ax.plot(time, velocities[:, j], label=joint_names[j])
ax.set_xlabel("Time (s)")
ax.set_ylabel("Velocity (rad/s)")
ax.set_title("Joint Velocities")
ax.legend()
ax.grid(True, alpha=0.3)

plt.tight_layout()
plt.savefig("simulation_results.png", dpi=150, bbox_inches="tight")
plt.show()
print("Saved: simulation_results.png")

Bước 6: Interactive Viewer

MuJoCo có built-in viewer cho phép tương tác trực tiếp với simulation:

"""
MuJoCo Interactive Viewer
Kéo thả robot, pause/resume simulation, thay đổi camera.
"""
import mujoco
import mujoco.viewer
import numpy as np
import time


def simple_controller(model, data):
    """Controller gọi mỗi simulation step."""
    t = data.time

    # Sine wave control -- robot sẽ di chuyển theo pattern
    data.ctrl[0] = 1.0 * np.sin(0.5 * t)        # shoulder_yaw
    data.ctrl[1] = -0.5 * np.sin(0.3 * t + 1.0)  # shoulder_pitch
    data.ctrl[2] = -0.3 * np.sin(0.7 * t + 0.5)  # elbow


model = mujoco.MjModel.from_xml_path("robot_arm_3dof.xml")
data = mujoco.MjData(model)

# Launch interactive viewer với controller callback
with mujoco.viewer.launch_passive(model, data) as viewer:
    start = time.time()
    while viewer.is_running() and time.time() - start < 30:
        step_start = time.time()

        # Apply control
        simple_controller(model, data)

        # Step simulation
        mujoco.mj_step(model, data)

        # Sync viewer
        viewer.sync()

        # Realtime: đợi cho đủ timestep
        dt = model.opt.timestep - (time.time() - step_start)
        if dt > 0:
            time.sleep(dt)

print("Viewer closed.")

Trong viewer, bạn có thể:

Click + kéo để xoay camera
Scroll để zoom
Double-click vào body để select
Ctrl + click để apply force (kéo robot)
Space để pause/resume
Backspace để reset

Bước 7: Đọc Sensor Data và PD Control

Thay vì gửi torque trực tiếp, hãy làm PD controller -- phương pháp điều khiển phổ biến nhất cho robot arm:

"""
PD Controller cho robot arm.
Di chuyển end-effector đến target position.
"""
import mujoco
import numpy as np


def pd_controller(model, data, target_qpos, kp=50.0, kd=5.0):
    """
    PD control: tau = Kp * (q_target - q) - Kd * dq

    Args:
        target_qpos: Target joint positions (3,)
        kp: Proportional gain
        kd: Derivative gain
    Returns:
        Control torques (3,)
    """
    q = data.qpos[:3]       # Current joint positions
    dq = data.qvel[:3]      # Current joint velocities

    # PD control law
    torque = kp * (target_qpos - q) - kd * dq

    # Clamp to actuator limits
    torque = np.clip(torque, -5.0, 5.0)
    return torque


# Load model
model = mujoco.MjModel.from_xml_path("robot_arm_3dof.xml")
data = mujoco.MjData(model)

# Waypoints -- robot sẽ đi qua từng điểm
waypoints = [
    np.array([0.0, -0.5, -1.0]),    # Vươn tới trước
    np.array([1.0, -0.3, -0.5]),    # Quay phải
    np.array([-1.0, -0.3, -0.5]),   # Quay trái
    np.array([0.0, 0.0, 0.0]),      # Về home
]

# Simulate
current_wp = 0
target = waypoints[current_wp]
step_count = 0
switch_every = 1500  # Đổi waypoint mỗi 3 seconds

print("=== PD Controller Simulation ===")
print(f"Waypoints: {len(waypoints)}")

for step in range(switch_every * len(waypoints)):
    # Switch waypoint
    if step > 0 and step % switch_every == 0:
        current_wp = (current_wp + 1) % len(waypoints)
        target = waypoints[current_wp]
        print(f"\n[Step {step}] Switching to waypoint {current_wp}: {target}")

    # Compute and apply PD control
    data.ctrl[:3] = pd_controller(model, data, target, kp=50.0, kd=5.0)

    # Step simulation
    mujoco.mj_step(model, data)

    # Print progress mỗi 500 steps
    if step % 500 == 0:
        error = np.linalg.norm(target - data.qpos[:3])
        ee_pos = data.sensordata[6:9]
        print(f"  Step {step:5d} | Error: {error:.4f} rad | "
              f"EE pos: [{ee_pos[0]:.3f}, {ee_pos[1]:.3f}, {ee_pos[2]:.3f}]")

print("\nDone!")

Load model từ MuJoCo Menagerie

Thay vì tự tạo model, bạn có thể dùng MuJoCo Menagerie -- collection các robot models chất lượng cao:

# Clone MuJoCo Menagerie
git clone https://github.com/google-deepmind/mujoco_menagerie.git

"""
Load robot từ MuJoCo Menagerie.
Ví dụ: Franka Emika Panda arm
"""
import mujoco
import mujoco.viewer
import numpy as np
import time

# Load Franka Panda từ menagerie
model = mujoco.MjModel.from_xml_path(
    "mujoco_menagerie/franka_emika_panda/panda.xml"
)
data = mujoco.MjData(model)

print(f"Franka Panda: {model.nq} DoF, {model.nu} actuators")
print(f"Joint names:")
for i in range(model.njnt):
    jnt = model.joint(i)
    print(f"  {i}: {jnt.name} (range: {model.jnt_range[i]})")

# Sine wave control demo
with mujoco.viewer.launch_passive(model, data) as viewer:
    start = time.time()
    while viewer.is_running() and time.time() - start < 20:
        t = data.time
        # Gentle motion
        for i in range(min(model.nu, 7)):
            data.ctrl[i] = 0.3 * np.sin(0.5 * t + i * 0.5)

        mujoco.mj_step(model, data)
        viewer.sync()

        dt = model.opt.timestep - (time.time() - time.time())
        if dt > 0:
            time.sleep(max(0, model.opt.timestep - 0.0001))

Menagerie có sẵn các robot:

Arms: Franka Panda, UR5e, KUKA iiwa, xArm7
Hands: Shadow Hand, Allegro Hand
Quadrupeds: Unitree Go1/Go2, ANYmal
Humanoids: Unitree H1, Google ALOHA
Grippers: Robotiq 2F-85

Tổng kết

Trong bài này, bạn đã học:

Cài đặt MuJoCo chỉ với pip install mujoco
MJCF format: body, joint, geom, actuator, sensor
Tạo robot arm 3-DOF từ scratch với XML
Simulate và đọc data trong Python
Visualize kết quả với matplotlib và interactive viewer
PD controller -- điều khiển robot di chuyển đến target
MuJoCo Menagerie -- sử dụng robot models có sẵn

MuJoCo rất phù hợp để bắt đầu học robot simulation vì nó nhẹ, nhanh, và API đơn giản. Từ đây, bạn có thể tiến lên RL training với Gymnasium/MuJoCo environments hoặc chuyển sang GPU-accelerated training với MJX.

Trong Part 3, mình sẽ chuyển sang NVIDIA Isaac Lab -- train locomotion policy với 4,096 parallel environments trên GPU.