Remove distance constraints.

- `distance` constraints were not very useful and increased the contact buffer within the constraint solver, violating a core design principle. Therefore they have been removed.
- The enum value `mjEQ_DISTANCE` is retained, but errors will be thrown in the parser and engine if these constraints are selected.

PiperOrigin-RevId: 467050101
Change-Id: Ie72a07e4fe338eea5107004a9fc0f5e47eb0c3b7

doc/APIreference.rst

@@ -324,7 +324,7 @@ mjtEq
        mjEQ_WELD,                      // fix relative position and orientation of two bodies
        mjEQ_JOINT,                     // couple the values of two scalar joints with cubic
        mjEQ_TENDON,                    // couple the lengths of two tendons with cubic
-       mjEQ_DISTANCE                   // fix the contact distance betweent two geoms
+       mjEQ_DISTANCE                   // unsupported, will cause an error if used
    } mjtEq;
 
 | Defined in `mjmodel.h <https://github.com/deepmind/mujoco/blob/main/include/mujoco/mjmodel.h>`_

doc/XMLreference.rst

@@ -4061,20 +4061,8 @@ This element constrains the length of one tendon to be a quartic polynomial of a
 :el-prefix:`equality/` **distance** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This element constrains the nearest distance between two geoms. When the distance attribute is set to 0 the two geom
+Distance equality constraints were removed in MuJoCo version 2.2.2. If you are using an earlier version, please switch
-surfaces slide over each other, otherwise they slide over a virtual cushion with depth equal to the specified distance.
+to the corresponding version of the documentation.
-This mechanism is implemented as a modification to the collision detector. For geom pairs handled by the general-purpose
-convex collider, large distance values in this constraint are handled approximately, due to the nature of the underlying
-collision algorithm.
-
-:at:`name`, :at:`class`, :at:`active`, :at:`solref`, :at:`solimp`
-   Same as in :ref:`connect <equality-connect>` element.
-:at:`geom1`: :at-val:`string, required`
-   Name of the first geom.
-:at:`geom2`: :at-val:`string, required`
-   Name of the second geom.
-:at:`distance`: :at-val:`real, "0"`
-   Desired distance between the two geom surfaces. The constraint solver enforces this distance softly.
 
 .. _tendon:
 

doc/changelog.rst

@@ -21,6 +21,10 @@ General
 - Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes.
 - ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``.
 
+Deleted/deprecated features
+^^^^^^^^^^^^^^^^^^^^^^^^^^^
+- Removed ``distance`` constraints.
+
 Bug fixes
 ^^^^^^^^^
 - Fixed rendering of some transparent geoms in reflection.

include/mujoco/mjmodel.h

@@ -163,7 +163,7 @@ typedef enum mjtEq_ {             // type of equality constraint
   mjEQ_WELD,                      // fix relative position and orientation of two bodies
   mjEQ_JOINT,                     // couple the values of two scalar joints with cubic
   mjEQ_TENDON,                    // couple the lengths of two tendons with cubic
-  mjEQ_DISTANCE                   // fix the contact distance betweent two geoms
+  mjEQ_DISTANCE                   // unsupported, will cause an error if used
 } mjtEq;
 
 

src/engine/engine_core_constraint.c

@@ -342,11 +342,10 @@ void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec)
 // equality constraints
 void mj_instantiateEquality(const mjModel* m, mjData* d) {
   int issparse = mj_isSparse(m), nv = m->nv;
-  int oldncon, id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
+  int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
-  mjtNum cpos[6], pos[2][3], ref[2], dif, deriv, dist;
+  mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
   mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
   mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
-  mjContact *con;
   mjMARKSTACK;
 
   // disabled or no equality contraints: return
@@ -529,70 +528,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         break;
 
       case mjEQ_DISTANCE:
-        // find contacts between constrained geoms
+        mju_error("distance equality constraints are no longer supported");
-        oldncon = d->ncon;
-        mj_collideGeoms(m, d, id[0], id[1], 1,
-                        mju_dist3(d->geom_xpos+3*id[0], d->geom_xpos+3*id[1]));
-
-        // make sure we got some contacts
-        if (oldncon==d->ncon) {
-          size = 0;
-          break;
-        }
-
-        // find smallest dist
-        dist = d->contact[oldncon].dist;
-        for (int j=1; j<d->ncon-oldncon; j++) {
-          if (d->contact[oldncon+j].dist<dist) {
-            dist = d->contact[oldncon+j].dist;
-          }
-        }
-
-        // collide again with adjusted distance (because libccd messes up with big margin)
-        d->ncon = oldncon;
-        mjtNum adjustment = 0.01;
-        mj_collideGeoms(m, d, id[0], id[1], 1, dist + adjustment);
-
-        // make sure we still got some contacts
-        if (oldncon==d->ncon) {
-          size = 0;
-          break;
-        }
-
-        // find smallest-dist contact
-        int k = 0;
-        dist = d->contact[oldncon].dist;
-        for (int j=1; j<d->ncon-oldncon; j++)
-          if (d->contact[oldncon+j].dist<dist) {
-            dist = d->contact[oldncon+j].dist;
-            k = j;
-          }
-
-        // move smallest-dist contact to first position, discard the rest
-        if (k>0) {
-          d->contact[oldncon] = d->contact[oldncon+k];
-        }
-        d->ncon = oldncon+1;
-        con = d->contact + oldncon;
-
-        // label contact, make sure solver does not include it
-        con->efc_address = -2-i;
-        con->exclude = 3;
-
-        // compute position error
-        cpos[0] = dist - data[0];
-
-        // compute Jacobian difference
-        NV = mj_jacDifPair(m, d, chain,
-                           m->geom_bodyid[con->geom1], m->geom_bodyid[con->geom2],
-                           con->pos, con->pos,
-                           jac[0], jac[1], jacdif, NULL, NULL, NULL);
-
-        // construct contact normal Jacobian
-        mju_mulMatMat(jac[0], con->frame, jacdif, 1, 3, NV);
-
-        size = 1;
-        break;
 
       default:                    // SHOULD NOT OCCUR
         mju_error_i("Invalid equality constraint type %d", m->eq_type[i]);
@@ -977,10 +913,7 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
         break;
 
       case mjEQ_DISTANCE:
-        // body translation
+        mju_error("distance equality constraints are no longer supported");
-        b1 = m->geom_bodyid[m->eq_obj1id[id]];
-        b2 = m->geom_bodyid[m->eq_obj2id[id]];
-        dA[i] = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
       }
       break;
 

