From 7feecf6e94623046382633d86758b735c5050454 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Tue, 31 Jan 2023 10:56:26 -0800 Subject: [PATCH] Improve behaviour of set-pose mouse interactions with free and mocap bodies. PiperOrigin-RevId: 506051745 Change-Id: If666757deb743f7bf3af4113d10976c3294e0879 --- doc/includes/references.h | 5 +++-- include/mujoco/mjvisualize.h | 5 +++-- src/engine/engine_vis_interact.c | 20 +++++++++++--------- src/engine/engine_vis_visualize.c | 4 ++-- test/engine/testdata/select.xml | 10 ++++++++++ unity/Runtime/Bindings/MjBindings.cs | 1 + 6 files changed, 30 insertions(+), 15 deletions(-) diff --git a/doc/includes/references.h b/doc/includes/references.h index 14ce8845..36eacea6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -1633,8 +1633,9 @@ struct mjvPerturb_ { // object selection and perturbation int skinselect; // selected skin id; negative: none int active; // perturbation bitmask (mjtPertBit) int active2; // secondary perturbation bitmask (mjtPertBit) - mjtNum refpos[3]; // desired position for selected object - mjtNum refquat[4]; // desired orientation for selected object + mjtNum refpos[3]; // reference position for selected object + mjtNum refquat[4]; // reference orientation for selected object + mjtNum reflocalpos[3]; // reference selection point in object coordinates mjtNum localpos[3]; // selection point in object coordinates mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb) }; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index bd56493c..d5ab29fe 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -154,8 +154,9 @@ struct mjvPerturb_ { // object selection and perturbation int skinselect; // selected skin id; negative: none int active; // perturbation bitmask (mjtPertBit) int active2; // secondary perturbation bitmask (mjtPertBit) - mjtNum refpos[3]; // desired position for selected object - mjtNum refquat[4]; // desired orientation for selected object + mjtNum refpos[3]; // reference position for selected object + mjtNum refquat[4]; // reference orientation for selected object + mjtNum reflocalpos[3]; // reference selection point in object coordinates mjtNum localpos[3]; // selection point in object coordinates mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb) }; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index 9f635ba1..b4a79fe0 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -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) diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 31284ee4..0d18f52c 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -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 } diff --git a/test/engine/testdata/select.xml b/test/engine/testdata/select.xml index f718f7f1..2dc99505 100644 --- a/test/engine/testdata/select.xml +++ b/test/engine/testdata/select.xml @@ -12,5 +12,15 @@ + + + + + + + + + + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 97f891ab..ffff4ea8 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -2466,6 +2466,7 @@ public unsafe struct mjvPerturb_ { public int active2; public fixed double refpos[3]; public fixed double refquat[4]; + public fixed double reflocalpos[3]; public fixed double localpos[3]; public double scale; }