# Mujoco add\_world\_frame not persistent for multiple frames

**URL:** <https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407>\
**Category:** Ask\
**Created:** [November 7, 2025, 2:17am UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407 "2025-11-07T02:17:38Z")\
**Posts on this page:** 11\
**Page:** 1

<div class="post-metadata">

**Author:** ![jehan](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jehan/32/347_2.png) [@jehan](https://forum.hello-robot.com/u/jehan)\
**Post date:** [November 7, 2025, 2:17am UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/1 "2025-11-07T02:17:39Z")

</div>

Hi Hello Robot,

I think I found an issue where I can only see one world frame at a time in the mujoco viewer. Is that supposed to be the case? Here is an example of what I am doing to try to add multiple coordinate frames that I added to `stretch_mujoco_driver`:

```auto
class StretchMujocoDriver(Node):
...    
     def add_mujoco_world_frame_callback(self, msg):
        """
        Callback for storing MuJoCo world frames for goal visualization.
        Receives a PointStamped message where frame_id is the goal identifier
        and point contains the (x, y, z) position.
        """
        position = (msg.point.x, msg.point.y, msg.point.z)
        frame_id = msg.header.frame_id

        # Store or update the goal frame
        self.goal_frames[frame_id] = position
        self.get_logger().info(
            f"Stored MuJoCo world frame '{frame_id}' at position {position}",
            throttle_duration_sec=1.0
        )
...
    def republish_goal_frames(self):
        """
        Periodically republish all stored goal frames to MuJoCo.
        """
        rotation = (0.0, 0.0, 0.0) # No rotation for goal markers

        for frame_id, position in self.goal_frames.items():
            try:
                self.sim.add_world_frame(position, rotation)
            except Exception as e:
                self.get_logger().error(
                    f"Failed to add MuJoCo world frame '{frame_id}': {e}"
                )
    def ros_setup(self):
...
        self.create_subscription(
            PointStamped,
            "/mujoco/add_world_frame",
            self.add_mujoco_world_frame_callback,
            10,
            callback_group=self.main_group,
        )
...
        self.goal_frames_timer = self.create_timer(
            0.033, # ~30 Hz to match MuJoCo render rate
            self.republish_goal_frames,
            callback_group=self.main_group,
        )
...

```

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 10, 2025, 3:28pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/2 "2025-11-10T15:28:58Z")

</div>

Hi Jehan,

`self.sim.add_world_frame(position, rotation)` is the correct API call, and I verified that it works on the latest main branch in stretch\_mujoco:

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/6/64031b0c58f9779bf19304911001e90c4c790aa0.jpeg)

I am not sure why your viewer is not updating. Could you share a node that I can run to debug? Thank you.

Best regards,  
Shehab

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 14, 2025, 11:29pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/3 "2025-11-14T23:29:43Z")

</div>

Hey @jehan,

Recreated your node and found that it definitely works:

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/3/3321d3dda579c90c971b227df96304db24f5dd16.png)

I used `ros2 topic pub /mujoco/add_world_frame geometry_msgs/msg/PointStamped "{header: {frame_id: 'world'}, point: {x: 0.0, y: 0.0, z: 0.0}}"` to publish the points.

I would suggest getting rid of the timer, because you would be creating A LOT of markers on every iteration, so just keep:

```auto
    def add_mujoco_world_frame_callback(self, msg):
        """
        Callback for storing MuJoCo world frames for goal visualization.
        Receives a PointStamped message where frame_id is the goal identifier
        and point contains the (x, y, z) position.
        """
        position = (msg.point.x, msg.point.y, msg.point.z)
        frame_id = msg.header.frame_id

        # Store or update the goal frame
        self.goal_frames[frame_id] = position
        self.get_logger().info(
            f"Stored MuJoCo world frame '{frame_id}' at position {position}",
            throttle_duration_sec=1.0
        )

        rotation = (0.0, 0.0, 0.0) # No rotation for goal markers

        self.sim.add_world_frame(position, rotation)

...
    def ros_setup(self):
...
        self.create_subscription(
            PointStamped,
            "/mujoco/add_world_frame",
            self.add_mujoco_world_frame_callback,
            10,
            callback_group=self.main_group,
        )
...

```

