chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 2 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)
python/mujoco/functions.cc
+52 -2
View File
@@ -1131,7 +1131,8 @@ PYBIND11_MODULE(_functions, pymodule) {
});
Def<traits::mjd_transitionFD>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d, mjtNum eps, mjtByte centered,
[](const raw::MjModel* m, raw::MjData* d,
mjtNum eps, mjtByte flg_centered,
std::optional<Eigen::Ref<EigenArrayXX>> A,
std::optional<Eigen::Ref<EigenArrayXX>> B,
std::optional<Eigen::Ref<EigenArrayXX>> C,
@@ -1153,12 +1154,61 @@ PYBIND11_MODULE(_functions, pymodule) {
throw py::type_error("D should be of shape (nsensordata, nu)");
}
return InterceptMjErrors(::mjd_transitionFD)(
m, d, eps, centered,
m, d, eps, flg_centered,
A.has_value() ? A->data() : nullptr,
B.has_value() ? B->data() : nullptr,
C.has_value() ? C->data() : nullptr,
D.has_value() ? D->data() : nullptr);
});
Def<traits::mjd_inverseFD>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d,
mjtNum eps, mjtByte flg_actuation,
std::optional<Eigen::Ref<EigenArrayXX>> DfDq,
std::optional<Eigen::Ref<EigenArrayXX>> DfDv,
std::optional<Eigen::Ref<EigenArrayXX>> DfDa,
std::optional<Eigen::Ref<EigenArrayXX>> DsDq,
std::optional<Eigen::Ref<EigenArrayXX>> DsDv,
std::optional<Eigen::Ref<EigenArrayXX>> DsDa,
std::optional<Eigen::Ref<EigenArrayXX>> DmDq) {
if (DfDq.has_value() &&
(DfDq->rows() != m->nv || DfDq->cols() != m->nv)) {
throw py::type_error("DfDq should be of shape (nv, nv)");
}
if (DfDv.has_value() &&
(DfDv->rows() != m->nv || DfDv->cols() != m->nv)) {
throw py::type_error("DfDv should be of shape (nv, nv)");
}
if (DfDa.has_value() &&
(DfDa->rows() != m->nv || DfDa->cols() != m->nv)) {
throw py::type_error("DfDa should be of shape (nv, nv)");
}
if (DsDq.has_value() &&
(DsDq->rows() != m->nv || DsDq->cols() != m->nsensordata)) {
throw py::type_error("DsDq should be of shape (nv, nsensordata)");
}
if (DsDv.has_value() &&
(DsDv->rows() != m->nv || DsDv->cols() != m->nsensordata)) {
throw py::type_error("DsDv should be of shape (nv, nsensordata)");
}
if (DsDa.has_value() &&
(DsDa->rows() != m->nv || DsDa->cols() != m->nsensordata)) {
throw py::type_error("DsDa should be of shape (nv, nsensordata)");
}
if (DmDq.has_value() &&
(DmDq->rows() != m->nv || DmDq->cols() != m->nM)) {
throw py::type_error("DmDq should be of shape (nv, nM)");
}
return InterceptMjErrors(::mjd_inverseFD)(
m, d, eps, flg_actuation,
DfDq.has_value() ? DfDq->data() : nullptr,
DfDv.has_value() ? DfDv->data() : nullptr,
DfDa.has_value() ? DfDa->data() : nullptr,
DsDq.has_value() ? DsDq->data() : nullptr,
DsDv.has_value() ? DsDv->data() : nullptr,
DsDa.has_value() ? DsDa->data() : nullptr,
DmDq.has_value() ? DmDq->data() : nullptr);
});
Def<traits::mju_Halton>(pymodule);
// Skipped: mju_strncpy (doesn't make sense in Python)
Def<traits::mju_sigmoid>(pymodule);