search

ARISE-Initiative / robosuite

robosuite: A Modular Simulation Framework and Benchmark for Robot Learning
https://robosuite.ai
Other
1.24k stars 397 forks source link

How to set joint type when loading custom MuJoCo xml ? Custom xml object not affected by gravity and suspend in mid air. #511

Closed johnny-wang16 closed 3 days ago

johnny-wang16 commented 1 week ago

Faucet XML file:

<mujoco model="faucet">
  <asset>
    <mesh name="link_0" file="meshes/link_0.obj" refquat="0.5 -0.5 0.5 0.5" scale="0.1 0.1 0.1"/>
    <mesh name="link_1" file="meshes/link_1.obj" refquat="0.5 -0.5 0.5 0.5" scale="0.1 0.1 0.1"/>
  </asset>
  <worldbody>
    <body>
      <body name="object">
        <inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
        <geom name="link_1_collision" type="mesh" mesh="link_1" group="0"/>
        <body name="link_0">
          <inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
          <geom name="link_0_collision" quat="1 0 0 0" type="mesh" mesh="link_0" group="0"/>
          <joint name="joint_0" type="hinge" pos="0.300886 -0.0055465 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-100 100"/>
        </body>
      </body>
      <site rgba="0 0 0 0" size="0.005" pos="0 0 -0.045" name="bottom_site"/>
      <site rgba="0 0 0 0" size="0.005" pos="0 0 0.03" name="top_site"/>
      <site rgba="0 0 0 0" size="0.005" pos="0.03 0.03 0" name="horizontal_radius_site"/>
    </body>
  </worldbody>
</mujoco>

I'd like to load a custom xml file (a faucet). I tested it in mujoco's interactive viewer (activated by the command python -m mujoco.viewer) to make sure the joint is revolute. I followed the tutorial and load it to one of the demo scenes here (code below). To achieve that, I mainly modified the xml and added a python class in the xml_objects.py file:

class FaucetObject(MujocoXMLObject):
    """
    Bread loaf object (used in PickPlace)
    """

    def __init__(self, name):
        super().__init__(
            xml_path_completion("objects/faucet.xml"),
            name=name,
            # joints=[dict(type="hinge", pos="0.300886 -0.0055465 0", axis="0 0 -1", range="-1.5708 1.5708")],
            joints=[dict(type="free", damping="0.0005")],
            obj_type="all",
            duplicate_collision_geoms=True,
        )

However, I am not sure how to set the joints argument correctly and couldn't find documentations or related issues. I looked at the DoorObject as it seems to also contain a revolute joint. but it sets it to None which makes the object immovable for some reason (is this intended!?). The joint of the faucet should be a revolute joint as indicated in the xml file. Could you let me know the correct way of setting the joints parameter such that it behaves like a revolute joint in robosuite? A weird behavior is observed by executing my current code where, after the gripper grab the faucet a couple times, the top part is detached from the bottom part (image below). In addition, it seems like the faucet would suspended in midair or stuck in table if i don't set joint type to "free" and set the position too high or too low. How to resolve that? I provided my testing code and the meshes of my object below:

Testing code:

import xml.etree.ElementTree as ET