Lastly, I launched the simulation with with `ros2 launch stretch_simulation stretch_mujoco_driver.launch.py use_mujoco_viewer:=true use_rviz:=false use_sim_time:=true mode:=navigation use_robocasa:=false`

I did have trouble getting the markers to show up with use\_robocase:=true. I’m not sure if this is because of interference from something robo-casa related, or if robocasa maps don’t start at 0,0,0 → I think it’s the latter.

Let me know if you continue having issues.

Best,  
Shehab

---

<div class="post-metadata">

**Author:** ![jehan](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jehan/32/347_2.png) [@jehan](https://forum.hello-robot.com/u/jehan)\
**Post date:** [November 17, 2025, 6:05pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/4 "2025-11-17T18:05:16Z")

</div>

Hi Shehab,

Thank you for looking into this! I tried taking out the timer as you recommended but still have the same error where only one world frame is shown on the viewer. I’m on the latest humble branch if that matters here (commit id: 24d0323fe1eb59588a3d70a963d7a71418fb7df3). I’m not sure what could be causing this as this publishing method is the only change I see based on my git history.

I published in the same way as you did in your screenshot and only see the last world frame published to `/mujoco/add_world_frame`.

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/c/c45d8532ab145febbc93a942b6dac598483d4dd6.jpeg)

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 17, 2025, 6:24pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/5 "2025-11-17T18:24:53Z")

</div>

Hi @jehan,

That’s odd. fyi, the screenshot I shared is also on the latest humble branch.

Are you calling the subscriber too quickly? Can you try to manually make the call using `ros2 topic pub /mujoco/add_world_frame geometry_msgs/msg/PointStamped "{header: {frame_id: 'world'}, point: {x: 0.0, y: 0.0, z: 0.0}}"` and `ros2 topic pub /mujoco/add_world_frame geometry_msgs/msg/PointStamped "{header: {frame_id: 'world'}, point: {x: 0.0, y: 0.0, z: 1.0}}"`?

Thanks for giving that a try.

---

<div class="post-metadata">

**Author:** ![jehan](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jehan/32/347_2.png) [@jehan](https://forum.hello-robot.com/u/jehan)\
**Post date:** [November 17, 2025, 6:58pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/6 "2025-11-17T18:58:46Z")

</div>

Hi @shehab ,

Yes I actually did manually make the call when I posted the above, just like the screenshot here:

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/4/434cfef884a759bbf2b76e57298fb953d6bd8448.jpeg)

Could there possibly be a version difference with mujoco? I am using 3.2.6.

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 17, 2025, 7:06pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/7 "2025-11-17T19:06:43Z")

</div>

3.2.6 is the right version!

Could you please try creating and running this python script outside the ROS2 environment:

```auto
from stretch_mujoco.stretch_mujoco_simulator import StretchMujocoSimulator

if __name__ == " __main__":

    sim = StretchMujocoSimulator()

    sim.start(headless=False)

    sim.add_world_frame((0.1,0,0 ), (0,0,0))
    sim.add_world_frame((0.2,0,0), (1.57,0,0)) # (x, y, z), (r, p, y)

    while sim.is_running(): ...

```

I think `stretch_mujoco` is already installed in your system python site packages at this point, but in case it’s not, fyi, `stretch_mujoco` should be at `~/ament_ws/src/stretch_ros2/stretch_simulation/stretch_mujoco_driver/dependencies/stretch_mujoco`

Thanks for letting me know if it works this way!

