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:
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,
)
...
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:
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.
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.
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}}"?
Could you please try creating and running this python script outside the ROS2 environment:
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
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:
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.
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.
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:
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()
