Add passive collision mode for flexes.
PiperOrigin-RevId: 801754907 Change-Id: I4147af2ee2596519d42cb9a03cf8d5488f0bc3e8
This commit is contained in:
committed by
Copybara-Service
parent
b9900db00e
commit
8acd83f317
@@ -3735,6 +3735,7 @@ saving the XML:
|
|||||||
.. _flexcomp-contact-solimp:
|
.. _flexcomp-contact-solimp:
|
||||||
.. _flexcomp-contact-margin:
|
.. _flexcomp-contact-margin:
|
||||||
.. _flexcomp-contact-gap:
|
.. _flexcomp-contact-gap:
|
||||||
|
.. _flexcomp-contact-passive:
|
||||||
|
|
||||||
.. |body/flexcomp/contact attrib list| replace::
|
.. |body/flexcomp/contact attrib list| replace::
|
||||||
:at:`internal`, :at:`selfcollide`, :at:`vertcollide`, :at:`activelayers`, :at:`contype`, :at:`conaffinity`,
|
:at:`internal`, :at:`selfcollide`, :at:`vertcollide`, :at:`activelayers`, :at:`contype`, :at:`conaffinity`,
|
||||||
@@ -4298,6 +4299,13 @@ extensions specific to flexes.
|
|||||||
|deformable/flex/contact attrib list|
|
|deformable/flex/contact attrib list|
|
||||||
Same meaning as regular :ref:`geom <body-geom>` attributes.
|
Same meaning as regular :ref:`geom <body-geom>` attributes.
|
||||||
|
|
||||||
|
.. _flex-contact-passive:
|
||||||
|
|
||||||
|
:at:`passive`: :at-val:`[true, false], "false"`
|
||||||
|
When enabled, the contact is not added to the contact solver but it is instead used to compute passive
|
||||||
|
(spring-damper) contact forces. All contacts, regardless of the specified condim, are frictionless (condim 1). This
|
||||||
|
is an experimental feature and might change in future releases.
|
||||||
|
|
||||||
|
|
||||||
.. _deformable-skin:
|
.. _deformable-skin:
|
||||||
|
|
||||||
|
|||||||
+2
-2
@@ -458,7 +458,7 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`margin<flexcomp-contact-margin>` | :ref:`gap<flexcomp-contact-gap>` | :ref:`internal<flexcomp-contact-internal>` | :ref:`selfcollide<flexcomp-contact-selfcollide>` | |
|
| | | | :ref:`margin<flexcomp-contact-margin>` | :ref:`gap<flexcomp-contact-gap>` | :ref:`internal<flexcomp-contact-internal>` | :ref:`selfcollide<flexcomp-contact-selfcollide>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`activelayers<flexcomp-contact-activelayers>` | :ref:`vertcollide<flexcomp-contact-vertcollide>` | | | |
|
| | | | :ref:`activelayers<flexcomp-contact-activelayers>` | :ref:`vertcollide<flexcomp-contact-vertcollide>` | :ref:`passive<flexcomp-contact-passive>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
|
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
|
||||||
@@ -508,7 +508,7 @@
|
|||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`margin<flex-contact-margin>` | :ref:`gap<flex-contact-gap>` | :ref:`internal<flex-contact-internal>` | :ref:`selfcollide<flex-contact-selfcollide>` | |
|
| | | | :ref:`margin<flex-contact-margin>` | :ref:`gap<flex-contact-gap>` | :ref:`internal<flex-contact-internal>` | :ref:`selfcollide<flex-contact-selfcollide>` | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
| | | | :ref:`activelayers<flex-contact-activelayers>` | :ref:`vertcollide<flex-contact-vertcollide>` | | | |
|
| | | | :ref:`activelayers<flex-contact-activelayers>` | :ref:`vertcollide<flex-contact-vertcollide>` | :ref:`passive<flex-contact-passive>` | | |
|
||||||
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
|
||||||
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
|
||||||
| |_2| flex |br| |_2| |L| | | .. table:: |
|
| |_2| flex |br| |_2| |L| | | .. table:: |
|
||||||
|
|||||||
@@ -54,6 +54,7 @@ General
|
|||||||
|
|
||||||
- Added support for shells with a curved reference configuration. See this `example
|
- Added support for shells with a curved reference configuration. See this `example
|
||||||
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/basket.xml>`__.
|
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/basket.xml>`__.
|
||||||
|
- Added experimental option for :ref:`passive<flex-contact-passive>` contacts involving flexes.
|
||||||
|
|
||||||
- Added support for assigning a default material to a mesh asset using the :ref:`mesh/material <asset-mesh-material>`
|
- Added support for assigning a default material to a mesh asset using the :ref:`mesh/material <asset-mesh-material>`
|
||||||
attribute.
|
attribute.
|
||||||
|
|||||||
@@ -123,7 +123,7 @@ struct mjContact_ { // result of collision detection functions
|
|||||||
int vert[2]; // vertex ids; -1 for geom or flex element
|
int vert[2]; // vertex ids; -1 for geom or flex element
|
||||||
|
|
||||||
// flag set by mj_setContact or mj_instantiateContact
|
// flag set by mj_setContact or mj_instantiateContact
|
||||||
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs
|
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive
|
||||||
|
|
||||||
// address computed by mj_instantiateContact
|
// address computed by mj_instantiateContact
|
||||||
int efc_address; // address in efc; -1: not included
|
int efc_address; // address in efc; -1: not included
|
||||||
@@ -1220,6 +1220,7 @@ struct mjModel_ {
|
|||||||
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
|
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
|
||||||
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
|
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
|
||||||
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
|
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
|
||||||
|
int* flex_passive; // passive collisions enabled (nflex x 1)
|
||||||
|
|
||||||
// flexes: other properties
|
// flexes: other properties
|
||||||
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
|
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
|
||||||
@@ -2104,6 +2105,7 @@ typedef struct mjsFlex_ { // flex specification
|
|||||||
mjtByte flatskin; // render flex skin with flat shading
|
mjtByte flatskin; // render flex skin with flat shading
|
||||||
int selfcollide; // mode for flex self collision
|
int selfcollide; // mode for flex self collision
|
||||||
int vertcollide; // mode for vertex collision
|
int vertcollide; // mode for vertex collision
|
||||||
|
int passive; // mode for passive collisions
|
||||||
int activelayers; // number of active element layers in 3D
|
int activelayers; // number of active element layers in 3D
|
||||||
int group; // group for visualizatioh
|
int group; // group for visualizatioh
|
||||||
double edgestiffness; // edge stiffness
|
double edgestiffness; // edge stiffness
|
||||||
|
|||||||
@@ -143,7 +143,7 @@ struct mjContact_ { // result of collision detection functions
|
|||||||
int vert[2]; // vertex ids; -1 for geom or flex element
|
int vert[2]; // vertex ids; -1 for geom or flex element
|
||||||
|
|
||||||
// flag set by mj_setContact or mj_instantiateContact
|
// flag set by mj_setContact or mj_instantiateContact
|
||||||
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs
|
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive
|
||||||
|
|
||||||
// address computed by mj_instantiateContact
|
// address computed by mj_instantiateContact
|
||||||
int efc_address; // address in efc; -1: not included
|
int efc_address; // address in efc; -1: not included
|
||||||
|
|||||||
@@ -899,6 +899,7 @@ struct mjModel_ {
|
|||||||
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
|
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
|
||||||
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
|
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
|
||||||
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
|
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
|
||||||
|
int* flex_passive; // passive collisions enabled (nflex x 1)
|
||||||
|
|
||||||
// flexes: other properties
|
// flexes: other properties
|
||||||
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
|
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
|
||||||
|
|||||||
@@ -437,6 +437,7 @@ typedef struct mjsFlex_ { // flex specification
|
|||||||
mjtByte flatskin; // render flex skin with flat shading
|
mjtByte flatskin; // render flex skin with flat shading
|
||||||
int selfcollide; // mode for flex self collision
|
int selfcollide; // mode for flex self collision
|
||||||
int vertcollide; // mode for vertex collision
|
int vertcollide; // mode for vertex collision
|
||||||
|
int passive; // mode for passive collisions
|
||||||
int activelayers; // number of active element layers in 3D
|
int activelayers; // number of active element layers in 3D
|
||||||
int group; // group for visualizatioh
|
int group; // group for visualizatioh
|
||||||
double edgestiffness; // edge stiffness
|
double edgestiffness; // edge stiffness
|
||||||
|
|||||||
@@ -335,6 +335,7 @@
|
|||||||
X ( mjtByte, flex_internal, nflex, 1 ) \
|
X ( mjtByte, flex_internal, nflex, 1 ) \
|
||||||
X ( int, flex_selfcollide, nflex, 1 ) \
|
X ( int, flex_selfcollide, nflex, 1 ) \
|
||||||
X ( int, flex_activelayers, nflex, 1 ) \
|
X ( int, flex_activelayers, nflex, 1 ) \
|
||||||
|
X ( int, flex_passive, nflex, 1 ) \
|
||||||
X ( int, flex_dim, nflex, 1 ) \
|
X ( int, flex_dim, nflex, 1 ) \
|
||||||
X ( int, flex_matid, nflex, 1 ) \
|
X ( int, flex_matid, nflex, 1 ) \
|
||||||
X ( int, flex_group, nflex, 1 ) \
|
X ( int, flex_group, nflex, 1 ) \
|
||||||
|
|||||||
@@ -570,6 +570,7 @@ class ModelC(PyTreeNode):
|
|||||||
flex_internal: jax.Array
|
flex_internal: jax.Array
|
||||||
flex_selfcollide: jax.Array
|
flex_selfcollide: jax.Array
|
||||||
flex_activelayers: jax.Array
|
flex_activelayers: jax.Array
|
||||||
|
flex_passive: jax.Array
|
||||||
flex_dim: jax.Array
|
flex_dim: jax.Array
|
||||||
flex_vertadr: jax.Array
|
flex_vertadr: jax.Array
|
||||||
flex_vertnum: jax.Array
|
flex_vertnum: jax.Array
|
||||||
|
|||||||
@@ -0,0 +1,36 @@
|
|||||||
|
<!-- Copyright 2024 DeepMind Technologies Limited
|
||||||
|
|
||||||
|
Licensed under the Apache License, Version 2.0 (the "License");
|
||||||
|
you may not use this file except in compliance with the License.
|
||||||
|
You may obtain a copy of the License at
|
||||||
|
|
||||||
|
http://www.apache.org/licenses/LICENSE-2.0
|
||||||
|
|
||||||
|
Unless required by applicable law or agreed to in writing, software
|
||||||
|
distributed under the License is distributed on an "AS IS" BASIS,
|
||||||
|
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||||
|
See the License for the specific language governing permissions and
|
||||||
|
limitations under the License.
|
||||||
|
-->
|
||||||
|
|
||||||
|
<mujoco model="Full-flex sphere">
|
||||||
|
<include file="scene.xml"/>
|
||||||
|
|
||||||
|
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
|
||||||
|
|
||||||
|
<size memory="10M"/>
|
||||||
|
|
||||||
|
<visual>
|
||||||
|
<map stiffness="500"/>
|
||||||
|
</visual>
|
||||||
|
|
||||||
|
<worldbody>
|
||||||
|
<geom type="box" pos="1.5 0 0.25" size=".5 2 .25"/>
|
||||||
|
<geom type="box" pos="0 0 0.25" size="2 2 .05" euler="0 15 0"/>
|
||||||
|
<flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 0 1" dim="3"
|
||||||
|
radius=".001" rgba="0 .7 .7 1" mass="5" name="slow">
|
||||||
|
<edge equality="true"/>
|
||||||
|
<contact selfcollide="none" internal="false" passive="true"/>
|
||||||
|
</flexcomp>
|
||||||
|
</worldbody>
|
||||||
|
</mujoco>
|
||||||
@@ -2509,6 +2509,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
doc='number of active element layers, 3D only',
|
doc='number of active element layers, 3D only',
|
||||||
array_extent=('nflex',),
|
array_extent=('nflex',),
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='flex_passive',
|
||||||
|
type=PointerType(
|
||||||
|
inner_type=ValueType(name='int'),
|
||||||
|
),
|
||||||
|
doc='passive collisions enabled',
|
||||||
|
array_extent=('nflex',),
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='flex_dim',
|
name='flex_dim',
|
||||||
type=PointerType(
|
type=PointerType(
|
||||||
@@ -4878,7 +4886,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='exclude',
|
name='exclude',
|
||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='0: include, 1: in gap, 2: fused, 3: no dofs',
|
doc='0: include, 1: in gap, 2: fused, 3: no dofs, 4: passive',
|
||||||
),
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='efc_address',
|
name='efc_address',
|
||||||
@@ -9125,6 +9133,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
|
|||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
doc='mode for vertex collision',
|
doc='mode for vertex collision',
|
||||||
),
|
),
|
||||||
|
StructFieldDecl(
|
||||||
|
name='passive',
|
||||||
|
type=ValueType(name='int'),
|
||||||
|
doc='mode for passive collisions',
|
||||||
|
),
|
||||||
StructFieldDecl(
|
StructFieldDecl(
|
||||||
name='activelayers',
|
name='activelayers',
|
||||||
type=ValueType(name='int'),
|
type=ValueType(name='int'),
|
||||||
|
|||||||
+140
-123
@@ -852,6 +852,86 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// compute Jacobian for contact, return number of DOFs affected
|
||||||
|
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||||
|
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||||
|
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||||
|
mjtNum* jac1r, mjtNum* jac2r, int* chain) {
|
||||||
|
// special case: single body on each side
|
||||||
|
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
|
||||||
|
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
|
||||||
|
// get bodies
|
||||||
|
int bid[2];
|
||||||
|
for (int side=0; side < 2; side++) {
|
||||||
|
bid[side] = (con->geom[side] >= 0) ?
|
||||||
|
m->geom_bodyid[con->geom[side]] :
|
||||||
|
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute Jacobian differences
|
||||||
|
if (dim > 3) {
|
||||||
|
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
||||||
|
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
|
||||||
|
} else {
|
||||||
|
return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
||||||
|
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// general case: flex elements involved
|
||||||
|
else {
|
||||||
|
// get bodies and weights
|
||||||
|
int nb = 0;
|
||||||
|
int bid[64];
|
||||||
|
mjtNum bweight[64];
|
||||||
|
for (int side=0; side < 2; side++) {
|
||||||
|
int nw = 0;
|
||||||
|
int vid[4];
|
||||||
|
mjtNum bw[4];
|
||||||
|
|
||||||
|
// geom
|
||||||
|
if (con->geom[side] >= 0) {
|
||||||
|
bid[nb] = m->geom_bodyid[con->geom[side]];
|
||||||
|
bweight[nb] = side ? +1 : -1;
|
||||||
|
nb++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// flex vert
|
||||||
|
else if (con->vert[side] >= 0) {
|
||||||
|
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
|
||||||
|
bw[0] = side ? +1 : -1;
|
||||||
|
nw = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// flex elem
|
||||||
|
else {
|
||||||
|
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
|
||||||
|
con->vert[1-side], con->pos, vid, bw);
|
||||||
|
|
||||||
|
// negative sign for first side of contact
|
||||||
|
if (side == 0) {
|
||||||
|
mju_scl(bw, bw, -1, nw);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// get body or node ids and weights
|
||||||
|
for (int k=0; k < nw; k++) {
|
||||||
|
if (m->flex_interp[con->flex[side]] == 0) {
|
||||||
|
bid[nb] = m->flex_vertbodyid[vid[k]];
|
||||||
|
bweight[nb] = bw[k];
|
||||||
|
nb++;
|
||||||
|
} else {
|
||||||
|
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
|
||||||
|
con->pos, bid+nb, bweight+nb, bw[k]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// combine weighted Jacobians
|
||||||
|
return mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// frictionless and frictional contacts
|
// frictionless and frictional contacts
|
||||||
void mj_instantiateContact(const mjModel* m, mjData* d) {
|
void mj_instantiateContact(const mjModel* m, mjData* d) {
|
||||||
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
|
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
|
||||||
@@ -880,142 +960,72 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
// find contacts to be included
|
// find contacts to be included
|
||||||
for (int i=0; i < ncon; i++) {
|
for (int i=0; i < ncon; i++) {
|
||||||
if (!d->contact[i].exclude) {
|
if (d->contact[i].exclude) {
|
||||||
// get contact info, safe efc_address
|
continue;
|
||||||
con = d->contact + i;
|
}
|
||||||
dim = con->dim;
|
|
||||||
con->efc_address = d->nefc;
|
|
||||||
|
|
||||||
// special case: single body on each side
|
// get contact info, save efc_address
|
||||||
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) &&
|
con = d->contact + i;
|
||||||
(con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
|
dim = con->dim;
|
||||||
// get bodies
|
con->efc_address = d->nefc;
|
||||||
int bid[2];
|
NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr,
|
||||||
for (int side=0; side < 2; side++) {
|
jac1p, jac2p, jac1r, jac2r, chain);
|
||||||
bid[side] = (con->geom[side] >= 0) ?
|
|
||||||
m->geom_bodyid[con->geom[side]] :
|
|
||||||
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
|
|
||||||
}
|
|
||||||
|
|
||||||
// compute Jacobian differences
|
// skip contact if no DOFs affected
|
||||||
if (dim > 3) {
|
if (NV == 0) {
|
||||||
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
con->efc_address = -1;
|
||||||
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
|
con->exclude = 3;
|
||||||
} else {
|
continue;
|
||||||
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
|
}
|
||||||
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// general case: flex elements involved
|
// rotate Jacobian differences to contact frame
|
||||||
else {
|
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
|
||||||
// get bodies and weights
|
if (dim > 3) {
|
||||||
int nb = 0;
|
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
|
||||||
int bid[64];
|
}
|
||||||
mjtNum bweight[64];
|
|
||||||
for (int side=0; side < 2; side++) {
|
|
||||||
int nw = 0;
|
|
||||||
int vid[4];
|
|
||||||
mjtNum bw[4];
|
|
||||||
|
|
||||||
// geom
|
// make frictionless contact
|
||||||
if (con->geom[side] >= 0) {
|
if (dim == 1) {
|
||||||
bid[nb] = m->geom_bodyid[con->geom[side]];
|
// add constraint
|
||||||
bweight[nb] = side ? +1 : -1;
|
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
|
||||||
nb++;
|
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
|
||||||
}
|
issparse ? NV : 0,
|
||||||
|
issparse ? chain : NULL);
|
||||||
|
}
|
||||||
|
|
||||||
// flex vert
|
// make pyramidal friction cone
|
||||||
else if (con->vert[side] >= 0) {
|
else if (ispyramid) {
|
||||||
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
|
// pos = dist
|
||||||
bw[0] = side ? +1 : -1;
|
cpos[0] = cpos[1] = con->dist;
|
||||||
nw = 1;
|
cmargin[0] = cmargin[1] = con->includemargin;
|
||||||
}
|
|
||||||
|
|
||||||
// flex elem
|
// one pair per friction dimension
|
||||||
else {
|
for (int k=1; k < con->dim; k++) {
|
||||||
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
|
// Jacobian for pair of opposing pyramid edges
|
||||||
con->vert[1-side], con->pos, vid, bw);
|
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
|
||||||
|
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
|
||||||
|
|
||||||
// negative sign for first side of contact
|
|
||||||
if (side == 0) {
|
|
||||||
mju_scl(bw, bw, -1, nw);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// get body or node ids and weights
|
|
||||||
for (int k=0; k < nw; k++) {
|
|
||||||
if (m->flex_interp[con->flex[side]] == 0) {
|
|
||||||
bid[nb] = m->flex_vertbodyid[vid[k]];
|
|
||||||
bweight[nb] = bw[k];
|
|
||||||
nb++;
|
|
||||||
} else {
|
|
||||||
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
|
|
||||||
con->pos, bid+nb, bweight+nb, bw[k]);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// combine weighted Jacobians
|
|
||||||
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
|
|
||||||
}
|
|
||||||
|
|
||||||
// skip contact if no DOFs affected
|
|
||||||
if (NV == 0) {
|
|
||||||
con->efc_address = -1;
|
|
||||||
con->exclude = 3;
|
|
||||||
continue;
|
|
||||||
}
|
|
||||||
|
|
||||||
// rotate Jacobian differences to contact frame
|
|
||||||
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
|
|
||||||
if (dim > 3) {
|
|
||||||
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
|
|
||||||
}
|
|
||||||
|
|
||||||
// make frictionless contact
|
|
||||||
if (dim == 1) {
|
|
||||||
// add constraint
|
// add constraint
|
||||||
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
|
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
|
||||||
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
|
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
|
||||||
issparse ? NV : 0,
|
issparse ? NV : 0,
|
||||||
issparse ? chain : NULL);
|
issparse ? chain : NULL);
|
||||||
}
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// make pyramidal friction cone
|
// make elliptic friction cone
|
||||||
else if (ispyramid) {
|
else {
|
||||||
// pos = dist
|
// normal pos = dist, all others 0
|
||||||
cpos[0] = cpos[1] = con->dist;
|
mju_zero(cpos, con->dim);
|
||||||
cmargin[0] = cmargin[1] = con->includemargin;
|
mju_zero(cmargin, con->dim);
|
||||||
|
cpos[0] = con->dist;
|
||||||
|
cmargin[0] = con->includemargin;
|
||||||
|
|
||||||
// one pair per friction dimension
|
// add constraint
|
||||||
for (int k=1; k < con->dim; k++) {
|
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
|
||||||
// Jacobian for pair of opposing pyramid edges
|
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
|
||||||
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
|
issparse ? NV : 0,
|
||||||
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
|
issparse ? chain : NULL);
|
||||||
|
|
||||||
// add constraint
|
|
||||||
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
|
|
||||||
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
|
|
||||||
issparse ? NV : 0,
|
|
||||||
issparse ? chain : NULL);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// make elliptic friction cone
|
|
||||||
else {
|
|
||||||
// normal pos = dist, all others 0
|
|
||||||
mju_zero(cpos, con->dim);
|
|
||||||
mju_zero(cmargin, con->dim);
|
|
||||||
cpos[0] = con->dist;
|
|
||||||
cmargin[0] = con->includemargin;
|
|
||||||
|
|
||||||
// add constraint
|
|
||||||
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
|
|
||||||
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
|
|
||||||
issparse ? NV : 0,
|
|
||||||
issparse ? chain : NULL);
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -1814,6 +1824,13 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
|
|||||||
for (int i=0; i < ncon; i++) {
|
for (int i=0; i < ncon; i++) {
|
||||||
mjContact* con = d->contact + i;
|
mjContact* con = d->contact + i;
|
||||||
|
|
||||||
|
// skip if passive
|
||||||
|
if ((con->flex[0] > -1 && m->flex_passive[con->flex[0]]) ||
|
||||||
|
(con->flex[1] > -1 && m->flex_passive[con->flex[1]])) {
|
||||||
|
con->efc_address = -1;
|
||||||
|
con->exclude = 4;
|
||||||
|
}
|
||||||
|
|
||||||
// skip if excluded
|
// skip if excluded
|
||||||
if (con->exclude) {
|
if (con->exclude) {
|
||||||
continue;
|
continue;
|
||||||
|
|||||||
@@ -65,9 +65,15 @@ void mj_instantiateFriction(const mjModel* m, mjData* d);
|
|||||||
// joint and tendon limits
|
// joint and tendon limits
|
||||||
void mj_instantiateLimit(const mjModel* m, mjData* d);
|
void mj_instantiateLimit(const mjModel* m, mjData* d);
|
||||||
|
|
||||||
// frictionelss and frictional contacts
|
// frictionless and frictional contacts
|
||||||
void mj_instantiateContact(const mjModel* m, mjData* d);
|
void mj_instantiateContact(const mjModel* m, mjData* d);
|
||||||
|
|
||||||
|
// compute Jacobian for contact, return number of DOFs affected
|
||||||
|
int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
|
||||||
|
mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
|
||||||
|
mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
|
||||||
|
mjtNum* jac1r, mjtNum* jac2r, int* chain);
|
||||||
|
|
||||||
|
|
||||||
//------------------------ parameter computation/extraction ----------------------------------------
|
//------------------------ parameter computation/extraction ----------------------------------------
|
||||||
|
|
||||||
|
|||||||
@@ -36,6 +36,9 @@
|
|||||||
|
|
||||||
//----------------------------- passive forces -----------------------------------------------------
|
//----------------------------- passive forces -----------------------------------------------------
|
||||||
|
|
||||||
|
// stiffness for passive contacts
|
||||||
|
static const mjtNum kContactStiffness = 1e4;
|
||||||
|
|
||||||
// local edge-based vertex indexing for 2D and 3D elements, 2D and 3D elements
|
// local edge-based vertex indexing for 2D and 3D elements, 2D and 3D elements
|
||||||
// have 3 and 6 edges, respectively so the missing indexes are set to 0
|
// have 3 and 6 edges, respectively so the missing indexes are set to 0
|
||||||
static const int edges[2][6][2] = {{{1, 2}, {2, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}},
|
static const int edges[2][6][2] = {{{1, 2}, {2, 0}, {0, 1}, {0, 0}, {0, 0}, {0, 0}},
|
||||||
@@ -515,6 +518,89 @@ static int mj_fluid(const mjModel* m, mjData* d) {
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
// passive contact forces
|
||||||
|
int mj_contactPassive(const mjModel* m, mjData* d) {
|
||||||
|
int ncon = d->ncon, issparse = mj_isSparse(m);
|
||||||
|
int dim, NV, nv = m->nv, *chain = NULL;
|
||||||
|
mjtNum *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r, *qfrc;
|
||||||
|
mjContact* con;
|
||||||
|
int has_contact = 0;
|
||||||
|
|
||||||
|
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// early return if no contact to be included
|
||||||
|
for (int i=0; i < ncon; i++) {
|
||||||
|
if (d->contact[i].exclude != 4) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
has_contact = 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (!has_contact) {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// allocate Jacobian
|
||||||
|
mj_markStack(d);
|
||||||
|
jac = mjSTACKALLOC(d, 6*nv, mjtNum);
|
||||||
|
jacdif = mjSTACKALLOC(d, 6*nv, mjtNum);
|
||||||
|
jacdifp = jacdif;
|
||||||
|
jacdifr = jacdif + 3*nv;
|
||||||
|
jac1p = mjSTACKALLOC(d, 3*nv, mjtNum);
|
||||||
|
jac2p = mjSTACKALLOC(d, 3*nv, mjtNum);
|
||||||
|
jac1r = mjSTACKALLOC(d, 3*nv, mjtNum);
|
||||||
|
jac2r = mjSTACKALLOC(d, 3*nv, mjtNum);
|
||||||
|
qfrc = mjSTACKALLOC(d, nv, mjtNum);
|
||||||
|
if (issparse) {
|
||||||
|
chain = mjSTACKALLOC(d, nv, int);
|
||||||
|
}
|
||||||
|
|
||||||
|
// find contacts to be included
|
||||||
|
for (int i=0; i < ncon; i++) {
|
||||||
|
if (d->contact[i].exclude != 4) {
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// get contact info, safe efc_address
|
||||||
|
con = d->contact + i;
|
||||||
|
dim = con->dim;
|
||||||
|
con->efc_address = -1;
|
||||||
|
NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr,
|
||||||
|
jac1p, jac2p, jac1r, jac2r, chain);
|
||||||
|
|
||||||
|
// skip contact if no DOFs affected
|
||||||
|
if (NV == 0) {
|
||||||
|
con->efc_address = -1;
|
||||||
|
con->exclude = 3;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// rotate Jacobian differences to contact frame
|
||||||
|
mju_mulMatMat(jac, con->frame, jacdifp, dim > 1 ? 3 : 1, 3, NV);
|
||||||
|
if (dim > 3) {
|
||||||
|
mju_mulMatMat(jac + 3*NV, con->frame, jacdifr, dim-3, 3, NV);
|
||||||
|
}
|
||||||
|
|
||||||
|
// compute passive contact force (dim = 1)
|
||||||
|
mjtNum scl = -kContactStiffness*con->dist;
|
||||||
|
if (!issparse) {
|
||||||
|
mju_addToScl(d->qfrc_spring, jac, scl, nv);
|
||||||
|
} else {
|
||||||
|
mju_scl(qfrc, jac, scl, NV);
|
||||||
|
for (int j=0; j < NV; j++) {
|
||||||
|
d->qfrc_spring[chain[j]] += qfrc[j];
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
mj_freeStack(d);
|
||||||
|
return has_contact;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// all passive forces
|
// all passive forces
|
||||||
void mj_passive(const mjModel* m, mjData* d) {
|
void mj_passive(const mjModel* m, mjData* d) {
|
||||||
int nv = m->nv;
|
int nv = m->nv;
|
||||||
@@ -540,9 +626,14 @@ void mj_passive(const mjModel* m, mjData* d) {
|
|||||||
// fluid forces
|
// fluid forces
|
||||||
int has_fluid = mj_fluid(m, d);
|
int has_fluid = mj_fluid(m, d);
|
||||||
|
|
||||||
|
// contact forces
|
||||||
|
mj_contactPassive(m, d);
|
||||||
|
|
||||||
// add passive forces into qfrc_passive
|
// add passive forces into qfrc_passive
|
||||||
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
|
mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv);
|
||||||
if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
|
if (has_fluid) {
|
||||||
|
mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv);
|
||||||
|
}
|
||||||
if (has_gravcomp) {
|
if (has_gravcomp) {
|
||||||
int njnt = m->njnt;
|
int njnt = m->njnt;
|
||||||
for (int i=0; i < njnt; i++) {
|
for (int i=0; i < njnt; i++) {
|
||||||
|
|||||||
@@ -3321,6 +3321,7 @@ void mjCModel::CopyObjects(mjModel* m) {
|
|||||||
m->flex_flatskin[i] = pfl->flatskin;
|
m->flex_flatskin[i] = pfl->flatskin;
|
||||||
m->flex_selfcollide[i] = pfl->selfcollide;
|
m->flex_selfcollide[i] = pfl->selfcollide;
|
||||||
m->flex_activelayers[i] = pfl->activelayers;
|
m->flex_activelayers[i] = pfl->activelayers;
|
||||||
|
m->flex_passive[i] = pfl->passive;
|
||||||
m->flex_bvhnum[i] = pfl->tree.Nbvh();
|
m->flex_bvhnum[i] = pfl->tree.Nbvh();
|
||||||
m->flex_bvhadr[i] = pfl->tree.Nbvh() ? bvh_adr : -1;
|
m->flex_bvhadr[i] = pfl->tree.Nbvh() ? bvh_adr : -1;
|
||||||
|
|
||||||
|
|||||||
@@ -316,9 +316,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"<"},
|
{"<"},
|
||||||
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
|
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
|
||||||
{"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
|
{"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
|
||||||
{"contact", "?", "14", "contype", "conaffinity", "condim", "priority",
|
{"contact", "?", "15", "contype", "conaffinity", "condim", "priority",
|
||||||
"friction", "solmix", "solref", "solimp", "margin", "gap",
|
"friction", "solmix", "solref", "solimp", "margin", "gap",
|
||||||
"internal", "selfcollide", "activelayers", "vertcollide"},
|
"internal", "selfcollide", "activelayers", "vertcollide", "passive"},
|
||||||
{"pin", "*", "4", "id", "range", "grid", "gridrange"},
|
{"pin", "*", "4", "id", "range", "grid", "gridrange"},
|
||||||
{"plugin", "*", "2", "plugin", "instance"},
|
{"plugin", "*", "2", "plugin", "instance"},
|
||||||
{"<"},
|
{"<"},
|
||||||
@@ -332,9 +332,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
|
|||||||
{"flex", "*", "13", "name", "group", "dim", "radius", "material",
|
{"flex", "*", "13", "name", "group", "dim", "radius", "material",
|
||||||
"rgba", "flatskin", "body", "vertex", "element", "texcoord", "elemtexcoord", "node"},
|
"rgba", "flatskin", "body", "vertex", "element", "texcoord", "elemtexcoord", "node"},
|
||||||
{"<"},
|
{"<"},
|
||||||
{"contact", "?", "14", "contype", "conaffinity", "condim", "priority",
|
{"contact", "?", "15", "contype", "conaffinity", "condim", "priority",
|
||||||
"friction", "solmix", "solref", "solimp", "margin", "gap",
|
"friction", "solmix", "solref", "solimp", "margin", "gap",
|
||||||
"internal", "selfcollide", "activelayers", "vertcollide"},
|
"internal", "selfcollide", "activelayers", "vertcollide", "passive"},
|
||||||
{"edge", "?", "2", "stiffness", "damping"},
|
{"edge", "?", "2", "stiffness", "damping"},
|
||||||
{"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
|
{"elasticity", "?", "5", "young", "poisson", "damping", "thickness", "elastic2d"},
|
||||||
{">"},
|
{">"},
|
||||||
@@ -1460,6 +1460,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
|
|||||||
if (MapValue(cont, "vertcollide", &flex->vertcollide, bool_map, 2)) {
|
if (MapValue(cont, "vertcollide", &flex->vertcollide, bool_map, 2)) {
|
||||||
flex->vertcollide = (n == 1);
|
flex->vertcollide = (n == 1);
|
||||||
}
|
}
|
||||||
|
if (MapValue(cont, "passive", &flex->passive, bool_map, 2)) {
|
||||||
|
flex->passive = (n == 1);
|
||||||
|
}
|
||||||
ReadAttrInt(cont, "activelayers", &flex->activelayers);
|
ReadAttrInt(cont, "activelayers", &flex->activelayers);
|
||||||
}
|
}
|
||||||
|
|
||||||
@@ -2721,6 +2724,9 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
|
|||||||
if (MapValue(cont, "vertcollide", &n, bool_map, 2)) {
|
if (MapValue(cont, "vertcollide", &n, bool_map, 2)) {
|
||||||
dflex.vertcollide = (n == 1);
|
dflex.vertcollide = (n == 1);
|
||||||
}
|
}
|
||||||
|
if (MapValue(cont, "passive", &n, bool_map, 2)) {
|
||||||
|
dflex.passive = (n == 1);
|
||||||
|
}
|
||||||
ReadAttrInt(cont, "activelayers", &dflex.activelayers);
|
ReadAttrInt(cont, "activelayers", &dflex.activelayers);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@@ -5477,6 +5477,7 @@ public unsafe struct mjModel_ {
|
|||||||
public byte* flex_internal;
|
public byte* flex_internal;
|
||||||
public int* flex_selfcollide;
|
public int* flex_selfcollide;
|
||||||
public int* flex_activelayers;
|
public int* flex_activelayers;
|
||||||
|
public int* flex_passive;
|
||||||
public int* flex_dim;
|
public int* flex_dim;
|
||||||
public int* flex_matid;
|
public int* flex_matid;
|
||||||
public int* flex_group;
|
public int* flex_group;
|
||||||
|
|||||||
Reference in New Issue
Block a user