Add site semantics to weld constraint. Fixes #1896

PiperOrigin-RevId: 675112981
Change-Id: Ie4c01440d54116075df89f480591700762892cbd

doc/XMLreference.rst

@@ -4372,7 +4372,8 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o
   satisfied in the configuration in which the model is defined.
 - :ref:`site1<equality-connect-site1>` and :ref:`site2<equality-connect-site2>` (both required). When using this
   specification, the two sites will be pulled together by the constraint, regardless of their position in the default
-  configuration.
+  configuration. An example of this specification is shown in
+  `this model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__.
 
 .. _equality-connect-name:
 
@@ -4421,11 +4422,11 @@ ball joint outside the kinematic tree. Connect constraints can be specified in o
 
 :at:`site1`: :at-val:`string, optional`
    Name of a site belonging to the first body participating in the constraint. When specified, :at:`site2` must also be
-   specified. The (:at:`site1`, :at:`site2`) specification is a more flexible alternative to the (:at:`body1`,
+   specified. The (:at:`site1`, :at:`site2`) specification is a more flexible alternative to the body-based
-   :at:`anchor`) specification, and is different in two ways. First, the sites are not required to overlap at the
+   specification, and is different in two ways. First, the sites are not required to overlap at the default
-   default configuration; if they do not overlap then the sites will "snap together" at the beginning of the
+   configuration; if they do not overlap then the sites will "snap together" at the beginning of the simulation. Second,
-   simulation. Second, changing the site positions in ``mjModel.site_pos`` at runtime will correctly change the position
+   changing the site positions in ``mjModel.site_pos`` at runtime will correctly change the position of the constraint
-   of the constraint (i.e. the content of ``mjModel.eq_data`` has no effect when this semantic is used).
+   (i.e. the content of ``mjModel.eq_data`` has no effect when this semantic is used).
 
 .. _equality-connect-site2:
 
@@ -4442,7 +4443,15 @@ This element creates a weld equality constraint. It attaches two bodies to each
 freedom between them (softly of course, like all other constraints in MuJoCo). The two bodies are not required to be
 close to each other. The relative body position and orientation being enforced by the constraint solver is the one in
 which the model was defined. Note that two bodies can also be welded together rigidly, by defining one body as a child
-of the other body, without any joint elements in the child body.
+of the other body, without any joint elements in the child body. Weld constraints can be specified in one of two ways:
+
+- Using :ref:`body1<equality-weld-body1>` (and optionally :ref:`anchor<equality-weld-anchor>`,
+  :ref:`relpose<equality-weld-relpose>`, :ref:`body2<equality-weld-body2>`). When using this specification, the
+  constraint is assumed to be satisfied at the configuration in which the model is defined.
+- :ref:`site1<equality-weld-site1>` and :ref:`site2<equality-weld-site2>` (both required). When using this
+  specification, the frames of the two sites will be aligned by the constraint, regardless of their position in the
+  default configuration. An example of this specification is shown in
+  `this model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__.
 
 .. _equality-weld-name:
 
@@ -4459,8 +4468,9 @@ of the other body, without any joint elements in the child body.
 
 .. _equality-weld-body1:
 
-:at:`body1`: :at-val:`string, required`
+:at:`body1`: :at-val:`string, optional`
-   Name of the first body.
+   Name of the first body participating in the constraint. Either this attribute and must be specified or :at:`site1`
+   and :at:`site2` must be specified.
 
 .. _equality-weld-body2:
 
@@ -4483,6 +4493,23 @@ of the other body, without any joint elements in the child body.
    this parameter is the same as for connect constraints, except that is relative to body2. If :at:`relpose` is
    specified, body1 will use the pose to compute its anchor point.
 
