Fix bug in implementation of torquescale

PiperOrigin-RevId: 577466789
Change-Id: I3c8e5dd9b4af9a629e777f7ccad8417075b0046c

doc/XMLreference.rst

@@ -4486,11 +4486,12 @@ of the other body, without any joint elements in the child body.
 .. _equality-weld-torquescale:
 
 :at:`torquescale`: :at-val:`real, "1"`
-   Relative torque-to-force ratio. This ratio is used by the weld to scale how much it "cares" about rotational
+   A constant that scales the angular residual (angular constraint violation). Notionally in units of
-   displacements vs. translational displacements. Setting this value to 0 makes the :el:`weld` behave like a
+   :math:`\textrm{torque}/\textrm{force}=\textrm{length}`. Intuitively this coefficient defines how much the weld
-   :el:`connect` constraint. Note that this value has units of length and can therefore be interpreted as follows.
+   "cares" about rotational displacements vs. translational displacements. Setting this value to 0 makes the :el:`weld`
-   Imagining that the weld is implemented by a patch of glue sticking the two bodies together, :at:`torquescale` can be
+   behave like a :el:`connect` constraint. Note that this value has units of length and can therefore be understood as
-   interpreted as the diameter of this glue patch.
+   follows. Imagining that the weld is implemented by a flat patch of glue sticking the two bodies together,
+   :at:`torquescale` can be interpreted as the diameter of this glue patch.
 
 
 .. _equality-joint:

doc/changelog.rst

@@ -25,16 +25,18 @@ Python bindings
 
 Simulate
 ^^^^^^^^
-
 6. :ref:`simulate<saSimulate>`: correct handling of "Pause update", "Fullscreen" and "VSync" buttons.
 
 Documentation
 ^^^^^^^^^^^^^
-
 7. Added documentation for the :ref:`UI` framework.
 8. Fixed typos and supported fields in docs (fixes :github:issue:`1105` and :github:issue:`1106`).
 
 
+Bug fixes
+^^^^^^^^^
+9. Fixed bug relating to welds modified with :ref:`torquescale<equality-weld-torquescale>`.
+
 Version 3.0.0 (October 18, 2023)
 --------------------------------
 

mjx/mujoco/mjx/_src/constraint.py

@@ -168,10 +168,10 @@ def _instantiate_weld(m: Model, d: Data) -> _Efc:
     jacdifr = 0.5 * jax.vmap(jac_fn)(jacdifr)
 
     j = jp.concatenate((jacdifp.T, jacdifr.T))
-    pos = jp.concatenate((cpos, crot))
+    pos = jp.concatenate((cpos, crot)).at[3:].mul(torquescale)
 
     # impedance, inverse constraint mass, reference acceleration
-    k, b, imp = _kbi(m, solref, solimp, math.norm(pos.at[3:].mul(torquescale)))
+    k, b, imp = _kbi(m, solref, solimp, math.norm(pos))
     invweight = m.body_invweight0[id1] + m.body_invweight0[id2]
     r = jp.maximum(invweight * (1 - imp) / imp, mujoco.mjMINVAL).repeat(3)
     aref = -b * (j @ d.qvel) - k * imp * pos

mjx/mujoco/mjx/_src/constraint_test.py

@@ -29,7 +29,7 @@ from mujoco.mjx._src.types import SolverType
 import numpy as np
 
 
-def _assert_eq(a, b, name, step, fname, atol=1e-3, rtol=1e-3):
+def _assert_eq(a, b, name, step, fname, atol=5e-3, rtol=5e-3):
   err_msg = f'mismatch: {name} at step {step} in {fname}'
   np.testing.assert_allclose(a, b, err_msg=err_msg, atol=atol, rtol=rtol)
 

src/engine/engine_core_constraint.c

@@ -539,6 +539,9 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         // compute position error
         mju_sub3(cpos, pos[0], pos[1]);
 
+        // get torquescale coefficient
+        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],
                            jac[1], jac[0], jacdif,
@@ -553,7 +556,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
         mju_mulQuat(quat, d->xquat+4*id[0], relpose);   // quat = q0*relpose
         mju_negQuat(quat1, d->xquat+4*id[1]);           // quat1 = neg(q1)
         mju_mulQuat(quat2, quat1, quat);                // quat2 = neg(q1)*q0*relpose
-        mju_copy3(cpos+3, quat2+1);                     // copy axis components
+        mju_scl3(cpos+3, quat2+1, torquescale);         // scale axis components by torquescale
 
         // correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose
         for (int j=0; j < NV; j++) {
@@ -572,8 +575,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
           jac[0][5*NV+j] = 0.5*quat3[3];
         }
 
-        // scale rotational jacobian by torquescale factor
+        // scale rotational jacobian by torquescale
-        mjtNum torquescale = data[10];
         mju_scl(jac[0]+3*NV, jac[0]+3*NV, torquescale, 3*NV);
 
         size = 6;

test/engine/testdata/core_constraint/soft_weld.xml

@@ -0,0 +1,15 @@
+<mujoco>
+  <statistic meansize=".5"/>
+  <worldbody>
+    <site name="0"/>
+    <light pos="0 0 1"/>
+    <body name="1" pos="-.5 -.3 0">
+      <freejoint/>
+      <geom type="box" size=".5 .3 .02" pos="0 0 -.07"/>
+    </body>
+  </worldbody>
+
+  <equality>
+    <weld body1="1" anchor="0 0 0" solimp="0 0.96 0.01" torquescale="0.2"/>
+  </equality>
+</mujoco>