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

Add mjd_inverseFD for finite-difference approximations of inverse dynamics Jacobians.

Browse Source 
Fixes #703.

PiperOrigin-RevId: 527899700
Change-Id: I10e41a381dcecf62c53b3b9aa72a4ce666161366
This commit is contained in:
Yuval Tassa
2023-04-28 09:03:39 -07:00
committed by Copybara-Service
parent 90e14ac8d0
commit c50177d301
13 changed files with 515 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python/mujoco/bindings_test.py
+30 -1
View File
@@ -41,7 +41,7 @@ TEST_XML = r"""
<body>
<inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
<site pos="0 0 -1" name="mysite" type="sphere"/>
<joint name="myhinge" type="hinge" axis="0 1 0"/>
<joint name="myhinge" type="hinge" axis="0 1 0" damping="1"/>
</body>
<body>
<inertial pos="0 0 0" mass="1" diaginertia="1 1 1"/>
@@ -54,6 +54,10 @@ TEST_XML = r"""
<actuator>
<position name="myactuator" joint="myhinge"/>
</actuator>
<sensor>
<jointvel name="myjointvel" joint="myhinge"/>
<accelerometer name="myaccelerometer" site="mysite"/>
</sensor>
</mujoco>
"""
@@ -1046,6 +1050,31 @@ Euler integrator, semi-implicit in velocity.
bodyexclude=0,
geomid=geomid)
def test_inverse_fd_none(self):
eps = 1e-6
flg_centered = 0
mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
None, None, None, None, None, None, None)
def test_inverse_fd(self):
eps = 1e-6
flg_centered = 0
df_dq = np.zeros((self.model.nv, self.model.nv))
df_dv = np.zeros((self.model.nv, self.model.nv))
df_da = np.zeros((self.model.nv, self.model.nv))
ds_dq = np.zeros((self.model.nv, self.model.nsensordata))
ds_dv = np.zeros((self.model.nv, self.model.nsensordata))
ds_da = np.zeros((self.model.nv, self.model.nsensordata))
dm_dq = np.zeros((self.model.nv, self.model.nM))
mujoco.mjd_inverseFD(self.model, self.data, eps, flg_centered,
df_dq, df_dv, df_da, ds_dq, ds_dv, ds_da, dm_dq)
self.assertGreater(np.linalg.norm(df_dq), eps)
self.assertGreater(np.linalg.norm(df_dv), eps)
self.assertGreater(np.linalg.norm(df_da), eps)
self.assertGreater(np.linalg.norm(ds_dq), eps)
self.assertGreater(np.linalg.norm(ds_dv), eps)
self.assertGreater(np.linalg.norm(ds_da), eps)
def test_mju_box_qp(self):
n = 5
res = np.zeros(n)