+.. _equality-weld-site1:
+
+:at:`site1`: :at-val:`string, optional`
+   Name of a site belonging to the first body participating in the constraint. When specified, :at:`site2` must also be
+   specified. The (:at:`site1`, :at:`site2`) specification is a more flexible alternative to the body-based
+   specification, and is different in two ways. First, the sites are not required to overlap at the default
+   configuration; if they do not overlap then the sites will "snap together" at the beginning of the simulation. Second,
+   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.e. the content of ``mjModel.eq_data`` has no
+   effect when this semantic is used, with the exception of :ref:`torquescale<equality-weld-torquescale>`).
+
+.. _equality-weld-site2:
+
+:at:`site2`: :at-val:`string, optional`
+   Name of a site belonging to the second body participating in the constraint. When specified, :at:`site1` must also be
+   specified. See the :ref:`site1<equality-weld-site1>` description for more details.
+
 .. _equality-weld-torquescale:
 
 :at:`torquescale`: :at-val:`real, "1"`

doc/XMLschema.rst

@@ -642,9 +642,9 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`name<equality-weld-name>`                                 | :ref:`class<equality-weld-class>`                               | :ref:`body1<equality-weld-body1>`                               | :ref:`body2<equality-weld-body2>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`relpose<equality-weld-relpose>`                           | :ref:`anchor<equality-weld-anchor>`                             | :ref:`active<equality-weld-active>`                             | :ref:`solref<equality-weld-solref>`                             | |
+|                                    |    |    | :ref:`relpose<equality-weld-relpose>`                           | :ref:`anchor<equality-weld-anchor>`                             | :ref:`site1<equality-weld-site1>`                               | :ref:`site2<equality-weld-site2>`                               | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`solimp<equality-weld-solimp>`                             | :ref:`torquescale<equality-weld-torquescale>`                   |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`active<equality-weld-active>`                             | :ref:`solref<equality-weld-solref>`                             | :ref:`solimp<equality-weld-solimp>`                             | :ref:`torquescale<equality-weld-torquescale>`                   | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| equality |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -2,7 +2,7 @@
 Changelog
 =========
 
-Version 3.2.3 (Sep 12, 2024)
+Version 3.2.3 (Sep 16, 2024)
 ----------------------------
 
 General
@@ -24,15 +24,23 @@ General
    detection is handled with a new native code path, rather than `libccd <https://github.com/danfis/libccd>`__.
    This feature is in early stages of testing, but users who've experienced issues related to collsion detection are
    welcome to experiment with it and report any issues.
-5. Added a new way of defining :ref:`connect<equality-connect>` equality constraints, using two sites rather than bodies.
+
-   The new semantic is useful when the assumption that the constraint is satisfied in the base configuration does not
+.. youtube:: kcM_oauk3ZA
-   hold. In this case the sites will "snap together" at the beginning of the simulation. Additionally, changing the site
+   :align: right
-   positions in ``mjModel.site_pos`` at runtime can be used to modify the constraint.
+   :width: 240px
+
+5. Added a new way of defining :ref:`connect<equality-connect>` and :ref:`weld<equality-weld>` equality constraints,
+   using two sites. The new semantic is useful when the assumption that the constraint is satisfied
+   in the base configuration does not hold. In this case the sites will "snap together" at the beginning of the
+   simulation. Additionally, changing the site positions (``mjModel.site_pos``) and orientations
+   ( ``mjModel.site_quat``) at runtime will correctly modify the constraint definition. This
+   `example model <https://github.com/google-deepmind/mujoco/blob/main/test/engine/testdata/equality_site.xml>`__ using
+   the new semantic is shown in the video on the right.
 6. Introduced **free joint alignment**, an optimization that applies to bodies with a free joint and no child bodies
    (simple free-floating bodies): automatically aligning the body frame with the inertial frame. This feature can be
    toggled individually using the :ref:`freejoint/align<body-freejoint-align>` attribute or globally using the compiler
-   :ref:`alignfree<compiler-alignfree>` attribute. The alignment diagonalizes the related 6x6 inertia sub-matrix, leading
+   :ref:`alignfree<compiler-alignfree>` attribute. The alignment diagonalizes the related 6x6 inertia sub-matrix,
-   to both faster simulation and more stable simulation of free bodies.
+   leading to both faster simulation and more stable simulation of free bodies.
 
    While this optimization is a strict improvement, it changes the semantics of the joint's degrees-of-freedom.
    Therefore, ``qpos`` and ``qvel`` values saved in older versions (for example, in :ref:`keyframes<keyframe>`) will

