diff --git a/doc/changelog.rst b/doc/changelog.rst index 68ed1b68..aa6533bc 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -62,6 +62,7 @@ Simulate ^^^^^^^^ - Renamed the directory in which the ``simulate`` application searches for plugins from ``plugin`` to ``mujoco_plugin``. +- Mouse force perturbations are now applied at the selection point rather than the body center of mass. Version 2.3.0 (October 18, 2022) diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 750553e3..9f635ba1 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -522,8 +522,13 @@ void mjv_initPerturb(const mjModel* m, const mjData* d, const mjvScene* scn, mjv return; } + // 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); + // copy - mju_copy3(pert->refpos, d->xipos + 3*sel); + mju_copy3(pert->refpos, selpos); mju_mulQuat(pert->refquat, d->xquat + 4*sel, m->body_iquat + 4*sel); // get camera info @@ -597,7 +602,7 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) int sel = pert->select; // exit if nothing to do - if (sel<0 ||sel>=m->nbody || !(pert->active | pert->active2)) { + if (sel<0 || sel>=m->nbody || !(pert->active | pert->active2)) { return; } @@ -607,29 +612,39 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) // global selbody velocity mj_objectVelocity(m, d, mjOBJ_BODY, sel, bvel, 0); - // spring perturbation, with critical damping - // - force - stiffness = m->vis.map.stiffness; - mass = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel]); - mju_sub3(result, pert->refpos, d->xipos+3*sel); - mju_scl3(result, result, stiffness*mass); - mju_addToScl3(result, bvel+3, -sqrtf(stiffness)*mass); + 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); - // - torque - stiffness = m->vis.map.stiffnessrot; - mass = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel+1]); - mju_mulQuat(xiquat, d->xquat+4*sel, m->body_iquat+4*sel); - mju_negQuat(xiquat, xiquat); - mju_mulQuat(difquat, pert->refquat, xiquat); - mju_quat2Vel(result+3, difquat, 1.0/(stiffness*mass)); - mju_addToScl3(result+3, bvel, -sqrtf(stiffness)*mass); + // 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_scl3(result, result, stiffness*mass); + mju_addToScl3(result, bvel+3, -sqrtf(stiffness)*mass); - // mask - if (!((pert->active | pert->active2) & mjPERT_TRANSLATE)) { - mju_zero3(result); + // torque w.r.t body com + mju_subFrom3(selpos, d->xipos+3*pert->select); + mju_cross(result+3, selpos, result); + + // add critically damped torque (torsional only) + stiffness = m->vis.map.stiffnessrot; + mass = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel+1]); + mju_normalize3(selpos); + mju_addToScl3(result+3, selpos, -sqrtf(stiffness)*mass*mju_dot3(selpos, bvel)); } - if (!((pert->active | pert->active2) & mjPERT_ROTATE)) { - mju_zero3(result+3); + + if (((pert->active | pert->active2) & mjPERT_ROTATE)) { + // spring perturbation torque, with critical damping + stiffness = m->vis.map.stiffnessrot; + mass = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel+1]); + mju_mulQuat(xiquat, d->xquat+4*sel, m->body_iquat+4*sel); + mju_negQuat(xiquat, xiquat); + mju_mulQuat(difquat, pert->refquat, xiquat); + mju_quat2Vel(result+3, difquat, 1.0/(stiffness*mass)); + mju_addToScl3(result+3, bvel, -sqrtf(stiffness)*mass); } } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index cf3d2a8c..577fd40e 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -566,10 +566,14 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, if ((pert->active | pert->active2) & mjPERT_TRANSLATE) { START + // compute selection point in world coordinates + mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select); + mju_addTo3(selpos, d->xpos+3*pert->select); + // construct geom sz[0] = scl * m->vis.scale.constraint; mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0], - d->xipos[3*i], d->xipos[3*i+1], d->xipos[3*i+2], + selpos[0], selpos[1], selpos[2], pert->refpos[0], pert->refpos[1], pert->refpos[2]); // prepare color @@ -666,6 +670,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, category = mjCAT_DECOR; if ((category & catmask) && pert->select>0 && vopt->flags[mjVIS_SELECT]) { int i=0; + // compute selection point in world coordinates mju_rotVecMat(selpos, pert->localpos, d->xmat+9*pert->select); mju_addTo3(selpos, d->xpos+3*pert->select); diff --git a/test/engine/testdata/select.xml b/test/engine/testdata/select.xml new file mode 100644 index 00000000..f718f7f1 --- /dev/null +++ b/test/engine/testdata/select.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + +