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

Add muscle actuators to MJX.

Browse Source 
PiperOrigin-RevId: 695334356
Change-Id: I6590ed6bdd4a8adc53d5c4e2641f472f7439cedc
This commit is contained in:
Taylor Howell
2024-11-11 07:45:11 -08:00
committed by Copybara-Service
parent 414b677eb8
commit 0f381a9ebd
10 changed files with 566 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files
mjx/mujoco/mjx/_src/support_test.py
+205
View File
@@ -221,6 +221,211 @@ class SupportTest(parameterized.TestCase):
force = force.at[3:].set(dx.contact.frame[j] @ force[3:])
np.testing.assert_allclose(result, force, rtol=1e-5, atol=2)
def test_muscle_gain_length(self):
lmin = 0.5
lmax = 1.5
np.testing.assert_allclose(
support.muscle_gain_length(0, lmin, lmax),
jp.zeros(1),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(0.5, lmin, lmax),
jp.zeros(1),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(0.6, lmin, lmax),
jp.array([0.08]),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(0.75, lmin, lmax),
jp.array([0.5]),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(1.0, lmin, lmax),
jp.ones(1),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(1.25, lmin, lmax),
jp.array([0.5]),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(1.5, lmin, lmax),
jp.zeros(1),
rtol=1e-5,
atol=1e-5,
)
np.testing.assert_allclose(
support.muscle_gain_length(2.0, lmin, lmax),
jp.zeros(1),
rtol=1e-5,
atol=1e-5,
)
def test_muscle_gain(self):
length = jp.array([1.0])
lengthrange = jp.array([0.0, 1.0])
acc0 = jp.array([1.0])
prm = jp.array([0.0, 1.0, 1.0, 200.0, 0.5, 3.0, 1.0, 0.0, 2.0, 0.0])
# V <= -1
vel = jp.array([-1.5])
np.testing.assert_allclose(
support.muscle_gain(length, vel, lengthrange, acc0, prm),
jp.array([-0.0]),
rtol=1e-5,
atol=1e-5,
)
# V <= 0
vel = jp.array([-0.5])
np.testing.assert_allclose(
support.muscle_gain(length, vel, lengthrange, acc0, prm),
jp.array([-0.25]),
rtol=1e-5,
atol=1e-5,
)
# V <= y
vel = jp.array([0.5])
np.testing.assert_allclose(
support.muscle_gain(length, vel, lengthrange, acc0, prm),
jp.array([-1.75]),
rtol=1e-5,
atol=1e-5,
)
# V > y
vel = jp.array([1.5])
np.testing.assert_allclose(
support.muscle_gain(length, vel, lengthrange, acc0, prm),
jp.array([-2.0]),
rtol=1e-5,
atol=1e-5,
)
# force < 0
prm = prm.at[2].set(-1.0)
np.testing.assert_allclose(
support.muscle_gain(length, vel, lengthrange, acc0, prm),
jp.array([-400.0]),
rtol=1e-5,
atol=1e-5,
)
def test_muscle_bias(self):
lengthrange = jp.array([0.0, 1.0])
acc0 = jp.array([1.0])
prm = jp.array([0.0, 1.0, 1.0, 200.0, 0.5, 3.0, 1.5, 1.3, 1.2, 0.0])
# L <= 1
length = jp.array([0.5])
np.testing.assert_allclose(
support.muscle_bias(length, lengthrange, acc0, prm),
jp.array([0.0]),
rtol=1e-5,
atol=1e-5,
)
# L <= b
length = jp.array([1.5])
np.testing.assert_allclose(
support.muscle_bias(length, lengthrange, acc0, prm),
jp.array([-0.1625]),
rtol=1e-5,
atol=1e-5,
)
# L > b
length = jp.array([2.5])
np.testing.assert_allclose(
support.muscle_bias(length, lengthrange, acc0, prm),
jp.array([-1.3]),
rtol=1e-5,
atol=1e-5,
)
# force < 0
prm = prm.at[2].set(-1.0)
np.testing.assert_allclose(
support.muscle_bias(length, lengthrange, acc0, prm),
jp.array([-260.0]),
rtol=1e-5,
atol=1e-5,
)
def test_smooth_muscle_dynamics(self):
# compute time constant as in Millard et al. (2013)
# https://doi.org/10.1115/1.4023390
def _muscle_dynamics_millard(ctrl, act, prm):
ctrlclamp = jp.clip(ctrl, 0, 1)
actclamp = jp.clip(act, 0, 1)
tau0 = prm[0] * (0.5 + 1.5 * actclamp)
tau1 = prm[1] / (0.5 + 1.5 * actclamp)
tau = jp.where(ctrlclamp > act, tau0, tau1)
return (ctrlclamp - act) / jp.maximum(mujoco.mjMINVAL, tau)
prm = jp.array([0.01, 0.04, 0.0])
# exact equality if tau_smooth = 0
for ctrl in [-0.1, 0.0, 0.4, 0.5, 1.0, 1.0]:
for act in [-0.1, 0.0, 0.4, 0.5, 1.0, 1.1]:
actdot_old = _muscle_dynamics_millard(ctrl, act, prm)
actdot_new = support.muscle_dynamics(ctrl, act, prm)
np.testing.assert_allclose(actdot_old, actdot_new, rtol=1e-5, atol=1e-5)
# positive tau_smooth
tau_smooth = 0.2
prm = prm.at[2].set(tau_smooth)
act = 0.5
eps = 1.0e-6
ctrl = 0.4 - eps # smaller than act by just over 0.5 * tau_smooth
np.testing.assert_allclose(
_muscle_dynamics_millard(ctrl, act, prm),
support.muscle_dynamics(ctrl, act, prm),
rtol=1e-5,
atol=1e-5,
)
ctrl = 0.6 + eps # larger than act by just over 0.5 * tau_smooth
np.testing.assert_allclose(
_muscle_dynamics_millard(ctrl, act, prm),
support.muscle_dynamics(ctrl, act, prm),
rtol=1e-5,
atol=1e-5,
)
# right in the middle should give average of time constants
tau_act = 0.2
tau_deact = 0.3
for dctrl in [0.0, 0.1, 0.2, 1.0, 1.1]:
lower = support.muscle_dynamics_timescale(
-dctrl, tau_act, tau_deact, tau_smooth
)
upper = support.muscle_dynamics_timescale(
dctrl, tau_act, tau_deact, tau_smooth
)
np.testing.assert_allclose(
0.5 * (upper + lower),
0.5 * (tau_act + tau_deact),
rtol=1e-5,
atol=1e-5,
)
if __name__ == '__main__':
absltest.main()