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

Add mjtSameFrame enum to specify frame alignment of bodies with their children.

Browse Source 
PiperOrigin-RevId: 666303674
Change-Id: I53dc56e1ca52afa280ae5abaf4158edc465fb1e0
This commit is contained in:
Yuval Tassa
2024-08-22 05:16:02 -07:00
committed by Copybara-Service
parent a2649d6cec
commit 8b03daa09b
13 changed files with 173 additions and 109 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+16 -21
View File
@@ -1614,43 +1614,38 @@ void mj_normalizeQuat(const mjModel* m, mjtNum* qpos) {
void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
const mjtNum pos[3], const mjtNum quat[4],
int body, mjtByte sameframe) {
mjtNum tmp[4];
mjtSameFrame sf = sameframe;
// position
if (xpos && pos) {
// compute
if (sameframe == 0) {
switch (sf) {
case mjSAMEFRAME_NONE:
mju_mulMatVec3(xpos, d->xmat+9*body, pos);
mju_addTo3(xpos, d->xpos+3*body);
}
// copy body position
else if (sameframe == 1) {
break;
case mjSAMEFRAME_BODY:
mju_copy3(xpos, d->xpos+3*body);
}
// copy inertial body position
else {
break;
case mjSAMEFRAME_INERTIA:
mju_copy3(xpos, d->xipos+3*body);
break;
}
}
// orientation
if (xmat && quat) {
// compute
if (sameframe == 0) {
mjtNum tmp[4];
switch (sf) {
case mjSAMEFRAME_NONE:
mju_mulQuat(tmp, d->xquat+4*body, quat);
mju_quat2Mat(xmat, tmp);
}
// copy body orientation
else if (sameframe == 1) {
break;
case mjSAMEFRAME_BODY:
mju_copy(xmat, d->xmat+9*body, 9);
}
// copy inertial body orientation
else {
break;
case mjSAMEFRAME_INERTIA:
mju_copy(xmat, d->ximat+9*body, 9);
break;
}
}
}