search

google-deepmind / mujoco

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

Collisions are not detected when the height field has high elevation value #1164

Closed HyunyoungJung closed 8 months ago

HyunyoungJung commented 11 months ago

Hi,

I'm a student and I'm trying to use MuJoCo for Humanoid control.

I'm looking for some help with the height field.

When I increase the height value (technically, "elevation_z" of hfield size) and put the robot on the highest spot, most of the contacts are not detected, and the robot penetrates the height map.

Here is the related video. High elevation value(0.81m) ezgif com-video-to-gif (4)

While in the low elevation value, the above problem doesn't appear.

Here is the related video Low elevation value(0m) ezgif com-video-to-gif (3)

Here is the height field grid size.

<mujoco>
  <worldbody>
    ...
    <asset>
    <hfield name='hf1' nrow='40'  ncol='40' size="5 5 0.15 0.1"/>
    </asset>
    ...
  </worldbody>
</mujoco>

Could you help me out with this problem?

yuvaltassa commented 11 months ago

This looks like it might be an issue in the midphase, cc @quagla

@HyunyoungJung could you please attach a zip file with a minimal model that shows the issue? I don't think you need the whole robot, probably just a sphere is enough.

yuvaltassa commented 11 months ago

Oh hold on

When I increase the height value (technically, "elevation_z" of hfield size)

Are you changing the size of the heightfield procedurally post compilation? Don't do that.

HyunyoungJung commented 11 months ago

Hi, Thanks for the reply. I tried to implement a minimal model, but when I changed the robot to a simple geom, it worked fine. Here is the minimal model with a simple geom(box).("pip install pip install mujoco-python-viewer" is required)

import mujoco
import os
import cv2
import mujoco_viewer
import numpy as np
xml = """
<mujoco model='reproduce_xml'>
  <compiler angle='degree' eulerseq='xyz' meshdir='stl'/>
  <option iterations='50' solver='Newton' timestep='0.0005' gravity='0 0 -9.806'/>
  <size nconmax='100' />
  <asset>
    <texture builtin='gradient' height='128' rgb1='0.4 0.6 0.8' rgb2='0 0 0' type='skybox' width='128'/>
    <hfield name='hf1' nrow='40'  ncol='40' size="5 5 0.15 0.1"/>
  </asset>
  <default>
    <default class='collision'>
      <geom conaffinity='1' condim='3' contype='1'/>
    </default>
    <equality solimp='0.9 0.95 0.001 0.1 2' solref='0.001 1.1' />
    <geom conaffinity='0' condim='1' contype='0' solref='0.005 1' />
    <joint solreflimit='0.001 1' />
  </default>
  <worldbody>
    <geom name='ground' type='hfield' hfield="hf1" conaffinity='15' condim='3' friction='0.7 0.01 0.005' pos='0 0 0' priority='100'/>
    <body name='left-toe-roll' pos='0 0 0' euler='0 90 0'>
        <inertial pos='9e-06 -0.028084 -0.023204' mass='0.531283' fullinertia='0.00187 0.001616 0.000843 0 0 0.000566' />
        <joint type='free' limited='false'/>
        <geom name='left-foot' type='box' size='0.04 0.1175 0.0115' pos='0 -0.0437 -0.0255' euler='-60 0 0' friction='0.7 0.01 0.005' contype='4' conaffinity='3' mass='0.531283' class='collision'/>
        <body name='left-foot' pos='0 -0.05456 -0.0315' euler='-60 0 -90'>
        </body>
    </body>
  </worldbody>
</mujoco>
"""
def viewer_setup(viewer, model):
    viewer.cam.trackbodyid = 1
    viewer.cam.distance = model.stat.extent * 1.5
    viewer.cam.lookat[2] = 0.
    viewer.cam.lookat[0] = 0
    viewer.cam.lookat[1] = 0.
    viewer.cam.azimuth = 180
    viewer.cam.distance = 5
    viewer.cam.elevation = -10
    viewer.vopt.geomgroup[0] = 1
    viewer._render_every_frame = True
    viewer._contacts = True
    viewer.vopt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTPOINT] = viewer._contacts
    viewer.vopt.flags[mujoco.mjtVisFlag.mjVIS_CONTACTFORCE] = viewer._contacts

def pyramid_sloped_terrain(slope=0.2, platform_size=1.):
    height_field_raw = np.zeros((40, 40))
    x = np.arange(0, 40)
    y = np.arange(0, 40)
    center_x = int(40 / 2)
    center_y = int(40 / 2)
    xx, yy = np.meshgrid(x, y, sparse=True)
    xx = (center_x - np.abs(center_x-xx)) / center_x
    yy = (center_y - np.abs(center_y-yy)) / center_y
    xx = xx.reshape(40, 1)
    yy = yy.reshape(1, 40)
    max_height = int(slope * (0.25 / 0.005) * (40 / 2))
    height_field_raw = (max_height * xx * yy).astype(height_field_raw.dtype)

    platform_size = int(platform_size / 0.25 / 2)
    x1 = 40 // 2 - platform_size
    x2 = 40 // 2 + platform_size
    y1 = 40 // 2 - platform_size
    y2 = 40 // 2 + platform_size

    min_h = min(height_field_raw[x1, y1], 0)
    max_h = max(height_field_raw[x1, y1], 0)
    height_field_raw = np.clip(height_field_raw, min_h, max_h)        
    height_in_meters = height_field_raw * 0.005
    max_dh = np.max(height_in_meters) - np.min(height_in_meters)
    normed_hfield = (height_in_meters - np.min(height_in_meters)) / \
                                 (np.max(height_in_meters) - np.min(height_in_meters))
    return normed_hfield, max_dh

hfield, dh = pyramid_sloped_terrain(0.2,1)
frames = []
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

viewer = mujoco_viewer.MujocoViewer(model, data, )
viewer_setup(viewer, model)

data.qpos[2] = 0.2
for itr in range(10000):
    mujoco.mj_step(model, data)
    frames.append(viewer.render())
    if itr == 100:
        model.hfield_data = hfield.reshape(-1)
        model.hfield_size[0, 2] = dh
        data = mujoco.MjData(model)

        data.qpos[2] = 1.5
        mujoco.mj_forward(model, data)

        viewer = mujoco_viewer.MujocoViewer(model, data, )
        viewer_setup(viewer, model)

fourcc = cv2.VideoWriter_fourcc(*'MP4V')
video_writer = cv2.VideoWriter(os.path.join("video.mp4"), fourcc, 30,
                                (frames[0].shape[1], frames[1].shape[0]))
for frame in frames:
    video_writer.write(frame)
video_writer.release()

However, if I change the geom to the robot (with the same code), the previously mentioned problem still happens.

HyunyoungJung commented 11 months ago

This is the result of the same code (with a slight change in body height) with full robot mjcf.

ezgif com-video-to-gif (3)

HyunyoungJung commented 8 months ago

As @yuvaltassa mentioned, I should've not changed the height value on the fly. I apologize for the confusion and will close this.