Add passive collision mode for flexes.

PiperOrigin-RevId: 801754907
Change-Id: I4147af2ee2596519d42cb9a03cf8d5488f0bc3e8
This commit is contained in:
Alessio Quaglino
2025-09-01 04:13:07 -07:00
committed by Copybara-Service
parent b9900db00e
commit 8acd83f317
17 changed files with 320 additions and 134 deletions
+8
View File
@@ -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
View File
@@ -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:: |
+1
View File
@@ -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.
+3 -1
View File
@@ -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
+1 -1
View File
@@ -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
+1
View File
@@ -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)
+1
View File
@@ -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
+1
View File
@@ -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 ) \
+1
View File
@@ -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
+36
View File
@@ -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>
+14 -1
View File
@@ -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'),
+55 -38
View File
@@ -852,40 +852,11 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// frictionless and frictional contacts // compute Jacobian for contact, return number of DOFs affected
void mj_instantiateContact(const mjModel* m, mjData* d) { int mj_contactJacobian(const mjModel* m, mjData* d, const mjContact* con, int dim,
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon; mjtNum* jac, mjtNum* jacdif, mjtNum* jacdifp,
int dim, NV, nv = m->nv, *chain = NULL; mjtNum* jacdifr, mjtNum* jac1p, mjtNum* jac2p,
mjContact* con; mjtNum* jac1r, mjtNum* jac2r, int* chain) {
mjtNum cpos[6], cmargin[6], *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) {
return;
}
mj_markStack(d);
// allocate Jacobian
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);
if (issparse) {
chain = mjSTACKALLOC(d, nv, int);
}
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (!d->contact[i].exclude) {
// get contact info, safe efc_address
con = d->contact + i;
dim = con->dim;
con->efc_address = d->nefc;
// special case: single body on each side // special case: single body on each side
if ((con->geom[0] >= 0 || (con->vert[0] >= 0 && m->flex_interp[con->flex[0]] == 0)) && 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))) { (con->geom[1] >= 0 || (con->vert[1] >= 0 && m->flex_interp[con->flex[1]] == 0))) {
@@ -899,10 +870,10 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// compute Jacobian differences // compute Jacobian differences
if (dim > 3) { if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr); jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else { } else {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos, return mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL); jac1p, jac2p, jacdifp, NULL, NULL, NULL);
} }
} }
@@ -957,8 +928,48 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
} }
// combine weighted Jacobians // combine weighted Jacobians
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3); return mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim > 3);
} }
}
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d) {
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
int dim, NV, nv = m->nv, *chain = NULL;
mjContact* con;
mjtNum cpos[6], cmargin[6], *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0 || nv == 0) {
return;
}
mj_markStack(d);
// allocate Jacobian
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);
if (issparse) {
chain = mjSTACKALLOC(d, nv, int);
}
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (d->contact[i].exclude) {
continue;
}
// get contact info, save efc_address
con = d->contact + i;
dim = con->dim;
con->efc_address = d->nefc;
NV = mj_contactJacobian(m, d, con, dim, jac, jacdif, jacdifp, jacdifr,
jac1p, jac2p, jac1r, jac2r, chain);
// skip contact if no DOFs affected // skip contact if no DOFs affected
if (NV == 0) { if (NV == 0) {
@@ -1017,7 +1028,6 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
issparse ? chain : NULL); issparse ? chain : NULL);
} }
} }
}
mj_freeStack(d); mj_freeStack(d);
} }
@@ -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;
+7 -1
View File
@@ -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 ----------------------------------------
+92 -1
View File
@@ -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++) {
+1
View File
@@ -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;
+10 -4
View File
@@ -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);
} }
+1
View File
@@ -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;