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

Improved behaviour of mouse-spring interactions.

Browse Source 
Also:

- Renamed recently introduced `mjvPerturb` member `reflocalpos` -> `refselpos` and corrected docstring.
- Added `mjvPerturb.localmass`: body spatial inertia at selection point.
- Removed `const` qualifier to `mjData` argument of `mjv_initPerturb()`, to facilitate spatial inertia computation.
- Improved `mj_Object{Velocity, Acceleration}` docstrings, clarifying that rotational velocities precede linear velocities.
- Removed redundant const qualifiers from non pointer arguments to `mju_dot` and `mju_writeNumBytes`.

PiperOrigin-RevId: 511002168
Change-Id: I27400f497947f330072d4abb7c9bf7e71e6c5d49
This commit is contained in:
Yuval Tassa
2023-02-20 08:39:53 -08:00
committed by Copybara-Service
parent 9756ed0d80
commit 7542c90cd2
15 changed files with 111 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+2 -2
View File
@@ -1029,7 +1029,7 @@ mj_objectVelocity
.. mujoco-include:: mj_objectVelocity
Compute object 6D velocity in object-centered frame, world/local orientation.
Compute object 6D velocity (rot:lin) in object-centered frame, world/local orientation.
.. _mj_objectAcceleration:
@@ -1038,7 +1038,7 @@ mj_objectAcceleration
.. mujoco-include:: mj_objectAcceleration
Compute object 6D acceleration in object-centered frame, world/local orientation.
Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.
.. _mj_contactForce:
doc/changelog.rst
+1 -1
View File
@@ -59,7 +59,7 @@ Bug fixes
:align: right
:width: 300px
#. The ``launch_repl`` functionality in the Python viewer has been fixed.
6. The ``launch_repl`` functionality in the Python viewer has been fixed.
#. Set ``time`` correctly in ``mjd_transitionFD``, to support time-dependent user code.
#. Fixed sensor data dimension validation when ``user`` type sensors are present.
#. Fixed incorrect plugin error message when a null ``nsensordata`` callback is encountered during model compilation.
doc/includes/references.h
+5 -5
View File
@@ -1641,8 +1641,9 @@ struct mjvPerturb_ { // object selection and perturbation
int active2; // secondary perturbation bitmask (mjtPertBit)
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 refselpos[3]; // reference position for selection point
mjtNum localpos[3]; // selection point in object coordinates
mjtNum localmass; // spatial inertia at selection point
mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb)
};
typedef struct mjvPerturb_ mjvPerturb;
@@ -1973,8 +1974,7 @@ void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx
mjtNum reldy, const mjvScene* scn, mjvPerturb* pert);
void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
const mjtNum roomup[3], mjvScene* scn);
void mjv_initPerturb(const mjModel* m, const mjData* d,
const mjvScene* scn, mjvPerturb* pert);
void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert);
void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert,
int flg_paused);
void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert);
@@ -2087,7 +2087,7 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n);
void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n);
mjtNum mju_normalize(mjtNum* res, int n);
mjtNum mju_norm(const mjtNum* res, int n);
mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, const int n);
mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n);
void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc);
void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc);
mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n);
@@ -2146,7 +2146,7 @@ mjtNum mju_sign(mjtNum x);
int mju_round(mjtNum x);
const char* mju_type2Str(int type);
int mju_str2Type(const char* str);
const char* mju_writeNumBytes(const size_t nbytes);
const char* mju_writeNumBytes(size_t nbytes);
const char* mju_warningText(int warning, size_t info);
int mju_isBad(mjtNum x);
int mju_isZero(mjtNum* vec, int n);
include/mujoco/mjvisualize.h
+2 -1
View File
@@ -157,8 +157,9 @@ struct mjvPerturb_ { // object selection and perturbation
int active2; // secondary perturbation bitmask (mjtPertBit)
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 refselpos[3]; // reference position for selection point
mjtNum localpos[3]; // selection point in object coordinates
mjtNum localmass; // spatial inertia at selection point
mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb)
};
typedef struct mjvPerturb_ mjvPerturb;
include/mujoco/mujoco.h
+5 -6
View File
@@ -423,11 +423,11 @@ MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* r
MJAPI void mj_applyFT(const mjModel* m, mjData* d, const mjtNum force[3], const mjtNum torque[3],
const mjtNum point[3], int body, mjtNum* qfrc_target);
// Compute object 6D velocity in object-centered frame, world/local orientation.
// Compute object 6D velocity (rot:lin) in object-centered frame, world/local orientation.
MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
// Compute object 6D acceleration in object-centered frame, world/local orientation.
// Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.
MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d,
int objtype, int objid, mjtNum res[6], int flg_local);
@@ -546,8 +546,7 @@ MJAPI void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reld
const mjtNum roomup[3], mjvScene* scn);
// Copy perturb pos,quat from selected body; set scale for perturbation.
MJAPI void mjv_initPerturb(const mjModel* m, const mjData* d,
const mjvScene* scn, mjvPerturb* pert);
MJAPI void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert);
// Set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise.
// Write d->qpos only if flg_paused and subtree root for selected body has free joint.
@@ -919,7 +918,7 @@ MJAPI mjtNum mju_normalize(mjtNum* res, int n);
MJAPI mjtNum mju_norm(const mjtNum* res, int n);
// Return dot-product of vec1 and vec2.
MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, const int n);
MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n);
// Multiply matrix and vector: res = mat * vec.
MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc);
@@ -1104,7 +1103,7 @@ MJAPI const char* mju_type2Str(int type);
MJAPI int mju_str2Type(const char* str);
// Return human readable number of bytes using standard letter suffix.
MJAPI const char* mju_writeNumBytes(const size_t nbytes);
MJAPI const char* mju_writeNumBytes(size_t nbytes);
// Construct a warning message given the warning type and info.
MJAPI const char* mju_warningText(int warning, size_t info);
introspect/functions.py
+5 -5
View File
@@ -2432,7 +2432,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
type=ValueType(name='int'),
),
),
doc='Compute object 6D velocity in object-centered frame, world/local orientation.', # pylint: disable=line-too-long
doc='Compute object 6D velocity (rot:lin) in object-centered frame, world/local orientation.', # pylint: disable=line-too-long
)),
('mj_objectAcceleration',
FunctionDecl(
@@ -2471,7 +2471,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
type=ValueType(name='int'),
),
),
doc='Compute object 6D acceleration in object-centered frame, world/local orientation.', # pylint: disable=line-too-long
doc='Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation.', # pylint: disable=line-too-long
)),
('mj_contactForce',
FunctionDecl(
@@ -3353,7 +3353,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData', is_const=True),
inner_type=ValueType(name='mjData'),
),
),
FunctionParameterDecl(
@@ -5651,7 +5651,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
FunctionParameterDecl(
name='n',
type=ValueType(name='int', is_const=True),
type=ValueType(name='int'),
),
),
doc='Return dot-product of vec1 and vec2.',
@@ -6978,7 +6978,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
parameters=(
FunctionParameterDecl(
name='nbytes',
type=ValueType(name='size_t', is_const=True),
type=ValueType(name='size_t'),
),
),
doc='Return human readable number of bytes using standard letter suffix.', # pylint: disable=line-too-long
src/engine/engine_util_blas.c
+1 -1
View File
@@ -627,7 +627,7 @@ mjtNum mju_norm(const mjtNum* res, int n) {
// vector dot-product
mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, const int n) {
mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n) {
mjtNum res = 0;
int i = 0;
int n_4 = n - 4;
src/engine/engine_util_blas.h
+1 -1
View File
@@ -170,7 +170,7 @@ MJAPI mjtNum mju_normalize(mjtNum* res, int n);
MJAPI mjtNum mju_norm(const mjtNum* res, int n);
// vector dot-product
MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, const int n);
MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n);
//------------------------------ matrix-vector operations ------------------------------------------
src/engine/engine_util_misc.c
+1 -1
View File
@@ -947,7 +947,7 @@ int mju_str2Type(const char* str) {
// return human readable number of bytes using standard letter suffix
const char* mju_writeNumBytes(const size_t nbytes) {
const char* mju_writeNumBytes(size_t nbytes) {
int i;
static mjTHREADLOCAL char message[20];
static const char suffix[] = " KMGTPE";
src/engine/engine_util_misc.h
+1 -1
View File
@@ -88,7 +88,7 @@ MJAPI const char* mju_type2Str(int type);
MJAPI int mju_str2Type(const char* str);
// return human readable number of bytes using standard letter suffix
MJAPI const char* mju_writeNumBytes(const size_t nbytes);
MJAPI const char* mju_writeNumBytes(size_t nbytes);
// warning text
MJAPI const char* mju_warningText(int warning, size_t info);
src/engine/engine_vis_interact.c
+64 -28
View File
@@ -21,6 +21,9 @@
#include <mujoco/mjexport.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_core_smooth.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_ray.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -385,7 +388,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);
mju_addToScl3(pert->refselpos, vec, pert->scale);
break;
case mjMOUSE_ROTATE_V:
@@ -514,10 +517,16 @@ void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy,
// copy perturb pos,quat from selected body; set scale for perturbation
void mjv_initPerturb(const mjModel* m, const mjData* d, const mjvScene* scn, mjvPerturb* pert) {
void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert) {
mjMARKSTACK;
int nv = m->nv;
int sel = pert->select;
mjtNum headpos[3], forward[3], dif[3];
mjtNum* jac = mj_stackAlloc(d, 3*nv);
mjtNum* jacM2 = mj_stackAlloc(d, 3*nv);
// invalid selected body: return
if (sel<=0 || sel>=m->nbody) {
return;
@@ -528,25 +537,34 @@ void mjv_initPerturb(const mjModel* m, const mjData* d, const mjvScene* scn, mjv
mju_rotVecMat(selpos, pert->localpos, d->xmat+9*sel);
mju_addTo3(selpos, d->xpos+3*sel);
// compute average spatial inertia at selection point
mj_jac(m, d, jac, NULL, selpos, sel);
mj_solveM2(m, d, jacM2, jac, 3);
mjtNum invmass = mju_dot(jacM2+0*nv, jacM2+0*nv, nv) +
mju_dot(jacM2+1*nv, jacM2+1*nv, nv) +
mju_dot(jacM2+2*nv, jacM2+2*nv, nv);
pert->localmass = 3 / mju_max(invmass, mjMINVAL);
// copy
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);
mju_copy3(pert->refselpos, selpos);
// get camera info
mjv_cameraInModel(headpos, forward, NULL, scn);
// compute scaling: rendered pert->refpos displacement = mouse displacement
mju_sub3(dif, pert->reflocalpos, headpos);
// compute scaling: rendered pert->refselpos displacement = mouse displacement
mju_sub3(dif, pert->refselpos, headpos);
pert->scale = mjv_frustumHeight(scn) * mju_dot3(dif, forward);
mjFREESTACK;
}
// set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise
// d->qpos written only if flg_paused and subtree root for selected body has free joint
void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert,
int flg_paused) {
void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, int flg_paused) {
int rootid = 0, sel = pert->select;
mjtNum pos1[3], quat1[4], pos2[3], quat2[4], refpos[3], refquat[4];
mjtNum *Rpos, *Rquat, *Cpos, *Cquat;
@@ -600,7 +618,6 @@ void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert,
// set perturb force,torque in d->xfrc_applied, if selected body is dynamic
void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert) {
mjtNum xiquat[4], difquat[4], bvel[6], mass, stiffness, *result;
int sel = pert->select;
// exit if nothing to do
@@ -608,11 +625,18 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert)
return;
}
// get pointer to body xfrc_applied
result = d->xfrc_applied + 6*sel;
// pointers to body xfrc_applied, force and torque
mjtNum *force = d->xfrc_applied + 6*sel;
mjtNum *torque = d->xfrc_applied + 6*sel + 3;
// global selbody velocity
// pointers to global selbody velocity, linear and rotational
mjtNum bvel[6];
mj_objectVelocity(m, d, mjOBJ_BODY, sel, bvel, 0);
mjtNum *body_linvel = bvel + 3;
mjtNum *body_rotvel = bvel;
// body rotational inertia
mjtNum inertia = 1.0/mju_max(mjMINVAL, m->body_invweight0[2*sel+1]);
if (((pert->active | pert->active2) & mjPERT_TRANSLATE)) {
// compute selection point in world coordinates
@@ -620,33 +644,45 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert)
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->reflocalpos, selpos);
mju_scl3(result, result, stiffness*mass);
mju_addToScl3(result, bvel+3, -sqrtf(stiffness)*mass);
// displacement of selection point from reference point
mjtNum diff[3];
mju_sub3(diff, selpos, pert->refselpos);
// torque w.r.t body com
mju_subFrom3(selpos, d->xipos+3*sel);
mju_cross(result+3, selpos, result);
// spring perturbation force
mjtNum stiffness = m->vis.map.stiffness;
mju_copy3(force, diff);
mju_scl3(force, force, -stiffness*pert->localmass);
// add critically damped torque (torsional only)
// moment arm w.r.t body com
mjtNum moment_arm[3];
mju_sub3(moment_arm, selpos, d->xipos+3*sel);
// translational velocity of selection point
mjtNum svel[3];
mju_cross(svel, body_rotvel, moment_arm);
mju_addTo3(svel, body_linvel);
// add critical damping force of selection point
mju_addToScl3(force, svel, -sqrtf(stiffness)*pert->localmass);
// torque on body com due to force
mju_cross(torque, moment_arm, force);
// add critically damped torsional torque along displacement axis
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));
mju_normalize3(diff);
mju_addToScl3(torque, diff, -sqrtf(stiffness)*inertia*mju_dot3(diff, body_rotvel));
}
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]);
mjtNum stiffness = m->vis.map.stiffnessrot;
mjtNum xiquat[4], difquat[4];
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);
mju_quat2Vel(torque, difquat, 1.0/(stiffness*inertia));
mju_addToScl3(torque, body_rotvel, -sqrtf(stiffness)*inertia);
}
}
src/engine/engine_vis_interact.h
+1 -2
View File
@@ -59,8 +59,7 @@ MJAPI void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reld
const mjtNum roomup[3], mjvScene* scn);
// copy perturb pos,quat from selected body; set scale perturbation
MJAPI void mjv_initPerturb(const mjModel* m, const mjData* d,
const mjvScene* scn, mjvPerturb* pert);
MJAPI void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert);
// set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise
// d->qpos written only if flg_paused and subtree root for selected body has free joint
src/engine/engine_vis_visualize.c
+2 -2
View File
@@ -587,7 +587,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->reflocalpos[0], pert->reflocalpos[1], pert->reflocalpos[2]);
pert->refselpos[0], pert->refselpos[1], pert->refselpos[2]);
// prepare color
mixcolor(rgba, m->vis.rgba.constraint,
@@ -605,7 +605,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->reflocalpos, mat, rgba);
mjv_initGeom(thisgeom, mjGEOM_SPHERE, sz, pert->refselpos, mat, rgba);
FINISH
}
test/engine/testdata/select.xml
Vendored
+18 -5
View File
@@ -1,24 +1,37 @@
<mujoco>
<visual>
<scale constraint="0.02"/>
</visual>
<worldbody>
<light pos="0 0 3"/>
<geom type="plane" size="1 1 .01"/>
<body pos="0 0 1" quat="0 0.534522 0.801784 0.267261">
<body name="non-diag inertia" pos="0 0 1" quat="0 0.534522 0.801784 0.267261">
<freejoint/>
<geom type="box" size="0.15 0.1 0.05" pos="0.15 0.1 0.05" euler="30 30 30" rgba="1 0 0 1"/>
</body>
<body pos="0 0 2" quat="0 0.534522 0.801784 0.267261">
<body name="diagonal inertia" pos="0 0 2" quat="0 0.534522 0.801784 0.267261">
<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">
<body name="mocap 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" >
<body name="small sphere" pos="-0.4 0.4 0.2">
<freejoint/>
<geom type="sphere" size="0.01" rgba="1 1 1 1"/>
</body>
<body name="heavy sphere" pos="-0.5 0.4 0.2">
<freejoint/>
<geom type="sphere" size="0.02" rgba="1 1 1 1" mass="1000"/>
</body>
<body name="ball joint" pos=".5 -.5 .5" >
<joint type="ball" damping="10"/>
<geom type="box" size="0.1 0.1 0.1"/>
</body>
unity/Runtime/Bindings/MjBindings.cs
+2 -1
View File
@@ -2472,8 +2472,9 @@ 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 refselpos[3];
public fixed double localpos[3];
public double localmass;
public double scale;
}