chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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;