Unstable stacked blocks #2795

heaplax · 2025-08-08T20:08:25Z

heaplax
Aug 8, 2025

Intro

Hi!

I am a graduate student at UMass, I use MuJoCo for my research on block stacking.

My setup

mujoco 3.3.4
python API
macOS 15.5

My question

I found that when multiple blocks are stacked together, they can become unstable.
The issue seems to accumulate over time — it’s fine right after stacking, but becomes unstable after a while.
The simplest way to reproduce is to stack 20 blocks one on top of another in sequence.

I have tested running without visualization, and the issue still occurs.
I have also tried adjusting the following parameters, but none of them resolved the problem:
• solref
• solimp
• friction
• timestep
• integrator
• iterations

I would appreciate it if someone could suggest a suitable set of parameters to fix this issue.

Below are two videos demonstrating the issue.

1.mov

2.mov

Minimal model and/or code that explain my question

After starting, type 'start' to begin stacking blocks.
You will notice that the stack is stable right after it’s built, but after some time, it will fall over.

Code:

import sys
import time
import threading
from typing import List, Tuple, Any

import mujoco
from mujoco import viewer


def build_xml(num_cubes: int, half_extent: float) -> str:
    """Build an MJCF string defining a plane and N free cubes with identical geometry.

    MuJoCo uses half-sizes for box geoms, so a 0.1m cube uses size=(0.05, 0.05, 0.05).
    Each cube has a free joint so we can directly set its pose via qpos.
    """
    geom_size = f"{half_extent} {half_extent} {half_extent}"

    bodies = []
    for i in range(num_cubes):
        bodies.append(
            f"""
    <body name="cube_{i}">
      <freejoint name="j_cube_{i}"/>
      <geom name="g_cube_{i}" type="box" size="{geom_size}" rgba="{_rgba_from_index(i)}" friction="0.8 0.1 0.1"/>
    </body>
            """.rstrip()
        )

    xml = f"""
<mujoco>
  <option timestep="0.002" gravity="0 0 -9.81" integrator="implicit"/>

  <asset>
    <texture name="grid" type="2d" builtin="checker" rgb1=".1 .2 .3" rgb2=".2 .3 .4" width="512" height="512"/>
    <material name="grid" texture="grid" texrepeat="6 6" texuniform="true" reflectance=".2"/>
  </asset>

  <default>
    <geom contype="1" conaffinity="1" condim="3" density="500"/>
  </default>

  <worldbody>
    <light name="light" pos="0 0 3" dir="0 0 -1"/>
    <geom name="ground" type="plane" size="10 10 1" material="grid" friction="1.0 0.1 0.1"/>
    {"\n".join(bodies)}
  </worldbody>
</mujoco>
"""
    return xml


def _rgba_from_index(i: int) -> str:
    # Repeat a set of pleasant colors; alpha=1
    palette = [
        (0.90, 0.10, 0.10, 1.0),
        (0.10, 0.60, 0.20, 1.0),
        (0.15, 0.35, 0.95, 1.0),
        (0.95, 0.60, 0.10, 1.0),
        (0.55, 0.15, 0.80, 1.0),
        (0.10, 0.75, 0.80, 1.0),
    ]
    r, g, b, a = palette[i % len(palette)]
    return f"{r} {g} {b} {a}"


def get_joint_qpos_addresses(model: Any, joint_names: List[str]) -> List[int]:
    addresses: List[int] = []
    mj_name2id = getattr(mujoco, "mj_name2id")
    mjtObj = getattr(mujoco, "mjtObj")
    mjOBJ_JOINT = getattr(mjtObj, "mjOBJ_JOINT")
    for name in joint_names:
        jid = mj_name2id(model, mjOBJ_JOINT, name)
        if jid < 0:
            raise RuntimeError(f"Joint not found: {name}")
        addresses.append(model.jnt_qposadr[jid])
    return addresses


def set_free_body_qpos(data: Any, qpos_addr: int, pos: Tuple[float, float, float], quat_wxyz: Tuple[float, float, float, float]) -> None:
    x, y, z = pos
    w, qx, qy, qz = quat_wxyz
    data.qpos[qpos_addr + 0] = x
    data.qpos[qpos_addr + 1] = y
    data.qpos[qpos_addr + 2] = z
    data.qpos[qpos_addr + 3] = w
    data.qpos[qpos_addr + 4] = qx
    data.qpos[qpos_addr + 5] = qy
    data.qpos[qpos_addr + 6] = qz