src/engine/engine_core_constraint.c

@@ -542,11 +542,22 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         break;
 
       case mjEQ_WELD:                 // fix relative position and orientation
-        // find global points
+        // find global points, body semantic
-        for (int j=0; j < 2; j++) {
+        if (m->eq_objtype[i] == mjOBJ_BODY) {
-          mjtNum* anchor = data + 3*(1-j);
+          for (int j=0; j < 2; j++) {
-          mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor);
+            mjtNum* anchor = data + 3*(1-j);
-          mju_addTo3(pos[j], d->xpos + 3*id[j]);
+            mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor);
+            mju_addTo3(pos[j], d->xpos + 3*id[j]);
+            body_id[j] = id[j];
+          }
+        }
+
+        // find global points, site semantic
+        else {
+          for (int j=0; j < 2; j++) {
+            mju_copy3(pos[j], d->site_xpos + 3*id[j]);
+            body_id[j] = m->site_bodyid[id[j]];
+          }
         }
 
         // compute position error
@@ -556,7 +567,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         mjtNum torquescale = data[10];
 
         // compute error Jacobian (opposite of contact: 0 - 1)
-        NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
+        NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0],
                            jac[1], jac[0], jacdif,
                            jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv);
 
@@ -564,11 +575,23 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         mju_copy(jac[0], jacdif, 3*NV);
         mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
 
-        // compute orientation error: neg(q1) * q0 * relpose (axis components only)
+        // orientation, body semantic
-        mjtNum* relpose = data+6;
+        if (m->eq_objtype[i] == mjOBJ_BODY) {
-        mju_mulQuat(quat, d->xquat+4*id[0], relpose);   // quat = q0*relpose
+          // compute orientation error: neg(q1) * q0 * relpose (axis components only)
-        mju_negQuat(quat1, d->xquat+4*id[1]);           // quat1 = neg(q1)
+          mjtNum* relpose = data+6;
-        mju_mulQuat(quat2, quat1, quat);                // quat2 = neg(q1)*q0*relpose
+          mju_mulQuat(quat, d->xquat+4*id[0], relpose);   // quat = q0*relpose
+          mju_negQuat(quat1, d->xquat+4*id[1]);           // quat1 = neg(q1)
+        }
+
+        // orientation, site semantic
+        else {
+          mjtNum quat_site1[4];
+          mju_mulQuat(quat, d->xquat+4*body_id[0], m->site_quat+4*id[0]);
+          mju_mulQuat(quat_site1, d->xquat+4*body_id[1], m->site_quat+4*id[1]);
+          mju_negQuat(quat1, quat_site1);
+        }
+
+        mju_mulQuat(quat2, quat1, quat);
         mju_scl3(cpos+3, quat2+1, torquescale);         // scale axis components by torquescale
 
         // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose

src/engine/engine_io.c

