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

Improve behaviour of set-pose mouse interactions with free and mocap bodies.

Browse Source 
PiperOrigin-RevId: 506051745
Change-Id: If666757deb743f7bf3af4113d10976c3294e0879
This commit is contained in:
Yuval Tassa
2023-01-31 10:56:26 -08:00
committed by Copybara-Service
parent f1af86cf82
commit 7feecf6e94
6 changed files with 30 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_vis_interact.c
+11 -9
View File
@@ -385,6 +385,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
case mjMOUSE_MOVE_V:
case mjMOUSE_MOVE_H:
mju_addToScl3(pert->refpos, vec, pert->scale);
mju_addToScl3(pert->reflocalpos, vec, pert->scale);
break;
case mjMOUSE_ROTATE_V:
@@ -524,18 +525,19 @@ void mjv_initPerturb(const mjModel* m, const mjData* d, const mjvScene* scn, mjv
// compute selection point in world coordinates
mjtNum selpos[3];
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select);
mju_addTo3(selpos, d->xpos+3*pert->select);
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*sel);
mju_addTo3(selpos, d->xpos+3*sel);
// copy
mju_copy3(pert->refpos, selpos);
mju_mulQuat(pert->refquat, d->xquat + 4*sel, m->body_iquat + 4*sel);
mju_copy3(pert->refpos, d->xipos+3*sel);
mju_mulQuat(pert->refquat, d->xquat+4*sel, m->body_iquat+4*sel);
mju_copy3(pert->reflocalpos, selpos);
// get camera info
mjv_cameraInModel(headpos, forward, NULL, scn);
// compute scaling: rendered pert->refpos displacement = mouse displacement
mju_sub3(dif, pert->refpos, headpos);
mju_sub3(dif, pert->reflocalpos, headpos);
pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward);
}
@@ -615,18 +617,18 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert)
if (((pert->active | pert->active2) & mjPERT_TRANSLATE)) {
// compute selection point in world coordinates
mjtNum selpos[3];
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select);
mju_addTo3(selpos, d->xpos+3*pert->select);
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*sel);
mju_addTo3(selpos, d->xpos+3*sel);
// spring perturbation force, with critical damping
stiffness = m->vis.map.stiffness;
mass = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel]);
mju_sub3(result, pert->refpos, selpos);
mju_sub3(result, pert->reflocalpos, selpos);
mju_scl3(result, result, stiffness*mass);
mju_addToScl3(result, bvel+3, -sqrtf(stiffness)*mass);
// torque w.r.t body com
mju_subFrom3(selpos, d->xipos+3*pert->select);
mju_subFrom3(selpos, d->xipos+3*sel);
mju_cross(result+3, selpos, result);
// add critically damped torque (torsional only)
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -575,7 +575,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = scl * m->vis.scale.constraint;
mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0],
selpos[0], selpos[1], selpos[2],
pert->refpos[0], pert->refpos[1], pert->refpos[2]);
pert->reflocalpos[0], pert->reflocalpos[1], pert->reflocalpos[2]);
// prepare color
mixcolor(rgba, m->vis.rgba.constraint,
@@ -593,7 +593,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
sz[0] = 2*sz[0];
sz[1] = sz[2] = sz[0];
mju_quat2Mat(mat, pert->refquat);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refpos, mat, rgba);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->reflocalpos, mat, rgba);
FINISH
}