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_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