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

Add mjSAMEFRAME_BODYROT and mjSAMEFRAME_INERTIAROT frame alignment shortcuts.

Browse Source 
PiperOrigin-RevId: 666318799
Change-Id: I0af1824d2bd9eaca7b48a9e33e09f873dd33b22d
This commit is contained in:
Yuval Tassa
2024-08-22 06:07:30 -07:00
committed by Copybara-Service
parent 8b03daa09b
commit 2efbd31762
7 changed files with 83 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_support.c
+4
View File
@@ -1620,6 +1620,8 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
if (xpos && pos) {
switch (sf) {
case mjSAMEFRAME_NONE:
case mjSAMEFRAME_BODYROT:
case mjSAMEFRAME_INERTIAROT:
mju_mulMatVec3(xpos, d->xmat+9*body, pos);
mju_addTo3(xpos, d->xpos+3*body);
break;
@@ -1641,9 +1643,11 @@ void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9],
mju_quat2Mat(xmat, tmp);
break;
case mjSAMEFRAME_BODY:
case mjSAMEFRAME_BODYROT:
mju_copy(xmat, d->xmat+9*body, 9);
break;
case mjSAMEFRAME_INERTIA:
case mjSAMEFRAME_INERTIAROT:
mju_copy(xmat, d->ximat+9*body, 9);
break;
}
src/user/user_model.cc
+14 -1
View File
@@ -2013,8 +2013,11 @@ void mjCModel::CopyTree(mjModel* m) {
// set sameframe
mjtSameFrame sameframe;
mjtNum* nullnum = static_cast<mjtNum*>(nullptr);
if (IsNullPose(m->body_ipos+3*i, m->body_iquat+4*i)) {
sameframe = mjSAMEFRAME_BODY;
} else if (IsNullPose(nullnum, m->body_iquat+4*i)) {
sameframe = mjSAMEFRAME_BODYROT;
} else {
sameframe = mjSAMEFRAME_NONE;
}
@@ -2064,7 +2067,7 @@ void mjCModel::CopyTree(mjModel* m) {
bool axis_aligned = ((std::abs(pj->axis[0]) > mjEPS) +
(std::abs(pj->axis[1]) > mjEPS) +
(std::abs(pj->axis[2]) > mjEPS)) == 1;
if (rotfound || !IsNullPose(m->jnt_pos+3*jid, static_cast<mjtNum*>(nullptr)) ||
if (rotfound || !IsNullPose(m->jnt_pos+3*jid, nullnum) ||
((pj->type == mjJNT_HINGE || pj->type == mjJNT_SLIDE) && !axis_aligned)) {
m->body_simple[i] = 0;
}
@@ -2172,10 +2175,15 @@ void mjCModel::CopyTree(mjModel* m) {
mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4);
// determine sameframe
double* nulldouble = static_cast<double*>(nullptr);
if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) {
sameframe = mjSAMEFRAME_BODY;
} else if (IsNullPose(nullnum, m->geom_quat+4*gid)) {
sameframe = mjSAMEFRAME_BODYROT;
} else if (IsSamePose(pg->pos, pb->ipos, pg->quat, pb->iquat)) {
sameframe = mjSAMEFRAME_INERTIA;
} else if (IsSamePose(nulldouble, nulldouble, pg->quat, pb->iquat)) {
sameframe = mjSAMEFRAME_INERTIAROT;
} else {
sameframe = mjSAMEFRAME_NONE;
}
@@ -2203,10 +2211,15 @@ void mjCModel::CopyTree(mjModel* m) {
mjuu_copyvec(m->site_rgba+4*sid, ps->rgba, 4);
// determine sameframe
double* nulldouble = static_cast<double*>(nullptr);
if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) {
sameframe = mjSAMEFRAME_BODY;
} else if (IsNullPose(nullnum, m->site_quat+4*sid)) {
sameframe = mjSAMEFRAME_BODYROT;
} else if (IsSamePose(ps->pos, pb->ipos, ps->quat, pb->iquat)) {
sameframe = mjSAMEFRAME_INERTIA;
} else if (IsSamePose(nulldouble, nulldouble, ps->quat, pb->iquat)) {
sameframe = mjSAMEFRAME_INERTIAROT;
} else {
sameframe = mjSAMEFRAME_NONE;
}