google-deepmind / dm_control

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

Attachment frames's inertial property #477

Open ajoardar007 opened 3 months ago

ajoardar007 commented 3 months ago

Hello,

I am trying to attach a body to another one, similar to your example here of child and parent.

However, my child body does have inertial-properties. Attaching the following child body

class Camera:
    #===================#
    #  C O N S T A N T  #
    #===================#
    _com_pos = (0, 0, 0.015)
    _dclass = 'wam\\viz'
    _mass = 0.095
    _mesh_name = 'intel_realsense_l515'
    _name = '\\rgb'
    _quat = (0, 0, 1, 0)
    #===============================#
    #  I N I T I A L I Z A T I O N  #
    #===============================#
    def __init__(self, name, fovy):
        # Make a MujoCo Root
        self.mjcf_model = mjcf.RootElement()
        # Add a body element: No need to add position info, as that will be added based on which `site` this body is going to be attached (https://github.com/google-deepmind/dm_control/blob/main/dm_control/mjcf/README.md#attachment-frames)
        self.camera_body = self.mjcf_model.worldbody.add('body',
                                                         name = name)
        # Add Inertial properties
        self.camera_body.add('inertial',
                             pos = self._com_pos, 
                             mass = self._mass)
        # Add the Geometry (https://github.com/google-deepmind/dm_control/blob/ed424509c0a0e8ddf7a43824924de483026ad9cc/dm_control/locomotion/soccer/humanoid.py#L50)
        self.camera_body.add('geom',
                             mesh = self._mesh_name,
                             dclass = self._dclass)
        # Add camera sensor
        self.camera_body.add('camera',
                             name = name+self._name,
                             pos = (0, 0, 0),
                             quat = self._quat,
                             fovy = fovy)

as such in the parent body

        # Add camera locations
        # An empty array to hold camera-sites
        camera_site = []
        for x in range(len(self._camera_pos)):
            camera_site.append(self.summit_body.add('site',
                                                    pos = self._camera_pos[x],
                                                    quat = self._camera_quat[x],
                                                    dclass = self._site_name))
        # Create camera
        # Initialize empty list of camera
        cameras = []
        for x in range(len(self._camera_pos)):
            cameras.append(Camera(name = self._name_summit+'_'+prefix+self._camera_name[x],
                                  fovy = self._camera_fovy[x]))
        # Attach the cameras
        for x in range(len(self._camera_pos)):
            camera_site[x].attach(cameras[x].mjcf_model)

causes the generated file to introduce an unnecessary body called unnamed_model, which is derived from the name of the worldbody (which, in this case, was initialized without a name).

<body name="smt_0" pos="0 0 0">
      <inertial pos="0 0 0.37" mass="125" diaginertia="1.391 6.8529999999999998 6.125"/>
      <geom name="smt_0\viz\base_link" class="summit\body\viz" mesh="base_link_summit"/>
      <body pos="0 0.36199999999999999 0.373" quat="0 0 0.70710700000000004 0.70710700000000004" name="unnamed_model/">
        <body name="unnamed_model/smt_0\front\camera\intel">
          <inertial pos="0 0 0.014999999999999999" mass="0.095000000000000001"/>
          <geom name="unnamed_model//unnamed_geom_0" class="unnamed_model/wam\viz" mesh="unnamed_model/intel_realsense_l515"/>
          <camera name="unnamed_model/smt_0\front\camera\intel\rgb" class="unnamed_model/" fovy="55" pos="0 0 0" quat="0 0 1 0"/>
        </body>
      </body>
</body>

What I am aiming for is a representation similar to your example here, such that I should have:

<body name="smt_0" pos="0 0 0">
      <inertial pos="0 0 0.37" mass="125" diaginertia="1.391 6.8529999999999998 6.125"/>
      <geom name="smt_0\viz\base_link" class="summit\body\viz" mesh="base_link_summit"/>
      <body pos="0 0.36199999999999999 0.373" quat="0 0 0.70710700000000004 0.70710700000000004" name="unnamed_model/">
          <inertial pos="0 0 0.014999999999999999" mass="0.095000000000000001"/>
          <geom name="unnamed_model//unnamed_geom_0" class="unnamed_model/wam\viz" mesh="unnamed_model/intel_realsense_l515"/>
          <camera name="unnamed_model/smt_0\front\camera\intel\rgb" class="unnamed_model/" fovy="55" pos="0 0 0" quat="0 0 1 0"/>
      </body>
</body>

but I am not allowed to do this because AttributeError: <inertial> is not a valid child of <worldbody>.

Question; How can I get such a clean representation while introducing inertial properties for attached-bodies?