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

Add private function mj_mulJacVec_island for Jacobian multiplication with sub indices corresponding to one island.

Browse Source 
PiperOrigin-RevId: 561648540
Change-Id: I8080a620807e824b40b5d6ba484beb06d081d444
This commit is contained in:
Yuval Tassa
2023-08-31 07:27:34 -07:00
committed by Copybara-Service
parent 9fd186ac2b
commit 85fd922b37
3 changed files with 107 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+39
View File
@@ -395,6 +395,45 @@ void mj_mulJacVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
// multiply Jacobian by vector, for one island
void mj_mulJacVec_island(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec, int island) {
// no island, call regular function
if (island < 0) {
mj_mulJacVec(m, d, res, vec);
return;
}
// sizes
int vecnnz = d->island_dofnum[island];
int resnnz = d->island_efcnum[island];
// indices
int* vecind = d->island_dofind + d->island_dofadr[island];
int* resind = d->island_efcind + d->island_efcadr[island];
// sparse Jacobian
if (mj_isSparse(m)) {
for (int i=0; i < resnnz; i++) {
int row = resind[i];
int Jnnz = d->efc_J_rownnz[row];
int Jrowadr = d->efc_J_rowadr[row];
int* Jind = d->efc_J_colind + Jrowadr;
mjtNum* J = d->efc_J + Jrowadr;
res[i] = mju_dotSparse2(vec, J, vecnnz, vecind, Jnnz, Jind);
}
}
// dense Jacobian
else {
int nv = m->nv;
for (int i=0; i < resnnz; i++) {
res[i] = mju_dotSparse(vec, d->efc_J + nv*resind[i], vecnnz, vecind);
}
}
}
// multiply JacobianT by vector
void mj_mulJacTVec(const mjModel* m, mjData* d, mjtNum* res, const mjtNum* vec) {
// exit if no constraints