edit: you should see two frames like in the screenshot at [stretch\_mujoco/docs/using\_mujoco\_simulator\_with\_stretch.md at main · hello-robot/stretch\_mujoco · GitHub](https://github.com/hello-robot/stretch_mujoco/blob/main/docs/using_mujoco_simulator_with_stretch.md#misc-stretch-mujoco-simulator-api-calls)

---

<div class="post-metadata">

**Author:** ![jehan](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jehan/32/347_2.png) [@jehan](https://forum.hello-robot.com/u/jehan)\
**Post date:** [November 17, 2025, 7:15pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/8 "2025-11-17T19:15:43Z")

</div>

It actually only shows one frame for me if I run the script above, only showing the latest world frame I add to it. I tried in a standalone python script and in ipython:

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/6/6ba85d4f80965ec14836b2775072d8c4df898819.jpeg)

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 17, 2025, 7:33pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/9 "2025-11-17T19:33:57Z")

</div>

I am confused; I just re-tried that script now on my machine using a fresh clone of the `stretch_mujoco` repo, and I get both arrows. Running on an Ubuntu 22.04 workstation. I tried both the default backend and egl MUJOCO\_GL’s, same result.

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/8/846b35d87e5ddc91eef1481412ea3edea3506484.jpeg)

Here is my pip list:

 ![image](https://canada1.discourse-cdn.com/flex030/uploads/hello_robot2/original/2X/b/bf41207ee749a3d9da68ded970a5a709b47bfe35.png)

fyi, this is the implementation for adding those reference frames [stretch\_mujoco/stretch\_mujoco/mujoco\_server\_passive.py at main · hello-robot/stretch\_mujoco · GitHub](https://github.com/hello-robot/stretch_mujoco/blob/main/stretch_mujoco/mujoco_server_passive.py#L177:)

```auto

            geom = user_scn.geoms[user_scn.ngeom]
            mujoco._functions.mjv_initGeom(
                geom,
                type= mjtGeom.mjGEOM_ARROW,
                size=size,
                pos=origin,
                mat=np.array(R).flatten(),
                rgba=colors[axis],
            )
            user_scn.ngeom += 1

```

I do not currently have other ideas for you to try, sorry - seems like a Mujoco thing. I’ll post again if I figure something out! In the meantime, I am happy to keep discussing/debugging this with you.

---

<div class="post-metadata">

**Author:** ![jehan](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/jehan/32/347_2.png) [@jehan](https://forum.hello-robot.com/u/jehan)\
**Post date:** [November 17, 2025, 11:13pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/10 "2025-11-17T23:13:31Z")

</div>

Hi Shehab, thank you for all the debugging steps. I found out that I had made changes to files in the `stretch_mujoco` dependency inadvertently (in `stretch_ros2/stretch_simulation/stretch_mujoco_driver/dependencies/stretch_mujoco`).

My VS Code didn’t automatically detect that subfolder as a repository so I hadn’t realized I made changes a while ago, so I set `“git.autoRepositoryDetection”: “subFolders”` and `“git.repositoryScanMaxDepth”: 10`.

Turns out that I had set `self.viewer.user_scn.ngeom = 0` at every `push_command()` in `MujocoServerPassive` since I wanted the world frames to disappear after they had been reached. I added a variable to `command_status` called `clear_coordinate_frame_arrows_viz` and now use it like so:

```auto
        if command_status.clear_coordinate_frame_arrows_viz:
            # Clear all user scene geometries
            self.viewer.user_scn.ngeom = 0
            command_status.clear_coordinate_frame_arrows_viz = False
            command_status.coordinate_frame_arrows_viz.clear()

```

This issue is resolved now on my side!

---

<div class="post-metadata">

**Author:** ![shehab](https://yyz2.discourse-cdn.com/flex030/user_avatar/forum.hello-robot.com/shehab/32/936_2.png) [@shehab](https://forum.hello-robot.com/u/shehab)\
**Post date:** [November 17, 2025, 11:14pm UTC](https://forum.hello-robot.com/t/mujoco-add-world-frame-not-persistent-for-multiple-frames/1407/11 "2025-11-17T23:14:43Z")

</div>

I am glad your issue is resolved!