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

Add support for excluded contacts in adhesion actuators.

Browse Source 
- Update example model.
- Update video.

PiperOrigin-RevId: 468464196
Change-Id: I603c5a89c2d5a736de127440e6d542beffc12583
This commit is contained in:
Yuval Tassa
2022-08-18 08:27:42 -07:00
committed by Copybara-Service
parent 4bfc2c0311
commit 833dc74074
4 changed files with 111 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/XMLreference.rst
+16 -11
View File
@@ -3409,7 +3409,8 @@ helps clarify the role of bodies and geoms in MuJoCo.
assumption of uniform density. The internal default of 1000 is the density of water in SI units. This attribute is
used only when the mass attribute above is unspecified.
:at:`shellinertia` :at-val:`[false, true], "false"`
If true, the geom's inertia is computed assuming that all the mass is concentrated on the boundary. In this case :at:`density` is interpreted as surface density rather than volumetric density.
If true, the geom's inertia is computed assuming that all the mass is concentrated on the boundary. In this case
:at:`density` is interpreted as surface density rather than volumetric density.
:at:`solmix`: :at-val:`real, "1"`
This attribute specifies the weight used for averaging of contact parameters, and interacts with the priority
attribute. See :ref:`CContact`.
@@ -4616,18 +4617,22 @@ This element has nine custom attributes in addition to the common attributes:
:el-prefix:`actuator/` **adhesion** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. youtube:: HdBue4MUZys
.. youtube:: BcHZ5BFeTmU
:align: right
:height: 200px
:height: 150px
This element defines an active adhesion actuator which injects force at contacts in the normal direction. On the right
is a video demonstrating this actuator type. The model shown in the video can be found `here
<https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_. The transmission target is a :el:`body`, and adhesive
forces are injected into all contacts involving geoms which belong to this body. The force is divided equally between
multiple active contacts. Because it requires contact, it cannot apply a force at a distance, and is more like the active
adhesion on the feet of geckos and insects rather than an industrial vacuum gripper. Adhesion actuators' length is
always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces are allowed). The underlying
:el:`general` attributes are set as follows:
This element defines an active adhesion actuator which injects forces at contacts in the normal direction, see
illustration video. The model shown in the video can be found `here
<https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_ and includes inline annotations. The transmission target
is a :el:`body`, and adhesive forces are injected into all contacts involving geoms which belong to this body. The force
is divided equally between multiple contacts. When the :at:`gap` attribute is not used, this actuator requires active
contacts and cannot apply a force at a distance, more like the active adhesion on the feet of geckos and insects rather
than an industrial vacuum gripper. In order to enable "suction at a distance", "inflate" the body's geoms by
:at:`margin` and add a corresponding :at:`gap` which activates contacts only after :at:`gap` penetration distance. This
will create a layer around the geom where contacts are detected but are inactive, and can be used for
applying the adhesive force. In the video above, such inactive contacts are blue, while active contacts are orange.
An adhesion actuator's length is always 0. :at:`ctrlrange` is required and must also be nonnegative (no repulsive forces
are allowed). The underlying :el:`general` attributes are set as follows:
=========== ======= =========== ========
Attribute Setting Attribute Setting
doc/changelog.rst
+10 -5
View File
@@ -7,15 +7,20 @@ Upcoming version (not yet released)
General
^^^^^^^
.. youtube:: BcHZ5BFeTmU
:align: right
:height: 150px
- Added :ref:`adhesion actuators<adhesion>`.
- Added an `active adhesion example model <https://github.com/deepmind/mujoco/tree/main/model/adhesion>`_.
- Added an `active adhesion example <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.
- Added parameter :at:`shellinertia` in :at:`geom` for treating a mesh as a boundary mesh (shell) for inertia
computations. This is currently supported only for meshes.
- Raise error if the orientation of mesh faces is not consistent, which causes the inertia computations to be
inaccurate. If this occurs, open the mesh in MeshLab or Blender and recalculate the faces.
- 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.
- 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.
model/adhesion/active_adhesion.xml
+17 -16
View File
@@ -1,4 +1,8 @@
<mujoco model="Active adhesion example">
<!--
Adding some fluid viscosity to prevent the hanging sphere from jiggling too much.
-->
<option viscosity="1"/>
<size nconmax="500" njmax="1000"/>
@@ -8,12 +12,7 @@
<default>
<joint damping=".3" axis="0 1 0"/>
<!--
By adding 3mm of margin yet making solimp barely apply any force until 3mm penetration, we get
3mm of "action at a distance". This is important so that small changes in distance don't lead
to loss of adhesion
-->
<geom type="box" friction=".5" margin=".003" solimp="0 .99 .003 .9 6"/>
<geom type="box" friction=".5"/>
<default class="wall">
<geom rgba=".5 .5 .5 .4"/>
</default>
@@ -22,7 +21,12 @@
<tendon rgba=".5 .5 .8 1"/>
</default>
<default class="active_adhesion">
<geom rgba=".8 .5 .5 1"/>
<!--
Geoms in the active_adhesion class are "inflated" with a margin of 1cm, but contacts are
activated only at a depth gap of 1cm, leading to a layer of inactive contacts at a 1cm
distance from the surface. However the contacts can be used by the adhesion actuator.
-->
<geom rgba=".8 .5 .5 1" margin=".01" gap=".01"/>
</default>
<default class="object">
<geom rgba=".5 .8 .5 1" density="100"/>
@@ -77,18 +81,15 @@
<site name="hook_left" pos=".08 0 .3"/>
<site name="hook_right" pos=".12 0 .3"/>
<body name="sphere" pos=".1 0 .2" childclass="active_adhesion">
<!--
Note we are adding damping to a free joint. This is an easy yet unphysical way to prevent
the sphere from jiggling too much on the winch tendons. An alternative would be to add fluid
density or viscosity.
-->
<joint type="free" damping=".1"/>
<freejoint/>
<!--
The composite balls in the crate have only 3 linear DoFs with condim=1, effectively
frictionless point particles. In order to make them stick to the sphere we give the sphere
priority 2, to force condim=3.
Also note the sphere has a margin+gap of 3cm as opposed to the 1cm of the arm box.
-->
<geom type="sphere" size=".03" priority="2"/>
<geom type="sphere" size=".03" priority="2" margin=".03" gap=".03"/>
<site name="pin_left" pos="-.025 0 .025"/>
<site name="pin_right" pos=".025 0 .025"/>
</body>
@@ -127,9 +128,9 @@
<actuator>
<position name="conveyor" joint="conveyor" ctrlrange="-.2 .2" ctrllimited="true" kp="400"/>
<position name="arm" joint="arm2" ctrlrange="-.8 1" ctrllimited="true" kp="10"/>
<adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="20"/>
<adhesion name="adhere_arm" body="4boxes" ctrlrange="0 1" gain="5"/>
<position name="winch" joint="winch" ctrlrange="-.7 .5" ctrllimited="true" kp="10"/>
<adhesion name="adhere_winch" body="sphere" ctrlrange="0 1" gain="20"/>
<adhesion name="adhere_winch" body="sphere" ctrlrange="0 1" gain="5"/>
</actuator>
<sensor>
src/engine/engine_core_smooth.c
+68 -18
View File
@@ -626,6 +626,12 @@ void mj_transmission(const mjModel* m, mjData* d) {
jacS = mj_stackAlloc(d, 3*nv);
mju_zero(moment, nu*nv);
// define variables required for body transmission, don't allocate
int issparse = mj_isSparse(m);
mjtNum* efc_force = NULL; // used as marker for allocation requirement
mjtNum *moment_exclude, *jacdifp, *jac1p, *jac2p;
int *chain;
// compute lengths and moments
for (int i=0; i<nu; i++) {
// extract info
@@ -791,37 +797,81 @@ void mj_transmission(const mjModel* m, mjData* d) {
// moment is average of all contact normal Jacobians
{
// find and count all relevant contacts, mark them in efc_force
int counter = 0;
mjtNum* efc_force = mj_stackAlloc(d, d->nefc);
// allocate stack variables for the first mjTRN_BODY
if (!efc_force) {
efc_force = mj_stackAlloc(d, d->nefc);
moment_exclude = mj_stackAlloc(d, nv);
jacdifp = mj_stackAlloc(d, 3*nv);
jac1p = mj_stackAlloc(d, 3*nv);
jac2p = mj_stackAlloc(d, 3*nv);
chain = issparse ? (int*)mj_stackAlloc(d, nv) : NULL;
}
// clear efc_force and moment_exclude
mju_zero(efc_force, d->nefc);
mju_zero(moment_exclude, nv);
// count all relevant contacts, accumulate Jacobians
int counter = 0;
for (int j=0; j<d->ncon; j++) {
const mjContact* con = d->contact+j;
if (m->geom_bodyid[con->geom1]==id || m->geom_bodyid[con->geom2]==id) {
if (!con->exclude) {
counter++;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// irrelevant contact, continue
if (b1 != id && b2 != id) {
continue;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
// mark contact normals in efc_force
if (!con->exclude) {
counter++;
// condim 1 or elliptic cones: normal is in the first row
if (con->dim == 1 || m->opt.cone==mjCONE_ELLIPTIC) {
efc_force[con->efc_address] = 1;
}
// pyramidal cones: average all pyramid directions
else {
int npyramid = con->dim-1; // number of frictional directions
for (int k=0; k<2*npyramid; k++) {
efc_force[con->efc_address+k] = 0.5/npyramid;
}
} else if (con->exclude == 1) {
// TODO(b/240848298): compute Jacobians for excluded contact (in gap)
}
}
// excluded contact, get sparse or dense Jacobian, accumulate
else if (con->exclude == 1) {
counter++;
// get Jacobian difference
int NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
// project Jacobian along the normal of the contact frame
mju_mulMatMat(jac, con->frame, jacdifp, 1, 3, NV);
// accumulate in moment_exclude
if (issparse) {
for (int k=0; k<NV; k++) {
moment_exclude[chain[k]] += jac[k];
}
} else {
mju_addTo(moment_exclude, jac, nv);
}
}
}
// moment is average over contact normal Jacobians, make negative for adhesion
if (counter) {
// accumulate active contact Jacobians into moment
mj_mulJacTVec(m, d, moment+i*nv, efc_force);
// add Jacobians from excluded contacts
mju_addTo(moment+i*nv, moment_exclude, nv);
// normalize by total contacts, flip sign
mju_scl(moment+i*nv, moment+i*nv, -1.0/counter, nv);
}
}