Attach a class-defined body to another class-defined body using joints

Comienza con la sección «MJCF attachment-frames» del README de dm_control/mjcf enlazado y sigue las dos llamadas a site.attach mostradas en el issue. Compara el XML actual con el anidamiento solicitado, conservando la relación de articulación de cada cuerpo adjunto; la tarea está completada cuando el XML generado coloca el hijo bajo el cuerpo que contiene la articulación, tal como se muestra.

Hello,
In order attach multiple links together, each having its own collision/visual mesh, inertia, joint, etc. I decided to construct a class that defines such a link, and it is then called multiple times, where each of its objects are initialized accordingly.

The issue that i am facing is that, when attaching such a child-body (Child) to the parent body, where the parent-body (Parent-2) has a joint w.r.t. its parent (Parent-1), I am unable to place the Child inside the XMLdefinition of Parent-2 where the joint is defined, preventing the effects of the joint (Joint 1-to-2) between Parent-1 and Parent-2 to effect the Child body.

Below, I define a class for constructing bodies with joints:

class Link_w_Joints:
    #===================#
    #  C O N S T A N T  #
    #===================#
    
    #===============================#
    #  I N I T I A L I Z A T I O N  #
    #===============================#
    # joint axis default value [0, 0, 1]: https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-joint
    def __init__(self, col_meshes, com_pos, dclass_col, inertia, joint_damping, joint_frictionloss, joint_name, joint_range, mass, name, viz_meshes, dclass_viz = None, joint_axis = [0, 0, 1]):
        # Make a MujoCo Root
        self.mjcf_model = mjcf.RootElement(model=name)
        # Add a body element:
        self.link_body = self.mjcf_model.worldbody.add('body',
                                                        name = name)
        # Add Inertial properties
        self.link_body.add('inertial',
                           pos = com_pos, 
                           mass = mass,
                           fullinertia = inertia)
        # Add Joints
        self.link_body.add('joint',
                           name = joint_name,
                           range = joint_range, 
                           damping = joint_damping,
                           axis = joint_axis,
                           frictionloss = joint_frictionloss,
                           dclass = 'rand')
        # Add the Geometry (https://github.com/google-deepmind/dm_control/blob/ed424509c0a0e8ddf7a43824924de483026ad9cc/dm_control/locomotion/soccer/humanoid.py#L50)
        # For visualization
        self.link_body.add('geom',
                           dclass = name+'\\viz' if dclass_viz is None else dclass_viz,
                           mesh = viz_meshes)
        # For Collision
        for x in range(len(col_meshes)):
            self.link_body.add('geom',
                               dclass = dclass_col,
                               mesh = col_meshes[x])

and below, I call the class Link_w_Joints multiple-times and attach successive-links (from ground to tool) to the preceding-ones:

