spec add_equality behave differently from mjcf(dm_control) equality.add

[benthebear93]

`### Intro

Hi!

I am a graduate student at SDU, I been using MuJoCo for my research on Manipulation.
I've been trying to use "spec" for few days and i encouter odd behavior when i use add_equality for welding the mocap and body.

This gif shows the mocap weld with box using equality.add function from mjcf (dm_control)

This gif shows the mocap weld with box using add_equality fuctuon from spec

They both have the same keyargument
solref = [1e-6, 1]
type = mj.mjtEq.mjEQ_WELD,

Is there something i am missing for spec api?

spec_test.zip
Here are the codes to re produce the problem.

My setup

MuJoCo 3.3.0
Spec and dm_control API , Python 3.12
Ubuntu 22.04

My question

No response

Minimal model and/or code that explain my question

def init(self):
    current_dir = os.path.dirname(os.path.abspath(__file__))
    scene_path = os.path.join(current_dir, "empty.xml")
    scene = mjcf.from_path(scene_path)
    # Add box body
    box_body = scene.worldbody.add(
        "body",
        name="box",
        pos="0.1 0.1 0.1"
    )
    box_body.add(
        "geom",
        name="box_geom",
        type="box",
        size="0.03 0.03 0.03",
        rgba="1 0 0 1"
    )
    box_body.add("freejoint")

    # Add mocap body
    mocap_body = scene.worldbody.add(
        "body",
        name="mocap",
        pos="0.1 0.1 0.2",
        mocap="true"
    )
    mocap_body.add(
        "geom",
        name="mocap_geom",
        type="sphere",
        size="0.02 0.02 0.02",
        rgba="0 1 0 1",
        contype="0",
        conaffinity="0"
    )

    # Add weld constraint between mocap and box
    scene.equality.add(
        "weld",
        name="weld_box_mocap",
        body1="mocap",
        body2="box",
        solref="1e-6 1",
        solimp="0.9 0.95 0.001 0.00001 1"
    )

    # Compile model and data
    m = mj.MjModel.from_xml_string(scene.to_xml_string(), scene.get_assets())
    d = mj.MjData(m)

    mj.mj_step(m, d)
    return m, d

---

def init(self):
    # Load base scene
    scene_path = os.path.join(os.path.dirname(__file__), "empty.xml")
    scene = mj.MjSpec.from_file(scene_path)
    scene.copy_during_attach = True

    # Add a box body
    box_body = scene.worldbody.add_body(name="box", pos=[0.1, 0.1, 0.1])
    box_body.add_geom(
        name="box_geom",
        type=mj.mjtGeom.mjGEOM_BOX,
        size=[0.03, 0.03, 0.03],
        rgba=[1, 0, 0, 1]
    )
    box_body.add_freejoint()

    # Add a mocap body
    mocap_body = scene.worldbody.add_body(name="mocap", mocap=True, pos=[0.1, 0.1, 0.1])
    mocap_body.add_geom(
        name="mocap_geom",
        type=mj.mjtGeom.mjGEOM_SPHERE,
        size=[0.02, 0.02, 0.02],
        rgba=[0, 1, 0, 1],
        contype=0,
        conaffinity=0,
    )

    # Add weld constraint between box and mocap
    eq = scene.add_equality(
        name="weld_box_mocap",
        objtype=mj.mjtObj.mjOBJ_BODY,
        type=mj.mjtEq.mjEQ_WELD,
        name1="mocap",
        name2="box",
        solref=[1e-6, 1],
        solimp=[0.9, 0.95, 0.001, 0.00001, 1],
    )
    print("Added equality constraint:", eq.name)

    m = scene.compile()
    d = mj.MjData(m)

    mj.mj_step(m, d)

    return m, d

Confirmations

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

[yuvaltassa]

Can you compare the relevant mjModel.eq_data rows?

[benthebear93]

@yuvaltassa
Sorry for late reply
for mjcf_weld_test
m.eq_data : [[ 0. 0. 0. 0. 0. -0.1 1. 0. 0. 0. 1. ]]

for spec_weld_test
m.eq_data : [[0. 1. 0. 0. 1. 0. 1. 0. 0. 0. 1.]]

it seems like they have different eq data, but I don't yet have a clear intuition for what that data implies. It would be nice if you could provide some insight on this.

[quagla]

Isn't the mocap body in two different positions i.e. [.1, .1, .1] vs [.1, .1, .2]?

[benthebear93]

That's my mistake. Although it doesn't really matter for the problem, visually, the mocap and the box are in the same place in the mjcf, but not in the spec. Here is the eq_data after fixing the mocap body position.

python3 -m sims.mjcf_weld_test
m.eq_data : [[0. 0. 0. 0. 0. 0. 1. 0. 0. 0. 1.]]

python3 -m sims.spec_weld_test
m.eq_data : [[0. 1. 0. 0. 1. 0. 1. 0. 0. 0. 1.]]

[quagla]

Also, could you please compare m.eq_type as well?

[benthebear93]

python3 -m sims.mjcf_weld_test
m.eq_data : [[0. 0. 0. 0. 0. 0. 1. 0. 0. 0. 1.]]
m.eq_type : [1]

python3 -m sims.spec_weld_test
m.eq_data : [[0. 1. 0. 0. 1. 0. 1. 0. 0. 0. 1.]]
m.eq_type : [1]

I can fix the weird behavior by just modifying eq_data, but do you know why this is happening?
The calculation of relpos and relquat (?) seems a bit off i guess?
Or am i doing something wrong?

[quagla]

So the issue is that when using mjSpec the default anchor position is [0, 1, 0].

This is defaulted to zero in the parser if no anchor is given
https://github.com/google-deepmind/mujoco/blob/main/src/xml/xml_native_reader.cc#L1987

but it needs to be done explicitly in mjSpec:

  eq->data[0] = 0;
  eq->data[1] = 0;
  eq->data[2] = 0;

[benthebear93]

Thanks for clarifying the issue. I will close the discussion.