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

Allow connect constraints to be specified using sites.

Browse Source 
PiperOrigin-RevId: 668925419
Change-Id: I10cb71e6706e617c830cd90d6d0836b94a5a8cfa
This commit is contained in:
Yuval Tassa
2024-08-29 07:09:52 -07:00
committed by Copybara-Service
parent 646455406c
commit a1036b86d3
19 changed files with 325 additions and 76 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+17 -5
View File
@@ -505,21 +505,33 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
size = 0;
NV = 0;
NV2 = 0;
int body_id[2];
// process according to type
switch ((mjtEq) m->eq_type[i]) {
case mjEQ_CONNECT: // connect bodies with ball joint
// find global points
for (int j=0; j < 2; j++) {
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j);
mju_addTo3(pos[j], d->xpos + 3*id[j]);
// find global points, body semantic
if (m->eq_objtype[i] == mjOBJ_BODY) {
for (int j=0; j < 2; j++) {
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], data + 3*j);
mju_addTo3(pos[j], d->xpos + 3*id[j]);
body_id[j] = id[j];
}
}
// find global points, site semantic
else {
for (int j=0; j < 2; j++) {
mju_copy3(pos[j], d->site_xpos + 3*id[j]);
body_id[j] = m->site_bodyid[id[j]];
}
}
// compute position error
mju_sub3(cpos, pos[0], pos[1]);
// compute Jacobian difference (opposite of contact: 0 - 1)
NV = mj_jacDifPair(m, d, chain, id[1], id[0], pos[1], pos[0],
NV = mj_jacDifPair(m, d, chain, body_id[1], body_id[0], pos[1], pos[0],
jac[1], jac[0], jacdif, NULL, NULL, NULL);
// copy difference into jac[0]
src/engine/engine_setconst.c
+15 -5
View File
@@ -286,12 +286,22 @@ static void set0(mjModel* m, mjData* d) {
// connect constraint
if (m->eq_type[i] == mjEQ_CONNECT) {
// pos = anchor position in global frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
switch ((mjtObj) m->eq_objtype[i]) {
case mjOBJ_BODY:
// pos = anchor position in global frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id1, 0);
// data[3-5] = anchor position in body2 local frame
mju_subFrom3(pos, d->xpos+3*id2);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
// data[3-5] = anchor position in body2 local frame
mju_subFrom3(pos, d->xpos+3*id2);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id2, pos);
break;
case mjOBJ_SITE:
// site-based connect, eq_data is unused
mju_zero(m->eq_data+mjNEQDATA*i, mjNEQDATA);
break;
default:
mjERROR("invalid objtype in connect constraint %d", i);
}
}
// weld constraint
src/engine/engine_vis_visualize.c
+12 -5
View File
@@ -2021,13 +2021,20 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_CONSTRAINT] && (category & catmask) && m->neq) {
// connect or weld
for (int i=0; i < m->neq; i++) {
if (d->eq_active[i] && (m->eq_type[i] == mjEQ_CONNECT || m->eq_type[i] == mjEQ_WELD)) {
int is_weld = m->eq_type[i] == mjEQ_WELD;
int is_connect = m->eq_type[i] == mjEQ_CONNECT;
if (d->eq_active[i] && (is_connect || is_weld)) {
// compute endpoints in global coordinates
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_WELD));
mju_addTo3(vec, d->xpos+3*j);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*(m->eq_type[i] == mjEQ_CONNECT));
mju_addTo3(end, d->xpos+3*k);
if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) {
mju_copy3(vec, d->site_xpos+3*j);
mju_copy3(end, d->site_xpos+3*k);
} else {
mju_mulMatVec3(vec, d->xmat+9*j, m->eq_data+mjNEQDATA*i+3*is_weld);
mju_addTo3(vec, d->xpos+3*j);
mju_mulMatVec3(end, d->xmat+9*k, m->eq_data+mjNEQDATA*i+3*is_connect);
mju_addTo3(end, d->xpos+3*k);
}
// construct geom
sz[0] = scl * m->vis.scale.constraint;
src/user/user_model.cc
+1
View File
@@ -2727,6 +2727,7 @@ void mjCModel::CopyObjects(mjModel* m) {
m->eq_type[i] = peq->type;
m->eq_obj1id[i] = peq->obj1id;
m->eq_obj2id[i] = peq->obj2id;
m->eq_objtype[i] = peq->objtype;
m->eq_active0[i] = peq->active;
mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF);
mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP);
src/user/user_objects.cc
+23 -24
View File
@@ -4793,65 +4793,64 @@ void mjCEquality::CopyFromSpec() {
void mjCEquality::ResolveReferences(const mjCModel* m) {
mjtObj objtype;
mjtObj object_type;
mjCBase *px1, *px2;
mjtJoint jt1, jt2;
// determine object type
if (type==mjEQ_CONNECT || type==mjEQ_WELD) {
objtype = mjOBJ_BODY;
if (type==mjEQ_WELD) {
object_type = mjOBJ_BODY;
} else if (type==mjEQ_CONNECT) {
if (objtype != mjOBJ_SITE && objtype != mjOBJ_BODY) {
throw mjCError(this, "connect constraint supports only sites and bodies");
}
object_type = objtype;
} else if (type==mjEQ_JOINT) {
objtype = mjOBJ_JOINT;
object_type = mjOBJ_JOINT;
} else if (type==mjEQ_TENDON) {
objtype = mjOBJ_TENDON;
object_type = mjOBJ_TENDON;
} else if (type==mjEQ_FLEX) {
objtype = mjOBJ_FLEX;
object_type = mjOBJ_FLEX;
} else {
throw mjCError(this, "invalid type in equality constraint");
}
// find object 1, get id
px1 = m->FindObject(objtype, name1_);
px1 = m->FindObject(object_type, name1_);
if (!px1) {
throw mjCError(this, "unknown element '%s' in equality constraint %d", name1_.c_str(), id);
throw mjCError(this, "unknown element '%s' in equality constraint", name1_.c_str());
}
obj1id = px1->id;
// find object 2, get id
if (!name2_.empty()) {
px2 = m->FindObject(objtype, name2_);
px2 = m->FindObject(object_type, name2_);
if (!px2) {
throw mjCError(this, "unknown element '%s' in equality constraint %d", name2_.c_str(), id);
}
obj2id = px2->id;
}
// object 2 unspecified: set to -1
else {
if (objtype==mjOBJ_GEOM) {
throw mjCError(this, "both geom are required in equality constraint");
} else {
obj2id = -1;
px2 = 0;
}
} else {
// object 2 unspecified: set to -1
obj2id = -1;
px2 = nullptr;
}
// set missing body = world
if (objtype==mjOBJ_BODY && obj2id==-1) {
if (object_type == mjOBJ_BODY && obj2id == -1) {
obj2id = 0;
}
// make sure the two objects are different
if (obj1id==obj2id) {
if (obj1id == obj2id) {
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1_.c_str(), id);
}
// make sure joints are scalar
if (type==mjEQ_JOINT) {
if (type == mjEQ_JOINT) {
jt1 = ((mjCJoint*)px1)->type;
jt2 = (px2 ? ((mjCJoint*)px2)->type : mjJNT_HINGE);
if ((jt1!=mjJNT_HINGE && jt1!=mjJNT_SLIDE) ||
(jt2!=mjJNT_HINGE && jt2!=mjJNT_SLIDE)) {
if ((jt1 != mjJNT_HINGE && jt1 != mjJNT_SLIDE) ||
(jt2 != mjJNT_HINGE && jt2 != mjJNT_SLIDE)) {
throw mjCError(this, "only HINGE and SLIDE joint allowed in constraint");
}
}
src/xml/xml_native_reader.cc
+35 -6
View File
@@ -363,8 +363,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"equality", "*", "0"},
{"<"},
{"connect", "*", "8", "name", "class", "body1", "body2", "anchor",
"active", "solref", "solimp"},
{"connect", "*", "10", "name", "class", "body1", "body2", "anchor",
"site1", "site2", "active", "solref", "solimp"},
{"weld", "*", "10", "name", "class", "body1", "body2", "relpose", "anchor",
"active", "solref", "solimp", "torquescale"},
{"joint", "*", "8", "name", "class", "joint1", "joint2", "polycoef",
@@ -1896,10 +1896,39 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) {
}
switch (equality->type) {
case mjEQ_CONNECT:
ReadAttrTxt(elem, "body1", name1, true);
ReadAttrTxt(elem, "body2", name2);
ReadAttr(elem, "anchor", 3, equality->data, text, true);
case mjEQ_CONNECT: {
auto maybe_site1 = ReadAttrStr(elem, "site1");
auto maybe_site2 = ReadAttrStr(elem, "site2");
auto maybe_body1 = ReadAttrStr(elem, "body1");
auto maybe_body2 = ReadAttrStr(elem, "body2");
bool has_anchor = ReadAttr(elem, "anchor", 3, equality->data, text);
bool maybe_site = maybe_site1.has_value() || maybe_site2.has_value();
bool maybe_body = maybe_body1.has_value() || maybe_body2.has_value() || has_anchor;
if (maybe_site && maybe_body) {
throw mjXError(elem, "body and site semantics cannot be mixed");
}
bool site_semantic = maybe_site1.has_value() && maybe_site2.has_value();
bool body_semantic = maybe_body1.has_value() && has_anchor;
if (site_semantic == body_semantic) {
throw mjXError(elem, "either both body1 and anchor must be defined,"
" or both site1 and site2 must be defined");
}
if (body_semantic) {
name1 = maybe_body1.value();
if (maybe_body2.has_value()) {
name2 = maybe_body2.value();
}
equality->objtype = mjOBJ_BODY;
} else {
name1 = maybe_site1.value();
name2 = maybe_site2.value();
equality->objtype = mjOBJ_SITE;
}
}
break;
case mjEQ_WELD:
src/xml/xml_native_writer.cc
+8 -3
View File
@@ -620,9 +620,14 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDe
switch (equality->type) {
case mjEQ_CONNECT:
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
WriteAttr(elem, "anchor", 3, equality->data);
if (equality->objtype == mjOBJ_BODY) {
WriteAttrTxt(elem, "body1", mjs_getString(equality->name1));
WriteAttrTxt(elem, "body2", mjs_getString(equality->name2));
WriteAttr(elem, "anchor", 3, equality->data);
} else {
WriteAttrTxt(elem, "site1", mjs_getString(equality->name1));
WriteAttrTxt(elem, "site2", mjs_getString(equality->name2));
}
break;
case mjEQ_WELD: