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

Introduce trilinear flex parametrization.

Browse Source 
These flexes use only 24 DOFs (3 per vertex of the bounding box), while colliding with the full high resolution mesh.

On an 8x8x8 cube, the performance using DOFs at all vertices is

```
 Simulation time      : 18.74 s
 Steps per second     : 533
 Realtime factor      : 0.53 x
 Time per step        : 1874.4 µs

 Contacts per step    : 114.88
 Constraints per step : 3322.51
 Degrees of freedom   : 1536
```

With the new implementation, it is the following:

```
 Simulation time      : 1.82 s
 Steps per second     : 5507
 Realtime factor      : 5.51 x
 Time per step        : 181.6 µs

 Contacts per step    : 38.84
 Constraints per step : 155.36
 Degrees of freedom   : 24
```

PiperOrigin-RevId: 721008829
Change-Id: I833df027527db578d86667cc4b24295bcf6f7d22
This commit is contained in:
Alessio Quaglino
2025-01-29 09:37:37 -08:00
committed by Copybara-Service
parent 1a4b821b6b
commit 7cdf180641
47 changed files with 8967 additions and 96 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+2 -2
View File
@@ -3572,8 +3572,8 @@ Construct quaternion performing rotation from z-axis to given vector.
.. mujoco-include:: mju_mat2Rot
extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion
returns the number of iterations required to converge
Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion.
Returns the number of iterations required to converge
.. _mju_euler2Quat:
doc/XMLreference.rst
+45
View File
@@ -3487,6 +3487,45 @@ saving the XML:
radius in 2D, and tetrahedra with radius in 3D. Certain flexcomp types imply a dimensionality, in which case the
value specified here is ignored.
.. youtube:: uNt3i8hrJu4
:align: right
:width: 240px
.. _body-flexcomp-dof:
:at:`dof`: :at-val:`[full, radial, trilinear], "full"`
The parametrization of the flex's degrees of freedom (dofs). See the video on the right illustrating the
different parametrizations with deformable spheres. The three models in the video are respectively
`sphere_full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_full.xml>`__,
`sphere_radial <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_radial.xml>`__
and `sphere_trilinear <https://github.com/google-deepmind/mujoco/blob/main/model/flex/sphere_trilinear.xml>`__.
**full**
Three translational dofs per vertex. This is the most expressive but also the most expensive option.
**radial**
A single radial translational dof per vertex. Note that unlike in the "full" case, the radial parametrization
requires a free joint at the flex's parent in order for free body motion to be possible. This type of
parametrization is appropriate for shapes that are relatively spherical.
**trilinear**
Three translational dofs at each corner of the bounding box of the flex, for a total of 24 dofs for the entire
flex, independent of the number of vertices. The positions of the vertices are updated using trilinear
interpolation over the bounding box.
.. youtube:: qJFbx-FR7Bc
:align: right
:width: 240px
Trilinear flexes are much faster than the previous two options, and are the preferred choice if the expected
deformations can be captured by the reduced parametriation. For example, see the video on the right comparing `full
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper_trilinear.xml>`__ flexes for modeling
deformable gripper pads.
Note that the choice of dof parametrization affects the deformation modes of the flex but has no effect on the
accuracy of the collision geometry, which always takes into account the high-resolution mesh of the flex.
.. _body-flexcomp-type:
:at:`type`: :at-val:`[grid, box, cylinder, ellipsoid, disc, circle, mesh, gmsh, direct], "grid"`
@@ -4081,6 +4120,12 @@ cases, the user will specify a :el:`flexcomp` which will then automatically cons
Integer group to which the flex belongs. This attribute can be used for custom tags. It is also used by the
visualizer to enable and disable the rendering of entire groups of flexes.
.. _deformable-flex-node:
:at:`node`: :at-val:`string(nnode), optional`
The degrees-of-freedom of the flex.
An array of MuJoCo body names (separated by white space) to which each node belongs. The number of body names
should equal the number of nodes (nnode). See the flexcomp :ref:`dof<body-flexcomp-dof>` attribute for more details.
.. _flex-edge:
doc/XMLschema.rst
+7 -7
View File
@@ -435,17 +435,17 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<body-flexcomp-name>` | :ref:`type<body-flexcomp-type>` | :ref:`group<body-flexcomp-group>` | :ref:`dim<body-flexcomp-dim>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`count<body-flexcomp-count>` | :ref:`spacing<body-flexcomp-spacing>` | :ref:`radius<body-flexcomp-radius>` | :ref:`rigid<body-flexcomp-rigid>` | |
| | | | :ref:`dof<body-flexcomp-dof>` | :ref:`count<body-flexcomp-count>` | :ref:`spacing<body-flexcomp-spacing>` | :ref:`radius<body-flexcomp-radius>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`mass<body-flexcomp-mass>` | :ref:`inertiabox<body-flexcomp-inertiabox>` | :ref:`scale<body-flexcomp-scale>` | :ref:`file<body-flexcomp-file>` | |
| | | | :ref:`rigid<body-flexcomp-rigid>` | :ref:`mass<body-flexcomp-mass>` | :ref:`inertiabox<body-flexcomp-inertiabox>` | :ref:`scale<body-flexcomp-scale>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`point<body-flexcomp-point>` | :ref:`element<body-flexcomp-element>` | :ref:`texcoord<body-flexcomp-texcoord>` | :ref:`material<body-flexcomp-material>` | |
| | | | :ref:`file<body-flexcomp-file>` | :ref:`point<body-flexcomp-point>` | :ref:`element<body-flexcomp-element>` | :ref:`texcoord<body-flexcomp-texcoord>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`rgba<body-flexcomp-rgba>` | :ref:`flatskin<body-flexcomp-flatskin>` | :ref:`pos<body-flexcomp-pos>` | :ref:`quat<body-flexcomp-quat>` | |
| | | | :ref:`material<body-flexcomp-material>` | :ref:`rgba<body-flexcomp-rgba>` | :ref:`flatskin<body-flexcomp-flatskin>` | :ref:`pos<body-flexcomp-pos>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`axisangle<body-flexcomp-axisangle>` | :ref:`xyaxes<body-flexcomp-xyaxes>` | :ref:`zaxis<body-flexcomp-zaxis>` | :ref:`euler<body-flexcomp-euler>` | |
| | | | :ref:`quat<body-flexcomp-quat>` | :ref:`axisangle<body-flexcomp-axisangle>` | :ref:`xyaxes<body-flexcomp-xyaxes>` | :ref:`zaxis<body-flexcomp-zaxis>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`origin<body-flexcomp-origin>` | | | | |
| | | | :ref:`euler<body-flexcomp-euler>` | :ref:`origin<body-flexcomp-origin>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
@@ -509,7 +509,7 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`material<deformable-flex-material>` | :ref:`rgba<deformable-flex-rgba>` | :ref:`flatskin<deformable-flex-flatskin>` | :ref:`body<deformable-flex-body>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`vertex<deformable-flex-vertex>` | :ref:`element<deformable-flex-element>` | :ref:`texcoord<deformable-flex-texcoord>` | | |
| | | | :ref:`vertex<deformable-flex-vertex>` | :ref:`element<deformable-flex-element>` | :ref:`texcoord<deformable-flex-texcoord>` | :ref:`node<deformable-flex-node>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flex |br| |_2| |L| | | .. table:: |
doc/changelog.rst
+19
View File
@@ -5,8 +5,27 @@ Changelog
Upcoming version (not yet released)
-----------------------------------
Feature promotion
^^^^^^^^^^^^^^^^^
.. youtube:: qJFbx-FR7Bc
:align: right
:width: 240px
- Introduced a new kind of **fast deformable body**, activated by setting :ref:`flexcomp/dof<body-flexcomp-dof>` to
"trilinear". This type of :ref:`deformable<CDeformable>` flex object has the same collision geometry as a regular
flex, but has far fewer degrees of freedom. Instead of 3 dofs per vertex, only the corners of the bounding box are
free to move, with the positions of the interior vertices computed with trilinear interpolation of the 8 corners, for
a total of 24 dofs for the entire flex object (or less, if some of the corners are pinned). This limits the types of
deformation achievable by the flex, but allows for much faster simulation. For example, see the video on the right
comparing `full <https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper.xml>`__ and `trilinear
<https://github.com/google-deepmind/mujoco/blob/main/model/flex/gripper_trilinear.xml>`__ flexes for modeling
deformable gripper pads.
General
^^^^^^^
- Separate collision and deformation meshes for :ref:`flex<deformable-flex>`. This enables a fixed cost for the soft
body computations, while preserving the fidelity of high-resolution collisions.
- Added :ref:`mjs_setDeepCopy` API function. When the deep copy flag is 0, attaching a model will not copy it to the
parent, so the original references to the child can be used to modify the parent after attachment. The default
behavior is to perform such a shallow copy. The old behavior of creating a deep copy of the child model while
doc/includes/references.h
+15
View File
@@ -901,6 +901,7 @@ struct mjModel_ {
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of flexes
int nflexnode; // number of dofs in all flexes
int nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
@@ -1153,6 +1154,9 @@ struct mjModel_ {
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
int* flex_matid; // material id for rendering (nflex x 1)
int* flex_group; // group for visibility (nflex x 1)
int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1)
int* flex_nodeadr; // first node address (nflex x 1)
int* flex_nodenum; // number of nodes (nflex x 1)
int* flex_vertadr; // first vertex address (nflex x 1)
int* flex_vertnum; // number of vertices (nflex x 1)
int* flex_edgeadr; // first edge address (nflex x 1)
@@ -1166,6 +1170,7 @@ struct mjModel_ {
int* flex_evpairadr; // first evpair address (nflex x 1)
int* flex_evpairnum; // number of evpairs (nflex x 1)
int* flex_texcoordadr; // address in flex_texcoord; -1: none (nflex x 1)
int* flex_nodebodyid; // node body ids (nflexnode x 1)
int* flex_vertbodyid; // vertex body ids (nflexvert x 1)
int* flex_edge; // edge vertex ids (2 per edge) (nflexedge x 2)
int* flex_elem; // element vertex ids (dim+1 per elem) (nflexelemdata x 1)
@@ -1175,6 +1180,8 @@ struct mjModel_ {
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3)
mjtNum* flex_node; // node positions in local body frames (nflexnode x 3)
mjtNum* flex_node0; // Cartesian node positions in qpos0 (nflexnode x 3)
mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1)
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
mjtNum* flex_radius; // radius around primitive element (nflex x 1)
@@ -1983,7 +1990,9 @@ typedef struct mjsFlex_ { // flex specification
double thickness; // thickness (2D only)
// mesh properties
mjStringVec* nodebody; // node body names
mjStringVec* vertbody; // vertex body names
mjDoubleVec* node; // node positions
mjDoubleVec* vert; // vertex positions
mjIntVec* elem; // element vertex ids
mjFloatVec* texcoord; // vertex texture coordinates
@@ -2968,6 +2977,10 @@ struct mjvSceneState_ {
int* flex_dim;
int* flex_matid;
int* flex_group;
int* flex_interp;
int* flex_nodeadr;
int* flex_nodenum;
int* flex_nodebodyid;
int* flex_vertadr;
int* flex_vertnum;
int* flex_elem;
@@ -2981,6 +2994,8 @@ struct mjvSceneState_ {
int* flex_texcoordadr;
int* flex_bvhadr;
int* flex_bvhnum;
mjtByte* flex_centered;
mjtNum* flex_node;
mjtNum* flex_radius;
float* flex_rgba;
doc/modeling.rst
+2 -12
View File
@@ -1335,9 +1335,7 @@ improved realism and accuracy. The edge-based model could be seen as a "lumped"
coupling of deformation modes (e.g. shear and volumetric) is averaged in a single quantity. The continuum model enables
instead to specify shear and volumetic stiffnesses separately using the `Poisson's ratio
<https://en.wikipedia.org/wiki/Poisson%27s_ratio>`__ of the material. For more details, see the `Saint Venant-Kirchhoff
<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ hyperelastic model. This
functionality is currently based on first-party :ref:`engine plugins<exPlugin>` as of MuJoCo 3.0 but may be integrated
into the engine in future releases.
<https://en.wikipedia.org/wiki/Hyperelastic_material#Saint_Venant%E2%80%93Kirchhoff_model>`__ hyperelastic model.
**Creation and visualization**.
@@ -1345,20 +1343,12 @@ into the engine in future releases.
<option timestep=".001"/>
<extension>
<plugin plugin="mujoco.elasticity.solid"/>
</extension>
<worldbody>
<flexcomp type="grid" count="24 4 4" spacing=".1 .1 .1" pos=".1 0 1.5"
radius=".0" rgba="0 .7 .7 1" name="softbody" dim="3" mass="7">
<contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
<edge damping="1"/>
<plugin plugin="mujoco.elasticity.solid">
<config key="poisson" value="0.2"/>
<!--Units are in Pa (SI)-->
<config key="young" value="5e4"/>
</plugin>
<elasticity poisson="0.2" young="5e4">
</flexcomp>
</worldbody>
include/mujoco/mjmodel.h
+9
View File
@@ -29,6 +29,7 @@
#define mjMAXIMP 0.9999 // maximum constraint impedance
#define mjMAXCONPAIR 50 // maximum number of contacts per geom pair
#define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth
#define mjMAXFLEXNODES 27 // maximum number of flex nodes
//---------------------------------- sizes ---------------------------------------------------------
@@ -43,6 +44,7 @@
#define mjNSOLVER 200 // size of one mjData.solver array
#define mjNISLAND 20 // number of mjData.solver arrays
//---------------------------------- enum types (mjt) ----------------------------------------------
typedef enum mjtDisableBit_ { // disable default feature bitflags
@@ -607,6 +609,7 @@ struct mjModel_ {
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of flexes
int nflexnode; // number of dofs in all flexes
int nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
@@ -859,6 +862,9 @@ struct mjModel_ {
int* flex_dim; // 1: lines, 2: triangles, 3: tetrahedra (nflex x 1)
int* flex_matid; // material id for rendering (nflex x 1)
int* flex_group; // group for visibility (nflex x 1)
int* flex_interp; // interpolation (0: vertex, 1: nodes) (nflex x 1)
int* flex_nodeadr; // first node address (nflex x 1)
int* flex_nodenum; // number of nodes (nflex x 1)
int* flex_vertadr; // first vertex address (nflex x 1)
int* flex_vertnum; // number of vertices (nflex x 1)
int* flex_edgeadr; // first edge address (nflex x 1)
@@ -872,6 +878,7 @@ struct mjModel_ {
int* flex_evpairadr; // first evpair address (nflex x 1)
int* flex_evpairnum; // number of evpairs (nflex x 1)
int* flex_texcoordadr; // address in flex_texcoord; -1: none (nflex x 1)
int* flex_nodebodyid; // node body ids (nflexnode x 1)
int* flex_vertbodyid; // vertex body ids (nflexvert x 1)
int* flex_edge; // edge vertex ids (2 per edge) (nflexedge x 2)
int* flex_elem; // element vertex ids (dim+1 per elem) (nflexelemdata x 1)
@@ -881,6 +888,8 @@ struct mjModel_ {
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_vert0; // vertex positions in qpos0 on [0, 1]^d (nflexvert x 3)
mjtNum* flex_node; // node positions in local body frames (nflexnode x 3)
mjtNum* flex_node0; // Cartesian node positions in qpos0 (nflexnode x 3)
mjtNum* flexedge_length0; // edge lengths in qpos0 (nflexedge x 1)
mjtNum* flexedge_invweight0; // edge inv. weight in qpos0 (nflexedge x 1)
mjtNum* flex_radius; // radius around primitive element (nflex x 1)
include/mujoco/mjspec.h
+2
View File
@@ -439,7 +439,9 @@ typedef struct mjsFlex_ { // flex specification
double thickness; // thickness (2D only)
// mesh properties
mjStringVec* nodebody; // node body names
mjStringVec* vertbody; // vertex body names
mjDoubleVec* node; // node positions
mjDoubleVec* vert; // vertex positions
mjIntVec* elem; // element vertex ids
mjFloatVec* texcoord; // vertex texture coordinates
include/mujoco/mjvisualize.h
+6
View File
@@ -516,6 +516,10 @@ struct mjvSceneState_ {
int* flex_dim;
int* flex_matid;
int* flex_group;
int* flex_interp;
int* flex_nodeadr;
int* flex_nodenum;
int* flex_nodebodyid;
int* flex_vertadr;
int* flex_vertnum;
int* flex_elem;
@@ -529,6 +533,8 @@ struct mjvSceneState_ {
int* flex_texcoordadr;
int* flex_bvhadr;
int* flex_bvhnum;
mjtByte* flex_centered;
mjtNum* flex_node;
mjtNum* flex_radius;
float* flex_rgba;
include/mujoco/mjxmacro.h
+8 -1
View File
@@ -81,6 +81,7 @@
XMJV( ncam ) \
XMJV( nlight ) \
XMJV( nflex ) \
X ( nflexnode ) \
XMJV( nflexvert ) \
X ( nflexedge ) \
X ( nflexelem ) \
@@ -326,6 +327,9 @@
XMJV( int, flex_dim, nflex, 1 ) \
XMJV( int, flex_matid, nflex, 1 ) \
XMJV( int, flex_group, nflex, 1 ) \
XMJV( int, flex_interp, nflex, 1 ) \
XMJV( int, flex_nodeadr, nflex, 1 ) \
XMJV( int, flex_nodenum, nflex, 1 ) \
XMJV( int, flex_vertadr, nflex, 1 ) \
XMJV( int, flex_vertnum, nflex, 1 ) \
X ( int, flex_edgeadr, nflex, 1 ) \
@@ -339,6 +343,7 @@
X ( int, flex_evpairadr, nflex, 1 ) \
X ( int, flex_evpairnum, nflex, 1 ) \
XMJV( int, flex_texcoordadr, nflex, 1 ) \
XMJV( int, flex_nodebodyid, nflexnode, 1 ) \
X ( int, flex_vertbodyid, nflexvert, 1 ) \
X ( int, flex_edge, nflexedge, 2 ) \
XMJV( int, flex_elem, nflexelemdata, 1 ) \
@@ -348,6 +353,8 @@
X ( int, flex_evpair, nflexevpair, 2 ) \
X ( mjtNum, flex_vert, nflexvert, 3 ) \
X ( mjtNum, flex_vert0, nflexvert, 3 ) \
XMJV( mjtNum, flex_node, nflexnode, 3 ) \
X ( mjtNum, flex_node0, nflexnode, 3 ) \
X ( mjtNum, flexedge_length0, nflexedge, 1 ) \
X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \
XMJV( mjtNum, flex_radius, nflex, 1 ) \
@@ -358,7 +365,7 @@
X ( mjtByte, flex_edgeequality, nflex, 1 ) \
X ( mjtByte, flex_rigid, nflex, 1 ) \
X ( mjtByte, flexedge_rigid, nflexedge, 1 ) \
X ( mjtByte, flex_centered, nflex, 1 ) \
XMJV( mjtByte, flex_centered, nflex, 1 ) \
XMJV( mjtByte, flex_flatskin, nflex, 1 ) \
XMJV( int, flex_bvhadr, nflex, 1 ) \
XMJV( int, flex_bvhnum, nflex, 1 ) \
include/mujoco/mujoco.h
+2 -2
View File
@@ -1125,8 +1125,8 @@ MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale);
// Construct quaternion performing rotation from z-axis to given vector.
MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]);
// extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion
// returns the number of iterations required to converge
// Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion.
// Returns the number of iterations required to converge
MJAPI int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]);
// Convert sequence of Euler angles (radians) to quaternion.
introspect/functions.py
+1 -1
View File
@@ -7429,7 +7429,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
),
),
doc='extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion returns the number of iterations required to converge', # pylint: disable=line-too-long
doc='Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion. Returns the number of iterations required to converge', # pylint: disable=line-too-long
)),
('mju_euler2Quat',
FunctionDecl(
introspect/structs.py
+109
View File
@@ -898,6 +898,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='int'),
doc='number of flexes',
),
StructFieldDecl(
name='nflexnode',
type=ValueType(name='int'),
doc='number of dofs in all flexes',
),
StructFieldDecl(
name='nflexvert',
type=ValueType(name='int'),
@@ -2440,6 +2445,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='group for visibility',
array_extent=('nflex',),
),
StructFieldDecl(
name='flex_interp',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='interpolation (0: vertex, 1: nodes)',
array_extent=('nflex',),
),
StructFieldDecl(
name='flex_nodeadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='first node address',
array_extent=('nflex',),
),
StructFieldDecl(
name='flex_nodenum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='number of nodes',
array_extent=('nflex',),
),
StructFieldDecl(
name='flex_vertadr',
type=PointerType(
@@ -2544,6 +2573,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='address in flex_texcoord; -1: none',
array_extent=('nflex',),
),
StructFieldDecl(
name='flex_nodebodyid',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='node body ids',
array_extent=('nflexnode',),
),
StructFieldDecl(
name='flex_vertbodyid',
type=PointerType(
@@ -2615,6 +2652,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([
doc='vertex positions in qpos0 on [0, 1]^d',
array_extent=('nflexvert', 3),
),
StructFieldDecl(
name='flex_node',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='node positions in local body frames',
array_extent=('nflexnode', 3),
),
StructFieldDecl(
name='flex_node0',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='Cartesian node positions in qpos0',
array_extent=('nflexnode', 3),
),
StructFieldDecl(
name='flexedge_length0',
type=PointerType(
@@ -7447,6 +7500,34 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='',
),
StructFieldDecl(
name='flex_interp',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='',
),
StructFieldDecl(
name='flex_nodeadr',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='',
),
StructFieldDecl(
name='flex_nodenum',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='',
),
StructFieldDecl(
name='flex_nodebodyid',
type=PointerType(
inner_type=ValueType(name='int'),
),
doc='',
),
StructFieldDecl(
name='flex_vertadr',
type=PointerType(
@@ -7538,6 +7619,20 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='',
),
StructFieldDecl(
name='flex_centered',
type=PointerType(
inner_type=ValueType(name='mjtByte'),
),
doc='',
),
StructFieldDecl(
name='flex_node',
type=PointerType(
inner_type=ValueType(name='mjtNum'),
),
doc='',
),
StructFieldDecl(
name='flex_radius',
type=PointerType(
@@ -10209,6 +10304,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
type=ValueType(name='double'),
doc='thickness (2D only)',
),
StructFieldDecl(
name='nodebody',
type=PointerType(
inner_type=ValueType(name='mjStringVec'),
),
doc='node body names',
),
StructFieldDecl(
name='vertbody',
type=PointerType(
@@ -10216,6 +10318,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([
),
doc='vertex body names',
),
StructFieldDecl(
name='node',
type=PointerType(
inner_type=ValueType(name='mjDoubleVec'),
),
doc='node positions',
),
StructFieldDecl(
name='vert',
type=PointerType(
model/composite/scene.xml
-2
View File
@@ -30,12 +30,10 @@
<texture name="texplane" type="2d" builtin="checker" rgb1=".2 .3 .4" rgb2=".1 0.15 0.2"
width="512" height="512" mark="cross" markrgb=".8 .8 .8"/>
<texture name="texcarpet" type="2d" file="carpet.png"/>
<texture name="texsponge" type="2d" file="sponge.png"/>
<texture name="texmarble" type="cube" file="marble.png"/>
<material name="matplane" reflectance="0.3" texture="texplane" texrepeat="1 1" texuniform="true"/>
<material name="matcarpet" texture="texcarpet"/>
<material name="matsponge" texture="texsponge" specular="0.3"/>
<material name="matmarble" texture="texmarble" rgba=".7 .7 .7 1"/>
</asset>
model/flex/asset/bunny.obj
+7474
View File
File diff suppressed because it is too large Load Diff
model/composite/asset/sponge.png → model/flex/asset/sponge.png
View File
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 634 KiB

After

Width:  |  Height:  |  Size: 634 KiB

model/flex/bunny.xml
+42
View File
@@ -0,0 +1,42 @@
<!-- 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="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<size memory="100M"/>
<visual>
<map stiffness="100"/>
</visual>
<worldbody>
<body>
<joint name="press" type="slide" axis="0 0 1" damping="500"/>
<geom type="box" size=".02 .2 .2" pos="0 0 .5"/>
</body>
<flexcomp type="mesh" file="bunny.obj" pos="0 0 .1" dim="2" euler="90 0 0"
radius=".001" rgba="0 .7 .7 1" mass=".05" name="softbody" dof="trilinear">
<elasticity young="1e3" poisson="0.1" damping="0.001"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
<actuator>
<position name="press" joint="press" gear="-1 0 0 0 0 0" ctrlrange="-1 1" kp="1000"/>
</actuator>
</mujoco>
model/flex/gripper.xml
+7 -2
View File
@@ -27,7 +27,7 @@
<geom type="box" size=".02 .1 .15" pos=".18 0 -.2" rgba=".2 .2 .2 1"/>
<flexcomp type="mesh" file="cap.obj" pos=".16 0 -.25" dim="3" euler="0 -90 0"
origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="left">
<edge equality="true"/>
<edge equality="true" solimp="0.95 0.99 0.001 0.5 2"/>
<contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
<pin id="0 2 3 4 5 6 7 9 11 13 15 17 19 41 45 46 50 53 57 58 61 176 177 179 180 184 185
187 188 192 193 195 196 200 201 203 204 208 209 211 212 214 215 216 217"/>
@@ -38,13 +38,18 @@
<geom type="box" size=".02 .1 .15" pos="-.18 0 -.2" rgba=".2 .2 .2 1"/>
<flexcomp type="mesh" file="cap.obj" pos="-.16 0 -.25" dim="3" euler="0 90 0"
origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="right">
<edge equality="true"/>
<edge equality="true" solimp="0.95 0.99 0.001 0.5 2"/>
<contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
<pin id="0 2 3 4 5 6 7 9 11 13 15 17 19 41 45 46 50 53 57 58 61 176 177 179 180 184 185
187 188 192 193 195 196 200 201 203 204 208 209 211 212 214 215 216 217"/>
</flexcomp>
</body>
</body>
<body>
<freejoint/>
<geom type="box" size=".05 .1 .1" pos="0 0 .1" rgba=".5 .5 0 1" priority="1"
contype="2" condim="6"/>
</body>
</worldbody>
<equality>
model/flex/gripper_trilinear.xml
+71
View File
@@ -0,0 +1,71 @@
<!-- 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="Gripper">
<include file="scene.xml"/>
<option cone="elliptic" impratio="10" integrator="implicitfast"/>
<worldbody>
<body name="hand" pos="0 0 .37">
<joint name="lift" type="slide" range="0 1"/>
<geom type="box" size=".2 .1 .05" rgba=".2 .2 .2 1"/>
<body name="right_gripper">
<joint name="right" type="slide" axis="-1 0 0"/>
<geom type="box" size=".02 .1 .15" pos=".18 0 -.2" rgba=".2 .2 .2 1"/>
<flexcomp type="mesh" file="cap.obj" pos=".16 0 -.25" dim="3" euler="0 -90 0"
origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="left"
dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping=".05"/>
<contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
<pin id="4 5 6 7"/>
</flexcomp>
</body>
<body name="left_gripper">
<joint name="left" type="slide" axis="1 0 0"/>
<geom type="box" size=".02 .1 .15" pos="-.18 0 -.2" rgba=".2 .2 .2 1"/>
<flexcomp type="mesh" file="cap.obj" pos="-.16 0 -.25" dim="3" euler="0 90 0"
origin="0 0 0" radius=".001" rgba="0 .7 .7 1" mass=".5" name="right"
dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping=".05"/>
<contact selfcollide="none" internal="false" contype="2" conaffinity="2"/>
<pin id="0 1 2 3"/>
</flexcomp>
</body>
</body>
<body>
<freejoint/>
<geom type="box" size=".05 .1 .1" pos="0 0 .1" rgba=".5 .5 0 1" priority="1"
contype="2" condim="6"/>
</body>
</worldbody>
<equality>
<joint joint1="right" joint2="left"/>
</equality>
<tendon>
<fixed name="grasp">
<joint joint="right" coef="1"/>
<joint joint="left" coef="1"/>
</fixed>
</tendon>
<actuator>
<position name="lift" joint="lift" kp="600" dampratio="1" ctrlrange="0 1"/>
<position name="grasp" tendon="grasp" kp="200" dampratio="1" ctrlrange="0 1"/>
</actuator>
</mujoco>
model/composite/softbox.xml → model/flex/softbox.xml
+6 -1
View File
@@ -20,12 +20,17 @@
<size memory="30M"/>
<asset>
<texture name="texsponge" type="2d" file="sponge.png"/>
<material name="matsponge" texture="texsponge" specular="0.3"/>
</asset>
<worldbody>
<body pos="0 0 1" name="body">
<freejoint/>
<geom size=".1" contype="0" conaffinity="0" group="4"/>
<flexcomp name="softbox" type="box" count="7 7 7" spacing=".04 .04 .04"
radius="0.01" dim="3" material="matsponge">
radius="0.01" dim="3" material="matsponge" dof="radial">
<contact internal="false" selfcollide="none"/>
<edge equality="true"/>
</flexcomp>
model/flex/sphere_full.xml
+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"/>
</flexcomp>
</worldbody>
</mujoco>
model/flex/sphere_radial.xml
+41
View File
@@ -0,0 +1,41 @@
<!-- 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="Radial 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"/>
<body name="body" pos="-.5 0 1">
<freejoint/>
<geom size=".1" contype="0" conaffinity="0" group="4"/>
<flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="radial" dof="radial">
<edge equality="true" solimp="0 0.9 0.01" solref=".02 1"/>
<!-- <elasticity young="6e2" poisson="0.3" damping="0.05"/> -->
<contact selfcollide="none" internal="false"/>
</flexcomp>
</body>
</worldbody>
</mujoco>
model/flex/sphere_trilinear.xml
+45
View File
@@ -0,0 +1,45 @@
<!-- 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="Trilinear flex spheres">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep="1e-3" integrator="implicitfast"/>
<size memory="10M"/>
<visual>
<map stiffness="100"/>
<!-- The trilinear bounding box is visualized by turning on "Flex Tree" (mjVIS_FLEXBVH).
In order to make the visualization clearer,
we make the regular collision BVH visualization invisible: -->
<rgba bv="0 0 0 0" bvactive="0 0 0 0"/>
</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 -.5 1" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="soft" dof="trilinear">
<elasticity young="1e3" poisson="0.49" damping="0.1"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
<flexcomp type="ellipsoid" count="8 8 8" spacing=".07 .07 .07" pos="-.5 .5 1" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="rubber" dof="trilinear">
<elasticity young="1e4" poisson="0.4" damping="3e-3"/>
<contact selfcollide="none" internal="false" solref="-5000 -10"/>
</flexcomp>
</worldbody>
</mujoco>
model/flex/trilinear.xml
+38
View File
@@ -0,0 +1,38 @@
<!-- 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="Trilinear">
<include file="scene.xml"/>
<option solver="CG" tolerance="1e-6" timestep=".001" integrator="implicitfast"/>
<size memory="10M"/>
<visual>
<map stiffness="100"/>
<rgba bv="0 0 0 0" bvactive="0 0 0 0"/>
</visual>
<worldbody>
<body mocap="true" pos="-.1 .05 0" zaxis=".5 0 1">
<geom type="capsule" size=".1 .1" group="1" condim="1"/>
</body>
<flexcomp type="grid" count="8 8 8" spacing=".07 .07 .07" pos="0 0 1" dim="3"
radius=".001" rgba="0 .7 .7 1" mass="5" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.01"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
src/engine/engine_collision_driver.c
+2 -2
View File
@@ -1859,7 +1859,7 @@ void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f];
for (int i=0; i <= dim; i++) {
if (b == bdata[edata[i]]) {
if (b >= 0 && b == bdata[edata[i]]) {
return;
}
}
@@ -1983,7 +1983,7 @@ void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2
for (int i1=0; i1 <= dim1; i1++) {
int b1 = bdata1[edata1[i1]];
for (int i2=0; i2 <= dim2; i2++) {
if (b1 == bdata2[edata2[i2]]) {
if (b1 >= 0 && b1 == bdata2[edata2[i2]]) {
return;
}
}
src/engine/engine_core_constraint.c
+86 -22
View File
@@ -181,7 +181,7 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
for (int i=0; i <= dim; i++) {
mjtNum dist = mju_dist3(point, vert+3*edata[i]);
weight[i] = 1.0/(mju_max(mjMINVAL, dist));
body[i] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[i]];
body[i] = m->flex_vertadr[f] + edata[i];
// check if element vertex matches v
if (edata[i] == v) {
@@ -206,6 +206,30 @@ static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, in
// compute body weights for a given contact vertex, return #bodies
static int mj_vertBodyWeight(const mjModel* m, const mjData* d, int f, int v,
const mjtNum point[3], int* body, mjtNum* weight, mjtNum bw) {
mjtNum* coord = m->flex_vert0 + 3*v;
int nstart = m->flex_nodeadr[f];
int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
int nb = 0;
for (int i = nstart; i < nend; i++) {
mjtNum w = ((i-nstart)&1 ? coord[2] : 1-coord[2]) *
((i-nstart)&2 ? coord[1] : 1-coord[1]) *
((i-nstart)&4 ? coord[0] : 1-coord[0]);
if (w < 1e-5) {
continue;
}
if (weight) weight[nb] = w * bw;
body[nb++] = m->flex_nodebodyid[i];
}
return nb;
}
// add contact to d->contact list; return 0 if success; 1 if buffer full
int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
// if nconmax is specified and ncon >= nconmax, warn and return error
@@ -971,8 +995,8 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
con->efc_address = d->nefc;
// special case: single body on each side
if ((con->geom[0] >= 0 || con->vert[0] >= 0) &&
(con->geom[1] >= 0 || con->vert[1] >= 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))) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
@@ -995,9 +1019,13 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
else {
// get bodies and weights
int nb = 0;
int bid[8];
mjtNum bweight[8];
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]];
@@ -1007,22 +1035,32 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// flex vert
else if (con->vert[side] >= 0) {
bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[nb] = side ? +1 : -1;
nb++;
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = side ? +1 : -1;
nw = 1;
}
// flex elem
else {
int nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid+nb, bweight+nb);
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(bweight+nb, bweight+nb, -1, nw);
mju_scl(bw, bw, -1, nw);
}
}
nb += 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]);
}
}
}
@@ -1201,8 +1239,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
tran = rot = 0;
for (int side=0; side < 2; side++) {
// get bodies and weights
int nb, bid[4];
mjtNum bweight[4];
int nb = 0, bid[32], vid[4], nw = 0;
mjtNum bweight[32], bw[4];
// geom
if (con->geom[side] >= 0) {
@@ -1213,15 +1251,27 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// flex vert
else if (con->vert[side] >= 0) {
bid[0] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[0] = 1;
nb = 1;
vid[0] = m->flex_vertadr[con->flex[side]] + con->vert[side];
bw[0] = 1;
nw = 1;
}
// flex elem
else {
nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid, bweight);
nw = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, vid, bw);
}
// get body or node ids and weights
for (int k=0; k < nw; k++) {
if (m->flex_interp[con->flex[side]] == 0) {
bid[k] = m->flex_vertbodyid[vid[k]];
bweight[k] = bw[k];
nb++;
} else {
nb = mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid, bweight, bw[k]);
}
}
// add weighted average over bodies
@@ -1881,8 +1931,11 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[8];
int nb = 0, bid[64];
for (int side=0; side < 2; side++) {
int nw = 0;
int vid[4];
// geom
if (con->geom[side] >= 0) {
bid[nb++] = m->geom_bodyid[con->geom[side]];
@@ -1890,7 +1943,7 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
// flex vert
else if (con->vert[side] >= 0) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
vid[nw++] = m->flex_vertadr[con->flex[side]] + con->vert[side];
}
// flex elem
@@ -1899,7 +1952,18 @@ static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int fdim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + con->elem[side]*(fdim+1);
for (int k=0; k <= fdim; k++) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[f] + edata[k]];
vid[nw++] = m->flex_vertadr[f] + edata[k];
}
}
// 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]];
nb++;
} else {
nb += mj_vertBodyWeight(m, d, con->flex[side], vid[k],
con->pos, bid+nb, NULL, 0);
}
}
}
src/engine/engine_core_smooth.c
+40 -8
View File
@@ -453,19 +453,51 @@ void mj_flex(const mjModel* m, mjData* d) {
for (int f=0; f < m->nflex; f++) {
int vstart = m->flex_vertadr[f];
int vend = m->flex_vertadr[f] + m->flex_vertnum[f];
int nstart = m->flex_nodeadr[f];
int nend = m->flex_nodeadr[f] + m->flex_nodenum[f];
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
// 0: vertices are the mesh vertices, 1: vertices are interpolated from nodal dofs
if (m->flex_interp[f] == 0) {
// centered: copy body position
if (m->flex_centered[f]) {
for (int i=vstart; i < vend; i++) {
mju_copy3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i < vend; i++) {
mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
}
// non-centered: map from local to global
// trilinear interpolation
else {
mjtNum nodexpos[mjMAXFLEXNODES];
if (m->flex_centered[f]) {
for (int i=nstart; i < nend; i++) {
mju_copy3(nodexpos + 3*(i-nstart), d->xpos + 3*m->flex_nodebodyid[i]);
}
} else {
for (int i=nstart; i < nend; i++) {
int j = i - nstart;
mju_mulMatVec3(nodexpos + 3*j, d->xmat + 9*m->flex_nodebodyid[i], m->flex_node + 3*i);
mju_addTo3(nodexpos + 3*j, d->xpos + 3*m->flex_nodebodyid[i]);
}
}
for (int i=vstart; i < vend; i++) {
mju_mulMatVec3(d->flexvert_xpos+3*i, d->xmat+9*m->flex_vertbodyid[i], m->flex_vert+3*i);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
mju_zero3(d->flexvert_xpos+3*i);
mjtNum* coord = m->flex_vert0 + 3*i;
for (int j=0; j < nend-nstart; j++) {
mjtNum coef = (j&1 ? coord[2] : 1-coord[2]) *
(j&2 ? coord[1] : 1-coord[1]) *
(j&4 ? coord[0] : 1-coord[0]);
mju_addToScl3(d->flexvert_xpos+3*i, nodexpos+3*j, coef);
}
}
}
}
@@ -542,7 +574,7 @@ void mj_flex(const mjModel* m, mjData* d) {
// compute lengths and Jacobians of edges
for (int f=0; f < m->nflex; f++) {
// skip if edges cannot generate forces
if (m->flex_rigid[f]) {
if (m->flex_rigid[f] || m->flex_interp[f]) {
continue;
}
src/engine/engine_io.c
+4 -3
View File
@@ -460,7 +460,7 @@ static void freeModelBuffers(mjModel* m) {
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh,
int nbvhstatic, int nbvhdynamic, int njnt, int ngeom, int nsite, int ncam,
int nlight, int nflex, int nflexvert, int nflexedge, int nflexelem,
int nlight, int nflex, int nflexnode, int nflexvert, int nflexedge, int nflexelem,
int nflexelemdata, int nflexelemedge, int nflexshelldata, int nflexevpair, int nflextexcoord,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
@@ -503,6 +503,7 @@ void mj_makeModel(mjModel** dest,
m->ncam = ncam;
m->nlight = nlight;
m->nflex = nflex;
m->nflexnode = nflexnode;
m->nflexvert = nflexvert;
m->nflexedge = nflexedge;
m->nflexelem = nflexelem;
@@ -634,7 +635,7 @@ mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
mj_makeModel(&dest,
src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh,
src->nbvhstatic, src->nbvhdynamic, src->njnt, src->ngeom, src->nsite,
src->ncam, src->nlight, src->nflex, src->nflexvert, src->nflexedge,
src->ncam, src->nlight, src->nflex, src->nflexnode, src->nflexvert, src->nflexedge,
src->nflexelem, src->nflexelemdata, src->nflexelemedge, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
@@ -828,7 +829,7 @@ mjModel* mj_loadModelBuffer(const void* buffer, int buffer_sz) {
ints[42], ints[43], ints[44], ints[45], ints[46], ints[47], ints[48],
ints[49], ints[50], ints[51], ints[52], ints[53], ints[54], ints[55],
ints[56], ints[57], ints[58], ints[59], ints[60], ints[61], ints[62],
ints[63], ints[64]);
ints[63], ints[64], ints[65]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_warning("Corrupted model, wrong size parameters");
mj_deleteModel(m);
src/engine/engine_io.h
+1 -1
View File
@@ -53,7 +53,7 @@ void mj_defaultStatistic(mjStatistic* stat);
// allocate mjModel
void mj_makeModel(mjModel** dest,
int nq, int nv, int nu, int na, int nbody, int nbvh, int nbvhstatic, int nbvhdynamic,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexvert,
int njnt, int ngeom, int nsite, int ncam, int nlight, int nflex, int nflexnode, int nflexvert,
int nflexedge, int nflexelem, int nflexelemdata, int nflexelemedge, int nflexshelldata,
int nflexevpair, int nflextexcoord, int nmesh, int nmeshvert, int nmeshnormal,
int nmeshtexcoord, int nmeshface, int nmeshgraph, int nskin, int nskinvert, int nskintexvert,
src/engine/engine_passive.c
+82
View File
@@ -122,6 +122,88 @@ static void mj_springdamper(const mjModel* m, mjData* d) {
continue;
}
if (m->flex_interp[f]) {
mjtNum xpos[mjMAXFLEXNODES], displ[mjMAXFLEXNODES], vel[mjMAXFLEXNODES];
mjtNum frc[mjMAXFLEXNODES], dmp[mjMAXFLEXNODES];
mjtNum com[3] = {0};
mjtNum* xpos0 = m->flex_node0 + 3*m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
int nstart = m->flex_nodeadr[f];
// compute positions
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mju_copy3(vel + 3*i, d->qvel + m->body_dofadr[bodyid[i]]);
}
} else {
mjtNum screw[6];
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
mj_objectVelocity(m, d, mjOBJ_BODY, bodyid[i], screw, 0);
mju_copy3(vel + 3*i, screw + 3);
}
}
// compute center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addToScl3(com, xpos+3*i, 1.0/m->flex_nodenum[f]);
}
// re-center positions using center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addToScl3(xpos+3*i, com, -1);
}
// compute the Jacobian at the center of mass
mjtNum mat[9] = {0};
mjtNum p[3] = {.5, .5, .5};
mju_defGradient(mat, p, xpos, 1);
// find rotation
mjtNum quat[4] = {1, 0, 0, 0};
mju_mat2Rot(quat, mat);
mju_negQuat(quat, quat);
// rotate vertices to quat and add reference center of mass
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_rotVecQuat(xpos+3*i, xpos+3*i, quat);
mju_addTo3(xpos+3*i, p);
mju_rotVecQuat(vel+3*i, vel+3*i, quat);
}
// compute displacement
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mju_addScl3(displ+3*i, xpos+3*i, xpos0+3*i, -1);
}
// compute force in the stretch frame
mju_mulMatVec(frc, k, displ, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
// compute damping force in stretch frame
mju_mulMatVec(dmp, k, vel, 3*m->flex_nodenum[f], 3*m->flex_nodenum[f]);
// rotate forces to global frame and add to qfrc
mju_negQuat(quat, quat);
for (int i = 0; i < m->flex_nodenum[f]; i++) {
mjtNum qfrc[3], qdmp[3];
mju_rotVecQuat(qfrc, frc+3*i, quat);
mju_rotVecQuat(qdmp, dmp+3*i, quat);
mju_scl3(qdmp, qdmp, m->flex_damping[f]);
if (m->flex_centered[f]) {
mju_addTo3(d->qfrc_spring+m->body_dofadr[bodyid[i]], qfrc);
mju_addTo3(d->qfrc_damper+m->body_dofadr[bodyid[i]], qdmp);
} else {
mj_applyFT(m, d, qfrc, 0, xpos+3*i, bodyid[i], d->qfrc_spring);
mj_applyFT(m, d, qdmp, 0, xpos+3*i, bodyid[i], d->qfrc_damper);
}
}
// do not continue with the rest of the flex passive forces
continue;
}
int nedge = (dim == 2) ? 3 : 6;
int nvert = (dim == 2) ? 3 : 4;
const int* elem = m->flex_elem + m->flex_elemdataadr[f];
src/engine/engine_setconst.c
+13
View File
@@ -213,6 +213,10 @@ static void set0(mjModel* m, mjData* d) {
if (nv) {
// compute flexedge_invweight0
for (int f=0; f < m->nflex; f++) {
if (m->flex_interp[f]) {
continue;
}
for (int i=m->flex_edgeadr[f]; i < m->flex_edgeadr[f]+m->flex_edgenum[f]; i++) {
// bodies connected by edge
int b1 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i]];
@@ -498,13 +502,22 @@ static void setStat(mjModel* m, mjData* d) {
// adjust body size for flex edges involving body
for (int f=0; f < m->nflex; f++) {
mjtNum meanedge = 0;
for (int e=m->flex_edgeadr[f]; e < m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
meanedge += m->flexedge_length0[e] / m->flex_edgenum[f];
if (m->flex_interp[f]) {
continue;
}
int b1 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f]+m->flex_edge[2*e+1]];
body[b1] = mju_max(body[b1], m->flexedge_length0[e]);
body[b2] = mju_max(body[b2], m->flexedge_length0[e]);
}
for (int v=m->flex_nodeadr[f]; v < m->flex_nodeadr[f]+m->flex_node[f]; v++) {
body[m->flex_nodebodyid[v]] = mju_max(body[m->flex_nodebodyid[v]], meanedge);
}
}
// compute meansize, make sure all sizes are above min
src/engine/engine_util_misc.c
+45
View File
@@ -452,6 +452,51 @@ void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]) {
// ----------------------------- Flex interpolation ------------------------------------------------
mjtNum static inline phi(mjtNum s, int i) {
if (i == 0) {
return 1-s;
} else {
return s;
}
}
mjtNum static inline dphi(mjtNum s, int i) {
if (i == 0) {
return -1;
} else {
return 1;
}
}
// evaluate the deformation gradient at p using the nodal dof values
void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order) {
mjtNum gradient[3];
mju_zero(res, 9);
for (int i = 0; i <= order; i++) {
for (int j = 0; j <= order; j++) {
for (int k = 0; k <= order; k++) {
int idx = 4*i + 2*j + k;
gradient[0] = dphi(p[0], i) * phi(p[1], j) * phi(p[2], k);
gradient[1] = phi(p[0], i) * dphi(p[1], j) * phi(p[2], k);
gradient[2] = phi(p[0], i) * phi(p[1], j) * dphi(p[2], k);
res[0] += dof[3*idx+0] * gradient[0];
res[1] += dof[3*idx+0] * gradient[1];
res[2] += dof[3*idx+0] * gradient[2];
res[3] += dof[3*idx+1] * gradient[0];
res[4] += dof[3*idx+1] * gradient[1];
res[5] += dof[3*idx+1] * gradient[2];
res[6] += dof[3*idx+2] * gradient[0];
res[7] += dof[3*idx+2] * gradient[1];
res[8] += dof[3*idx+2] * gradient[2];
}
}
}
}
//------------------------------ actuator models ---------------------------------------------------
// normalized muscle length-gain curve
src/engine/engine_util_misc.h
+5
View File
@@ -53,6 +53,11 @@ MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]);
// all 3 semi-axes of a geom
MJAPI void mju_geomSemiAxes(const mjModel* m, int geom_id, mjtNum semiaxes[3]);
// ----------------------------- Flex interpolation ------------------------------------------------
// evaluate the deformation gradient at p using the nodal dof values
MJAPI void mju_defGradient(mjtNum res[9], const mjtNum p[3], const mjtNum* dof, int order);
// ----------------------------- Base64 -----------------------------------------------------------
// encode data as Base64 into buf (including padding and null char)
src/engine/engine_vis_interact.c
+17 -2
View File
@@ -828,9 +828,24 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if closer intersection found
if (newdist >= 0 && (newdist < flexdist || flexdist < 0)) {
flexdist = newdist;
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
if (m->flex_interp[i]) {
mjtNum* vert0 = m->flex_vert0 + 3*(m->flex_vertadr[i] + vertid);
int l = vert0[0] > 0.5 ? 1 : 0;
int j = vert0[1] > 0.5 ? 1 : 0;
int k = vert0[2] > 0.5 ? 1 : 0;
int nodeid = 4*l+2*j+k;
flexbodyid = m->flex_nodebodyid[m->flex_nodeadr[i] + nodeid];
if (m->flex_centered[i]) {
mju_copy3(flexpnt, d->xpos + 3*flexbodyid);
} else {
mju_mulMatVec3(flexpnt, d->xmat + 9*flexbodyid, m->flex_node + 3*nodeid);
mju_addTo3(flexpnt, d->xpos + 3*flexbodyid);
}
} else {
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
*flexid = i;
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
}
}
src/engine/engine_vis_visualize.c
+41
View File
@@ -713,6 +713,47 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
FINISH
}
}
if (!m->flex_interp[f]) {
continue;
}
// control points box
mjtNum xpos[mjMAXFLEXNODES];
int nstart = m->flex_nodeadr[f];
int* bodyid = m->flex_nodebodyid + m->flex_nodeadr[f];
if (m->flex_centered[f]) {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_copy3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
} else {
for (int i=0; i < m->flex_nodenum[f]; i++) {
mju_mulMatVec3(xpos + 3*i, d->xmat + 9*bodyid[i], m->flex_node + 3*(i+nstart));
mju_addTo3(xpos + 3*i, d->xpos + 3*bodyid[i]);
}
}
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (scn->ngeom >= scn->maxgeom) break;
if (i == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*(i+1)+2*j+k));
FINISH
}
if (j == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*(j+1)+k));
FINISH
}
if (k == 0) {
START
mjv_connector(thisgeom, mjGEOM_LINE, 3, xpos+3*(4*i+2*j+k), xpos+3*(4*i+2*j+(k+1)));
FINISH
}
}
}
}
}
}
src/user/user_flexcomp.cc
+65 -12
View File
@@ -86,6 +86,7 @@ mjCFlexcomp::mjCFlexcomp(void) {
mjuu_setvec(quat, 1, 0, 0, 0);
rigid = false;
centered = false;
doftype = mjFCOMPDOF_FULL;
mjs_defaultPlugin(&plugin);
mjs_defaultOrientation(&alt);
@@ -102,10 +103,6 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
mjCModel* model = static_cast<mjCBody*>(body->element)->model;
mjsCompiler* compiler = static_cast<mjCBody*>(body->element)->compiler;
mjsFlex* dflex = def.spec.flex;
bool radial = (type == mjFCOMPTYPE_BOX ||
type == mjFCOMPTYPE_CYLINDER ||
type == mjFCOMPTYPE_ELLIPSOID);
bool direct = (type == mjFCOMPTYPE_DIRECT ||
type == mjFCOMPTYPE_MESH ||
type == mjFCOMPTYPE_GMSH);
@@ -122,7 +119,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
// check counts
for (int i=0; i < 3; i++) {
if (count[i] < 1 || ((radial && count[i] < 2) && dflex->dim == 3)) {
if (count[i] < 1 || ((doftype == mjFCOMPDOF_RADIAL && count[i] < 2) && dflex->dim == 3)) {
return comperr(error, "Count too small", error_sz);
}
}
@@ -260,6 +257,15 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
point[3*i+2] = newp[2];
}
// compute bounding box of points
double minmax[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
for (int i=0; i < npnt; i++) {
for (int j=0; j < 3; j++) {
minmax[j+0] = std::min(minmax[j+0], point[3*i+j]);
minmax[j+3] = std::max(minmax[j+3], point[3*i+j]);
}
}
// construct pinned array
pinned = vector<bool>(npnt, rigid);
@@ -337,7 +343,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// center of radial body is always pinned
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
pinned[0] = true;
}
@@ -434,8 +440,8 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
continue;
}
// pinned: parent body
if (pinned[i]) {
// pinned or trilinear: parent body
if (pinned[i] || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_appendString(pf->vertbody, mjs_getString(body->name));
// add plugin
@@ -448,7 +454,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
// not pinned: new body
// not pinned and not trilinear: new body
else {
// add new body at vertex coordinates
mjsBody* pb = mjs_addBody(body, 0);
@@ -465,7 +471,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
pb->explicitinertial = true;
// add radial slider
if (radial) {
if (doftype == mjFCOMPDOF_RADIAL) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
// set properties
@@ -476,7 +482,7 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
// add three orthogonal sliders
else {
else if (doftype == mjFCOMPDOF_FULL) {
for (int j=0; j < 3; j++) {
// add joint to body
mjsJoint* jnt = mjs_addJoint(pb, 0);
@@ -513,7 +519,54 @@ bool mjCFlexcomp::Make(mjsBody* body, char* error, int error_sz) {
}
}
if (!centered) {
// create nodal mesh for trilinear interpolation
if (doftype == mjFCOMPDOF_TRILINEAR) {
std::vector<double> node(24, 0);
for (int i=0; i < 2; i++) {
for (int j=0; j < 2; j++) {
for (int k=0; k < 2; k++) {
if (pinned[i*4+j*2+k]) {
node[3*(i*4+j*2+k)+0] = i == 0 ? minmax[0] : minmax[3];
node[3*(i*4+j*2+k)+1] = j == 0 ? minmax[1] : minmax[4];
node[3*(i*4+j*2+k)+2] = k == 0 ? minmax[2] : minmax[5];
mjs_appendString(pf->nodebody, mjs_getString(body->name));
continue;
}
mjsBody* pb = mjs_addBody(body, 0);
pb->pos[0] = i == 0 ? minmax[0] : minmax[3];
pb->pos[1] = j == 0 ? minmax[1] : minmax[4];
pb->pos[2] = k == 0 ? minmax[2] : minmax[5];
mjuu_zerovec(pb->ipos, 3);
pb->mass = mass / 8;
pb->inertia[0] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[1] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->inertia[2] = pb->mass*(2.0*inertiabox*inertiabox)/3.0;
pb->explicitinertial = true;
for (int d=0; d < 3; d++) {
mjsJoint* jnt = mjs_addJoint(pb, 0);
jnt->type = mjJNT_SLIDE;
mjuu_setvec(jnt->pos, 0, 0, 0);
mjuu_setvec(jnt->axis, 0, 0, 0);
jnt->axis[d] = 1;
}
// construct node name, add to nodebody
char txt[100];
mju::sprintf_arr(txt, "%s_%d_%d_%d", name.c_str(), i, j, k);
mjs_setString(pb->name, txt);
mjs_appendString(pf->nodebody, mjs_getString(pb->name));
}
}
}
if (!centered) {
mjs_setDouble(pf->node, node.data(), node.size());
}
}
if (!centered || doftype == mjFCOMPDOF_TRILINEAR) {
mjs_setDouble(pf->vert, point.data(), point.size());
}
src/user/user_flexcomp.h
+10
View File
@@ -40,6 +40,15 @@ typedef enum _mjtFcompType {
} mjtFcompType;
typedef enum _mjtDof {
mjFCOMPDOF_FULL = 0,
mjFCOMPDOF_RADIAL,
mjFCOMPDOF_TRILINEAR,
mjNFCOMPDOFS
} mjtDof;
class mjCFlexcomp {
public:
mjCFlexcomp(void);
@@ -73,6 +82,7 @@ class mjCFlexcomp {
double inertiabox; // size of inertia box for each body
bool equality; // create edge equality constraint
std::string file; // mesh/gmsh file name
mjtDof doftype; // dof type, all vertices or trilinear interpolation
// pin specifications
std::vector<int> pinid; // ids of points to pin
src/user/user_mesh.cc
+237 -6
View File
@@ -13,6 +13,7 @@
// limitations under the License.
#include <algorithm>
#include <array>
#include <climits>
#include <cmath>
#include <csetjmp>
@@ -2402,7 +2403,7 @@ void mjCSkin::LoadSKN(mjResource* resource) {
//--------------------- elasticity implementation --------------------------------------------------
//-------------------------- nonlinear elasticity --------------------------------------------------
// hash function for std::pair
struct PairHash
@@ -2628,6 +2629,153 @@ void inline ComputeStiffness(std::vector<double>& stiffness,
MetricTensor<T>(stiffness.data(), t, mu, la, basis);
}
//----------------------------- linear elasticity --------------------------------------------------
// Gauss Legendre quadrature points in 1 dimension on the interval [a, b]
void quadratureGaussLegendre(double* points, double* weights,
const int order, const double a, const double b) {
if (order > 2)
mju_error("Integration order > 2 not yet supported.");
// x is on [-1, 1], p on [a, b]
double p0 = (a+b)/2.;
double dpdx = (b-a)/2;
points[0] = -dpdx/sqrt(3) + p0;
points[1] = dpdx/sqrt(3) + p0;
weights[0] = dpdx;
weights[1] = dpdx;
}
// evaluate 1-dimensional basis function
double phi(const double s, const double component) {
if (component == 0) {
return 1-s;
} else {
return s;
}
}
// evaluate gradient fo 1-dimensional basis function
double dphi(const double s, const double component) {
if (component == 0) {
return -1;
} else {
return 1;
}
}
typedef std::array<std::array<double, 3>, 3> Matrix;
// symmetrize a tensor
Matrix inline sym(const Matrix& tensor) {
Matrix eps;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
eps[i][j] = (tensor[i][j] + tensor[j][i]) / 2;
}
}
return eps;
}
// compute tensor inner product
Matrix inline inner(const Matrix& tensor1, const Matrix& tensor2) {
Matrix inner;
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
inner[i][j] = tensor1[i][0] * tensor2[0][j] +
tensor1[i][1] * tensor2[1][j] +
tensor1[i][2] * tensor2[2][j];
}
}
return inner;
}
// compute trace of a tensor
double inline trace(const Matrix& tensor) {
return tensor[0][0] + tensor[1][1] + tensor[2][2];
}
void inline ComputeLinearStiffness(std::vector<double>& K,
const double* pos,
double E, double nu) {
// only linear elements are supported for now
int order = 2;
int n = std::pow(order, 3);
int ndof = 3*n;
// compute quadrature points
std::vector<double> points(order); // quadrature points
std::vector<double> weight(order); // quadrature weights
quadratureGaussLegendre(points.data(), weight.data(), order, 0, 1);
// compute element transformation
double dx = (pos+12)[0] - pos[0];
double dy = (pos+ 6)[1] - pos[1];
double dz = (pos+ 3)[2] - pos[2];
double detJ = dx * dy * dz;
double invJ[3] = {1.0 / dx, 1.0 / dy, 1.0 / dz};
// compute stiffness matrix
std::vector<std::array<double, 3>> F(n);
double la = E * nu / (1 + nu) / (1 - 2 * nu);
double mu = E / (2 * (1 + nu));
// loop over quadrature points
for (int ps=0; ps < order; ps++) {
for (int pt=0; pt < order; pt++) {
for (int pu=0; pu < order; pu++) {
double s = points[ps];
double t = points[pt];
double u = points[pu];
double dvol = weight[ps] * weight[pt] * weight[pu] * detJ;
int dof = 0;
// cartesian product of basis functions
for (int bx=0; bx < order; bx++) {
for (int by=0; by < order; by++) {
for (int bz=0; bz < order; bz++) {
std::array<double, 3> gradient;
gradient[0] = dphi(s, bx) * phi(t, by) * phi(u, bz);
gradient[1] = phi(s, bx) * dphi(t, by) * phi(u, bz);
gradient[2] = phi(s, bx) * phi(t, by) * dphi(u, bz);
F[dof++] = gradient;
}
}
}
if (dof != n) { // SHOULD NOT OCCUR
throw mjCError(NULL, "incorrect number of basis functions");
}
// tensor contraction of the gradients of elastic strains
// (d(F+F')/dx : d(F+F')/dx)
for (int i=0; i < n; i++) {
for (int j=0; j < n; j++) {
Matrix du;
Matrix dv;
du.fill({0, 0, 0});
dv.fill({0, 0, 0});
for (int k=0; k < 3; k++) {
for (int l=0; l < 3; l++) {
du[k][0] = invJ[0] * F[i][0];
du[k][1] = invJ[1] * F[i][1];
du[k][2] = invJ[2] * F[i][2];
dv[l][0] = invJ[0] * F[j][0];
dv[l][1] = invJ[1] * F[j][1];
dv[l][2] = invJ[2] * F[j][2];
K[ndof*(3*i+k) + 3*j+l] -= la * trace(du) * trace(dv) * dvol;
K[ndof*(3*i+k) + 3*j+l] -= mu * trace(inner(sym(du), sym(dv))) * dvol;
mjuu_zerovec(du[k].data(), 3);
mjuu_zerovec(dv[l].data(), 3);
}
}
}
}
}
}
}
}
//------------------ class mjCFlex implementation --------------------------------------------------
// constructor
@@ -2641,6 +2789,7 @@ mjCFlex::mjCFlex(mjCModel* _model) {
// clear internal variables
nvert = 0;
nnode = 0;
nedge = 0;
nelem = 0;
matid = -1;
@@ -2673,13 +2822,17 @@ void mjCFlex::PointToLocal() {
spec.name = &name;
spec.material = &spec_material_;
spec.vertbody = &spec_vertbody_;
spec.nodebody = &spec_nodebody_;
spec.vert = &spec_vert_;
spec.node = &spec_node_;
spec.texcoord = &spec_texcoord_;
spec.elem = &spec_elem_;
spec.info = &info;
material = nullptr;
vertbody = nullptr;
nodebody = nullptr;
vert = nullptr;
node = nullptr;
texcoord = nullptr;
elem = nullptr;
}
@@ -2690,6 +2843,9 @@ void mjCFlex::NameSpace(const mjCModel* m) {
for (auto& name : spec_vertbody_) {
name = m->prefix + name + m->suffix;
}
for (auto& name : spec_nodebody_) {
name = m->prefix + name + m->suffix;
}
}
@@ -2699,7 +2855,9 @@ void mjCFlex::CopyFromSpec() {
spec.info = &info;
material_ = spec_material_;
vertbody_ = spec_vertbody_;
nodebody_ = spec_nodebody_;
vert_ = spec_vert_;
node_ = spec_node_;
texcoord_ = spec_texcoord_;
elem_ = spec_elem_;
@@ -2722,6 +2880,8 @@ void mjCFlex::DelTexcoord() {
void mjCFlex::ResolveReferences(const mjCModel* m) {
vertbodyid.clear();
nodebodyid.clear();
for (const auto& vertbody : vertbody_) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, vertbody);
if (pbody) {
@@ -2730,12 +2890,21 @@ void mjCFlex::ResolveReferences(const mjCModel* m) {
throw mjCError(this, "unknown body '%s' in flex", vertbody.c_str());
}
}
for (const auto& nodebody : nodebody_) {
mjCBase* pbody = m->FindObject(mjOBJ_BODY, nodebody);
if (pbody) {
nodebodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unknown body '%s' in flex", nodebody.c_str());
}
}
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
CopyFromSpec();
interpolated = !nodebody_.empty();
// set nelem; check sizes
if (dim<1 || dim>3) {
@@ -2747,8 +2916,8 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (elem_.size() % (dim+1)) {
throw mjCError(this, "elem size must be multiple of (dim+1)");
}
if (vertbody_.empty()) {
throw mjCError(this, "vertbody is empty");
if (vertbody_.empty() && !interpolated) {
throw mjCError(this, "vertbody and nodebody are both empty");
}
if (vert_.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
@@ -2756,6 +2925,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
if (edgestiffness>0 && dim>1) {
throw mjCError(this, "edge stiffness only available for dim=1, please use elasticity plugins");
}
if (interpolated && selfcollide != mjFLEXSELF_NONE) {
throw mjCError(this, "trilinear interpolation cannot do self-collision");
}
if (interpolated && internal) {
throw mjCError(this, "trilinear interpolation cannot do internal collisions");
}
nelem = (int)elem_.size()/(dim+1);
// set nvert, rigid, centered; check size
@@ -2773,6 +2948,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
throw mjCError(this, "not enough vertices");
}
// set nnode
nnode = (int)nodebody_.size();
if (nnode && nnode!=8) {
throw mjCError(this, "number of nodes must be 2^dim, it is %d", "", nnode);
}
// check elem vertex ids
for (const auto& elem : elem_) {
if (elem<0 || elem>=nvert) {
@@ -2812,7 +2993,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// determine rigid if not already set
if (!rigid) {
if (!rigid && !interpolated) {
rigid = true;
for (unsigned i=1; i < vertbodyid.size(); i++) {
if (vertbodyid[i]!=vertbodyid[0]) {
@@ -2823,7 +3004,7 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
// determine centered if not already set
if (!centered) {
if (!centered && !interpolated) {
centered = true;
for (const auto& vert : vert_) {
if (vert!=0) {
@@ -2833,6 +3014,16 @@ void mjCFlex::Compile(const mjVFS* vfs) {
}
}
if (!centered && interpolated) {
centered = true;
for (const auto& node : node_) {
if (node!=0) {
centered = false;
break;
}
}
}
// compute global vertex positions
vertxpos = std::vector<double> (3*nvert);
for (int i=0; i < nvert; i++) {
@@ -2841,11 +3032,32 @@ void mjCFlex::Compile(const mjVFS* vfs) {
mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add vertex offset within body if not centered
if (!centered) {
if (!centered || interpolated) {
double offset[3];
mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(vertxpos.data()+3*i, offset, 3);
}
if (interpolated) {
// this should happen in ResolveReferences but we need a body id in this loop to compute
// the global vertex position, this is a hack since it is the id of the parent body
vertbodyid[i] = -1;
}
}
// compute global node positions
std::vector<double> nodexpos = std::vector<double> (3*nnode);
for (int i=0; i < nnode; i++) {
// get body id, set nodexpos = body.xpos0
int b = nodebodyid[i];
mjuu_copyvec(nodexpos.data()+3*i, model->Bodies()[b]->xpos0, 3);
// add node offset within body if not centered
if (!centered) {
double offset[3];
mjuu_rotVecQuat(offset, node_.data()+3*i, model->Bodies()[b]->xquat0);
mjuu_addtovec(nodexpos.data()+3*i, offset, 3);
}
}
// reorder tetrahedra so right-handed face orientation is outside
@@ -2909,7 +3121,17 @@ void mjCFlex::Compile(const mjVFS* vfs) {
throw mjCError(this, "Poisson ratio must be in [0, 0.5)");
}
stiffness.assign(21*nelem, 0);
if (interpolated) {
int min_size = ceil(nodexpos.size()*nodexpos.size() / 21);
if (min_size > nelem) {
throw mjCError(this, "Trilinear dofs are require at least %d elements", "", min_size);
}
ComputeLinearStiffness(stiffness, nodexpos.data(), young, poisson);
}
for (unsigned int t = 0; t < nelem; t++) {
if (interpolated) {
continue;
}
if (dim==2) {
ComputeStiffness<Stencil2D>(stiffness, vertxpos,
elem_.data() + (dim + 1) * t, t, young,
@@ -2928,6 +3150,9 @@ void mjCFlex::Compile(const mjVFS* vfs) {
useredge = VectorToString(edgeidx_);
for (const auto& vbodyid : vertbodyid) {
if (vbodyid < 0) {
continue;
}
if (model->Bodies()[vbodyid]->plugin.element) {
mjCPlugin* plugin_instance =
static_cast<mjCPlugin*>(model->Bodies()[vbodyid]->plugin.element);
@@ -2954,6 +3179,12 @@ void mjCFlex::Compile(const mjVFS* vfs) {
vert0_[3*j+k] = (vertxpos[3*j+k] - bvh[k]) / size + 0.5;
}
}
// store node cartesian positions
node0_.assign(3*nnode, 0);
for (int i=0; i < nnode; i++) {
mjuu_copyvec(node0_.data()+3*i, nodexpos.data()+3*i, 3);
}
}
src/user/user_model.cc
+34 -4
View File
@@ -823,6 +823,7 @@ void mjCModel::Clear() {
nv = 0;
nu = 0;
na = 0;
nflexnode = 0;
nflexvert = 0;
nflexedge = 0;
nflexelem = 0;
@@ -1733,6 +1734,7 @@ void mjCModel::SetSizes() {
// flex counts
for (int i=0; i<nflex; i++) {
nflexnode += flexes_[i]->nnode;
nflexvert += flexes_[i]->nvert;
nflexedge += flexes_[i]->nedge;
nflexelem += flexes_[i]->nelem;
@@ -2746,7 +2748,7 @@ int mjCModel::CountNJmom(const mjModel* m) {
// copy objects outside kinematic tree
void mjCModel::CopyObjects(mjModel* m) {
int adr, bone_adr, vert_adr, normal_adr, face_adr, texcoord_adr;
int adr, bone_adr, vert_adr, node_adr, normal_adr, face_adr, texcoord_adr;
int edge_adr, elem_adr, elemdata_adr, elemedge_adr, shelldata_adr, evpair_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr;
@@ -2826,6 +2828,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// flexes
vert_adr = 0;
node_adr = 0;
edge_adr = 0;
elem_adr = 0;
elemdata_adr = 0;
@@ -2865,6 +2868,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->flex_dim[i] = pfl->dim;
m->flex_vertadr[i] = vert_adr;
m->flex_vertnum[i] = pfl->nvert;
m->flex_nodeadr[i] = node_adr;
m->flex_nodenum[i] = pfl->nnode;
m->flex_edgeadr[i] = edge_adr;
m->flex_edgenum[i] = pfl->nedge;
m->flex_elemadr[i] = elem_adr;
@@ -2924,16 +2929,27 @@ void mjCModel::CopyObjects(mjModel* m) {
}
// copy or set vert
if (pfl->centered) {
if (pfl->centered && !pfl->interpolated) {
mjuu_zerovec(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
mjuu_copyvec(m->flex_vert + 3*vert_adr, pfl->vert_.data(), 3*pfl->nvert);
}
// copy or set node
if (pfl->centered && pfl->interpolated) {
mjuu_zerovec(m->flex_node + 3*node_adr, 3*pfl->nnode);
}
else if (pfl->interpolated) {
mjuu_copyvec(m->flex_node + 3*node_adr, pfl->node_.data(), 3*pfl->nnode);
}
// copy vert0
mjuu_copyvec(m->flex_vert0 + 3*vert_adr, pfl->vert0_.data(), 3*pfl->nvert);
// copy node0
mjuu_copyvec(m->flex_node0 + 3*node_adr, pfl->node0_.data(), 3*pfl->nnode);
// copy or set vertbodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nvert; k++) {
@@ -2944,6 +2960,18 @@ void mjCModel::CopyObjects(mjModel* m) {
memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
}
// copy or set nodebodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nnode; k++) {
m->flex_nodebodyid[node_adr + k] = pfl->nodebodyid[0];
}
} else {
memcpy(m->flex_nodebodyid + node_adr, pfl->nodebodyid.data(), pfl->nnode*sizeof(int));
}
// set interpolation type, only two types for now
m->flex_interp[i] = pfl->interpolated;
// convert edge pairs to int array, set edge rigid
for (int k=0; k<pfl->nedge; k++) {
m->flex_edge[2*(edge_adr+k)] = pfl->edge[k].first;
@@ -2951,8 +2979,9 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pfl->rigid) {
m->flexedge_rigid[edge_adr+k] = 1;
} else {
} else if (!pfl->interpolated) {
// check if vertex body weldids are the same
// unsupported by trilinear interpolation
int b1 = pfl->vertbodyid[pfl->edge[k].first];
int b2 = pfl->vertbodyid[pfl->edge[k].second];
m->flexedge_rigid[edge_adr+k] = (bodies_[b1]->weldid == bodies_[b2]->weldid);
@@ -2961,6 +2990,7 @@ void mjCModel::CopyObjects(mjModel* m) {
// advance counters
vert_adr += pfl->nvert;
node_adr += pfl->nnode;
edge_adr += pfl->nedge;
elem_adr += pfl->nelem;
elemdata_adr += (pfl->dim+1) * pfl->nelem;
@@ -4317,7 +4347,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
// create low-level model
mj_makeModel(&m,
nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
ncam, nlight, nflex, nflexnode, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexelemedge, nflexshelldata, nflexevpair, nflextexcoord,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
src/user/user_model.h
+1
View File
@@ -86,6 +86,7 @@ class mjCModel_ : public mjsElement {
int nbvh; // number of total boundary volume hierarchies
int nbvhstatic; // number of static boundary volume hierarchies
int nbvhdynamic; // number of dynamic boundary volume hierarchies
int nflexnode; // number of nodes in all flexes
int nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
src/user/user_objects.h
+9
View File
@@ -746,12 +746,15 @@ class mjCLight : public mjCLight_, private mjsLight {
class mjCFlex_ : public mjCBase {
protected:
int nvert; // number of verices
int nnode; // number of nodes
int nedge; // number of edges
int nelem; // number of elements
int matid; // material id
bool rigid; // all vertices attached to the same body
bool centered; // all vertices coordinates (0,0,0)
bool interpolated; // vertices are interpolated from nodes
std::vector<int> vertbodyid; // vertex body ids
std::vector<int> nodebodyid; // node body ids
std::vector<std::pair<int, int>> edge; // edge vertex ids
std::vector<int> shell; // shell fragment vertex ids (dim per fragment)
std::vector<int> elemlayer; // element layer (distance from border)
@@ -764,14 +767,18 @@ class mjCFlex_ : public mjCBase {
// variable-size data
std::vector<std::string> vertbody_; // vertex body names
std::vector<std::string> nodebody_; // node body names
std::vector<double> vert_; // vertex positions
std::vector<double> node_; // node positions
std::vector<int> elem_; // element vertex ids
std::vector<float> texcoord_; // vertex texture coordinates
std::string material_; // name of material used for rendering
std::string spec_material_;
std::vector<std::string> spec_vertbody_;
std::vector<std::string> spec_nodebody_;
std::vector<double> spec_vert_;
std::vector<double> spec_node_;
std::vector<int> spec_elem_;
std::vector<float> spec_texcoord_;
};
@@ -804,6 +811,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
const std::vector<double>& get_elemaabb() const { return elemaabb_; }
const std::vector<int>& get_elem() const { return elem_; }
const std::vector<float>& get_texcoord() const { return texcoord_; }
const std::vector<std::string>& get_nodebody() const { return nodebody_; }
bool HasTexcoord() const; // texcoord not null
void DelTexcoord(); // delete texcoord
@@ -816,6 +824,7 @@ class mjCFlex: public mjCFlex_, private mjsFlex {
void CreateShellPair(void); // create shells and evpairs
std::vector<double> vert0_; // vertex positions in [0, 1]^d in the bounding box
std::vector<double> node0_; // node Cartesian positions
};
src/xml/xml_native_reader.cc
+20 -4
View File
@@ -311,7 +311,7 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"flexcomp", "*", "25", "name", "type", "group", "dim",
{"flexcomp", "*", "26", "name", "type", "group", "dim", "dof",
"count", "spacing", "radius", "rigid", "mass", "inertiabox",
"scale", "file", "point", "element", "texcoord", "material", "rgba",
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler", "origin"},
@@ -331,8 +331,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
{"deformable", "*", "0"},
{"<"},
{"flex", "*", "11", "name", "group", "dim", "radius", "material",
"rgba", "flatskin", "body", "vertex", "element", "texcoord"},
{"flex", "*", "12", "name", "group", "dim", "radius", "material",
"rgba", "flatskin", "body", "vertex", "element", "texcoord", "node"},
{"<"},
{"contact", "?", "13", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap",
@@ -803,6 +803,14 @@ const mjMap fcomp_map[mjNFCOMPTYPES] = {
};
// flexcomp dof type
const mjMap fdof_map[mjNFCOMPDOFS] = {
{"full", mjFCOMPDOF_FULL},
{"radial", mjFCOMPDOF_RADIAL},
{"trilinear", mjFCOMPDOF_TRILINEAR}
};
// flex selfcollide type
const mjMap flexself_map[5] = {
{"none", mjFLEXSELF_NONE},
@@ -1324,7 +1332,7 @@ void mjXReader::Statistic(XMLElement* section) {
// flex element parser
void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
string text, name, material;
string text, name, material, nodebody;
int n;
// read attributes
@@ -1347,6 +1355,9 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) {
if (ReadAttrTxt(elem, "body", text, true)) {
mjs_setStringVec(flex->vertbody, text.c_str());
}
if (ReadAttrTxt(elem, "node", nodebody)) {
mjs_setStringVec(flex->nodebody, nodebody.c_str());
}
auto vert = ReadAttrVec<double>(elem, "vertex");
if (vert.has_value()) {
mjs_setDouble(flex->vert, vert->data(), vert->size());
@@ -2678,6 +2689,11 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) {
fcomp.texcoord = std::move(texcoord.value());
}
// dof type
if (MapValue(elem, "dof", &n, fdof_map, mjNFCOMPDOFS)) {
fcomp.doftype = (mjtDof)n;
}
// edge
XMLElement* edge = FirstChildElement(elem, "edge");
if (edge) {
src/xml/xml_native_writer.cc
+4
View File
@@ -158,6 +158,10 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) {
text = VectorToString(flex->get_texcoord());
WriteAttrTxt(elem, "texcoord", text);
}
if (!flex->get_nodebody().empty()) {
text = VectorToString(flex->get_nodebody());
WriteAttrTxt(elem, "node", text);
}
// contact subelement
XMLElement* cont = InsertEnd(elem, "contact");
test/engine/engine_util_misc_test.cc
+89
View File
@@ -32,6 +32,7 @@ namespace {
using ::testing::DoubleNear;
using ::testing::HasSubstr;
using ::testing::Ne;
using ::testing::Pointwise;
using ::testing::StrEq;
using ::testing::ElementsAreArray;
@@ -210,6 +211,94 @@ TEST_F(UtilMiscTest, MuscleGainLength) {
EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0);
}
// --------------------------------- Interpolation -----------------------------
using InterpolationTest = MujocoTest;
TEST_F(InterpolationTest, mju_defGradient) {
int order = 1;
mjtNum mat[9];
mjtNum p1[3] = {.5, .5, .5};
mjtNum p2[3] = {.25, .25, .25};
mjtNum dof0[24] = {0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 1, 1,
1, 0, 0, 1, 0, 1, 1, 1, 0, 1, 1, 1};
// identity
mjtNum dof1[24];
for (int i = 0; i < 24; ++i) dof1[i] = dof0[i];
mju_defGradient(mat, p1, dof1, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
// translation
mjtNum dof2[24];
for (int i = 0; i < 24; ++i) dof2[i] = 2 + dof0[i];
mju_defGradient(mat, p1, dof2, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
mju_defGradient(mat, p2, dof2, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 1, 0, 0, 0, 1}));
// constant stretch
mjtNum dof3[24];
for (int i = 0; i < 24; ++i) dof3[i] = 2*dof0[i];
mju_defGradient(mat, p1, dof3, order);
EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
mju_defGradient(mat, p2, dof3, order);
EXPECT_THAT(mat, ElementsAreArray({2, 0, 0, 0, 2, 0, 0, 0, 2}));
// axial stretch
mjtNum dof4[24];
for (int i = 0; i < 24; ++i) dof4[i] = (i%3 == 1 ? 2 : 1)*dof0[i];
mju_defGradient(mat, p1, dof4, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
mju_defGradient(mat, p2, dof4, order);
EXPECT_THAT(mat, ElementsAreArray({1, 0, 0, 0, 2, 0, 0, 0, 1}));
// z-axis 90 degree rotation
mjtNum dof5[24];
for (int i = 0; i < 8; ++i) {
mjtNum quat[4] = {0, 0, 0, 1};
mjtNum axis[3] = {0, 0, 1};
mju_axisAngle2Quat(quat, axis, mjPI/2);
mju_rotVecQuat(dof5 + 3*i, dof0 + 3*i, quat);
}
mju_defGradient(mat, p1, dof5, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
mju_defGradient(mat, p2, dof5, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), {0, -1, 0, 1, 0, 0, 0, 0, 1}));
// z-axis 30 degree rotation
mjtNum dof6[24];
mjtNum rot6[9];
for (int i = 0; i < 8; ++i) {
mjtNum quat[4];
mjtNum axis[3] = {0, 0, 1};
mju_axisAngle2Quat(quat, axis, mjPI/6);
mju_rotVecQuat(dof6 + 3*i, dof0 + 3*i, quat);
mju_quat2Mat(rot6, quat);
}
mju_defGradient(mat, p1, dof6, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
mju_defGradient(mat, p2, dof6, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot6));
// z-axis CoM rotation
mjtNum dof7[24];
mjtNum rot7[9];
for (int i = 0; i < 8; ++i) {
mjtNum quat[4];
mjtNum axis[3] = {0, 0, 1};
mjtNum offset[3] = {-.5, -.5, 0};
mju_axisAngle2Quat(quat, axis, mjPI/6);
mju_add3(dof7 + 3*i, dof0 + 3*i, offset);
mju_rotVecQuat(dof7 + 3*i, dof0 + 3*i, quat);
mju_quat2Mat(rot7, quat);
}
mju_defGradient(mat, p1, dof7, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
mju_defGradient(mat, p2, dof7, order);
EXPECT_THAT(mat, Pointwise(DoubleNear(1e-8), rot7));
}
// --------------------------------- Base64 ------------------------------------
using Base64Test = MujocoTest;
test/testdata/flex.xml
Vendored
+7 -1
View File
@@ -29,7 +29,7 @@
<freejoint/>
<flexcomp name="f1" type="ellipsoid" rgba=".8 .2 .2 1" radius="0.001" count="4 4 4"
spacing=".025 .025 .025" dim="3" mass="1">
spacing=".025 .025 .025" dim="3" mass="1" dof="radial">
<edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
</flexcomp>
</body>
@@ -44,6 +44,12 @@
zaxis="1 1 1" count="3 3 3" mass="1" spacing="0.02 0.03 0.04">
<edge equality="true" solref="0.15 0.2" stiffness="0" damping="0"/>
</flexcomp>
<flexcomp type="grid" count="8 8 8" spacing=".007 .007 .007" pos="0 0 1.5" dim="3"
radius=".0001" rgba="0 .7 .7 1" mass=".25" name="softbody" dof="trilinear">
<elasticity young="1e4" poisson="0.1" damping="0.01"/>
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
test/user/user_flex_test.cc
+154 -1
View File
@@ -28,14 +28,15 @@
namespace mujoco {
namespace {
using ::testing::DoubleNear;
using ::testing::IsNull;
using ::testing::NotNull;
using ::testing::HasSubstr;
using ::testing::Pointwise;
using UserFlexTest = MujocoTest;
TEST_F(UserFlexTest, ParentMustHaveName) {
static constexpr char xml[] = R"(
<mujoco>
@@ -282,6 +283,158 @@ TEST_F(UserFlexTest, BoundingBoxCoordinates) {
mj_deleteData(d);
}
TEST_F(UserFlexTest, TrilinearCannotDoSelfCollision) {
std::array<char, 1024> error;
static constexpr char xml_selfcoll[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="auto" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
mjModel* m1 = LoadModelFromString(xml_selfcoll, error.data(), error.size());
EXPECT_THAT(m1, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("trilinear interpolation cannot do self-collision"));
static constexpr char xml_internal[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none" internal="true"/>
</flexcomp>
</worldbody>
</mujoco>
)";
mjModel* m2 = LoadModelFromString(xml_internal, error.data(), error.size());
EXPECT_THAT(m2, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("trilinear interpolation cannot do internal"));
}
TEST_F(UserFlexTest, TrilinearInterpolation) {
static constexpr char xml_trilinear[] = R"(
<mujoco>
<worldbody>
<geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m1 = LoadModelFromString(xml_trilinear, error.data(), error.size());
ASSERT_THAT(m1, NotNull()) << error.data();
mjData* d1 = mj_makeData(m1);
mj_step(m1, d1);
static constexpr char xml_linear[] = R"(
<mujoco>
<worldbody>
<geom type="plane" pos="0 0 -.5" size="10 10 .1"/>
<flexcomp name="test" type="grid" count="2 2 2" spacing="1 1 1" dim="3">
<contact selfcollide="none" internal="false"/>
</flexcomp>
</worldbody>
</mujoco>
)";
mjModel* m2 = LoadModelFromString(xml_linear, error.data(), error.size());
ASSERT_THAT(m2, NotNull()) << error.data();
mjData* d2 = mj_makeData(m2);
mj_step(m2, d2);
EXPECT_EQ(m1->nflexvert, m2->nflexvert);
for (int i = 0; i < 3*m1->nflexvert; ++i) {
EXPECT_EQ(m1->flex_vert[i], d2->flexvert_xpos[i]);
EXPECT_EQ(m1->flex_vert0[i], m2->flex_vert0[i]);
EXPECT_EQ(d1->flexvert_xpos[i], d2->flexvert_xpos[i]);
}
EXPECT_EQ(m1->nM, m2->nM);
for (int i = 0; i < m1->nM; ++i) {
EXPECT_EQ(d1->qM[i], d2->qM[i]);
}
EXPECT_EQ(m1->nbody, m2->nbody);
for (int i = 0; i < m2->nbody; ++i) {
if (i == 0) {
continue;
}
EXPECT_EQ(m1->body_mass[i], m2->body_mass[i]);
for (int j = 0; j < 2; ++j) {
EXPECT_EQ(m1->body_invweight0[i*2+j], m2->body_invweight0[i*2+j]);
}
for (int j = 0; j < 10; ++j) {
EXPECT_NEAR(d1->cinert[10*i+j], d2->cinert[i*10+j], 1e-5) << i;
EXPECT_NEAR(d1->crb[10*i+j], d2->crb[i*10+j], 1e-5) << i;
}
}
EXPECT_EQ(d1->ncon, 4);
EXPECT_EQ(d2->ncon, 4);
for (int i = 0; i < d1->ncon; ++i) {
EXPECT_EQ(d1->contact[i].dist, d2->contact[i].dist);
EXPECT_EQ(d1->contact[i].mu, d2->contact[i].mu);
for (int j = 0; j < 5; ++j) {
EXPECT_EQ(d1->contact[i].friction[j], d2->contact[i].friction[j]);
}
for (int j = 0; j < 3; ++j) {
EXPECT_EQ(d1->contact[i].pos[j], d2->contact[i].pos[j]);
}
for (int j = 0; j < 9; ++j) {
EXPECT_EQ(d1->contact[i].frame[j], d2->contact[i].frame[j]);
}
for (int j = 0; j < 36; ++j) {
EXPECT_EQ(d1->contact[i].H[j], d2->contact[i].H[j]);
}
}
EXPECT_EQ(d1->nefc, 4*(d1->contact[0].dim-1)*2);
EXPECT_EQ(d2->nefc, 4*(d2->contact[0].dim-1)*2);
EXPECT_EQ(d1->nJ, d2->nJ);
for (int i = 0; i < d1->nefc; ++i) {
EXPECT_EQ(d1->efc_diagApprox[i], d2->efc_diagApprox[i]);
EXPECT_EQ(d1->efc_D[i], d2->efc_D[i]);
}
mj_deleteModel(m1);
mj_deleteModel(m2);
mj_deleteData(d1);
mj_deleteData(d2);
}
TEST_F(UserFlexTest, StiffnessMatrix) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="grid" count="3 3 3" spacing="1 1 1" dim="3" dof="trilinear">
<contact selfcollide="none" internal="false"/>
<elasticity young="1"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(m, NotNull()) << error.data();
EXPECT_NE(m->flex_stiffness[0], 0);
EXPECT_EQ(m->nflexnode, 8);
// constants are in the kernel
mjtNum ones[24], zeros[24], res[24];
for (int i = 0; i < 3*m->nflexnode; ++i) {
zeros[i] = 0;
ones[i] = 1;
}
mju_mulMatVec(res, m->flex_stiffness, ones, 3*m->nflexnode, 3*m->nflexnode);
EXPECT_THAT(res, Pointwise(DoubleNear(1e-8), zeros));
mj_deleteModel(m);
}
TEST_F(UserFlexTest, LoadMSHBinary_41_Success) {
const std::string xml_path =
GetTestDataFilePath("user/testdata/cube_41_binary_vol_gmshApp.xml");
unity/Runtime/Bindings/MjBindings.cs
+14
View File
@@ -41,6 +41,7 @@ public const double mjMINIMP = 0.0001;
public const double mjMAXIMP = 0.9999;
public const int mjMAXCONPAIR = 50;
public const int mjMAXTREEDEPTH = 50;
public const int mjMAXFLEXNODES = 27;
public const int mjNEQDATA = 11;
public const int mjNDYN = 10;
public const int mjNGAIN = 10;
@@ -5191,6 +5192,7 @@ public unsafe struct mjModel_ {
public int ncam;
public int nlight;
public int nflex;
public int nflexnode;
public int nflexvert;
public int nflexedge;
public int nflexelem;
@@ -5411,6 +5413,9 @@ public unsafe struct mjModel_ {
public int* flex_dim;
public int* flex_matid;
public int* flex_group;
public int* flex_interp;
public int* flex_nodeadr;
public int* flex_nodenum;
public int* flex_vertadr;
public int* flex_vertnum;
public int* flex_edgeadr;
@@ -5424,6 +5429,7 @@ public unsafe struct mjModel_ {
public int* flex_evpairadr;
public int* flex_evpairnum;
public int* flex_texcoordadr;
public int* flex_nodebodyid;
public int* flex_vertbodyid;
public int* flex_edge;
public int* flex_elem;
@@ -5433,6 +5439,8 @@ public unsafe struct mjModel_ {
public int* flex_evpair;
public double* flex_vert;
public double* flex_vert0;
public double* flex_node;
public double* flex_node0;
public double* flexedge_length0;
public double* flexedge_invweight0;
public double* flex_radius;
@@ -6296,6 +6304,10 @@ public unsafe struct model {
public int* flex_dim;
public int* flex_matid;
public int* flex_group;
public int* flex_interp;
public int* flex_nodeadr;
public int* flex_nodenum;
public int* flex_nodebodyid;
public int* flex_vertadr;
public int* flex_vertnum;
public int* flex_elem;
@@ -6309,6 +6321,8 @@ public unsafe struct model {
public int* flex_texcoordadr;
public int* flex_bvhadr;
public int* flex_bvhnum;
public byte* flex_centered;
public double* flex_node;
public double* flex_radius;
public float* flex_rgba;
public int* hfield_pathadr;