from robosuite.models import MujocoWorldBase
from robosuite.models.arenas.table_arena import TableArena
from robosuite.models.grippers import PandaGripper, RethinkGripper
from robosuite.models.objects import BoxObject, BreadObject, FaucetObject
from robosuite.utils import OpenCVRenderer
from robosuite.utils.binding_utils import MjRenderContextOffscreen, MjSim
from robosuite.utils.mjcf_utils import new_actuator, new_joint
import pdb
if __name__ == "__main__":

    # start with an empty world
    world = MujocoWorldBase()

    # add a table
    arena = TableArena(table_full_size=(0.4, 0.4, 0.05), table_offset=(0, 0, 1.1), has_legs=False)
    world.merge(arena)

    # add a gripper
    gripper = RethinkGripper()
    # Create another body with a slider joint to which we'll add this gripper
    gripper_body = ET.Element("body", name="gripper_base")
    gripper_body.set("pos", "0 0 1.3")
    gripper_body.set("quat", "0 0 1 0")  # flip z
    gripper_body.append(new_joint(name="gripper_z_joint", type="slide", axis="0 0 1", damping="50"))
    # Add the dummy body with the joint to the global worldbody
    world.worldbody.append(gripper_body)
    # Merge the actual gripper as a child of the dummy body
    world.merge(gripper, merge_body="gripper_base")
    # Create a new actuator to control our slider joint
    world.actuator.append(new_actuator(joint="gripper_z_joint", act_type="position", name="gripper_z", kp="500"))

    faucet_object = FaucetObject(name="faucet")
    mujoco_object = faucet_object.get_obj()
    world.merge_assets(faucet_object)

    # world.worldbody.append(mujoco_object)

    # Set the position of this object
    mujoco_object.set("pos", "0 0 1.1")
    # Add our object to the world body
    world.worldbody.append(mujoco_object)

    # add reference objects for x and y axes
    x_ref = BoxObject(
        name="x_ref", size=[0.01, 0.01, 0.01], rgba=[0, 1, 0, 1], obj_type="visual", joints=None
    ).get_obj()
    x_ref.set("pos", "0.2 0 1.105")
    world.worldbody.append(x_ref)
    y_ref = BoxObject(
        name="y_ref", size=[0.01, 0.01, 0.01], rgba=[0, 0, 1, 1], obj_type="visual", joints=None
    ).get_obj()
    y_ref.set("pos", "0 0.2 1.105")
    world.worldbody.append(y_ref)

    # start simulation
    model = world.get_model(mode="mujoco")

    sim = MjSim(model)
    viewer = OpenCVRenderer(sim)
    render_context = MjRenderContextOffscreen(sim, device_id=-1)
    sim.add_render_context(render_context)

    sim_state = sim.get_state()

    # for gravity correction
    gravity_corrected = ["gripper_z_joint"]
    _ref_joint_vel_indexes = [sim.model.get_joint_qvel_addr(x) for x in gravity_corrected]

    # Set gripper parameters
    gripper_z_id = sim.model.actuator_name2id("gripper_z")
    gripper_z_low = 0.07
    gripper_z_high = -0.02
    gripper_z_is_low = False

    gripper_jaw_ids = [sim.model.actuator_name2id(x) for x in gripper.actuators]
    gripper_open = [-0.0115, 0.0115]
    gripper_closed = [0.020833, -0.020833]
    gripper_is_closed = True

    # hardcode sequence for gripper looping trajectory
    seq = [(False, False), (True, False), (True, True), (False, True)]

    sim.set_state(sim_state)
    step = 0
    T = 500
    while True:
        if step % 100 == 0:
            print("step: {}".format(step))

            # Get contact information
            for contact in sim.data.contact[0 : sim.data.ncon]:

                geom_name1 = sim.model.geom_id2name(contact.geom1)
                geom_name2 = sim.model.geom_id2name(contact.geom2)
                if geom_name1 == "floor" and geom_name2 == "floor":
                    continue

                print("geom1: {}, geom2: {}".format(geom_name1, geom_name2))
                print("contact id {}".format(id(contact)))
                print("friction: {}".format(contact.friction))
                print("normal: {}".format(contact.frame[0:3]))

        # Iterate through gripping trajectory
        if step % T == 0:
            plan = seq[int(step / T) % len(seq)]
            gripper_z_is_low, gripper_is_closed = plan
            print("changing plan: gripper low: {}, gripper closed {}".format(gripper_z_is_low, gripper_is_closed))

        # Control gripper
        if gripper_z_is_low:
            sim.data.ctrl[gripper_z_id] = gripper_z_low
        else:
            sim.data.ctrl[gripper_z_id] = gripper_z_high
        if gripper_is_closed:
            sim.data.ctrl[gripper_jaw_ids] = gripper_closed
        else:
            sim.data.ctrl[gripper_jaw_ids] = gripper_open

        # Step through sim
        sim.step()
        sim.data.qfrc_applied[_ref_joint_vel_indexes] = sim.data.qfrc_bias[_ref_joint_vel_indexes]
        viewer.render()
        step += 1

meshes of the object to be added in to objects/meshes folder: link_0 link_1 .

Weird behavior: Screenshot from 2024-09-05 00-29-46

kevin-thankyou-lin commented 1 week ago

I think we've a faucet in robocasa. However, I'm not too sure how that was created; @Abhiram824 would you know if we've examples for faucets?

image

Re:

DoorObject as it seems to also contain a revolute joint. but it sets it to None which makes the object immovable for some reason (is this intended!?).

Right - if you remove the joint, then it becomes a fixed joint.

Abhiram824 commented 1 week ago

I think we've a faucet in robocasa. However, I'm not too sure how that was created; @Abhiram824 would you know if we've examples for faucets?

image

Re:

DoorObject as it seems to also contain a revolute joint. but it sets it to None which makes the object immovable for some reason (is this intended!?).

Right - if you remove the joint, then it becomes a fixed joint.

Yep! We do have faucets. Perhaps the xml file and the corresponding code will be of help.

johnny-wang16 commented 1 week ago

Hi all, thanks for your help! I figured out my problem actually.

The reason why it's detaching like that is because i set the scale of meshes to be 0.1 but did not change the position of the joint. When examining the scaled xml in mujoco, i saw that the joint is actually in mid air. After scaling the joint position, the top part stays attached to the bottom part. Ideally, I would like to set the scale of the object programmatically but seems like the only way now is to modify the xml files?

Regrading the correct way of initializing custom xml objects, i think this is the correct way:

class FaucetObject(MujocoXMLObject):
    """
    Bread loaf object (used in PickPlace)
    """

    def __init__(self, name):
        super().__init__(
            xml_path_completion("objects/faucet.xml"),
            name=name,
            joints=[dict(type="free", damping="0.0005")],
            obj_type="all",
            duplicate_collision_geoms=True,
        )

After trial and errors, my understanding of this jointparameter is that, it is not referring to any joints that the object already contains (the ones already specified in the xml). Rather, it is determining what the object as a whole should behave. Setting this parameter seemed to add a new joint to the top level of the object tree and based on the joint type, the object would behave differently. In my case, I'd like it to be freely movable in 3D space so setting it to "free" seemed to work for me. Is my understanding correct?

Another question, I was trying the demo_device_control.py with my faucet xml but somehow the gripper stops before it actually touches it. I was suspecting its a mesh problem but the problem persist after I cleaned my mesh with open3D. The following is a short clip illustrating this problem. Any ideas how to solve it ? Short clip

kevin-thankyou-lin commented 1 week ago

Great to see you've fixed it!

Yes, right now it's modifying the xml files, though you can modify xml files programmatically. The current class definition assumes the FaucetObject has only a fixed scale specified by the xml file --- did you want to scale it in a one-off way, or did you want to pass in a scale parameter?

Joint parameter: I think one way to check this is to print out the xml see what happens:

print(env.sim.model.get_xml()).

Re: weird physics: maybe visualize the collision mesh? Perhaps the collision mesh differs from the visual mesh?

Abhiram824 commented 1 week ago

Great to see you've fixed it!

Yes, right now it's modifying the xml files, though you can modify xml files programmatically. The current class definition assumes the FaucetObject has only a fixed scale specified by the xml file --- did you want to scale it in a one-off way, or did you want to pass in a scale parameter?

Joint parameter: I think one way to check this is to print out the xml see what happens:

print(env.sim.model.get_xml()).

Re: weird physics: maybe visualize the collision mesh? Perhaps the collision mesh differs from the visual mesh?

You can visualize the collision mesh and visual meshes as needed by with the following script in the Robocasa codebase: https://github.com/robocasa/robocasa/blob/main/robocasa/scripts/browse_mjcf_model.py.

johnny-wang16 commented 6 days ago

