Support for MuJoCo composite objects

Hi!

I am currently updating my project to use the newest robosuite version (the source code here on Github). However, I am having some difficulties integrating composite objects into my environment. Currently, as a test, I am trying to replace the cube in the lift environment with a composite object. The composite object is specified by an XML file

<mujoco model="soft_human_torso">
  <asset>
    <texture file="../textures/skin.png" type="2d" name="skin-tex"/>
    <material name="skin-material" reflectance="0.5" texrepeat="1 1" texture="skin-tex"/>
  </asset>
  <worldbody>
    <body>
        <freejoint/>
        <body name="object" pos="0 0.1 0.87" quat="0.5 0.5 -0.5 -0.5">
          <composite type="cylinder" count="9 4 11" spacing="0.035" solrefsmooth="-1324.17 -17.59">
            <geom type="capsule" size=".0075 0.025" rgba=".8 .2 .1 1" contype="0" group="1"/>
            <skin material="skin-material" texcoord="true"/>
          </composite>
        </body>
        <site rgba="0 0 0 0" size="0.005" pos="0 0 -0.07" name="bottom_site"/>
        <site rgba="0 0 0 0" size="0.005" pos="0 0 0.07" name="top_site"/>
        <site rgba="0 0 0 0" size="0.005" pos="0.2 0.2 0" name="horizontal_radius_site"/>
    </body>
  </worldbody>
</mujoco>

and the object class is implemented as

import numpy as np

from robosuite.models.objects import MujocoXMLObject
from robosuite.utils.mjcf_utils import array_to_string

class SoftTorsoObject(MujocoXMLObject):
    """
    Soft object
    """

    def __init__(self, name, damping=None, stiffness=None):
        super().__init__("my_models/assets/objects/soft_human_torso.xml", name=name)

        self.damping = damping
        self.stiffness = stiffness

        if self.damping is not None:
            self.set_damping(damping)
        if self.stiffness is not None:
            self.set_stiffness(stiffness)

    def _get_composite_element(self):
        collision = self.worldbody.find("./body/body[@name='object']")
        return collision.find("./composite")

    def set_damping(self, damping):
        """
        Helper function to override the soft body's damping directly in the XML
        Args:
            damping (float, must be greater than zero): damping parameter to override the ones specified in the XML
        """
        assert damping > 0, 'Damping must be greater than zero'

        composite = self._get_composite_element()
        solref_str = composite.get('solrefsmooth').split(' ')
        stiffness = float(solref_str[0])

        solref = np.array([stiffness, -damping])
        composite.set('solrefsmooth', array_to_string(solref))

    def set_stiffness(self, stiffness):
        """
        Helper function to override the soft body's stiffness directly in the XML
        Args:
            stiffness (float, must be greater than zero): stiffness parameter to override the ones specified in the XML
        """
        assert stiffness > 0, 'Damping must be greater than zero'

        composite = self._get_composite_element()
        solref_str = composite.get('solrefsmooth').split(' ')
        damping = float(solref_str[1])

        solref = np.array([-stiffness, damping])
        composite.set('solrefsmooth', array_to_string(solref))

I have encountered some problems with the objects.py file when substituting the composite object for the cube object. First, the geom in composite objects does not have a name attribute. Hence, the for-loop in the _get_object_subtree(self) function in objects.py needed to be slightly modified

def _get_object_subtree(self):
        # Parse object
        obj = copy.deepcopy(self.worldbody.find("./body/body[@name='object']"))
        # Rename this top level object body (will have self.naming_prefix added later)
        obj.attrib["name"] = "main"
        # Get all geom_pairs in this tree
        geom_pairs = self._get_geoms(obj)

        # Define a temp function so we don't duplicate so much code
        obj_type = self.obj_type

        def _should_keep(el):
            return int(el.get("group")) in GEOMTYPE2GROUP[obj_type]

        # Loop through each of these pairs and modify them according to @elements arg
        for i, (parent, element) in enumerate(geom_pairs):
            # Delete non-relevant geoms and rename remaining ones
            if not _should_keep(element):
                parent.remove(element)
            else:
                if parent.tag == 'composite': #ADD CHECK FOR COMOPSITE OBJECT HERE
                    pass
                else:
                    g_name = element.get("name")
                    g_name = g_name if g_name is not None else f"g{i}"
                    element.set("name", g_name)
                # Also optionally duplicate collision geoms if requested (and this is a collision geom)
                if self.duplicate_collision_geoms and element.get("group") in {None, "0"}:
                    parent.append(self._duplicate_visual_from_collision(element))
                    # Also manually set the visual appearances to the original collision model
                    element.set("rgba", array_to_string(OBJECT_COLLISION_COLOR))
                    if element.get("material") is not None:
                        del element.attrib["material"]
        # add joint(s)
        for joint_spec in self.joint_specs:
            obj.append(new_joint(**joint_spec))
        # Lastly, add a site for this object
        template = self.get_site_attrib_template()
        template["rgba"] = "1 0 0 0"
        template["name"] = "default_site"
        obj.append(ET.Element("site", attrib=template))

        return obj

