Attach an object to the gripper

[ziyi105]

Hi,

I trying to make an object "attached" to the gripper if the gripper gets close enough to the object, and when the robot moves, the object will move with it. Is it possible to do it? If so, any hints how I can achieve this?

Thanks in advance.

[snasiriany]

Hi, perhaps "weld" elements can achieve this. Please see these references: https://mujoco.readthedocs.io/en/stable/XMLreference.html#equality-weld https://github.com/google-deepmind/mujoco/issues/789

You can programmatically activate the weld when the object and robot get close. To get the distance of the gripper and object:

    obj_pos = env.sim.data.body_xpos[env.obj_body_id[obj_name]]
    gripper_site_pos = env.sim.data.site_xpos[env.robots[0].eef_site_id["right"]]
    gripper_obj_close = np.linalg.norm(gripper_site_pos - obj_pos) < th

Feel free to re-open the issue if you have followup questions.

[ziyi105]

@snasiriany Thanks for your reply! I have tried adding weld joint by updating the xml file like this:

        def attach_chair_to_gripper():
            """Create a weld constraint between the chair and the gripper."""
            sim_state = self.sim.get_state()
            xml_str = self.sim.model.get_xml()
            # print(f"Body names in the simulation: {self.sim.model.body_names}")

            weld_joint_xml = """
            <equality>
                <weld body1="gripper0_right_eef" body2="stool_main" solref="0.02 1" solimp="0.9 0.95 0.001" />
            </equality>
            """
            if '</mujoco>' in xml_str:
                xml_str = xml_str.replace('</mujoco>', weld_joint_xml + '</mujoco>')

            self.sim = self.sim.from_xml_string(xml_str)
            self.grab_chair = True
            self.sim.set_state(sim_state)
            self.sim.forward()

        def detach_chair_from_gripper():
            """Remove the weld constraint between the chair and the gripper."""
            # Step 1: Get the current XML model as a string
            sim_state = self.sim.get_state()
            self.grab_chair = False
            xml_str = self.sim.model.get_xml()

            weld_joint_xml = """
            <equality>
                <weld body1="gripper0_right_eef" body2="stool_main" solref="0.02 1" solimp="0.9 0.95 0.001" />
            </equality>
            """
            if weld_joint_xml in xml_str:
                xml_str = xml_str.replace(weld_joint_xml, "")

            self.sim = self.sim.from_xml_string(xml_str)

            self.sim.set_state(sim_state)
            self.sim.forward()

        # print(f"XML file path: {self.sim.model.xml_path}")
        # Check for proximity and attach/detach chair if needed
        if check_proximity(sim=self.sim, gripper_site='gripper0_right_grip_site', chair_site='stool_default_site') and not self.grab_chair:
            attach_chair_to_gripper()
        elif self.grab_chair:
            detach_chair_from_gripper()   

However, when the gripper is close enough to the chair and the threshold is met, the simulation freezes. I am facing difficulty with updating and reloading the xml file because it will either cause the simulation to crash, or if i use reset() the whole scene will be randomized and reset.

[snasiriany]

I think the issue is that you're recreating the self.sim variable:

self.sim = self.sim.from_xml_string(xml_str)

you will probably need to modify it "in place" - ie access and change sim variables without creating a new sim

[ziyi105]

@snasiriany I tried to use eq.active0 to toggle the weld joint:

        if check_proximity(self.sim, "gripper0_right_grip_site", "stool_default_site"):
            print("enabled weld")
            self.sim.model.eq_active0[0] = 1  # Enable the weld
        else:
            self.sim.model.eq_active0[0] = 0  # Disable the weld

I put this in the step method in manipulation_env.py, but it refreshes every step and the weld joint will be turned off.