Thanks @kevin-thankyou-lin and @Abhiram824 ! Scaling: I'd like to pass in a scale parameter. Is there a way to programmatically do that? The reason is because i'd like to do it on all the shapenet mobility objects and won't be able to adjust each by hand.

Joint parameter: I printedprint(env.sim.model.get_xml() and found the that my guess is correct. Here's the resulting xml of the faucet part:

    <body name="faucet_main">
      <inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
      <joint name="faucet_joint0" type="free" limited="false" actuatorfrclimited="false" damping="0.0005"/>
      <geom name="faucet_link_1_collision" type="mesh" rgba="0.5 0 0 1" mesh="faucet_link_1"/>
      <geom name="faucet_link_1_collision_visual" type="mesh" contype="0" conaffinity="0" group="1" mass="0" mesh="faucet_link_1"/>
      <site name="faucet_default_site" pos="0 0 0" size="0.002" rgba="1 0 0 0"/>
      <body name="faucet_link_0">
        <inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
        <joint name="faucet_joint_hinge" pos="0.0300886 -0.00055465 0" axis="0 0 -1" range="-1.5708 1.5708" actuatorfrcrange="-100 100"/>
        <geom name="faucet_link_0_collision" type="mesh" rgba="0.5 0 0 1" mesh="faucet_link_0"/>
        <geom name="faucet_link_0_collision_visual" type="mesh" contype="0" conaffinity="0" group="1" mass="0" mesh="faucet_link_0"/>
      </body>
    </body>

So it actually adds a new free "joint" to the top level of the faucet to make the object move freely in 3D. In my opinion, this way of initializing the class for custom objects is confusing.

Weird physics: I also visualize the collision mesh by running that script but not sure how to visualize the visual mesh. When i didn't enable the flag for visualizing collision, I didn't see anything (maybe because my xml doesnt contain a visual mesh!?). But the visualization for collision seems fine to me. Video of visualization here.

kevin-thankyou-lin commented 4 days ago

Scaling: we don't have that yet; I'd do it by adding that as an attribute to the relevant object class, the super class + other logic in the base class. You'll need add some xml parsing logic:

class FaucetObject(MujocoXMLObject):
    """
    Bread loaf object (used in PickPlace)
    """

    def __init__(self, name: str, scale: Optional[float] = None):   # new arg
        super().__init__(
            xml_path_completion("objects/faucet.xml"),
            name=name,
            scale=scale,   # new arg
            joints=[dict(type="free", damping="0.0005")],
            obj_type="all",
            duplicate_collision_geoms=True,
        )

feel free to give it a shot and make a PR!

Joint parameter: gotcha, what would you suggest for making it less confusing?

Physics: Looks like the vis and collision meshes are the same --- the other thing to try is to convex decompose the meshes. The physics engine doesn't work too well if the meshes aren't convex. Kevin Zakka has a nice tool for this: https://github.com/kevinzakka/obj2mjcf

kevin-thankyou-lin commented 4 days ago

Also, did you mean the PartNet-Mobility objects?

johnny-wang16 commented 4 days ago

Scaling: Got it. It's not too urgent for me so ill try it when i need to .

Joint parameter: Maybe it's because im not familiar with MuJoCo so its confusing to me. But i feel like in the object part of documentation, maybe add more description after: "The optional joints argument can also specify a custom set of joints to apply to the given object (defaults to “default”, which is a single free joint)." Saying something along the lines of: "This joint argument determines the DOF of the object as a whole and does not interfere with the joints already in the object" would be nice.

Physics: got it. thanks for the info. I tried with other objects in partnet mobility and seems like this problem is specific to this type of faucet. I'll ignore it for now.

Thanks for answering all my questions and the wonderful project you guys have built!

johnny-wang16 commented 4 days ago

Yes. I mean PartNet-Mobility Dataset.

kevin-thankyou-lin commented 2 days ago

Gotcha, you're very welcome! Thanks for your kind words; I'll pass along to the dev team :) Feel free to ask if you've more questions. I'll update the docs - thanks!