src/engine/engine_core_smooth.c

@@ -1904,11 +1904,13 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
 
     case mjEQ_JOINT:
     case mjEQ_TENDON:
-    case mjEQ_DISTANCE:
       // increment 1 row
       i++;
       break;
 
+    case mjEQ_DISTANCE:
+      mju_error("distance equality constraints are no longer supported");
+
     default:
       mju_error_i("Unknown constraint type type %d", m->eq_type[id]);    // SHOULD NOT OCCUR
     }

src/engine/engine_io.c

@@ -1370,13 +1370,7 @@ const char* mj_validateReferences(const mjModel* m) {
         }
         break;
       case mjEQ_DISTANCE:
-        if (obj1id >= m->ngeom || obj1id < 0) {
+        return "distance equality constraints are no longer supported";
-          return "Invalid model: eq_obj1id out of bounds.";
-        }
-        if (obj2id >= m->ngeom || obj2id < 0) {
-          return "Invalid model: eq_obj2id out of bounds.";
-        }
-        break;
       case mjEQ_WELD:
       case mjEQ_CONNECT:
         if (obj1id >= m->nbody || obj1id < 0) {

src/xml/xml_native_reader.cc

@@ -1328,9 +1328,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
       break;
 
     case mjEQ_DISTANCE:
-      ReadAttrTxt(elem, "geom1", pequality->name1, true);
+      throw mjXError(elem, "support for distance equality contraints was removed in MuJoCo 2.2.2");
-      ReadAttrTxt(elem, "geom2", pequality->name2, true);
-      ReadAttr(elem, "distance", 1, pequality->data, text);
       break;
 
     default:                    // SHOULD NOT OCCUR

test/engine/engine_io_test.cc

@@ -549,7 +549,6 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) {
     <equality>
       <connect anchor="0 0 0" body1="body1" />
       <weld body1="body1" body2="body2" />
-      <distance geom1="geom1" geom2="geom2" />
       <joint joint1="joint1"/>
       <joint joint1="joint1" joint2="joint2"/>
       <tendon tendon1="tendon1"/>
@@ -590,18 +589,6 @@ TEST_F(ValidateReferencesTest, EqualityConstraints) {
   EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
   model->eq_obj2id[1] = model->nbody - 1;
 
-  // distance constraint
-  model->eq_obj1id[2] = -1;
-  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
-  model->eq_obj1id[2] = model->ngeom;
-  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj1id"));
-  model->eq_obj1id[2] = 1;
-
-  model->eq_obj2id[2] = -1;
-  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
-  model->eq_obj2id[2] = model->ngeom;
-  EXPECT_THAT(mj_validateReferences(model), HasSubstr("eq_obj2id"));
-  model->eq_obj2id[2] = model->ngeom - 1;
   mj_deleteModel(model);
 }
 

unity/Runtime/Components/Equality/MjDistance.cs

@@ -1,48 +0,0 @@
-// Copyright 2019 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//      http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-using System;
-using System.Linq;
-using System.Xml;
-using UnityEngine;
-
-namespace Mujoco {
-
-  public class MjDistance : MjBaseConstraint {
-    public MjGeom Geom1;
-    public MjGeom Geom2;
-    protected override string _constraintName => "distance";
-
-    protected override void FromMjcf(XmlElement mjcf) {
-      Geom1 = mjcf.GetObjectReferenceAttribute<MjGeom>("geom1");
-      Geom2 = mjcf.GetObjectReferenceAttribute<MjGeom>("geom2");
-    }
-
-    protected override void ToMjcf(XmlElement mjcf) {
-      if (Geom1 == null || Geom2 == null) {
-        throw new NullReferenceException($"Both geoms in distance {name} must be assigned.");
-      }
-
-      mjcf.SetAttribute("geom1", Geom1.MujocoName);
-      mjcf.SetAttribute("geom2", Geom2.MujocoName);
-    }
-
-    public void OnValidate() {
-      if (Geom1 != null && Geom1 == Geom2) {
-        Debug.LogError("Geom1 and Geom2 can't be the same - resetting Geom2.", this);
-        Geom2 = null;
-      }
-    }
-  }
-}

unity/Runtime/Components/Equality/MjDistance.cs.meta

@@ -1,13 +0,0 @@
-fileFormatVersion: 2
-guid: ad0689a32f102481fb189836c11931f9
-timeCreated: 1552309644
-licenseType: Pro
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: 

unity/Runtime/Importer/MjcfImporter.cs

@@ -296,9 +296,6 @@ public class MjcfImporter {
       case "tendon":
         equalityType = typeof(MjTendonConstraint);
         break;
-      case "distance":
-        equalityType = typeof(MjDistance);
-        break;
       default:
         Debug.Log($"The importer does not yet support equality <{node.Name}>.");
         break;

unity/Tests/Editor/Components/Equality/MjDistanceTests.cs

@@ -1,74 +0,0 @@
-// Copyright 2019 DeepMind Technologies Limited
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//      http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
-
-using System;
-using System.Collections;
-using System.Collections.Generic;
-using System.Xml;
-using NUnit.Framework;
-using UnityEngine;
-
-namespace Mujoco {
-  [TestFixture]
-  public class MjDistanceTests {
-    private MjGeom _geom1;
-    private MjGeom _geom2;
-    private MjDistance _distance;
-    private XmlDocument _doc;
-
-    [SetUp]
-    public void SetUp() {
-      _geom1 = new GameObject("geom1").AddComponent<MjGeom>();
-      _geom2 = new GameObject("geom2").AddComponent<MjGeom>();
-      _distance = new GameObject("distance").AddComponent<MjDistance>();
-      _doc = new XmlDocument();
-    }
-
-    [TearDown]
-    public void TearDown() {
-      UnityEngine.Object.DestroyImmediate(_distance.gameObject);
-      UnityEngine.Object.DestroyImmediate(_geom1.gameObject);
-      UnityEngine.Object.DestroyImmediate(_geom2.gameObject);
-    }
-
-    [Test]
-    public void ErrorThrownWhenGeom1Empty() {
-      // This is an illegal MJCF, but the purpose of this test is to verify that if
-      // the user didn't assign the geom in the editor, an error will be thrown when play is hit.
-      _doc.LoadXml("<geom/>");
-      _distance.ParseMjcf(_doc.GetElementsByTagName("geom")[0] as XmlElement);
-      Assert.That(() => { _distance.GenerateMjcf("name", _doc); }, Throws.Exception);
-    }
-
-    // In this constraint both elements are required.
-    [Test]
-    public void ErrorThrownWhenGeom2Empty() {
-      _doc.LoadXml("<geom geom1='geom1'/>");
-      _distance.ParseMjcf(_doc.GetElementsByTagName("geom")[0] as XmlElement);
-      Assert.That(() => { _distance.GenerateMjcf("name", _doc); }, Throws.Exception);
-    }
-
-    [Test]
-    public void ParseAndGenerate() {
-      _doc.LoadXml("<distance geom1='geom1' geom2='geom2'/>");
-      _distance.ParseMjcf(_doc.GetElementsByTagName("distance")[0] as XmlElement);
-      Assert.That(_distance.Geom1, Is.EqualTo(_geom1));
-      Assert.That(_distance.Geom2, Is.EqualTo(_geom2));
-      var mjcf = _distance.GenerateMjcf("name", _doc);
-      Assert.That(mjcf.OuterXml, Does.Contain("<distance"));
-      Assert.That(mjcf.OuterXml, Does.Contain("geom1=\""));
-      Assert.That(mjcf.OuterXml, Does.Contain("geom2=\""));
-    }
-  }
-}

unity/Tests/Editor/Components/Equality/MjDistanceTests.cs.meta

@@ -1,13 +0,0 @@
-fileFormatVersion: 2
-guid: 528955688a5574fa5ba1433088808a1b
-timeCreated: 1552313909
-licenseType: Pro
-MonoImporter:
-  externalObjects: {}
-  serializedVersion: 2
-  defaultReferences: []
-  executionOrder: 0
-  icon: {instanceID: 0}
-  userData: 
-  assetBundleName: 
-  assetBundleVariant: