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

Add site semantics to weld constraint. Fixes #1896

Browse Source 
PiperOrigin-RevId: 675112981
Change-Id: Ie4c01440d54116075df89f480591700762892cbd
This commit is contained in:
Taylor Howell
2024-09-16 05:44:53 -07:00
committed by Copybara-Service
parent 2da15cf137
commit 60a1921b34
13 changed files with 368 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_core_constraint.c
+34 -11
View File
@@ -542,11 +542,22 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
break;
case mjEQ_WELD: // fix relative position and orientation
// find global points
for (int j=0; j < 2; j++) {
mjtNum* anchor = data + 3*(1-j);
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor);
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++) {
mjtNum* anchor = data + 3*(1-j);
mju_mulMatVec3(pos[j], d->xmat + 9*id[j], anchor);
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
@@ -556,7 +567,7 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mjtNum torquescale = data[10];
// compute error Jacobian (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,
jac[1]+3*nv, jac[0]+3*nv, jacdif+3*nv);
@@ -564,11 +575,23 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
mju_copy(jac[0], jacdif, 3*NV);
mju_copy(jac[0]+3*NV, jacdif+3*nv, 3*NV);
// compute orientation error: neg(q1) * q0 * relpose (axis components only)
mjtNum* relpose = data+6;
mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose
mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1)
mju_mulQuat(quat2, quat1, quat); // quat2 = neg(q1)*q0*relpose
// orientation, body semantic
if (m->eq_objtype[i] == mjOBJ_BODY) {
// compute orientation error: neg(q1) * q0 * relpose (axis components only)
mjtNum* relpose = data+6;
mju_mulQuat(quat, d->xquat+4*id[0], relpose); // quat = q0*relpose
mju_negQuat(quat1, d->xquat+4*id[1]); // quat1 = neg(q1)
}
// orientation, site semantic
else {
mjtNum quat_site1[4];
mju_mulQuat(quat, d->xquat+4*body_id[0], m->site_quat+4*id[0]);
mju_mulQuat(quat_site1, d->xquat+4*body_id[1], m->site_quat+4*id[1]);
mju_negQuat(quat1, quat_site1);
}
mju_mulQuat(quat2, quat1, quat);
mju_scl3(cpos+3, quat2+1, torquescale); // scale axis components by torquescale
// correct rotation Jacobian: 0.5 * neg(q1) * (jac0-jac1) * q0 * relpose
src/engine/engine_io.c
+17 -5
View File
@@ -2239,6 +2239,7 @@ const char* mj_validateReferences(const mjModel* m) {
for (int i=0; i < m->neq; i++) {
int obj1id = m->eq_obj1id[i];
int obj2id = m->eq_obj2id[i];
int objtype = m->eq_objtype[i];
switch ((mjtEq) m->eq_type[i]) {
case mjEQ_JOINT:
if (obj1id >= m->njnt || obj1id < 0) {
@@ -2262,11 +2263,22 @@ const char* mj_validateReferences(const mjModel* m) {
case mjEQ_WELD:
case mjEQ_CONNECT:
if (obj1id >= m->nbody || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
if (obj2id >= m->nbody || obj2id < 0) {
return "Invalid model: eq_obj2id out of bounds.";
if (objtype == mjOBJ_BODY) {
if (obj1id >= m->nbody || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
if (obj2id >= m->nbody || obj2id < 0) {
return "Invalid model: eq_obj2id out of bounds.";
}
} else if (objtype == mjOBJ_SITE) {
if (obj1id >= m->nsite || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
if (obj2id >= m->nsite || obj2id < 0) {
return "Invalid model: eq_obj2id out of bounds.";
}
} else {
return "Invalid model: eq_objtype is not body or site.";
}
break;
src/engine/engine_setconst.c
+26 -19
View File
@@ -307,26 +307,33 @@ static void set0(mjModel* m, mjData* d) {
// weld constraint
else if (m->eq_type[i] == mjEQ_WELD) {
// skip if user has set any quaternion data
if (m->eq_data[mjNEQDATA*i+6] ||
m->eq_data[mjNEQDATA*i+7] ||
m->eq_data[mjNEQDATA*i+8] ||
m->eq_data[mjNEQDATA*i+9]) {
// normalize quaternion just in case
mju_normalize4(m->eq_data+mjNEQDATA*i+6);
continue;
switch ((mjtObj) m->eq_objtype[i]) {
case mjOBJ_BODY: {
// skip if user has set any quaternion data
if (!mju_isZero(m->eq_data + mjNEQDATA*i + 6, 4)) {
// normalize quaternion just in case
mju_normalize4(m->eq_data+mjNEQDATA*i+6);
continue;
}
// anchor position is in body2 local frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
// data[3-5] = anchor position in body1 local frame
mju_subFrom3(pos, d->xpos+3*id1);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
// data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
mju_negQuat(quat, d->xquat+4*id1);
mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
break;
}
case mjOBJ_SITE: {
break;
}
default:
mjERROR("invalid objtype in weld constraint %d", i);
}
// anchor position is in body2 local frame
mj_local2Global(d, pos, 0, m->eq_data+mjNEQDATA*i, 0, id2, 0);
// data[3-5] = anchor position in body1 local frame
mju_subFrom3(pos, d->xpos+3*id1);
mju_mulMatTVec3(m->eq_data+mjNEQDATA*i+3, d->xmat+9*id1, pos);
// data[6-9] = neg(xquat1)*xquat2 = "xquat2-xquat1" in body1 local frame
mju_negQuat(quat, d->xquat+4*id1);
mju_mulQuat(m->eq_data+mjNEQDATA*i+6, quat, d->xquat+4*id2);
}
}
src/engine/engine_vis_visualize.c
+8 -3
View File
@@ -2026,29 +2026,34 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
int is_connect = m->eq_type[i] == mjEQ_CONNECT;
if (d->eq_active[i] && (is_connect || is_weld)) {
// compute endpoints in global coordinates
mjtNum *xmat_j, *xmat_k;
int j = m->eq_obj1id[i], k = m->eq_obj2id[i];
if (is_connect && m->eq_objtype[i] == mjOBJ_SITE) {
if (m->eq_objtype[i] == mjOBJ_SITE) {
mju_copy3(vec, d->site_xpos+3*j);
mju_copy3(end, d->site_xpos+3*k);
xmat_j = d->site_xmat+9*j;
xmat_k = d->site_xmat+9*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);
xmat_j = d->xmat+9*j;
xmat_k = d->xmat+9*k;
}
// construct geom
sz[0] = scl * m->vis.scale.constraint;
START
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, d->xmat+9*j, m->vis.rgba.connect);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, vec, xmat_j, m->vis.rgba.connect);
if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
}
FINISH
START
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, d->xmat+9*k, m->vis.rgba.constraint);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, end, xmat_k, m->vis.rgba.constraint);
if (vopt->label == mjLABEL_CONSTRAINT) {
makeLabel(m, mjOBJ_EQUALITY, i, thisgeom->label);
}