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
doc/includes/references.h
+3 -2
View File
@@ -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)
};
include/mujoco/mjvisualize.h
+3 -2
View File
@@ -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)
};
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
}
test/engine/testdata/select.xml
Vendored
+10
View File
@@ -12,5 +12,15 @@
<freejoint/>
<geom type="box" size="0.15 0.1 0.05" rgba="0 0 1 1"/>
</body>
<body pos="-.5 -.5 .5" mocap="true" quat=".1 .2 .3 .4">
<geom type="box" size="0.15 0.1 0.05" pos="0.15 0.1 0.05" euler="30 30 30" rgba="0 .7 0 1"/>
</body>
<!-- TODO(b/266543914): Fix force perurbation on ball joints -->
<body pos=".5 -.5 .5" >
<joint type="ball" damping="10"/>
<geom type="box" size="0.1 0.1 0.1"/>
</body>
</worldbody>
</mujoco>
unity/Runtime/Bindings/MjBindings.cs
+1
View File
@@ -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;
}