Simulate docking process of between robots using the Weld equality #2323

ZhenHuang0829 · 2025-01-03T11:56:10Z

ZhenHuang0829
Jan 3, 2025

Intro

Hi!

I am a graduate student at Beijing Institute of Technology, and I am using MuJoCo to simulate the docking/connecting process between two floating robots in space, using the weld equality constraint to constrain them together.

My setup

mujoco3.2.6, Python API, window10 x64

My question

In my simulation, the weld equality is added during runtime using the add_equality() function with setting 2 sites. Because the moment and configuration of docking may vary, and here says that 'the frames of the two sites will be aligned by the constraint, regardless of their position in the default configuration'.
This is the initialization of my test demo:

However, the behavior is weird and seems unstable with default solimp and solref settings：

Then I tried to adjust these parameters, the behavior is stable but still not correct(relative position of two site changed), I knew the equality is soft but seems too soft in floating scenario：

So, my question is:

The document says that 'changing the site position and orientation in mjModel.site_pos and mjModel.site_quat at runtime will correctly change the position and orientation of the constraint', I wonder what the 'correctly' means? Does it mean that pos and quat between two sites will not change(i.e., they will be totally welded)？
How to build weld equality during runtime in a right way?
Are there any suggestions on how to simulate multiple docking processes in MuJoCo?

Minimal model and/or code that explain my question

Model:

minimal XML

<mujoco model="simple_demo">
  <option timestep="0.0001"/>
  <option gravity="0 0 0"/>

  <worldbody>
    <light name="light" pos="0 0 10"/>
    <body name="obj_1" pos="0 0 0" euler="0 90 0">
      <joint name="free_1" type="free"/>
      <geom name="capsule_1_1" type="capsule" size="0.05 0.3" mass="0.2" rgba="1 0 0 1"/>
      <body name="interface_1" pos="0 0 0.5">
        <joint name="joint_1" type="hinge" axis="0 1 0" pos="0 0 -0.15"/>
        <geom name="capsule_1_2" type="capsule" size="0.05 0.1" mass="0.1" rgba="1 0 0 1"/>
        <site name="site_1" quat="1 0 0 0"/>
      </body>
    </body>
    <body name="obj_2" pos="1.3 0 0" euler="0 -90 0">
      <joint name="free_2" type="free"/>
      <geom name="capsule_2_1" type="capsule" size="0.05 0.3" mass="0.2" rgba="1 1 0 1"/>
      <body name="interface_2" pos="0 0 0.5">
        <joint name="joint_2" type="hinge" axis="0 1 0" pos="0 0 -0.15"/>
        <geom name="capsule_2_2" type="capsule" size="0.05 0.1" mass="0.1" rgba="1 1 0 1"/>
        <site name="site_2" quat="1 0 0 0"/>
      </body>
    </body>
  </worldbody>

  <actuator>
    <position name="motor_1" joint="joint_1"/>
    <position name="motor_2" joint="joint_2"/>
  </actuator>

  <equality>
<!--    <weld name="connection_12" body1="interface_1" body2="interface_2"/>-->
  </equality>
</mujoco>

Code:

import mujoco
import mujoco.viewer
from math import pi

with open('simple_demo.xml', 'r') as f:
    xml = f.read()

model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)
spec = mujoco.MjSpec.from_string(xml)

delta = 0.00001
with mujoco.viewer.launch_passive(model, data) as viewer:
    viewer.sync()
    while viewer.is_running():
        if data.time > 20:
            idx = [index for index, obj in enumerate(spec.equalities) if obj.name == 'connection_12']
            if len(idx) == 0:
                # spec.add_equality(name='connection_12', type=mujoco.mjtEq.mjEQ_WELD, objtype=mujoco.mjtObj.mjOBJ_SITE,
                #                   name1='site_1', name2='site_2')
                spec.add_equality(name='connection_12', type=mujoco.mjtEq.mjEQ_WELD, objtype=mujoco.mjtObj.mjOBJ_SITE,
                                  name1='site_1', name2='site_2',
                                  solimp=[0.9, 0.99, 1e-6, 0.9, 6], solref=[0.0001, 1e6]) # tuned parameter
                model = spec.compile()
            if data.actuator('motor_1').ctrl < pi/2:
                data.actuator('motor_1').ctrl += delta
        mujoco.mj_step(model, data)
        viewer.sync()

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

Answered by ZhenHuang0829

Jan 10, 2025

Build weld joint using sites assuming these two site are overlapping. It would be better using bodies two define weld joint and define their relative pose in eq_data.

View full answer

Balint-H · 2025-01-04T12:00:04Z

Balint-H
Jan 4, 2025
Collaborator

When loading just your XML in simulate with the weld active it looks correct. The simplest solution then is to enable-disable a constraint during runtime is via the eq_active vector in data. Simply add the weld constraint from the start in your XML/spec, disable it, and enable as needed by setting the corresponding element in the eq_active vector (which is only one element if this is your only constraint).

I think the issue might be that adding the weld via MjSpec does not calculate the offsets to satisfy the constraint at q0, while the MJCF compiler does. If you do want to dynamically add the weld via MjSpec, you might need to calculate and set the relevant eq_data fields that define the offset.

1 reply

ZhenHuang0829 Jan 6, 2025
Author

Thanks for your comment! In my actual scenario, I do have many constraints and they need to be added dynamically. So it's not easy to add the weld constraint from the start in XML.

If my understanding is correct, does the eq_data have no effect when assigning two sites to build a weld equality? So I need to assign two bodies?

yuvaltassa · 2025-01-06T02:18:40Z

yuvaltassa
Jan 6, 2025
Maintainer

I don't think you sites are aligned properly, the frames are rotated. Do you know that you can visualize site frames? It's covered in the simulate tutorial video.

1 reply

ZhenHuang0829 Jan 6, 2025
Author

Thanks for your comment!
yes, frames of two sites are rotated:

and now I found that the 'unstable' behavior is due to they 'snap together' when the weld is defined. Is it possible to let them keep the relative pose when the weld is declared and they don’t snap?

ZhenHuang0829 · 2025-01-10T01:47:45Z

ZhenHuang0829
Jan 10, 2025
Author

Build weld joint using sites assuming these two site are overlapping. It would be better using bodies two define weld joint and define their relative pose in eq_data.

0 replies

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Simulate docking process of between robots using the Weld equality #2323

{{title}}

Replies: 3 comments 2 replies

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{title}}

{{title}}

Select a reply

Simulate docking process of between robots using the Weld equality #2323

ZhenHuang0829 Jan 3, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 3 comments · 2 replies

Balint-H Jan 4, 2025 Collaborator

ZhenHuang0829 Jan 6, 2025 Author

yuvaltassa Jan 6, 2025 Maintainer

ZhenHuang0829 Jan 6, 2025 Author

ZhenHuang0829 Jan 10, 2025 Author

ZhenHuang0829
Jan 3, 2025

Replies: 3 comments 2 replies

Balint-H
Jan 4, 2025
Collaborator

ZhenHuang0829 Jan 6, 2025
Author

yuvaltassa
Jan 6, 2025
Maintainer

ZhenHuang0829 Jan 6, 2025
Author

ZhenHuang0829
Jan 10, 2025
Author