def stdin_listener(start_event: threading.Event) -> None:
    for line in sys.stdin:
        if line.strip().lower() == "start":
            start_event.set()
            break


def main() -> None:
    num_cubes = 20
    cube_edge = 0.1  # meters
    half_extent = cube_edge / 2.0

    xml = build_xml(num_cubes=num_cubes, half_extent=half_extent)
    MjModel = getattr(mujoco, "MjModel")
    MjData = getattr(mujoco, "MjData")
    model = MjModel.from_xml_string(xml)
    data = MjData(model)

    # Joint name list and their qpos addresses (7 per freejoint)
    joint_names = [f"j_cube_{i}" for i in range(num_cubes)]
    qpos_addrs = get_joint_qpos_addresses(model, joint_names)

    # Arrange cubes side-by-side along +X at z=half_extent (resting on plane)
    spacing = cube_edge * 1.2  # small gap to avoid initial contact
    x0 = -((num_cubes - 1) * spacing) / 2.0
    initial_positions: List[Tuple[float, float, float]] = [
        (x0 + i * spacing, 0.0, half_extent) for i in range(num_cubes)
    ]

    # Identity quaternion in MuJoCo is (1, 0, 0, 0)
    identity_quat = (1.0, 0.0, 0.0, 0.0)

    mj_resetData = getattr(mujoco, "mj_resetData")
    mj_forward = getattr(mujoco, "mj_forward")
    mj_step = getattr(mujoco, "mj_step")

    mj_resetData(model, data)
    for addr, pos in zip(qpos_addrs, initial_positions):
        set_free_body_qpos(data, addr, pos, identity_quat)
    mj_forward(model, data)

    # Start stdin listener thread for the "start" command
    start_event = threading.Event()
    t = threading.Thread(target=stdin_listener, args=(start_event,), daemon=True)
    t.start()

    # Stacking control
    stack_next_index = 0
    stack_last_time = 0.0
    stack_interval_s = 0.25  # place one cube onto the stack every 0.25s once started

    # Passive viewer loop
    with viewer.launch_passive(model, data) as v:
        # Optional: show joints, etc.
        # opt = mujoco.MjvOption()
        # opt.flags[mujoco.mjtVisFlag.mjVIS_JOINT] = True
        # with v.lock():
        #     v.opt.flags[:] = opt.flags[:]
        # v.sync()

        while v.is_running():
            step_t0 = time.time()

            # If stacking has been requested, move cubes one-by-one into tower positions
            if start_event.is_set() and stack_next_index < num_cubes:
                now = step_t0
                if (now - stack_last_time) >= stack_interval_s:
                    # Target position for the next cube in the tower at origin (x=y=0)
                    target_z = half_extent + stack_next_index * (2.0 * half_extent)  # 0.05 + k*0.1
                    addr = qpos_addrs[stack_next_index]
                    set_free_body_qpos(data, addr, (0, -0.4, target_z), identity_quat)
                    mj_forward(model, data)
                    stack_next_index += 1
                    stack_last_time = now

            # Step physics and sync rendering
            mj_step(model, data)
            v.sync()

            # Sleep to approximately match real-time
            dt = model.opt.timestep - (time.time() - step_t0)
            if dt > 0:
                time.sleep(dt)


if __name__ == "__main__":
    main()

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

qiuwch · 2025-08-26T03:16:56Z

qiuwch
Aug 26, 2025

Probably enable the multiccd flag can help? https://mujoco.readthedocs.io/en/stable/XMLreference.html#option-flag-multiccd

1 reply

heaplax Aug 26, 2025
Author

I added "model.opt.enableflags |= mujoco.mjtEnableBit.mjENBL_MULTICCD" to enable it, but the problem still persists.

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Unstable stacked blocks #2795

Uh oh!

{{title}}

Uh oh!

Replies: 1 comment 1 reply

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

Unstable stacked blocks #2795

Uh oh!

heaplax Aug 8, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 1 comment · 1 reply

Uh oh!

qiuwch Aug 26, 2025

Uh oh!

heaplax Aug 26, 2025 Author

heaplax
Aug 8, 2025

Replies: 1 comment 1 reply

qiuwch
Aug 26, 2025

heaplax Aug 26, 2025
Author