After resolving this problem, I encountered a new error stating:

Failed to load XML from string. mj_loadXML error: b'Error: mass and inertia of moving bodies must be positive\nObject name = torso_main, id = 16, line = 172, column = 5'

The XML file around line 172 is the following

<body name="torso_main" pos="0 0.1 0.87" quat="0.5 0.5 -0.5 -0.5">
          <composite type="cylinder" count="9 4 11" spacing="0.035" solrefsmooth="-1324.17 -17.59">
            <geom type="capsule" mass="10" size=".0075 0.025" rgba=".8 .2 .1 1" contype="0" group="1" />
            <skin material="torso_skin-material" texcoord="true" />
          </composite>
        <joint type="free" name="torso_joint0" /><site pos="0 0 0" size="0.002 0.002 0.002" rgba="1 0 0 0" type="sphere" group="0" name="torso_default_site" />
</body>

Sorry for the long post, but to summarize, I am looking for a way to integrate a composite object into an environment. I have tried to solve the MJCF error without any luck, and suggestions on how to fix this would be greatly appreciated! :)

With that being said, I am a huge fan of the new update! Great work :D

Hi @hermanjakobsen ,

This is great work! Adding support for native Mujoco composite objects would be a huge plus for robosuite -- if we can get your code working robustly, perhaps we can merge this via a PR :)

A couple comments:

Composite objects do in fact have names for all their geoms; they're just auto-generated and runtime. See this section for more details. The key takeaway here is that even though you overrode the name setting functionality, it's important to keep track of the auto-generated names so that calling the object's contact_geoms property returns the correct names. This isn't necessary a huge issue (it won't break your code), but important to keep in mind for robustness.
Regarding your XML error, I believe this is due to the type of composite body you're trying to create. See this guide for Mujoco modeling specifics. Note specifically the first couple of lines in the Rope and loop section -- there's a specific protocol for creating these type of objects in an XML. It seems like you've already got the equivalent functionality in your code snippet, but it's a good sanity check to make sure. Can you remove your joint and instead add <freejoint/> above your composite tag and see if that works?

Okay, I managed to fix it! First off, I had to change the grouping of the geom to "0". Thus, a working XML file looks like

<mujoco model="soft_human_torso">
  <asset>
    <texture file="../textures/skin.png" type="2d" name="skin-tex"/>
    <material name="skin-material" reflectance="0.5" texrepeat="1 1" texture="skin-tex"/>
  </asset>
  <worldbody>
    <body>
        <body name="object" pos="0 0.1 0.87" quat="0.5 0.5 -0.5 -0.5">
          <composite type="cylinder" count="9 4 11" spacing="0.035" solrefsmooth="-1324.17 -17.59">
            <geom type="capsule" size=".0075 0.025" rgba=".8 .2 .1 1" contype="0" group="0"/>
            <skin material="skin-material" texcoord="true"/>
          </composite>
        </body>
    </body>
  </worldbody>
</mujoco>

When creating the python object, I had to deactivate duplicate_collision_geoms, so the first line of the __init__ function looks like

super().__init__("my_models/assets/objects/soft_human_torso.xml", name=name, duplicate_collision_geoms=False)

Setting duplicate_collision_geoms=True resulted in the following error

Traceback (most recent call last):
  File "main.py", line 29, in <module>
    contact_btw_probe_and_body_demo(1, 'main', save_data=False)
  File "/home/hermankj/Documents/masters_thesis/src/demos.py", line 76, in contact_btw_probe_and_body_demo
    env = suite.make(
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/environments/base.py", line 41, in make
    return REGISTERED_ENVS[env_name](*args, **kwargs)
  File "/home/hermankj/Documents/masters_thesis/src/my_environments/ultrasound.py", line 140, in __init__
    super().__init__(
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/environments/manipulation/manipulation_env.py", line 147, in __init__
    super().__init__(
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/environments/robot_env.py", line 186, in __init__
    super().__init__(
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/environments/base.py", line 138, in __init__
    self._load_model()
  File "/home/hermankj/Documents/masters_thesis/src/my_environments/ultrasound.py", line 198, in _load_model
    self.soft_torso = SoftTorsoObject('torso')      
  File "/home/hermankj/Documents/masters_thesis/src/my_models/objects/xml_objects.py", line 21, in __init__
    super().__init__("my_models/assets/objects/soft_human_torso.xml", name=name, duplicate_collision_geoms=True)
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/models/objects/objects.py", line 327, in __init__
    self._obj = self._get_object_subtree()
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/models/objects/objects.py", line 359, in _get_object_subtree
    parent.append(self._duplicate_visual_from_collision(element))
  File "/home/hermankj/Documents/masters_thesis/venv/lib/python3.8/site-packages/robosuite/models/objects/objects.py", line 407, in _duplicate_visual_from_collision
    vis_element.set("name", vis_element.get("name") + "_visual")
TypeError: unsupported operand type(s) for +: 'NoneType' and 'str

which I think is linked to disabling the naming for composite geoms. Further, it does not makes sense to make a (visual) duplicate of the geoms in composite objects as the geom sub-element can only appear once under the composite tag, which is stated here. In order to make _duplicate_visual_from_collision(element) work for composite objects I guess it would be necessary to copy the whole body element containing composite and geom, and then give the geom element the desired visual attributes. Hope that made sense :)

As of now, it is possible to simulate composite objects by adding the if-check in _get_object_subtree(self) in objects.py (effectively skipping the geom naming code) and setting duplicate_collision_geoms=False in the init function for the object class.

Hi @hermanjakobsen ,

Thanks for taking the time to look into this! This is great news. Are there any additional problems you are running into?

This could be a very useful feature for others, if you're interested in generalizing the code and submitting a PR to merge into master in the future.

There is some more inconsistency. For example, in composite, the objects have a different naming schema, the name of a cloth object might be "B3_5" so the code of objects.py will fail, which needs the body to be named as 'object'

Hi @cremebrule ,

I have come to the conclusion that most of the problems using composite objects occur due to the geoms in the composite objects not having a directly accessible name attribute in the xml tree. For instance, _get_object_attributes will not work as intended since it is not possible to extract the geom attributes based on the geom name. As an effect, functions as check_contact will not work.

I have thought about how to solve the naming issue, and a possible solution could be to use the prefix attribute of the body component as a substitute for the geom's name attribute in some way. Another solution could be to use the auto-generated names for the geoms (e.g. G0_0_0). However, these names are only available in the generated MjModel and not in the xml tree.

Anyhow, I think implementing support for native MuJoCo objects would require some tedious rework of the object generating infrastructure. Since this part of the code is sensitive to errors and it is critical that the infrastructure is robust, I will unfortunately not make an attempt at creating a PR. I think that you guys, who know the code base best, will create a much better solution than me :)

That being said, I hope you guys will consider implementing support for Native MuJoCo in the near future! fingers crossed

I met the contact problem too, the thing is that my "cloth" will fall through the table. Hope to chat with someone who is familiar with that.

@JieFeng-cse , robosuite unfortunately does not officially support Mujoco-native composite objects. I would suggest implementing a workaround similar to what @hermanjakobsen has posted above. Hopefully we will be able to implement this in v1.3! :D

Closing for now.

@cremebrule Any updates on this issue?

I am also working with composite object simulation in robosuite. It would be great if robosuite officially support these composite objects because sometimes finding a workaround adds another layer of complexity of the task. For instance, I am blocked on how to get the contact_geoms information for composite object.

Hi @hermanjakobsen ,

This is great work! Adding support for native Mujoco composite objects would be a huge plus for robosuite -- if we can get your code working robustly, perhaps we can merge this via a PR :)

A couple comments:

Composite objects do in fact have names for all their geoms; they're just auto-generated and runtime. See this section for more details. The key takeaway here is that even though you overrode the name setting functionality, it's important to keep track of the auto-generated names so that calling the object's contact_geoms property returns the correct names. This isn't necessary a huge issue (it won't break your code), but important to keep in mind for robustness.

Regarding your XML error, I believe this is due to the type of composite body you're trying to create. See this guide for Mujoco modeling specifics. Note specifically the first couple of lines in the Rope and loop section -- there's a specific protocol for creating these type of objects in an XML. It seems like you've already got the equivalent functionality in your code snippet, but it's a good sanity check to make sure. Can you remove your joint and instead add <freejoint/> above your composite tag and see if that works?

@cremebrule Any updates on this issue?

I am also working with composite object simulation in robosuite. It would be great if robosuite officially support these composite objects because sometimes finding a workaround adds another layer of complexity of the task. For instance, I am blocked on how to get the contact_geoms information for composite object.

Hi @hermanjakobsen , This is great work! Adding support for native Mujoco composite objects would be a huge plus for robosuite -- if we can get your code working robustly, perhaps we can merge this via a PR :) A couple comments:

Composite objects do in fact have names for all their geoms; they're just auto-generated and runtime. See this section for more details. The key takeaway here is that even though you overrode the name setting functionality, it's important to keep track of the auto-generated names so that calling the object's contact_geoms property returns the correct names. This isn't necessary a huge issue (it won't break your code), but important to keep in mind for robustness.

Regarding your XML error, I believe this is due to the type of composite body you're trying to create. See this guide for Mujoco modeling specifics. Note specifically the first couple of lines in the Rope and loop section -- there's a specific protocol for creating these type of objects in an XML. It seems like you've already got the equivalent functionality in your code snippet, but it's a good sanity check to make sure. Can you remove your joint and instead add <freejoint/> above your composite tag and see if that works?

I would also be interested in that. Did you manage to get it to work? I am attempting to model a wire

@cremebrule Any updates on this issue? I am also working with composite object simulation in robosuite. It would be great if robosuite officially support these composite objects because sometimes finding a workaround adds another layer of complexity of the task. For instance, I am blocked on how to get the contact_geoms information for composite object.

Hi @hermanjakobsen , This is great work! Adding support for native Mujoco composite objects would be a huge plus for robosuite -- if we can get your code working robustly, perhaps we can merge this via a PR :) A couple comments:

Composite objects do in fact have names for all their geoms; they're just auto-generated and runtime. See this section for more details. The key takeaway here is that even though you overrode the name setting functionality, it's important to keep track of the auto-generated names so that calling the object's contact_geoms property returns the correct names. This isn't necessary a huge issue (it won't break your code), but important to keep in mind for robustness.

Regarding your XML error, I believe this is due to the type of composite body you're trying to create. See this guide for Mujoco modeling specifics. Note specifically the first couple of lines in the Rope and loop section -- there's a specific protocol for creating these type of objects in an XML. It seems like you've already got the equivalent functionality in your code snippet, but it's a good sanity check to make sure. Can you remove your joint and instead add <freejoint/> above your composite tag and see if that works?

I would also be interested in that. Did you manage to get it to work? I am attempting to model a wire

Not directly, but indirectly using regex and prefix in composite objects. AFAIK robosuite doesn't keep a track of composite bodies. We can differentiate between different composite bodies by adding a prefix in the composite body xml tag, as shown below:

 <composite prefix="EE" type="box" count="6 4 4" spacing="0.05" solrefsmooth="0.004 1">
        <skin texcoord="true" material="red-wood" rgba=".7 .7 .7 1"/>
        <geom type="capsule" size=".015 0.05" rgba=".8 .2 .1 1" mass="0.01" friction="0.01 0.005 0.0001" group="0"/>
 </composite>

Then, we can find these composite bodies in the environment using the regex pattern, and match the pattern with the known contact geometries.

def _check_gripper_contact_with_table(self, model):
        """Get the contact at the gripper object
        Args:
            model (MujocoModel): An instance of MujocoModel
        Returns:
            boolean: True when gripper touch the table, otherwise false
        """
        # Initialize default gripper contact with table
        self.has_touched_table = False
        # Hard-coded regex pattern extracted from the autogenerated composite mujoco gripper model
        gripper_regex_pattern = "^gripper0_EEG[0-9]+_[0-9]+_[0-9]+$"
        # Loop through all the contacts points
        for contact in self.sim.data.contact[: self.sim.data.ncon]:
            # Get the corresponding geometry pairs <g1, g2>
            g1, g2 = self.sim.model.geom_id2name(contact.geom1), self.sim.model.geom_id2name(contact.geom2)
            # Match the hard-coded gripper regex with geometry pairs
            match1 = re.search(gripper_regex_pattern, g1)
            match2 = re.search(gripper_regex_pattern, g2)
            # Check if match found
            if match1 != None or match2 != None:
                if g1 == "table_collision"  or g2 == "table_collision":
                print(g1, g2)
                self.has_touched_table = True       # Gripper in contact with table
                print(40 * '-' + "Gripper in contact with table"+ 40 * '-')
                return self.has_touched_table

I have followed the work by @hermanjakobsen in https://github.com/hermanjakobsen/robotic-ultrasound-imaging

ARISE-Initiative / robosuite

Support for MuJoCo composite objects #170