google-deepmind / mujoco

Multi-Joint dynamics with Contact. A general purpose physics simulator.
https://mujoco.org
Apache License 2.0
8.1k stars 811 forks source link

Runtime HField geometry change not reflected visually #812

Closed kylestach closed 1 year ago

kylestach commented 1 year ago

I'd like to change the geometry of a HField at runtime. I am using the following code:

hfield = physics.bind(self._hfield_chunks[geom_idx])
res = hfield.nrow

# Generate terrain in a 2D numpy array bounded to [0, 1]...

start_idx = hfield.adr
physics.model.hfield_data[start_idx:start_idx + res**2] = terrain.ravel()

with physics.contexts.gl.make_current() as ctx:
    ctx.call(mjlib.mjr_uploadHField,
             physics.model.ptr,
             physics.contexts.mujoco.ptr,
             hfield.element_id)

Unfortunately, while the physics model seems to change as expected, the visualization is not updating to match. I'm running this code on Linux with MuJoCo v2.3.2. I'm using dm_control v1.0.9 and the above code is called by the after_step() callback.

yuvaltassa commented 1 year ago

Can you please check if, on your system, the visualisation of the random heightfield in the Control Suite Quadruped (video) works correctly?

kylestach commented 1 year ago

@yuvaltassa Just to clarify, I specify initial heightfield data in initialize_episode(), and that does work correctly. However, I'd like to update heightfield data in the middle of an episode, based on runtime conditions (the only heightfields that should be updated are far away from any movable entities, so contacts are not an issue).

To accomplish this I am generating new heightfield data in after_step() and calling the above code to set the physics and visual models. However, the visualization still shows the original visual model (though the physics model seems to be updated correctly).

The use case is creating environments consisting of (very large/infinite) procedurally-generated terrain without requiring massive heightmaps to be loaded into the simulator. I'd like to maintain a grid of loaded "chunks" surrounding a robot, and when the robot moves these chunks should unload/reload so that they are always centered on the robot. To do this, when the robot crosses a chunk boundary I'm moving all of the furthest-away chunks to the positions that now need to be loaded, and filling them with the correct data (except, of course, that the visual model does not seem to be updating).

ripkea commented 1 year ago

Are there any updates on this? I'm facing a similar problem

yuvaltassa commented 1 year ago

Sorry for the delay. To be honest it's not entirely clear how this is possible. There is nothing special about the Python run loop. MuJoCo doesn't know or care if you are initialising your env or in the middle of the episode.

Could one of you please post a MWE where updating the hfield works at init but not mid-episode?

ripkea commented 1 year ago

Sorry for the delay. To be honest it's not entirely clear how this is possible. There is nothing special about the Python run loop. MuJoCo doesn't know or care if you are initialising your env or in the middle of the episode.

Could one of you please post a MWE where updating the hfield works at init but not mid-episode?

This is an example where I initialize the heightmap outside of the xml but before starting the viewer, it works fine:

import mujoco
import mujoco.viewer as v
import time
import numpy as np

xml = """
<mujoco>
    <asset>
        <hfield name="terrain"  nrow="16" ncol="16" size="1 1 1 1"/>
    </asset>
  <worldbody>
    <light name="top" pos="0 0 1"/>
      <geom name="red_sphere"  size=".2 .2 .2" rgba="1 0 0 1" pos="0 0 2"/>
        <geom name="heightfield" type="hfield" hfield="terrain"
     />
  </worldbody>
</mujoco>"""
model = mujoco.MjModel.from_xml_string(xml)

data = mujoco.MjData(model)

num_rows = model.hfield_nrow[0]
num_cols = model.hfield_ncol[0]
new_hfield = np.ones((num_rows * num_cols))
new_hfield[:128] = 2.
model.hfield_data = new_hfield

with v.launch_passive(model, data) as v:
  start = time.time()

  while v.is_running() and time.time() - start < 30:

    step_start = time.time()
    mujoco.mj_step(model, data)
    v.sync()

    time_until_next_step = model.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
      time.sleep(time_until_next_step)

If you move the line " model.hfield_data = new_hfield " into the while loop, it stops working.

saran-t commented 1 year ago

@ripkea This seems like the same issue as #958 and #965. Fix is incoming.