Skip shared dofs for contact constraints
PiperOrigin-RevId: 884455239 Change-Id: I7ee36a13c28be88e12380b4790f150ad62f268c1
This commit is contained in:
Taylor Howell
committed by
Copybara-Service
Copybara-Service
parent
eca5758bf9
commit
bb38a34869
@@ -468,7 +468,8 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
|
||||
// compute Jacobian difference (opposite of contact: 0 - 1)
|
||||
NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0],
|
||||
jac[1], jac[0], jacdif, NULL, NULL, NULL, issparse);
|
||||
jac[1], jac[0], jacdif, NULL, NULL, NULL, issparse,
|
||||
/*flg_skipcommon=*/0);
|
||||
|
||||
// copy difference into jac[0]
|
||||
mju_copy(jac[0], jacdif, 3*NV);
|
||||
@@ -504,7 +505,8 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
// compute error Jacobian (opposite of contact: 0 - 1)
|
||||
NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0],
|
||||
jac[1], jac[0], jacdif,
|
||||
jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv, issparse);
|
||||
jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv, issparse,
|
||||
/*flg_skipcommon=*/0);
|
||||
|
||||
// copy difference into jac[0], compress translation:rotation if sparse
|
||||
mju_copy(jac[0], jacdif, 3*NV);
|
||||
@@ -670,7 +672,8 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
|
||||
for (int n = 0; n < nodenum; n++) {
|
||||
int chain_nnz = mj_bodyChain(m, bodyid[n], chain_col);
|
||||
mju_zero(blk_jac, 3*nv);
|
||||
mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col);
|
||||
mj_jacSparse(m, d, blk_jac, NULL, xpos + 3*n, bodyid[n], chain_nnz, chain_col,
|
||||
/*flg_skipcommon=*/0);
|
||||
|
||||
// expand sparse Jacobian to dense row format
|
||||
for (int r = 0; r < 3; r++) {
|
||||
@@ -1297,14 +1300,13 @@ int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int di
|
||||
m->geom_bodyid[con->geom[side]] :
|
||||
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
|
||||
}
|
||||
|
||||
// compute Jacobian differences
|
||||
// compute Jacobian differences, skipping common dofs
|
||||
if (dim > 3) {
|
||||
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
||||
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr, mj_isSparse(m));
|
||||
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr, mj_isSparse(m), 1);
|
||||
} else {
|
||||
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
||||
jac1p, jac2p, jacdifp, NULL, NULL, NULL, mj_isSparse(m));
|
||||
jac1p, jac2p, jacdifp, NULL, NULL, NULL, mj_isSparse(m), 1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -2067,7 +2069,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
}
|
||||
|
||||
NV = mj_jacDifPair(m, NULL, chain, id[1], id[0], NULL, NULL,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse);
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse,
|
||||
/*flg_skipcommon=*/0);
|
||||
break;
|
||||
|
||||
case mjEQ_WELD:
|
||||
@@ -2083,7 +2086,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
}
|
||||
|
||||
NV = mj_jacDifPair(m, NULL, chain, id[1], id[0], NULL, NULL,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse);
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse,
|
||||
/*flg_skipcommon=*/0);
|
||||
break;
|
||||
|
||||
case mjEQ_JOINT:
|
||||
@@ -2138,7 +2142,8 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
|
||||
int b1 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e]];
|
||||
int b2 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e+1]];
|
||||
NV += mj_jacDifPair(m, NULL, chain, b1, b2, NULL, NULL,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse);
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, issparse,
|
||||
/*flg_skipcommon=*/0);
|
||||
}
|
||||
}
|
||||
break;
|
||||
@@ -2252,55 +2257,72 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
|
||||
// compute NV only if nnz requested
|
||||
int NV = 0;
|
||||
if (nnz) {
|
||||
// get bodies
|
||||
int nb = 0, bid[729];
|
||||
for (int side=0; side < 2; side++) {
|
||||
// geom
|
||||
if (con->geom[side] >= 0) {
|
||||
bid[nb++] = m->geom_bodyid[con->geom[side]];
|
||||
}
|
||||
|
||||
// flex
|
||||
else {
|
||||
int nw = 0;
|
||||
int vid[4];
|
||||
mjtNum vweight[4];
|
||||
|
||||
// flex vert
|
||||
if (con->vert[side] >= 0) {
|
||||
vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
|
||||
vweight[0] = 1;
|
||||
}
|
||||
|
||||
// flex elem
|
||||
else {
|
||||
int f = con->flex[side];
|
||||
int fdim = m->flex_dim[f];
|
||||
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
|
||||
for (int k=0; k <= fdim; k++) {
|
||||
vid[nw++] = m->flex_vertadr[f] + edata[k];
|
||||
}
|
||||
|
||||
if (m->flex_interp[f]) {
|
||||
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
|
||||
con->vert[1-side], con->pos, vid, vweight);
|
||||
}
|
||||
}
|
||||
|
||||
// get body or node ids and weights
|
||||
if (m->flex_interp[con->flex[side]] == 0) {
|
||||
for (int k=0; k < nw; k++) {
|
||||
bid[nb] = m->flex_vertbodyid[vid[k]];
|
||||
nb++;
|
||||
}
|
||||
} else {
|
||||
nb += mj_vertBodyWeight(m, d, con->flex[side], vid, bid+nb, NULL, vweight, nw);
|
||||
}
|
||||
// single body on each side (geom-geom or flex vert-vert): skip common dofs
|
||||
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
|
||||
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
|
||||
// get bodies
|
||||
int bid[2];
|
||||
for (int side=0; side < 2; side++) {
|
||||
bid[side] = (con->geom[side] >= 0) ?
|
||||
m->geom_bodyid[con->geom[side]] :
|
||||
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
|
||||
}
|
||||
NV = mj_jacDifPair(m, NULL, chain, bid[0], bid[1], NULL, NULL,
|
||||
NULL, NULL, NULL, NULL, NULL, NULL, mj_isSparse(m), 1);
|
||||
}
|
||||
|
||||
// count non-zeros in merged chain
|
||||
NV = mj_jacSumCount(m, d, chain, nb, bid);
|
||||
// general case: flex elements involved
|
||||
else {
|
||||
// get bodies
|
||||
int nb = 0, bid[729];
|
||||
for (int side=0; side < 2; side++) {
|
||||
// geom
|
||||
if (con->geom[side] >= 0) {
|
||||
bid[nb++] = m->geom_bodyid[con->geom[side]];
|
||||
}
|
||||
|
||||
// flex
|
||||
else {
|
||||
int nw = 0;
|
||||
int vid[4];
|
||||
mjtNum vweight[4];
|
||||
|
||||
// flex vert
|
||||
if (con->vert[side] >= 0) {
|
||||
vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
|
||||
vweight[0] = 1;
|
||||
}
|
||||
|
||||
// flex elem
|
||||
else {
|
||||
int f = con->flex[side];
|
||||
int fdim = m->flex_dim[f];
|
||||
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
|
||||
for (int k=0; k <= fdim; k++) {
|
||||
vid[nw++] = m->flex_vertadr[f] + edata[k];
|
||||
}
|
||||
|
||||
if (m->flex_interp[f]) {
|
||||
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
|
||||
con->vert[1-side], con->pos, vid, vweight);
|
||||
}
|
||||
}
|
||||
|
||||
// get body or node ids and weights
|
||||
if (m->flex_interp[con->flex[side]] == 0) {
|
||||
for (int k=0; k < nw; k++) {
|
||||
bid[nb] = m->flex_vertbodyid[vid[k]];
|
||||
nb++;
|
||||
}
|
||||
} else {
|
||||
nb += mj_vertBodyWeight(m, d, con->flex[side], vid, bid+nb, NULL, vweight, nw);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// count non-zeros in merged chain
|
||||
NV = mj_jacSumCount(m, d, chain, nb, bid);
|
||||
}
|
||||
if (!NV) {
|
||||
continue;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user