Allow connect constraints to be specified using sites.

PiperOrigin-RevId: 668925419
Change-Id: I10cb71e6706e617c830cd90d6d0836b94a5a8cfa

doc/XMLreference.rst

@@ -4347,9 +4347,15 @@ all equality constraint types, thus we document them only once, under the :ref:`
 :el-prefix:`equality/` |-| **connect** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This element creates an equality constraint that connects two bodies at a point. The point is not necessarily within the
+This element creates an equality constraint that connects two bodies at a point. The constraint effectively defines a
-geoms volumes of either body. This constraint can be used to define ball joints outside the kinematic tree.
+ball joint outside the kinematic tree. Connect constraints can be specified in one of two ways
 
+- Using :ref:`body1<equality-connect-body1>` and :ref:`anchor<equality-connect-anchor>` (both required) and
+  optionally :ref:`body2<equality-connect-body2>`. When using this specification, the constraint is assumed to be
+  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.
 
 .. _equality-connect-name:
 
@@ -4377,8 +4383,9 @@ geoms volumes of either body. This constraint can be used to define ball joints
 
 .. _equality-connect-body1:
 
-:at:`body1`: :at-val:`string, required`
+:at:`body1`: :at-val:`string, optional`
-   Name of the first body participating in the constraint.
+   Name of the first body participating in the constraint. Either this attribute and :at:`anchor` must be specified, or
+   :at:`site1` and :at:`site2` must be specified.
 
 .. _equality-connect-body2:
 
@@ -4388,15 +4395,26 @@ geoms volumes of either body. This constraint can be used to define ball joints
 
 .. _equality-connect-anchor:
 
-:at:`anchor`: :at-val:`real(3), required`
+:at:`anchor`: :at-val:`real(3), optional`
-   Coordinates of the 3D anchor point where the two bodies are connected. In the compiled mjModel the anchor is stored
+   Coordinates of the 3D anchor point where the two bodies are connected, in the local coordinate frame of :at:`body1`.
-   twice, relative to the local frame of each body. At runtime this yields two global points computed by forward
+   The constraint is assumed to be satisfied in the configuration in which the model is defined, which lets the compiler
-   kinematics; the constraint solver pushes these points towards each other. In the MJCF model however only one point is
+   compute the associated anchor point for :at:`body2`.
-   given. We assume that the equality constraint is exactly satisfied in the configuration in which the model is defined
-   (this applies to all other constraint types as well). The compiler uses the single anchor specified in the MJCF model
-   to compute the two body-relative anchor points in mjModel. Specified relative to the local coordinate frame of
-   the *first* body.
 
+.. _equality-connect-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 (:at:`body1`,
+   :at:`anchor`) 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 positions in ``mjModel.site_pos`` at runtime will correctly change the position
+   of the constraint (i.e. the content of ``mjModel.eq_data`` has no effect when this semantic is used).
+
+.. _equality-connect-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-connect-site1>` description for more details.
 
 .. _equality-weld:
 

doc/XMLschema.rst

@@ -631,7 +631,9 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`name<equality-connect-name>`                              | :ref:`class<equality-connect-class>`                            | :ref:`body1<equality-connect-body1>`                            | :ref:`body2<equality-connect-body2>`                            | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`anchor<equality-connect-anchor>`                          | :ref:`active<equality-connect-active>`                          | :ref:`solref<equality-connect-solref>`                          | :ref:`solimp<equality-connect-solimp>`                          | |
+|                                    |    |    | :ref:`anchor<equality-connect-anchor>`                          | :ref:`site1<equality-connect-site1>`                            | :ref:`site2<equality-connect-site2>`                            | :ref:`active<equality-connect-active>`                          | |
+|                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+|                                    |    |    | :ref:`solref<equality-connect-solref>`                          | :ref:`solimp<equality-connect-solimp>`                          |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| equality |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |

doc/changelog.rst