@@ -2239,6 +2239,7 @@ const char* mj_validateReferences(const mjModel* m) {
   for (int i=0; i < m->neq; i++) {
     int obj1id = m->eq_obj1id[i];
     int obj2id = m->eq_obj2id[i];
+    int objtype = m->eq_objtype[i];
     switch ((mjtEq) m->eq_type[i]) {
     case mjEQ_JOINT:
       if (obj1id >= m->njnt || obj1id < 0) {
@@ -2262,11 +2263,22 @@ const char* mj_validateReferences(const mjModel* m) {
 
     case mjEQ_WELD:
     case mjEQ_CONNECT:
-      if (obj1id >= m->nbody || obj1id < 0) {
+      if (objtype == mjOBJ_BODY) {
-        return "Invalid model: eq_obj1id out of bounds.";
+        if (obj1id >= m->nbody || obj1id < 0) {
-      }
+          return "Invalid model: eq_obj1id out of bounds.";
-      if (obj2id >= m->nbody || obj2id < 0) {
+        }
-        return "Invalid model: eq_obj2id out of bounds.";
+        if (obj2id >= m->nbody || obj2id < 0) {
+          return "Invalid model: eq_obj2id out of bounds.";
+        }
+      } else if (objtype == mjOBJ_SITE) {
+        if (obj1id >= m->nsite || obj1id < 0) {
+          return "Invalid model: eq_obj1id out of bounds.";
+        }
+        if (obj2id >= m->nsite || obj2id < 0) {
+          return "Invalid model: eq_obj2id out of bounds.";
+        }
+      } else {
+        return "Invalid model: eq_objtype is not body or site.";
       }
       break;
 

src/engine/engine_setconst.c

@@ -307,26 +307,33 @@ static void set0(mjModel* m, mjData* d) {
 
     // weld constraint
     else if (m->eq_type[i] == mjEQ_WELD) {
-      // skip if user has set any quaternion data
+      switch ((mjtObj) m->eq_objtype[i]) {
-      if (m->eq_data[mjNEQDATA*i+6] ||
+        case mjOBJ_BODY: {
-          m->eq_data[mjNEQDATA*i+7] ||
+          // skip if user has set any quaternion data
-          m->eq_data[mjNEQDATA*i+8] ||
+          if (!mju_isZero(m->eq_data + mjNEQDATA*i + 6, 4)) {
-          m->eq_data[mjNEQDATA*i+9]) {
+            // normalize quaternion just in case
-        // normalize quaternion just in case
+            mju_normalize4(m->eq_data+mjNEQDATA*i+6);
-        mju_normalize4(m->eq_data+mjNEQDATA*i+6);
+            continue;
-        continue;
+          }
+
+          // anchor position is in body2 local frame
+          mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
+
+          // data[3-5] = anchor position in body1 local frame
+          mju_subFrom3(pos, d->xpos+3*id1);
+          mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
+
+          // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
+          mju_negQuat(quat, d->xquat+4*id1);
+          mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
+          break;
+        }
+        case mjOBJ_SITE: {
+          break;
+        }
+        default:
+          mjERROR("invalid objtype in weld constraint %d", i);
       }
-
-      // anchor position is in body2 local frame
-      mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
-
-      // data[3-5] = anchor position in body1 local frame
-      mju_subFrom3(pos, d->xpos+3*id1);
-      mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
-
-      // data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
-      mju_negQuat(quat, d->xquat+4*id1);
-      mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
     }
   }
 

src/engine/engine_vis_visualize.c

@@ -2026,29 +2026,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
       int is_connect = m->eq_type[i] == mjEQ_CONNECT;
       if (d->eq_active[i] && (is_connect || is_weld)) {
         // compute endpoints in global coordinates
+        mjtNum *xmat_j, *xmat_k;
         int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
-        if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) {
+        if (m->eq_objtype[i] == mjOBJ_SITE) {
           mju_copy3(vec, d->site_xpos+3*j);
           mju_copy3(end, d->site_xpos+3*k);
+          xmat_j = d->site_xmat+9*j;
+          xmat_k = d->site_xmat+9*k;
         } else {
           mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*is_weld);
           mju_addTo3(vec, d->xpos+3*j);
           mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*is_connect);
           mju_addTo3(end, d->xpos+3*k);
+          xmat_j = d->xmat+9*j;
+          xmat_k = d->xmat+9*k;
         }
 
         // construct geom
         sz[0] = scl * m->vis.scale.constraint;
 
         START
-        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect);
+        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect);
         if (vopt->label == mjLABEL_CONSTRAINT) {
           makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
         }
         FINISH
 
         START
-        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint);
+        mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint);
         if (vopt->label == mjLABEL_CONSTRAINT) {
           makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
         }

src/user/user_objects.cc

@@ -4920,7 +4920,10 @@ void mjCEquality::ResolveReferences(const mjCModel* m) {
 
   // determine object type
   if (type==mjEQ_WELD) {
-    object_type = mjOBJ_BODY;
+    if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
+      throw mjCError(this, "weld constraint supports only sites and bodies");
+    }
+    object_type = objtype;
   } else if (type==mjEQ_CONNECT) {
     if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
       throw mjCError(this, "connect constraint supports only sites and bodies");

src/xml/xml_native_reader.cc

@@ -365,8 +365,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
     {"<"},
         {"connect", "*", "10", "name", "class", "body1", "body2", "anchor",
             "site1", "site2", "active", "solref", "solimp"},
-        {"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor",
+        {"weld", "*", "12", "name", "class", "body1", "body2", "relpose", "anchor",
-            "active", "solref", "solimp", "torquescale"},
+            "site1", "site2", "active", "solref", "solimp", "torquescale"},
         {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
             "active", "solref", "solimp"},
         {"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
@@ -1934,13 +1934,50 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
       }
       break;
 
-    case mjEQ_WELD:
+    case mjEQ_WELD: {
-      ReadAttrTxt(elem, "body1", name1, true);
+        auto maybe_site1 = ReadAttrStr(elem, "site1");
-      ReadAttrTxt(elem, "body2", name2);
+        auto maybe_site2 = ReadAttrStr(elem, "site2");
-      ReadAttr(elem, "relpose", 7, equality->data+3, text);
+        auto maybe_body1 = ReadAttrStr(elem, "body1");
-      ReadAttr(elem, "torquescale", 1, equality->data+10, text);
+        auto maybe_body2 = ReadAttrStr(elem, "body2");
-      if (!ReadAttr(elem, "anchor", 3, equality->data, text)) {
+        bool has_anchor = ReadAttr(elem, "anchor", 3, equality->data, text);
-        mjuu_zerovec(equality->data, 3);
+        bool has_relpose = ReadAttr(elem, "relpose", 7, equality->data+3, text);
+
+        bool maybe_site = maybe_site1.has_value() || maybe_site2.has_value();
+        bool maybe_body = maybe_body1.has_value() ||
+                          maybe_body2.has_value() ||
+                          has_anchor              ||
+                          has_relpose;
+
+        if (maybe_site && maybe_body) {
+          throw mjXError(elem, "body and site semantics cannot be mixed");
+        }
+
+        bool site_semantic = maybe_site1.has_value() && maybe_site2.has_value();
+        bool body_semantic = maybe_body1.has_value();
+
+        if (site_semantic == body_semantic) {
+          throw mjXError(
+              elem,
+              "either body1 must be defined and optionally {body2, anchor, relpose},"
+              " or site1 and site2 must be defined");
+        }
+
+        if (body_semantic) {
+          name1 = maybe_body1.value();
+          if (maybe_body2.has_value()) {
+            name2 = maybe_body2.value();
+          }
+          equality->objtype = mjOBJ_BODY;
+          if (!has_anchor) {
+            mjuu_zerovec(equality->data, 3);
+          }
+        } else {
+          name1 = maybe_site1.value();
+          name2 = maybe_site2.value();
+          equality->objtype = mjOBJ_SITE;
+        }
+
+        ReadAttr(elem, "torquescale", 1, equality->data+10, text);
       }
       break;
 

src/xml/xml_native_writer.cc

@@ -631,11 +631,16 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
       break;
 
     case mjEQ_WELD:
-      WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
+      if (equality->objtype == mjOBJ_BODY) {
-      WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
+        WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
-      WriteAttr(elem, "anchor", 3, equality->data);
+        WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
+        WriteAttr(elem, "anchor", 3, equality->data);
+        WriteAttr(elem, "relpose", 7, equality->data+3);
+      } else {
+        WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
+        WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
+      }
       WriteAttr(elem, "torquescale", 1, equality->data+10);
-      WriteAttr(elem, "relpose", 7, equality->data+3);
       break;
 
     case mjEQ_JOINT:

test/engine/testdata/equality_site.xml

@@ -0,0 +1,48 @@
+<mujoco model="Connect and Weld defined with sites">
+  <option viscosity="1">
+    <flag contact="disable"/>
+  </option>
+
+  <visual>
+    <scale framewidth="0.05" framelength="0.4"/>
+  </visual>
+
+  <default>
+    <default class="free">
+      <geom type="box" size=".2" fromto=".2 .2 0 .2 .2 -2" rgba=".4 .7 .6 1"/>
+    </default>
+    <site rgba=".0 .7 0 1"/>
+  </default>
+
+  <worldbody>
+    <geom pos="0 0 -2" type="plane" size="10 10 .01"/>
+    <light pos="0 0 20"/>
+
+    <site name="a1" pos="-2 0 0" euler="-90 0 0"/>
+    <body pos="-2 1 0">
+      <freejoint/>
+      <geom class="free"/>
+      <site name="a2"/>
+    </body>
+
+    <site name="b1" euler="-90 0 0"/>
+    <body pos="0 1 0">
+      <freejoint/>
+      <geom class="free"/>
+      <site name="b2"/>
+    </body>
+
+    <site name="c1" pos="2 0 0" euler="-90 0 0"/>
+    <body pos="2 1 0">
+      <freejoint/>
+      <geom class="free"/>
+      <site name="c2"/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <connect name="connect" site1="a1" site2="a2"/>
+    <weld    name="weak weld" site1="b1" site2="b2" torquescale="0.2"/>
+    <weld    name="weld" site1="c1" site2="c2"/>
+  </equality>
+</mujoco>

test/engine/testdata/weld.xml

@@ -47,6 +47,16 @@
       <freejoint/>
       <geom class="free"/>
     </body>
+
+    <body name="box5" pos="5 0 0">
+      <geom type="box" class="static"/>
+      <site name="box5"/>
+    </body>
+    <body name="beam5" pos="5 0 0">
+      <freejoint/>
+      <geom class="free"/>
+      <site name="beam5" pos="0 -1 0" euler="45 20 5"/>
+    </body>
   </worldbody>
 
   <equality>
@@ -54,5 +64,6 @@
     <weld name="anchor no torques" body1="box2" body2="beam2" torquescale="0" anchor="0 -2 0"/>
     <weld name="relpose"           body1="box3" body2="beam3" relpose="0 0 0 1 -.3 0 0"/>
     <weld name="relpose+anchor"    body1="box4" body2="beam4" relpose="0 0 0 1 -.3 0 0" anchor="0 0 -1"/>
+    <weld name="site"              site1="box5" site2="beam5"/>
   </equality>
 </mujoco>

test/user/user_objects_test.cc

@@ -2328,14 +2328,14 @@ TEST_F(UserObjectsTest, BadConnect) {
   string xml = base.replace(pos, len, "<connect body1='1' anchor='0 0 1'/>");
   char error[1024];
   mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
-  EXPECT_THAT(m, NotNull()) << error;
+  ASSERT_THAT(m, NotNull()) << error;
   EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 1, 0, 0, 2));
   mj_deleteModel(m);
 
   // good model using site semantic
   xml = base.replace(pos, len, "<connect site1='0' site2='1'/>");
   m = LoadModelFromString(xml.c_str(), error, sizeof(error));
-  EXPECT_THAT(m, NotNull()) << error;
+  ASSERT_THAT(m, NotNull()) << error;
   EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
   mj_deleteModel(m);
 
@@ -2345,7 +2345,7 @@ TEST_F(UserObjectsTest, BadConnect) {
   // bad model (missing anchor)
   xml = base.replace(pos, len, "<connect body1='1'/>");
   m = LoadModelFromString(xml.c_str(), error, sizeof(error));
-  EXPECT_THAT(m, IsNull());
+  ASSERT_THAT(m, IsNull());
   EXPECT_THAT(error, HasSubstr(error_missing));
 
   char error_mixed[] = "body and site semantics cannot be mixed"
@@ -2354,13 +2354,13 @@ TEST_F(UserObjectsTest, BadConnect) {
   // bad model (mixing body and site)
   xml = base.replace(pos, len, "<connect body1='1' site1='1'/>");
   m = LoadModelFromString(xml.c_str(), error, sizeof(error));
-  EXPECT_THAT(m, IsNull());
+  ASSERT_THAT(m, IsNull());
   EXPECT_THAT(error, HasSubstr(error_mixed));
 
   // load spec with no constraints
   xml = base.erase(pos, len);
   mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
-  EXPECT_THAT(s, NotNull()) << error;
+  ASSERT_THAT(s, NotNull()) << error;
 
   // add a connect but don't set objtype
   mjsEquality* equality = mjs_addEquality(s, nullptr);
@@ -2370,14 +2370,119 @@ TEST_F(UserObjectsTest, BadConnect) {
 
   // expect compilation to fail
   m = mj_compile(s, nullptr);
-  EXPECT_THAT(m, IsNull());
+  ASSERT_THAT(m, IsNull());
   EXPECT_THAT(mjs_getError(s),
               HasSubstr("connect constraint supports only sites and bodies"));
 
   // set objtype, expect compilation to succeed
   equality->objtype = mjOBJ_SITE;
   m = mj_compile(s, nullptr);
-  EXPECT_THAT(m, NotNull()) << mjs_getError(s);
+  ASSERT_THAT(m, NotNull()) << mjs_getError(s);
+  mj_deleteModel(m);
+  mj_deleteSpec(s);
+}
+
+TEST_F(UserObjectsTest, BadWeld) {
+  string base = R"(
+  <mujoco>
+    <worldbody>
+      <site name="0" size="1"/>
+      <body name="1" pos="0 0 1">
+        <freejoint/>
+        <geom size="1"/>
+        <site name="1"/>
+      </body>
+    </worldbody>
+    <equality>
+      WELD
+    </equality>
+  </mujoco>
+  )";
+  int pos = base.find("WELD");
+  int len = 4;
+
+  // good model using body semantic
+  string xml = base.replace(pos, len, "<weld body1='1' anchor='0 0 1' torquescale='2'/>");
+  char error[1024];
+  mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  EXPECT_THAT(AsVector(m->eq_data, 10),
+              ElementsAre(0, 0, 1, 0, 0, 0, 1, 0, 0, 0));
+  mj_deleteModel(m);
+
+  // good model using site semantic
+  xml = base.replace(pos, len, "<weld site1='0' site2='1' torquescale='2'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, NotNull()) << error;
+  EXPECT_THAT(AsVector(m->eq_data, 10),
+              ElementsAre(0, 1, 0, 0, 0, 0, 0, 0, 0, 0));
+  mj_deleteModel(m);
+
+  char error_mixed[] =
+      "body and site semantics cannot be mixed"
+      "\nElement 'weld', line 12";
+
+  // bad model (mixing body and site)
+  xml = base.replace(pos, len, "<weld body1='1' site1='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_mixed));
+
+  // bad model (mixing site and anchor)
+  xml = base.replace(pos, len, "<weld anchor='0 0 1' site1='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_mixed));
+
+  // bad model (mixing site and relpose)
+  xml = base.replace(pos, len, "<weld relpose='0 0 0 1 0 0 0' site1='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_mixed));
+
+  // bad model (body and site semantics are valid, but both are specified)
+  xml = base.replace(pos, len, "<weld body1='1' site1='1' site2='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_mixed));
+
+  char error_underspecified[] =
+      "either body1 must be defined and optionally {body2, anchor, "
+      "relpose}, or site1 and site2 must be defined\nElement 'weld', line 12";
+
+  // bad model (underspecified body semantics)
+  xml = base.replace(pos, len, "<weld anchor='0 0 1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_underspecified));
+
+  // bad model (underspecified site semantics)
+  xml = base.replace(pos, len, "<weld site2='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_underspecified));
+
+  // load spec with no constraints
+  xml = base.erase(pos, len);
+  mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
+  ASSERT_THAT(s, NotNull()) << error;
+
+  // add a weld but don't set objtype
+  mjsEquality* equality = mjs_addEquality(s, nullptr);
+  equality->type = mjEQ_WELD;
+  mjs_setString(equality->name1, "0");
+  mjs_setString(equality->name2, "1");
+
+  // expect compilation to fail
+  m = mj_compile(s, nullptr);
+  ASSERT_THAT(m, IsNull());
+  EXPECT_THAT(mjs_getError(s),
+              HasSubstr("weld constraint supports only sites and bodies"));
+
+  // set objtype, expect compilation to succeed
+  equality->objtype = mjOBJ_SITE;
+  m = mj_compile(s, nullptr);
+  ASSERT_THAT(m, NotNull()) << mjs_getError(s);
   mj_deleteModel(m);
   mj_deleteSpec(s);
 }