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

Add vertex equality option.

Browse Source 
PiperOrigin-RevId: 860299454
Change-Id: I7befd8e5f4c8bac6d74684a96a9cc3cee9ae1e29
This commit is contained in:
Alessio Quaglino
2026-01-23 16:29:17 -08:00
committed by Copybara-Service
parent a5de40b400
commit 7da271c687
24 changed files with 391 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+62 -4
View File
@@ -376,6 +376,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
int flex_edgeadr, flex_edgenum;
int flex_vertadr, flex_vertnum;
mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
@@ -643,6 +644,32 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
}
break;
case mjEQ_FLEXVERT:
// add two constraints per vertex
flex_vertadr = m->flex_vertadr[id[0]];
flex_vertnum = m->flex_vertnum[id[0]];
for (int v=flex_vertadr; v < flex_vertadr+flex_vertnum; v++) {
for (int j=0; j < 2; j++) {
cpos[0] = d->flexvert_length[2*v+j];
int row = 2*v+j;
if (issparse) {
mj_addConstraint(m, d, d->flexvert_J + m->flexvert_J_rowadr[row],
cpos, 0, 0, 1, mjCNSTR_EQUALITY, i,
m->flexvert_J_rownnz[row],
m->flexvert_J_colind + m->flexvert_J_rowadr[row]);
} else {
mju_zero(jac[0], nv); // reuse first row of jac[0]
int rowadr = m->flexvert_J_rowadr[row];
int rownnz = m->flexvert_J_rownnz[row];
for (int k=0; k<rownnz; k++) {
jac[0][m->flexvert_J_colind[rowadr+k]] = d->flexvert_J[rowadr+k];
}
mj_addConstraint(m, d, jac[0], cpos, 0, 0, 1, mjCNSTR_EQUALITY, i, 0, NULL);
}
}
}
break;
default: // SHOULD NOT OCCUR
mjERROR("invalid equality constraint type %d", m->eq_type[i]);
}
@@ -1138,8 +1165,23 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
i--;
break;
case mjEQ_FLEXVERT:
// process all vertices for this flex
f = m->eq_obj1id[id];
int vertadr = m->flex_vertadr[f];
int vertnum = m->flex_vertnum[f];
for (int v=vertadr; v<vertadr+vertnum; v++) {
int bodyid = m->flex_vertbodyid[v];
dA[i++] = m->body_invweight0[2*bodyid];
dA[i++] = m->body_invweight0[2*bodyid];
}
// adjust constraint counter
i--;
break;
default:
mjERROR("unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR
mjERROR("unknown constraint type %d", d->efc_type[i]); // SHOULD NOT OCCUR
}
break;
@@ -1605,7 +1647,7 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
int nv = m->nv, neq = m->neq;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
int issparse = (nnz != NULL);
int flex_edgeadr, flex_edgenum;
int flex_edgeadr, flex_edgenum, flex_vertadr, flex_vertnum;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -1728,14 +1770,30 @@ static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
}
break;
case mjEQ_FLEXVERT:
flex_vertadr = m->flex_vertadr[id[0]];
flex_vertnum = m->flex_vertnum[id[0]];
size = 2 * flex_vertnum;
if (nnz) {
for (int v=flex_vertadr; v < flex_vertadr+flex_vertnum; v++) {
NV += m->flexvert_J_rownnz[2*v+0];
NV += m->flexvert_J_rownnz[2*v+1];
}
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown constraint type type %d", m->eq_type[i]); // SHOULD NOT OCCUR
mjERROR("unknown constraint type %d", m->eq_type[i]); // SHOULD NOT OCCUR
}
// accumulate counts; flex NV already accumulated
ne += mj_addConstraintCount(m, size, NV);
nnze += (m->eq_type[i] == mjEQ_FLEX) ? NV : size*NV;
if (m->eq_type[i] == mjEQ_FLEX || m->eq_type[i] == mjEQ_FLEXVERT) {
nnze += NV;
} else {
nnze += size*NV;
}
}
if (nnz) {
src/engine/engine_core_smooth.c
+5
View File
@@ -2775,6 +2775,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
break;
case mjEQ_FLEXVERT:
k = m->eq_obj1id[id];
i += 2*m->flex_vertnum[k];
break;
default:
mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR
}
src/engine/engine_io.c
+1
View File
@@ -1898,6 +1898,7 @@ const char* mj_validateReferences(const mjModel* m) {
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
if (obj1id >= m->nflex || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
src/engine/engine_island.c
+3 -1
View File
@@ -352,7 +352,9 @@ static int findEdges(const mjModel* m, const mjData* d, int* treenedge, int* edg
// row i is still in the same constraint: skip it,
if (efc_type == d->efc_type[i] && efc_id == d->efc_id[i]) {
// unless it is a flex equality, where the tree pattern changes per dof
if (!(efc_type == mjCNSTR_EQUALITY && m->eq_type[efc_id] == mjEQ_FLEX)) {
if (!(efc_type == mjCNSTR_EQUALITY &&
(m->eq_type[efc_id] == mjEQ_FLEX ||
m->eq_type[efc_id] == mjEQ_FLEXVERT))) {
continue;
}
}
src/engine/engine_sleep.c
+2
View File
@@ -399,6 +399,7 @@ int mj_wakeEquality(const mjModel* m, mjData* d) {
mjERROR("tendon equality does not yet support sleeping");
continue;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
mjERROR("flex equality does not yet support sleeping");
continue;
default:
@@ -641,6 +642,7 @@ static mjtSleepState mj_equalitySleepState(const mjModel* m, const mjData* d, in
objtype = mjOBJ_TENDON;
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
objtype = mjOBJ_FLEX;
break;
default:
src/user/user_flexcomp.cc
+2 -2
View File
@@ -81,7 +81,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(scale, 1, 1, 1);
mass = 1;
inertiabox = 0.005;
equality = false;
equality = 0;
mjuu_setvec(pos, 0, 0, 0);
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
@@ -602,7 +602,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
if (equality) {
mjsEquality* pe = mjs_addEquality(&model->spec, &def.spec);
mjs_setDefault(pe->element, &model->Default()->spec);
pe->type = mjEQ_FLEX;
pe->type = equality == 1 ? mjEQ_FLEX : mjEQ_FLEXVERT;
pe->active = true;
mjs_setString(pe->name1, name.c_str());
}
src/user/user_flexcomp.h
+1 -1
View File
@@ -81,7 +81,7 @@ class mjCFlexcomp {
double origin[3]; // origin for generating a 3D mesh from a convex 2D mesh
double mass; // total mass of auto-generated bodies
double inertiabox; // size of inertia box for each body
bool equality; // create edge equality constraint
int equality; // create equality constraint, 0:none, 1:edge, 2:vert
std::string file; // mesh/gmsh file name
mjtDof doftype; // dof type, all vertices or trilinear interpolation
src/user/user_model.cc
+9 -4
View File
@@ -3403,11 +3403,16 @@ void mjCModel::CopyObjects(mjModel* m) {
// find equality constraint referencing this flex
m->flex_edgeequality[i] = 0;
for (int k=0; k < (int)equalities_.size(); k++) {
if (equalities_[k]->type == mjEQ_FLEX && equalities_[k]->name1_ == pfl->name) {
m->flex_edgeequality[i] = 1;
break;
if (equalities_[k]->name1_ == pfl->name) {
if (equalities_[k]->type == mjEQ_FLEX) {
m->flex_edgeequality[i] = 1;
break;
}
if (equalities_[k]->type == mjEQ_FLEXVERT) {
m->flex_edgeequality[i] = 2;
break;
}
}
// TODO: support flex_edgeequality = 2
}
// copy bvh data (flex aabb computed dynamically in mjData)
src/user/user_objects.cc
+2 -2
View File
@@ -5995,7 +5995,7 @@ void mjCEquality::ResolveReferences(const mjCModel* m) {
object_type = mjOBJ_JOINT;
} else if (type == mjEQ_TENDON) {
object_type = mjOBJ_TENDON;
} else if (type == mjEQ_FLEX) {
} else if (type == mjEQ_FLEX || type == mjEQ_FLEXVERT) {
object_type = mjOBJ_FLEX;
} else {
throw mjCError(this, "invalid type in equality constraint");
@@ -6052,7 +6052,7 @@ void mjCEquality::Compile(void) {
ResolveReferences(model);
// make sure flex is not rigid
if (type == mjEQ_FLEX && model->Flexes()[obj1id]->rigid) {
if ((type == mjEQ_FLEX || type == mjEQ_FLEXVERT) && model->Flexes()[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1_.c_str(), id);
}
}
src/xml/xml_native_reader.cc
+14 -4
View File
@@ -364,6 +364,8 @@ std::vector<const char*> MJCF[nMJCF] = {
"active", "solref", "solimp"},
{"flex", "*", "name", "class", "flex",
"active", "solref", "solimp"},
{"flexvert", "*", "name", "class", "flex",
"active", "solref", "solimp"},
{">"},
{"tendon", "*"},
@@ -667,13 +669,14 @@ const mjMap solver_map[solver_sz] = {
// constraint type
const int equality_sz = 6;
const int equality_sz = 7;
const mjMap equality_map[equality_sz] = {
{"connect", mjEQ_CONNECT},
{"weld", mjEQ_WELD},
{"joint", mjEQ_JOINT},
{"tendon", mjEQ_TENDON},
{"flex", mjEQ_FLEX},
{"flexvert", mjEQ_FLEXVERT},
{"distance", mjEQ_DISTANCE}
};
@@ -917,6 +920,14 @@ const mjMap elastic2d_map[5] = {
};
// flex equality type
const mjMap flexeq_map[3] = {
{"false", 0},
{"true", 1},
{"vert", 2},
};
//---------------------------------- class mjXReader implementation --------------------------------
@@ -2186,6 +2197,7 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
ReadAttrTxt(elem, "flex", name1, true);
break;
@@ -2744,9 +2756,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
// edge
XMLElement* edge = FirstChildElement(elem, "edge");
if (edge) {
if (MapValue(edge, "equality", &n, bool_map, 2)) {
fcomp.equality = (n == 1);
}
MapValue(edge, "equality", &fcomp.equality, flexeq_map, 3);
ReadAttr(edge, "solref", mjNREF, fcomp.def.spec.equality->solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.spec.equality->solimp, text, false, false);
ReadAttr(edge, "stiffness", 1, &dflex.edgestiffness, text);
src/xml/xml_native_reader.h
+1 -1
View File
@@ -102,7 +102,7 @@ class mjXReader : public mjXBase {
};
// MJCF schema
#define nMJCF 244
#define nMJCF 245
extern std::vector<const char*> MJCF[nMJCF];
#endif // MUJOCO_SRC_XML_XML_NATIVE_READER_H_
src/xml/xml_native_writer.cc
+1
View File
@@ -710,6 +710,7 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
break;
case mjEQ_FLEX:
case mjEQ_FLEXVERT:
WriteAttrTxt(elem, "flex", mjs_getString(equality->name1));
break;