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

Rename mj_stackAlloc to mj_stackAllocNum.

Browse Source 
PiperOrigin-RevId: 561040193
Change-Id: I49ea21ffb5b596471d2aacefb1961c11c5c905d0
This commit is contained in:
Saran Tunyasuvunakool
2023-08-29 08:42:05 -07:00
committed by Copybara-Service
parent ea5e00cad8
commit b979a26b10
31 changed files with 187 additions and 185 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_collision_driver.c
+1 -1
View File
@@ -732,7 +732,7 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) {
mju_eig3(eigval, frame, quat, cov);
// allocate AABB; clear world entry (not used)
aabb = mj_stackAlloc(d, 6*nbody);
aabb = mj_stackAllocNum(d, 6*nbody);
mju_zero(aabb, 6);
// construct body AABB for the aligned frame, count collidable
src/engine/engine_core_constraint.c
+15 -15
View File
@@ -433,14 +433,14 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
}
// allocate space
jac[0] = mj_stackAlloc(d, 6*nv);
jac[1] = mj_stackAlloc(d, 6*nv);
jacdif = mj_stackAlloc(d, 6*nv);
jac[0] = mj_stackAllocNum(d, 6*nv);
jac[1] = mj_stackAllocNum(d, 6*nv);
jacdif = mj_stackAllocNum(d, 6*nv);
if (issparse) {
chain = mj_stackAllocInt(d, nv);
chain2 = mj_stackAllocInt(d, nv);
buf_ind = mj_stackAllocInt(d, nv);
sparse_buf = mj_stackAlloc(d, nv);
sparse_buf = mj_stackAllocNum(d, nv);
}
// find active equality constraints
@@ -641,7 +641,7 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
}
// allocate Jacobian
jac = mj_stackAlloc(d, nv);
jac = mj_stackAllocNum(d, nv);
// find frictional dofs
for (int i=0; i < nv; i++) {
@@ -702,7 +702,7 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
}
// allocate Jacobian
jac = mj_stackAlloc(d, nv);
jac = mj_stackAllocNum(d, nv);
// find joint limits
for (int i=0; i < m->njnt; i++) {
@@ -847,13 +847,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
}
// allocate Jacobian
jac = mj_stackAlloc(d, 6*NV);
jacdifp = mj_stackAlloc(d, 3*NV);
jacdifr = mj_stackAlloc(d, 3*NV);
jac1p = mj_stackAlloc(d, 3*NV);
jac2p = mj_stackAlloc(d, 3*NV);
jac1r = mj_stackAlloc(d, 3*NV);
jac2r = mj_stackAlloc(d, 3*NV);
jac = mj_stackAllocNum(d, 6*NV);
jacdifp = mj_stackAllocNum(d, 3*NV);
jacdifr = mj_stackAllocNum(d, 3*NV);
jac1p = mj_stackAllocNum(d, 3*NV);
jac2p = mj_stackAllocNum(d, 3*NV);
jac1r = mj_stackAllocNum(d, 3*NV);
jac2r = mj_stackAllocNum(d, 3*NV);
if (issparse) {
chain = mj_stackAllocInt(d, NV);
}
@@ -1755,8 +1755,8 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
// space for backsubM2(J')' and its traspose
mjtNum* JM2 = mj_stackAlloc(d, nefc*nv);
mjtNum* JM2T = mj_stackAlloc(d, nv*nefc);
mjtNum* JM2 = mj_stackAllocNum(d, nefc*nv);
mjtNum* JM2T = mj_stackAllocNum(d, nv*nefc);
// sparse
if (mj_isSparse(m)) {
src/engine/engine_core_smooth.c
+19 -19
View File
@@ -179,7 +179,7 @@ void mj_kinematics(const mjModel* m, mjData* d) {
void mj_comPos(const mjModel* m, mjData* d) {
mjtNum offset[3], axis[3];
mjMARKSTACK;
mjtNum* mass_subtree = mj_stackAlloc(d, m->nbody);
mjtNum* mass_subtree = mj_stackAllocNum(d, m->nbody);
// clear subtree
mju_zero(mass_subtree, m->nbody);
@@ -401,14 +401,14 @@ void mj_tendon(const mjModel* m, mjData* d) {
}
// allocate space
jac1 = mj_stackAlloc(d, 3*nv);
jac2 = mj_stackAlloc(d, 3*nv);
jacdif = mj_stackAlloc(d, 3*nv);
tmp = mj_stackAlloc(d, nv);
jac1 = mj_stackAllocNum(d, 3*nv);
jac2 = mj_stackAllocNum(d, 3*nv);
jacdif = mj_stackAllocNum(d, 3*nv);
tmp = mj_stackAllocNum(d, nv);
if (issparse) {
chain = mj_stackAllocInt(d, nv);
buf_ind = mj_stackAllocInt(d, nv);
sparse_buf = mj_stackAlloc(d, nv);
sparse_buf = mj_stackAllocNum(d, nv);
}
// clear results
@@ -629,9 +629,9 @@ void mj_transmission(const mjModel* m, mjData* d) {
}
// allocate space, clear moments
jac = mj_stackAlloc(d, 3*nv);
jacA = mj_stackAlloc(d, 3*nv);
jacS = mj_stackAlloc(d, 3*nv);
jac = mj_stackAllocNum(d, 3*nv);
jacA = mj_stackAllocNum(d, 3*nv);
jacS = mj_stackAllocNum(d, 3*nv);
mju_zero(moment, nu*nv);
// define variables required for body transmission, don't allocate
@@ -804,7 +804,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
// reference site defined
else {
int refid = m->actuator_trnid[2*i+1];
if (!jacref) jacref = mj_stackAlloc(d, 3*nv);
if (!jacref) jacref = mj_stackAllocNum(d, 3*nv);
// clear length
length[i] = 0;
@@ -855,7 +855,7 @@ void mj_transmission(const mjModel* m, mjData* d) {
mju_rotVecMat(wrench, gear+3, d->site_xmat+9*refid);
// moment_tmp: global Jacobian projected on wrench, add to moment
if (!moment_tmp) moment_tmp = mj_stackAlloc(d, nv);
if (!moment_tmp) moment_tmp = mj_stackAllocNum(d, nv);
mju_mulMatTVec(moment_tmp, jacS, wrench, 3, nv);
mju_addTo(moment+i*nv, moment_tmp, nv);
}
@@ -871,11 +871,11 @@ void mj_transmission(const mjModel* m, mjData* d) {
{
// allocate stack variables for the first mjTRN_BODY
if (!efc_force) {
efc_force = mj_stackAlloc(d, d->nefc);
moment_exclude = mj_stackAlloc(d, nv);
jacdifp = mj_stackAlloc(d, 3*nv);
jac1p = mj_stackAlloc(d, 3*nv);
jac2p = mj_stackAlloc(d, 3*nv);
efc_force = mj_stackAllocNum(d, d->nefc);
moment_exclude = mj_stackAllocNum(d, nv);
jacdifp = mj_stackAllocNum(d, 3*nv);
jac1p = mj_stackAllocNum(d, 3*nv);
jac2p = mj_stackAllocNum(d, 3*nv);
chain = issparse ? mj_stackAllocInt(d, nv) : NULL;
}
@@ -1324,7 +1324,7 @@ void mj_comVel(const mjModel* m, mjData* d) {
void mj_subtreeVel(const mjModel* m, mjData* d) {
mjtNum dx[3], dv[3], dp[3], dL[3];
mjMARKSTACK;
mjtNum* body_vel = mj_stackAlloc(d, 6*m->nbody);
mjtNum* body_vel = mj_stackAllocNum(d, 6*m->nbody);
// bodywise quantities
for (int i=0; i < m->nbody; i++) {
@@ -1390,8 +1390,8 @@ void mj_subtreeVel(const mjModel* m, mjData* d) {
void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result) {
mjtNum tmp[6], tmp1[6];
mjMARKSTACK;
mjtNum* loc_cacc = mj_stackAlloc(d, m->nbody*6);
mjtNum* loc_cfrc_body = mj_stackAlloc(d, m->nbody*6);
mjtNum* loc_cacc = mj_stackAllocNum(d, m->nbody*6);
mjtNum* loc_cfrc_body = mj_stackAllocNum(d, m->nbody*6);
// set world acceleration to -gravity
mju_zero(loc_cacc, 6);
src/engine/engine_derivative.c
+16 -16
View File
@@ -400,11 +400,11 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
mjMARKSTACK;
mjtNum* Dcvel = mj_stackAlloc(d, nbody*6*nv);
mjtNum* Dcdofdot = mj_stackAlloc(d, nv*6*nv);
mjtNum* Dcacc = mj_stackAlloc(d, nbody*6*nv);
mjtNum* Dcfrcbody = mj_stackAlloc(d, nbody*6*nv);
mjtNum* row = mj_stackAlloc(d, nv);
mjtNum* Dcvel = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* Dcdofdot = mj_stackAllocNum(d, nv*6*nv);
mjtNum* Dcacc = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* Dcfrcbody = mj_stackAllocNum(d, nbody*6*nv);
mjtNum* row = mj_stackAllocNum(d, nv);
// compute Dcvel and Dcdofdot
mjd_comVel_vel_dense(m, d, Dcvel, Dcdofdot);
@@ -615,11 +615,11 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
mjMARKSTACK;
mjtNum* Dcdofdot = mj_stackAlloc(d, 6*m->nD);
mjtNum* Dcvel = mj_stackAlloc(d, 6*m->nB);
mjtNum* Dcacc = mj_stackAlloc(d, 6*m->nB);
mjtNum* Dcfrcbody = mj_stackAlloc(d, 6*m->nB);
mjtNum* row = mj_stackAlloc(d, nv);
mjtNum* Dcdofdot = mj_stackAllocNum(d, 6*m->nD);
mjtNum* Dcvel = mj_stackAllocNum(d, 6*m->nB);
mjtNum* Dcacc = mj_stackAllocNum(d, 6*m->nB);
mjtNum* Dcfrcbody = mj_stackAllocNum(d, 6*m->nB);
mjtNum* row = mj_stackAllocNum(d, nv);
// clear
mju_zero(Dcdofdot, 6*m->nD);
@@ -735,7 +735,7 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
// allocate dense row
mjMARKSTACK;
mjtNum* row = mj_stackAlloc(d, nv);
mjtNum* row = mj_stackAllocNum(d, nv);
// process non-zero elements of B
for (int i=0; i < n; i++) {
@@ -774,7 +774,7 @@ static void addJTBJSparse(
// allocate row
mjMARKSTACK;
mjtNum* row = mj_stackAlloc(d, nv);
mjtNum* row = mj_stackAllocNum(d, nv);
// compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) )
for (int i = 0; i < n; i++) {
@@ -1231,8 +1231,8 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
int nv = m->nv;
int nnz = nv;
int rownnz[6], rowadr[6];
mjtNum* J = mj_stackAlloc(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 3*nv);
mjtNum* J = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAllocNum(d, 3*nv);
int* colind = mj_stackAllocInt(d, 6*nv);
int* colind_compressed = mj_stackAllocInt(d, 6*nv);
@@ -1342,8 +1342,8 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
int nv = m->nv;
int rownnz[6], rowadr[6];
mjtNum* J = mj_stackAlloc(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 3*nv);
mjtNum* J = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAllocNum(d, 3*nv);
int* colind = mj_stackAllocInt(d, 6*nv);
mjtNum lvel[6], wind[6], lwind[6], box[3], B;
src/engine/engine_derivative_fd.c
+25 -25
View File
@@ -197,8 +197,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* qfrc_passive = mj_stackAlloc(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv);
mjtNum* qfrc_passive = mj_stackAllocNum(d, nv);
mjtNum* fd = mj_stackAllocNum(d, nv);
int* cnt = mj_stackAllocInt(d, nv);
// clear row counters
@@ -248,9 +248,9 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* plus = mj_stackAlloc(d, nv);
mjtNum* minus = mj_stackAlloc(d, nv);
mjtNum* fd = mj_stackAlloc(d, nv);
mjtNum* plus = mj_stackAllocNum(d, nv);
mjtNum* minus = mj_stackAllocNum(d, nv);
mjtNum* fd = mj_stackAllocNum(d, nv);
int* cnt = mj_stackAllocInt(d, nv);
// clear row counters
@@ -332,22 +332,22 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
mjMARKSTACK;
// states
mjtNum *state = mj_stackAlloc(d, nq+nv+na); // current state
mjtNum *next = mj_stackAlloc(d, nq+nv+na); // next state
mjtNum *next_plus = mj_stackAlloc(d, nq+nv+na); // forward-nudged next state
mjtNum *next_minus = mj_stackAlloc(d, nq+nv+na); // backward-nudged next state
mjtNum *state = mj_stackAllocNum(d, nq+nv+na); // current state
mjtNum *next = mj_stackAllocNum(d, nq+nv+na); // next state
mjtNum *next_plus = mj_stackAllocNum(d, nq+nv+na); // forward-nudged next state
mjtNum *next_minus = mj_stackAllocNum(d, nq+nv+na); // backward-nudged next state
// warmstart accelerations
mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAlloc(d, nv);
mjtNum *warmstart = mjDISABLED(mjDSBL_WARMSTART) ? NULL : mj_stackAllocNum(d, nv);
// sensors
int skipsensor = !DsDq && !DsDv && !DsDa && !DsDu;
mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values
mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAlloc(d, ns); // forward-nudged sensors
mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAlloc(d, ns); // backward-nudged sensors
mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values
mjtNum *sensor_plus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // forward-nudged sensors
mjtNum *sensor_minus = skipsensor ? NULL : mj_stackAllocNum(d, ns); // backward-nudged sensors
// controls
mjtNum *ctrl = mj_stackAlloc(d, nu);
mjtNum *ctrl = mj_stackAllocNum(d, nu);
// save current inputs
mjtNum time = d->time;
@@ -508,7 +508,7 @@ void mjd_stepFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered,
// finite-difference positions: skip=mjSTAGE_NONE
if (DyDq || DsDq) {
mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation
mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation
for (int i=0; i < nv; i++) {
// nudge forward
mju_zero(dpos, nv);
@@ -582,10 +582,10 @@ void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_cente
mjMARKSTACK;
// allocate transposed matrices
mjtNum *AT = A ? mj_stackAlloc(d, ndx*ndx) : NULL; // state-transition matrix (transposed)
mjtNum *BT = B ? mj_stackAlloc(d, nu*ndx) : NULL; // control-transition matrix (transposed)
mjtNum *CT = C ? mj_stackAlloc(d, ndx*ns) : NULL; // state-observation matrix (transposed)
mjtNum *DT = D ? mj_stackAlloc(d, nu*ns) : NULL; // control-observation matrix (transposed)
mjtNum *AT = A ? mj_stackAllocNum(d, ndx*ndx) : NULL; // state-transition matrix (transposed)
mjtNum *BT = B ? mj_stackAllocNum(d, nu*ndx) : NULL; // control-transition matrix (transposed)
mjtNum *CT = C ? mj_stackAllocNum(d, ndx*ns) : NULL; // state-observation matrix (transposed)
mjtNum *DT = D ? mj_stackAllocNum(d, nu*ns) : NULL; // control-observation matrix (transposed)
// set offset pointers
if (A) {
@@ -648,11 +648,11 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
int skipsensor = !DsDq && !DsDv && !DsDa;
// local vectors
mjtNum *pos = mj_stackAlloc(d, nq); // position
mjtNum *force = mj_stackAlloc(d, nv); // force
mjtNum *force_plus = mj_stackAlloc(d, nv); // nudged force
mjtNum *sensor = skipsensor ? NULL : mj_stackAlloc(d, ns); // sensor values
mjtNum *mass = DmDq ? mj_stackAlloc(d, nM) : NULL; // mass matrix
mjtNum *pos = mj_stackAllocNum(d, nq); // position
mjtNum *force = mj_stackAllocNum(d, nv); // force
mjtNum *force_plus = mj_stackAllocNum(d, nv); // nudged force
mjtNum *sensor = skipsensor ? NULL : mj_stackAllocNum(d, ns); // sensor values
mjtNum *mass = DmDq ? mj_stackAllocNum(d, nM) : NULL; // mass matrix
// save current positions
mju_copy(pos, d->qpos, nq);
@@ -706,7 +706,7 @@ void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuatio
// position: skip = mjSTAGE_NONE
if (DfDq || DsDq || DmDq) {
mjtNum *dpos = mj_stackAlloc(d, nv); // allocate position perturbation
mjtNum *dpos = mj_stackAllocNum(d, nv); // allocate position perturbation
for (int i=0; i < nv; i++) {
// nudge
mju_zero(dpos, nv);
src/engine/engine_forward.c
+13 -13
View File
@@ -200,7 +200,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) {
// local, clamped copy of ctrl
mjMARKSTACK;
mjtNum *ctrl = mj_stackAlloc(d, nu);
mjtNum *ctrl = mj_stackAllocNum(d, nu);
if (mjDISABLED(mjDSBL_CLAMPCTRL)) {
mju_copy(ctrl, d->ctrl, nu);
} else {
@@ -426,7 +426,7 @@ static void warmstart(const mjModel* m, mjData* d) {
// warmstart with best of (qacc_warmstart, qacc_smooth)
if (!mjDISABLED(mjDSBL_WARMSTART)) {
mjMARKSTACK;
mjtNum* jar = mj_stackAlloc(d, nefc);
mjtNum* jar = mj_stackAllocNum(d, nefc);
// start with qacc = qacc_warmstart
mju_copy(d->qacc, d->qacc_warmstart, nv);
@@ -443,7 +443,7 @@ static void warmstart(const mjModel* m, mjData* d) {
if (m->opt.solver == mjSOL_PGS) {
// cost(force_warmstart)
mjtNum PGS_warmstart = mju_dot(d->efc_force, d->efc_b, nefc);
mjtNum* ARf = mj_stackAlloc(d, nefc);
mjtNum* ARf = mj_stackAllocNum(d, nefc);
if (mj_isSparse(m))
mju_mulMatVecSparse(ARf, d->efc_AR, d->efc_force, nefc,
d->efc_AR_rownnz, d->efc_AR_rowadr,
@@ -463,7 +463,7 @@ static void warmstart(const mjModel* m, mjData* d) {
// non-PGS
else {
// add Gauss to cost(qacc_warmstart)
mjtNum* Ma = mj_stackAlloc(d, nv);
mjtNum* Ma = mj_stackAllocNum(d, nv);
mj_mulM(m, d, Ma, d->qacc_warmstart);
for (int i=0; i < nv; i++) {
cost_warmstart += 0.5*(Ma[i]-d->qfrc_smooth[i])*(d->qacc_warmstart[i]-d->qacc_smooth[i]);
@@ -590,8 +590,8 @@ static void mj_advance(const mjModel* m, mjData* d,
void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
int nv = m->nv, nM = m->nM;
mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv);
mjtNum* qacc = mj_stackAlloc(d, nv);
mjtNum* qfrc = mj_stackAllocNum(d, nv);
mjtNum* qacc = mj_stackAllocNum(d, nv);
// check for dof damping if disable flag is not set
int dof_damping = 0;
@@ -612,7 +612,7 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// damping: integrate implicitly
else {
if (!skipfactor) {
mjtNum* MhB = mj_stackAlloc(d, nM);
mjtNum* MhB = mj_stackAllocNum(d, nM);
// MhB = M + h*diag(B)
mju_copy(MhB, d->qM, m->nM);
@@ -673,10 +673,10 @@ void mj_RungeKutta(const mjModel* m, mjData* d, int N) {
}
// allocate space for intermediate solutions
dX = mj_stackAlloc(d, 2*nv+na);
dX = mj_stackAllocNum(d, 2*nv+na);
for (int i=0; i < N; i++) {
X[i] = mj_stackAlloc(d, nq+nv+na);
F[i] = mj_stackAlloc(d, nv+na);
X[i] = mj_stackAllocNum(d, nq+nv+na);
F[i] = mj_stackAllocNum(d, nv+na);
}
// precompute C and T; C,T,A have size (N-1)
@@ -756,8 +756,8 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv);
mjtNum* qacc = mj_stackAlloc(d, nv);
mjtNum* qfrc = mj_stackAllocNum(d, nv);
mjtNum* qacc = mj_stackAllocNum(d, nv);
// set qfrc = qfrc_smooth + qfrc_constraint
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
@@ -790,7 +790,7 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mjd_smooth_vel(m, d, /* flg_bias = */ 0);
// modified mass matrix MhB = qDeriv[Lower]
mjtNum* MhB = mj_stackAlloc(d, m->nM);
mjtNum* MhB = mj_stackAllocNum(d, m->nM);
mj_copyD2MSparse(m, d, MhB, d->qDeriv);
// set MhB = M - dt*qDeriv
src/engine/engine_inverse.c
+9 -9
View File
@@ -96,7 +96,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjtNum *qacc = d->qacc;
mjMARKSTACK;
mjtNum* qfrc = mj_stackAlloc(d, nv);
mjtNum* qfrc = mj_stackAllocNum(d, nv);
// use selected integrator
switch ((mjtIntegrator) m->opt.integrator) {
@@ -149,11 +149,11 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjd_smooth_vel(m, d, /* flg_bias = */ 0);
// save mass matrix
mjtNum* qMsave = mj_stackAlloc(d, m->nM);
mjtNum* qMsave = mj_stackAllocNum(d, m->nM);
mju_copy(qMsave, d->qM, m->nM);
// set M = M - dt*qDeriv (reduced to M nonzeros)
mjtNum* qDerivReduced = mj_stackAlloc(d, m->nM);
mjtNum* qDerivReduced = mj_stackAllocNum(d, m->nM);
mj_copyD2MSparse(m, d, qDerivReduced, d->qDeriv);
mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM);
@@ -186,7 +186,7 @@ void mj_invConstraint(const mjModel* m, mjData* d) {
}
mjMARKSTACK;
mjtNum* jar = mj_stackAlloc(d, nefc);
mjtNum* jar = mj_stackAllocNum(d, nefc);
// compute jar = Jac*qacc - aref
mj_mulJacVec(m, d, jar, d->qacc);
@@ -233,7 +233,7 @@ void mj_inverseSkip(const mjModel* m, mjData* d,
if (mjENABLED(mjENBL_INVDISCRETE)) {
// save current qacc
qacc = mj_stackAlloc(d, nv);
qacc = mj_stackAllocNum(d, nv);
mju_copy(qacc, d->qacc, nv);
// modify qacc in-place
@@ -286,10 +286,10 @@ void mj_compareFwdInv(const mjModel* m, mjData* d) {
}
// allocate
qforce = mj_stackAlloc(d, nv);
dif = mj_stackAlloc(d, nv);
save_qfrc_constraint = mj_stackAlloc(d, nv);
save_efc_force = mj_stackAlloc(d, nefc);
qforce = mj_stackAllocNum(d, nv);
dif = mj_stackAllocNum(d, nv);
save_qfrc_constraint = mj_stackAllocNum(d, nv);
save_efc_force = mj_stackAllocNum(d, nefc);
// qforce = qfrc_applied + J'*xfrc_applied + qfrc_actuator
// should equal result of inverse dynamics
src/engine/engine_io.c
+1 -1
View File
@@ -1266,7 +1266,7 @@ void* mj_stackAllocByte(mjData* d, size_t size) {
return (void*)start_ptr;
}
mjtNum* mj_stackAlloc(mjData* d, int size) {
mjtNum* mj_stackAllocNum(mjData* d, int size) {
return (mjtNum*)mj_stackAllocByte(d, size * sizeof(mjtNum));
}
src/engine/engine_io.h
+1 -1
View File
@@ -103,7 +103,7 @@ MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key);
MJAPI void* mj_arenaAlloc(mjData* d, int bytes, int alignment);
// mjData stack allocate for array of mjtNums
MJAPI mjtNum* mj_stackAlloc(mjData* d, int size);
MJAPI mjtNum* mj_stackAllocNum(mjData* d, int size);
// mjData stack allocate for array of ints
MJAPI int* mj_stackAllocInt(mjData* d, int size);
src/engine/engine_print.c
+1 -1
View File
@@ -755,7 +755,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
}
// allocate full inertia
M = mj_stackAlloc(d, m->nv*m->nv);
M = mj_stackAllocNum(d, m->nv*m->nv);
#ifdef MEMORY_SANITIZER
// If memory sanitizer is active, d->buffer will be marked as poisoned, even
src/engine/engine_ray.c
+1 -1
View File
@@ -1161,7 +1161,7 @@ void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum*
mjMARKSTACK;
// allocate source
mjtNum* geom_ba = mj_stackAlloc(d, 4*m->ngeom);
mjtNum* geom_ba = mj_stackAllocNum(d, 4*m->ngeom);
int* geom_eliminate = mj_stackAllocInt(d, m->ngeom);
// initialize source
src/engine/engine_sensor.c
+1 -1
View File
@@ -861,7 +861,7 @@ void mj_energyVel(const mjModel* m, mjData* d) {
return;
}
vec = mj_stackAlloc(d, m->nv);
vec = mj_stackAllocNum(d, m->nv);
// kinetic energy: 0.5 * qvel' * M * qvel
mj_mulM(m, d, vec, d->qvel);
src/engine/engine_setconst.c
+3 -3
View File
@@ -62,8 +62,8 @@ static void set0(mjModel* m, mjData* d) {
int id, id1, id2, dnum, nv = m->nv;
mjtNum A[36] = {0}, pos[3], quat[4];
mjMARKSTACK;
mjtNum* jac = mj_stackAlloc(d, 6*nv);
mjtNum* tmp = mj_stackAlloc(d, 6*nv);
mjtNum* jac = mj_stackAllocNum(d, 6*nv);
mjtNum* tmp = mj_stackAllocNum(d, 6*nv);
int* cammode = 0;
int* lightmode = 0;
@@ -284,7 +284,7 @@ static void setStat(mjModel* m, mjData* d) {
mjtNum xmax[3] = {-1E+10, -1E+10, -1E+10};
mjtNum rbound;
mjMARKSTACK;
mjtNum* body = mj_stackAlloc(d, m->nbody);
mjtNum* body = mj_stackAllocNum(d, m->nbody);
// compute bounding box of bodies, joint centers, geoms and sites
for (int i=1; i < m->nbody; i++) {
src/engine/engine_solver.c
+20 -20
View File
@@ -309,7 +309,7 @@ void mj_solPGS(const mjModel* m, mjData* d, int maxiter) {
mjtNum v[6], v1[6], Athis[36], Ac[25], bc[5], res[6], oldforce[6];
mjContact* con;
mjMARKSTACK;
mjtNum* ARinv = mj_stackAlloc(d, nefc);
mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc);
// precompute inverse diagonal of AR
@@ -511,7 +511,7 @@ void mj_solNoSlip(const mjModel* m, mjData* d, int maxiter) {
mjtNum v[5], Ac[25], bc[5], res[5], oldforce[5], delta[5], mid, y, K0, K1;
mjContact* con;
mjMARKSTACK;
mjtNum* ARinv = mj_stackAlloc(d, nefc);
mjtNum* ARinv = mj_stackAllocNum(d, nefc);
int* oldstate = mj_stackAllocInt(d, nefc);
// precompute inverse diagonal of A
@@ -755,20 +755,20 @@ static void CGallocate(const mjModel* m, mjData* d,
memset(ctx, 0, sizeof(mjCGContext));
// common arrays
ctx->Jaref = mj_stackAlloc(d, nefc);
ctx->Jv = mj_stackAlloc(d, nefc);
ctx->Ma = mj_stackAlloc(d, nv);
ctx->Mv = mj_stackAlloc(d, nv);
ctx->grad = mj_stackAlloc(d, nv);
ctx->Mgrad = mj_stackAlloc(d, nv);
ctx->search = mj_stackAlloc(d, nv);
ctx->quad = mj_stackAlloc(d, nefc*3);
ctx->Jaref = mj_stackAllocNum(d, nefc);
ctx->Jv = mj_stackAllocNum(d, nefc);
ctx->Ma = mj_stackAllocNum(d, nv);
ctx->Mv = mj_stackAllocNum(d, nv);
ctx->grad = mj_stackAllocNum(d, nv);
ctx->Mgrad = mj_stackAllocNum(d, nv);
ctx->search = mj_stackAllocNum(d, nv);
ctx->quad = mj_stackAllocNum(d, nefc*3);
// Hessian (Newton only)
ctx->flg_Newton = flg_Newton;
if (flg_Newton) {
ctx->H = mj_stackAlloc(d, nv*nv);
ctx->Hcone = mj_stackAlloc(d, nv*nv);
ctx->H = mj_stackAllocNum(d, nv*nv);
ctx->Hcone = mj_stackAllocNum(d, nv*nv);
ctx->rownnz = mj_stackAllocInt(d, nv);
ctx->rowadr = mj_stackAllocInt(d, nv);
ctx->colind = mj_stackAllocInt(d, nv*nv);
@@ -1279,8 +1279,8 @@ static void HessianCone(const mjModel* m, mjData* d, mjCGContext* ctx) {
mjMARKSTACK;
// storage for L'*J
mjtNum* LTJ = mj_stackAlloc(d, 6*nv);
mjtNum* LTJ_row = mj_stackAlloc(d, nv);
mjtNum* LTJ = mj_stackAllocNum(d, 6*nv);
mjtNum* LTJ_row = mj_stackAllocNum(d, nv);
int* LTJ_ind = mj_stackAllocInt(d, nv);
// start with Hcone = H
@@ -1357,7 +1357,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
mjMARKSTACK;
// compute D corresponding to quad states
mjtNum* D = mj_stackAlloc(d, nefc);
mjtNum* D = mj_stackAllocNum(d, nefc);
for (int i=0; i < nefc; i++) {
if (d->efc_state[i] == mjCNSTRSTATE_QUADRATIC) {
D[i] = d->efc_D[i];
@@ -1372,7 +1372,7 @@ static void HessianDirect(const mjModel* m, mjData* d, mjCGContext* ctx) {
int nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz);
mjtNum* M = mj_stackAlloc(d, nnz);
mjtNum* M = mj_stackAllocNum(d, nnz);
mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
// compute H = J'*D*J
@@ -1446,7 +1446,7 @@ static void HessianIncremental(const mjModel* m, mjData* d,
mjMARKSTACK;
// local space
mjtNum* vec = mj_stackAlloc(d, nv);
mjtNum* vec = mj_stackAllocNum(d, nv);
int* vec_ind = mj_stackAllocInt(d, nv);
// clear update counter
@@ -1521,9 +1521,9 @@ static void mj_solCGNewton(const mjModel* m, mjData* d, int maxiter, int flg_New
// allocate local storage
if (!flg_Newton) {
gradold = mj_stackAlloc(d, nv);
Mgradold = mj_stackAlloc(d, nv);
Mgraddif = mj_stackAlloc(d, nv);
gradold = mj_stackAllocNum(d, nv);
Mgradold = mj_stackAllocNum(d, nv);
Mgraddif = mj_stackAllocNum(d, nv);
}
int* oldstate = mj_stackAllocInt(d, nefc);
src/engine/engine_support.c
+8 -8
View File
@@ -279,7 +279,7 @@ void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr
void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body) {
int nv = m->nv;
mjMARKSTACK;
mjtNum* jacp_b = mj_stackAlloc(d, 3*nv);
mjtNum* jacp_b = mj_stackAllocNum(d, 3*nv);
// clear output
mju_zero(jacp, 3*nv);
@@ -325,8 +325,8 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
// get full Jacobian of point
mjMARKSTACK;
mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAlloc(d, 3*nv));
mjtNum* jacr = mj_stackAlloc(d, 3*nv);
mjtNum* jacp = (jacPoint ? jacPoint : mj_stackAllocNum(d, 3*nv));
mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mj_jac(m, d, jacp, jacr, point, body);
// jacAxis_col = cross(jacr_col, axis)
@@ -957,7 +957,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int nnz = m->nD; // use sparse dof-dof matrix
int* M_rownnz = mj_stackAllocInt(d, nv); // actual nnz count
int* M_colind = mj_stackAllocInt(d, nnz);
mjtNum* M = mj_stackAlloc(d, nnz);
mjtNum* M = mj_stackAllocNum(d, nnz);
mj_makeMSparse(m, d, M, M_rownnz, NULL, M_colind);
mj_addMSparse(m, d, dst, rownnz, rowadr, colind, M,
@@ -1051,7 +1051,7 @@ void mj_addMSparse(const mjModel* m, mjData* d, mjtNum* dst,
mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, nv);
mjtNum* sparse_buf = mj_stackAlloc(d, nv);
mjtNum* sparse_buf = mj_stackAllocNum(d, nv);
// add to destination
for (int i=0; i < nv; i++) {
@@ -1162,9 +1162,9 @@ void mj_applyFT(const mjModel* m, mjData* d,
// allocate local variables
mjMARKSTACK;
mjtNum* jacp = mj_stackAlloc(d, 3*nv);
mjtNum* jacr = mj_stackAlloc(d, 3*nv);
mjtNum* qforce = mj_stackAlloc(d, nv);
mjtNum* jacp = mj_stackAllocNum(d, 3*nv);
mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mjtNum* qforce = mj_stackAllocNum(d, nv);
// make sure body is in range
if (body < 0 || body >= m->nbody) {
src/engine/engine_util_solve.c
+2 -2
View File
@@ -150,7 +150,7 @@ int mju_cholFactorSparse(mjtNum* mat, int n, mjtNum mindiag,
mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, n);
mjtNum* sparse_buf = mj_stackAlloc(d, n);
mjtNum* sparse_buf = mj_stackAllocNum(d, n);
// shrink rows so that rownnz ends at diagonal
for (int r=0; r < n; r++) {
@@ -256,7 +256,7 @@ int mju_cholUpdateSparse(mjtNum* mat, mjtNum* x, int n, int flg_plus,
mjData* d) {
mjMARKSTACK;
int* buf_ind = mj_stackAllocInt(d, n);
mjtNum* sparse_buf = mj_stackAlloc(d, n);
mjtNum* sparse_buf = mj_stackAllocNum(d, n);
// backpass over rows corresponding to non-zero x(r)
int rank = n, i = x_nnz - 1;
src/engine/engine_util_sparse.c
+1 -1
View File
@@ -559,7 +559,7 @@ void mju_sqrMatTDSparse(mjtNum* res, const mjtNum* mat, const mjtNum* matT,
mjMARKSTACK;
// a dense row buffer that stores the current row in the resulting matrix
mjtNum* buffer = mj_stackAlloc(d, nc);
mjtNum* buffer = mj_stackAllocNum(d, nc);
// these mark the currently set columns in the dense row buffer,
// used for when creating the resulting sparse row
src/engine/engine_vis_interact.c
+2 -2
View File
@@ -524,8 +524,8 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
int sel = pert->select;
mjtNum headpos[3], forward[3], dif[3];
mjtNum* jac = mj_stackAlloc(d, 3*nv);
mjtNum* jacM2 = mj_stackAlloc(d, 3*nv);
mjtNum* jac = mj_stackAllocNum(d, 3*nv);
mjtNum* jacM2 = mj_stackAllocNum(d, 3*nv);
// invalid selected body: return
if (sel <= 0 || sel >= m->nbody) {