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

Fix contact frame inconsistency in visualization with mjContact.frame

Browse Source
This commit is contained in:
Yuxuan Jiang
2025-01-10 20:45:46 +08:00
committed by GitHub
parent 6f7fba794b
commit 5fa6b43539
1 changed files with 3 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_visualize.c
+3 -8
View File
@@ -115,7 +115,7 @@ static void makeTriangle(mjvGeom* thisgeom, const mjtNum v0[3], const mjtNum v1[
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags, static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
const mjvOption* vopt, mjvScene* scn) { const mjvOption* vopt, mjvScene* scn) {
int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR; int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR;
mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6], axis[3]; mjtNum mat[9], vec[3], frc[3], confrc[6], axis[3];
mjtNum framewidth, framelength, scl = m->stat.meansize; mjtNum framewidth, framelength, scl = m->stat.meansize;
mjContact* con; mjContact* con;
mjvGeom* thisgeom; mjvGeom* thisgeom;
@@ -131,10 +131,8 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// get pointer // get pointer
con = d->contact + i; con = d->contact + i;
// mat = contact rotation matrix (normal along z) // mat = contact frame rotation matrix (normal along x)
mju_copy(tmp, con->frame+3, 6); mju_transpose(mat, con->frame, 3, 3);
mju_copy(tmp+6, con->frame, 3);
mju_transpose(mat, tmp, 3, 3);
// contact point // contact point
if (flags[mjVIS_CONTACTPOINT]) { if (flags[mjVIS_CONTACTPOINT]) {
@@ -244,9 +242,6 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
continue; continue;
} }
// mat = contact frame rotation matrix (normal along x)
mju_transpose(mat, con->frame, 3, 3);
// get contact force:torque in contact frame // get contact force:torque in contact frame
mj_contactForce(m, d, i, confrc); mj_contactForce(m, d, i, confrc);