class WAM:
    #===================#
    #  C O N S T A N T  #
    #===================#
    _body_name = ['base_link',
                  'shoulder_yaw_link',
                  'shoulder_pitch_link']
    _bhand_name = 'bhand'
    _dclass_col = 'wam\\col'
    _camera_pos = [0, 0.085, 0.02]
    _camera_name = "camera"
    _camera_fovy = 55
    _col_mesh_names = [['shoulder_link_convex_decomposition_p1',
                        'shoulder_link_convex_decomposition_p2',
                        'shoulder_link_convex_decomposition_p3'],
                       ['shoulder_pitch_link_convex']]
    _com_pos = [[-0.00443422, -0.00066489, -0.12189039],
                [-0.00236983, -0.01542114, 0.03105614]]
    _inertial = [[0.13488033, 0.09046330, 0.11328369, -0.00012485, 0.00213041, -0.00068555],
                [0.02140958, 0.01558906, 0.01377875, -0.00002461, 0.00027172, 0.00181920]]
    _joint_damping = [1000,
                       1000]
    _joint_frictionloss = 1000
    _joint_name = ['wam\\J1',
                    'wam\\J2']
    _joint_range = [[-2.6, 2.6],
                     [-2.0, 2.0]]
    _mass = [10.76768767,
              3.8749]
    _pos = [[0, 0, 0.346],
            [0, 0, 0]]
    # quant=w x y z 
    _quat = [[1, 0, 0, 0],
            [0.707107, -0.707107, 0, 0]]
    _site_name = 'site'
    _viz_mesh_names = ['shoulder_link_fine',
                       'shoulder_pitch_link_fine']
    _wam_name = 'wam'
    #===============================#
    #  I N I T I A L I Z A T I O N  #
    #===============================#
    def __init__(self, name):
        # Make a MujoCo Root
        self.mjcf_model = mjcf.RootElement(model=name)
        # Add 'base_link' element:
        self.base_link = self.mjcf_model.worldbody.add('body',
                                                        name = self._wam_name+'\\'+self._body_name[0],
                                                        pos = (0, 0, 0))

        # Attach site for 'shoulder_yaw_link'
        self.shoulder_yaw_link_site = self.base_link.add('site',
                                                          pos = self._pos[0],
                                                          quat = self._quat[0],
                                                          dclass = self._site_name)
        # Create link 'shoulder_yaw_link'
        self.shoulder_yaw_link = Link_w_Joints(col_meshes = self._col_mesh_names[0],
                                                com_pos = self._com_pos[0],
                                                dclass_col = 'wam\\col',
                                                inertia = self._inertial[0], 
                                                joint_damping = self._joint_damping[0],
                                                joint_frictionloss = self._joint_frictionloss,
                                                joint_name = self._joint_name[0],
                                                joint_range = self._joint_range[0],
                                                mass = self._mass[0],
                                                name = self._wam_name+'\\'+self._body_name[1],
                                                viz_meshes = self._viz_mesh_names[0])
        # Attach 'shoulder_yaw_link'
        self.shoulder_yaw_link_site.attach(self.shoulder_yaw_link.mjcf_model)
        
        # Attach site for 'shoulder_pitch_link'
        self.shoulder_pitch_link_site = self.shoulder_yaw_link.mjcf_model.worldbody.add('site',
                                                                                                                                         pos = self._pos[1],
                                                                                                                                         quat = self._quat[1],
                                                                                                                                         dclass = self._site_name)
        # Create link 'shoulder_pitch_link'
        self.shoulder_pitch_link = Link_w_Joints(col_meshes = self._col_mesh_names[1],
                                               com_pos = self._com_pos[1],
                                               dclass_col = 'wam\\col',
                                               inertia = self._inertial[1], 
                                               joint_damping = self._joint_damping[1],
                                               joint_frictionloss = self._joint_frictionloss,
                                               joint_name = self._joint_name[1],
                                               joint_range = self._joint_range[1],
                                               mass = self._mass[1],
                                               name = self._wam_name+'\\'+self._body_name[2],
                                               viz_meshes = self._viz_mesh_names[1])
        # Attach 'shoulder_pitch_link'
        self.shoulder_pitch_link_site.attach(self.shoulder_pitch_link.mjcf_model)

I have already identified the cause of said problem:

1. When attaching a body to a site, the worldbody-element of the Child is converted into a body-element and inserted into Parent-2 link,
2. Because the same thing had happened when attaching Parent-2 to Parent-1, the Child does not end up inside the body-element that holds the joint-definition, but in the outer-one that was defined when the worldbody-element of the child-body is converted into a body-element.

Below, you will find the output of the above script which shows the above-described behavior:

