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

Delete sparsity structures from mjData, use the ones in mjModel

Browse Source 
PiperOrigin-RevId: 795067950
Change-Id: If4f52e40ac49f7c6986b33ef9c8b534199dfd21a
This commit is contained in:
Yuval Tassa
2025-08-14 09:11:47 -07:00
committed by Copybara-Service
parent 7eb13777f8
commit 977f94e9df
25 changed files with 120 additions and 349 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+6 -6
View File
@@ -2037,7 +2037,7 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
// inverse square root of D from inertia LDL decomposition
mjtNum* sqrtInvD = mjSTACKALLOC(d, nv, mjtNum);
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
sqrtInvD[i] = 1 / mju_sqrt(d->qLD[diag]);
}
@@ -2074,10 +2074,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
}
// traverse row j of C, marking new unique nonzeros
int nnzC = d->M_rownnz[j];
int adrC = d->M_rowadr[j];
int nnzC = m->M_rownnz[j];
int adrC = m->M_rowadr[j];
for (int k=0; k < nnzC; k++) {
int c = d->M_colind[adrC + k];
int c = m->M_colind[adrC + k];
if (marker[c] != r) {
marker[c] = r;
nnz++;
@@ -2157,10 +2157,10 @@ void mj_projectConstraint(const mjModel* m, mjData* d) {
continue;
}
int j = B_colind[i];
int adrC = d->M_rowadr[j];
int adrC = m->M_rowadr[j];
mju_addToSclSparseInc(B + adrB, d->qLD + adrC,
nnzB, B_colind + adrB,
d->M_rownnz[j]-1, d->M_colind + adrC, -b);
m->M_rownnz[j]-1, m->M_colind + adrC, -b);
}
// B(r,:) <- sqrt(inv(D)) * B(r,:)
src/engine/engine_core_smooth.c
+11 -11
View File
@@ -1472,9 +1472,9 @@ void mj_transmission(const mjModel* m, mjData* d) {
// add tendon armature to M
void mj_tendonArmature(const mjModel* m, mjData* d) {
int nv = m->nv, ntendon = m->ntendon, issparse = mj_isSparse(m);
const int* M_rownnz = d->M_rownnz;
const int* M_rowadr = d->M_rowadr;
const int* M_colind = d->M_colind;
const int* M_rownnz = m->M_rownnz;
const int* M_rowadr = m->M_rowadr;
const int* M_colind = m->M_colind;
for (int k=0; k < ntendon; k++) {
mjtNum armature = m->tendon_armature[k];
@@ -1555,7 +1555,7 @@ void mj_crb(const mjModel* m, mjData* d) {
if (m->dof_simplenum[i]) {
int n = i + m->dof_simplenum[i];
for (; i < n; i++) {
d->M[d->M_rowadr[i]] = m->dof_M0[i];
d->M[m->M_rowadr[i]] = m->dof_M0[i];
}
// finish or else fall through with next row
@@ -1565,7 +1565,7 @@ void mj_crb(const mjModel* m, mjData* d) {
}
// init M(i,i) with armature inertia
int Madr_ij = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int Madr_ij = m->M_rowadr[i] + m->M_rownnz[i] - 1;
d->M[Madr_ij] = m->dof_armature[i];
// precompute buf = crb_body_i * cdof_i
@@ -1585,7 +1585,7 @@ void mj_makeM(const mjModel* m, mjData* d) {
TM_START;
mj_crb(m, d);
mj_tendonArmature(m, d);
mju_scatter(d->qM, d->M, d->mapM2M, m->nC);
mju_scatter(d->qM, d->M, m->mapM2M, m->nC);
TM_END(mjTIMER_POS_INERTIA);
}
@@ -1659,7 +1659,7 @@ void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, mjtNum* qLD
void mj_factorM(const mjModel* m, mjData* d) {
TM_START;
mju_copy(d->qLD, d->M, m->nC);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qLD, d->qLDiagInv, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind);
TM_ADD(mjTIMER_POS_INERTIA);
}
@@ -1893,7 +1893,7 @@ void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n) {
mju_copy(x, y, n*m->nv);
}
mj_solveLD(x, d->qLD, d->qLDiagInv, m->nv, n,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
@@ -1904,9 +1904,9 @@ void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y,
int nv = m->nv;
// local copies of key variables
const int* rownnz = d->M_rownnz;
const int* rowadr = d->M_rowadr;
const int* colind = d->M_colind;
const int* rownnz = m->M_rownnz;
const int* rowadr = m->M_rowadr;
const int* colind = m->M_colind;
const int* diagnum = m->dof_simplenum;
const mjtNum* qLD = d->qLD;
src/engine/engine_derivative.c
+20 -20
View File
@@ -458,9 +458,9 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) {
mju_scl(row, Dcfrcbody + (m->dof_bodyid[i]*6+k)*nv, d->cdof[i*6+k], nv);
// dense to sparse: qDeriv -= row
int end = d->D_rowadr[i] + d->D_rownnz[i];
for (int adr=d->D_rowadr[i]; adr < end; adr++) {
d->qDeriv[adr] -= row[d->D_colind[adr]];
int end = m->D_rowadr[i] + m->D_rownnz[i];
for (int adr=m->D_rowadr[i]; adr < end; adr++) {
d->qDeriv[adr] -= row[m->D_colind[adr]];
}
}
}
@@ -492,7 +492,7 @@ static void copyFromParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
}
// copy: guaranteed to be at beginning of sparse array, due to sorting
mju_copy(mat + 6*d->B_rowadr[n], mat + 6*d->B_rowadr[m->body_parentid[n]], 6*ndof);
mju_copy(mat + 6*m->B_rowadr[n], mat + 6*m->B_rowadr[m->body_parentid[n]], 6*ndof);
}
@@ -507,10 +507,10 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// find matching nonzeros
int np = m->body_parentid[n];
int i = 0, ip = 0;
while (i < d->B_rownnz[n] && ip < d->B_rownnz[np]) {
while (i < m->B_rownnz[n] && ip < m->B_rownnz[np]) {
// columns match
if (d->B_colind[d->B_rowadr[n] + i] == d->B_colind[d->B_rowadr[np] + ip]) {
mju_addTo(mat + 6*(d->B_rowadr[np] + ip), mat + 6*(d->B_rowadr[n] + i), 6);
if (m->B_colind[m->B_rowadr[n] + i] == m->B_colind[m->B_rowadr[np] + ip]) {
mju_addTo(mat + 6*(m->B_rowadr[np] + ip), mat + 6*(m->B_rowadr[n] + i), 6);
// advance both
i++;
@@ -518,7 +518,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
}
// mismatch columns: advance parent
else if (d->B_colind[d->B_rowadr[n] + i] > d->B_colind[d->B_rowadr[np] + ip]) {
else if (m->B_colind[m->B_rowadr[n] + i] > m->B_colind[m->B_rowadr[np] + ip]) {
ip++;
}
@@ -534,7 +534,7 @@ static void addToParent(const mjModel* m, mjData* d, mjtNum* mat, int n) {
// derivative of cvel, cdof_dot w.r.t qvel
static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* Dcdofdot) {
int nv = m->nv, nbody = m->nbody;
int* Badr = d->B_rowadr, * Dadr = d->D_rowadr;
int* Badr = m->B_rowadr, * Dadr = m->D_rowadr;
mjtNum mat[36], matT[36]; // 6x6 matrices
// forward pass over bodies: accumulate Dcvel, set Dcdofdot
@@ -603,9 +603,9 @@ static void mjd_comVel_vel(const mjModel* m, mjData* d, mjtNum* Dcvel, mjtNum* D
// subtract d qfrc_bias / d qvel from qDeriv
static void mjd_rne_vel(const mjModel* m, mjData* d) {
int nv = m->nv, nbody = m->nbody;
const int* Badr = d->B_rowadr;
const int* Dadr = d->D_rowadr;
const int* Bnnz = d->B_rownnz;
const int* Badr = m->B_rowadr;
const int* Dadr = m->D_rowadr;
const int* Bnnz = m->B_rownnz;
mjtNum mat[36], mat1[36], mat2[36], dmul[36], tmp[6];
@@ -710,10 +710,10 @@ static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum*
mju_scl(row, J+j*nv, J[i*nv+k] * B[i*n+j], nv);
// add row to qDeriv(k,:)
int rownnz_k = d->D_rownnz[k];
int rownnz_k = m->D_rownnz[k];
for (int s=0; s < rownnz_k; s++) {
int adr = d->D_rowadr[k] + s;
d->qDeriv[adr] += row[d->D_colind[adr]];
int adr = m->D_rowadr[k] + s;
d->qDeriv[adr] += row[m->D_colind[adr]];
}
}
}
@@ -748,8 +748,8 @@ static void addJTBJSparse(
int colik = J_colind[ik];
// qDeriv(k,:) += J(j,:) * J(i,k)*B(i,j)
mju_addToSclSparseInc(d->qDeriv + d->D_rowadr[colik], J + adr_j,
d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik],
mju_addToSclSparseInc(d->qDeriv + m->D_rowadr[colik], J + adr_j,
m->D_rownnz[colik], m->D_colind + m->D_rowadr[colik],
nnz_j, J_colind + adr_j,
J[ik]*B[i*n+j]);
}
@@ -1433,12 +1433,12 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
// dof damping
for (int i=0; i < nv; i++) {
int nnz_i = d->D_rownnz[i];
int nnz_i = m->D_rownnz[i];
for (int j=0; j < nnz_i; j++) {
int ij = d->D_rowadr[i] + j;
int ij = m->D_rowadr[i] + j;
// identify diagonal element
if (d->D_colind[ij] == i) {
if (m->D_colind[ij] == i) {
d->qDeriv[ij] -= m->dof_damping[i];
break;
}
src/engine/engine_derivative_fd.c
+5 -5
View File
@@ -204,8 +204,8 @@ void mjd_passive_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// copy to i-th column of qDeriv
for (int j=0; j < nv; j++) {
int adr = d->D_rowadr[j] + cnt[j];
if (cnt[j] < d->D_rownnz[j] && d->D_colind[adr] == i) {
int adr = m->D_rowadr[j] + cnt[j];
if (cnt[j] < m->D_rownnz[j] && m->D_colind[adr] == i) {
d->qDeriv[adr] = fd[j];
cnt[j]++;
}
@@ -263,8 +263,8 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// copy to sparse qDeriv
for (int j=0; j < nv; j++) {
if (cnt[j] < d->D_rownnz[j] && d->D_colind[d->D_rowadr[j]+cnt[j]] == i) {
d->qDeriv[d->D_rowadr[j]+cnt[j]] = fd[j];
if (cnt[j] < m->D_rownnz[j] && m->D_colind[m->D_rowadr[j]+cnt[j]] == i) {
d->qDeriv[m->D_rowadr[j]+cnt[j]] = fd[j];
cnt[j]++;
}
}
@@ -272,7 +272,7 @@ void mjd_smooth_velFD(const mjModel* m, mjData* d, mjtNum eps) {
// make sure final row counters equal rownnz
for (int i=0; i < nv; i++) {
if (cnt[i] != d->D_rownnz[i]) {
if (cnt[i] != m->D_rownnz[i]) {
mjERROR("error in constructing FD sparse derivative");
}
}
src/engine/engine_forward.c
+10 -10
View File
@@ -869,18 +869,18 @@ void mj_EulerSkip(const mjModel* m, mjData* d, int skipfactor) {
// qH = M + h*diag(B)
mju_copy(d->qH, d->M, nC);
for (int i=0; i < nv; i++) {
d->qH[d->M_rowadr[i] + d->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
d->qH[m->M_rowadr[i] + m->M_rownnz[i] - 1] += m->opt.timestep * m->dof_damping[i];
}
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
}
// solve
mju_add(qfrc, d->qfrc_smooth, d->qfrc_constraint, nv);
mju_copy(qacc, qfrc, m->nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
}
// advance state and time
@@ -1025,18 +1025,18 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// gather qLU <- qM (lower to full)
mju_gather(d->qLU, d->qM, d->mapM2D, nD);
mju_gather(d->qLU, d->qM, m->mapM2D, nD);
// set qLU = qM - dt*qDeriv
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
// factorize qLU
int* scratch = mjSTACKALLOC(d, nv, int);
mju_factorLUSparse(d->qLU, nv, scratch, d->D_rownnz, d->D_rowadr, d->D_colind);
mju_factorLUSparse(d->qLU, nv, scratch, m->D_rownnz, m->D_rowadr, m->D_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind);
mju_solveLUSparse(qacc, d->qLU, qfrc, nv, m->D_rownnz, m->D_rowadr, m->D_diag, m->D_colind);
}
// IMPLICITFAST
@@ -1047,22 +1047,22 @@ void mj_implicitSkip(const mjModel* m, mjData* d, int skipfactor) {
// modified mass matrix: gather MhB <- qDeriv (full to lower)
mjtNum* MhB = mjSTACKALLOC(d, nM, mjtNum);
mju_gather(MhB, d->qDeriv, d->mapD2M, nM);
mju_gather(MhB, d->qDeriv, m->mapD2M, nM);
// set MhB = M - dt*qDeriv
mju_addScl(MhB, d->qM, MhB, -m->opt.timestep, nM);
// gather qH <- MhB (legacy to CSR)
mju_gather(d->qH, MhB, d->mapM2M, nC);
mju_gather(d->qH, MhB, m->mapM2M, nC);
// factorize in-place
mj_factorI(d->qH, d->qHDiagInv, nv, d->M_rownnz, d->M_rowadr, d->M_colind);
mj_factorI(d->qH, d->qHDiagInv, nv, m->M_rownnz, m->M_rowadr, m->M_colind);
}
// solve for qacc: (qM - dt*qDeriv) * qacc = qfrc
mju_copy(qacc, qfrc, nv);
mj_solveLD(qacc, d->qH, d->qHDiagInv, nv, 1,
d->M_rownnz, d->M_rowadr, d->M_colind);
m->M_rownnz, m->M_rowadr, m->M_colind);
} else {
mjERROR("integrator must be implicit or implicitfast");
src/engine/engine_inverse.c
+3 -3
View File
@@ -116,14 +116,14 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
mjd_smooth_vel(m, d, /* flg_bias = */ 1);
// gather qLU <- qM (lower to full)
mju_gather(d->qLU, d->qM, d->mapM2D, nD);
mju_gather(d->qLU, d->qM, m->mapM2D, nD);
// set qLU = qM - dt*qDeriv
mju_addToScl(d->qLU, d->qDeriv, -m->opt.timestep, m->nD);
// set qfrc = qLU * qacc
mju_mulMatVecSparse(qfrc, d->qLU, qacc, nv,
d->D_rownnz, d->D_rowadr, d->D_colind, /*rowsuper=*/NULL);
m->D_rownnz, m->D_rowadr, m->D_colind, /*rowsuper=*/NULL);
break;
case mjINT_IMPLICITFAST:
@@ -137,7 +137,7 @@ static void mj_discreteAcc(const mjModel* m, mjData* d) {
// set M = M - dt*qDeriv (reduced to M nonzeros)
mjtNum* qDerivReduced = mjSTACKALLOC(d, m->nM, mjtNum);
for (int i=0; i < nM; i++) {
qDerivReduced[i] = d->qDeriv[d->mapD2M[i]];
qDerivReduced[i] = d->qDeriv[m->mapD2M[i]];
}
mju_addToScl(d->qM, qDerivReduced, -m->opt.timestep, m->nM);
src/engine/engine_io.c
+6 -34
View File
@@ -1118,18 +1118,18 @@ void mj_makeBSparse(int nv, int nbody, int nB,
// check D and B sparsity for consistency
static void checkDBSparse(const mjModel* m, mjData* d) {
static void checkDBSparse(const mjModel* m) {
// process all dofs
for (int j = 0; j < m->nv; j++) {
// get body for this dof
int i = m->dof_bodyid[j];
// D[row j] and B[row i] should be identical
if (d->D_rownnz[j] != d->B_rownnz[i]) {
if (m->D_rownnz[j] != m->B_rownnz[i]) {
mjERROR("rows have different nnz");
}
for (int k = 0; k < d->D_rownnz[j]; k++) {
if (d->D_colind[d->D_rowadr[j] + k] != d->B_colind[d->B_rowadr[i] + k]) {
for (int k = 0; k < m->D_rownnz[j]; k++) {
if (m->D_colind[m->D_rowadr[j] + k] != m->B_colind[m->B_rowadr[i] + k]) {
mjERROR("rows have different colind");
}
}
@@ -2004,36 +2004,8 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
}
}
// construct sparse matrix representations
if (m->body_dofadr) {
mj_markStack(d);
int* remaining = mjSTACKALLOC(d, m->nv, int);
int* count = mjSTACKALLOC(d, m->nbody, int);
int* M = mjSTACKALLOC(d, m->nM, int);
int* D = mjSTACKALLOC(d, m->nD, int);
// make D
mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum,
d->D_rownnz, d->D_rowadr, d->D_diag, d->D_colind,
/*reduced=*/0, /*upper=*/1, remaining);
// make B, check D and B
mj_makeBSparse(m->nv, m->nbody, m->nB, m->body_dofnum, m->body_parentid,
m->body_dofadr, d->B_rownnz, d->B_rowadr, d->B_colind, count);
checkDBSparse(m, d);
// make C
mj_makeDofDofSparse(m->nv, m->nC, m->nD, m->nM, m->dof_parentid, m->dof_simplenum,
d->M_rownnz, d->M_rowadr, NULL, d->M_colind,
/*reduced=*/1, /*upper=*/0, remaining);
// make index mappings: mapM2D, mapD2M, mapM2C, mapM2M
mj_makeDofDofMaps(m->nv, m->nM, m->nC, m->nD, m->dof_Madr, m->dof_simplenum, m->dof_parentid,
d->D_rownnz, d->D_rowadr, d->D_colind, d->M_rownnz, d->M_rowadr,
d->mapM2D, d->mapD2M, d->mapM2M, remaining, M, D);
mj_freeStack(d);
}
// check consistency of sparse matrix representations
checkDBSparse(m);
// restore pluginstate and plugindata
if (d->nplugin) {
src/engine/engine_island.c
+1 -1
View File
@@ -521,7 +521,7 @@ void mj_island(const mjModel* m, mjData* d) {
// inertia: block-diagonalize both iLD <- qLD and iM <- qM
mju_blockDiagSparse(d->iLD, d->iM_rownnz, d->iM_rowadr, d->iM_colind,
d->qLD, d->M_rownnz, d->M_rowadr, d->M_colind,
d->qLD, m->M_rownnz, m->M_rowadr, m->M_colind,
nidof, nisland,
d->map_idof2dof, d->map_dof2idof,
d->island_idofadr, d->island_idofadr,
src/engine/engine_print.c
+7 -32
View File
@@ -1155,51 +1155,26 @@ void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filena
d->moment_rowadr, d->moment_colind, fp, float_format);
printArray("CRB", m->nbody, 10, d->crb, fp, float_format);
printInertia("QM", d->qM, m, fp, float_format);
printSparse("M", d->M, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, d->M_rownnz,
d->M_rowadr, d->M_colind, fp, float_format);
printSparse("M", d->M, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printSparse("QLD", d->qLD, m->nv, m->M_rownnz,
m->M_rowadr, m->M_colind, fp, float_format);
printArray("QLDIAGINV", m->nv, 1, d->qLDiagInv, fp, float_format);
if (!mju_isZero(d->qHDiagInv, m->nv)) {
printSparse("QH", d->qH, m->nv, d->M_rownnz, d->M_rowadr, d->M_colind, fp, float_format);
printSparse("QH", d->qH, m->nv, m->M_rownnz, m->M_rowadr, m->M_colind, fp, float_format);
printArray("QHDIAGINV", m->nv, 1, d->qHDiagInv, fp, float_format);
}
// B sparse structure
mj_printSparsity("B: body-dof matrix", m->nbody, m->nv, d->B_rowadr, NULL, d->B_rownnz, NULL,
d->B_colind, fp);
printArrayInt("B_ROWNNZ", 1, m->nbody, d->B_rownnz, fp);
printArrayInt("B_ROWADR", 1, m->nbody, d->B_rowadr, fp);
printArrayInt("B_COLIND", 1, m->nB, d->B_colind, fp);
// M sparse structure
mj_printSparsity("M: reduced inertia matrix", m->nv, m->nv, d->M_rowadr, NULL, d->M_rownnz,
NULL, d->M_colind, fp);
printArrayInt("M_ROWNNZ", 1, m->nv, d->M_rownnz, fp);
printArrayInt("M_ROWADR", 1, m->nv, d->M_rowadr, fp);
printArrayInt("M_COLIND", 1, m->nC, d->M_colind, fp);
printArrayInt("MAPM2M", 1, m->nC, d->mapM2M, fp);
// D sparse structure
mj_printSparsity("D: dof-dof matrix", m->nv, m->nv,
d->D_rowadr, d->D_diag, d->D_rownnz, NULL, d->D_colind, fp);
printArrayInt("D_ROWNNZ", 1, m->nv, d->D_rownnz, fp);
printArrayInt("D_ROWADR", 1, m->nv, d->D_rowadr, fp);
printArrayInt("D_COLIND", 1, m->nD, d->D_colind, fp);
printArrayInt("MAPM2D", 1, m->nD, d->mapM2D, fp);
printArrayInt("MAPD2M", 1, m->nM, d->mapD2M, fp);
// print qDeriv
if (!mju_isZero(d->qDeriv, m->nD)) {
printSparse("QDERIV", d->qDeriv, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind,
printSparse("QDERIV", d->qDeriv, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind,
fp, float_format);
}
// print qLU
if (!mju_isZero(d->qLU, m->nD)) {
printSparse("QLU", d->qLU, m->nv, d->D_rownnz, d->D_rowadr, d->D_colind, fp, float_format);
printSparse("QLU", d->qLU, m->nv, m->D_rownnz, m->D_rowadr, m->D_colind, fp, float_format);
}
// contact
src/engine/engine_solver.c
+3 -3
View File
@@ -885,9 +885,9 @@ static void CGpointers(const mjModel* m, const mjData* d, mjCGContext* ctx, int
ctx->qacc = d->qacc;
// inertia
ctx->M_rownnz = d->M_rownnz;
ctx->M_rowadr = d->M_rowadr;
ctx->M_colind = d->M_colind;
ctx->M_rownnz = m->M_rownnz;
ctx->M_rowadr = m->M_rowadr;
ctx->M_colind = m->M_colind;
ctx->M = d->M;
ctx->qLD = d->qLD;
ctx->qLDiagInv = d->qLDiagInv;
src/engine/engine_support.c
+5 -5
View File
@@ -1058,14 +1058,14 @@ void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec)
// non-simple: add off-diagonals
if (!m->dof_simplenum[i]) {
int adr = d->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, d->M_rownnz[i] - 1, d->M_colind+adr);
int adr = m->M_rowadr[i];
res[i] += mju_dotSparse(qLD+adr, vec, m->M_rownnz[i] - 1, m->M_colind+adr);
}
}
// res *= sqrt(D)
for (int i=0; i < nv; i++) {
int diag = d->M_rowadr[i] + d->M_rownnz[i] - 1;
int diag = m->M_rowadr[i] + m->M_rownnz[i] - 1;
res[i] *= mju_sqrt(qLD[diag]);
}
}
@@ -1084,7 +1084,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
int* buf_ind = mjSTACKALLOC(d, nv, int);
mju_addToMatSparse(dst, rownnz, rowadr, colind, nv,
d->M, d->M_rownnz, d->M_rowadr, d->M_colind,
d->M, m->M_rownnz, m->M_rowadr, m->M_colind,
buf_val, buf_ind);
mj_freeStack(d);
@@ -1092,7 +1092,7 @@ void mj_addM(const mjModel* m, mjData* d, mjtNum* dst,
// dense
else {
mju_addToSymSparse(dst, d->M, nv, d->M_rownnz, d->M_rowadr, d->M_colind, /*flg_upper=*/ 1);
mju_addToSymSparse(dst, d->M, nv, m->M_rownnz, m->M_rowadr, m->M_colind, /*flg_upper=*/ 1);
}
}