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

Move core_constraint arena allocations to local function.

Browse Source 
PiperOrigin-RevId: 558823297
Change-Id: I4a564f0d05c17634e70fbff16e8bdca2e7d7fb63
This commit is contained in:
Yuval Tassa
2023-08-21 10:06:25 -07:00
committed by Copybara-Service
parent 8e54516071
commit 22fd0586b1
1 changed files with 45 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+45 -37
View File
@@ -58,6 +58,48 @@ static inline void clearEfc(mjData* d) {
// allocate efc arrays on arena, return 1 on success, 0 on failure
static int arenaAllocEfc(const mjModel* m, mjData* d) {
#undef MJ_M
#define MJ_M(n) m->n
#undef MJ_D
#define MJ_D(n) d->n
// move arena pointer to end of contact array
d->parena = d->ncon * sizeof(mjContact);
// poison remaining memory
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
#endif
#define X(type, name, nr, nc) \
d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
if (!d->name) { \
mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum)); \
clearEfc(d); \
d->parena = d->ncon * sizeof(mjContact); \
return 0; \
}
MJDATA_ARENA_POINTERS_PRIMAL
if (mj_isDual(m)) {
MJDATA_ARENA_POINTERS_DUAL
}
#undef X
#undef MJ_M
#define MJ_M(n) n
#undef MJ_D
#define MJ_D(n) n
return 1;
}
// determine type of friction cone
int mj_isPyramidal(const mjModel* m) {
if (m->opt.cone == mjCONE_PYRAMIDAL) {
@@ -1613,8 +1655,6 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
//---------------------------- top-level API for constraint construction ---------------------------
// driver: call all functions above
void mj_makeConstraint(const mjModel* m, mjData* d) {
// clear sizes
@@ -1635,43 +1675,11 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
}
d->nefc = nefc_allocated;
// ========== begin arena allocation
#undef MJ_M
#define MJ_M(n) m->n
#undef MJ_D
#define MJ_D(n) d->n
// move arena pointer to end of contact array
d->parena = d->ncon * sizeof(mjContact);
// poison remaining memory
#ifdef ADDRESS_SANITIZER
ASAN_POISON_MEMORY_REGION(
(char*)d->arena + d->parena, (d->nstack - d->pstack) * sizeof(mjtNum) - d->parena);
#endif
#define X(type, name, nr, nc) \
d->name = mj_arenaAlloc(d, sizeof(type) * (nr) * (nc), _Alignof(type)); \
if (!d->name) { \
mj_warning(d, mjWARN_CNSTRFULL, d->nstack * sizeof(mjtNum)); \
clearEfc(d); \
d->parena = d->ncon * sizeof(mjContact); \
return; \
// allocate efc arrays on arena
if (!arenaAllocEfc(m, d)) {
return;
}
MJDATA_ARENA_POINTERS_PRIMAL
if (mj_isDual(m)) {
MJDATA_ARENA_POINTERS_DUAL
}
#undef X
#undef MJ_M
#define MJ_M(n) n
#undef MJ_D
#define MJ_D(n) n
// ========== end arena allocation
// clear tendon_efcadr
for (int i=0; i < m->ntendon; i++) {
d->tendon_efcadr[i] = -1;