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

If inertia is being visualised, draw body frames at the inertial frame.

Browse Source 
PiperOrigin-RevId: 532458388
Change-Id: Ie58cea8dee4215ad9e40ef827361bc3793c8f449
This commit is contained in:
Yuval Tassa
2023-05-16 07:54:34 -07:00
committed by Copybara-Service
parent b81793371b
commit 4ea8218466
1 changed files with 6 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+6 -8
View File
@@ -776,6 +776,9 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
continue; continue;
} }
mjtNum* xmat = vopt->flags[mjVIS_INERTIA] ? d->ximat+9*i : d->xmat+9*i;
mjtNum* xpos = vopt->flags[mjVIS_INERTIA] ? d->xipos+3*i : d->xpos+3*i;
// draw the three axes (separate geoms) // draw the three axes (separate geoms)
for (int j=0; j<3; j++) { for (int j=0; j<3; j++) {
START START
@@ -784,16 +787,11 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
for (int k=0; k<3; k++) { for (int k=0; k<3; k++) {
axis[k] = (j==k ? sz[1] : 0); axis[k] = (j==k ? sz[1] : 0);
} }
mju_mulMatVec(vec, d->xmat+9*i, axis, 3, 3); mju_mulMatVec(vec, xmat, axis, 3, 3);
// create a cylinder // create a cylinder
mjv_makeConnector(thisgeom, mjGEOM_CYLINDER, sz[0], mjv_makeConnector(thisgeom, mjGEOM_CYLINDER, sz[0], xpos[0], xpos[1], xpos[2],
d->xpos[3*i+0], xpos[0] + vec[0], xpos[1] + vec[1], xpos[2] + vec[2]);
d->xpos[3*i+1],
d->xpos[3*i+2],
d->xpos[3*i+0] + vec[0],
d->xpos[3*i+1] + vec[1],
d->xpos[3*i+2] + vec[2]);
// set color: R, G or B depending on axis // set color: R, G or B depending on axis
for (int k=0; k<3; k++) { for (int k=0; k<3; k++) {