diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index ef72df1f..555ff95a 100644 --- a/doc/XMLreference.rst +++ b/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 - displacements vs. translational displacements. Setting this value to 0 makes the :el:`weld` behave like a - :el:`connect` constraint. Note that this value has units of length and can therefore be interpreted as follows. - Imagining that the weld is implemented by a patch of glue sticking the two bodies together, :at:`torquescale` can be - interpreted as the diameter of this glue patch. + A constant that scales the angular residual (angular constraint violation). Notionally in units of + :math:`\textrm{torque}/\textrm{force}=\textrm{length}`. Intuitively this coefficient defines how much the weld + "cares" about rotational displacements vs. translational displacements. Setting this value to 0 makes the :el:`weld` + behave like a :el:`connect` constraint. Note that this value has units of length and can therefore be understood as + 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: diff --git a/doc/changelog.rst b/doc/changelog.rst index 386e63b6..95e437ca 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -25,16 +25,18 @@ Python bindings Simulate ^^^^^^^^ - 6. :ref:`simulate`: 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`. + Version 3.0.0 (October 18, 2023) -------------------------------- diff --git a/mjx/mujoco/mjx/_src/constraint.py b/mjx/mujoco/mjx/_src/constraint.py index 91e83cb9..34263053 100644 --- a/mjx/mujoco/mjx/_src/constraint.py +++ b/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 diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index 2c8c1a45..3b912ce9 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/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) diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index dbe268d9..29f884cd 100644 --- a/src/engine/engine_core_constraint.c +++ b/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 - mjtNum torquescale = data[10]; + // scale rotational jacobian by torquescale mju_scl(jac[0]+3*NV, jac[0]+3*NV, torquescale, 3*NV); size = 6; diff --git a/test/engine/testdata/core_constraint/soft_weld.xml b/test/engine/testdata/core_constraint/soft_weld.xml new file mode 100644 index 00000000..5ee950b4 --- /dev/null +++ b/test/engine/testdata/core_constraint/soft_weld.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + +