chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Fix bug in MJX frictionloss.

Browse Source 
PiperOrigin-RevId: 684198925
Change-Id: I5f6575759729712550bcb8d7659aaa7a657c66fc
This commit is contained in:
Baruch Tabanpour
2024-10-09 15:49:54 -07:00
committed by Copybara-Service
parent a26ba2012e
commit 22e4f7fc31
4 changed files with 39 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+2 -1
View File
@@ -18,7 +18,8 @@ MJX
^^^
- Added ``mocap_pos`` and ``mocap_quat`` in kinematics.
- Added support for :ref:`spatial tendons <tendon-spatial>` with pulleys and external sphere and cylinder wrapping.
- Add sphere-cylinder and sphere-ellipsoid collision functions (:github:issue:`2126`).
- Added sphere-cylinder and sphere-ellipsoid collision functions (:github:issue:`2126`).
- Fixed a bug with frictionloss constraints.
Bug fixes
^^^^^^^^^
mjx/mujoco/mjx/_src/solver.py
+1 -1
View File
@@ -161,7 +161,7 @@ class _LSPoint(PyTreeNode):
linear_pos = (x >= rf)[:, None]
qf = linear_neg * jp.array([f * (-0.5 * rf - ctx.Jaref), -f * jv, z]).T
qf += linear_pos * jp.array([f * (-0.5 * rf + ctx.Jaref), f * jv, z]).T
quad = jp.where(f[:, None] > 0, qf, quad)
quad = jp.where((linear_neg | linear_pos) & (f[:, None] > 0), qf, quad)
if m.opt.cone == ConeType.ELLIPTIC:
mu, u0 = ctx.fri[:, 0], ctx.u[:, 0]
mjx/mujoco/mjx/_src/solver_test.py
+15
View File
@@ -132,5 +132,20 @@ class SolverTest(parameterized.TestCase):
nnz = dx.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force')
def test_quad_frictionloss(self):
"""Test a case with quadratic frictionloss constraints."""
m = test_util.load_test_file('quadratic_frictionloss.xml')
d = mujoco.MjData(m)
mujoco.mj_resetDataKeyframe(m, d, 0)
mujoco.mj_forward(m, d)
dx = jax.jit(mjx.solve)(mjx.put_model(m), mjx.put_data(m, d))
_assert_attr_eq(d, dx, 'qacc')
_assert_attr_eq(d, dx, 'qfrc_constraint')
nnz = dx.efc_J.any(axis=1)
_assert_eq(d.efc_force, dx.efc_force[nnz], 'efc_force')
if __name__ == '__main__':
absltest.main()
mjx/mujoco/mjx/test_data/quadratic_frictionloss.xml
+21
View File
@@ -0,0 +1,21 @@
<!-- tests a box on a plane with quadratic frictionloss constraints -->
<mujoco>
<option iterations="10" ls_iterations="6"/>
<asset>
<texture builtin="gradient" height="100" rgb1="1 1 1" rgb2="0 0 0" type="skybox" width="100"/>
<texture builtin="checker" height="100" name="texplane" rgb1="0 0 0" rgb2="0.8 0.8 0.8" type="2d" width="100"/>
<material name="MatPlane" reflectance="0.5" shininess="1" specular="1" texrepeat="200 200" texture="texplane"/>
</asset>
<worldbody>
<light cutoff="100" diffuse="1 1 1" dir="-0 0 -1.3" directional="true" exponent="1" pos="0 0 1.3" specular=".1 .1 .1"/>
<geom conaffinity="1" condim="3" material="MatPlane" name="floor" pos="0 0 0" size="40 40 40" type="plane"/>
<camera pos="-0.323 -4.170 2.949" xyaxes="1.000 -0.024 -0.000 0.010 0.427 0.904"/>
<body name="peg" pos="0.0 0.0 0.02" euler="30 0 0">
<joint type="free" stiffness="0" damping="0" frictionloss="0.005" armature="0"/>
<geom mass="0.044" condim="6" friction="1 0.05 0.001" pos="0 0 0" size="0.048 0.01 0.01" type="box" name="red_peg" rgba="1 0 0 1"/>
</body>
</worldbody>
<keyframe>
<key name="home" qpos='0.00382621 -0.000816274 0.011149 0.992081 0.0959616 0.00580468 0.08083'/>
</keyframe>
</mujoco>