search

google-deepmind / dm_control

Google DeepMind's software stack for physics-based simulation and Reinforcement Learning environments, using MuJoCo.
Apache License 2.0
3.64k stars 648 forks source link

Advice modeling simple block sliding on floor #343

Open bryanvis opened 1 year ago

bryanvis commented 1 year ago

I'm getting started with dm_control and am trying to model a bare bones environment: a box sliding around a floor. To do this, I adapted from mujoco-worldgen just a box with 3 slide joints and corresponding actuators. I also use the floor arena and then edited down locomotion walker/target tasks to be as simple as possible. XML here. Note that if I don't include joint armature = 1, then it immediately fails w/ Nan, Inf or huge value in QACC at DOF 5 at time t=0.000. When I include it though, the box does not move.

<mujoco>  
    <compiler angle="radian"/>
    <default>
    <joint armature="1" />
    </default>
    <worldbody>
        <body name="agent" pos="0.2 0.2 0.4">
            <camera name="fake_egocentric"  pos=".25 0 .11" xyaxes="0 -1 0 0 0 1" fovy="90" />
            <joint axis="1 0 0" damping="10" name="tx" pos="0 0 0" type="slide"></joint>
            <joint axis="0 1 0" damping="10" name="ty" pos="0 0 0" type="slide"></joint>
            <joint axis="0 0 1" damping="10000" name="tz" pos="0 0 0" type="slide"></joint>
            <joint axis="0 0 1" damping="10"  name="rz" pos="0 0 0" type="hinge"></joint>
            <geom name="agent"  mass="1" pos="0 0 0" rgba="0.52734375 0.8046875 0.9765625 1" size="0.2 0.2 0.2" type="box" euler="1.57 0 0"></geom>
        </body>
    </worldbody>
    <actuator>
        <motor name="motortx" gear="100" joint="tx" ></motor>
        <motor name="motorty" gear="100" joint="ty"></motor>
        <motor name="motorrz" gear="100" joint="rz"></motor>
    </actuator>
</mujoco>

Full zip of code is attached.

ice 2.zip

yuvaltassa commented 1 year ago

The model you posted loads with no issue and has only 4 DoFs (yet the error is complaining about DOF 5), so the problem is elsewhere, something about the walker that you are adding?

bryanvis commented 1 year ago

Edit: see below

bryanvis commented 1 year ago

Ah, in the target task this line self._walker.create_root_joints(self._arena.attach(self._walker)) is adding a joint. It's still not obvious to me why the simulation is breaking immediately

bryanvis commented 1 year ago

Revisited w/ the composer tutorial. I compressed it down to a single file:

from dm_control import composer
from dm_control.composer.observation import observable
import numpy as np
from dm_control.locomotion.arenas import floors
from dm_control import mjcf

def make_frank():
  random_state = np.random.RandomState(42)
  rgba = random_state.uniform([0, 0, 0, 1], [1, 1, 1, 1])
  model = mjcf.RootElement()
  model.compiler.angle = 'radian'  # Use radians.
  box = model.worldbody.add('body', pos=[0.05,0.05,0.05])
  box.add(
      'geom', name='torso', type='box', size=[0.05,0.05,0.05], rgba=rgba)
  x = box.add("joint", axis=[1,0,0], type="slide", pos=(0,0,0))
  model.actuator.add("motor", gear="100", joint=x)
  # y = box.add("joint", axis=[0,1,0], type="slide", name="my", pos=(0,0,0))
  # model.actuator.add("motor", gear="100", joint=y)
  return model

class Creature(composer.Entity):
  def _build(self):
    self._model = make_frank()
  def _build_observables(self):
    return CreatureObservables(self)
  @property
  def mjcf_model(self):
    return self._model
  @property
  def actuators(self):
    return tuple(self._model.find_all('actuator'))

class CreatureObservables(composer.Observables):
  @composer.observable
  def joint_positions(self):
    all_joints = self._entity.mjcf_model.find_all('joint')
    return observable.MJCFFeature('qpos', all_joints)
  @composer.observable
  def joint_velocities(self):
    all_joints = self._entity.mjcf_model.find_all('joint')
    return observable.MJCFFeature('qvel', all_joints)

NUM_SUBSTEPS = 25
class BasicTask(composer.Task):
  def __init__(self, creature):
    self._creature = creature
    self._arena = floors.Floor()
    self._arena.add_free_entity(self._creature)
    self._arena.mjcf_model.worldbody.add('light', pos=(0, 0, 4))
    self._creature.observables.joint_positions.enabled = True
    self._creature.observables.joint_velocities.enabled = True
    self.control_timestep = NUM_SUBSTEPS * self.physics_timestep
  @property
  def root_entity(self):
    return self._arena
  def initialize_episode_mjcf(self, random_state):
    pass
  def initialize_episode(self, physics, random_state):
    pass
  def get_reward(self, physics):
    return 0

def get_task():
  creature = Creature()
  task = BasicTask(creature)
  env = composer.Environment(task, random_state=np.random.RandomState(42))
  return env

Some observations:

  1. this runs (using viewer w/ static policy) but the cube "stutters" at the beginning and wobbles eventually. Adding damping makes it stay still.
  2. adding the Y joint/motor immediately causes the simulation to break, with the same error but DOF 6.
  3. substituting make_frank() with make_creature() as in the tutorial runs fine.
bryanvis commented 1 year ago

To try to debug further I got the arena xml at runtime:

<mujoco model="floor">
  <compiler boundmass="1e-05" boundinertia="1e-11" coordinate="local" angle="radian" eulerseq="xyz"/>
  <option timestep="0.002" integrator="RK4" cone="elliptic" noslip_iterations="5" noslip_tolerance="0.0"/>
  <visual>
    <headlight ambient="0.40000000000000002 0.40000000000000002 0.40000000000000002" diffuse="0.80000000000000004 0.80000000000000004 0.80000000000000004" specular="0.10000000000000001 0.10000000000000001 0.10000000000000001"/>
    <map znear="0.01"/>
    <scale forcewidth="0.01" contactwidth="0.06" contactheight="0.01" jointwidth="0.01" framelength="0.3" framewidth="0.01"/>
  </visual>
  <default>
    <default class="/"/>
    <default class="unnamed_model/"/>
  </default>
  <asset>
    <texture name="groundplane" type="2d" builtin="checker" rgb1="0.20000000000000001 0.29999999999999999 0.40000000000000002" rgb2="0.10000000000000001 0.20000000000000001 0.29999999999999999" mark="edge" markrgb="0.80000000000000004 0.80000000000000004 0.80000000000000004" width="200" height="200"/>
    <material name="groundplane" class="/" texture="groundplane" texrepeat="2 2" texuniform="true" reflectance="0.2"/>
  </asset>
  <worldbody>
    <geom name="groundplane" class="/" type="plane" size="8 8 0.25" material="groundplane"/>
    <camera name="top_camera" class="/" fovy="10.058147163570894" pos="0 0 100" quat="1 0 0 0"/>
    <body name="unnamed_model/">
      <freejoint name="unnamed_model/"/>
      <body name="unnamed_model//unnamed_body_0" pos="0.050000000000000003 0.050000000000000003 0.050000000000000003">
        <geom name="unnamed_model/torso" class="unnamed_model/" type="box" size="0.050000000000000003 0.050000000000000003 0.050000000000000003" rgba="0.15601864044243652 0.15599452033620265 0.058083612168199461 1" mass="1.0"/>
        <joint name="unnamed_model//unnamed_joint_0" class="unnamed_model/" type="slide" pos="0 0 0" axis="1 0 0"/>
        <joint name="unnamed_model/my" class="unnamed_model/" type="slide" pos="0 0 0" axis="0 1 0"/>
      </body>
    </body>
    <light name="//unnamed_light_0" class="/" pos="0 0 4"/>
  </worldbody>
  <actuator>
    <motor name="unnamed_model//unnamed_actuator_0" class="unnamed_model/" gear="100" joint="unnamed_model//unnamed_joint_0"/>
    <motor name="unnamed_model//unnamed_actuator_1" class="unnamed_model/" gear="100" joint="unnamed_model/my"/>
  </actuator>
</mujoco>

Some things I tried: changing the integrator to RK4; reducing the timestep; changing solver params. Would be happy to hear any suggestions to fix this :)

yuvaltassa commented 1 year ago

This is a very strange model, not clear to me what you are trying to do. Maybe if I understood your goal I could help you.

You have a free body with no mass that only compiles because of the boundmass and boundinertia (so really it has a very tiny mass), and then this body has a chid with two slider joints? I don't understand why you would do that. Do you want the body to have 6 DoFs or 2? These chained 7 DoFs make no sense. Also your actuators are far too strong.

bryanvis commented 1 year ago

@yuvaltassa Sorry, that XML is the compiled one at runtime, maybe the code in the comment before that makes more sense. The goal is to have a cube that is the entire agent; it should slide along the floor, and be able to move in the x and y direction as well as rotate on the xy plane (so 3 actions.) I've been able to accomplish the x/y direction but not the rotation — I'll try to get a minimal working code for that in a followup.

yuvaltassa commented 1 year ago

So you want the cube to have 4 DoFs? How could it rotate if it can't move up? The corners will collide with the floor.

bryanvis commented 1 year ago

By "rotate on the xy plane", I just meant that the cube should be able to spin as you might spin a top — don't need to move up. Does that make sense?

bryanvis commented 1 year ago

XML:

<mujoco model="ice">  
    <compiler angle="radian"/>
    <default class="root">
          <joint pos='0 0 0' limited='false' armature='0' damping='0' stiffness='0'/>
    </default>
    <worldbody>
        <body name="agent" pos="0.05 0.05 0.05">
            <camera name="fake_egocentric"  pos="5 0 5" xyaxes="0 -1 0 0 0 1" fovy="90" />
            <joint class="root" axis="1 0 0"  name="tx" pos="0 0 0" type="slide"></joint>
            <joint class="root" axis="0 1 0"  name="ty" pos="0 0 0" type="slide"></joint>
            <geom  mass="1" pos="0 0 0" rgba="0.52734375 0.8046875 0.9765625 1" size="0.05 0.05 0.05" type="box" euler="1.57 0 0"></geom>
        </body>
    </worldbody>
    <actuator>
        <motor name="motortx" gear="100" joint="tx" ></motor>
        <motor name="motorty" gear="100" joint="ty"></motor>
    </actuator>
</mujoco>

Code:

from dm_control import composer
from dm_control.composer.observation import observable
import numpy as np
from dm_control.locomotion.soccer.pitch import Pitch

from dm_control import mjcf
import os
_XML_DIRNAME = os.path.join(os.path.dirname(__file__), '../ice/')
_XML_FILENAME = 'ice.xml'
random_state = np.random.RandomState(42)

class Creature(composer.Entity):
  """A multi-legged creature derived from `composer.Entity`."""
  def _build(self):
    self._model = mjcf.from_path(os.path.join(_XML_DIRNAME, _XML_FILENAME))
  def _build_observables(self):
    return CreatureObservables(self)
  @property
  def mjcf_model(self):
    return self._model
  @property
  def actuators(self):
    return tuple(self._model.find_all('actuator'))
  def create_root_joints(self, attachment_frame):
    root_class = self._model.find('default', 'root')
    root_x = attachment_frame.add(
        'joint', name='root_x', type='slide', axis=[1, 0, 0], dclass=root_class)
    root_y = attachment_frame.add(
        'joint', name='root_y', type='slide', axis=[0, 1, 0], dclass=root_class)

# Add simple observable features for joint angles and velocities.
class CreatureObservables(composer.Observables):
  @composer.observable
  def joint_positions(self):
    all_joints = self._entity.mjcf_model.find_all('joint')
    return observable.MJCFFeature('qpos', all_joints)
  @composer.observable
  def joint_velocities(self):
    all_joints = self._entity.mjcf_model.find_all('joint')
    return observable.MJCFFeature('qvel', all_joints)

from dm_control.locomotion.arenas import floors
NUM_SUBSTEPS = 25
class BasicTask(composer.Task):
  def __init__(self, creature):
    self._creature = creature
    self._arena = floors.Floor()
    self._creature.create_root_joints(self._arena.attach(self._creature))
    self._arena.mjcf_model.worldbody.add('light', pos=(0, 0, 20))
    self._creature.observables.joint_positions.enabled = True
    self._creature.observables.joint_velocities.enabled = True
    self.control_timestep = NUM_SUBSTEPS * self.physics_timestep
  @property
  def root_entity(self):
    return self._arena
  def initialize_episode_mjcf(self, random_state):
    pass
  def initialize_episode(self, physics, random_state):
    pass
  def get_reward(self, physics):
    return 0

def get_task():
  creature = Creature()
  task = BasicTask(creature)
  env = composer.Environment(task, random_state=np.random.RandomState(42))
  return env

Here's what I got working for x/y direction. Admittedly, I pieced this together using the soccer example, and it'd be great to have guidance on the right way to model this.

(Some confusions I had anyways: Why does this only work with the cube's geom having euler rotation 1.57 and not 0 in the axis? How exactly is the create_root_joints making things work here?)

yuvaltassa commented 1 year ago

From your description, sounds like you want to add a hinge joint with an axis in the z-direction?

Not sure what you mean about euler, in what way does it "not work"?

bryanvis commented 1 year ago

@yuvaltassa I tried adding such a hinge as so:

<mujoco model="ice">  
    <compiler angle="radian"/>
    <worldbody>
        <body name="agent" pos="0 0 0.05">
            <camera name="fake_egocentric"  pos="5 0 5" xyaxes="0 -1 0 0 0 1" fovy="90" />
            <joint  axis="1 0 0"  name="tx" pos="0 0 0" type="slide"></joint>
            <joint  axis="0 1 0"  name="ty" pos="0 0 0" type="slide"></joint>
            <joint  axis="0 0 1"  name="rz" pos="0 0 0" type="hinge"></joint>
            <geom  mass="1" pos="0 0 0" rgba="0.52734375 0.8046875 0.9765625 1" size="0.05 0.05 0.05" type="box" euler="1.57 0 0"></geom>
        </body>
    </worldbody>
    <actuator>
        <motor name="motortx" gear="100" joint="tx" ></motor>
        <motor name="motorty" gear="100" joint="ty"></motor>
        <motor name="motorrz" gear="10" joint="rz"></motor> 
    </actuator>
</mujoco>

That does what I want it to by itself. But adding the floor & compiling (see code at the end) leads to this one at runtime:

<mujoco model="floor">
  <compiler boundmass="1e-05" boundinertia="1e-11" coordinate="local" angle="radian" eulerseq="xyz"/>
  <option timestep="0.002" cone="elliptic" noslip_iterations="5" noslip_tolerance="0.0"/>
  <visual>
    <headlight ambient="0.40000000000000002 0.40000000000000002 0.40000000000000002" diffuse="0.80000000000000004 0.80000000000000004 0.80000000000000004" specular="0.10000000000000001 0.10000000000000001 0.10000000000000001"/>
    <map znear="0.01"/>
    <scale forcewidth="0.01" contactwidth="0.06" contactheight="0.01" jointwidth="0.01" framelength="0.3" framewidth="0.01"/>
  </visual>
  <default>
    <default class="/"/>
    <default class="ice/"/>
  </default>
  <asset>
    <texture name="groundplane" type="2d" builtin="checker" rgb1="0.20000000000000001 0.29999999999999999 0.40000000000000002" rgb2="0.10000000000000001 0.20000000000000001 0.29999999999999999" mark="edge" markrgb="0.80000000000000004 0.80000000000000004 0.80000000000000004" width="200" height="200"/>
    <material name="groundplane" class="/" texture="groundplane" texrepeat="2 2" texuniform="true" reflectance="0.2"/>
  </asset>
  <worldbody>
    <geom name="groundplane" class="/" type="plane" size="8 8 0.25" material="groundplane"/>
    <camera name="top_camera" class="/" fovy="10.058147163570894" pos="0 0 100" quat="1 0 0 0"/>
    <body name="ice/">
      <body name="ice/agent" pos="0 0 0.050000000000000003">
        <camera name="ice/fake_egocentric" class="ice/" fovy="90.0" pos="5 0 5" xyaxes="0 -1 0 0 0 1"/>
        <joint name="ice/tx" class="ice/" type="slide" pos="0 0 0" axis="1 0 0"/>
        <joint name="ice/ty" class="ice/" type="slide" pos="0 0 0" axis="0 1 0"/>
        <joint name="ice/rz" class="ice/" type="hinge" pos="0 0 0" axis="0 0 1"/>
        <geom name="ice//unnamed_geom_0" class="ice/" type="box" size="0.050000000000000003 0.050000000000000003 0.050000000000000003" rgba="0.52734375 0.8046875 0.9765625 1" mass="1.0" pos="0 0 0" euler="1.5700000000000001 0 0"/>
      </body>
    </body>
    <light name="//unnamed_light_0" class="/" pos="0 0 20"/>
  </worldbody>
  <actuator>
    <motor name="ice/motortx" class="ice/" gear="100" joint="ice/tx"/>
    <motor name="ice/motorty" class="ice/" gear="100" joint="ice/ty"/>
    <motor name="ice/motorrz" class="ice/" gear="10" joint="ice/rz"/>
  </actuator>
</mujoco>

And here's a video when applying [0,0,1] action. I expect it to spin around the center of the cube.

bryanvis commented 1 year ago

Ok, I think this was simply due to friction and setting "condim" to 1 for both the floor & the cube gets me the behavior I was looking for! I'll try to tune it a bit. (If this is covering up something wrong, do let me know though :) )