<body name="wam_7dof_bhand_0\wam\base_link" pos="0 0 0">
          <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\" pos="0 0 0.34599999999999997" quat="1 0 0 0">
            <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_yaw_link">
                <inertial pos="-0.00443422 -0.00066489000000000001 -0.12189039" mass="10.767687670000001" fullinertia="0.13488032999999999 0.090463299999999996 0.11328369000000001 -0.00012485 0.0021304100000000001 -0.00068555000000000005"/>
                <joint name="wam\J1" class="rand" axis="0 0 1" range="-2.6000000000000001 2.6000000000000001" damping="1000" frictionloss="1000"/>
                <geom name="//unnamed_geom_18" class="wam\shoulder_yaw_link\viz" mesh="shoulder_link_fine"/>
                <geom name="//unnamed_geom_19" class="wam\col" mesh="shoulder_link_convex_decomposition_p1"/>
                <geom name="//unnamed_geom_20" class="wam\col" mesh="shoulder_link_convex_decomposition_p2"/>
                <geom name="//unnamed_geom_21" class="wam\col" mesh="shoulder_link_convex_decomposition_p3"/>
            </body>
            <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_pitch_link\" pos="0 0 0" quat="0.70710700000000004 -0.70710700000000004 0 0">
              <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_pitch_link\wam\shoulder_pitch_link">
                  <inertial pos="-0.0023698299999999999 -0.01542114 0.031056139999999999" mass="3.8748999999999998" fullinertia="0.021409580000000001 0.01558906 0.013778749999999999 -2.461e-05 0.00027171999999999998 0.0018192"/>
                  <joint name="wam\J2" class="rand" axis="0 0 1" range="-2 2" damping="1000" frictionloss="1000"/>
                  <geom name="//unnamed_geom_22" class="wam\shoulder_pitch_link\viz" mesh="shoulder_pitch_link_fine"/>
                  <geom name="//unnamed_geom_23" class="wam\col" mesh="shoulder_pitch_link_convex"/>
              </body>

            </body>
        </body>
 </body>

What is desired is the following output (the problematic-lines have been commented <-- -->):

<body name="wam_7dof_bhand_0\wam\base_link" pos="0 0 0">
          <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\" pos="0 0 0.34599999999999997" quat="1 0 0 0">
            <--body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_yaw_link"-->
                <inertial pos="-0.00443422 -0.00066489000000000001 -0.12189039" mass="10.767687670000001" fullinertia="0.13488032999999999 0.090463299999999996 0.11328369000000001 -0.00012485 0.0021304100000000001 -0.00068555000000000005"/>
                <joint name="wam\J1" class="rand" axis="0 0 1" range="-2.6000000000000001 2.6000000000000001" damping="1000" frictionloss="1000"/>
                <geom name="//unnamed_geom_18" class="wam\shoulder_yaw_link\viz" mesh="shoulder_link_fine"/>
                <geom name="//unnamed_geom_19" class="wam\col" mesh="shoulder_link_convex_decomposition_p1"/>
                <geom name="//unnamed_geom_20" class="wam\col" mesh="shoulder_link_convex_decomposition_p2"/>
                <geom name="//unnamed_geom_21" class="wam\col" mesh="shoulder_link_convex_decomposition_p3"/>
            <--/body-->
            <body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_pitch_link\" pos="0 0 0" quat="0.70710700000000004 -0.70710700000000004 0 0">
              <--body name="wam_7dof_bhand_0\wam\shoulder_yaw_link\wam\shoulder_pitch_link\wam\shoulder_pitch_link"-->
                  <inertial pos="-0.0023698299999999999 -0.01542114 0.031056139999999999" mass="3.8748999999999998" fullinertia="0.021409580000000001 0.01558906 0.013778749999999999 -2.461e-05 0.00027171999999999998 0.0018192"/>
                  <joint name="wam\J2" class="rand" axis="0 0 1" range="-2 2" damping="1000" frictionloss="1000"/>
                  <geom name="//unnamed_geom_22" class="wam\shoulder_pitch_link\viz" mesh="shoulder_pitch_link_fine"/>
                  <geom name="//unnamed_geom_23" class="wam\col" mesh="shoulder_pitch_link_convex"/>
              <--/body-->

            </body>
        </body>
 </body>

Can we make changes that does not create a new body-element, and, instead, appends the the site's pos and quat information to the child's body-element, which is defined inside the child's worldbody element? This will also address the issue here as well