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

Add catenary visualisation to string-like tendons.

Browse Source 
https://youtu.be/I2q7D0Vda-A

PiperOrigin-RevId: 468687422
Change-Id: I84f34e149cf098346a77a51f265113d8fa4f2525
This commit is contained in:
Yuval Tassa
2022-08-19 06:24:21 -07:00
committed by Copybara-Service
parent 7bd686cd87
commit ae0ac86ec3
4 changed files with 293 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+17 -9
View File
@@ -12,26 +12,34 @@ General
:align: right
:height: 150px
- Added :ref:`adhesion actuators<adhesion>`.
- Added an `active adhesion example <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ and video:
- Added :ref:`adhesion actuators<adhesion>` mimicking vaccum grippers and adhesive biomechanical appendages.
- Added related `example model <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ and video:
- Added :ref:`mj_jacSubtreeCom` for computing the translational Jacobian of the center-of-mass of a subtree.
- Added moment of inertia computation for concave meshes. This is currently activated by setting the compiler flag
:at:`exactmeshinertia` to ``true`` (defaults to ``false``). This default may change in the future.
- Joint and tendon ``limited`` attribute and actuator ``ctrllimited``, ``forcelimited`` and ``actlimited`` attributes
now default to ``auto`` rather than ``false``. Limits are automatically set to ``true`` if the corresponding range *is
defined* and ``false`` otherwise.
.. attention::
This is a minor breaking change. In models where a range was defined but :at:`limited` was unspecified, the target
element will now be limited. Explicitly set limited to ``false`` to revert to the previous behavior.
- Added parameter :at:`shellinertia` to :at:`geom`, for locating the inferred inertia on the boundary (shell).
Currently only meshes are supported.
- For meshes from which volumetric inertia is inferred, raise error if the orientation of mesh faces is not consistent.
If this occurs, fix the mesh in e.g., MeshLab or Blender.
.. youtube:: I2q7D0Vda-A
:align: right
:height: 150px
- Added catenary visualisation for hanging tendons. The model seen in the video can be found
`here <https://github.com/deepmind/mujoco/tree/main/test/engine/testdata/catenary.xml>`_.
- Added ``azimuth`` and ``elevation`` attributes to :ref:`visual/global<global>`, defining the initial orientation of
the free camera at model load time.
- Added ``mjv_defaultFreeCamera`` which sets the default free camera, respecting the above attributes.
- ``simulate`` now supports taking a screenshot via a button in the File section or via ``Ctrl-P``.
- Joint and tendon ``limited`` attribute and actuator ``ctrllimited``, ``forcelimited`` and ``actlimited`` attributes
now default to ``"auto"``, which means they are automatically set to ``true`` if the corresponding range is
defined and disabled otherwise.
.. attention::
This is a minor breaking change. Code where a range was defined and limited was unspecified will now
be limited. Please explicitly set limited to ``false`` to revert to the previous behavior.
Deleted/deprecated features
^^^^^^^^^^^^^^^^^^^^^^^^^^^
src/engine/engine_vis_visualize.c
+237 -13
View File
@@ -446,13 +446,12 @@ static int bodycategory(const mjModel* m, int bodyid) {
}
}
// add abstract geoms
void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
const mjvPerturb* pert, int catmask, mjvScene* scn) {
int objtype, category;
mjtNum sz[3], mat[9], selpos[3];
mjtNum catenary[3*mjNCATENARY];
mjtNum *cur, *nxt, *xpos, *xfrc;
mjtNum vec[3], end[3], axis[3], rod, len, det, tmp[9], quat[4];
mjtByte broken;
@@ -1351,27 +1350,77 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
if (vopt->flags[mjVIS_TENDON] && (category & catmask)) {
for (int i=0; i<m->ntendon; i++) {
if (vopt->tendongroup[mjMAX(0, mjMIN(mjNGROUP-1, m->tendon_group[i]))]) {
for (int j=d->ten_wrapadr[i]; j<d->ten_wrapadr[i]+d->ten_wrapnum[i]-1; j++) {
if (d->wrap_obj[j]!=-2 && d->wrap_obj[j+1]!=-2) {
// conditions for drawing a catenary
int draw_catenary =
!mjDISABLED(mjDSBL_GRAVITY) && // gravity enabled
mju_norm3(m->opt.gravity) > mjMINVAL && // gravity strictly nonzero
m->tendon_num[i] == 2 && // only two sites on the tendon
m->tendon_limited[i] == 1 && // limited length range
m->tendon_range[2*i] == 0 && // range lower-bound is 0
m->tendon_stiffness[i] == 0 && // no stiffness
m->tendon_damping[i] == 0 && // no damping
m->tendon_frictionloss[i] == 0; // no frictionloss
// conditions not met: draw straight lines
if (!draw_catenary) {
for (int j=d->ten_wrapadr[i]; j<d->ten_wrapadr[i]+d->ten_wrapnum[i]-1; j++) {
if (d->wrap_obj[j]!=-2 && d->wrap_obj[j+1]!=-2) {
START
// determine width: smaller for segments inside wrapping objects
if (d->wrap_obj[j]>=0 && d->wrap_obj[j+1]>=0) {
sz[0] = 0.5 * m->tendon_width[i];
} else {
sz[0] = m->tendon_width[i];
}
// construct geom
mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0],
d->wrap_xpos[3*j], d->wrap_xpos[3*j+1], d->wrap_xpos[3*j+2],
d->wrap_xpos[3*j+3], d->wrap_xpos[3*j+4], d->wrap_xpos[3*j+5]);
// set material if given
setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags);
// vopt->label: only the first segment
if (vopt->label==mjLABEL_TENDON && j==d->ten_wrapadr[i]) {
makeLabel(m, mjOBJ_TENDON, i, thisgeom->label);
}
FINISH
}
}
}
// special case handling of string-like tendons under gravity
else {
// two hanging points: x0, x1
mjtNum x0[3], x1[3];
mju_copy3(x0, d->wrap_xpos + 3*d->ten_wrapadr[i]);
mju_copy3(x1, d->wrap_xpos + 3*d->ten_wrapadr[i] + 3);
// length of the tendon
mjtNum length = m->tendon_range[2*i+1];
// points along catenary path
int npoints = mjv_catenary(x0, x1, m->opt.gravity, length, catenary);
// draw npoints-1 segments
for (int j=0; j<npoints-1; j++) {
START
// determine width: smaller for segments inside wrapping objects
if (d->wrap_obj[j]>=0 && d->wrap_obj[j+1]>=0) {
sz[0] = 0.5 * m->tendon_width[i];
} else {
sz[0] = m->tendon_width[i];
}
sz[0] = m->tendon_width[i];
// construct geom
mjv_makeConnector(thisgeom, mjGEOM_CAPSULE, sz[0],
d->wrap_xpos[3*j], d->wrap_xpos[3*j+1], d->wrap_xpos[3*j+2],
d->wrap_xpos[3*j+3], d->wrap_xpos[3*j+4], d->wrap_xpos[3*j+5]);
catenary[3*j], catenary[3*j+1], catenary[3*j+2],
catenary[3*j+3], catenary[3*j+4], catenary[3*j+5]);
// set material if given
setMaterial(m, thisgeom, m->tendon_matid[i], m->tendon_rgba+4*i, vopt->flags);
// vopt->label: only the first segment
if (vopt->label==mjLABEL_TENDON && j==d->ten_wrapadr[i]) {
if (vopt->label==mjLABEL_TENDON && npoints/2) {
makeLabel(m, mjOBJ_TENDON, i, thisgeom->label);
}
@@ -1954,3 +2003,178 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt,
mjv_updateActiveSkin(m, d, scn, opt);
}
}
//----------------------------------- catenary functions -------------------------------------------
// returns hyperbolic cosine and optionally computes hyperbolic sine
static inline mjtNum cosh_sinh(mjtNum x, mjtNum *sinh) {
mjtNum expx = mju_exp(x);
if (sinh) {
*sinh = 0.5 * (expx - 1/expx);
}
return 0.5 * (expx + 1/expx);
}
// returns intercept of the catenary equation
static inline mjtNum catenary_intercept(mjtNum v, mjtNum h, mjtNum length) {
return 1/mju_sqrt(mju_sqrt(length*length - v*v)/h - 1);
}
// returns residual of catenary equation and optionally computes its gradient w.r.t b
static inline mjtNum catenary_residual(mjtNum b, mjtNum intercept, mjtNum *grad) {
mjtNum a = 0.5 / b;
mjtNum sinh, cosh = cosh_sinh(a, &sinh);
if (grad) {
*grad = (a*cosh - sinh) * mju_pow(2*b*sinh - 1, -1.5);
}
return 1/mju_sqrt(2*b*sinh - 1) - intercept;
}
// convergence tolerance for catenary solver
static const mjtNum tolerance = 1e-9;
// solve trancendental catenary equation using change of variables proposed in
// https://math.stackexchange.com/a/1002996
static inline mjtNum solve_catenary(mjtNum v, mjtNum h, mjtNum length) {
mjtNum intercept = catenary_intercept(v, h, length);
// initial guess using linear approximation to catenary_residual
mjtNum b = intercept / mju_sqrt(24);
// Newton steps to convergence (usually ~ 5 steps)
for (int i=0; i<50; i++) {
// get value and gradient
mjtNum grad;
mjtNum res = catenary_residual(b, intercept, &grad);
if (mju_abs(res) < tolerance) {
break;
}
// Newton step
mjtNum step = -res / grad;
// backtracking line-search is not essential but can reduce number of iterations
for (int j=0; j<10; j++) {
mjtNum new_res = catenary_residual(b + step, intercept, NULL);
if (mju_abs(new_res) < mju_abs(res)) {
break;
} else {
step *= 0.5;
}
}
// take step
b += step;
}
return b;
}
// points along catenary of given length between x0 and x1, returns number of points
int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length,
mjtNum catenary[3*mjNCATENARY]) {
mjtNum dist = mju_dist3(x0, x1);
// tendon is stretched longer than length: draw straight line
if (dist > length) {
// copy start and end points
mju_copy3(catenary+0, x0);
mju_copy3(catenary+3, x1);
return 2;
}
// tendon is shorter than length
else {
// normalized up vector
mjtNum up[3];
mju_scl3(up, gravity, -1);
mju_normalize3(up);
// x0 to x1
mjtNum x01[3];
mju_sub3(x01, x1, x0);
// make across orthonormal to up, points from x0 to x1
mjtNum across[3];
mju_copy3(across, x01);
mjtNum tmp[3];
mju_scl3(tmp, up, mju_dot3(up, across));
mju_subFrom3(across, tmp);
mjtNum norm = mju_normalize3(across);
// if across is numerically tiny, just set to 0
if (norm < mjMINVAL) {
mju_zero3(across);
}
// extents in the suspension plane
mjtNum h = mju_dot3(x01, across); // horizontal suspension extent
mjtNum v = mju_dot3(x01, up); // vertical height difference of x1 and x0
// near vertical tendon, use hanging bead approximation: 3 points
if (length > 100*h) {
// solve for location of bead hanging on tendon
mjtNum d_up = -0.5*(mju_sqrt(length*length - h*h) - v); // down from x0
mjtNum d_across = h*d_up / (2*d_up - v); // across from x0
// start point
mju_copy3(catenary+0, x0);
// midpoint: bead location
mju_copy3(catenary+3, x0);
mju_addToScl3(catenary+3, up, d_up);
mju_addToScl3(catenary+3, across, d_across);
// end point
mju_copy3(catenary+6, x1);
return 3;
}
// compute catenary: mjNCATENARY points
else {
// b*h: scaled catenary flatness
mjtNum bh = solve_catenary(v, h, length) * h;
// horizontal and vertical offsets
mjtNum h_offset = -0.5 * (mju_log((length+v) / (length-v)) * bh - h);
mjtNum v_offset = -cosh_sinh(h_offset / bh, NULL) * bh;
// start point
mju_copy3(catenary+0, x0);
// hanging points
for (int i=1; i<mjNCATENARY-1; i++) {
// linearly spaced horizontal offset
mjtNum horizontal = i*h/mjNCATENARY;
mju_addScl3(catenary+3*i, x0, across, horizontal);
// vertical offset, evaluate catenary values
mjtNum vertical = bh * cosh_sinh((horizontal - h_offset) / bh, NULL) + v_offset;
mju_addToScl3(catenary+3*i, up, vertical);
}
// end point
mju_copy3(catenary+3*(mjNCATENARY-1), x1);
return mjNCATENARY;
}
}
return 0; // SHOULD NOT OCCUR
}
src/engine/engine_vis_visualize.h
+5
View File
@@ -54,6 +54,11 @@ MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
// update visible skins only
MJAPI void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
#define mjNCATENARY 31
int mjv_catenary(const mjtNum x0[3], const mjtNum x1[3], const mjtNum gravity[3], mjtNum length,
mjtNum catenary[3*mjNCATENARY]);
#ifdef __cplusplus
}
#endif
test/engine/testdata/catenary.xml
Vendored
+34
View File
@@ -0,0 +1,34 @@
<mujoco>
<visual>
<global elevation="-20"/>
</visual>
<worldbody>
<geom type="plane" size="1 1 .1"/>
<light pos="0 -.2 2" dir="0 1 -4" diffuse="1 1 1"/>
<body pos="0 0 .8">
<joint type="slide" stiffness="1000"/>
<geom size=".05" mass="20"/>
<site name="hook" pos="0 0 -.05"/>
</body>
<body pos="-.15 -.15 .3">
<joint name="hinge" type="hinge" axis="0 0 1" damping=".5"/>
<geom type="cylinder" size=".01" fromto="0 0 0 0 0 .1"/>
<geom type="capsule" size=".01" fromto="0 0 0 .25 0 0"/>
<geom size=".02" pos=".25 0 0"/>
<site name="corner" pos=".25 0 .02"/>
</body>
</worldbody>
<tendon>
<spatial limited="true" range="0 .5" rgba="0 .1 1 1" width=".005">
<site site="hook"/>
<site site="corner"/>
</spatial>
</tendon>
<actuator>
<general joint="hinge" biasprm="2" biastype="affine"/>
</actuator>
</mujoco>