@@ -16,6 +16,10 @@ General
      the :ref:`mjOption` struct. This is because the new convex collision detection pipeline (see below) does not use
      the MPR algorithm. The semantics of these options remain identical.
 
+- 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
+  hold. In this case the sites will "snap together" at the beginning of the simulation. Additionally, changing the site
+  positions in ``mjModel.site_pos`` at runtime can be used to modify the constraint.
 - Added the :ref:`nativeccd<option-flag-nativeccd>` flag. When this flag is enabled, general convex collision
   detection is handled natively, as opposed to using `libccd <https://github.com/danfis/libccd>`__. This feature is in
   early stages of testing.
@@ -24,7 +28,7 @@ General
 - When :ref:`attaching<meAttachment>` sub-models, :ref:`keyframes<keyframe>` will now be correctly merged into the
   parent model, but only on the first attachment.
 - Added the :ref:`mjtSameFrame` enum which contains the possible frame alignments of bodies and their children. These
-  alignments are used as shortcuts in :ref:`mj_kinematics`.
+  alignments are used for computation shortcuts in :ref:`mj_kinematics`.
 
 MJX
 ^^^

doc/includes/references.h

@@ -1269,6 +1269,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
+  int*      eq_objtype;           // type of both objects (mjtObj)            (neq x 1)
   mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)
@@ -2090,6 +2091,7 @@ typedef struct mjsEquality_ {      // equality specification
   mjtByte active;                  // is equality initially active
   mjString* name1;                 // name of object 1
   mjString* name2;                 // name of object 2
+  mjtObj objtype;                  // type of both objects
   mjtNum solref[mjNREF];           // solver reference
   mjtNum solimp[mjNIMP];           // solver impedance
   mjString* info;                  // message appended to errors
@@ -2991,6 +2993,7 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
+    int* eq_objtype;
     mjtNum* eq_data;
 
     int* tendon_num;

include/mujoco/mjmodel.h

@@ -985,6 +985,7 @@ struct mjModel_ {
   int*      eq_type;              // constraint type (mjtEq)                  (neq x 1)
   int*      eq_obj1id;            // id of object 1                           (neq x 1)
   int*      eq_obj2id;            // id of object 2                           (neq x 1)
+  int*      eq_objtype;           // type of both objects (mjtObj)            (neq x 1)
   mjtByte*  eq_active0;           // initial enable/disable constraint state  (neq x 1)
   mjtNum*   eq_solref;            // constraint solver reference              (neq x mjNREF)
   mjtNum*   eq_solimp;            // constraint solver impedance              (neq x mjNIMP)

include/mujoco/mjspec.h

@@ -578,6 +578,7 @@ typedef struct mjsEquality_ {      // equality specification
   mjtByte active;                  // is equality initially active
   mjString* name1;                 // name of object 1
   mjString* name2;                 // name of object 2
+  mjtObj objtype;                  // type of both objects
   mjtNum solref[mjNREF];           // solver reference
   mjtNum solimp[mjNIMP];           // solver impedance
   mjString* info;                  // message appended to errors

include/mujoco/mjvisualize.h

@@ -578,6 +578,7 @@ struct mjvSceneState_ {
     int* eq_type;
     int* eq_obj1id;
     int* eq_obj2id;
+    int* eq_objtype;
     mjtNum* eq_data;
 
     int* tendon_num;

include/mujoco/mjxmacro.h

@@ -438,6 +438,7 @@
     XMJV( int,     eq_type,               neq,           1                    ) \
     XMJV( int,     eq_obj1id,             neq,           1                    ) \
     XMJV( int,     eq_obj2id,             neq,           1                    ) \
+    XMJV( int,     eq_objtype,            neq,           1                    ) \
     X   ( mjtByte, eq_active0,            neq,           1                    ) \
     X   ( mjtNum,  eq_solref,             neq,           mjNREF               ) \
     X   ( mjtNum,  eq_solimp,             neq,           mjNIMP               ) \

introspect/structs.py

@@ -3088,6 +3088,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='id of object 2                           (neq x 1)',
              ),
+             StructFieldDecl(
+                 name='eq_objtype',
+                 type=PointerType(
+                     inner_type=ValueType(name='int'),
+                 ),
+                 doc='type of both objects (mjtObj)            (neq x 1)',
+             ),
              StructFieldDecl(
                  name='eq_active0',
                  type=PointerType(
@@ -7217,6 +7224,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                              ),
                              doc='',
                          ),
+                         StructFieldDecl(
+                             name='eq_objtype',
+                             type=PointerType(
+                                 inner_type=ValueType(name='int'),
+                             ),
+                             doc='',
+                         ),
                          StructFieldDecl(
                              name='eq_data',
                              type=PointerType(
@@ -10305,6 +10319,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
                  ),
                  doc='name of object 2',
              ),
+             StructFieldDecl(
+                 name='objtype',
+                 type=ValueType(name='mjtObj'),
+                 doc='type of both objects',
+             ),
              StructFieldDecl(
                  name='solref',
                  type=ArrayType(

src/engine/engine_core_constraint.c

@@ -505,21 +505,33 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
       size = 0;
       NV = 0;
       NV2 = 0;
+      int body_id[2];
 
       // process according to type
       switch ((mjtEq) m->eq_type[i]) {
       case mjEQ_CONNECT:              // connect bodies with ball joint
-        // find global points
+        // find global points, body semantic
-        for (int j=0; j < 2; j++) {
+        if (m->eq_objtype[i] == mjOBJ_BODY) {
-          mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j);
+          for (int j=0; j < 2; j++) {
-          mju_addTo3(pos[j], d->xpos + 3*id[j]);
+            mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j);
+            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
         mju_sub3(cpos, pos[0], pos[1]);
 
         // compute Jacobian difference (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, NULL, NULL, NULL);
 
         // copy difference into jac[0]

src/engine/engine_setconst.c

@@ -286,12 +286,22 @@ static void set0(mjModel* m, mjData* d) {
 
     // connect constraint
     if (m->eq_type[i] == mjEQ_CONNECT) {
-      // pos = anchor position in global frame
+      switch ((mjtObj) m->eq_objtype[i]) {
-      mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
+        case mjOBJ_BODY:
+          // pos = anchor position in global frame
+          mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
 
-      // data[3-5] = anchor position in body2 local frame
+          // data[3-5] = anchor position in body2 local frame
-      mju_subFrom3(pos, d->xpos+3*id2);
+          mju_subFrom3(pos, d->xpos+3*id2);
-      mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
+          mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
+          break;
+        case mjOBJ_SITE:
+          // site-based connect, eq_data is unused
+          mju_zero(m->eq_data+mjNEQDATA*i, mjNEQDATA);
+          break;
+        default:
+          mjERROR("invalid objtype in connect constraint %d", i);
+      }
     }
 
     // weld constraint

src/engine/engine_vis_visualize.c

@@ -2021,13 +2021,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
   if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
     // connect or weld
     for (int i=0; i < m->neq; i++) {
-      if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
+      int is_weld = m->eq_type[i] == mjEQ_WELD;
+      int is_connect = m->eq_type[i] == mjEQ_CONNECT;
+      if (d->eq_active[i] && (is_connect || is_weld)) {
         // compute endpoints in global coordinates
         int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
-        mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD));
+        if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) {
-        mju_addTo3(vec, d->xpos+3*j);
+          mju_copy3(vec, d->site_xpos+3*j);
-        mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT));
+          mju_copy3(end, d->site_xpos+3*k);
-        mju_addTo3(end, d->xpos+3*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);
+        }
 
         // construct geom
         sz[0] = scl * m->vis.scale.constraint;

src/user/user_model.cc

@@ -2727,6 +2727,7 @@ void mjCModel::CopyObjects(mjModel* m) {
     m->eq_type[i] = peq->type;
     m->eq_obj1id[i] = peq->obj1id;
     m->eq_obj2id[i] = peq->obj2id;
+    m->eq_objtype[i] = peq->objtype;
     m->eq_active0[i] = peq->active;
     mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF);
     mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP);

src/user/user_objects.cc

@@ -4793,65 +4793,64 @@ void mjCEquality::CopyFromSpec() {
 
 
 void mjCEquality::ResolveReferences(const mjCModel* m) {
-  mjtObj objtype;
+  mjtObj object_type;
   mjCBase *px1, *px2;
   mjtJoint jt1, jt2;
 
   // determine object type
-  if (type==mjEQ_CONNECT || type==mjEQ_WELD) {
+  if (type==mjEQ_WELD) {
-    objtype = mjOBJ_BODY;
+    object_type = mjOBJ_BODY;
+  } else if (type==mjEQ_CONNECT) {
+    if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
+      throw mjCError(this, "connect constraint supports only sites and bodies");
+    }
+    object_type = objtype;
   } else if (type==mjEQ_JOINT) {
-    objtype = mjOBJ_JOINT;
+    object_type = mjOBJ_JOINT;
   } else if (type==mjEQ_TENDON) {
-    objtype = mjOBJ_TENDON;
+    object_type = mjOBJ_TENDON;
   } else if (type==mjEQ_FLEX) {
-    objtype = mjOBJ_FLEX;
+    object_type = mjOBJ_FLEX;
   } else {
     throw mjCError(this, "invalid type in equality constraint");
   }
 
   // find object 1, get id
-  px1 = m->FindObject(objtype, name1_);
+  px1 = m->FindObject(object_type, name1_);
   if (!px1) {
-    throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id);
+    throw mjCError(this, "unknown element '%s' in equality constraint", name1_.c_str());
   }
   obj1id = px1->id;
 
   // find object 2, get id
   if (!name2_.empty()) {
-    px2 = m->FindObject(objtype, name2_);
+    px2 = m->FindObject(object_type, name2_);
     if (!px2) {
       throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id);
     }
     obj2id = px2->id;
-  }
+  } else {
-
+    // object 2 unspecified: set to -1
-  // object 2 unspecified: set to -1
+    obj2id = -1;
-  else {
+    px2 = nullptr;
-    if (objtype==mjOBJ_GEOM) {
-      throw mjCError(this, "both geom are required in equality constraint");
-    } else {
-      obj2id = -1;
-      px2 = 0;
-    }
   }
 
   // set missing body = world
-  if (objtype==mjOBJ_BODY && obj2id==-1) {
+  if (object_type == mjOBJ_BODY && obj2id == -1) {
     obj2id = 0;
   }
 
   // make sure the two objects are different
-  if (obj1id==obj2id) {
+  if (obj1id == obj2id) {
     throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id);
   }
 
   // make sure joints are scalar
-  if (type==mjEQ_JOINT) {
+  if (type == mjEQ_JOINT) {
     jt1 = ((mjCJoint*)px1)->type;
     jt2 = (px2 ? ((mjCJoint*)px2)->type : mjJNT_HINGE);
-    if ((jt1!=mjJNT_HINGE && jt1!=mjJNT_SLIDE) ||
+    if ((jt1 != mjJNT_HINGE && jt1 != mjJNT_SLIDE) ||
-        (jt2!=mjJNT_HINGE && jt2!=mjJNT_SLIDE)) {
+        (jt2 != mjJNT_HINGE && jt2 != mjJNT_SLIDE)) {
       throw mjCError(this, "only HINGE and SLIDE joint allowed in constraint");
     }
   }

src/xml/xml_native_reader.cc

@@ -363,8 +363,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
 
     {"equality", "*", "0"},
     {"<"},
-        {"connect", "*", "8", "name", "class", "body1", "body2", "anchor",
+        {"connect", "*", "10", "name", "class", "body1", "body2", "anchor",
-            "active", "solref", "solimp"},
+            "site1", "site2", "active", "solref", "solimp"},
         {"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor",
             "active", "solref", "solimp", "torquescale"},
         {"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
@@ -1896,10 +1896,39 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
     }
 
     switch (equality->type) {
-    case mjEQ_CONNECT:
+    case mjEQ_CONNECT: {
-      ReadAttrTxt(elem, "body1", name1, true);
+        auto maybe_site1 = ReadAttrStr(elem, "site1");
-      ReadAttrTxt(elem, "body2", name2);
+        auto maybe_site2 = ReadAttrStr(elem, "site2");
-      ReadAttr(elem, "anchor", 3, equality->data, text, true);
+        auto maybe_body1 = ReadAttrStr(elem, "body1");
+        auto maybe_body2 = ReadAttrStr(elem, "body2");
+        bool has_anchor = ReadAttr(elem, "anchor", 3, equality->data, 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;
+
+        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() && has_anchor;
+        if (site_semantic == body_semantic) {
+          throw mjXError(elem, "either both body1 and anchor must be defined,"
+                         " or both 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;
+        } else {
+          name1 = maybe_site1.value();
+          name2 = maybe_site2.value();
+          equality->objtype = mjOBJ_SITE;
+        }
+      }
       break;
 
     case mjEQ_WELD:

src/xml/xml_native_writer.cc

@@ -620,9 +620,14 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
 
     switch (equality->type) {
     case mjEQ_CONNECT:
-      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);
+      } else {
+        WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
+        WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
+      }
       break;
 
     case mjEQ_WELD:

test/engine/testdata/connect.xml

@@ -0,0 +1,52 @@
+<mujoco>
+  <option>
+    <flag contact="disable"/>
+  </option>
+
+  <default>
+    <default class="static">
+      <geom size=".5 .1 .5" rgba=".5 .7 .5 .3"/>
+    </default>
+    <default class="free">
+      <geom type="box" size=".2" fromto="0 0 0 0 -1 0" rgba=".4 .7 .6 .3"/>
+    </default>
+  </default>
+
+  <worldbody>
+    <geom pos="0 0 -2" type="plane" size="10 10 .01"/>
+    <light pos="0 0 20"/>
+
+    <body name="box1" pos="-3 0 0">
+      <geom type="box" class="static"/>
+    </body>
+    <body name="beam1" pos="-3 0 0">
+      <freejoint/>
+      <geom class="free"/>
+    </body>
+
+    <body name="box2" pos="-1 0 0">
+      <geom type="box" class="static"/>
+    </body>
+    <body name="beam2" pos="-1 0 0">
+      <freejoint/>
+      <geom class="free"/>
+    </body>
+
+    <body name="box3" pos="1 0 0">
+      <geom type="box" class="static"/>
+      <site name="box3" pos=".5 -.1 .5" type="box" size=".05 .05 .05"/>
+    </body>
+    <body name="beam3" pos="1 0 0">
+      <freejoint/>
+      <geom class="free"/>
+      <site name="beam3" pos=".2 -.1 .2" type="box" size=".05 .05 .05"/>
+    </body>
+
+  </worldbody>
+
+  <equality>
+    <connect body1="beam1" anchor="0 0 0"/>
+    <connect body1="box2" body2="beam2" anchor=".5 .1 .5"/>
+    <connect site1="box3" site2="beam3"/>
+  </equality>
+</mujoco>

test/user/user_objects_test.cc

@@ -38,6 +38,7 @@ std::vector<mjtNum> AsVector(const mjtNum* array, int n) {
   return std::vector<mjtNum>(array, array + n);
 }
 
+using std::string;
 using ::testing::ElementsAre;
 using ::testing::HasSubstr;
 using ::testing::IsNull;
@@ -392,7 +393,7 @@ using KeyframeTest = MujocoTest;
 constexpr char kKeyframePath[] = "user/testdata/keyframe.xml";
 
 TEST_F(KeyframeTest, CheckValues) {
-  const std::string xml_path = GetTestDataFilePath(kKeyframePath);
+  const string xml_path = GetTestDataFilePath(kKeyframePath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
   ASSERT_THAT(model, NotNull());
   EXPECT_EQ(model->nkey, 7);
@@ -410,7 +411,7 @@ TEST_F(KeyframeTest, CheckValues) {
 }
 
 TEST_F(KeyframeTest, ResetDataKeyframe) {
-  const std::string xml_path = GetTestDataFilePath(kKeyframePath);
+  const string xml_path = GetTestDataFilePath(kKeyframePath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
   ASSERT_THAT(model, NotNull());
   mjData* data = mj_makeData(model);
@@ -442,7 +443,7 @@ TEST_F(KeyframeTest, ResetDataKeyframe) {
 }
 
 TEST_F(KeyframeTest, ResetDataKeyframeAcceptsNegativeKeyframe) {
-  const std::string xml_path = GetTestDataFilePath(kKeyframePath);
+  const string xml_path = GetTestDataFilePath(kKeyframePath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
   ASSERT_THAT(model, NotNull());
   mjData* data = mj_makeData(model);
@@ -650,7 +651,7 @@ static constexpr int kSphereBodyId = 1, kCylinderBodyId = 2,
                      kCapsuleBodyId = 3, kCapsuleGeomId = 2;
 
 TEST_F(MjCGeomTest, CapsuleMass) {
-  const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
+  const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
   // Mass of capsule should equal mass of cylinder + mass of sphere.
   mjtNum sphere_cylinder_mass =
@@ -661,7 +662,7 @@ TEST_F(MjCGeomTest, CapsuleMass) {
 }
 
 TEST_F(MjCGeomTest, CapsuleInertiaZ) {
-  const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
+  const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
   // z-inertia of capsule should equal sphere + cylinder z-inertia.
   mjtNum sphere_cylinder_z_inertia =
@@ -673,7 +674,7 @@ TEST_F(MjCGeomTest, CapsuleInertiaZ) {
 }
 
 TEST_F(MjCGeomTest, CapsuleInertiaX) {
-  const std::string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
+  const string xml_path = GetTestDataFilePath(kCapsuleInertiaPath);
   mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, 0, 0);
 
   // The CoM of a solid hemisphere is 3/8*radius away from from the disk.
@@ -1168,7 +1169,7 @@ TEST_F(MjCGeomTest, BadMeshZeroMassDensityDoesntError) {
 using MjCHFieldTest = MujocoTest;
 
 TEST_F(MjCHFieldTest, PngMap) {
-  const std::string xml_path =
+  const string xml_path =
       GetTestDataFilePath("user/testdata/hfield_png.xml");
   std::array<char, 1024> error;
   mjModel* model =
@@ -1853,7 +1854,7 @@ constexpr char kKeyAutoLimits[] = "user/testdata/auto_limits.xml";
 
 // check joint limit values when automatically inferred based on range
 TEST_F(LimitedTest, JointLimited) {
-  const std::string path = GetTestDataFilePath(kKeyAutoLimits);
+  const string path = GetTestDataFilePath(kKeyAutoLimits);
   std::array<char, 1024> err;
   mjModel* model = mj_loadXML(path.c_str(), nullptr, err.data(), err.size());
   ASSERT_THAT(model, NotNull()) << err.data();
@@ -2046,8 +2047,10 @@ TEST_F(SpringrangeTest, InvalidRange) {
   EXPECT_THAT(error.data(), HasSubstr("line 9"));
 }
 
+using UserObjectsTest = MujocoTest;
+
 // ------------- test frame ----------------------------------------------------
-TEST_F(MujocoTest, Frame) {
+TEST_F(UserObjectsTest, Frame) {
   static constexpr char xml[] = R"(
   <mujoco>
     <worldbody>
@@ -2141,7 +2144,7 @@ TEST_F(MujocoTest, Frame) {
   mj_deleteData(d);
 }
 
-TEST_F(MujocoTest, FrameTransformsLight) {
+TEST_F(UserObjectsTest, FrameTransformsLight) {
   static constexpr char xml[] = R"(
   <mujoco>
     <worldbody>
@@ -2153,7 +2156,7 @@ TEST_F(MujocoTest, FrameTransformsLight) {
   )";
   std::array<char, 1024> error;
   mjModel* m = LoadModelFromString(xml, error.data(), error.size());
-  EXPECT_THAT(m, testing::NotNull()) << error.data();
+  EXPECT_THAT(m, NotNull()) << error.data();
   EXPECT_EQ(m->nlight, 1);
 
   constexpr mjtNum eps = 1e-14;
@@ -2170,7 +2173,7 @@ TEST_F(MujocoTest, FrameTransformsLight) {
 
 
 // ------------- test bvh ------------------------------------------------------
-TEST_F(MujocoTest, RobustBVH) {
+TEST_F(UserObjectsTest, RobustBVH) {
   static constexpr char xml1[] = R"(
   <mujoco>
     <worldbody>
@@ -2203,10 +2206,10 @@ TEST_F(MujocoTest, RobustBVH) {
 
   std::array<char, 1024> error;
   mjModel* m1 = LoadModelFromString(xml1, error.data(), error.size());
-  EXPECT_THAT(m1, testing::NotNull()) << error.data();
+  EXPECT_THAT(m1, NotNull()) << error.data();
 
   mjModel* m2 = LoadModelFromString(xml2, error.data(), error.size());
-  EXPECT_THAT(m2, testing::NotNull()) << error.data();
+  EXPECT_THAT(m2, NotNull()) << error.data();
 
   EXPECT_EQ(m1->nbvh, m2->nbvh);
   for (int i = 0; i < m1->nbvh; i++) {
@@ -2217,5 +2220,84 @@ TEST_F(MujocoTest, RobustBVH) {
   mj_deleteModel(m2);
 }
 
+// ------------- test equality compilation -------------------------------------
+
+TEST_F(UserObjectsTest, BadConnect) {
+  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>
+      CONNECT
+    </equality>
+  </mujoco>
+  )";
+  int pos = base.find("CONNECT");
+  int len = 7;
+
+  // good model using body semantic
+  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;
+  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;
+  EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
+  mj_deleteModel(m);
+
+  char error_missing[] = "either both body1 and anchor must be defined,"
+      " or both site1 and site2 must be defined\nElement 'connect', line 12";
+
+  // bad model (missing anchor)
+  xml = base.replace(pos, len, "<connect body1='1'/>");
+  m = LoadModelFromString(xml.c_str(), error, sizeof(error));
+  EXPECT_THAT(m, IsNull());
+  EXPECT_THAT(error, HasSubstr(error_missing));
+
+  char error_mixed[] = "body and site semantics cannot be mixed"
+      "\nElement 'connect', line 12";
+
+  // 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());
+  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;
+
+  // add a connect but don't set objtype
+  mjsEquality* equality = mjs_addEquality(s, nullptr);
+  equality->type = mjEQ_CONNECT;
+  mjs_setString(equality->name1, "0");
+  mjs_setString(equality->name2, "1");
+
+  // expect compilation to fail
+  m = mj_compile(s, nullptr);
+  EXPECT_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);
+  mj_deleteModel(m);
+  mj_deleteSpec(s);
+}
+
 }  // namespace
 }  // namespace mujoco

unity/Runtime/Bindings/MjBindings.cs

@@ -5500,6 +5500,7 @@ public unsafe struct mjModel_ {
   public int* eq_type;
   public int* eq_obj1id;
   public int* eq_obj2id;
+  public int* eq_objtype;
   public byte* eq_active0;
   public double* eq_solref;
   public double* eq_solimp;
@@ -6300,6 +6301,7 @@ public unsafe struct model {
   public int* eq_type;
   public int* eq_obj1id;
   public int* eq_obj2id;
+  public int* eq_objtype;
   public double* eq_data;
   public int* tendon_num;
   public int* tendon_matid;