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

Fix flex constraint Jacobian for edges belonging to a single body.

Browse Source 
PiperOrigin-RevId: 575161094
Change-Id: If4153bcb751cdd6dbdddeb08672f7107c7c5bbc2
This commit is contained in:
Alessio Quaglino
2023-10-20 02:48:46 -07:00
committed by Copybara-Service
parent e4a9f535a4
commit 01a932e052
12 changed files with 97 additions and 38 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+33 -19
View File
@@ -663,9 +663,13 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
case mjEQ_FLEX:
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// add one constraint per edge
// add one constraint per non-rigid edge
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// position error
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
@@ -1057,7 +1061,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// compute diagApprox
void mj_diagApprox(const mjModel* m, mjData* d) {
int id, dim, b1, b2, weldcnt = 0, edgecnt = 0;
int id, dim, b1, b2, f, weldcnt = 0;
int nefc = d->nefc;
mjtNum tran, rot, fri, *dA = d->efc_diagApprox;
mjContact* con = NULL;
@@ -1067,11 +1071,6 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// get constraint id
id = d->efc_id[i];
// clear edge counter
if (d->efc_type[i] != mjEQ_FLEX) {
edgecnt = 0;
}
// process according to constraint type
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
@@ -1108,8 +1107,18 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEX:
dA[i] = m->flexedge_invweight0[m->flex_edgeadr[m->eq_obj1id[id]] + edgecnt];
edgecnt++;
// process all non-rigid edges for this flex
f = m->eq_obj1id[id];
int flex_edgeadr = m->flex_edgeadr[f];
int flex_edgenum = m->flex_edgenum[f];
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
if (!m->flexedge_rigid[e]) {
dA[i++] = m->flexedge_invweight0[e];
}
}
// adjust constraint counter
i--;
break;
default:
@@ -1642,21 +1651,26 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
break;
case mjEQ_FLEX:
size = m->flex_edgenum[id[0]];
if (!nnz) {
break;
}
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// init with all edges, subract rigid later
size = flex_edgenum;
// process edges of this flex
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
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]];
// rigid: reduce size and skip
if (m->flexedge_rigid[e]) {
size--;
continue;
}
// accumulate NV
NV += mj_jacDifPairCount(m, chain, b1, b2, issparse);
// accumulate NV if needed
if (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_jacDifPairCount(m, chain, b1, b2, issparse);
}
}
break;
src/engine/engine_core_smooth.c
+10 -2
View File
@@ -1857,8 +1857,16 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
break;
case mjEQ_FLEX:
// increment edgenum rows
i += m->flex_edgenum[m->eq_obj1id[id]];
// increment with number of non-rigid edges
k = m->eq_obj1id[id];
int flex_edgeadr = m->flex_edgeadr[k];
int flex_edgenum = m->flex_edgenum[k];
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
if (!m->flexedge_rigid[e]) {
i++;
}
}
break;
default:
src/engine/engine_derivative.c
+9 -2
View File
@@ -1474,9 +1474,16 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
for (int f=0; f < m->nflex; f++) {
if (!m->flex_rigid[f] && m->flex_edgedamping[f]) {
mjtNum B = -m->flex_edgedamping[f];
int flex_edgeadr = m->flex_edgeadr[f];
int flex_edgenum = m->flex_edgenum[f];
// process non-rigid edges of this flex
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
src/engine/engine_passive.c
+6 -1
View File
@@ -106,9 +106,14 @@ void mj_passive(const mjModel* m, mjData* d) {
continue;
}
// process edges of this flex (global edge index)
// process non-rigid edges of this flex (global edge index)
int edgeend = m->flex_edgeadr[f] + m->flex_edgenum[f];
for (int e=m->flex_edgeadr[f]; e < edgeend; e++) {
// skip rigid
if (m->flexedge_rigid[e]) {
continue;
}
// compute spring-damper force along edge
frc = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e])
- damping * d->flexedge_velocity[e];
src/engine/engine_sensor.c
+8 -4
View File
@@ -956,10 +956,14 @@ void mj_energyPos(const mjModel* m, mjData* d) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[i]; e < m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
// process non-rigid edges of this flex
int flex_edgeadr = m->flex_edgeadr[i];
int flex_edgenum = m->flex_edgenum[i];
for (int e=flex_edgeadr; e < flex_edgeadr+flex_edgenum; e++) {
if (!m->flexedge_rigid[e]) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
};
}
}
}
src/engine/engine_setconst.c
+6 -1
View File
@@ -206,8 +206,13 @@ static void set0(mjModel* m, mjData* d) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]];
// rigid edge: set to 0
if (m->flexedge_rigid[i]) {
m->flexedge_invweight0[i] = 0;
}
// accelerate edges that connect simple bodies with no rotations
if (m->body_simple[b1] == 2 && m->body_simple[b2] == 2) {
else if (m->body_simple[b1] == 2 && m->body_simple[b2] == 2) {
m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2;
}