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

Add sensor matrices to mjd_transitionFD.

Browse Source 
- Also add missing `mj_jacSubtreeCom` to Python bindings.

PiperOrigin-RevId: 471610181
Change-Id: I31410d194527ce6a7bdbd01e176f3a41efa52d84
This commit is contained in:
Yuval Tassa
2022-09-01 13:00:02 -07:00
committed by Copybara-Service
parent 46da1285af
commit e3a82247c2
7 changed files with 157 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
python/mujoco/functions.cc
+25 -2
View File
@@ -355,6 +355,17 @@ PYBIND11_MODULE(_functions, pymodule) {
m, d, jacp.has_value() ? jacp->data() : nullptr,
jacr.has_value() ? jacr->data() : nullptr, body);
});
Def<traits::mj_jacSubtreeCom>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d,
std::optional<Eigen::Ref<EigenArrayXX>> jacp, int body) {
if (jacp.has_value() &&
(jacp->rows() != 3 || jacp->cols() != m->nv)) {
throw py::type_error("jacp should be of shape (3, nv)");
}
return InterceptMjErrors(::mj_jacSubtreeCom)(
m, d, jacp.has_value() ? jacp->data() : nullptr, body);
});
Def<traits::mj_jacGeom>(
pymodule,
[](const raw::MjModel* m, raw::MjData* d,
@@ -1033,7 +1044,9 @@ PYBIND11_MODULE(_functions, pymodule) {
pymodule,
[](const raw::MjModel* m, raw::MjData* d, mjtNum eps, mjtByte centered,
std::optional<Eigen::Ref<EigenArrayXX>> A,
std::optional<Eigen::Ref<EigenArrayXX>> B) {
std::optional<Eigen::Ref<EigenArrayXX>> B,
std::optional<Eigen::Ref<EigenArrayXX>> C,
std::optional<Eigen::Ref<EigenArrayXX>> D) {
if (A.has_value() &&
(A->rows() != 2*m->nv+m->na || A->cols() != 2*m->nv+m->na)) {
throw py::type_error("A should be of shape (2*nv+na, 2*nv+na)");
@@ -1042,10 +1055,20 @@ PYBIND11_MODULE(_functions, pymodule) {
(B->rows() != 2*m->nv+m->na || B->cols() != m->nu)) {
throw py::type_error("B should be of shape (2*nv+na, nu)");
}
if (C.has_value() &&
(C->rows() != m->nsensordata || C->cols() != 2*m->nv+m->na)) {
throw py::type_error("C should be of shape (nsensordata, 2*nv+na)");
}
if (D.has_value() &&
(D->rows() != m->nsensordata || D->cols() != m->nu)) {
throw py::type_error("D should be of shape (nsensordata, nu)");
}
return InterceptMjErrors(::mjd_transitionFD)(
m, d, eps, centered,
A.has_value() ? A->data() : nullptr,
B.has_value() ? B->data() : nullptr);
B.has_value() ? B->data() : nullptr,
C.has_value() ? C->data() : nullptr,
D.has_value() ? D->data() : nullptr);
});
Def<traits::mju_Halton>(pymodule);
// Skipped: mju_strncpy (doesn't make sense in Python)