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

Add Flex component.

Browse Source 
PiperOrigin-RevId: 572830650
Change-Id: I6908228087b7b9683be3506c8d9cdc725ed5dcd5
This commit is contained in:
Alessio Quaglino
2023-10-12 10:15:46 +01:00
committed by Saran Tunyasuvunakool
parent 649a474788
commit 5a70ad08ab
82 changed files with 9658 additions and 1581 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/APIreference/functions.rst
+19
View File
@@ -724,6 +724,15 @@ mj_camlight
Compute camera and light positions and orientations.
.. _mj_flex:
mj_flex
~~~~~~~
.. mujoco-include:: mj_flex
Compute flex-related quantities.
.. _mj_tendon:
mj_tendon
@@ -948,6 +957,16 @@ mju_rayGeom
Intersect ray with pure geom, return nearest distance or -1 if no intersection.
.. _mju_rayFlex:
mju_rayFlex
~~~~~~~~~~~
.. mujoco-include:: mju_rayFlex
Intersect ray with flex, return nearest distance or -1 if no intersection,
and also output nearest vertex id.
.. _mju_raySkin:
mju_raySkin
doc/XMLreference.rst
+227 -5
View File
@@ -1322,9 +1322,9 @@ also known as terrain map, is a 2D matrix of elevation data. The data can be spe
.. _asset-hfield-content_type:
:at:`content_type`: :at-val:`string, optional`
:at:`content_type`: :at-val: `string, optional`
If the file attribute is specified, then this sets the
`Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`__ (formerly known as MIME types) of the
`Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`_ (formerly known as MIME types) of the
file to be loaded. Any filename extensions will be overloaded. Currently ``image/png`` and
``image/vnd.mujoco.hfield`` are supported.
@@ -1578,6 +1578,7 @@ Associate this mesh with an :ref:`engine plugin<exPlugin>`. Either :at:`plugin`
.. _deformable-skin:
.. _asset-skin:
:el-prefix:`asset/` |-| **skin** (*)
@@ -1641,11 +1642,13 @@ Similar to the other custom binary formats used in MuJoCo, the file size in byte
compiler. The skin file format has subelements so the overall file size formula is difficult to write down, but should
be clear from the above specification.
.. _deformable-skin-name:
.. _asset-skin-name:
:at:`name`: :at-val:`string, optional`
Name of the skin.
.. _deformable-skin-file:
.. _asset-skin-file:
:at:`file`: :at-val:`string, optional`
@@ -1653,11 +1656,13 @@ be clear from the above specification.
:ref:`compiler <compiler>`. If the file is omitted, the skin specification must be provided in the XML using the
attributes below.
.. _deformable-skin-vertex:
.. _asset-skin-vertex:
:at:`vertex`: :at-val:`real(3*nvert), optional`
Vertex 3D positions, in the global bind pose where the skin is defined.
.. _deformable-skin-texcoord:
.. _asset-skin-texcoord:
:at:`texcoord`: :at-val:`real(2*nvert), optional`
@@ -1667,22 +1672,26 @@ be clear from the above specification.
skin using this attribute. Otherwise the texture will appear to be stationary in the world while the skin moves
around (creating an interesting effect but probably not as intended).
.. _deformable-skin-face:
.. _asset-skin-face:
:at:`face`: :at-val:`int(3*nface), optional`
Trinagular skin faces. Each face is a triple of vertex indices, which are integers between zero and nvert-1.
.. _deformable-skin-inflate:
.. _asset-skin-inflate:
:at:`inflate`: :at-val:`real, "0"`
If this number is not zero, the position of vertex during updating will be offset along the vertex normal, but the
distance specified in this attribute. This is particularly useful for skins representing flexible 2D shapes.
.. _deformable-skin-material:
.. _asset-skin-material:
:at:`material`: :at-val:`string, optional`
If specified, this attribute applies a material to the skin.
.. _deformable-skin-rgba:
.. _asset-skin-rgba:
:at:`rgba`: :at-val:`real(4), "0.5 0.5 0.5 1"`
@@ -1690,6 +1699,7 @@ be clear from the above specification.
only. This is not as flexible as the material mechanism, but is more convenient and is often sufficient. If the value
of this attribute is different from the internal default, it takes precedence over the material.
.. _deformable-skin-group:
.. _asset-skin-group:
:at:`group`: :at-val:`int, "0"`
@@ -1919,9 +1929,11 @@ adjust it properly through the XML.
.. _option-o_solimp:
:at:`o_solref`, :at:`o_solimp`
These attributes replace the solref and solimp parameters of all active contact pairs when contact override is
enabled. See :ref:`CSolver` for details.
.. _option-o_friction:
:at:`o_solref`, :at:`o_solimp`, :at:`o_friction`
These attributes replace the solref, solimp, and friction parameters of all active contact pairs when contact
override is enabled. See :ref:`CSolver` for details.
.. _option-integrator:
@@ -3702,6 +3714,160 @@ Associate this composite with an :ref:`engine plugin<exPlugin>`. Either :at:`plu
:at:`instance`: :at-val:`string, optional`
Instance name, used for explicit plugin instantiation.
.. _body-flexcomp:
:el-prefix:`body/` |-| **flex** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. _body-flexcomp-name:
.. _body-flexcomp-class:
.. _body-flexcomp-type:
.. _body-flexcomp-dim:
.. _body-flexcomp-count:
.. _body-flexcomp-spacing:
.. _body-flexcomp-radius:
.. _body-flexcomp-rigid:
.. _body-flexcomp-mass:
.. _body-flexcomp-inertiabox:
.. _body-flexcomp-scale:
.. _body-flexcomp-file:
.. _body-flexcomp-point:
.. _body-flexcomp-element:
.. _body-flexcomp-material:
.. _body-flexcomp-rgba:
.. _body-flexcomp-texcoord:
.. _body-flexcomp-pos:
.. _body-flexcomp-quat:
.. _body-flexcomp-axisangle:
.. _body-flexcomp-xyaxes:
.. _body-flexcomp-zaxis:
.. _body-flexcomp-euler:
.. _body-flexcomp-selfcollide:
.. _body-flexcomp-flatskin:
.. _flex-edge:
.. _flexcomp-edge:
.. _flex-edge-equality:
.. _flexcomp-edge-equality:
.. _flex-edge-solref:
.. _flexcomp-edge-solref:
.. _flex-edge-solimp:
.. _flexcomp-edge-solimp:
.. _flex-edge-stiffness:
.. _flexcomp-edge-stiffness:
.. _flex-edge-damping:
.. _flexcomp-edge-damping:
.. _flex-contact:
.. _flexcomp-contact:
.. _flexcomp-contact-:
.. _flex-contact-contype:
.. _flexcomp-contact-contype:
.. _flex-contact-conaffinity:
.. _flexcomp-contact-conaffinity:
.. _flex-contact-condim:
.. _flexcomp-contact-condim:
.. _flex-contact-priority:
.. _flexcomp-contact-priority:
.. _flex-contact-friction:
.. _flexcomp-contact-friction:
.. _flex-contact-solmix:
.. _flexcomp-contact-solmix:
.. _flex-contact-solref:
.. _flexcomp-contact-solref:
.. _flex-contact-solimp:
.. _flexcomp-contact-solimp:
.. _flex-contact-margin:
.. _flexcomp-contact-margin:
.. _flex-contact-gap:
.. _flexcomp-contact-gap:
.. _flexcomp-pin:
.. _flexcomp-pin-id:
.. _flexcomp-pin-range:
.. _flexcomp-pin-grid:
.. _flexcomp-pin-gridrange:
.. _deformable:
.. _deformable-flex:
.. _deformable-flex-name:
.. _deformable-flex-group:
.. _deformable-flex-material:
.. _deformable-flex-radius:
.. _deformable-flex-rgba:
.. _deformable-flex-texcoord:
.. _deformable-flex-flatskin:
.. _deformable-flex-selfcollide:
.. _deformable-flex-dim:
.. _deformable-flex-body:
.. _deformable-flex-vertex:
.. _deformable-flex-element:
**deformable** (*)
~~~~~~~~~~~~~~~
.. _contact:
**contact** (*)
@@ -4027,6 +4193,24 @@ This element constrains the length of one tendon to be a quartic polynomial of a
positions.
.. _equality-flex:
:el-prefix:`equality/` |-| **flex** (*)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. _equality-flex-name:
.. _equality-flex-class:
.. _equality-flex-flex:
.. _equality-flex-active:
.. _equality-flex-solref:
.. _equality-flex-solimp:
.. _equality-distance:
:el-prefix:`equality/` |-| **distance** (*)
@@ -7165,6 +7349,44 @@ tendon, slidersite, cranksite.
All :ref:`adhesion <actuator-adhesion>` attributes are available here except: name, class, body.
.. _default-flex:
.. _default-flex-contype:
.. _default-flex-conaffinity:
.. _default-flex-condim:
.. _default-flex-priority:
.. _default-flex-material:
.. _default-flex-friction:
.. _default-flex-solmix:
.. _default-flex-solref:
.. _default-flex-solimp:
.. _default-flex-margin:
.. _default-flex-gap:
.. _default-flex-stiffness:
.. _default-flex-damping:
.. _default-flex-radius:
.. _default-flex-rgba:
.. _default-flex-dim:
:el-prefix:`default/` |-| **flex** (?)
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
.. _custom:
**custom** (*)
doc/XMLschema.rst
+111 -4
View File
@@ -222,13 +222,13 @@
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`wind<option-wind>` | :ref:`magnetic<option-magnetic>` | :ref:`density<option-density>` | :ref:`viscosity<option-viscosity>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`o_margin<option-o_margin>` | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`integrator<option-integrator>` | |
| | | | :ref:`o_margin<option-o_margin>` | :ref:`o_solref<option-o_solref>` | :ref:`o_solimp<option-o_solimp>` | :ref:`o_friction<option-o_friction>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | :ref:`iterations<option-iterations>` | |
| | | | :ref:`integrator<option-integrator>` | :ref:`cone<option-cone>` | :ref:`jacobian<option-jacobian>` | :ref:`solver<option-solver>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`ls_iterations<option-ls_iterations>` | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | :ref:`sdf_iterations<option-sdf_iterations>` | |
| | | | :ref:`iterations<option-iterations>` | :ref:`ls_iterations<option-ls_iterations>` | :ref:`noslip_iterations<option-noslip_iterations>` | :ref:`mpr_iterations<option-mpr_iterations>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`sdf_initpoints<option-sdf_initpoints>` | | | | |
| | | | :ref:`sdf_iterations<option-sdf_iterations>` | :ref:`sdf_initpoints<option-sdf_initpoints>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| option |br| |_| |L| | | .. table:: |
@@ -481,6 +481,104 @@
| | | | :ref:`key<plugin-config-key>` | :ref:`value<plugin-config-value>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| body |br| |_| |L| | | .. table:: |
| :ref:`flexcomp | \* | :class: mjcf-attributes |
| <body-flexcomp>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<body-flexcomp-name>` | :ref:`class<body-flexcomp-class>` | :ref:`type<body-flexcomp-type>` | :ref:`dim<body-flexcomp-dim>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`flatskin<body-flexcomp-flatskin>` | :ref:`count<body-flexcomp-count>` | :ref:`spacing<body-flexcomp-spacing>` | :ref:`radius<body-flexcomp-radius>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`rigid<body-flexcomp-rigid>` | :ref:`mass<body-flexcomp-mass>` | :ref:`inertiabox<body-flexcomp-inertiabox>` | :ref:`scale<body-flexcomp-scale>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`file<body-flexcomp-file>` | :ref:`point<body-flexcomp-point>` | :ref:`element<body-flexcomp-element>` | :ref:`texcoord<body-flexcomp-texcoord>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`material<body-flexcomp-material>` | :ref:`rgba<body-flexcomp-rgba>` | :ref:`selfcollide<body-flexcomp-selfcollide>` | :ref:`flatskin<body-flexcomp-flatskin>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`pos<body-flexcomp-pos>` | :ref:`quat<body-flexcomp-quat>` | :ref:`axisangle<body-flexcomp-axisangle>` | :ref:`xyaxes<body-flexcomp-xyaxes>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`zaxis<body-flexcomp-zaxis>` | :ref:`euler<body-flexcomp-euler>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
| :ref:`edge | ? | :class: mjcf-attributes |
| <flexcomp-edge>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`equality<flexcomp-edge-equality>` | :ref:`solref<flexcomp-edge-solref>` | :ref:`solimp<flexcomp-edge-solimp>` | :ref:`stiffness<flexcomp-edge-stiffness>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`damping<flexcomp-edge-damping>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
| :ref:`contact | ? | :class: mjcf-attributes |
| <flexcomp-contact>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`contype<flexcomp-contact-contype>` | :ref:`conaffinity<flexcomp-contact-conaffinity>` | :ref:`condim<flexcomp-contact-condim>` | :ref:`priority<flexcomp-contact-priority>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`friction<flexcomp-contact-friction>` | :ref:`solmix<flexcomp-contact-solmix>` | :ref:`solref<flexcomp-contact-solref>` | :ref:`solimp<flexcomp-contact-solimp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`margin<flexcomp-contact-margin>` | :ref:`gap<flexcomp-contact-gap>` | :ref:`<flexcomp-contact->` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flexcomp |br| |_2| |L| | | .. table:: |
| :ref:`pin | \* | :class: mjcf-attributes |
| <flexcomp-pin>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`id<flexcomp-pin-id>` | :ref:`range<flexcomp-pin-range>` | :ref:`grid<flexcomp-pin-grid>` | :ref:`gridrange<flexcomp-pin-gridrange>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | *no attributes* |
| :ref:`deformable<deformable>` | | |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| deformable |br| |_| |L| | | .. table:: |
| :ref:`flex | \* | :class: mjcf-attributes |
| <deformable-flex>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<deformable-flex-name>` | :ref:`group<deformable-flex-group>` | :ref:`dim<deformable-flex-dim>` | :ref:`radius<deformable-flex-radius>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`material<deformable-flex-material>` | :ref:`rgba<deformable-flex-rgba>` | :ref:`flatskin<deformable-flex-flatskin>` | :ref:`selfcollide<deformable-flex-selfcollide>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body<deformable-flex-body>` | :ref:`vertex<deformable-flex-vertex>` | :ref:`element<deformable-flex-element>` | :ref:`texcoord<deformable-flex-texcoord>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flex |br| |_2| |L| | | .. table:: |
| :ref:`contact | ? | :class: mjcf-attributes |
| <flex-contact>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`contype<flex-contact-contype>` | :ref:`conaffinity<flex-contact-conaffinity>` | :ref:`condim<flex-contact-condim>` | :ref:`priority<flex-contact-priority>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`friction<flex-contact-friction>` | :ref:`solmix<flex-contact-solmix>` | :ref:`solref<flex-contact-solref>` | :ref:`solimp<flex-contact-solimp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`margin<flex-contact-margin>` | :ref:`gap<flex-contact-gap>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| flex |br| |_2| |L| | | .. table:: |
| :ref:`edge | ? | :class: mjcf-attributes |
| <flex-edge>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`stiffness<flex-edge-stiffness>` | :ref:`damping<flex-edge-damping>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| deformable |br| |_| |L| | | .. table:: |
| :ref:`skin | \* | :class: mjcf-attributes |
| <deformable-skin>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<deformable-skin-name>` | :ref:`file<deformable-skin-file>` | :ref:`material<deformable-skin-material>` | :ref:`rgba<deformable-skin-rgba>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`inflate<deformable-skin-inflate>` | :ref:`vertex<deformable-skin-vertex>` | :ref:`texcoord<deformable-skin-texcoord>` | :ref:`face<deformable-skin-face>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`group<deformable-skin-group>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_2| skin |br| |_2| |L| | | .. table:: |
| :ref:`bone | \* | :class: mjcf-attributes |
| <skin-bone>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`body<skin-bone-body>` | :ref:`bindpos<skin-bone-bindpos>` | :ref:`bindquat<skin-bone-bindquat>` | :ref:`vertid<skin-bone-vertid>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`vertweight<skin-bone-vertweight>` | | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | *no attributes* |
| :ref:`contact<contact>` | | |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
@@ -543,6 +641,15 @@
| | | | :ref:`polycoef<equality-tendon-polycoef>` | :ref:`active<equality-tendon-active>` | :ref:`solref<equality-tendon-solref>` | :ref:`solimp<equality-tendon-solimp>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| |_| equality |br| |_| |L| | | .. table:: |
| :ref:`flex | \* | :class: mjcf-attributes |
| <equality-flex>` | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`name<equality-flex-name>` | :ref:`class<equality-flex-class>` | :ref:`flex<equality-flex-flex>` | :ref:`active<equality-flex-active>` | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
| | | | :ref:`solref<equality-flex-solref>` | :ref:`solimp<equality-flex-solimp>` | | | |
| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
| mujoco |br| |L| | | *no attributes* |
| :ref:`tendon<tendon>` | | |
+------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
doc/changelog.rst
+2 -2
View File
@@ -179,8 +179,8 @@ General
:ref:`Cartesian actuator<actuator-general-refsite>` forces are realizable by individual motors at the joints.
See :ref:`CForceRange` for details.
#. Added an optional ``content_type`` attribute to hfield, texture, and mesh assets. This attribute supports a formatted
`Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`__ (previously known as MIME type)
string used to determine the type of the asset file without resorting to pulling the type from the file extension.
`Media Type <https://www.iana.org/assignments/media-types/media-types.xhtml>`_ (previously known as MIME type) string
used to determine the type of the asset file without resorting to pulling the type from the file extension.
#. Added analytic derivatives for quaternion :ref:`subtraction<mjd_subQuat>` and :ref:`integration<mjd_quatIntegrate>`
(rotation with an angular velocity). Derivatives are in the 3D tangent space.
#. Added :ref:`mjv_connector` which has identical functionality to :ref:`mjv_makeConnector`, but with more convenient
doc/includes/references.h
+170 -27
View File
@@ -81,7 +81,7 @@ typedef enum mjtTimer_ { // internal timers
mjNTIMER // number of timers
} mjtTimer;
struct mjContact_ { // result of collision detection functions
// contact parameters set by geom-specific collision detector
// contact parameters set by near-phase collision function
mjtNum dist; // distance between nearest points; neg: penetration
mjtNum pos[3]; // position of contact point: midpoint between geoms
mjtNum frame[9]; // normal is in [0-2]
@@ -97,12 +97,16 @@ struct mjContact_ { // result of collision detection functions
mjtNum mu; // friction of regularized cone, set by mj_makeConstraint
mjtNum H[36]; // cone Hessian, set by mj_updateConstraint
// contact descriptors set by mj_collideGeoms
// contact descriptors set by mj_collideXXX
int dim; // contact space dimensionality: 1, 3, 4 or 6
int geom1; // id of geom 1
int geom2; // id of geom 2
int geom[2]; // geom ids; -1 for flex
int flex[2]; // flex ids; -1 for geom
int elem[2]; // element ids; -1 for geom or flex vertex
int vert[2]; // vertex ids; -1 for geom or flex element
// flag set by mj_instantianteEquality
// flag set by mj_setContact or mj_instantiateContact
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs
// address computed by mj_instantiateContact
@@ -160,12 +164,6 @@ struct mjData_ {
int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island
mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics
int nbodypair_broad; // number of body pairs in collision according to the broad-phase
int nbodypair_narrow; // number of body pairs actually in collision in the narrow-phase
int ngeompair_mid; // number of geom pairs in collision according to the mid-phase
int ngeompair_narrow; // number of geom pairs actually in collision in the narrow-phase
// variable sizes
int ne; // number of equality constraints
int nf; // number of friction constraints
@@ -242,14 +240,23 @@ struct mjData_ {
mjtNum* cdof; // com-based motion axis of each dof (nv x 6)
mjtNum* cinert; // com-based body inertia and mass (nbody x 10)
// computed by mj_fwdPosition/mj_flex
mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3)
mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6)
int* flexedge_J_rownnz; // number of non-zeros in Jacobian row (nflexedge x 1)
int* flexedge_J_rowadr; // row start address in colind array (nflexedge x 1)
int* flexedge_J_colind; // column indices in sparse Jacobian (nflexedge x nv)
mjtNum* flexedge_J; // flex edge Jacobian (nflexedge x nv)
mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1)
// computed by mj_fwdPosition/mj_tendon
int* ten_wrapadr; // start address of tendon's path (ntendon x 1)
int* ten_wrapnum; // number of wrap points in path (ntendon x 1)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (ntendon x nv)
mjtNum* ten_length; // tendon lengths (ntendon x 1)
mjtNum* ten_J; // tendon Jacobian (ntendon x nv)
mjtNum* ten_length; // tendon lengths (ntendon x 1)
int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap*2 x 1)
mjtNum* wrap_xpos; // Cartesian 3D points in all path (nwrap*2 x 3)
@@ -267,11 +274,13 @@ struct mjData_ {
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
// computed by mj_collisionTree
mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6)
mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1)
//-------------------- POSITION, VELOCITY dependent
// computed by mj_fwdVelocity
mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1)
mjtNum* ten_velocity; // tendon velocities (ntendon x 1)
mjtNum* actuator_velocity; // actuator velocities (nu x 1)
@@ -290,8 +299,8 @@ struct mjData_ {
mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3)
// computed by mj_Euler or mj_implicit
mjtNum* qH; // L'*D*L factorization of modified M (nM x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
mjtNum* qH; // L'*D*L factorization of modified M (nM x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* D_rownnz; // non-zeros in each row (nv x 1)
@@ -446,6 +455,7 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
@@ -489,6 +499,7 @@ typedef enum mjtEq_ { // type of equality constraint
mjEQ_WELD, // fix relative position and orientation of two bodies
mjEQ_JOINT, // couple the values of two scalar joints with cubic
mjEQ_TENDON, // couple the lengths of two tendons with cubic
mjEQ_FLEX, // fix all edge lengths of a flex
mjEQ_DISTANCE // unsupported, will cause an error if used
} mjtEq;
typedef enum mjtWrap_ { // type of tendon wrap object
@@ -539,6 +550,7 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_SITE, // site
mjOBJ_CAMERA, // camera
mjOBJ_LIGHT, // light
mjOBJ_FLEX, // flex
mjOBJ_MESH, // mesh
mjOBJ_SKIN, // skin
mjOBJ_HFIELD, // heightfield
@@ -650,6 +662,13 @@ typedef enum mjtLRMode_ { // mode for actuator length range computation
mjLRMODE_MUSCLEUSER, // process muscle and user actuators
mjLRMODE_ALL // process all actuators
} mjtLRMode;
typedef enum mjtFlexSelf_ { // mode for flex selfcollide
mjFLEXSELF_NONE = 0, // no self-collisions
mjFLEXSELF_NARROW, // skip midphase, go directly to narrowphase
mjFLEXSELF_BVH, // use BVH in midphase (if midphase enabled)
mjFLEXSELF_SAP, // use SAP in midphase
mjFLEXSELF_AUTO // choose between BVH and SAP automatically
} mjtFlexSelf;
struct mjLROpt_ { // options for mj_setLengthRange()
// flags
int mode; // which actuators to process (mjtLRMode)
@@ -696,6 +715,7 @@ struct mjOption_ { // physics options
mjtNum o_margin; // margin
mjtNum o_solref[mjNREF]; // solref
mjtNum o_solimp[mjNIMP]; // solimp
mjtNum o_friction[5]; // friction
// discrete settings
int integrator; // integration mode (mjtIntegrator)
@@ -723,9 +743,9 @@ struct mjVisual_ { // visualization options
float linewidth; // line width for wireframe and ray rendering
float glow; // glow coefficient for selected body
float realtime; // initial real-time factor (1: real time)
int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
} global;
struct { // rendering quality
@@ -821,12 +841,22 @@ struct mjModel_ {
int nu; // number of actuators/controls = dim(ctrl)
int na; // number of activation states = dim(act)
int nbody; // number of bodies
int nbvh; // number of bounding volumes in all bodies
int nbvh; // number of total bounding volumes in all bodies
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
int njnt; // number of joints
int ngeom; // number of geoms
int nsite; // number of sites
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of 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
int nflexelemdata; // number of element vertex ids in all flexes
int nflexshelldata; // number of shell fragment vertex ids in all flexes
int nflexevpair; // number of element-vertex pairs in all flexes
int nflextexcoord; // number of vertices with texture coordinates
int nmesh; // number of meshes
int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes
@@ -925,16 +955,19 @@ struct mjModel_ {
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_margin; // MAX over all geom margins (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id; -1: not in use (nbody x 1)
int* body_contype; // OR over all geom contypes (nbody x 1)
int* body_conaffinity; // OR over all geom conaffinities (nbody x 1)
int* body_bvhadr; // address of bvh root (nbody x 1)
int* body_bvhnum; // number of bounding volumes (nbody x 1)
// bounding volume hierarchy
int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1)
int* bvh_child; // left and right children in tree (nbvh x 2)
int* bvh_geomid; // geom id of the node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6)
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -1043,6 +1076,59 @@ struct mjModel_ {
float* light_diffuse; // diffuse rgb (alpha=1) (nlight x 3)
float* light_specular; // specular rgb (alpha=1) (nlight x 3)
// flexes: contact properties
int* flex_contype; // flex contact type (nflex x 1)
int* flex_conaffinity; // flex contact affinity (nflex x 1)
int* flex_condim; // contact dimensionality (1, 3, 4, 6) (nflex x 1)
int* flex_priority; // flex contact priority (nflex x 1)
mjtNum* flex_solmix; // mix coef for solref/imp in contact pair (nflex x 1)
mjtNum* flex_solref; // constraint solver reference: contact (nflex x mjNREF)
mjtNum* flex_solimp; // constraint solver impedance: contact (nflex x mjNIMP)
mjtNum* flex_friction; // friction for (slide, spin, roll) (nflex x 3)
mjtNum* flex_margin; // detect contact if dist<margin (nflex x 1)
mjtNum* flex_gap; // include in solver if dist<margin-gap (nflex x 1)
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
// flexes: other properties
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_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)
int* flex_edgenum; // number of edges (nflex x 1)
int* flex_elemadr; // first element address (nflex x 1)
int* flex_elemnum; // number of elements (nflex x 1)
int* flex_elemdataadr; // first element vertex id address (nflex x 1)
int* flex_shellnum; // number of shells (nflex x 1)
int* flex_shelldataadr; // first shell data address (nflex x 1)
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_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)
int* flex_elemlayer; // element distance from surface, 3D only (nflexelem x 1)
int* flex_shell; // shell fragment vertex ids (dim per frag) (nflexshelldata x 1)
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_xvert0; // Cartesian vertex positions in qpos0 (nflexvert 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)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
mjtByte* flex_edgeequality; // is edge equality constraint defined (nflex x 1)
mjtByte* flex_rigid; // are all verices in the same body (nflex x 1)
mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1)
mjtByte* flex_flatskin; // render flex skin with flat shading (nflex x 1)
int* flex_bvhadr; // address of bvh root; -1: no bvh (nflex x 1)
int* flex_bvhnum; // number of bounding volumes (nflex x 1)
float* flex_rgba; // rgba when material is omitted (nflex x 4)
float* flex_texcoord; // vertex texture coordinates (nflextexcoord x 2)
// meshes
int* mesh_vertadr; // first vertex address (nmesh x 1)
int* mesh_vertnum; // number of vertices (nmesh x 1)
@@ -1117,7 +1203,7 @@ struct mjModel_ {
int* pair_dim; // contact dimensionality (npair x 1)
int* pair_geom1; // id of geom1 (npair x 1)
int* pair_geom2; // id of geom2 (npair x 1)
int* pair_signature; // (body1+1) << 16 + body2+1 (npair x 1)
int* pair_signature; // body1 << 16 + body2 (npair x 1)
mjtNum* pair_solref; // solver reference: contact normal (npair x mjNREF)
mjtNum* pair_solreffriction; // solver reference: contact friction (npair x mjNREF)
mjtNum* pair_solimp; // solver impedance: contact (npair x mjNIMP)
@@ -1126,7 +1212,7 @@ struct mjModel_ {
mjtNum* pair_friction; // tangent1, 2, spin, roll1, 2 (npair x 5)
// excluded body pairs for collision detection
int* exclude_signature; // (body1+1) << 16 + body2+1 (nexclude x 1)
int* exclude_signature; // body1 << 16 + body2 (nexclude x 1)
// equality constraints
int* eq_type; // constraint type (mjtEq) (neq x 1)
@@ -1246,6 +1332,7 @@ struct mjModel_ {
int* name_siteadr; // site name pointers (nsite x 1)
int* name_camadr; // camera name pointers (ncam x 1)
int* name_lightadr; // light name pointers (nlight x 1)
int* name_flexadr; // flex name pointers (nflex x 1)
int* name_meshadr; // mesh name pointers (nmesh x 1)
int* name_skinadr; // skin name pointers (nskin x 1)
int* name_hfieldadr; // hfield name pointers (nhfield x 1)
@@ -1732,6 +1819,7 @@ typedef enum mjtLabel_ { // object labeling
mjLABEL_TENDON, // tendon labels
mjLABEL_ACTUATOR, // actuator labels
mjLABEL_CONSTRAINT, // constraint labels
mjLABEL_FLEX, // flex labels
mjLABEL_SKIN, // skin labels
mjLABEL_SELECTION, // selected object
mjLABEL_SELPNT, // coordinates of selection point
@@ -1778,7 +1866,12 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_SELECT, // selection point
mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjVIS_FLEXVERT, // flex vertices
mjVIS_FLEXEDGE, // flex edges
mjVIS_FLEXFACE, // flex element faces
mjVIS_FLEXSKIN, // flex smooth skin (disables the rest)
mjVIS_BODYBVH, // body bounding volume hierarchy
mjVIS_FLEXBVH, // flex bounding volume hierarchy
mjVIS_MESHBVH, // mesh bounding volume hierarchy
mjVIS_SDFITER, // iterations of SDF gradient descent
@@ -1805,6 +1898,7 @@ typedef enum mjtStereo_ { // type of stereo rendering
} mjtStereo;
struct mjvPerturb_ { // object selection and perturbation
int select; // selected body id; non-positive: none
int flexselect; // selected flex id; negative: none
int skinselect; // selected skin id; negative: none
int active; // perturbation bitmask (mjtPertBit)
int active2; // secondary perturbation bitmask (mjtPertBit)
@@ -1853,7 +1947,7 @@ struct mjvGeom_ { // abstract geom
int category; // visual category
int texid; // texture id; -1: no texture
int texuniform; // uniform cube mapping
int texcoord; // mesh geom has texture coordinates
int texcoord; // mesh or flex geom has texture coordinates
int segid; // segmentation id; -1: not shown
// OpenGL info
@@ -1896,9 +1990,11 @@ struct mjvOption_ { // abstract visualization options
mjtByte jointgroup[mjNGROUP]; // joint visualization by group
mjtByte tendongroup[mjNGROUP]; // tendon visualization by group
mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group
mjtByte flexgroup[mjNGROUP]; // flex visualization by group
mjtByte skingroup[mjNGROUP]; // skin visualization by group
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int flex_layer; // element layer to be visualized for 3D flex
};
typedef struct mjvOption_ mjvOption;
struct mjvScene_ { // abstract scene passed to OpenGL renderer
@@ -1908,13 +2004,32 @@ struct mjvScene_ { // abstract scene passed to OpenGL renderer
mjvGeom* geoms; // buffer for geoms (ngeom)
int* geomorder; // buffer for ordering geoms by distance to camera (ngeom)
// flex data
int nflex; // number of flexes
int* flexedgeadr; // address of flex edges (nflex)
int* flexedgenum; // number of edges in flex (nflex)
int* flexvertadr; // address of flex vertices (nflex)
int* flexvertnum; // number of vertices in flex (nflex)
int* flexfaceadr; // address of flex faces (nflex)
int* flexfacenum; // number of flex faces allocated (nflex)
int* flexfaceused; // number of flex faces currently in use (nflex)
int* flexedge; // flex edge data (2*nflexedge)
float* flexvert; // flex vertices (3*nflexvert)
float* flexface; // flex faces vertices (9*sum(flexfacenum))
float* flexnormal; // flex face normals (9*sum(flexfacenum))
float* flextexcoord; // flex face texture coordinates (6*sum(flexfacenum))
mjtByte flexvertopt; // copy of mjVIS_FLEXVERT mjvOption flag
mjtByte flexedgeopt; // copy of mjVIS_FLEXEDGE mjvOption flag
mjtByte flexfaceopt; // copy of mjVIS_FLEXFACE mjvOption flag
mjtByte flexskinopt; // copy of mjVIS_FLEXSKIN mjvOption flag
// skin data
int nskin; // number of skins
int* skinfacenum; // number of faces in skin (nskin)
int* skinvertadr; // address of skin vertices (nskin)
int* skinvertnum; // number of vertices in skin (nskin)
float* skinvert; // skin vertex data (nskin)
float* skinnormal; // skin normal data (nskin)
float* skinvert; // skin vertex data (3*nskinvert)
float* skinnormal; // skin normal data (3*nskinvert)
// OpenGL lights
int nlight; // number of lights currently in buffer
@@ -1998,6 +2113,7 @@ struct mjvSceneState_ {
int na;
int nbody;
int nbvh;
int nbvhstatic;
int njnt;
int ngeom;
int nsite;
@@ -2005,6 +2121,8 @@ struct mjvSceneState_ {
int nlight;
int nmesh;
int nskin;
int nflex;
int nflexvert;
int nskinvert;
int nskinface;
int nskinbone;
@@ -2041,7 +2159,7 @@ struct mjvSceneState_ {
int* bvh_depth;
int* bvh_child;
int* bvh_geomid;
int* bvh_nodeid;
mjtNum* bvh_aabb;
int* jnt_type;
@@ -2081,6 +2199,24 @@ struct mjvSceneState_ {
float* light_diffuse;
float* light_specular;
mjtByte* flex_flatskin;
int* flex_dim;
int* flex_matid;
int* flex_group;
int* flex_vertadr;
int* flex_vertnum;
int* flex_elem;
int* flex_elemadr;
int* flex_elemnum;
int* flex_elemdataadr;
int* flex_shell;
int* flex_shellnum;
int* flex_shelldataadr;
int* flex_bvhadr;
int* flex_bvhnum;
mjtNum* flex_radius;
float* flex_rgba;
int* mesh_bvhadr;
int* mesh_bvhnum;
int* mesh_texcoordadr;
@@ -2212,6 +2348,8 @@ struct mjvSceneState_ {
int* efc_island;
int* tendon_efcadr;
mjtNum* flexvert_xpos;
mjContact* contact;
mjtNum* efc_force;
} data;
@@ -2291,6 +2429,7 @@ void mj_checkAcc(const mjModel* m, mjData* d);
void mj_kinematics(const mjModel* m, mjData* d);
void mj_comPos(const mjModel* m, mjData* d);
void mj_camlight(const mjModel* m, mjData* d);
void mj_flex(const mjModel* m, mjData* d);
void mj_tendon(const mjModel* m, mjData* d);
void mj_transmission(const mjModel* m, mjData* d);
void mj_crb(const mjModel* m, mjData* d);
@@ -2365,6 +2504,9 @@ mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
const mjtNum pnt[3], const mjtNum vec[3]);
mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype);
mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]);
void mjv_defaultCamera(mjvCamera* cam);
@@ -2399,7 +2541,8 @@ void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert);
mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]);
void mjv_defaultOption(mjvOption* opt);
void mjv_defaultFigure(mjvFigure* fig);
void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3],
include/mujoco/mjdata.h
+21 -12
View File
@@ -97,7 +97,7 @@ typedef enum mjtTimer_ { // internal timers
//---------------------------------- mjContact -----------------------------------------------------
struct mjContact_ { // result of collision detection functions
// contact parameters set by geom-specific collision detector
// contact parameters set by near-phase collision function
mjtNum dist; // distance between nearest points; neg: penetration
mjtNum pos[3]; // position of contact point: midpoint between geoms
mjtNum frame[9]; // normal is in [0-2]
@@ -113,12 +113,16 @@ struct mjContact_ { // result of collision detection functions
mjtNum mu; // friction of regularized cone, set by mj_makeConstraint
mjtNum H[36]; // cone Hessian, set by mj_updateConstraint
// contact descriptors set by mj_collideGeoms
// contact descriptors set by mj_collideXXX
int dim; // contact space dimensionality: 1, 3, 4 or 6
int geom1; // id of geom 1
int geom2; // id of geom 2
int geom[2]; // geom ids; -1 for flex
int flex[2]; // flex ids; -1 for geom
int elem[2]; // element ids; -1 for geom or flex vertex
int vert[2]; // vertex ids; -1 for geom or flex element
// flag set by mj_instantianteEquality
// flag set by mj_setContact or mj_instantiateContact
int exclude; // 0: include, 1: in gap, 2: fused, 3: no dofs
// address computed by mj_instantiateContact
@@ -188,12 +192,6 @@ struct mjData_ {
int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island
mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc
// collision statistics
int nbodypair_broad; // number of body pairs in collision according to the broad-phase
int nbodypair_narrow; // number of body pairs actually in collision in the narrow-phase
int ngeompair_mid; // number of geom pairs in collision according to the mid-phase
int ngeompair_narrow; // number of geom pairs actually in collision in the narrow-phase
// variable sizes
int ne; // number of equality constraints
int nf; // number of friction constraints
@@ -270,14 +268,23 @@ struct mjData_ {
mjtNum* cdof; // com-based motion axis of each dof (nv x 6)
mjtNum* cinert; // com-based body inertia and mass (nbody x 10)
// computed by mj_fwdPosition/mj_flex
mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3)
mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6)
int* flexedge_J_rownnz; // number of non-zeros in Jacobian row (nflexedge x 1)
int* flexedge_J_rowadr; // row start address in colind array (nflexedge x 1)
int* flexedge_J_colind; // column indices in sparse Jacobian (nflexedge x nv)
mjtNum* flexedge_J; // flex edge Jacobian (nflexedge x nv)
mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1)
// computed by mj_fwdPosition/mj_tendon
int* ten_wrapadr; // start address of tendon's path (ntendon x 1)
int* ten_wrapnum; // number of wrap points in path (ntendon x 1)
int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1)
int* ten_J_rowadr; // row start address in colind array (ntendon x 1)
int* ten_J_colind; // column indices in sparse Jacobian (ntendon x nv)
mjtNum* ten_length; // tendon lengths (ntendon x 1)
mjtNum* ten_J; // tendon Jacobian (ntendon x nv)
mjtNum* ten_length; // tendon lengths (ntendon x 1)
int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap*2 x 1)
mjtNum* wrap_xpos; // Cartesian 3D points in all path (nwrap*2 x 3)
@@ -295,11 +302,13 @@ struct mjData_ {
mjtNum* qLDiagSqrtInv; // 1/sqrt(diag(D)) (nv x 1)
// computed by mj_collisionTree
mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6)
mjtByte* bvh_active; // volume has been added to collisions (nbvh x 1)
//-------------------- POSITION, VELOCITY dependent
// computed by mj_fwdVelocity
mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1)
mjtNum* ten_velocity; // tendon velocities (ntendon x 1)
mjtNum* actuator_velocity; // actuator velocities (nu x 1)
@@ -318,8 +327,8 @@ struct mjData_ {
mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3)
// computed by mj_Euler or mj_implicit
mjtNum* qH; // L'*D*L factorization of modified M (nM x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
mjtNum* qH; // L'*D*L factorization of modified M (nM x 1)
mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1)
// computed by mj_resetData
int* D_rownnz; // non-zeros in each row (nv x 1)
include/mujoco/mjmodel.h
+88 -8
View File
@@ -109,6 +109,7 @@ typedef enum mjtGeom_ { // type of geometric shape
mjGEOM_ARROW1, // arrow without wedges
mjGEOM_ARROW2, // arrow in both directions
mjGEOM_LINE, // line
mjGEOM_FLEX, // flex
mjGEOM_SKIN, // skin
mjGEOM_LABEL, // text label
mjGEOM_TRIANGLE, // triangle connecting a frame
@@ -166,6 +167,7 @@ typedef enum mjtEq_ { // type of equality constraint
mjEQ_WELD, // fix relative position and orientation of two bodies
mjEQ_JOINT, // couple the values of two scalar joints with cubic
mjEQ_TENDON, // couple the lengths of two tendons with cubic
mjEQ_FLEX, // fix all edge lengths of a flex
mjEQ_DISTANCE // unsupported, will cause an error if used
} mjtEq;
@@ -228,6 +230,7 @@ typedef enum mjtObj_ { // type of MujoCo object
mjOBJ_SITE, // site
mjOBJ_CAMERA, // camera
mjOBJ_LIGHT, // light
mjOBJ_FLEX, // flex
mjOBJ_MESH, // mesh
mjOBJ_SKIN, // skin
mjOBJ_HFIELD, // heightfield
@@ -353,6 +356,15 @@ typedef enum mjtLRMode_ { // mode for actuator length range computation
} mjtLRMode;
typedef enum mjtFlexSelf_ { // mode for flex selfcollide
mjFLEXSELF_NONE = 0, // no self-collisions
mjFLEXSELF_NARROW, // skip midphase, go directly to narrowphase
mjFLEXSELF_BVH, // use BVH in midphase (if midphase enabled)
mjFLEXSELF_SAP, // use SAP in midphase
mjFLEXSELF_AUTO // choose between BVH and SAP automatically
} mjtFlexSelf;
//---------------------------------- mjLROpt -------------------------------------------------------
struct mjLROpt_ { // options for mj_setLengthRange()
@@ -408,6 +420,7 @@ struct mjOption_ { // physics options
mjtNum o_margin; // margin
mjtNum o_solref[mjNREF]; // solref
mjtNum o_solimp[mjNIMP]; // solimp
mjtNum o_friction[5]; // friction
// discrete settings
int integrator; // integration mode (mjtIntegrator)
@@ -439,9 +452,9 @@ struct mjVisual_ { // visualization options
float linewidth; // line width for wireframe and ray rendering
float glow; // glow coefficient for selected body
float realtime; // initial real-time factor (1: real time)
int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
int offwidth; // width of offscreen buffer
int offheight; // height of offscreen buffer
int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid)
} global;
struct { // rendering quality
@@ -545,12 +558,22 @@ struct mjModel_ {
int nu; // number of actuators/controls = dim(ctrl)
int na; // number of activation states = dim(act)
int nbody; // number of bodies
int nbvh; // number of bounding volumes in all bodies
int nbvh; // number of total bounding volumes in all bodies
int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel)
int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData)
int njnt; // number of joints
int ngeom; // number of geoms
int nsite; // number of sites
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of 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
int nflexelemdata; // number of element vertex ids in all flexes
int nflexshelldata; // number of shell fragment vertex ids in all flexes
int nflexevpair; // number of element-vertex pairs in all flexes
int nflextexcoord; // number of vertices with texture coordinates
int nmesh; // number of meshes
int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes
@@ -649,16 +672,19 @@ struct mjModel_ {
mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3)
mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2)
mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1)
mjtNum* body_margin; // MAX over all geom margins (nbody x 1)
mjtNum* body_user; // user data (nbody x nuser_body)
int* body_plugin; // plugin instance id; -1: not in use (nbody x 1)
int* body_contype; // OR over all geom contypes (nbody x 1)
int* body_conaffinity; // OR over all geom conaffinities (nbody x 1)
int* body_bvhadr; // address of bvh root (nbody x 1)
int* body_bvhnum; // number of bounding volumes (nbody x 1)
// bounding volume hierarchy
int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1)
int* bvh_child; // left and right children in tree (nbvh x 2)
int* bvh_geomid; // geom id of the node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // bounding box of node (center, size) (nbvh x 6)
int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1)
mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6)
// joints
int* jnt_type; // type of joint (mjtJoint) (njnt x 1)
@@ -767,6 +793,59 @@ struct mjModel_ {
float* light_diffuse; // diffuse rgb (alpha=1) (nlight x 3)
float* light_specular; // specular rgb (alpha=1) (nlight x 3)
// flexes: contact properties
int* flex_contype; // flex contact type (nflex x 1)
int* flex_conaffinity; // flex contact affinity (nflex x 1)
int* flex_condim; // contact dimensionality (1, 3, 4, 6) (nflex x 1)
int* flex_priority; // flex contact priority (nflex x 1)
mjtNum* flex_solmix; // mix coef for solref/imp in contact pair (nflex x 1)
mjtNum* flex_solref; // constraint solver reference: contact (nflex x mjNREF)
mjtNum* flex_solimp; // constraint solver impedance: contact (nflex x mjNIMP)
mjtNum* flex_friction; // friction for (slide, spin, roll) (nflex x 3)
mjtNum* flex_margin; // detect contact if dist<margin (nflex x 1)
mjtNum* flex_gap; // include in solver if dist<margin-gap (nflex x 1)
mjtByte* flex_internal; // internal flex collision enabled (nflex x 1)
int* flex_selfcollide; // self collision mode (mjtFlexSelf) (nflex x 1)
int* flex_activelayers; // number of active element layers, 3D only (nflex x 1)
// flexes: other properties
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_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)
int* flex_edgenum; // number of edges (nflex x 1)
int* flex_elemadr; // first element address (nflex x 1)
int* flex_elemnum; // number of elements (nflex x 1)
int* flex_elemdataadr; // first element vertex id address (nflex x 1)
int* flex_shellnum; // number of shells (nflex x 1)
int* flex_shelldataadr; // first shell data address (nflex x 1)
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_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)
int* flex_elemlayer; // element distance from surface, 3D only (nflexelem x 1)
int* flex_shell; // shell fragment vertex ids (dim per frag) (nflexshelldata x 1)
int* flex_evpair; // (element, vertex) collision pairs (nflexevpair x 2)
mjtNum* flex_vert; // vertex positions in local body frames (nflexvert x 3)
mjtNum* flex_xvert0; // Cartesian vertex positions in qpos0 (nflexvert 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)
mjtNum* flex_edgestiffness; // edge stiffness (nflex x 1)
mjtNum* flex_edgedamping; // edge damping (nflex x 1)
mjtByte* flex_edgeequality; // is edge equality constraint defined (nflex x 1)
mjtByte* flex_rigid; // are all verices in the same body (nflex x 1)
mjtByte* flex_centered; // are all vertex coordinates (0,0,0) (nflex x 1)
mjtByte* flex_flatskin; // render flex skin with flat shading (nflex x 1)
int* flex_bvhadr; // address of bvh root; -1: no bvh (nflex x 1)
int* flex_bvhnum; // number of bounding volumes (nflex x 1)
float* flex_rgba; // rgba when material is omitted (nflex x 4)
float* flex_texcoord; // vertex texture coordinates (nflextexcoord x 2)
// meshes
int* mesh_vertadr; // first vertex address (nmesh x 1)
int* mesh_vertnum; // number of vertices (nmesh x 1)
@@ -841,7 +920,7 @@ struct mjModel_ {
int* pair_dim; // contact dimensionality (npair x 1)
int* pair_geom1; // id of geom1 (npair x 1)
int* pair_geom2; // id of geom2 (npair x 1)
int* pair_signature; // (body1+1) << 16 + body2+1 (npair x 1)
int* pair_signature; // body1 << 16 + body2 (npair x 1)
mjtNum* pair_solref; // solver reference: contact normal (npair x mjNREF)
mjtNum* pair_solreffriction; // solver reference: contact friction (npair x mjNREF)
mjtNum* pair_solimp; // solver impedance: contact (npair x mjNIMP)
@@ -850,7 +929,7 @@ struct mjModel_ {
mjtNum* pair_friction; // tangent1, 2, spin, roll1, 2 (npair x 5)
// excluded body pairs for collision detection
int* exclude_signature; // (body1+1) << 16 + body2+1 (nexclude x 1)
int* exclude_signature; // body1 << 16 + body2 (nexclude x 1)
// equality constraints
int* eq_type; // constraint type (mjtEq) (neq x 1)
@@ -970,6 +1049,7 @@ struct mjModel_ {
int* name_siteadr; // site name pointers (nsite x 1)
int* name_camadr; // camera name pointers (ncam x 1)
int* name_lightadr; // light name pointers (nlight x 1)
int* name_flexadr; // flex name pointers (nflex x 1)
int* name_meshadr; // mesh name pointers (nmesh x 1)
int* name_skinadr; // skin name pointers (nskin x 1)
int* name_hfieldadr; // hfield name pointers (nhfield x 1)
include/mujoco/mjvisualize.h
+57 -6
View File
@@ -74,6 +74,7 @@ typedef enum mjtLabel_ { // object labeling
mjLABEL_TENDON, // tendon labels
mjLABEL_ACTUATOR, // actuator labels
mjLABEL_CONSTRAINT, // constraint labels
mjLABEL_FLEX, // flex labels
mjLABEL_SKIN, // skin labels
mjLABEL_SELECTION, // selected object
mjLABEL_SELPNT, // coordinates of selection point
@@ -124,7 +125,12 @@ typedef enum mjtVisFlag_ { // flags enabling model element visualization
mjVIS_SELECT, // selection point
mjVIS_STATIC, // static bodies
mjVIS_SKIN, // skin
mjVIS_MIDPHASE, // mid-phase bounding volume hierarchy
mjVIS_FLEXVERT, // flex vertices
mjVIS_FLEXEDGE, // flex edges
mjVIS_FLEXFACE, // flex element faces
mjVIS_FLEXSKIN, // flex smooth skin (disables the rest)
mjVIS_BODYBVH, // body bounding volume hierarchy
mjVIS_FLEXBVH, // flex bounding volume hierarchy
mjVIS_MESHBVH, // mesh bounding volume hierarchy
mjVIS_SDFITER, // iterations of SDF gradient descent
@@ -159,6 +165,7 @@ typedef enum mjtStereo_ { // type of stereo rendering
struct mjvPerturb_ { // object selection and perturbation
int select; // selected body id; non-positive: none
int flexselect; // selected flex id; negative: none
int skinselect; // selected skin id; negative: none
int active; // perturbation bitmask (mjtPertBit)
int active2; // secondary perturbation bitmask (mjtPertBit)
@@ -219,7 +226,7 @@ struct mjvGeom_ { // abstract geom
int category; // visual category
int texid; // texture id; -1: no texture
int texuniform; // uniform cube mapping
int texcoord; // mesh geom has texture coordinates
int texcoord; // mesh or flex geom has texture coordinates
int segid; // segmentation id; -1: not shown
// OpenGL info
@@ -270,9 +277,11 @@ struct mjvOption_ { // abstract visualization options
mjtByte jointgroup[mjNGROUP]; // joint visualization by group
mjtByte tendongroup[mjNGROUP]; // tendon visualization by group
mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group
mjtByte flexgroup[mjNGROUP]; // flex visualization by group
mjtByte skingroup[mjNGROUP]; // skin visualization by group
mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag)
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int bvh_depth; // depth of the bounding volume hierarchy to be visualized
int flex_layer; // element layer to be visualized for 3D flex
};
typedef struct mjvOption_ mjvOption;
@@ -286,13 +295,32 @@ struct mjvScene_ { // abstract scene passed to OpenGL renderer
mjvGeom* geoms; // buffer for geoms (ngeom)
int* geomorder; // buffer for ordering geoms by distance to camera (ngeom)
// flex data
int nflex; // number of flexes
int* flexedgeadr; // address of flex edges (nflex)
int* flexedgenum; // number of edges in flex (nflex)
int* flexvertadr; // address of flex vertices (nflex)
int* flexvertnum; // number of vertices in flex (nflex)
int* flexfaceadr; // address of flex faces (nflex)
int* flexfacenum; // number of flex faces allocated (nflex)
int* flexfaceused; // number of flex faces currently in use (nflex)
int* flexedge; // flex edge data (2*nflexedge)
float* flexvert; // flex vertices (3*nflexvert)
float* flexface; // flex faces vertices (9*sum(flexfacenum))
float* flexnormal; // flex face normals (9*sum(flexfacenum))
float* flextexcoord; // flex face texture coordinates (6*sum(flexfacenum))
mjtByte flexvertopt; // copy of mjVIS_FLEXVERT mjvOption flag
mjtByte flexedgeopt; // copy of mjVIS_FLEXEDGE mjvOption flag
mjtByte flexfaceopt; // copy of mjVIS_FLEXFACE mjvOption flag
mjtByte flexskinopt; // copy of mjVIS_FLEXSKIN mjvOption flag
// skin data
int nskin; // number of skins
int* skinfacenum; // number of faces in skin (nskin)
int* skinvertadr; // address of skin vertices (nskin)
int* skinvertnum; // number of vertices in skin (nskin)
float* skinvert; // skin vertex data (nskin)
float* skinnormal; // skin normal data (nskin)
float* skinvert; // skin vertex data (3*nskinvert)
float* skinnormal; // skin normal data (3*nskinvert)
// OpenGL lights
int nlight; // number of lights currently in buffer
@@ -384,6 +412,7 @@ struct mjvSceneState_ {
int na;
int nbody;
int nbvh;
int nbvhstatic;
int njnt;
int ngeom;
int nsite;
@@ -391,6 +420,8 @@ struct mjvSceneState_ {
int nlight;
int nmesh;
int nskin;
int nflex;
int nflexvert;
int nskinvert;
int nskinface;
int nskinbone;
@@ -427,7 +458,7 @@ struct mjvSceneState_ {
int* bvh_depth;
int* bvh_child;
int* bvh_geomid;
int* bvh_nodeid;
mjtNum* bvh_aabb;
int* jnt_type;
@@ -467,6 +498,24 @@ struct mjvSceneState_ {
float* light_diffuse;
float* light_specular;
mjtByte* flex_flatskin;
int* flex_dim;
int* flex_matid;
int* flex_group;
int* flex_vertadr;
int* flex_vertnum;
int* flex_elem;
int* flex_elemadr;
int* flex_elemnum;
int* flex_elemdataadr;
int* flex_shell;
int* flex_shellnum;
int* flex_shelldataadr;
int* flex_bvhadr;
int* flex_bvhnum;
mjtNum* flex_radius;
float* flex_rgba;
int* mesh_bvhadr;
int* mesh_bvhnum;
int* mesh_texcoordadr;
@@ -598,6 +647,8 @@ struct mjvSceneState_ {
int* efc_island;
int* tendon_efcadr;
mjtNum* flexvert_xpos;
mjContact* contact;
mjtNum* efc_force;
} data;
include/mujoco/mjxmacro.h
+76 -7
View File
@@ -58,7 +58,8 @@
X( wind, 3 ) \
X( magnetic, 3 ) \
X( o_solref, mjNREF ) \
X( o_solimp, mjNIMP )
X( o_solimp, mjNIMP ) \
X( o_friction, 5 )
//-------------------------------- mjModel ---------------------------------------------------------
@@ -71,11 +72,21 @@
XMJV( na ) \
XMJV( nbody ) \
XMJV( nbvh ) \
XMJV( nbvhstatic ) \
X ( nbvhdynamic ) \
XMJV( njnt ) \
XMJV( ngeom ) \
XMJV( nsite ) \
XMJV( ncam ) \
XMJV( nlight ) \
XMJV( nflex ) \
XMJV( nflexvert ) \
X ( nflexedge ) \
X ( nflexelem ) \
X ( nflexelemdata ) \
X ( nflexshelldata ) \
X ( nflexevpair ) \
X ( nflextexcoord ) \
XMJV( nmesh ) \
X ( nmeshvert ) \
X ( nmeshnormal ) \
@@ -185,14 +196,17 @@
XMJV( mjtNum, body_inertia, nbody, 3 ) \
X ( mjtNum, body_invweight0, nbody, 2 ) \
X ( mjtNum, body_gravcomp, nbody, 1 ) \
X ( mjtNum, body_margin, nbody, 1 ) \
X ( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \
X ( int, body_plugin, nbody, 1 ) \
X ( int, body_contype, nbody, 1 ) \
X ( int, body_conaffinity, nbody, 1 ) \
XMJV( int, body_bvhadr, nbody, 1 ) \
XMJV( int, body_bvhnum, nbody, 1 ) \
XMJV( int, bvh_depth, nbvh, 1 ) \
XMJV( int, bvh_child, nbvh, 2 ) \
XMJV( int, bvh_geomid, nbvh, 1 ) \
XMJV( mjtNum, bvh_aabb, nbvh, 6 ) \
XMJV( int, bvh_nodeid, nbvh, 1 ) \
XMJV( mjtNum, bvh_aabb, nbvhstatic, 6 ) \
XMJV( int, jnt_type, njnt, 1 ) \
X ( int, jnt_qposadr, njnt, 1 ) \
X ( int, jnt_dofadr, njnt, 1 ) \
@@ -288,6 +302,55 @@
XMJV( float, light_ambient, nlight, 3 ) \
XMJV( float, light_diffuse, nlight, 3 ) \
XMJV( float, light_specular, nlight, 3 ) \
X ( int, flex_contype, nflex, 1 ) \
X ( int, flex_conaffinity, nflex, 1 ) \
X ( int, flex_condim, nflex, 1 ) \
X ( int, flex_priority, nflex, 1 ) \
X ( mjtNum, flex_solmix, nflex, 1 ) \
X ( mjtNum, flex_solref, nflex, mjNREF ) \
X ( mjtNum, flex_solimp, nflex, mjNIMP ) \
X ( mjtNum, flex_friction, nflex, 3 ) \
X ( mjtNum, flex_margin, nflex, 1 ) \
X ( mjtNum, flex_gap, nflex, 1 ) \
X ( mjtByte, flex_internal, nflex, 1 ) \
X ( int, flex_selfcollide, nflex, 1 ) \
X ( int, flex_activelayers, nflex, 1 ) \
XMJV( int, flex_dim, nflex, 1 ) \
XMJV( int, flex_matid, nflex, 1 ) \
XMJV( int, flex_group, nflex, 1 ) \
XMJV( int, flex_vertadr, nflex, 1 ) \
XMJV( int, flex_vertnum, nflex, 1 ) \
X ( int, flex_edgeadr, nflex, 1 ) \
X ( int, flex_edgenum, nflex, 1 ) \
XMJV( int, flex_elemadr, nflex, 1 ) \
XMJV( int, flex_elemnum, nflex, 1 ) \
XMJV( int, flex_elemdataadr, nflex, 1 ) \
XMJV( int, flex_shellnum, nflex, 1 ) \
XMJV( int, flex_shelldataadr, nflex, 1 ) \
X ( int, flex_evpairadr, nflex, 1 ) \
X ( int, flex_evpairnum, nflex, 1 ) \
X ( int, flex_texcoordadr, nflex, 1 ) \
X ( int, flex_vertbodyid, nflexvert, 1 ) \
X ( int, flex_edge, nflexedge, 2 ) \
XMJV( int, flex_elem, nflexelemdata, 1 ) \
X ( int, flex_elemlayer, nflexelem, 1 ) \
XMJV( int, flex_shell, nflexshelldata,1 ) \
X ( int, flex_evpair, nflexevpair, 2 ) \
X ( mjtNum, flex_vert, nflexvert, 3 ) \
X ( mjtNum, flex_xvert0, nflexvert, 3 ) \
X ( mjtNum, flexedge_length0, nflexedge, 1 ) \
X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \
XMJV( mjtNum, flex_radius, nflex, 1 ) \
X ( mjtNum, flex_edgestiffness, nflex, 1 ) \
X ( mjtNum, flex_edgedamping, nflex, 1 ) \
X ( mjtByte, flex_edgeequality, nflex, 1 ) \
X ( mjtByte, flex_rigid, nflex, 1 ) \
X ( mjtByte, flex_centered, nflex, 1 ) \
XMJV( mjtByte, flex_flatskin, nflex, 1 ) \
XMJV( int, flex_bvhadr, nflex, 1 ) \
XMJV( int, flex_bvhnum, nflex, 1 ) \
XMJV( float, flex_rgba, nflex, 4 ) \
X ( float, flex_texcoord, nflextexcoord, 2 ) \
X ( int, mesh_vertadr, nmesh, 1 ) \
X ( int, mesh_vertnum, nmesh, 1 ) \
X ( int, mesh_normaladr, nmesh, 1 ) \
@@ -456,6 +519,7 @@
XMJV( int, name_siteadr, nsite, 1 ) \
XMJV( int, name_camadr, ncam, 1 ) \
XMJV( int, name_lightadr, nlight, 1 ) \
X ( int, name_flexadr, nflex, 1 ) \
X ( int, name_meshadr, nmesh, 1 ) \
X ( int, name_skinadr, nskin, 1 ) \
X ( int, name_hfieldadr, nhfield, 1 ) \
@@ -522,6 +586,13 @@
XMJV( mjtNum, subtree_com, nbody, 3 ) \
X ( mjtNum, cdof, nv, 6 ) \
X ( mjtNum, cinert, nbody, 10 ) \
XMJV( mjtNum, flexvert_xpos, nflexvert, 3 ) \
X ( mjtNum, flexelem_aabb, nflexelem, 6 ) \
X ( int, flexedge_J_rownnz, nflexedge, 1 ) \
X ( int, flexedge_J_rowadr, nflexedge, 1 ) \
X ( int, flexedge_J_colind, nflexedge, MJ_M(nv) ) \
X ( mjtNum, flexedge_J, nflexedge, MJ_M(nv) ) \
X ( mjtNum, flexedge_length, nflexedge, 1 ) \
XMJV( int, ten_wrapadr, ntendon, 1 ) \
XMJV( int, ten_wrapnum, ntendon, 1 ) \
X ( int, ten_J_rownnz, ntendon, 1 ) \
@@ -538,7 +609,9 @@
X ( mjtNum, qLD, nM, 1 ) \
X ( mjtNum, qLDiagInv, nv, 1 ) \
X ( mjtNum, qLDiagSqrtInv, nv, 1 ) \
X ( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \
XMJV( mjtByte, bvh_active, nbvh, 1 ) \
X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \
X ( mjtNum, ten_velocity, ntendon, 1 ) \
X ( mjtNum, actuator_velocity, nu, 1 ) \
X ( mjtNum, cvel, nbody, 6 ) \
@@ -644,10 +717,6 @@
X( int, maxuse_con ) \
X( int, maxuse_efc ) \
X( int, solver_nisland ) \
X( int, nbodypair_broad ) \
X( int, nbodypair_narrow ) \
X( int, ngeompair_mid ) \
X( int, ngeompair_narrow ) \
X( int, ne ) \
X( int, nf ) \
X( int, nl ) \
include/mujoco/mujoco.h
+12 -2
View File
@@ -312,6 +312,9 @@ MJAPI void mj_comPos(const mjModel* m, mjData* d);
// Compute camera and light positions and orientations.
MJAPI void mj_camlight(const mjModel* m, mjData* d);
// Compute flex-related quantities.
MJAPI void mj_flex(const mjModel* m, mjData* d);
// Compute tendon lengths, velocities and moment arms.
MJAPI void mj_tendon(const mjModel* m, mjData* d);
@@ -518,6 +521,12 @@ MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3],
const mjtNum pnt[3], const mjtNum vec[3], int geomtype);
// Intersect ray with flex, return nearest distance or -1 if no intersection,
// and also output nearest vertex id.
MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
// Intersect ray with skin, return nearest distance or -1 if no intersection,
// and also output nearest vertex id.
MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
@@ -593,10 +602,11 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
// Return the average of two OpenGL cameras.
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
// Select geom or skin with mouse, return bodyid; -1: none selected.
// Select geom, flex or skin with mouse, return bodyid; -1: none selected.
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]);
//---------------------------------- Visualization -------------------------------------------------
introspect/enums.py
+52 -31
View File
@@ -90,9 +90,10 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjGEOM_ARROW1', 101),
('mjGEOM_ARROW2', 102),
('mjGEOM_LINE', 103),
('mjGEOM_SKIN', 104),
('mjGEOM_LABEL', 105),
('mjGEOM_TRIANGLE', 106),
('mjGEOM_FLEX', 104),
('mjGEOM_SKIN', 105),
('mjGEOM_LABEL', 106),
('mjGEOM_TRIANGLE', 107),
('mjGEOM_NONE', 1001),
]),
)),
@@ -167,7 +168,8 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjEQ_WELD', 1),
('mjEQ_JOINT', 2),
('mjEQ_TENDON', 3),
('mjEQ_DISTANCE', 4),
('mjEQ_FLEX', 4),
('mjEQ_DISTANCE', 5),
]),
)),
('mjtWrap',
@@ -246,22 +248,23 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjOBJ_SITE', 6),
('mjOBJ_CAMERA', 7),
('mjOBJ_LIGHT', 8),
('mjOBJ_MESH', 9),
('mjOBJ_SKIN', 10),
('mjOBJ_HFIELD', 11),
('mjOBJ_TEXTURE', 12),
('mjOBJ_MATERIAL', 13),
('mjOBJ_PAIR', 14),
('mjOBJ_EXCLUDE', 15),
('mjOBJ_EQUALITY', 16),
('mjOBJ_TENDON', 17),
('mjOBJ_ACTUATOR', 18),
('mjOBJ_SENSOR', 19),
('mjOBJ_NUMERIC', 20),
('mjOBJ_TEXT', 21),
('mjOBJ_TUPLE', 22),
('mjOBJ_KEY', 23),
('mjOBJ_PLUGIN', 24),
('mjOBJ_FLEX', 9),
('mjOBJ_MESH', 10),
('mjOBJ_SKIN', 11),
('mjOBJ_HFIELD', 12),
('mjOBJ_TEXTURE', 13),
('mjOBJ_MATERIAL', 14),
('mjOBJ_PAIR', 15),
('mjOBJ_EXCLUDE', 16),
('mjOBJ_EQUALITY', 17),
('mjOBJ_TENDON', 18),
('mjOBJ_ACTUATOR', 19),
('mjOBJ_SENSOR', 20),
('mjOBJ_NUMERIC', 21),
('mjOBJ_TEXT', 22),
('mjOBJ_TUPLE', 23),
('mjOBJ_KEY', 24),
('mjOBJ_PLUGIN', 25),
]),
)),
('mjtConstraint',
@@ -371,6 +374,18 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjLRMODE_ALL', 3),
]),
)),
('mjtFlexSelf',
EnumDecl(
name='mjtFlexSelf',
declname='enum mjtFlexSelf_',
values=dict([
('mjFLEXSELF_NONE', 0),
('mjFLEXSELF_NARROW', 1),
('mjFLEXSELF_BVH', 2),
('mjFLEXSELF_SAP', 3),
('mjFLEXSELF_AUTO', 4),
]),
)),
('mjtTaskStatus',
EnumDecl(
name='mjtTaskStatus',
@@ -506,13 +521,14 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjLABEL_TENDON', 7),
('mjLABEL_ACTUATOR', 8),
('mjLABEL_CONSTRAINT', 9),
('mjLABEL_SKIN', 10),
('mjLABEL_SELECTION', 11),
('mjLABEL_SELPNT', 12),
('mjLABEL_CONTACTPOINT', 13),
('mjLABEL_CONTACTFORCE', 14),
('mjLABEL_ISLAND', 15),
('mjNLABEL', 16),
('mjLABEL_FLEX', 10),
('mjLABEL_SKIN', 11),
('mjLABEL_SELECTION', 12),
('mjLABEL_SELPNT', 13),
('mjLABEL_CONTACTPOINT', 14),
('mjLABEL_CONTACTFORCE', 15),
('mjLABEL_ISLAND', 16),
('mjNLABEL', 17),
]),
)),
('mjtFrame',
@@ -560,10 +576,15 @@ ENUMS: Mapping[str, EnumDecl] = dict([
('mjVIS_SELECT', 21),
('mjVIS_STATIC', 22),
('mjVIS_SKIN', 23),
('mjVIS_MIDPHASE', 24),
('mjVIS_MESHBVH', 25),
('mjVIS_SDFITER', 26),
('mjNVISFLAG', 27),
('mjVIS_FLEXVERT', 24),
('mjVIS_FLEXEDGE', 25),
('mjVIS_FLEXFACE', 26),
('mjVIS_FLEXSKIN', 27),
('mjVIS_BODYBVH', 28),
('mjVIS_FLEXBVH', 29),
('mjVIS_MESHBVH', 30),
('mjVIS_SDFITER', 31),
('mjNVISFLAG', 32),
]),
)),
('mjtRndFlag',
introspect/enums_test.py
+1 -1
View File
@@ -61,7 +61,7 @@ class EnumsTest(absltest.TestCase):
self.assertEqual(enum_decl.values['mjGEOM_ARROW'], 100)
self.assertEqual(enum_decl.values['mjGEOM_ARROW1'], 101)
self.assertEqual(enum_decl.values['mjGEOM_ARROW2'], 102)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 106)
self.assertEqual(enum_decl.values['mjGEOM_TRIANGLE'], 107)
# Skip a few...
self.assertEqual(enum_decl.values['mjGEOM_NONE'], 1001)
introspect/functions.py
+91 -1
View File
@@ -1456,6 +1456,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Compute camera and light positions and orientations.',
)),
('mj_flex',
FunctionDecl(
name='mj_flex',
return_type=ValueType(name='void'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData'),
),
),
),
doc='Compute flex-related quantities.',
)),
('mj_tendon',
FunctionDecl(
name='mj_tendon',
@@ -3161,6 +3181,69 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
doc='Intersect ray with pure geom, return nearest distance or -1 if no intersection.', # pylint: disable=line-too-long
)),
('mju_rayFlex',
FunctionDecl(
name='mju_rayFlex',
return_type=ValueType(name='mjtNum'),
parameters=(
FunctionParameterDecl(
name='m',
type=PointerType(
inner_type=ValueType(name='mjModel', is_const=True),
),
),
FunctionParameterDecl(
name='d',
type=PointerType(
inner_type=ValueType(name='mjData', is_const=True),
),
),
FunctionParameterDecl(
name='flex_layer',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='flg_vert',
type=ValueType(name='mjtByte'),
),
FunctionParameterDecl(
name='flg_edge',
type=ValueType(name='mjtByte'),
),
FunctionParameterDecl(
name='flg_face',
type=ValueType(name='mjtByte'),
),
FunctionParameterDecl(
name='flg_skin',
type=ValueType(name='mjtByte'),
),
FunctionParameterDecl(
name='flexid',
type=ValueType(name='int'),
),
FunctionParameterDecl(
name='pnt',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
),
FunctionParameterDecl(
name='vec',
type=PointerType(
inner_type=ValueType(name='mjtNum', is_const=True),
),
),
FunctionParameterDecl(
name='vertid',
type=ArrayType(
inner_type=ValueType(name='int'),
extents=(1,),
),
),
),
doc='Intersect ray with flex, return nearest distance or -1 if no intersection, and also output nearest vertex id.', # pylint: disable=line-too-long
)),
('mju_raySkin',
FunctionDecl(
name='mju_raySkin',
@@ -3815,6 +3898,13 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
extents=(1,),
),
),
FunctionParameterDecl(
name='flexid',
type=ArrayType(
inner_type=ValueType(name='int'),
extents=(1,),
),
),
FunctionParameterDecl(
name='skinid',
type=ArrayType(
@@ -3823,7 +3913,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([
),
),
),
doc='Select geom or skin with mouse, return bodyid; -1: none selected.', # pylint: disable=line-too-long
doc='Select geom, flex or skin with mouse, return bodyid; -1: none selected.', # pylint: disable=line-too-long
)),
('mjv_defaultOption',
FunctionDecl(
introspect/structs.py
+810 -38
View File
File diff suppressed because it is too large Load Diff
python/mujoco/bindings_test.py
+1 -1
View File
@@ -829,7 +829,7 @@ Euler integrator, semi-implicit in velocity.
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW, 100)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW1, 101)
self.assertEqual(mujoco.mjtGeom.mjGEOM_ARROW2, 102)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 106)
self.assertEqual(mujoco.mjtGeom.mjGEOM_TRIANGLE, 107)
self.assertEqual(mujoco.mjtGeom.mjGEOM_NONE, 1001)
def test_enum_from_int(self):
python/mujoco/functions.cc
+2
View File
@@ -199,6 +199,7 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mj_kinematics>(pymodule);
Def<traits::mj_comPos>(pymodule);
Def<traits::mj_camlight>(pymodule);
Def<traits::mj_flex>(pymodule);
Def<traits::mj_tendon>(pymodule);
Def<traits::mj_transmission>(pymodule);
Def<traits::mj_crb>(pymodule);
@@ -581,6 +582,7 @@ PYBIND11_MODULE(_functions, pymodule) {
Def<traits::mj_rayHfield>(pymodule);
Def<traits::mj_rayMesh>(pymodule);
Def<traits::mju_rayGeom>(pymodule);
Def<traits::mju_rayFlex>(pymodule);
Def<traits::mju_raySkin>(pymodule);
// Interaction
python/mujoco/structs.cc
+109 -2
View File
@@ -490,7 +490,11 @@ MjContactWrapper::MjWrapper()
X(solref),
X(solreffriction),
X(solimp),
X(H) {}
X(H),
X(geom),
X(flex),
X(elem),
X(vert) {}
MjContactWrapper::MjWrapper(raw::MjContact* ptr, py::handle owner)
: WrapperBase(ptr, owner),
@@ -500,7 +504,11 @@ MjContactWrapper::MjWrapper(raw::MjContact* ptr, py::handle owner)
X(solref),
X(solreffriction),
X(solimp),
X(H) {}
X(H),
X(geom),
X(flex),
X(elem),
X(vert) {}
#undef X
MjContactWrapper::MjWrapper(const MjContactWrapper& other)
@@ -1110,6 +1118,7 @@ MjvOptionWrapper::MjWrapper()
X(jointgroup),
X(tendongroup),
X(actuatorgroup),
X(flexgroup),
X(skingroup),
X(flags) {}
#undef X
@@ -1141,6 +1150,18 @@ MjvSceneWrapper::MjWrapper()
nskinvert(0),
XN(geoms, 0),
XN(geomorder, 0),
XN(flexedgeadr, 0),
XN(flexedgenum, 0),
XN(flexvertadr, 0),
XN(flexvertnum, 0),
XN(flexfaceadr, 0),
XN(flexfacenum, 0),
XN(flexfaceused, 0),
XN(flexedge, 0),
XN(flexvert, 0),
XN(flexface, 0),
XN(flexnormal, 0),
XN(flextexcoord, 0),
XN(skinfacenum, 0),
XN(skinvertadr, 0),
XN(skinvertnum, 0),
@@ -1170,8 +1191,54 @@ MjvSceneWrapper::MjWrapper(const MjModelWrapper& model, int maxgeom)
}
return nskinvert;
}(model.get())),
nflexface([](const raw::MjModel* m) {
int nflexface = 0;
int flexfacenum = 0;
for (int f=0; f < m->nflex; f++) {
if (m->flex_dim[f] == 0) {
// 1D : 0
flexfacenum = 0;
} else if (m->flex_dim[f] == 2) {
// 2D: 2*fragments + 2*elements
flexfacenum = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
} else {
// 3D: max(fragments, 4*maxlayer)
// find number of elements in biggest layer
int maxlayer = 0, layer = 0, nlayer = 1;
while (nlayer) {
nlayer = 0;
for (int e=0; e < m->flex_elemnum[f]; e++) {
if (m->flex_elemlayer[m->flex_elemadr[f]+e] == layer) {
nlayer++;
}
}
maxlayer = mjMAX(maxlayer, nlayer);
layer++;
}
flexfacenum = mjMAX(m->flex_shellnum[f], 4*maxlayer);
}
// accumulate over flexes
nflexface += flexfacenum;
}
return nflexface;
}(model.get())),
nflexedge(model.get()->nflexedge),
nflexvert(model.get()->nflexvert),
XN(geoms, ptr_->maxgeom),
XN(geomorder, ptr_->maxgeom),
XN(flexedgeadr, ptr_->nflex),
XN(flexedgenum, ptr_->nflex),
XN(flexvertadr, ptr_->nflex),
XN(flexvertnum, ptr_->nflex),
XN(flexfaceadr, ptr_->nflex),
XN(flexfacenum, ptr_->nflex),
XN(flexfaceused, ptr_->nflex),
XN(flexedge, 2*nflexedge),
XN(flexvert, 3*nflexvert),
XN(flexface, 9*nflexface),
XN(flexnormal, 9*nflexface),
XN(flextexcoord, 6*nflexface),
XN(skinfacenum, ptr_->nskin),
XN(skinvertadr, ptr_->nskin),
XN(skinvertnum, ptr_->nskin),
@@ -1208,6 +1275,18 @@ MjvSceneWrapper::MjWrapper(const MjvSceneWrapper& other)
XN(geoms, ptr_->ngeom);
XN(geomorder, ptr_->ngeom);
XN(flexedgeadr, ptr_->nflex);
XN(flexedgenum, ptr_->nflex);
XN(flexvertadr, ptr_->nflex);
XN(flexvertnum, ptr_->nflex);
XN(flexfaceadr, ptr_->nflex);
XN(flexfacenum, ptr_->nflex);
XN(flexfaceused, ptr_->nflex);
XN(flexedge, 2*nflexedge);
XN(flexvert, 3*nflexvert);
XN(flexface, 9*nflexface);
XN(flexnormal, 9*nflexface);
XN(flextexcoord, 6*nflexface);
XN(skinfacenum, ptr_->nskin);
XN(skinvertadr, ptr_->nskin);
XN(skinvertnum, ptr_->nskin);
@@ -1756,6 +1835,10 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(solreffriction);
X(solimp);
X(H);
X(geom);
X(flex);
X(elem);
X(vert);
#undef X
py::class_<MjContactList> mjContactList(m, "_MjContactList");
@@ -1794,6 +1877,10 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(int, geom2);
X(int, exclude);
X(int, efc_address);
XN(int, geom);
XN(int, flex);
XN(int, elem);
XN(int, vert);
#undef X
#undef XN
@@ -1985,6 +2072,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
c.get()->var = rhs; \
})
X(select);
X(flexselect);
X(skinselect);
X(active);
X(active2);
@@ -2154,6 +2242,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(label);
X(frame);
X(bvh_depth);
X(flex_layer);
#undef X
#define X(var) DefinePyArray(mjvOption, #var, &MjvOptionWrapper::var)
@@ -2162,6 +2251,7 @@ This is useful for example when the MJB is not available as a file on disk.)"));
X(jointgroup);
X(tendongroup);
X(actuatorgroup);
X(flexgroup);
X(skingroup);
X(flags);
#undef X
@@ -2185,8 +2275,13 @@ This is useful for example when the MJB is not available as a file on disk.)"));
})
X(maxgeom);
X(ngeom);
X(nflex);
X(nskin);
X(nlight);
X(flexvertopt);
X(flexedgeopt);
X(flexfaceopt);
X(flexskinopt);
X(enabletransform);
X(scale);
X(stereo);
@@ -2196,6 +2291,18 @@ This is useful for example when the MJB is not available as a file on disk.)"));
#define X(var) DefinePyArray(mjvScene, #var, &MjvSceneWrapper::var)
X(geoms);
X(geomorder);
X(flexedgeadr);
X(flexedgenum);
X(flexvertadr);
X(flexvertnum);
X(flexfaceadr);
X(flexfacenum);
X(flexfaceused);
X(flexedge);
X(flexvert);
X(flexface);
X(flexnormal);
X(flextexcoord);
X(skinfacenum);
X(skinvertadr);
X(skinvertnum);
python/mujoco/structs.h
+18
View File
@@ -508,6 +508,10 @@ class MjWrapper<raw::MjContact> : public WrapperBase<raw::MjContact> {
X(solreffriction);
X(solimp);
X(H);
X(geom);
X(flex);
X(elem);
X(vert);
#undef X
};
@@ -733,6 +737,7 @@ class MjWrapper<raw::MjvOption> : public WrapperBase<raw::MjvOption> {
X(jointgroup);
X(tendongroup);
X(actuatorgroup);
X(flexgroup);
X(skingroup);
X(flags);
#undef X
@@ -754,10 +759,23 @@ class MjWrapper<raw::MjvScene> : public WrapperBase<raw::MjvScene> {
~MjWrapper() = default;
int nskinvert;
int nflexface, nflexedge, nflexvert;
#define X(dtype, var) py_array_or_tuple_t<dtype> var
X(mjvGeom, geoms);
X(int, geomorder);
X(int, flexedgeadr);
X(int, flexedgenum);
X(int, flexvertadr);
X(int, flexvertnum);
X(int, flexfaceadr);
X(int, flexfacenum);
X(int, flexfaceused);
X(int, flexedge);
X(float, flexvert);
X(float, flexface);
X(float, flexnormal);
X(float, flextexcoord);
X(int, skinfacenum);
X(int, skinvertadr);
X(int, skinvertnum);
sample/testspeed.cc
-16
View File
@@ -32,8 +32,6 @@ mjData* d[maxthread];
// per-thread statistics
double accuracy_broad[maxthread];
double accuracy_mid[maxthread];
int contacts[maxthread];
int constraints[maxthread];
double simtime[maxthread];
@@ -87,8 +85,6 @@ void simulate(int id, int nstep, mjtNum* ctrl) {
// clear statistics
contacts[id] = 0;
constraints[id] = 0;
accuracy_broad[id] = 0;
accuracy_mid[id] = 0;
// run and time
double start = gettm();
@@ -102,16 +98,6 @@ void simulate(int id, int nstep, mjtNum* ctrl) {
// accumulate statistics
contacts[id] += d[id]->ncon;
constraints[id] += d[id]->nefc;
if (d[id]->nbodypair_broad) {
accuracy_broad[id] += (100.0*d[id]->nbodypair_narrow)/d[id]->nbodypair_broad;
} else {
accuracy_broad[id] += 100;
}
if (d[id]->ngeompair_mid) {
accuracy_mid[id] += (100.0*d[id]->nbodypair_narrow)/d[id]->ngeompair_mid;
} else {
accuracy_mid[id] += 100;
}
}
simtime[id] = 1e-6 * (gettm() - start);
}
@@ -241,8 +227,6 @@ int main(int argc, char** argv) {
std::printf(" Steps per second : %.0f\n", nstep/simtime[0]);
std::printf(" Realtime factor : %.2f x\n", nstep*m->opt.timestep/simtime[0]);
std::printf(" Time per step : %.1f %ss\n\n", 1e6*simtime[0]/nstep, mu_str);
std::printf(" Broadphase accuracy : %.2f%%\n", accuracy_broad[0]/nstep);
std::printf(" Midphase accuracy : %.2f%%\n", accuracy_mid[0]/nstep);
std::printf(" Contacts per step : %.2f\n", static_cast<float>(contacts[0])/nstep);
std::printf(" Constraints per step : %.2f\n", static_cast<float>(constraints[0])/nstep);
std::printf(" Degrees of freedom : %d\n\n", m->nv);
simulate/simulate.cc
+18 -3
View File
@@ -713,6 +713,7 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) {
{mjITEM_EDITNUM, "Margin", 2, &(opt->o_margin), "1"},
{mjITEM_EDITNUM, "Sol Imp", 2, &(opt->o_solimp), "5"},
{mjITEM_EDITNUM, "Sol Ref", 2, &(opt->o_solref), "2"},
{mjITEM_EDITNUM, "Friction", 2, &(opt->o_friction), "5"},
{mjITEM_END}
};
@@ -765,7 +766,7 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) {
2,
&(sim->opt.label),
"None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n"
"Actuator\nConstraint\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"
"Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland"
},
{
mjITEM_SELECT,
@@ -846,6 +847,7 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) {
// create tree slider
mjuiDef defTree[] = {
{mjITEM_SLIDERINT, "Tree depth", 2, &sim->opt.bvh_depth, "0 20"},
{mjITEM_SLIDERINT, "Flex layer", 2, &sim->opt.flex_layer, "0 10"},
{mjITEM_END}
};
mjui_add(&sim->ui0, defTree);
@@ -962,6 +964,13 @@ void MakeGroupSection(mj::Simulate* sim, int oldstate) {
{mjITEM_CHECKBYTE, "Actuator 3", 2, sim->opt.actuatorgroup+3, ""},
{mjITEM_CHECKBYTE, "Actuator 4", 2, sim->opt.actuatorgroup+4, ""},
{mjITEM_CHECKBYTE, "Actuator 5", 2, sim->opt.actuatorgroup+5, ""},
{mjITEM_SEPARATOR, "Flex groups", 1},
{mjITEM_CHECKBYTE, "Flex 0", 2, sim->opt.flexgroup, ""},
{mjITEM_CHECKBYTE, "Flex 1", 2, sim->opt.flexgroup+1, ""},
{mjITEM_CHECKBYTE, "Flex 2", 2, sim->opt.flexgroup+2, ""},
{mjITEM_CHECKBYTE, "Flex 3", 2, sim->opt.flexgroup+3, ""},
{mjITEM_CHECKBYTE, "Flex 4", 2, sim->opt.flexgroup+4, ""},
{mjITEM_CHECKBYTE, "Flex 5", 2, sim->opt.flexgroup+5, ""},
{mjITEM_SEPARATOR, "Skin groups", 1},
{mjITEM_CHECKBYTE, "Skin 0", 2, sim->opt.skingroup, ""},
{mjITEM_CHECKBYTE, "Skin 1", 2, sim->opt.skingroup+1, ""},
@@ -1551,6 +1560,7 @@ void UiEvent(mjuiState* state) {
case mjKEY_PAGE_UP: // select parent body
if ((sim->m_ || sim->is_passive_) && sim->pert.select > 0) {
sim->pert.select = sim->body_parentid_[sim->pert.select];
sim->pert.flexselect = -1;
sim->pert.skinselect = -1;
// stop perturbation if world reached
@@ -1829,6 +1839,7 @@ void Simulate::Sync() {
X(o_margin);
X(o_solref);
X(o_solimp);
X(o_friction);
X(integrator);
X(cone);
X(jacobian);
@@ -1959,12 +1970,12 @@ void Simulate::Sync() {
// find geom and 3D click point, get corresponding body
mjrRect r = pending_.select_state.rect[3];
mjtNum selpnt[3];
int selgeom, selskin;
int selgeom, selflex, selskin;
int selbody = mjv_select(m_, d_, &this->opt,
static_cast<mjtNum>(r.width) / r.height,
(pending_.select_state.x - r.left) / r.width,
(pending_.select_state.y - r.bottom) / r.height,
&this->scn, selpnt, &selgeom, &selskin);
&this->scn, selpnt, &selgeom, &selflex, &selskin);
// set lookat point, start tracking is requested
if (selmode==2 || selmode==3) {
@@ -1991,6 +2002,7 @@ void Simulate::Sync() {
if (selbody>=0) {
// record selection
this->pert.select = selbody;
this->pert.flexselect = selflex;
this->pert.skinselect = selskin;
// compute localpos
@@ -1999,6 +2011,7 @@ void Simulate::Sync() {
mju_mulMatTVec(this->pert.localpos, d_->xmat + 9*this->pert.select, tmp, 3, 3);
} else {
this->pert.select = 0;
this->pert.flexselect = -1;
this->pert.skinselect = -1;
}
}
@@ -2209,6 +2222,7 @@ void Simulate::LoadOnRenderThread() {
// clear perturbation state
this->pert.active = 0;
this->pert.select = 0;
this->pert.flexselect = -1;
this->pert.skinselect = -1;
// align and scale view unless reloading the same file
@@ -2360,6 +2374,7 @@ void Simulate::Render() {
IsDifferent(opt_prev_.jointgroup, opt.jointgroup) ||
IsDifferent(opt_prev_.tendongroup, opt.tendongroup) ||
IsDifferent(opt_prev_.actuatorgroup, opt.actuatorgroup) ||
IsDifferent(opt_prev_.flexgroup, opt.flexgroup) ||
IsDifferent(opt_prev_.skingroup, opt.skingroup))) {
mjui0_update_section(this, SECT_GROUP);
}
src/engine/engine_collision_box.c
+35 -22
View File
@@ -16,6 +16,7 @@
#include <string.h>
#include <mujoco/mjmacro.h>
#include <mujoco/mujoco.h>
#include "engine/engine_collision_primitive.h"
#include "engine/engine_util_blas.h"
@@ -36,16 +37,16 @@ static void mju_clampVec(mjtNum* vec, const mjtNum* limit, int n)
}
static int _SphereBox(mjContact* con, mjtNum mindist,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
// raw sphere : box
int mjraw_SphereBox(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2)
{
int i, k;
mjtNum tmp[3], center[3], clamped[3], deepest[3], nearest[3];
mjtNum pos[3];
mjtNum dist, closest;
mju_sub3(tmp, pos1, pos2);
mju_rotVecMatT(center, tmp, mat2);
@@ -56,10 +57,9 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
mju_sub3(tmp, clamped, center);
dist = mju_normalize3(tmp);
if (dist - size1[0] > mindist)
if (dist - size1[0] > margin)
return 0;
// sphere center inside box
if (dist <= mjMINVAL) {
closest = (size2[0] + size2[1] + size2[2]) * 2;
@@ -93,32 +93,36 @@ static int _SphereBox(mjContact* con, mjtNum mindist,
return 1;
}
// sphere : box
int mjc_SphereBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO;
return _SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_SphereBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// GENERAL THEORY OF OPERATION
// the following code is mostly for finding (line segment)/(box) collision
// after which box-sphere is called
// First the closest point to the box is found.
// Then a "sensible" second point is found if the angle
// between the segment and the box is low enough < 45
/* GENERAL THEORY OF OPERATION
the following code is mostly for finding (line segment)/(box) collision
after which box-sphere is called
// In the comments that follow, capsule just means the capsule's line segment
// It might be hard to understand all comments but you would need
// a picture to see what is happening at each line of the code
First the closest point to the box is found.
Then a "sensible" second point is found if the angle
between the segment and the box is low enough < 45
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO
In the comments that follow, capsule just means the capsule's line segment
It might be hard to understand all comments but you would need
a picture to see what is happening at each line of the code
*/
// raw capsule : box
int mjraw_CapsuleBox(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
mjtNum tmp1[3], tmp2[3], tmp3[3], halfaxis[3], axis[3], dif[3];
mjtNum pos[3]; // position of capsule in box-local frame
@@ -576,7 +580,7 @@ skip:
mju_addTo3(tmp2, pos2);
// collide with
n = _SphereBox(con, margin, tmp2, mat1, size1, pos2, mat2, size2);
n = mjraw_SphereBox(con, margin, tmp2, mat1, size1, pos2, mat2, size2);
if (secondpos > -3) { // secondpos was modified
@@ -584,15 +588,24 @@ skip:
mju_addToScl3(tmp1, halfaxis, secondpos + bestsegmentpos); // note the summation
mju_rotVecMat(tmp2, tmp1, mat2);
mju_addTo3(tmp2, pos2);
n += _SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
n += mjraw_SphereBox(con + n, margin, tmp2, mat1, size1, pos2, mat2, size2);
}
return n;
}
// capsule : box
int mjc_CapsuleBox(const mjModel* m, const mjData* d, mjContact* con,
int g1, int g2, mjtNum margin)
{
mjGETINFO
return mjraw_CapsuleBox(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// box : box
int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2, mjtNum margin)
{
const mjtNum* pos1 = D->geom_xpos + 3 * g1;
src/engine/engine_collision_convex.c
+260 -13
View File
@@ -15,6 +15,7 @@
#include "engine/engine_collision_convex.h"
#include <math.h>
#include <stddef.h>
#include <ccd/ccd.h>
#include <ccd/vec3.h>
@@ -28,12 +29,29 @@
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
// ccd center function
void mjccd_center(const void *obj, ccd_vec3_t *center) {
const mjtCCD* ccd = (const mjtCCD*)obj;
int g = ccd->geom;
int f = ccd->flex;
int e = ccd->elem;
int v = ccd->vert;
// return geom position
mju_copy3(center->v, ccd->data->geom_xpos + 3*ccd->geom);
if (g>=0) {
mju_copy3(center->v, ccd->data->geom_xpos + 3*g);
}
// return flex element position
else if (e>=0) {
mju_copy3(center->v, ccd->data->flexelem_aabb + 6*(ccd->model->flex_elemadr[f]+e));
}
// return flex vertex position
else {
mju_copy3(center->v, ccd->data->flexvert_xpos + 3*(ccd->model->flex_vertadr[f]+v));
}
}
@@ -45,6 +63,46 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
const mjData* d = ccd->data;
int g = ccd->geom;
//-------------------------- flex element or vertex -----------------------------
if (g<0) {
int f = ccd->flex;
int dim = m->flex_dim[f];
mjtNum *res = vec->v;
const mjtNum *dir = _dir->v;
// flex element
if (ccd->elem>=0) {
int e = ccd->elem;
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// find element vertex with largest projection along dir
mju_copy3(res, vert+3*edata[0]);
mjtNum best = mju_dot3(res, dir);
for (int i=1; i<=dim; i++) {
mjtNum dot = mju_dot3(vert+3*edata[i], dir);
// better vertex found: assign
if (dot>best) {
best = dot;
mju_copy3(res, vert+3*edata[i]);
}
}
// add radius and margin/2
mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin);
return;
}
// flex vertex
else {
const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + ccd->vert);
mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*ccd->margin);
return;
}
}
//-------------------------- geom -------------------------------------------
float* vertdata;
int ibest, graphadr, numvert, change, locid;
int *vert_edgeadr, *vert_globalid, *edge_localid;
@@ -211,7 +269,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) {
// find single convex-convex collision, using libccd
static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjContact* con, mjtNum margin) {
ccd_vec3_t dir, pos;
ccd_real_t depth;
if (ccdMPRPenetration(obj1, obj2, ccd, &depth, &dir, &pos) == 0) {
@@ -226,8 +284,10 @@ static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd,
mju_copy3(con->pos, pos.v);
mju_zero3(con->frame+3);
// fix contact frame normal
mjc_fixNormal(m, d, con, g1, g2);
// both geoms: fix contact frame normal
if (obj1->geom>=0 && obj2->geom>=0) {
mjc_fixNormal(m, d, con, obj1->geom, obj2->geom);
}
return 1;
}
@@ -278,8 +338,8 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9],
int mjc_Convex(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
ccd_t ccd;
mjtCCD obj1 = {m, d, g1, -1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, g2, -1, margin, {1, 0, 0, 0}};
mjtCCD obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}};
// init ccd structure
ccd.first_dir = ccdFirstDirDefault;
@@ -288,12 +348,12 @@ int mjc_Convex(const mjModel* m, const mjData* d,
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd paramters
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find initial contact
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, g1, g2, margin);
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
// look for additional contacts
if (ncon && mjENABLED(mjENBL_MULTICCD) // TODO(tassa) leave as bitflag or make geom attribute (?)
@@ -342,7 +402,7 @@ int mjc_Convex(const mjModel* m, const mjData* d,
mju_rotateFrame(con[0].pos, invrot, d->geom_xmat+9*g2, d->geom_xpos+3*g2);
// search for new contact
int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, g1, g2, margin);
int new_contact = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con+ncon, margin);
// check new contact
if (new_contact && mjc_isDistinctContact(con, ncon + 1, tolerance)) {
@@ -410,7 +470,7 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d,
mjGETINFO
mjtNum dist, dif[3], normal[3] = {mat1[2], mat1[5], mat1[8]};
ccd_vec3_t dir, vec;
mjtCCD obj = {m, d, g2, -1, 0, {1, 0, 0, 0}};
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
// get support point in -normal direction
ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]);
@@ -584,14 +644,14 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
int hid = m->geom_dataid[g1];
int nrow = m->hfield_nrow[hid];
int ncol = m->hfield_ncol[hid];
int dr[2], cnt, rmin, rmax, cmin, cmax, nvert;
int dr[2], cnt, rmin, rmax, cmin, cmax;
const float* data = m->hfield_data + m->hfield_adr[hid];
mjtPrism prism;
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCD obj = {m, d, g2, -1, 0, {1, 0, 0, 0}};
mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}};
ccd_t ccd;
// point size1 to hfield size instead of geom1 size
@@ -711,7 +771,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d,
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r < rmax; r++) {
nvert = 0;
int nvert = 0;
for (int c=cmin; c <= cmax; c++) {
for (int i=0; i < 2; i++) {
// send vertex to prism constructor
@@ -1018,3 +1078,190 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in
mju_zero3(con->frame+3);
}
}
//---------------------------- flex collisions ---------------------------------------------
// geom-elem or elem-elem or vert-elem convex collision using ccd
int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) {
ccd_t ccd;
mjtCCD obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}};
mjtCCD obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}};
// init ccd structure
ccd.first_dir = ccdFirstDirDefault;
ccd.center1 = mjccd_center;
ccd.center2 = mjccd_center;
ccd.support1 = mjccd_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// find contacts
int ncon = mjc_MPRIteration(&obj1, &obj2, &ccd, m, d, con, margin);
return ncon;
}
// test a heighfield geom and a flex flex element for collision
int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con,
int g, int f, int e, mjtNum margin) {
mjtNum vec[3], dx, dy;
mjtNum xmin, xmax, ymin, ymax, zmin, zmax;
int dr[2], cnt, rmin, rmax, cmin, cmax;
mjtPrism prism;
// get hfield info
int hid = m->geom_dataid[g];
int nrow = m->hfield_nrow[hid];
int ncol = m->hfield_ncol[hid];
mjtNum* hpos = d->geom_xpos + 3*g;
mjtNum* hmat = d->geom_xmat + 9*g;
mjtNum* hsize = m->hfield_size + 4*hid;
const float* hdata = m->hfield_data + m->hfield_adr[hid];
// get elem indo
int dim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
mjtNum* evert[4] = {NULL, NULL, NULL, NULL};
for (int i=0; i<=dim; i++) {
evert[i] = d->flexvert_xpos + 3*(m->flex_vertadr[f] + edata[i]);
}
mjtNum* ecenter = d->flexelem_aabb + 6*(m->flex_elemadr[f]+e);
// ccd-related
ccd_vec3_t dirccd, vecccd;
ccd_real_t depth;
mjtCCD obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}};
ccd_t ccd;
//------------------------------------- AABB computation, box-box test
// save elem vertices, transform to hfield frame
mjtNum savevert[4][3];
for (int i=0; i<=dim; i++) {
mju_copy3(savevert[i], evert[i]);
mju_sub3(vec, evert[i], hpos);
mju_mulMatTVec(evert[i], hmat, vec, 3, 3);
}
// save elem center, transform to hfield frame
mjtNum savecenter[3];
mju_copy3(savecenter, ecenter);
mju_sub3(vec, ecenter, hpos);
mju_mulMatTVec(ecenter, hmat, vec, 3, 3);
// compute elem bounding box (in hfield frame)
xmin = xmax = evert[0][0];
ymin = ymax = evert[0][1];
zmin = zmax = evert[0][2];
for (int i=1; i<=dim; i++) {
xmin = mju_min(xmin, evert[i][0]);
xmax = mju_max(xmax, evert[i][0]);
ymin = mju_min(ymin, evert[i][1]);
ymax = mju_max(ymax, evert[i][1]);
zmin = mju_min(zmin, evert[i][2]);
zmax = mju_max(zmax, evert[i][2]);
}
// box-box test
if ((xmin-margin > hsize[0]) || (xmax+margin < -hsize[0]) ||
(ymin-margin > hsize[1]) || (ymax+margin < -hsize[1]) ||
(zmin-margin > hsize[2]) || (zmax+margin < -hsize[3])) {
// restore vertices and center
for (int i=0; i<=dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
return 0;
}
// compute sub-grid bounds
cmin = (int) floor((xmin + hsize[0]) / (2*hsize[0]) * (ncol-1));
cmax = (int) ceil ((xmax + hsize[0]) / (2*hsize[0]) * (ncol-1));
rmin = (int) floor((ymin + hsize[1]) / (2*hsize[1]) * (nrow-1));
rmax = (int) ceil ((ymax + hsize[1]) / (2*hsize[1]) * (nrow-1));
cmin = mjMAX(0, cmin);
cmax = mjMIN(ncol-1, cmax);
rmin = mjMAX(0, rmin);
rmax = mjMIN(nrow-1, rmax);
//------------------------------------- collision testing
// init ccd structure
ccd.first_dir = prism_firstdir;
ccd.center1 = prism_center;
ccd.center2 = mjccd_center;
ccd.support1 = prism_support;
ccd.support2 = mjccd_support;
// set ccd parameters
ccd.max_iterations = m->opt.mpr_iterations;
ccd.mpr_tolerance = m->opt.mpr_tolerance;
// compute real-valued grid step, and triangulation direction
dx = (2.0*hsize[0]) / (ncol-1);
dy = (2.0*hsize[1]) / (nrow-1);
dr[0] = 1;
dr[1] = 0;
// set zbottom value using base size
prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3];
// process all prisms in sub-grid
cnt = 0;
for (int r=rmin; r<rmax; r++) {
int nvert = 0;
for (int c=cmin; c<=cmax; c++) {
for (int k=0; k<2; k++) {
// send vertex to prism constructor
addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1],
hdata[(r+dr[k])*ncol+c]*hsize[2]+margin);
// check for enough vertices
if (nvert>2) {
// prism height test
if (prism.v[3][2]<zmin && prism.v[4][2]<zmin && prism.v[5][2]<zmin) {
continue;
}
// run MPR, save contact
if (ccdMPRPenetration(&prism, &obj, &ccd, &depth, &dirccd, &vecccd)==0) {
if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) {
// fill in contact data, transform to global coordinates
con[cnt].dist = -depth;
mju_rotVecMat(con[cnt].frame, dirccd.v, hmat);
mju_rotVecMat(con[cnt].pos, vecccd.v, hmat);
mju_addTo3(con[cnt].pos, hpos);
mju_zero3(con[cnt].frame+3);
// count, stop if max number reached
cnt++;
if (cnt>=mjMAXCONPAIR) {
r = rmax+1;
c = cmax+1;
k = 3;
break;
}
}
}
}
}
}
}
// restore elem vertices and center
for (int i=0; i<=dim; i++) {
mju_copy3(evert[i], savevert[i]);
}
mju_copy3(ecenter, savecenter);
return cnt;
}
src/engine/engine_collision_convex.h
+11 -1
View File
@@ -44,6 +44,9 @@ struct _mjtCCD {
const mjData* data;
int geom;
int meshindex;
int flex;
int elem;
int vert;
mjtNum margin;
mjtNum rotate[4];
};
@@ -54,7 +57,7 @@ typedef struct _mjtCCD mjtCCD;
void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec);
// pairwise collision functions using ccd
// pairwise geom collision functions using ccd
int mjc_PlaneConvex (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_ConvexHField (const mjModel* m, const mjData* d,
@@ -62,6 +65,13 @@ int mjc_ConvexHField (const mjModel* m, const mjData* d,
int mjc_Convex (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
// geom-elem or elem-elem or vert-elem collision function using ccd
int mjc_ConvexElem (const mjModel* m, const mjData* d, mjContact* con,
int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin);
// heighfield-elem collision function using ccd
int mjc_HFieldElem (const mjModel* m, const mjData* d, mjContact* con,
int g, int f, int e, mjtNum margin);
// fix contact frame normal
void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2);
src/engine/engine_collision_driver.c
+1606 -648
View File
File diff suppressed because it is too large Load Diff
src/engine/engine_collision_driver.h
+35 -12
View File
@@ -23,12 +23,6 @@
extern "C" {
#endif
struct mjCollisionTree_ {
int node1;
int node2;
};
typedef struct mjCollisionTree_ mjCollisionTree;
// collision function pointers and max contact pairs
MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES];
@@ -38,15 +32,44 @@ MJAPI void mj_collision(const mjModel* m, mjData* d);
// applies Separating Axis Theorem for rotated AABBs
MJAPI int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6],
const mjtNum xpos1[3], const mjtNum xmat1[9],
const mjtNum xpos2[3], const mjtNum xmat2[9],
const mjtNum xpos2[3], const mjtNum xmat2[9], mjtNum margin,
mjtNum product[36], mjtNum offset[12], mjtByte* initialize);
// broad phase collision detection; return list of body pairs for narrow phase
int mj_broadphase(const mjModel* m, mjData* d, int* bodypair, int maxpair);
// is element active (for collisions)
MJAPI int mj_isElemActive(const mjModel* m, int f, int e);
// checks if pair is already present in pair_geom and calls narrow phase
void mj_collideGeomPair(const mjModel* m, mjData* d, int g1, int g2, int merged,
int startadr, int pairadr);
// binary search between two bodyflex trees
void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2,
int merged, int startadr, int pairadr);
// broad phase collision detection; return list of bodyflex pairs
int mj_broadphase(const mjModel* m, mjData* d, int* bfpair, int maxpair);
// test two geoms for collision, apply filters, add to contact list
void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2);
// test a plane geom and a flex for collision, add to contact list
void mj_collidePlaneFlex(const mjModel* m, mjData* d, int g, int f);
// test for internal flex collisions, add to contact list
void mj_collideFlexInternal(const mjModel* m, mjData* d, int f);
// test active element self-collisions with SAP
void mj_collideFlexSAP(const mjModel* m, mjData* d, int f);
// test a geom and an elem for collision, add to contact list
void mj_collideGeomElem(const mjModel* m, mjData* d, int g, int f, int e);
// test two elems for collision, add to contact list
void mj_collideElems(const mjModel* m, mjData* d, int f1, int e1, int f2, int e2);
// test element and vertex for collision, add to contact list
void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v);
// number of possible collisions based on filters and geom types
int mj_contactFilter(int contype1, int conaffinity1,
int contype2, int conaffinity2);
#ifdef __cplusplus
}
src/engine/engine_collision_primitive.c
+168 -30
View File
@@ -26,10 +26,10 @@
//--------------------------- plane collisions -----------------------------------------------------
// plane : sphere (actual implementation, can be called with modified parameters)
static int _PlaneSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// raw plane : sphere
static int mjraw_PlaneSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// set normal
con[0].frame[0] = mat1[2];
con[0].frame[1] = mat1[5];
@@ -57,7 +57,7 @@ static int _PlaneSphere(mjContact* con, mjtNum margin,
int mjc_PlaneSphere(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return _PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -74,11 +74,11 @@ int mjc_PlaneCapsule(const mjModel* m, const mjData* d,
// get point 1, do sphere-plane test
mjtNum pos[3];
mju_add3(pos, pos2, segment);
int n1 = _PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
int n1 = mjraw_PlaneSphere(con, margin, pos1, mat1, size1, pos, mat2, size2);
// get point 2, do sphere-plane test
mju_sub3(pos, pos2, segment);
int n2 = _PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
int n2 = mjraw_PlaneSphere(con+n1, margin, pos1, mat1, size1, pos, mat2, size2);
// align contact frames with capsule axis
if (n1) {
@@ -249,9 +249,9 @@ int mjc_PlaneBox(const mjModel* m, const mjData* d,
//--------------------------- sphere and capsule collisions ----------------------------------------
// sphere : sphere (actual implementation, can be called with modified parameters)
static int _SphereSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
static int mjraw_SphereSphere(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// check bounding spheres (this is called from other functions)
mjtNum dif[3] = {pos1[0] - pos2[0], pos1[1] - pos2[1], pos1[2] - pos2[2]};
mjtNum cdist_sqr = mju_dot3(dif, dif);
@@ -288,16 +288,15 @@ static int _SphereSphere(mjContact* con, mjtNum margin,
int mjc_SphereSphere(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return _SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// sphere : capsule
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
// raw sphere : capsule
int mjraw_SphereCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// get capsule length and axis
mjtNum len = size2[1];
mjtNum axis[3] = {mat2[2], mat2[5], mat2[8]};
@@ -309,7 +308,16 @@ int mjc_SphereCapsule(const mjModel* m, const mjData* d,
// find nearest point on segment, do sphere-sphere test
mju_scl3(vec, axis, x);
mju_addTo3(vec, pos2);
return _SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size2);
}
// sphere : capsule
int mjc_SphereCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return mjraw_SphereCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
@@ -348,7 +356,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
// side collision: use sphere-sphere
if (collide_side) {
mju_addTo3(a_proj, pos2);
return _SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, a_proj, mat2, size2);
}
// cap collision: use plane-sphere
@@ -367,7 +375,7 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
mju_addScl3(pos_cap, pos2, axis, -height);
mat_cap = flipmat;
}
int ncon = _PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
int ncon = mjraw_PlaneSphere(con, margin, pos_cap, mat_cap, size2, pos1, mat1, size1);
if (ncon) {
// flip frame normal (because mjGEOM_PLANE < mjGEOM_SPHERE < mjGEOM_CYLINDER)
mju_scl3(con->frame, con->frame, -1);
@@ -383,16 +391,15 @@ int mjc_SphereCylinder(const mjModel* m, const mjData* d,
// sphere-sphere with point sphere at the corner
mjtNum size_zero[1] = {0};
return _SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size_zero);
return mjraw_SphereSphere(con, margin, pos1, mat1, size1, vec, mat2, size_zero);
}
// capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
// raw capsule : capsule
int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2) {
// get capsule axes (scaled) and center difference
mjtNum axis1[3] = {mat1[2] * size1[1], mat1[5] * size1[1], mat1[8] * size1[1]};
mjtNum axis2[3] = {mat2[2] * size2[1], mat2[5] * size2[1], mat2[8] * size2[1]};
@@ -434,7 +441,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
return _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
return mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
}
// parallel axes
@@ -447,14 +454,14 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjtNum vec2[3];
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
int n1 = _SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
int n1 = mjraw_SphereSphere(con, margin, vec1, mat1, size1, vec2, mat2, size2);
// x1 = -1
mju_sub3(vec1, pos1, axis1);
x2 = mju_clip((v + mb) / mc, -1, 1);
mju_scl3(vec2, axis2, x2);
mju_addTo3(vec2, pos2);
int n2 = _SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
int n2 = mjraw_SphereSphere(con+n1, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2 >= 2) {
@@ -466,7 +473,7 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjtNum x1 = mju_clip((u - mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
int n3 = _SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
int n3 = mjraw_SphereSphere(con+n1+n2, margin, vec1, mat1, size1, vec2, mat2, size2);
// return if two contacts already found
if (n1+n2+n3 >= 2) {
@@ -478,8 +485,139 @@ int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
x1 = mju_clip((u + mb) / ma, -1, 1);
mju_scl3(vec1, axis1, x1);
mju_addTo3(vec1, pos1);
int n4 = _SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
int n4 = mjraw_SphereSphere(con+n1+n2+n3, margin, vec1, mat1, size1, vec2, mat2, size2);
return n1+n2+n3+n4;
}
}
// capsule : capsule
int mjc_CapsuleCapsule(const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin) {
mjGETINFO
return mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2);
}
// sign of (signed) area of planar triangle
static mjtNum areaSign(const mjtNum p1[2], const mjtNum p2[2], const mjtNum p3[2]) {
return mju_sign((p1[0]-p3[0])*(p2[1]-p3[1]) - (p2[0]-p3[0])*(p1[1]-p3[1]));
}
// find nearest point to p within line segment (u,v); return distance to p
static mjtNum pointSegment(mjtNum res[2], const mjtNum p[2],
const mjtNum u[2], const mjtNum v[2]) {
// make u the origin
mjtNum uv[2] = {v[0]-u[0], v[1]-u[1]};
mjtNum up[2] = {p[0]-u[0], p[1]-u[1]};
// project: find a s.t. uv is orthogonal to (up-a*uv)
mjtNum a = mju_dot(uv, up, 2) / mju_max(mjMINVAL, mju_dot(uv, uv, 2));
// find nearest point to p, clamp to u or v if a is not in (0,1)
if (a<=0) {
res[0] = u[0];
res[1] = u[1];
} else if (a>=1) {
res[0] = v[0];
res[1] = v[1];
} else {
mju_addScl(res, u, uv, a, 2);
}
// compute distance
return mju_sqrt((res[0]-p[0])*(res[0]-p[0]) + (res[1]-p[1])*(res[1]-p[1]));
}
// sphere : triangle with radius
int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt) {
mjtNum rbound = margin + rs + rt;
mjtNum X[3];
// make t1 the origin: triangle is (O,A,B); sphere center is S
mjtNum S[3] = { s[0]-t1[0], s[1]-t1[1], s[2]-t1[2]};
mjtNum A[3] = {t2[0]-t1[0], t2[1]-t1[1], t2[2]-t1[2]};
mjtNum B[3] = {t3[0]-t1[0], t3[1]-t1[1], t3[2]-t1[2]};
// N is normal to triangle plane
mjtNum N[3];
mju_cross(N, A, B);
mju_normalize3(N);
// dstS is signed distance from S to plane; exit if too large
mjtNum dstS = mju_dot3(N, S);
if (mju_abs(dstS) > rbound) {
return 0;
}
// P is projection of S in triangle plane
mjtNum P[3];
mju_addScl3(P, S, N, -dstS);
// construct orthogonal axes (V1~A, V2) of triangle plane
mjtNum V1[3], V2[3];
mju_copy3(V1, A);
mjtNum lenA = mju_normalize3(V1);
mju_cross(V2, N, A);
mju_normalize3(V2);
// triangle is (o,a,b), sphere center is p
mjtNum o[2] = {0, 0};
mjtNum a[2] = {lenA, 0}; // equals {mju_dot3(V1, A), mju_dot3(V2, A)}
mjtNum b[2] = {mju_dot3(V1, B), mju_dot3(V2, B)};
mjtNum p[2] = {mju_dot3(V1, P), mju_dot3(V2, P)};
// copmuted signs of areas of (p,o,a), (p,a,b), (p,b,o)
mjtNum sign1 = areaSign(p, o, a);
mjtNum sign2 = areaSign(p, a, b);
mjtNum sign3 = areaSign(p, b, o);
// p is inside triangle
if (sign1==sign2 && sign2==sign3) {
// P is nearest point to S within triangle
mju_copy3(X, P);
}
// p is not inside triangle
else {
// find nearest point to p on triangle edges (o,a), (a,b), (b,o)
mjtNum x[3][2], dstx[3];
dstx[0] = pointSegment(x[0], p, o, a);
dstx[1] = pointSegment(x[1], p, a, b);
dstx[2] = pointSegment(x[2], p, b, o);
// select minimum
int best = (dstx[0]<dstx[1] && dstx[0]<dstx[2]) ? 0 : (dstx[1]<dstx[2] ? 1 : 2);
// convert x[best] to 3D
mju_scl3(X, V1, x[best][0]);
mju_addToScl3(X, V2, x[best][1]);
}
// X is now the nearest point to S within the 3D triangle (O,A,B)
// compute contact normal and distance
mjtNum nrm[3] = {X[0]-S[0], X[1]-S[1], X[2]-S[2]};
mjtNum dst = mju_normalize3(nrm);
// exit if too far
if (dst>rbound) {
return 0;
}
// construct contact
con[0].dist = dst - rs - rt;
mju_addScl3(con[0].pos, s, nrm, rs + con[0].dist/2);
mju_copy3(con[0].frame, nrm);
mju_zero3(con[0].frame+3);
return 1;
}
src/engine/engine_collision_primitive.h
+15 -1
View File
@@ -32,6 +32,20 @@
extern "C" {
#endif
// raw collision functions (called by mjc_XXX)
int mjraw_SphereCapsule (mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_CapsuleCapsule(mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_CapsuleBox (mjContact* con, mjtNum margin,
const mjtNum* pos1, const mjtNum* mat1, const mjtNum* size1,
const mjtNum* pos2, const mjtNum* mat2, const mjtNum* size2);
int mjraw_SphereTriangle(mjContact* con, mjtNum margin,
const mjtNum* s, mjtNum rs,
const mjtNum* t1, const mjtNum* t2, const mjtNum* t3, mjtNum rt);
// plane collisions
int mjc_PlaneSphere (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
@@ -52,7 +66,7 @@ int mjc_SphereCylinder (const mjModel* m, const mjData* d,
int mjc_CapsuleCapsule (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
// box collisions: from boxcollisions.c
// box collisions: from engine_collision_box.c
int mjc_CapsuleBox (const mjModel* m, const mjData* d,
mjContact* con, int g1, int g2, mjtNum margin);
int mjc_SphereBox (const mjModel* m, const mjData* d,
src/engine/engine_collision_sdf.c
+1 -1
View File
@@ -447,7 +447,7 @@ static void collideBVH(const mjModel* m, mjData* d, int g,
int* faces, int* npoints, int* n0,
const mjSDF* sdf) {
const int bvhadr = m->mesh_bvhadr[m->geom_dataid[g]];
const int* faceid = m->bvh_geomid + bvhadr;
const int* faceid = m->bvh_nodeid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
mjtByte* visited = d->bvh_active + bvhadr;
src/engine/engine_core_constraint.c
+337 -184
View File
@@ -121,6 +121,21 @@ int mj_isDual(const mjModel* m) {
// assign/clamp contact friction parameters
void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source) {
if (mjENABLED(mjENBL_OVERRIDE)) {
for (int i=0; i<5; i++) {
target[i] = mju_max(mjMINMU, m->opt.o_friction[i]);
}
} else {
for (int i=0; i<5; i++) {
target[i] = mju_max(mjMINMU, source[i]);
}
}
}
// assign/override contact reference parameters
void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source) {
if (mjENABLED(mjENBL_OVERRIDE)) {
@@ -154,6 +169,46 @@ mjtNum mj_assignMargin(const mjModel* m, mjtNum source) {
// compute element bodies and weights for given contact point, return #bodies
// if v is one of the element vertices, reduce element to fragment
static int mj_elemBodyWeight(const mjModel* m, const mjData* d, int f, int e, int v,
const mjtNum point[3], int* body, mjtNum* weight) {
// get flex info
int dim = m->flex_dim[f];
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// compute inverse distances from contact point to element vertices
// save body ids, find vertex v in element
int vid = -1;
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]];
// check if element vertex matches v
if (edata[i]==v) {
vid = i;
}
}
// v found in e: skip and shift remaining
if (vid>=0) {
while (vid<dim) {
weight[vid] = weight[vid+1];
body[vid] = body[vid+1];
vid++;
}
dim--;
}
// normalize weights
mju_normalize(weight, dim+1);
return dim+1;
}
// 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
@@ -186,11 +241,10 @@ int mj_addContact(const mjModel* m, mjData* d, const mjContact* con) {
// add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id
// return 0 if success; 1 if buffer full
int mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
static void mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain) {
int empty, nv = m->nv, nefc = d->nefc;
int *nnz = d->efc_J_rownnz, *adr = d->efc_J_rowadr, *ind = d->efc_J_colind;
mjtNum *J = d->efc_J;
@@ -231,7 +285,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
empty = 0;
} else if (empty) {
// all rows are empty, return early
return 0;
return;
}
// chain required in sparse mode
@@ -257,7 +311,7 @@ int mj_addConstraint(const mjModel* m, mjData* d,
// all rows empty: skip constraint
if (empty) {
return 0;
return;
}
// set constraint pos, margin, frictionloss, type, id
@@ -278,85 +332,6 @@ int mj_addConstraint(const mjModel* m, mjData* d,
} else if (type == mjCNSTR_LIMIT_JOINT || type == mjCNSTR_LIMIT_TENDON) {
d->nl += size;
}
return 0;
}
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
int da1, da2, NV = 0;
// skip fixed bodies
while (b1 && !m->body_dofnum[b1]) {
b1 = m->body_parentid[b1];
}
while (b2 && !m->body_dofnum[b2]) {
b2 = m->body_parentid[b2];
}
// neither body is movable: empty chain
if (b1 == 0 && b2 == 0) {
return 0;
}
// intialize last dof address for each body
da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;
// merge chains
while (da1 >= 0 || da2 >= 0) {
chain[NV] = mjMAX(da1, da2);
if (da1 == chain[NV]) {
da1 = m->dof_parentid[da1];
}
if (da2 == chain[NV]) {
da2 = m->dof_parentid[da2];
}
NV++;
}
// reverse order of chain: make it increasing
for (int i=0; i < NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
// swap bodies if wrong order
if (b1 > b2) {
int tmp = b1;
b1 = b2;
b2 = tmp;
}
// init
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2];
// both fixed: nothing to do
if (n1 == 0 && n2 == 0) {
return 0;
}
// copy b1 dofs
for (int i=0; i < n1; i++) {
chain[i] = m->body_dofadr[b1] + i;
}
// copy b2 dofs
for (int i=0; i < n2; i++) {
chain[n1+i] = m->body_dofadr[b2] + i;
}
return (n1+n2);
}
@@ -497,6 +472,7 @@ void mj_mulJacTVec_island(const mjModel* m, const mjData* d, mjtNum* res, const
void mj_instantiateEquality(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL, *buf_ind = NULL;
int flex_edgeadr, flex_edgenum;
mjtNum cpos[6], pos[2][3], ref[2], dif, deriv;
mjtNum quat[4], quat1[4], quat2[4], quat3[4], axis[3];
mjtNum *jac[2], *jacdif, *data, *sparse_buf = NULL;
@@ -684,18 +660,39 @@ void mj_instantiateEquality(const mjModel* m, mjData* d) {
size = 1;
break;
case mjEQ_FLEX:
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// add one constraint per edge
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
// position error
cpos[0] = d->flexedge_length[e] - m->flexedge_length0[e];
// add constraint: sparse or dense
if (issparse) {
mj_addConstraint(m, d, d->flexedge_J+d->flexedge_J_rowadr[e], cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
d->flexedge_J_rownnz[e],
d->flexedge_J_colind+d->flexedge_J_rowadr[e]);
} else {
mj_addConstraint(m, d, d->flexedge_J+e*nv, cpos, 0, 0,
1, mjCNSTR_EQUALITY, i,
0, NULL);
}
}
break;
default: // SHOULD NOT OCCUR
mjERROR("invalid equality constraint type %d", m->eq_type[i]);
}
// add constraint
if (size) {
if (mj_addConstraint(m, d, jac[0], cpos, 0, 0,
size, mjCNSTR_EQUALITY, i,
issparse ? NV : 0,
issparse ? chain : NULL)) {
break;
}
mj_addConstraint(m, d, jac[0], cpos, 0, 0,
size, mjCNSTR_EQUALITY, i,
issparse ? NV : 0,
issparse ? chain : NULL);
}
}
}
@@ -732,12 +729,10 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
}
// add constraint
if (mj_addConstraint(m, d, jac, 0, 0, m->dof_frictionloss[i],
1, mjCNSTR_FRICTION_DOF, i,
issparse ? 1 : 0,
issparse ? &i : NULL)) {
break;
}
mj_addConstraint(m, d, jac, 0, 0, m->dof_frictionloss[i],
1, mjCNSTR_FRICTION_DOF, i,
issparse ? 1 : 0,
issparse ? &i : NULL);
}
}
@@ -746,17 +741,14 @@ void mj_instantiateFriction(const mjModel* m, mjData* d) {
if (m->tendon_frictionloss[i] > 0) {
int efcadr = d->nefc;
// add constraint
if (mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv),
0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) {
break;
} else {
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
mj_addConstraint(m, d, d->ten_J + (issparse ? d->ten_J_rowadr[i] : i*nv),
0, 0, m->tendon_frictionloss[i],
1, mjCNSTR_FRICTION_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL);
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
}
}
@@ -809,12 +801,10 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
}
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i,
issparse ? 1 : 0,
issparse ? m->jnt_dofadr+i : NULL)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i,
issparse ? 1 : 0,
issparse ? m->jnt_dofadr+i : NULL);
}
}
}
@@ -845,10 +835,8 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
mju_scl3(jac, angleAxis, -1);
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 3, chain)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 3, chain);
}
// dense
@@ -858,10 +846,8 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
mju_scl3(jac + m->jnt_dofadr[i], angleAxis, -1);
// add constraint
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 0, 0)) {
break;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_JOINT, i, 0, 0);
}
}
}
@@ -891,16 +877,13 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// add constraint
int efcadr = d->nefc;
if (mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL)) {
break;
} else {
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
mj_addConstraint(m, d, jac, &dist, &margin, 0,
1, mjCNSTR_LIMIT_TENDON, i,
issparse ? d->ten_J_rownnz[i] : 0,
issparse ? d->ten_J_colind+d->ten_J_rowadr[i] : NULL);
// set tendon_efcadr
if (d->tendon_efcadr[i] == -1) {
d->tendon_efcadr[i] = efcadr;
}
}
}
@@ -915,9 +898,9 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) {
// frictionless and frictional contacts
void mj_instantiateContact(const mjModel* m, mjData* d) {
int ispyramid = mj_isPyramidal(m), issparse = mj_isSparse(m), ncon = d->ncon;
int dim, b1, b2, NV = m->nv, *chain = NULL;
int dim, NV, nv = m->nv, *chain = NULL;
mjContact* con;
mjtNum cpos[6], cmargin[6], *jac, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
mjtNum cpos[6], cmargin[6], *jac, *jacdif, *jacdifp, *jacdifr, *jac1p, *jac2p, *jac1r, *jac2r;
if (mjDISABLED(mjDSBL_CONTACT) || ncon == 0) {
return;
@@ -926,36 +909,84 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
mj_markStack(d);
// allocate Jacobian
jac = mj_stackAllocNum(d, 6*NV);
jacdifp = mj_stackAllocNum(d, 3*NV);
jacdifr = mj_stackAllocNum(d, 3*NV);
jac1p = mj_stackAllocNum(d, 3*NV);
jac2p = mj_stackAllocNum(d, 3*NV);
jac1r = mj_stackAllocNum(d, 3*NV);
jac2r = mj_stackAllocNum(d, 3*NV);
jac = mj_stackAllocNum(d, 6*nv);
jacdif = mj_stackAllocNum(d, 6*nv);
jacdifp = jacdif;
jacdifr = jacdif + 3*nv;
jac1p = mj_stackAllocNum(d, 3*nv);
jac2p = mj_stackAllocNum(d, 3*nv);
jac1r = mj_stackAllocNum(d, 3*nv);
jac2r = mj_stackAllocNum(d, 3*nv);
if (issparse) {
chain = mj_stackAllocInt(d, NV);
chain = mj_stackAllocInt(d, nv);
}
// find contacts to be included
for (int i=0; i < ncon; i++) {
if (!d->contact[i].exclude) {
// get pointer to this contact, info
// get contact info, safe efc_address
con = d->contact + i;
dim = con->dim;
b1 = m->geom_bodyid[con->geom1];
b2 = m->geom_bodyid[con->geom2];
// save efc_address
con->efc_address = d->nefc;
// compute Jacobian differences
if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
NV = mj_jacDifPair(m, d, chain, b1, b2, con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
// special case: single body on each side
if ((con->geom[0]>=0 || con->vert[0]>=0) &&
(con->geom[1]>=0 || con->vert[1]>=0)) {
// get bodies
int bid[2];
for (int side=0; side < 2; side++) {
bid[side] = (con->geom[side]>=0) ?
m->geom_bodyid[con->geom[side]] :
m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// compute Jacobian differences
if (dim > 3) {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, jac1r, jac2r, jacdifr);
} else {
NV = mj_jacDifPair(m, d, chain, bid[0], bid[1], con->pos, con->pos,
jac1p, jac2p, jacdifp, NULL, NULL, NULL);
}
}
// general case: flex elements involved
else {
// get bodies and weights
int nb = 0;
int bid[8];
mjtNum bweight[8];
for (int side=0; side<2; side++) {
// geom
if (con->geom[side]>=0) {
bid[nb] = m->geom_bodyid[con->geom[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// flex vert
else if (con->vert[side]>=0) {
bid[nb] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
bweight[nb] = side ? +1 : -1;
nb++;
}
// 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);
// negative sign for first side of contact
if (side==0) {
mju_scl(bweight+nb, bweight+nb, -1, nw);
}
nb += nw;
}
}
// combine weighted Jacobians
NV = mj_jacSum(m, d, chain, nb, bid, bweight, con->pos, jacdif, dim>3);
}
// skip contact if no DOFs affected
@@ -973,7 +1004,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// make frictionless contact
if (dim == 1) {
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jac, &(con->dist), &(con->includemargin), 0,
1, mjCNSTR_CONTACT_FRICTIONLESS, i,
issparse ? NV : 0,
@@ -992,7 +1023,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
mju_addScl(jacdifp, jac, jac + k*NV, con->friction[k-1], NV);
mju_addScl(jacdifp + NV, jac, jac + k*NV, -con->friction[k-1], NV);
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jacdifp, cpos, cmargin, 0,
2, mjCNSTR_CONTACT_PYRAMIDAL, i,
issparse ? NV : 0,
@@ -1008,7 +1039,7 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
cpos[0] = con->dist;
cmargin[0] = con->includemargin;
// add constraint (already checked space)
// add constraint
mj_addConstraint(m, d, jac, cpos, cmargin, 0,
con->dim, mjCNSTR_CONTACT_ELLIPTIC, i,
issparse ? NV : 0,
@@ -1026,15 +1057,21 @@ void mj_instantiateContact(const mjModel* m, mjData* d) {
// compute diagApprox
void mj_diagApprox(const mjModel* m, mjData* d) {
int id, dim, b1, b2, weldcnt = 0;
int id, dim, b1, b2, weldcnt = 0, edgecnt = 0;
int nefc = d->nefc;
mjtNum tran, rot, fri, *dA = d->efc_diagApprox;
mjContact* con = NULL;
// loop over all constraints, compute approximate inverse inertia
for (int i=0; i < nefc; i++) {
// get constraint id
id = d->efc_id[i];
// clear edge counter
if (d->efc_type[i]!=mjEQ_FLEX) {
edgecnt = 0;
}
// process according to constraint type
switch ((mjtConstraint) d->efc_type[i]) {
case mjCNSTR_EQUALITY:
@@ -1070,6 +1107,11 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
m->tendon_invweight0[m->eq_obj2id[id]]);
break;
case mjEQ_FLEX:
dA[i] = m->flexedge_invweight0[m->flex_edgeadr[m->eq_obj1id[id]] + edgecnt];
edgecnt++;
break;
default:
mjERROR("unknown constraint type type %d", d->efc_type[i]); // SHOULD NOT OCCUR
}
@@ -1091,14 +1133,41 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
case mjCNSTR_CONTACT_FRICTIONLESS:
case mjCNSTR_CONTACT_PYRAMIDAL:
case mjCNSTR_CONTACT_ELLIPTIC:
// get body ids and dim
b1 = m->geom_bodyid[d->contact[id].geom1];
b2 = m->geom_bodyid[d->contact[id].geom2];
dim = d->contact[id].dim;
// get contact info
con = d->contact + id;
dim = con->dim;
// precompute translational and rotational components
tran = m->body_invweight0[2*b1] + m->body_invweight0[2*b2];
rot = m->body_invweight0[2*b1+1] + m->body_invweight0[2*b2+1];
// add the average translation and rotation components from both sides
tran = rot = 0;
for (int side=0; side<2; side++) {
// get bodies and weights
int nb, bid[4];
mjtNum bweight[4];
// geom
if (con->geom[side]>=0) {
bid[0] = m->geom_bodyid[con->geom[side]];
bweight[0] = 1;
nb = 1;
// 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;
// flex elem
} else {
nb = mj_elemBodyWeight(m, d, con->flex[side], con->elem[side],
con->vert[1-side], con->pos, bid, bweight);
}
// add weighted average over bodies
for (int k=0; k<nb; k++) {
tran += m->body_invweight0[2*bid[k]] * bweight[k];
rot += m->body_invweight0[2*bid[k]+1] * bweight[k];
}
}
// set frictionless
if (d->efc_type[i] == mjCNSTR_CONTACT_FRICTIONLESS) {
@@ -1118,8 +1187,8 @@ void mj_diagApprox(const mjModel* m, mjData* d) {
// set pyramidal
else {
for (int j=0; j < dim-1; j++) {
fri = d->contact[id].friction[j];
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j < 2 ? tran : rot);
fri = con->friction[j];
dA[i+2*j] = dA[i+2*j+1] = tran + fri*fri*(j<2 ? tran : rot);
}
// processed 2*dim-2 elements in one i-loop iteration; advance counter
@@ -1444,6 +1513,39 @@ static int mj_jacDifPairCount(const mjModel* m, int* chain,
// count the non-zero columns of the Jacobian returned by mj_jacSum
static int mj_jacSumCount(const mjModel* m, mjData* d, int* chain,
int n, const int* body) {
int nv = m->nv, NV;
mj_markStack(d);
int* bodychain = mj_stackAllocInt(d, nv);
int* tempchain = mj_stackAllocInt(d, nv);
// set first
NV = mj_bodyChain(m, body[0], chain);
// accumulate remaining
for (int i=1; i<n; i++) {
// get body chain
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
continue;
}
// accumulate chains
NV = mju_addChains(tempchain, nv, NV, bodyNV, chain, bodychain);
if (NV) {
mju_copyInt(chain, tempchain, NV);
}
}
mj_freeStack(d);
return NV;
}
// return number of constraint non-zeros, handle dense and dof-less cases
static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) {
// over count for dense allocation
@@ -1456,11 +1558,12 @@ static inline int mj_addConstraintCount(const mjModel* m, int size, int NV) {
// count equality constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
static int mj_ne(const mjModel* m, mjData* d, int* nnz) {
int ne = 0, nnze = 0;
int nv = m->nv, neq = m->neq;
int id[2], size, NV, NV2, *chain = NULL, *chain2 = NULL;
int issparse = (nnz != NULL);
int flex_edgeadr, flex_edgenum;
// disabled or no equality constraints: return
if (mjDISABLED(mjDSBL_EQUALITY) || m->nemax == 0) {
@@ -1535,12 +1638,34 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
NV = 2;
}
break;
case mjEQ_FLEX:
size = m->flex_edgenum[id[0]];
if (!nnz) {
break;
}
flex_edgeadr = m->flex_edgeadr[id[0]];
flex_edgenum = m->flex_edgenum[id[0]];
// process edges of this flex
for (int e=flex_edgeadr; e<flex_edgeadr+flex_edgenum; e++) {
int b1 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[id[0]] + m->flex_edge[2*e+1]];
// accumulate NV
NV += mj_jacDifPairCount(m, chain, b1, b2, issparse);
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown constraint type type %d", m->eq_type[i]); // SHOULD NOT OCCUR
}
// accumulate counts; flex NV already accumulated
ne += mj_addConstraintCount(m, size, NV);
nnze += size*NV;
nnze += (m->eq_type[i]==mjEQ_FLEX) ? NV : size*NV;
}
}
@@ -1555,7 +1680,7 @@ static inline int mj_ne(const mjModel* m, mjData* d, int* nnz) {
// count frictional constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
static int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
int nf = 0, nnzf = 0;
int nv = m->nv, ntendon = m->ntendon;
@@ -1587,7 +1712,7 @@ static inline int mj_nf(const mjModel* m, const mjData* d, int *nnz) {
// count limit constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
static int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
int nnzl = 0, nl = 0;
int ntendon = m->ntendon;
int side;
@@ -1654,10 +1779,9 @@ static inline int mj_nl(const mjModel* m, const mjData* d, int *nnz) {
// count contact constraints, count Jacobian nonzeros if nnz is not NULL
static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
static int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int nnzc = 0, nc = 0;
int ispyramid = mj_isPyramidal(m), ncon = d->ncon;
int issparse = (nnz != NULL);
if (mjDISABLED(mjDSBL_CONTACT) || !ncon) {
return 0;
@@ -1667,19 +1791,49 @@ static inline int mj_nc(const mjModel* m, mjData* d, int* nnz) {
int *chain = mj_stackAllocInt(d, m->nv);
for (int i=0; i < ncon; i++) {
if (d->contact[i].exclude) {
continue;
}
mjContact* con = d->contact + i;
int dim = con->dim;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
int NV = mj_jacDifPairCount(m, chain, b1, b2, issparse);
if (!NV) {
// skip if excluded
if (con->exclude) {
continue;
}
// compute NV only if nnz requested
int NV = 0;
if (nnz) {
// get bodies
int nb = 0, bid[8];
for (int side=0; side < 2; side++) {
// geom
if (con->geom[side]>=0) {
bid[nb++] = m->geom_bodyid[con->geom[side]];
}
// flex vert
else if (con->vert[side] >= 0) {
bid[nb++] = m->flex_vertbodyid[m->flex_vertadr[con->flex[side]] + con->vert[side]];
}
// flex elem
else {
int f = con->flex[side];
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]];
}
}
}
// count non-zeros in merged chain
NV = mj_jacSumCount(m, d, chain, nb, bid);
if (!NV) {
continue;
}
}
// count according to friction type
int dim = con->dim;
if (dim == 1) {
nc++;
nnzc += NV;
@@ -1743,7 +1897,6 @@ void mj_makeConstraint(const mjModel* m, mjData* d) {
mj_instantiateLimit(m, d);
mj_instantiateContact(m, d);
// check sparse allocation
if (mj_isSparse(m)) {
if (d->ne != ne_allocated) {
src/engine/engine_core_constraint.h
+4 -14
View File
@@ -57,25 +57,15 @@ void mj_assignRef(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override solver impedance parameters
void mj_assignImp(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override geom/limit/tendon margin
// assign/clamp contact friction parameters
void mj_assignFriction(const mjModel* m, mjtNum* target, const mjtNum* source);
// assign/override geom margin
mjtNum mj_assignMargin(const mjModel* m, mjtNum source);
// add contact to d->contact list; return 0 if success; 1 if buffer full
MJAPI int mj_addContact(const mjModel* m, mjData* d, const mjContact* con);
// add #size rows to constraint Jacobian; set pos, margin, frictionloss, type, id
// result: 0=success; 1=buffer full
int mj_addConstraint(const mjModel* m, mjData* d,
const mjtNum* jac, const mjtNum* pos,
const mjtNum* margin, mjtNum frictionloss,
int size, int type, int id, int NV, const int* chain);
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2);
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2);
//-------------------------- constraint instantiation ----------------------------------------------
src/engine/engine_core_smooth.c
+236 -4
View File
@@ -386,6 +386,221 @@ void mj_camlight(const mjModel* m, mjData* d) {
// update dynamic BVH; leaf aabbs must be updated before call
void mj_updateDynamicBVH(const mjModel* m, mjData* d, int bvhadr, int bvhnum) {
mj_markStack(d);
int* modified = mj_stackAllocInt(d, bvhnum);
mju_zeroInt(modified, bvhnum);
// mark leafs as modified
for (int i=0; i<bvhnum; i++) {
if (m->bvh_nodeid[bvhadr+i]>=0) {
modified[i] = 1;
}
}
// update non-leafs in backward pass (parents come before children)
for (int i=bvhnum-1; i>=0; i--) {
if (m->bvh_nodeid[bvhadr+i]<0) {
int child1 = m->bvh_child[2*(bvhadr+i)];
int child2 = m->bvh_child[2*(bvhadr+i)+1];
// update if either child is modified
if (modified[child1] || modified[child2]) {
mjtNum* aabb = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + i);
const mjtNum* aabb1 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child1);
const mjtNum* aabb2 = d->bvh_aabb_dyn + 6*(bvhadr - m->nbvhstatic + child2);
// compute new (min, max)
mjtNum xmin[3], xmax[3];
for (int k=0; k<3; k++) {
xmin[k] = mju_min(aabb1[k] - aabb1[k+3], aabb2[k] - aabb2[k+3]);
xmax[k] = mju_max(aabb1[k] + aabb1[k+3], aabb2[k] + aabb2[k+3]);
}
// convert to (center, size)
for (int k=0; k<3; k++) {
aabb[k] = 0.5*(xmax[k]+xmin[k]);
aabb[k+3] = 0.5*(xmax[k]-xmin[k]);
}
modified[i] = 1;
}
}
}
mj_freeStack(d);
}
// compute flex-related quantities
void mj_flex(const mjModel* m, mjData* d) {
int nv = m->nv, issparse = mj_isSparse(m);
int* rowadr = d->flexedge_J_rowadr, *rownnz = d->flexedge_J_rownnz;
mjtNum* J = d->flexedge_J;
// skip if no flexes
if (!m->nflex) {
return;
}
// compute Cartesian positions of flex vertices
for (int f=0; f<m->nflex; f++) {
int vstart = m->flex_vertadr[f];
int vend = m->flex_vertadr[f] + m->flex_vertnum[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]);
}
}
// non-centered: map from local to global
else {
for (int i=vstart; i<vend; i++) {
mju_rotVecMat(d->flexvert_xpos+3*i, m->flex_vert+3*i, d->xmat+9*m->flex_vertbodyid[i]);
mju_addTo3(d->flexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]);
}
}
}
// compute flex element aabb
for (int f=0; f<m->nflex; f++) {
int dim = m->flex_dim[f];
// process elements of this flex
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
const mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[f];
// compute min and max along each global axis
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vert+3*edata[0]);
mju_copy3(xmax, vert+3*edata[0]);
for (int i=1; i<=dim; i++) {
for (int j=0; j<3; j++) {
mjtNum value = vert[3*edata[i]+j];
xmin[j] = mju_min(xmin[j], value);
xmax[j] = mju_max(xmax[j], value);
}
}
// compute aabb (center, size)
int base = m->flex_elemadr[f] + e;
d->flexelem_aabb[6*base+0] = 0.5*(xmax[0]+xmin[0]);
d->flexelem_aabb[6*base+1] = 0.5*(xmax[1]+xmin[1]);
d->flexelem_aabb[6*base+2] = 0.5*(xmax[2]+xmin[2]);
d->flexelem_aabb[6*base+3] = 0.5*(xmax[0]-xmin[0]) + m->flex_radius[f];
d->flexelem_aabb[6*base+4] = 0.5*(xmax[1]-xmin[1]) + m->flex_radius[f];
d->flexelem_aabb[6*base+5] = 0.5*(xmax[2]-xmin[2]) + m->flex_radius[f];
}
}
// update flex bhv_aabb_dyn if needed
if (!mjDISABLED(mjDSBL_MIDPHASE)) {
for (int f=0; f<m->nflex; f++) {
if (m->flex_bvhadr[f]>=0) {
int flex_bvhadr = m->flex_bvhadr[f];
int flex_bvhnum = m->flex_bvhnum[f];
// copy element aabbs to bhv leaf aabbs
for (int i=flex_bvhadr; i<flex_bvhadr+flex_bvhnum; i++) {
if (m->bvh_nodeid[i]>=0) {
mju_copy(d->bvh_aabb_dyn + 6*(i - m->nbvhstatic),
d->flexelem_aabb + 6*(m->flex_elemadr[f] + m->bvh_nodeid[i]), 6);
}
}
// update dynamic BVH
mj_updateDynamicBVH(m, d, m->flex_bvhadr[f], m->flex_bvhnum[f]);
}
}
}
// allocate space
mj_markStack(d);
mjtNum* jac1 = mj_stackAllocNum(d, 3*nv);
mjtNum* jac2 = mj_stackAllocNum(d, 3*nv);
mjtNum* jacdif = mj_stackAllocNum(d, 3*nv);
int* chain = issparse ? mj_stackAllocInt(d, nv) : NULL;
// clear Jacobian: sparse or dense
if (issparse) {
mju_zeroInt(rowadr, m->nflexedge);
mju_zeroInt(rownnz, m->nflexedge);
} else {
mju_zero(J, m->nflexedge*nv);
}
// 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] ||
(m->flex_edgeequality[f]==0 &&
m->flex_edgestiffness[f]==0 && m->flex_edgedamping[f]==0)) {
continue;
}
// process edges of this flex
int vbase = m->flex_vertadr[f];
int ebase = m->flex_edgeadr[f];
for (int e=0; e<m->flex_edgenum[f]; e++) {
int v1 = m->flex_edge[2*(ebase+e)];
int v2 = m->flex_edge[2*(ebase+e)+1];
int b1 = m->flex_vertbodyid[vbase+v1];
int b2 = m->flex_vertbodyid[vbase+v2];
mjtNum* pos1 = d->flexvert_xpos + 3*(vbase+v1);
mjtNum* pos2 = d->flexvert_xpos + 3*(vbase+v2);
// vec = unit vector from v1 to v2, compute edge length
mjtNum vec[3];
mju_sub3(vec, pos2, pos1);
d->flexedge_length[ebase+e] = mju_normalize3(vec);
// sparse edge Jacobian
if (issparse) {
// set rowadr
if (ebase+e>0) {
rowadr[ebase+e] = rowadr[ebase+e-1] + rownnz[ebase+e-1];
}
// get endpoint Jacobians, subtract
int NV = mj_jacDifPair(m, d, chain, b1, b2, pos1, pos2,
jac1, jac2, jacdif, NULL, NULL, NULL);
// no dofs: skip
if (!NV) {
continue;
}
// apply chain rule to compute edge Jacobian
mju_mulMatTVec(J + rowadr[ebase+e], jacdif, vec, 3, NV);
// copy sparsity info
rownnz[ebase+e] = NV;
mju_copyInt(d->flexedge_J_colind + rowadr[ebase+e], chain, NV);
}
// dense edge Jacobian
else {
// get endpoint Jacobians, subtract
mj_jac(m, d, jac1, NULL, pos1, b1);
mj_jac(m, d, jac2, NULL, pos2, b2);
mju_sub(jacdif, jac2, jac1, 3*nv);
// apply chain rule to compute edge Jacobian
mju_mulMatTVec(J + (ebase+e)*nv, jacdif, vec, 3, nv);
}
}
}
mj_freeStack(d);
}
// compute tendon lengths and moments
void mj_tendon(const mjModel* m, mjData* d) {
int issparse = mj_isSparse(m), nv = m->nv, nten = m->ntendon;
@@ -893,8 +1108,15 @@ void mj_transmission(const mjModel* m, mjData* d) {
int counter = 0;
for (int j=0; j < d->ncon; j++) {
const mjContact* con = d->contact+j;
int b1 = m->geom_bodyid[con->geom1];
int b2 = m->geom_bodyid[con->geom2];
// contact involving flex, continue
if (con->geom[0]<0 || con->geom[1]<0) {
continue;
}
// get body ids
int b1 = m->geom_bodyid[con->geom[0]];
int b2 = m->geom_bodyid[con->geom[1]];
// irrelevant contact, continue
if (b1 != id && b2 != id) {
@@ -1549,6 +1771,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// get contact pointer
con = d->contact+i;
// skip contact involving flex
if (con->geom[0]<0 || con->geom[1]<0) {
continue;
}
// tmp = contact-local force:torque vector
mj_contactForce(m, d, i, lfrc);
@@ -1558,7 +1785,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
// body 1
int k;
if ((k = m->geom_bodyid[con->geom1])) {
if ((k = m->geom_bodyid[con->geom[0]])) {
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
@@ -1567,7 +1794,7 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
}
// body 2
if ((k = m->geom_bodyid[con->geom2])) {
if ((k = m->geom_bodyid[con->geom[1]])) {
// tmp = subtree CoM-based torque_force vector
mju_transformSpatial(cfrc_com, cfrc, 1, d->subtree_com+3*m->body_rootid[k], con->pos, 0);
@@ -1631,6 +1858,11 @@ void mj_rnePostConstraint(const mjModel* m, mjData* d) {
i++;
break;
case mjEQ_FLEX:
// increment edgenum rows
i += m->flex_edgenum[m->eq_obj1id[id]];
break;
default:
mjERROR("unknown constraint type type %d", m->eq_type[id]); // SHOULD NOT OCCUR
}
src/engine/engine_core_smooth.h
+3
View File
@@ -33,6 +33,9 @@ MJAPI void mj_comPos(const mjModel* m, mjData* d);
// compute camera and light positions and orientations
MJAPI void mj_camlight(const mjModel* m, mjData* d);
// compute flex-related quantities
MJAPI void mj_flex(const mjModel* m, mjData* d);
// compute tendon lengths, velocities and moment arms
MJAPI void mj_tendon(const mjModel* m, mjData* d);
src/engine/engine_derivative.c
+44 -73
View File
@@ -25,6 +25,7 @@
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "engine/engine_util_spatial.h"
#include "engine/engine_util_sparse.h"
@@ -693,41 +694,6 @@ static void mjd_rne_vel(const mjModel* m, mjData* d) {
//--------------------- utility functions for (d force / d vel) Jacobians --------------------------
// construct sparse Jacobian structure of body; return nnz
static int bodyJacSparse(const mjModel* m, int body, int* ind) {
// skip fixed bodies
while (body > 0 && m->body_dofnum[body] == 0) {
body = m->body_parentid[body];
}
// body is not movable: empty chain
if (body == 0) {
return 0;
}
// count dofs
int nnz = 0;
int dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof >= 0) {
nnz++;
dof = m->dof_parentid[dof];
}
// fill array in reverse (increasing dof)
int cnt = 0;
dof = m->body_dofadr[body] + m->body_dofnum[body] - 1;
while (dof >= 0) {
ind[nnz-cnt-1] = dof;
cnt++;
dof = m->dof_parentid[dof];
}
return nnz;
}
// add J'*B*J to qDeriv
static void addJTBJ(const mjModel* m, mjData* d, const mjtNum* J, const mjtNum* B, int n) {
int nv = m->nv;
@@ -778,30 +744,23 @@ static void addJTBJSparse(
// compute qDeriv(k,p) += sum_{i,j} ( J(i,k)*B(i,j)*J(j,p) )
for (int i = 0; i < n; i++) {
for (int j = 0; j < n; j++) {
int offset_i = offset+i, offset_j = offset+j;
if (!B[i*n+j]) {
continue;
}
mju_zero(row, nv);
// loop over non-zero elements of J(i,:)
for (int k = 0; k < J_rownnz[offset+i]; k++) {
int ik = J_rowadr[offset+i] + k;
mjtNum scl = J[ik]*B[i*n+j];
for (int k = 0; k < J_rownnz[offset_i]; k++) {
int ik = J_rowadr[offset_i] + k;
int colik = J_colind[ik];
// loop over non-zero elements of J(j,:)
// (pJ is the sparse column index into J)
for (int pJ = 0; pJ < J_rownnz[offset+j]; pJ++) {
int adr = J_rowadr[offset+j] + pJ;
row[J_colind[adr]] = scl * J[adr];
}
// row = J(i,k)*B(i,j)*J(j,:)
mju_scl(row, J + J_rowadr[offset_j], J[ik]*B[i*n+j], J_rownnz[offset_j]);
// add row to qDeriv(k,:)
// (pD is the sparse column index into qDeriv)
for (int pD = 0; pD < d->D_rownnz[k]; pD++) {
int adr = d->D_rowadr[k] + pD;
d->qDeriv[adr] += row[d->D_colind[adr]];
}
// qDeriv(k,:) += row
mju_addToSparseInc(d->qDeriv + d->D_rowadr[colik], row,
d->D_rownnz[colik], d->D_colind + d->D_rowadr[colik],
J_rownnz[offset_j], J_colind + J_rowadr[offset_j]);
}
}
}
@@ -1242,7 +1201,7 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
if (mj_isSparse(m)) {
// get sparse body Jacobian structure
nnz = bodyJacSparse(m, bodyid, colind);
nnz = mj_bodyChain(m, bodyid, colind);
// prepare rownnz, rowadr, colind for all 6 rows
for (int i=0; i < 6; i++) {
@@ -1282,16 +1241,11 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
// subtract translational component from grom velocity
mju_subFrom3(lvel+3, lwind+3);
// get body global Jacobian: rotation then translation
mj_jacGeom(m, d, J+3*nv, J, geomid);
// compress geom Jacobian in-place
// get geom global Jacobian: rotation then translation
if (mj_isSparse(m)) {
for (int i=0; i < 6; i++) {
for (int k=0; k < nnz; k++) {
J[i*nnz+k] = J[i*nv+colind[k]];
}
}
mj_jacSparse(m, d, J+3*nnz, J, d->geom_xpos+3*geomid, m->geom_bodyid[geomid], nnz, colind);
} else {
mj_jacGeom(m, d, J+3*nv, J, geomid);
}
// rotate (compressed) Jacobian to local frame
@@ -1334,6 +1288,7 @@ void mjd_ellipsoidFluid(const mjModel* m, mjData* d, int bodyid) {
}
// fluid forces based on inertia-box approximation
void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
{
@@ -1368,23 +1323,16 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
// subtract translational component from body velocity
mju_subFrom3(lvel+3, lwind+3);
// get body global Jacobian: rotation then translation
mj_jacBodyCom(m, d, J+3*nv, J, i);
// init with dense
int nnz = nv;
// prepare for sparse
// sparse Jacobian
if (mj_isSparse(m)) {
// get sparse body Jacobian structure
nnz = bodyJacSparse(m, i, colind);
nnz = mj_bodyChain(m, i, colind);
// compress body Jacobian in-place
for (int j=0; j < 6; j++) {
for (int k=0; k < nnz; k++) {
J[j*nnz+k] = J[j*nv+colind[k]];
}
}
// get sparse jacBodyCom
mj_jacSparse(m, d, J+3*nnz, J, d->xipos+3*i, i, nnz, colind);
// prepare rownnz, rowadr, colind for all 6 rows
rownnz[0] = nnz;
@@ -1398,6 +1346,11 @@ void mjd_inertiaBoxFluid(const mjModel* m, mjData* d, int i)
}
}
// dense Jacobian
else {
mj_jacBodyCom(m, d, J+3*nv, J, i);
}
// rotate (compressed) Jacobian to local frame
mju_mulMatTMat(tmp, d->ximat+9*i, J, 3, 3, nnz);
mju_copy(J, tmp, 3*nnz);
@@ -1517,6 +1470,24 @@ void mjd_passive_vel(const mjModel* m, mjData* d) {
}
}
// flex edge damping
for (int f=0; f<m->nflex; f++) {
if (!m->flex_rigid[f] && m->flex_edgedamping[f]) {
mjtNum B = -m->flex_edgedamping[f];
// process edges of this flex
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
// add sparse or dense
if (mj_isSparse(m)) {
addJTBJSparse(m, d, d->flexedge_J, &B, 1, e,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind);
} else {
addJTBJ(m, d, d->flexedge_J+e*nv, &B, 1);
}
}
}
}
// tendon damping
for (int i=0; i < m->ntendon; i++) {
if (m->tendon_damping[i] > 0) {
src/engine/engine_forward.c
+11 -2
View File
@@ -127,6 +127,7 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
mj_kinematics(m, d);
mj_comPos(m, d);
mj_camlight(m, d);
mj_flex(m, d);
mj_tendon(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
@@ -182,6 +183,14 @@ void mj_fwdPosition(const mjModel* m, mjData* d) {
void mj_fwdVelocity(const mjModel* m, mjData* d) {
TM_START;
// flexedge velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge,
d->flexedge_J_rownnz, d->flexedge_J_rowadr, d->flexedge_J_colind, NULL);
} else {
mju_mulMatVec(d->flexedge_velocity, d->flexedge_J, d->qvel, m->nflexedge, m->nv);
}
// tendon velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
@@ -190,10 +199,10 @@ void mj_fwdVelocity(const mjModel* m, mjData* d) {
mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv);
}
// actuator velocity
// actuator velocity: always dense
mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv);
// standard velocity computations
// com-based velocities, passive forces, constraint references
mj_comVel(m, d);
mj_passive(m, d);
mj_referenceConstraint(m, d);
src/engine/engine_inverse.c
+3 -23
View File
@@ -25,7 +25,7 @@
#include "engine/engine_derivative.h"
#include "engine/engine_io.h"
#include "engine/engine_macro.h"
#include "engine/engine_passive.h"
#include "engine/engine_forward.h"
#include "engine/engine_sensor.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -40,6 +40,7 @@ void mj_invPosition(const mjModel* m, mjData* d) {
mj_kinematics(m, d);
mj_comPos(m, d);
mj_camlight(m, d);
mj_flex(m, d);
mj_tendon(m, d);
TM_END(mjTIMER_POS_KINEMATICS);
@@ -63,28 +64,7 @@ void mj_invPosition(const mjModel* m, mjData* d) {
// velocity-dependent computations
void mj_invVelocity(const mjModel* m, mjData* d) {
TM_START;
// tendon velocity: dense or sparse
if (mj_isSparse(m)) {
mju_mulMatVecSparse(d->ten_velocity, d->ten_J, d->qvel, m->ntendon,
d->ten_J_rownnz, d->ten_J_rowadr, d->ten_J_colind, NULL);
} else {
mju_mulMatVec(d->ten_velocity, d->ten_J, d->qvel, m->ntendon, m->nv);
}
// actuator velocity
mju_mulMatVec(d->actuator_velocity, d->actuator_moment, d->qvel, m->nu, m->nv);
// standard velocity computations
mj_comVel(m, d);
mj_passive(m, d);
mj_referenceConstraint(m, d);
// compute qfrc_bias with abbreviated RNE (without acceleration)
mj_rne(m, d, 0, d->qfrc_bias);
TM_END(mjTIMER_VELOCITY);
mj_fwdVelocity(m, d);
}
src/engine/engine_io.c
+69 -39
View File
@@ -144,6 +144,11 @@ void mj_defaultOption(mjOption* opt) {
// solver overrides
opt->o_margin = 0;
mj_defaultSolRefImp(opt->o_solref, opt->o_solimp);
opt->o_friction[0] = 1;
opt->o_friction[1] = 1;
opt->o_friction[2] = 0.005;
opt->o_friction[3] = 0.0001;
opt->o_friction[4] = 0.0001;
// discrete options
opt->integrator = mjINT_EULER;
@@ -420,7 +425,7 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
#if (__has_builtin(__builtin_add_overflow) && __has_builtin(__builtin_mul_overflow)) \
|| (defined(__GNUC__) && __GNUC__ >= 5)
// supported by GCC and Clang
int to_add = 0;
size_t to_add = 0;
if (__builtin_mul_overflow(nc, nr, &to_add)) return 0;
if (__builtin_mul_overflow(to_add, type_size, &to_add)) return 0;
if (__builtin_add_overflow(to_add, SKIP(*offset), &to_add)) return 0;
@@ -438,18 +443,20 @@ static int safeAddToBufferSize(intptr_t* offset, size_t* nbuffer,
// allocate and initialize mjModel structure
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
int nmeshgraph, int nskin, int nskinvert, int nskintexvert, int nskinface,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
int neq, int ntendon, int nwrap, int nsensor,
int nnumeric, int nnumericdata, int ntext, int ntextdata,
int ntuple, int ntupledata, int nkey, int nmocap, int nplugin,
int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
mjModel* mj_makeModel(
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 nflexelemdata,
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,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata, int ntex,
int ntexdata, int nmat, int npair, int nexclude, int neq, int ntendon,
int nwrap, int nsensor, int nnumeric, int nnumericdata, int ntext,
int ntextdata, int ntuple, int ntupledata, int nkey, int nmocap,
int nplugin, int npluginattr, int nuser_body, int nuser_jnt, int nuser_geom,
int nuser_site, int nuser_cam, int nuser_tendon, int nuser_actuator,
int nuser_sensor, int nnames, int npaths) {
intptr_t offset = 0;
// allocate mjModel
@@ -466,11 +473,21 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->na = na;
m->nbody = nbody;
m->nbvh = nbvh;
m->nbvhstatic = nbvhstatic;
m->nbvhdynamic = nbvhdynamic;
m->njnt = njnt;
m->ngeom = ngeom;
m->nsite = nsite;
m->ncam = ncam;
m->nlight = nlight;
m->nflex = nflex;
m->nflexvert = nflexvert;
m->nflexedge = nflexedge;
m->nflexelem = nflexelem;
m->nflexelemdata = nflexelemdata;
m->nflexshelldata = nflexshelldata;
m->nflexevpair = nflexevpair;
m->nflextexcoord = nflextexcoord;
m->nmesh = nmesh;
m->nmeshvert = nmeshvert;
m->nmeshnormal = nmeshnormal;
@@ -514,7 +531,7 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
m->nuser_sensor = nuser_sensor;
m->nnames = nnames;
m->nnames_map = mjLOAD_MULTIPLE
* (nbody + njnt + ngeom + nsite + ncam + nlight + nmesh
* (nbody + njnt + ngeom + nsite + ncam + nlight + nflex + nmesh
+ nskin + nhfield + ntex + nmat + npair + nexclude + neq
+ ntendon + nu + nsensor + nnumeric + ntext + ntuple
+ nkey + nplugin);
@@ -578,26 +595,28 @@ mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int n
return m;
}
// copy mjModel, if dest==NULL create new model
mjModel* mj_copyModel(mjModel* dest, const mjModel* src) {
void* save_bufptr;
// allocate new model if needed
if (!dest) {
dest = mj_makeModel(src->nq, src->nv, src->nu, src->na, src->nbody, src->nbvh, src->njnt,
src->ngeom, src->nsite, src->ncam, src->nlight, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor,
src->nnumeric, src->nnumericdata, src->ntext, src->ntextdata,
src->ntuple, src->ntupledata, src->nkey, src->nmocap, src->nplugin,
src->npluginattr, src->nuser_body, src->nuser_jnt, src->nuser_geom,
src->nuser_site, src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
dest = mj_makeModel(
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->nflexelem, src->nflexelemdata, src->nflexshelldata,
src->nflexevpair, src->nflextexcoord, src->nmesh, src->nmeshvert,
src->nmeshnormal, src->nmeshtexcoord, src->nmeshface, src->nmeshgraph,
src->nskin, src->nskinvert, src->nskintexvert, src->nskinface,
src->nskinbone, src->nskinbonevert, src->nhfield, src->nhfielddata,
src->ntex, src->ntexdata, src->nmat, src->npair, src->nexclude,
src->neq, src->ntendon, src->nwrap, src->nsensor, src->nnumeric,
src->nnumericdata, src->ntext, src->ntextdata, src->ntuple,
src->ntupledata, src->nkey, src->nmocap, src->nplugin, src->npluginattr,
src->nuser_body, src->nuser_jnt, src->nuser_geom, src->nuser_site,
src->nuser_cam, src->nuser_tendon, src->nuser_actuator,
src->nuser_sensor, src->nnames, src->npaths);
}
if (!dest) {
mjERROR("failed to make mjModel. Invalid sizes.");
@@ -762,7 +781,9 @@ mjModel* mj_loadModel(const char* filename, const mjVFS* vfs) {
ints[28], ints[29], ints[30], ints[31], ints[32], ints[33], ints[34],
ints[35], ints[36], ints[37], ints[38], ints[39], ints[40], ints[41],
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[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]);
if (!m || m->nbuffer != sizes[getnsize()-1]) {
mju_closeResource(r);
mju_warning("Corrupted model, wrong size parameters");
@@ -1508,12 +1529,6 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
mju_zeroInt(d->solver_nnz, mjNISLAND);
mju_zero(d->solver_fwdinv, 2);
// clear collision diagnostics
d->nbodypair_broad = 0;
d->nbodypair_narrow = 0;
d->ngeompair_mid = 0;
d->ngeompair_narrow = 0;
// clear variable sizes
d->ne = 0;
d->nf = 0;
@@ -1751,6 +1766,8 @@ static int numObjects(const mjModel* m, mjtObj objtype) {
return m->ncam;
case mjOBJ_LIGHT:
return m->nlight;
case mjOBJ_FLEX:
return m->nflex;
case mjOBJ_MESH:
return m->nmesh;
case mjOBJ_SKIN:
@@ -1795,6 +1812,8 @@ const char* mj_validateReferences(const mjModel* m) {
// ntarget: number of elements in array where references are pointing
// numarray: if refarray is an adr array, numarray is the corresponding num array, otherwise 0
// add flex fields (b/303056369)
#define MJMODEL_REFERENCES \
X(body_parentid, nbody, nbody , 0 ) \
X(body_rootid, nbody, nbody , 0 ) \
@@ -1948,11 +1967,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->npair; i++) {
int pair_body1 = (m->pair_signature[i] & 0xFFFF) - 1;
int pair_body1 = (m->pair_signature[i] & 0xFFFF);
if (pair_body1 >= m->nbody || pair_body1 < 0) {
return "Invalid model: pair_body1 out of bounds.";
}
int pair_body2 = (m->pair_signature[i] >> 16) - 1;
int pair_body2 = (m->pair_signature[i] >> 16);
if (pair_body2 >= m->nbody || pair_body2 < 0) {
return "Invalid model: pair_body2 out of bounds.";
}
@@ -1991,6 +2010,17 @@ const char* mj_validateReferences(const mjModel* m) {
}
break;
case mjEQ_FLEX:
if (obj1id >= m->nflex || obj1id < 0) {
return "Invalid model: eq_obj1id out of bounds.";
}
// -1 is the value used if second object is omitted
if (obj2id != -1) {
return "Invalid model: eq_obj2id must be -1.";
}
break;
default:
// might occur in case of the now-removed distance equality constraint
mjERROR("unknown equality constraint type.");
@@ -2096,11 +2126,11 @@ const char* mj_validateReferences(const mjModel* m) {
}
}
for (int i=0; i < m->nexclude; i++) {
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF) - 1;
int exclude_body1 = (m->exclude_signature[i] & 0xFFFF);
if (exclude_body1 >= m->nbody || exclude_body1 < 0) {
return "Invalid model: exclude_body1 out of bounds.";
}
int exclude_body2 = (m->exclude_signature[i] >> 16) - 1;
int exclude_body2 = (m->exclude_signature[i] >> 16);
if (exclude_body2 >= m->nbody || exclude_body2 < 0) {
return "Invalid model: exclude_body2 out of bounds.";
}
src/engine/engine_io.h
+4 -3
View File
@@ -51,9 +51,10 @@ void mj_defaultStatistic(mjStatistic* stat);
//------------------------------- mjModel ----------------------------------------------------------
// allocate mjModel
mjModel* mj_makeModel(int nq, int nv, int nu, int na, int nbody, int nbvh, int njnt,
int ngeom, int nsite, int ncam, int nlight,
int nmesh, int nmeshvert, int nmeshnormal, int nmeshtexcoord, int nmeshface,
mjModel* mj_makeModel(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 nflexelemdata, 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,
int nskinbone, int nskinbonevert, int nhfield, int nhfielddata,
int ntex, int ntexdata, int nmat, int npair, int nexclude,
src/engine/engine_island.c
+2 -2
View File
@@ -235,8 +235,8 @@ static int treeFirst(const mjModel* m, const mjData* d, int tree[2], int i) {
if (efc_type == mjCNSTR_CONTACT_FRICTIONLESS ||
efc_type == mjCNSTR_CONTACT_PYRAMIDAL ||
efc_type == mjCNSTR_CONTACT_ELLIPTIC) {
tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom1]];
tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom2]];
tree[0] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[0]]];
tree[1] = m->body_treeid[m->geom_bodyid[d->contact[efc_id].geom[1]]];
// handle static bodies
if (tree[0] < 0) {
src/engine/engine_passive.c
+42 -2
View File
@@ -51,6 +51,11 @@ void mj_passive(const mjModel* m, mjData* d) {
for (int i=0; i < m->njnt; i++) {
stiffness = m->jnt_stiffness[i];
// disabled : nothing to do
if (stiffness==0) {
continue;
}
int padr = m->jnt_qposadr[i];
int dadr = m->jnt_dofadr[i];
@@ -86,8 +91,38 @@ void mj_passive(const mjModel* m, mjData* d) {
// dof-level dampers
for (int i=0; i < m->nv; i++) {
damping = m->dof_damping[i];
d->qfrc_passive[i] -= damping*d->qvel[i];
if ((damping = m->dof_damping[i]) != 0) {
d->qfrc_passive[i] -= damping*d->qvel[i];
}
}
// flexedge-level spring-dampers
for (int f=0; f < m->nflex; f++) {
stiffness = m->flex_edgestiffness[f];
damping = m->flex_edgedamping[f];
// disabled or rigid: nothing to do
if (m->flex_rigid[f] || (stiffness==0 && damping==0)) {
continue;
}
// process edges of this flex (global edge index)
int edgeend = m->flex_edgeadr[f] + m->flex_edgenum[f];
for (int e=m->flex_edgeadr[f]; e < edgeend; e++) {
// compute spring-damper force along edge
frc = stiffness * (m->flexedge_length0[e] - d->flexedge_length[e])
- damping * d->flexedge_velocity[e];
// transform to joint torque, add to qfrc_passive: dense or sparse
if (issparse) {
int end = d->flexedge_J_rowadr[e] + d->flexedge_J_rownnz[e];
for (int j=d->flexedge_J_rowadr[e]; j < end; j++) {
d->qfrc_passive[d->flexedge_J_colind[j]] += d->flexedge_J[j] * frc;
}
} else {
mju_addToScl(d->qfrc_passive, d->flexedge_J+e*nv, frc, nv);
}
}
}
// tendon-level spring-dampers
@@ -95,6 +130,11 @@ void mj_passive(const mjModel* m, mjData* d) {
stiffness = m->tendon_stiffness[i];
damping = m->tendon_damping[i];
// disabled : nothing to do
if (stiffness == 0 && damping == 0) {
continue;
}
// compute spring force along tendon
mjtNum length = d->ten_length[i];
mjtNum lower = m->tendon_lengthspring[2*i];
src/engine/engine_print.c
+37 -14
View File
@@ -361,6 +361,14 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->nbody) fprintf(fp, "\n");
// BVHs
for (int i=0; i<m->nbvh; i++) {
fprintf(fp, "\nBVH %d:\n", i);
object_class = &m->nbvh;
MJMODEL_POINTERS
}
if (m->nbvh) fprintf(fp, "\n");
// joints
for (int i=0; i < m->njnt; i++) {
fprintf(fp, "\nJOINT %d:\n", i);
@@ -419,6 +427,16 @@ void mj_printFormattedModel(const mjModel* m, const char* filename, const char*
}
if (m->nlight) fprintf(fp, "\n");
// flexes
for (int i=0; i<m->nflex; i++) {
fprintf(fp, "\nFLEX %d:\n", i);
fprintf(fp, " " NAME_FORMAT, "name");
fprintf(fp, " %s\n", m->names + m->name_flexadr[i]);
object_class = &m->nflex;
MJMODEL_POINTERS
}
if (m->nflex) fprintf(fp, "\n");
// meshes
for (int i=0; i < m->nmesh; i++) {
fprintf(fp, "\nMESH %d:\n", i);
@@ -914,6 +932,18 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("CDOF", m->nv, 6, d->cdof, fp, float_format);
printArray("CINERT", m->nbody, 10, d->cinert, fp, float_format);
printArray("FLEXVERT_XPOS", m->nflexvert, 3, d->flexvert_xpos, fp, float_format);
printArray("FLEXELEM_AABB", m->nflexelem, 6, d->flexelem_aabb, fp, float_format);
if (!mj_isSparse(m)) {
printArray("FLEXEDGE_J", m->nflexedge, m->nv, d->flexedge_J, fp, float_format);
} else {
printArrayInt("FLEXEDGE_J_ROWNNZ", m->nflexedge, 1, d->flexedge_J_rownnz, fp);
printArrayInt("FLEXEDGE_J_ROWADR", m->nflexedge, 1, d->flexedge_J_rowadr, fp);
printSparse("FLEXEDGE_J", d->flexedge_J, m->nflexedge, d->flexedge_J_rownnz,
d->flexedge_J_rowadr, d->flexedge_J_colind, fp, float_format);
}
printArray("FLEXEDGE_LENGTH", m->nflexedge, 1, d->flexedge_length, fp, float_format);
printArray("TEN_LENGTH", m->ntendon, 1, d->ten_length, fp, float_format);
if (!mj_isSparse(m)) {
printArray("TEN_MOMENT", m->ntendon, m->nv, d->ten_J, fp, float_format);
@@ -1001,20 +1031,12 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
// contact
fprintf(fp, "CONTACT\n");
for (int i=0; i < d->ncon; i++) {
fprintf(fp, " %d:\n dim %d\n geom ", i, d->contact[i].dim);
const char* geom1 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom1);
if (geom1) {
fprintf(fp, "%s ", geom1);
} else {
fprintf(fp, "%d ", d->contact[i].geom1);
}
const char* geom2 = mj_id2name(m, mjOBJ_GEOM, d->contact[i].geom2);
if (geom2) {
if (geom1) fprintf(fp, " "); // two spaces between two names
fprintf(fp, "%s\n", geom2);
} else {
fprintf(fp, "%d\n", d->contact[i].geom2);
}
fprintf(fp, " %d:\n dim %d\n", i, d->contact[i].dim);
fprintf(fp, " gfev %d %d %d %d : %d %d %d %d\n",
d->contact[i].geom[0], d->contact[i].flex[0],
d->contact[i].elem[0], d->contact[i].vert[0],
d->contact[i].geom[1], d->contact[i].flex[1],
d->contact[i].elem[1], d->contact[i].vert[1]);
fprintf(fp, " exclude %d\n efc_address %d\n",
d->contact[i].exclude, d->contact[i].efc_address);
printVector(" solref ", d->contact[i].solref, mjNREF, fp, float_format);
@@ -1054,6 +1076,7 @@ void mj_printFormattedData(const mjModel* m, mjData* d, const char* filename,
printArray("EFC_D", d->nefc, 1, d->efc_D, fp, float_format);
printArray("EFC_R", d->nefc, 1, d->efc_R, fp, float_format);
printArray("FLEXEDGE_VELOCITY", m->nflexedge, 1, d->flexedge_velocity, fp, float_format);
printArray("TEN_VELOCITY", m->ntendon, 1, d->ten_velocity, fp, float_format);
printArray("ACTUATOR_VELOCITY", m->nu, 1, d->actuator_velocity, fp, float_format);
src/engine/engine_ray.c
+181 -3
View File
@@ -629,7 +629,7 @@ mjtNum mju_rayTree(const mjModel* m, const mjData* d, int id, const mjtNum* pnt,
const mjtNum* vec) {
const int meshid = m->geom_dataid[id];
const int bvhadr = m->mesh_bvhadr[meshid];
const int* faceid = m->bvh_geomid + bvhadr;
const int* faceid = m->bvh_nodeid + bvhadr;
const mjtNum* bvh = m->bvh_aabb + 6*bvhadr;
const int* child = m->bvh_child + 2*bvhadr;
@@ -835,6 +835,184 @@ mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
// intersect ray with flex, return nearest vertex id
mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]) {
int dim = m->flex_dim[flexid];
// compute bounding box
mjtNum box[3][2] = {{0, 0}, {0, 0}, {0, 0}};
mjtNum* vert = d->flexvert_xpos + 3*m->flex_vertadr[flexid];
for (int i=0; i<m->flex_vertnum[flexid]; i++) {
for (int j=0; j<3; j++) {
// update minimum along side j
if (box[j][0]>vert[3*i+j] || i==0) {
box[j][0] = vert[3*i+j];
}
// update maximum along side j
if (box[j][1]<vert[3*i+j] || i==0) {
box[j][1] = vert[3*i+j];
}
}
}
// adjust box for radius
mjtNum radius = m->flex_radius[flexid];
for (int j=0; j<3; j++) {
box[j][0] -= radius;
box[j][1] += radius;
}
// construct box geom
mjtNum pos[3], size[3], mat[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
for (int j=0; j<3; j++) {
pos[j] = 0.5*(box[j][0]+box[j][1]);
size[j] = 0.5*(box[j][1]-box[j][0]);
}
// apply bounding-box filter
if (ray_box(pos, mat, size, pnt, vec, NULL)<0) {
return -1;
}
// construct basis vectors of normal plane
mjtNum b0[3] = {1, 1, 1}, b1[3];
if (mju_abs(vec[0])>=mju_abs(vec[1]) && mju_abs(vec[0])>=mju_abs(vec[2])) {
b0[0] = 0;
} else if (mju_abs(vec[1])>=mju_abs(vec[2])) {
b0[1] = 0;
} else {
b0[2] = 0;
}
mju_addScl3(b1, b0, vec, -mju_dot3(vec, b0)/mju_dot3(vec, vec));
mju_normalize3(b1);
mju_cross(b0, b1, vec);
mju_normalize3(b0);
// init solution
mjtNum x = -1;
// check edges if rendered, or if skin
if (flg_edge || (dim>1 && flg_skin)) {
for (int e=m->flex_edgeadr[flexid];
e<m->flex_edgeadr[flexid]+m->flex_edgenum[flexid]; e++) {
// get vertices for this edge
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid]+m->flex_edge[2*e+1]);
// construct capsule geom
mju_add3(pos, v1, v2);
mju_scl3(pos, pos, 0.5);
mjtNum dif[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]};
size[0] = radius;
size[1] = 0.5*mju_normalize3(dif);
mjtNum quat[4];
mju_quatZ2Vec(quat, dif);
mju_quat2Mat(mat, quat);
// intersect ray with capsule
mjtNum sol = mju_rayGeom(pos, mat, size, pnt, vec, mjGEOM_CAPSULE);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
// construct intersection point
mjtNum intersect[3];
mju_addScl3(intersect, pnt, vec, sol);
// find nearest vertex
if (mju_dist3(v1, intersect) < mju_dist3(v2, intersect)) {
*vertid = m->flex_edge[2*e];
}
else {
*vertid = m->flex_edge[2*e+1];
}
}
}
}
// check vertices if rendered (and edges not checked)
else if (flg_vert && !(dim>1 && flg_skin)) {
for (int v=0; v<m->flex_vertnum[flexid]; v++) {
// get vertex
mjtNum* vpos = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + v);
// construct sphere geom
size[0] = radius;
// intersect ray with sphere
mjtNum sol = mju_rayGeom(vpos, NULL, size, pnt, vec, mjGEOM_SPHERE);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
*vertid = v;
}
}
}
// check faces if rendered
if (dim>1 && (flg_face || flg_skin)) {
for (int e=0; e<m->flex_elemnum[flexid]; e++) {
// skip if 3D element is not visible
int elayer = m->flex_elemlayer[m->flex_elemadr[flexid]+e];
if (dim==3 && ((flg_skin && elayer>0) || (!flg_skin && elayer!=flex_layer))) {
continue;
}
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[flexid] + e*(dim+1);
mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[0]);
mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[1]);
mjtNum* v3 = d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[2]);
mjtNum* v4 = dim==2 ? NULL : d->flexvert_xpos + 3*(m->flex_vertadr[flexid] + edata[3]);
mjtNum* vptr[4][3] = {{v1, v2, v3}, {v1, v2, v4}, {v1, v3, v4}, {v2, v3, v4}};
int vid[4][3] = {{0, 1, 2}, {0, 1, 3}, {0, 2, 3}, {1, 2, 3}};
// process triangles of this element
for (int i=0; i<(dim==2?1:4); i++) {
// copy vertices into triangle representation
mjtNum v[3][3];
for (int j=0; j<3; j++)
mju_copy3(v[j], vptr[i][j]);
// intersect ray with triangle
mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
// update
if (sol>=0 && (x<0 || sol<x)) {
x = sol;
// construct intersection point
mjtNum intersect[3];
mju_addScl3(intersect, pnt, vec, sol);
// find nearest vertex
mjtNum dist[3] = {
mju_dist3(v[0], intersect),
mju_dist3(v[1], intersect),
mju_dist3(v[2], intersect)
};
if (dist[0]<=dist[1] && dist[0]<=dist[2]) {
*vertid = edata[vid[i][0]];
} else if (dist[1]<=dist[2]){
*vertid = edata[vid[i][1]];
} else {
*vertid = edata[vid[i][2]];
}
}
}
}
}
return x;
}
// intersect ray with skin, return nearest vertex id
mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]) {
@@ -881,7 +1059,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
mju_normalize3(b0);
// init solution
mjtNum x = -1, sol;
mjtNum x = -1;
// process all faces
for (int i=0; i < nface; i++) {
@@ -900,7 +1078,7 @@ mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
}
// solve
sol = ray_triangle(v, pnt, vec, b0, b1);
mjtNum sol = ray_triangle(v, pnt, vec, b0, b1);
// update
if (sol >= 0 && (x < 0 || sol < x)) {
src/engine/engine_ray.h
+5
View File
@@ -58,6 +58,11 @@ MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid,
MJAPI mjtNum mju_rayGeom(const mjtNum* pos, const mjtNum* mat, const mjtNum* size,
const mjtNum* pnt, const mjtNum* vec, int geomtype);
// intersect ray with flex, return nearest vertex id
MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert,
mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
// intersect ray with skin, return nearest vertex id
MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert,
const mjtNum* pnt, const mjtNum* vec, int vertid[1]);
src/engine/engine_sensor.c
+24 -6
View File
@@ -662,7 +662,7 @@ void mj_sensorVel(const mjModel* m, mjData* d) {
// acceleration/force-dependent sensors
void mj_sensorAcc(const mjModel* m, mjData* d) {
int rootid, bodyid, objtype, objid, body1, body2, adr, nusersensor = 0;
int rootid, bodyid, objtype, objid, adr, nusersensor = 0;
int ne = d->ne, nf = d->nf, nefc = d->nefc;
mjtNum tmp[6], conforce[6], conray[3];
mjContact* con;
@@ -713,13 +713,15 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// find contacts in sensor zone, add normal forces
for (int j=0; j < d->ncon; j++) {
// contact pointer, contacting bodies
// contact pointer, contacting bodies (-1 for flex)
con = d->contact + j;
body1 = m->geom_bodyid[con->geom1];
body2 = m->geom_bodyid[con->geom2];
int conbody[2];
for (int k=0; k<2; k++) {
conbody[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] : -1;
}
// select contacts involving sensorized body
if (con->efc_address >= 0 && (bodyid == body1 || bodyid == body2)) {
if (con->efc_address >= 0 && (bodyid == conbody[0] || bodyid == conbody[1])) {
// get contact force:torque in contact frame
mj_contactForce(m, d, j, conforce);
@@ -733,7 +735,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
mju_normalize3(conray);
// flip ray direction if sensor is on body2
if (bodyid == body2) {
if (bodyid == conbody[1]) {
mju_scl3(conray, conray, -1);
}
@@ -945,6 +947,22 @@ void mj_energyPos(const mjModel* m, mjData* d) {
d->energy[0] += 0.5*stiffness*displacement*displacement;
}
}
// add flex-level springs
if (!mjDISABLED(mjDSBL_PASSIVE)) {
for (int i=0; i<m->nflex; i++) {
stiffness = m->flex_edgestiffness[i];
if (m->flex_rigid[i] || stiffness==0) {
continue;
}
// process edges of this flex
for (int e=m->flex_edgeadr[i]; e<m->flex_edgeadr[i]+m->flex_edgenum[i]; e++) {
mjtNum displacement = m->flexedge_length0[e] - d->flexedge_length[e];
d->energy[0] += 0.5*stiffness*displacement*displacement;
}
}
}
}
src/engine/engine_setconst.c
+50 -3
View File
@@ -95,6 +95,7 @@ static void set0(mjModel* m, mjData* d) {
// run remaining computations
mj_crb(m, d);
mj_factorM(m, d);
mj_flex(m, d);
mj_tendon(m, d);
mj_transmission(m, d);
@@ -106,7 +107,9 @@ static void set0(mjModel* m, mjData* d) {
m->light_mode[i] = lightmode[i];
}
// set tendon_length0, actuator_length0
// copy fields
mju_copy(m->flex_xvert0, d->flexvert_xpos, 3*m->nflexvert);
mju_copy(m->flexedge_length0, d->flexedge_length, m->nflexedge);
mju_copy(m->tendon_length0, d->ten_length, m->ntendon);
mju_copy(m->actuator_length0, d->actuator_length, m->nu);
@@ -194,9 +197,42 @@ static void set0(mjModel* m, mjData* d) {
}
}
// compute tendon_invweight0
// compute flexedge_invweight0, tendon_invweight0, actuator_acc0
if (nv) {
for (int i=0; i < m->ntendon; i++) {
// compute flexedge_invweight0
for (int f=0; f<m->nflex; f++) {
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]];
int b2 = m->flex_vertbodyid[m->flex_vertadr[f] + m->flex_edge[2*i+1]];
// accelerate edges that connect simple bodies with no rotations
if (m->body_simple[b1]==2 && m->body_simple[b2]==2) {
m->flexedge_invweight0[i] = (1/m->body_mass[b1] + 1/m->body_mass[b2])/2;
}
// handle general edge
else {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
int end = d->flexedge_J_rowadr[i] + d->flexedge_J_rownnz[i];
for (int j=d->flexedge_J_rowadr[i]; j<end; j++) {
tmp[d->flexedge_J_colind[j]] = d->flexedge_J[j];
}
} else {
mju_copy(tmp, d->flexedge_J+i*nv, nv);
}
// solve into tmp+nv
mj_solveM(m, d, tmp+nv, tmp, 1);
m->flexedge_invweight0[i] = mju_dot(tmp, tmp+nv, nv);
}
}
}
// compute tendon_invweight0
for (int i=0; i<m->ntendon; i++) {
// make dense vector into tmp
if (mj_isSparse(m)) {
mju_zero(tmp, nv);
@@ -385,6 +421,17 @@ static void setStat(mjModel* m, mjData* d) {
}
}
// adjust body size for flex edges involving body
for (int f=0; f<m->nflex; f++) {
for (int e=m->flex_edgeadr[f]; e<m->flex_edgeadr[f]+m->flex_edgenum[f]; e++) {
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]);
}
}
// compute meansize, make sure all sizes are above min
if (m->nbody > 1) {
m->stat.meansize = 0;
src/engine/engine_support.c
+244 -12
View File
@@ -233,6 +233,131 @@ void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int
//-------------------------- sparse chains ---------------------------------------------------------
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2) {
int da1, da2, NV = 0;
// skip fixed bodies
while (b1 && !m->body_dofnum[b1]) {
b1 = m->body_parentid[b1];
}
while (b2 && !m->body_dofnum[b2]) {
b2 = m->body_parentid[b2];
}
// neither body is movable: empty chain
if (b1==0 && b2==0) {
return 0;
}
// intialize last dof address for each body
da1 = m->body_dofadr[b1] + m->body_dofnum[b1] - 1;
da2 = m->body_dofadr[b2] + m->body_dofnum[b2] - 1;
// merge chains
while (da1>=0 || da2>=0) {
chain[NV] = mjMAX(da1, da2);
if (da1==chain[NV]) {
da1 = m->dof_parentid[da1];
}
if (da2==chain[NV]) {
da2 = m->dof_parentid[da2];
}
NV++;
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2) {
// swap bodies if wrong order
if (b1>b2) {
int tmp = b1;
b1 = b2;
b2 = tmp;
}
// init
int n1 = m->body_dofnum[b1], n2 = m->body_dofnum[b2];
// both fixed: nothing to do
if (n1==0 && n2==0) {
return 0;
}
// copy b1 dofs
for (int i=0; i<n1; i++) {
chain[i] = m->body_dofadr[b1] + i;
}
// copy b2 dofs
for (int i=0; i<n2; i++) {
chain[n1+i] = m->body_dofadr[b2] + i;
}
return (n1+n2);
}
// get body chain
int mj_bodyChain(const mjModel* m, int body, int* chain) {
// simple body
if (m->body_simple[body]) {
int dofnum = m->body_dofnum[body];
for (int i=0; i<dofnum; i++) {
chain[i] = m->body_dofadr[body] + i;
}
return dofnum;
}
// general case
else {
// skip fixed bodies
while (body && !m->body_dofnum[body]) {
body = m->body_parentid[body];
}
// not movable: empty chain
if (body==0) {
return 0;
}
// intialize last dof
int da = m->body_dofadr[body] + m->body_dofnum[body] - 1;
int NV = 0;
// construct chain from child to parent
while (da>=0) {
chain[NV++] = da;
da = m->dof_parentid[da];
}
// reverse order of chain: make it increasing
for (int i=0; i<NV/2; i++) {
int tmp = chain[i];
chain[i] = chain[NV-i-1];
chain[NV-i-1] = tmp;
}
return NV;
}
}
//-------------------------- Jacobians -------------------------------------------------------------
// compute 3/6-by-nv Jacobian of global point attached to given body
@@ -374,7 +499,7 @@ void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacA
// compute 3/6-by-nv sparse Jacobian of global point attached to given body
void mj_jacSparse(const mjModel* m, const mjData* d,
mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
int NV, int* chain) {
int NV, const int* chain) {
int da, ci;
mjtNum offset[3], tmp[3], *cdof = d->cdof;
@@ -578,6 +703,80 @@ int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
// dense or sparse weighted sum of multiple body Jacobians at same point
int mj_jacSum(const mjModel* m, mjData* d, int* chain,
int n, const int* body, const mjtNum* weight,
const mjtNum point[3], mjtNum* jac, int flg_rot) {
int nv = m->nv, NV;
mjtNum* jacp = jac;
mjtNum* jacr = flg_rot ? jac + 3*nv : NULL;
mj_markStack(d);
mjtNum* jtmp = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv);
mjtNum* jp = jtmp;
mjtNum* jr = flg_rot ? jtmp + 3*nv : NULL;
// sparse
if (mj_isSparse(m)) {
mjtNum* buf = mj_stackAllocNum(d, flg_rot ? 6*nv : 3*nv);
int* buf_ind = mj_stackAllocInt(d, nv);
int* bodychain = mj_stackAllocInt(d, nv);
// set first
NV = mj_bodyChain(m, body[0], chain);
if (NV) {
// get Jacobian
if (m->body_simple[body[0]]) {
mj_jacSparseSimple(m, d, jacp, jacr, point, body[0], 1, NV, 0);
} else {
mj_jacSparse(m, d, jacp, jacr, point, body[0], NV, chain);
}
// apply weight
mju_scl(jac, jac, weight[0], flg_rot ? 6*NV : 3*NV);
}
// accumulate remaining
for (int i=1; i<n; i++) {
// get body chain and Jacobian
int bodyNV = mj_bodyChain(m, body[i], bodychain);
if (!bodyNV) {
continue;
}
if (m->body_simple[body[i]]) {
mj_jacSparseSimple(m, d, jp, jr, point, body[i], 1, bodyNV, 0);
} else {
mj_jacSparse(m, d, jp, jr, point, body[i], bodyNV, bodychain);
}
// combine sparse matrices
NV = mju_addToSparseMat(jac, jtmp, nv, flg_rot ? 6 : 3, weight[i],
NV, bodyNV, chain, bodychain, buf, buf_ind);
}
}
// dense
else {
// set first
mj_jac(m, d, jacp, jacr, point, body[0]);
mju_scl(jac, jac, weight[0], flg_rot ? 6*nv : 3*nv);
// accumulate remaining
for (int i=1; i<n; i++) {
mj_jac(m, d, jp, jr, point, body[i]);
mju_addToScl(jac, jtmp, weight[i], flg_rot ? 6*nv : 3*nv);
}
NV = nv;
}
mj_freeStack(d);
return NV;
}
//-------------------------- name functions --------------------------------------------------------
// get number of objects and name addresses for given object type
@@ -635,6 +834,14 @@ static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) {
}
mjFALLTHROUGH;
case mjOBJ_FLEX:
*mapadr -= mjLOAD_MULTIPLE*m->nflex;
if (num < 0) {
*padr = m->name_flexadr;
num = m->nflex;
}
mjFALLTHROUGH;
case mjOBJ_MESH:
*mapadr -= mjLOAD_MULTIPLE*m->nmesh;
if (num < 0) {
@@ -1251,8 +1458,8 @@ void mj_applyFT(const mjModel* m, mjData* d,
// allocate local variables
mj_markStack(d);
mjtNum* jacp = mj_stackAllocNum(d, 3*nv);
mjtNum* jacr = mj_stackAllocNum(d, 3*nv);
mjtNum* jacp = force ? mj_stackAllocNum(d, 3*nv) : NULL;
mjtNum* jacr = torque ? mj_stackAllocNum(d, 3*nv) : NULL;
mjtNum* qforce = mj_stackAllocNum(d, nv);
// make sure body is in range
@@ -1260,17 +1467,42 @@ void mj_applyFT(const mjModel* m, mjData* d,
mjERROR("invalid body %d", body);
}
// compute Jacobians
mj_jac(m, d, jacp, jacr, point, body);
// sparse case
if (mj_isSparse(m)) {
// construct chain and sparse Jacobians
int* chain = mj_stackAllocInt(d, nv);
int NV = mj_bodyChain(m, body, chain);
mj_jacSparse(m, d, jacp, jacr, point, body, NV, chain);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, NV);
for (int i=0; i<NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, NV);
for (int i=0; i<NV; i++) {
qfrc_target[chain[i]] += qforce[i];
}
}
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
// dense case
else {
// compute Jacobians
mj_jac(m, d, jacp, jacr, point, body);
// compute J'*f and accumulate
if (force) {
mju_mulMatTVec(qforce, jacp, force, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
}
if (torque) {
mju_mulMatTVec(qforce, jacr, torque, 3, nv);
mju_addTo(qfrc_target, qforce, nv);
}
}
mj_freeStack(d);
src/engine/engine_support.h
+18 -1
View File
@@ -44,6 +44,18 @@ MJAPI void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigne
MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int spec);
//-------------------------- sparse chains ---------------------------------------------------------
// merge dof chains for two bodies
int mj_mergeChain(const mjModel* m, int* chain, int b1, int b2);
// merge dof chains for two simple bodies
int mj_mergeChainSimple(const mjModel* m, int* chain, int b1, int b2);
// get body chain
int mj_bodyChain(const mjModel* m, int body, int* chain);
//-------------------------- Jacobians -------------------------------------------------------------
// compute 3/6-by-nv Jacobian of global point attached to given body
@@ -77,7 +89,7 @@ MJAPI void mj_jacPointAxis(const mjModel* m, mjData* d,
// compute 3/6-by-nv sparse Jacobian of global point attached to given body
void mj_jacSparse(const mjModel* m, const mjData* d,
mjtNum* jacp, mjtNum* jacr, const mjtNum* point, int body,
int NV, int* chain);
int NV, const int* chain);
// sparse Jacobian difference for simple body contacts
void mj_jacSparseSimple(const mjModel* m, const mjData* d,
@@ -90,6 +102,11 @@ MJAPI int mj_jacDifPair(const mjModel* m, const mjData* d, int* chain,
mjtNum* jac1p, mjtNum* jac2p, mjtNum* jacdifp,
mjtNum* jac1r, mjtNum* jac2r, mjtNum* jacdifr);
// dense or sparse weighted sum of multiple body Jacobians at same point
int mj_jacSum(const mjModel* m, mjData* d, int* chain,
int n, const int* body, const mjtNum* weight,
const mjtNum point[3], mjtNum* jac, int flg_rot);
//-------------------------- name functions --------------------------------------------------------
src/engine/engine_util_container.c
+3 -3
View File
@@ -68,7 +68,8 @@ void mju_arrayListAdd(mjArrayList* array_list, void* element) {
cursor = cursor->next_segment;
}
// copy element into segment
memcpy(cursor->buffer + cursor->element_size * cursor->size, element, cursor->element_size);
memcpy((mjtByte*)cursor->buffer + cursor->element_size * cursor->size,
element, cursor->element_size);
++cursor->size;
}
@@ -92,7 +93,6 @@ void* mju_arrayListAt(const mjArrayList* array_list, size_t index) {
return NULL;
}
return cursor->buffer +
(cursor->element_size * (index - total_capacity));
return (mjtByte*)cursor->buffer + (cursor->element_size * (index - total_capacity));
}
src/engine/engine_util_errmem.h
+1
View File
@@ -72,6 +72,7 @@ MJAPI void mju_writeLog(const char* type, const char* msg);
//------------------------------ internal error macros --------------------------------------------
// internal macro to prepend the calling function name to the error message
#pragma warning(disable : 4996) // needed to use strncpy with Visual Studio
#define mjERROR(...) \
{ \
char _errbuf[1024]; \
src/engine/engine_util_misc.c
+7
View File
@@ -954,6 +954,9 @@ const char* mju_type2Str(int type) {
case mjOBJ_LIGHT:
return "light";
case mjOBJ_FLEX:
return "flex";
case mjOBJ_MESH:
return "mesh";
@@ -1043,6 +1046,10 @@ int mju_str2Type(const char* str) {
return mjOBJ_LIGHT;
}
else if (!strcmp(str, "flex")) {
return mjOBJ_FLEX;
}
else if (!strcmp(str, "mesh")) {
return mjOBJ_MESH;
}
src/engine/engine_util_sparse.c
+170
View File
@@ -368,6 +368,176 @@ void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNu
// dst += src, only at common non-zero indices
void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
int nnzdst, const int* inddst,
int nnzsrc, const int* indsrc) {
if (!nnzdst || !nnzsrc) {
return;
}
int adrs = 0, adrd = 0, inds = indsrc[0], indd = inddst[0];
while (1) {
// common non-zero index
if (inds==indd) {
// add
dst[adrd] += src[adrs];
// advance src
if (++adrs<nnzsrc) {
inds = indsrc[adrs];
} else {
return;
}
// advance dst
if (++adrd<nnzdst) {
indd = inddst[adrd];
} else {
return;
}
}
// src non-zero index smaller: advance src
else if (inds<indd) {
if (++adrs<nnzsrc) {
inds = indsrc[adrs];
} else {
return;
}
}
// dst non-zero index smaller: advance dst
else {
if (++adrd<nnzdst) {
indd = inddst[adrd];
} else {
return;
}
}
}
}
// add to sparse matrix: dst = dst + scl*src, return nnz of result
int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
mjtNum* buf, int* buf_ind) {
// check for identical pattern
if (dst_nnz==src_nnz) {
if (dst_nnz==0) {
return 0;
}
if (mju_compare(dst_ind, src_ind, dst_nnz)) {
// combine mjtNum data directly
mju_addToScl(dst, src, scl, nrow*dst_nnz);
return dst_nnz;
}
}
// prepare to merge scr and dst into buf^T
int si = 0, di = 0, nnz = 0;
int sadr = src_nnz ? src_ind[0] : n+1;
int dadr = dst_nnz ? dst_ind[0] : n+1;
// merge matrices
while (si<src_nnz || di<dst_nnz) {
// both
if (sadr==dadr) {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz] + scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
di++;
sadr = si<src_nnz ? src_ind[si] : n+1;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
}
// dst only
else if (dadr<sadr) {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = dst[di + k*dst_nnz];
}
buf_ind[nnz++] = dadr;
di++;
dadr = di<dst_nnz ? dst_ind[di] : n+1;
}
// src only
else {
for (int k=0; k<nrow; k++) {
buf[nrow*nnz + k] = scl*src[si + k*src_nnz];
}
buf_ind[nnz++] = sadr;
si++;
sadr = si<src_nnz ? src_ind[si] : n+1;
}
}
// copy transposed buf into dst
mju_transpose(dst, buf, nnz, nrow);
mju_copyInt(dst_ind, buf_ind, nnz);
return nnz;
}
// add(merge) two chains
int mju_addChains(int* res, int n, int NV1, int NV2,
const int* chain1, const int* chain2) {
// check for identical pattern
if (NV1==NV2) {
if (NV1==0) {
return 0;
}
if (mju_compare(chain1, chain2, NV1)) {
mju_copyInt(res, chain1, NV1);
return NV1;
}
}
// prepare to merge
int i1 = 0, i2 = 0, NV = 0;
int adr1 = NV1 ? chain1[0] : n+1;
int adr2 = NV2 ? chain2[0] : n+1;
// merge chains
while (i1<NV1 || i2<NV2) {
// both
if (adr1==adr2) {
res[NV++] = adr1;
i1++;
i2++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
adr2 = i2<NV2 ? chain2[i2] : n+1;
}
// chain1 only
else if (adr1<adr2) {
res[NV++] = adr1;
i1++;
adr1 = i1<NV1 ? chain1[i1] : n+1;
}
// chain2 only
else {
res[NV++] = adr2;
i2++;
adr2 = i2<NV2 ? chain2[i2] : n+1;
}
}
return NV;
}
// compress layout of sparse matrix
void mju_compressSparse(mjtNum* mat, int nr, int nc, int* rownnz, int* rowadr, int* colind) {
rowadr[0] = 0;
src/engine/engine_util_sparse.h
+14
View File
@@ -63,6 +63,20 @@ int mju_combineSparse(mjtNum* dst, const mjtNum* src, mjtNum a, mjtNum b,
void mju_combineSparseInc(mjtNum* dst, const mjtNum* src, int n, mjtNum a, mjtNum b,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind);
// dst += src, only at common non-zero indices
void mju_addToSparseInc(mjtNum* dst, const mjtNum* src,
int nnzdst, const int* inddst,
int nnzsrc, const int* indsrc);
// add to sparse matrix: dst = dst + scl*src, return nnz of result
int mju_addToSparseMat(mjtNum* dst, const mjtNum* src, int n, int nrow, mjtNum scl,
int dst_nnz, int src_nnz, int* dst_ind, const int* src_ind,
mjtNum* buf, int* buf_ind);
// add(merge) two chains
int mju_addChains(int* res, int n, int NV1, int NV2,
const int* chain1, const int* chain2);
// transpose sparse matrix
MJAPI void mju_transposeSparse(mjtNum* res, const mjtNum* mat, int nr, int nc,
int* res_rownnz, int* res_rowadr, int* res_colind,
src/engine/engine_vis_init.c
+120 -14
View File
@@ -43,6 +43,7 @@ const char* mjLABELSTRING[mjNLABEL] = {
"Tendon",
"Actuator",
"Constraint",
"Flex",
"Skin",
"Selection",
"SelPoint",
@@ -91,8 +92,13 @@ const char* mjVISSTRING[mjNVISFLAG][3] = {
{"S&elect Point", "0", "E"},
{"Static Bo&dy", "1", "D"},
{"Skin", "1", ";"},
{"Flex Vert", "1", "."},
{"Flex Edge", "1", ""},
{"Flex Face", "1", ""},
{"Flex Skin", "0", ""},
{"Body Tree", "0", "`"},
{"Mesh Tree", "0", ""},
{"Flex Tree", "0", ""},
{"Mesh Tree", "0", "\\"},
{"SDF iters", "0", ""}
};
@@ -107,7 +113,7 @@ const char* mjRNDSTRING[mjNRNDFLAG][3] = {
{"Fog", "0", "G"},
{"Haze", "1", "/"},
{"Segment", "0", ","},
{"Id Color", "0", "."},
{"Id Color", "0", ""},
{"Cull Face", "1", ""}
};
@@ -143,6 +149,96 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
scn->scale = 1;
scn->rotate[0] = 1;
// set number of flexes
scn->nflex = m ? m->nflex : 0;
// allocate flex data
if (scn->nflex) {
int nflex = scn->nflex;
// allocate fixed
scn->flexedgeadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexedgenum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexvertadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexvertnum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfaceadr = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfacenum = (int*) mju_malloc(nflex*sizeof(int));
scn->flexfaceused= (int*) mju_malloc(nflex*sizeof(int));
scn->flexedge = (int*) mju_malloc(2*m->nflexedge*sizeof(int));
scn->flexvert = (float*) mju_malloc(3*m->nflexvert*sizeof(float));
// count max number of flex faces to be rendered (depending on vis options)
int nface = 0;
for (int f=0; f<nflex; f++) {
// 1D : 0
if (m->flex_dim[f]==0) {
scn->flexfacenum[f] = 0;
}
// 2D: 2*fragments + 2*elements
else if (m->flex_dim[f]==2) {
scn->flexfacenum[f] = 2*m->flex_shellnum[f] + 2*m->flex_elemnum[f];
}
// 3D: max(fragments, 4*maxlayer)
else {
// find number of elements in biggest layer
int maxlayer = 0, layer = 0, nlayer = 1;
while (nlayer) {
// count elements in this layer
nlayer = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
if (m->flex_elemlayer[m->flex_elemadr[f]+e]==layer) {
nlayer++;
}
}
// accumulate max over layers, advance layer
maxlayer = mjMAX(maxlayer, nlayer);
layer++;
}
scn->flexfacenum[f] = mjMAX(m->flex_shellnum[f], 4*maxlayer);
}
// accumulate over flexes
nface += scn->flexfacenum[f];
}
// allocate face-related
scn->flexface = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
scn->flexnormal = nface ? (float*) mju_malloc(9*nface*sizeof(float)) : NULL;
scn->flextexcoord = nface ? (float*) mju_malloc(6*nface*sizeof(float)) : NULL;
// check allocation
if (!scn->flexedgeadr ||
!scn->flexedgenum ||
!scn->flexfaceadr ||
!scn->flexfacenum ||
!scn->flexfaceused||
!scn->flexvertadr ||
!scn->flexvertnum ||
!scn->flexedge ||
!scn->flexvert ||
(nface && !scn->flexface) ||
(nface && !scn->flexnormal) ||
(nface && !scn->flextexcoord)) {
mju_error("Could not allocate flex buffers");
}
// copy constant edge and vertex data
memcpy(scn->flexedgeadr, m->flex_edgeadr, nflex*sizeof(int));
memcpy(scn->flexedgenum, m->flex_edgenum, nflex*sizeof(int));
memcpy(scn->flexvertadr, m->flex_vertadr, nflex*sizeof(int));
memcpy(scn->flexvertnum, m->flex_vertnum, nflex*sizeof(int));
memcpy(scn->flexedge, m->flex_edge, 2*m->nflexedge*sizeof(int));
// compute flexfaceadr
for (int f=0; f<nflex; f++) {
scn->flexfaceadr[f] = f==0 ? 0 : scn->flexfaceadr[f-1]+scn->flexfacenum[f-1];
}
}
// set number of skins
if (m) {
scn->nskin = m->nskin;
@@ -152,19 +248,14 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
// allocate skin data
if (scn->nskin) {
// compute number of vertices in all skins
int nskin = m->nskin;
int totvert = 0;
for (int i=0; i < nskin; i++) {
totvert += m->skin_vertnum[i];
}
// allocate
scn->skinfacenum = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvertadr = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvertnum = (int*) mju_malloc(nskin*sizeof(int));
scn->skinvert = (float*) mju_malloc(3*totvert*sizeof(float));
scn->skinnormal = (float*) mju_malloc(3*totvert*sizeof(float));
scn->skinvert = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
scn->skinnormal = (float*) mju_malloc(3*m->nskinvert*sizeof(float));
// check allocation
if (!scn->skinfacenum ||
@@ -176,11 +267,9 @@ void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom) {
}
// copy constant data
for (int i=0; i < nskin; i++) {
scn->skinfacenum[i] = m->skin_facenum[i];
scn->skinvertadr[i] = m->skin_vertadr[i];
scn->skinvertnum[i] = m->skin_vertnum[i];
}
mju_copyInt(scn->skinfacenum, m->skin_facenum, nskin);
mju_copyInt(scn->skinvertadr, m->skin_vertadr, nskin);
mju_copyInt(scn->skinvertnum, m->skin_vertnum, nskin);
}
// mjvGeom, mjvLight, mjvGLCamera objects are invalid
@@ -193,6 +282,20 @@ void mjv_freeScene(mjvScene* scn) {
// free buffers allocated by mjv_makeScene
mju_free(scn->geoms);
mju_free(scn->geomorder);
mju_free(scn->flexedgeadr);
mju_free(scn->flexedgenum);
mju_free(scn->flexvertadr);
mju_free(scn->flexvertnum);
mju_free(scn->flexfaceadr);
mju_free(scn->flexfacenum);
mju_free(scn->flexfaceused);
mju_free(scn->flexedge);
mju_free(scn->flexvert);
mju_free(scn->flexface);
mju_free(scn->flexnormal);
mju_free(scn->flextexcoord);
mju_free(scn->skinfacenum);
mju_free(scn->skinvertadr);
mju_free(scn->skinvertnum);
@@ -224,6 +327,7 @@ void mjv_defaultOption(mjvOption* vopt) {
vopt->jointgroup[i] = state;
vopt->tendongroup[i] = state;
vopt->actuatorgroup[i] = state;
vopt->flexgroup[i] = state;
vopt->skingroup[i] = state;
}
@@ -232,6 +336,7 @@ void mjv_defaultOption(mjvOption* vopt) {
}
vopt->bvh_depth = 1;
vopt->flex_layer = 0;
}
@@ -271,6 +376,7 @@ void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam) {
void mjv_defaultPerturb(mjvPerturb* pert) {
memset(pert, 0, sizeof(mjvPerturb));
pert->flexselect = -1;
pert->skinselect = -1;
pert->refquat[0] = 1;
pert->scale = 1;
src/engine/engine_vis_interact.c
+66 -11
View File
@@ -545,6 +545,12 @@ void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPertur
mju_dot(jacM2+2*nv, jacM2+2*nv, nv);
pert->localmass = 3 / mju_max(invmass, mjMINVAL);
// scale localmass with flex average number of edges per vertex
if (pert->flexselect>=0 && !m->flex_rigid[pert->flexselect]) {
pert->localmass *= (2.0*m->flex_edgenum[pert->flexselect]) /
(mjtNum)m->flex_vertnum[pert->flexselect];
}
// copy
mju_copy3(pert->refpos, d->xipos+3*sel);
mju_mulQuat(pert->refquat, d->xquat+4*sel, m->body_iquat+4*sel);
@@ -731,10 +737,11 @@ mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2)
// Select geom or skin with mouse, return bodyid; -1: none selected.
// Select geom, flex or skin with mouse, return bodyid; -1: none selected.
int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]) {
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]) {
// get average camera
mjvGLCamera cam = mjv_averageCamera(scn->camera, scn->camera+1);
@@ -759,9 +766,35 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum geomdist = mj_ray(m, d, pos, ray, vopt->geomgroup,
vopt->flags[mjVIS_STATIC], -1, geomid);
// find intersection with flexes
int flexbodyid = -1;
mjtNum flexdist = -1;
mjtNum flexpnt[3] = {0, 0, 0};
*flexid = -1;
if (vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) {
for (int i=0; i<m->nflex; i++) {
// process one flex
int vertid;
mjtNum newdist = mju_rayFlex(m, d, vopt->flex_layer,
vopt->flags[mjVIS_FLEXVERT], vopt->flags[mjVIS_FLEXEDGE],
vopt->flags[mjVIS_FLEXFACE], vopt->flags[mjVIS_FLEXSKIN],
i, pos, ray, &vertid);
// update if closer intersection found
if (newdist>=0 && (newdist<flexdist || flexdist<0)) {
flexdist = newdist;
flexbodyid = m->flex_vertbodyid[m->flex_vertadr[i] + vertid];
*flexid = i;
mju_copy3(flexpnt, d->flexvert_xpos + 3*(m->flex_vertadr[i] + vertid));
}
}
}
// find intersection with skins
int bodyid = -1;
int skinbodyid = -1;
mjtNum skindist = -1;
mjtNum skinpnt[3] = {0, 0, 0};
*skinid = -1;
if (vopt->flags[mjVIS_SKIN]) {
for (int i=0; i < m->nskin; i++) {
@@ -774,7 +807,6 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if closer intersection found
if (newdist >= 0 && (newdist < skindist || skindist < 0)) {
// assign result
skindist = newdist;
// find body with largest weight for this vertex
@@ -792,8 +824,9 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
// update if matching id and bigger weight
if (vid == vertid && vweight > bestweight) {
bestweight = vweight;
bodyid = m->skin_bonebodyid[j];
skinbodyid = m->skin_bonebodyid[j];
*skinid = i;
mju_f2n(skinpnt, scn->skinvert + 3*(m->skin_vertadr[i] + vertid), 3);
}
}
}
@@ -802,21 +835,43 @@ int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
}
// no intersection
if (geomdist < 0 && skindist < 0) {
if (geomdist < 0 && flexdist < 0 && skindist < 0) {
return -1;
}
// geom only, or geom closer than skin
else if (geomdist >= 0 && (skindist < 0 || skindist > geomdist)) {
mju_addScl3(selpnt, pos, ray, geomdist);
// find smallest non-negative distance
mjtNum raydist[3] = {geomdist, flexdist, skindist};
int best = -1;
for (int i=0; i<3; i++) {
if (raydist[i]>=0) {
if (best<0 || raydist[best]>raydist[i]) {
best = i;
}
}
}
// geom
if (best == 0) {
*flexid = -1;
*skinid = -1;
mju_addScl3(selpnt, pos, ray, raydist[best]);
return m->geom_bodyid[*geomid];
}
// flex
else if (best == 1) {
*geomid = -1;
*skinid = -1;
mju_copy3(selpnt, flexpnt);
return flexbodyid;
}
// skin
else {
mju_addScl3(selpnt, pos, ray, skindist);
*geomid = -1;
return bodyid;
*flexid = -1;
mju_copy3(selpnt, skinpnt);
return skinbodyid;
}
}
src/engine/engine_vis_interact.h
+3 -2
View File
@@ -72,10 +72,11 @@ MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb*
// return the average of two OpenGL cameras
MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2);
// Select geom or skin with mouse, return bodyid; -1: none selected.
// Select geom, flex or skin with mouse, return bodyid; -1: none selected.
MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt,
mjtNum aspectratio, mjtNum relx, mjtNum rely,
const mjvScene* scn, mjtNum selpnt[3], int geomid[1], int skinid[1]);
const mjvScene* scn, mjtNum selpnt[3],
int geomid[1], int flexid[1], int skinid[1]);
#ifdef __cplusplus
}
src/engine/engine_vis_state.c
+6
View File
@@ -247,6 +247,12 @@ int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOption* opt
// update camera
mjv_updateCamera(&m, &d, cam, scn);
// update flexes
if (opt->flags[mjVIS_FLEXVERT] || opt->flags[mjVIS_FLEXEDGE] ||
opt->flags[mjVIS_FLEXFACE] || opt->flags[mjVIS_FLEXSKIN]) {
mjv_updateActiveFlex(&m, &d, scn, opt);
}
// update skins
if (opt->flags[mjVIS_SKIN]) {
mjv_updateActiveSkin(&m, &d, scn, opt);
src/engine/engine_vis_visualize.c
+494 -57
View File
@@ -22,6 +22,7 @@
#include <mujoco/mjmodel.h>
#include <mujoco/mjvisualize.h>
#include "engine/engine_array_safety.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_support.h"
#include "engine/engine_util_blas.h"
@@ -96,7 +97,6 @@ static void islandColor(float rgba[4], int islanddofadr) {
// add contact-related geoms in mjvObject
static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
const mjvOption* vopt, mjvScene* scn) {
int body1, body2;
int objtype = mjOBJ_UNKNOWN, category = mjCAT_DECOR;
mjtNum mat[9], tmp[9], vec[3], frc[3], confrc[6], axis[3];
mjtNum framewidth, framelength, scl = m->stat.meansize;
@@ -147,13 +147,42 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// label contacting geom names or ids
if (vopt->label == mjLABEL_CONTACTPOINT) {
const char* name1 = mj_id2name(m, mjOBJ_GEOM, con->geom1);
char id1[10];
mjSNPRINTF(id1, "%d", con->geom1);
const char* name2 = mj_id2name(m, mjOBJ_GEOM, con->geom2);
char id2[10];
mjSNPRINTF(id2, "%d", con->geom2);
mjSNPRINTF(thisgeom->label, "%s | %s", name1 ? name1 : id1 , name2 ? name2 : id2);
char contactlabel[2][10];
for (int k=0; k<2; k++) {
// make geom label
if (con->geom[k]>=0) {
const char* geomname = mj_id2name(m, mjOBJ_GEOM, con->geom[k]);
if (geomname) {
mjSNPRINTF(contactlabel[k], "%s", geomname);
}
else {
mjSNPRINTF(contactlabel[k], "g%d", con->geom[k]);
}
}
// make flex elem or vert label
else {
const char* flexname = mj_id2name(m, mjOBJ_FLEX, con->flex[k]);
if (flexname) {
if (con->elem[k]>=0) {
mjSNPRINTF(contactlabel[k], "%s.e%d", flexname, con->elem[k]);
}
else {
mjSNPRINTF(contactlabel[k], "%s.v%d", flexname, con->vert[k]);
}
}
else {
if (con->elem[k]>=0) {
mjSNPRINTF(contactlabel[k], "f%d.e%d", con->flex[k], con->elem[k]);
}
else {
mjSNPRINTF(contactlabel[k], "f%d.v%d", con->flex[k], con->vert[k]);
}
}
}
}
mjSNPRINTF(thisgeom->label, "%s | %s", contactlabel[0], contactlabel[1]);
}
FINISH
@@ -234,12 +263,15 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
// scale vector
mju_scl3(vec, vec, m->vis.map.force/m->stat.meanmass);
// get body ids
body1 = m->geom_bodyid[con->geom1];
body2 = m->geom_bodyid[con->geom2];
// get bodyflex ids
int bf[2];
for (int k=0; k<2; k++) {
bf[k] = (con->geom[k]>=0) ? m->geom_bodyid[con->geom[k]] :
m->nbody + con->flex[k];
}
// make sure arrow points towards body with higher id
if (body1 > body2) {
// make sure arrow points towards bodyflex with higher id
if (bf[0] > bf[1]) {
mju_scl3(vec, vec, -1);
}
@@ -249,8 +281,8 @@ static void addContactGeom(const mjModel* m, mjData* d, const mjtByte* flags,
mjtNum to[3];
mju_add3(to, from, vec);
mjv_connector(thisgeom,
body1 > 0 && body2 > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW,
m->vis.scale.forcewidth * scl,from, to);
bf[0] > 0 && bf[1] > 0 && !split ? mjGEOM_ARROW2 : mjGEOM_ARROW,
m->vis.scale.forcewidth * scl,from, to);
f2f(thisgeom->rgba, j == 2 ? m->vis.rgba.contactfriction : m->vis.rgba.contactforce, 4);
if (vopt->label == mjLABEL_CONTACTFORCE && j == (split ? 1 : 0)) {
mjSNPRINTF(thisgeom->label, "%-.3g", mju_norm3(frc));
@@ -477,12 +509,15 @@ static int bodycategory(const mjModel* m, int bodyid) {
// draw bounding box
static void drawBoundingBox(mjvGeom* thisgeom, mjData* d, mjvScene* scn,
static void drawBoundingBox(mjData* d, mjvScene* scn,
const mjtNum aabb[6], const mjtNum xpos[3],
const mjtNum xmat[9], const float rgba[4],
int i, int objtype, int category) {
const mjtNum xmat[9], const float rgba[4]) {
mjtNum x[3];
mjtNum dist[3][3];
mjvGeom* thisgeom;
int category = mjCAT_DECOR;
int objtype = mjOBJ_UNKNOWN;
int i = -1;
if (xmat != NULL) {
mju_rotVecMat(x, aabb, xmat);
@@ -558,49 +593,98 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
catmask &= (~mjCAT_STATIC);
}
// flex
objtype = mjOBJ_FLEX;
category = mjCAT_DYNAMIC;
if ((vopt->flags[mjVIS_FLEXVERT] || vopt->flags[mjVIS_FLEXEDGE] ||
vopt->flags[mjVIS_FLEXFACE] || vopt->flags[mjVIS_FLEXSKIN]) &&
(category & catmask)) {
for (int i=0; i<m->nflex; i++) {
if (vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[i]))]) {
START
// construct geom, pos = first vertex
mjv_initGeom(thisgeom, mjGEOM_FLEX, NULL,
d->flexvert_xpos + 3*m->flex_vertadr[i], NULL, NULL);
// size[0] = radius
thisgeom->size[0] = m->flex_radius[i];
// set material properties
setMaterial(m, thisgeom, m->flex_matid[i], m->flex_rgba+4*i, vopt->flags);
// set texcoord
if (m->flex_texcoordadr[i]>=0) {
thisgeom->texcoord = 1;
}
else {
thisgeom->texid = -1;
}
// glow flex if selected
if (pert->flexselect==i) {
markselected(&m->vis, thisgeom);
}
// skip if alpha is 0
if (thisgeom->rgba[3]==0) {
continue;
}
// vopt->label
if (vopt->label==mjLABEL_FLEX) {
makeLabel(m, mjOBJ_FLEX, i, thisgeom->label);
}
FINISH
}
}
}
// skin
objtype = mjOBJ_SKIN;
category = mjCAT_DYNAMIC;
if (vopt->flags[mjVIS_SKIN] && (category & catmask)) {
for (int i=0; i < m->nskin; i++) {
START
if (vopt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
START
// construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL,
d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL);
// construct geom, pos = first bone
mjv_initGeom(thisgeom, mjGEOM_SKIN, NULL,
d->xpos + 3*m->skin_bonebodyid[m->skin_boneadr[i]], NULL, NULL);
// set material properties
setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags);
// set material properties
setMaterial(m, thisgeom, m->skin_matid[i], m->skin_rgba+4*i, vopt->flags);
// glow skin if selected
if (pert->skinselect == i) {
markselected(&m->vis, thisgeom);
// glow skin if selected
if (pert->skinselect == i) {
markselected(&m->vis, thisgeom);
}
// set texcoord
if (m->skin_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
}
// skip if alpha is 0
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label == mjLABEL_SKIN) {
makeLabel(m, mjOBJ_SKIN, i, thisgeom->label);
}
FINISH
}
// set texcoord
if (m->skin_texcoordadr[i] >= 0) {
thisgeom->texcoord = 1;
}
// skip if alpha is 0
if (thisgeom->rgba[3] == 0) {
continue;
}
// vopt->label
if (vopt->label == mjLABEL_SKIN) {
makeLabel(m, mjOBJ_SKIN, i, thisgeom->label);
}
FINISH
}
}
// bounding volume hierarchy
if (vopt->flags[mjVIS_MIDPHASE]) {
int bodyid = 0;
// body BVH
if (vopt->flags[mjVIS_BODYBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int i = 0; i < m->nbvh; i++) {
for (int i = 0; i < m->nbvhstatic; i++) {
int isleaf = m->bvh_child[2*i] == -1 && m->bvh_child[2*i+1] == -1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != vopt->bvh_depth) {
@@ -610,7 +694,8 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
}
// find geom number
int geomid = m->bvh_geomid[i];
int bodyid = 0;
int geomid = m->bvh_nodeid[i];
while (i >= m->body_bvhadr[bodyid] + m->body_bvhnum[bodyid]) {
if (++bodyid >= m->nbody) {
break;
@@ -631,11 +716,41 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
}
}
// mesh bounding volume hierarchy
// flex BVH
if (vopt->flags[mjVIS_FLEXBVH]) {
float rgba[] = {1, 0, 0, 0.1};
for (int f=0; f<m->nflex; f++) {
if (m->flex_bvhnum[f] &&
vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) {
for (int i=m->flex_bvhadr[f]; i<m->flex_bvhadr[f]+m->flex_bvhnum[f]; i++) {
int isleaf = m->bvh_child[2*i]==-1 && m->bvh_child[2*i+1]==-1;
if (scn->ngeom >= scn->maxgeom) break;
if (m->bvh_depth[i] != vopt->bvh_depth) {
if (!isleaf || m->bvh_depth[i] > vopt->bvh_depth) {
continue;
}
}
// set box data
mjtNum *aabb = d->bvh_aabb_dyn + 6*(i - m->nbvhstatic);
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
// b/304453879 : add LINEBOX geom for bounding box visualization
START
mjv_initGeom(thisgeom, mjGEOM_BOX, aabb+3, aabb, NULL, rgba);
FINISH
}
}
}
}
// mesh BVH
if (vopt->flags[mjVIS_MESHBVH]) {
float rgba[] = {1, 0, 0, 1};
for (int geomid = 0; geomid < m->ngeom; geomid++) {
@@ -666,7 +781,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt,
rgba[0] = d->bvh_active[i] ? 1 : 0;
rgba[1] = d->bvh_active[i] ? 0 : 1;
drawBoundingBox(thisgeom, d, scn, aabb, xpos, xmat, rgba, i, objtype, category);
drawBoundingBox(d, scn, aabb, xpos, xmat, rgba);
}
}
}
@@ -2123,6 +2238,320 @@ void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn
}
// construct face, flat normals
static void makeFace(float* _face, float* _normal, mjtNum radius, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
// compute normal
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set vertices: offset by radius*normal
mjtNum temp[3];
mju_addScl3(temp, v0, nrm, radius);
mju_n2f(face, temp, 3);
mju_addScl3(temp, v1, nrm, radius);
mju_n2f(face+3, temp, 3);
mju_addScl3(temp, v2, nrm, radius);
mju_n2f(face+6, temp, 3);
// set normals
mju_n2f(normal, nrm, 3);
mju_n2f(normal+3, nrm, 3);
mju_n2f(normal+6, nrm, 3);
}
// add face normal to vertices
static void addNormal(mjtNum* vertnorm, const mjtNum* vertxpos,
int i0, int i1, int i2) {
// compute normal*area
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// accumulate at each vertex
mju_addTo3(vertnorm + 3*i0, nrm);
mju_addTo3(vertnorm + 3*i1, nrm);
mju_addTo3(vertnorm + 3*i2, nrm);
}
// construct face, smooth normals
static void makeSmooth(float* _face, float* _normal, mjtNum radius, mjtByte flg_flat,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1, int i2) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
int ind[3] = {i0, i1, i2};
int sign = radius>0 ? 1 : -1;
// flat shading
if (flg_flat) {
// compute face normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
const mjtNum* v2 = vertxpos + 3*i2;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
mju_normalize3(nrm);
// set all vertex normals equal to face normal
for (int k=0; k<3; k++){
normal[3*k+0] = (float) (sign*nrm[0]);
normal[3*k+1] = (float) (sign*nrm[1]);
normal[3*k+2] = (float) (sign*nrm[2]);
}
}
// smooth shading
else {
for (int k=0; k<3; k++){
normal[3*k+0] = (float) (sign*vertnorm[3*ind[k]+0]);
normal[3*k+1] = (float) (sign*vertnorm[3*ind[k]+1]);
normal[3*k+2] = (float) (sign*vertnorm[3*ind[k]+2]);
}
}
// set positions: vertices offset by radius*normal
for (int k=0; k<3; k++){
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + radius*vertnorm[3*ind[k]+2]);
}
}
// construct side in 2D face
static void makeSide(float* _face, float* _normal, mjtNum radius,
const mjtNum* vertnorm, const mjtNum* vertxpos,
int nface, int i0, int i1) {
float* face = _face + 9*nface;
float* normal = _normal + 9*nface;
// compute normal
const mjtNum* v0 = vertxpos + 3*i0;
const mjtNum* v1 = vertxpos + 3*i1;
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum nrm[3];
mju_cross(nrm, v01, vertnorm+3*i1);
if (radius<0) {
mju_scl3(nrm, nrm, -1);
}
mju_normalize3(nrm);
// set normals
for (int k=0; k<3; k++){
normal[3*k+0] = (float) nrm[0];
normal[3*k+1] = (float) nrm[1];
normal[3*k+2] = (float) nrm[2];
}
// set positions
int ind[3] = {i0, i1, i1};
for (int k=0; k<3; k++){
mjtNum sign = (k==1 ? -1 : +1);
face[3*k+0] = (float) (vertxpos[3*ind[k]+0] + sign*radius*vertnorm[3*ind[k]+0]);
face[3*k+1] = (float) (vertxpos[3*ind[k]+1] + sign*radius*vertnorm[3*ind[k]+1]);
face[3*k+2] = (float) (vertxpos[3*ind[k]+2] + sign*radius*vertnorm[3*ind[k]+2]);
}
}
// copy texcoord for face
static void copyTex(float* dst, const float* src, int nface, int i0, int i1, int i2) {
if (!dst || !src) {
return;
}
dst[6*nface+0] = src[2*i0];
dst[6*nface+1] = src[2*i0+1];
dst[6*nface+2] = src[2*i1];
dst[6*nface+3] = src[2*i1+1];
dst[6*nface+4] = src[2*i2];
dst[6*nface+5] = src[2*i2+1];
}
// update visible flexes only
void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// save flex visualization flags in scene (needed by renderer)
scn->flexvertopt = opt->flags[mjVIS_FLEXVERT];
scn->flexedgeopt = opt->flags[mjVIS_FLEXEDGE];
scn->flexfaceopt = opt->flags[mjVIS_FLEXFACE];
scn->flexskinopt = opt->flags[mjVIS_FLEXSKIN];
// convert vertex positions from mjtNum to float
for (int v=0; v<3*m->nflexvert; v++) {
scn->flexvert[v] = (float) d->flexvert_xpos[v];
}
// construct faces
for (int f=0; f<m->nflex; f++) {
int dim = m->flex_dim[f];
mjtNum radius = m->flex_radius[f];
mjtByte flg_flat = m->flex_flatskin[f];
const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f];
float* face = scn->flexface + 9*scn->flexfaceadr[f];
float* normal = scn->flexnormal + 9*scn->flexfaceadr[f];
float* texdst = m->flex_texcoordadr[f]>=0 ?
scn->flextexcoord + 6*scn->flexfaceadr[f] : NULL;
const float* texsrc = m->flex_texcoordadr[f]>=0 ?
m->flex_texcoord + 2*m->flex_texcoordadr[f] : NULL;
// 1D, or face and skin disabled: no faces
if (dim==1 || (!opt->flags[mjVIS_FLEXFACE] && !opt->flags[mjVIS_FLEXSKIN])) {
scn->flexfaceused[f] = 0;
}
// 2D or 3D face: faces from elements, flat normals, texture
else if (!opt->flags[mjVIS_FLEXSKIN]) {
int nface = 0;
for (int e=0; e<m->flex_elemnum[f]; e++) {
// in 3D, show only elements in selected layer
if (dim==2 || m->flex_elemlayer[m->flex_elemadr[f]+e]==opt->flex_layer) {
// get element data
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
// triangles: two faces per element
if (dim==2) {
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeFace(face, normal, radius, vertxpos, nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[1]);
nface++;
}
// tetrahedra: four faces per element
else {
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[2], edata[3]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[3]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[0], edata[3], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[3], edata[1]);
nface++;
makeFace(face, normal, radius, vertxpos,
nface, edata[1], edata[3], edata[2]);
copyTex(texdst, texsrc, nface, edata[1], edata[3], edata[2]);
nface++;
}
}
}
// save face count
scn->flexfaceused[f] = nface;
}
// 2D or 3D skin: faces from elements (2D) or shells (3D), smooth normals, texture
else {
// allocate and clear vertex normals for smoothing
mj_markStack(d);
mjtNum* vertnorm = mj_stackAllocNum(d, 3*m->flex_vertnum[f]);
mju_zero(vertnorm, 3*m->flex_vertnum[f]);
// add vertex normals: top element sides in 2D, shell fragments in 3D
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
addNormal(vertnorm, vertxpos, edata[0], edata[1], edata[2]);
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
addNormal(vertnorm, vertxpos, sdata[0], sdata[1], sdata[2]);
}
}
// normalize vertex normals
for (int i=0; i<m->flex_vertnum[f]; i++) {
mju_normalize3(vertnorm+3*i);
}
// create faces, offset along smoothed vertex normals, and texcoord
int nface = 0;
if (dim==2) {
for (int e=0; e<m->flex_elemnum[f]; e++) {
const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1);
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[1], edata[2]);
copyTex(texdst, texsrc, nface, edata[0], edata[1], edata[2]);
nface++;
makeSmooth(face, normal, -radius, flg_flat, vertnorm, vertxpos,
nface, edata[0], edata[2], edata[1]);
copyTex(texdst, texsrc, nface, edata[0], edata[2], edata[1]);
nface++;
}
} else {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSmooth(face, normal, radius, flg_flat, vertnorm, vertxpos,
nface, sdata[0], sdata[1], sdata[2]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[2]);
nface++;
}
}
// 2D: close sides using shell fragments
if (dim==2) {
for (int s=0; s<m->flex_shellnum[f]; s++) {
const int* sdata = m->flex_shell + m->flex_shelldataadr[f] + s*dim;
makeSide(face, normal, radius, vertnorm, vertxpos,
nface, sdata[0], sdata[1]);
copyTex(texdst, texsrc, nface, sdata[0], sdata[1], sdata[1]);
nface++;
makeSide(face, normal, -radius, vertnorm, vertxpos,
nface, sdata[1], sdata[0]);
copyTex(texdst, texsrc, nface, sdata[1], sdata[0], sdata[0]);
nface++;
}
}
// save face count
scn->flexfaceused[f] = nface;
mj_freeStack(d);
}
// check face count, SHOULD NOT OCCUR
if (scn->flexfaceused[f] > scn->flexfacenum[f]) {
mju_error("too many flex faces in mjv_updateActiveFlex");
}
}
}
// update all skins, here for backward API compatibility
void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
mjvOption opt;
@@ -2132,6 +2561,7 @@ void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn) {
}
// update visible skins only
void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt) {
// process skins
@@ -2146,7 +2576,8 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
memset(scn->skinvert + 3*vertadr, 0, 3*vertnum*sizeof(float));
memset(scn->skinnormal + 3*vertadr, 0, 3*vertnum*sizeof(float));
if (opt->skingroup[m->skin_group[i]]) {
// update only if visible
if (opt->skingroup[mjMAX(0, mjMIN(mjNGROUP-1, m->skin_group[i]))]) {
// accumulate positions from all bones
for (int j=m->skin_boneadr[i];
j < m->skin_boneadr[i]+m->skin_bonenum[i];
@@ -2234,10 +2665,10 @@ void mjv_updateActiveSkin(const mjModel* m, mjData* d, mjvScene* scn, const mjvO
// normalize normals
for (int k=vertadr; k < vertadr+vertnum; k++) {
float s = sqrtf(
scn->skinnormal[3*k]*scn->skinnormal[3*k] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
scn->skinnormal[3*k]*scn->skinnormal[3*k] +
scn->skinnormal[3*k+1]*scn->skinnormal[3*k+1] +
scn->skinnormal[3*k+2]*scn->skinnormal[3*k+2]
);
float scl = 1/mjMAX(mjMINVAL, s);
scn->skinnormal[3*k] *= scl;
@@ -2291,6 +2722,12 @@ void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt,
// update camera
mjv_updateCamera(m, d, cam, scn);
// update flexes
if (opt->flags[mjVIS_FLEXVERT] || opt->flags[mjVIS_FLEXEDGE] ||
opt->flags[mjVIS_FLEXFACE] || opt->flags[mjVIS_FLEXSKIN]) {
mjv_updateActiveFlex(m, d, scn, opt);
}
// update skins
if (opt->flags[mjVIS_SKIN]) {
mjv_updateActiveSkin(m, d, scn, opt);
src/engine/engine_vis_visualize.h
+3
View File
@@ -53,6 +53,9 @@ MJAPI void mjv_makeLights(const mjModel* m, mjData* d, mjvScene* scn);
// update camera only
MJAPI void mjv_updateCamera(const mjModel* m, mjData* d, mjvCamera* cam, mjvScene* scn);
// update visible flexes only
MJAPI void mjv_updateActiveFlex(const mjModel* m, mjData* d, mjvScene* scn, const mjvOption* opt);
// update skins only
MJAPI void mjv_updateSkin(const mjModel* m, mjData* d, mjvScene* scn);
src/render/render_gl3.c
+81 -2
View File
@@ -284,9 +284,9 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
}
}
// apply coordinate transformation, except for skin which is global
// apply coordinate transformation, except for flex and skin which are global
glPushMatrix();
if (geom->type != mjGEOM_SKIN) {
if (geom->type != mjGEOM_FLEX && geom->type != mjGEOM_SKIN) {
glTranslatef(geom->pos[0], geom->pos[1], geom->pos[2]);
glMultMatrixf(mat);
}
@@ -412,6 +412,85 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos,
glEnd();
break;
case mjGEOM_FLEX: // flex
// no texture for vertices and edges
if (glIsEnabled(GL_TEXTURE_2D)) {
glDisable(GL_TEXTURE_2D);
}
// vertex spheres
if (size[0]>0 && scn->flexvertopt &&
!(scn->flexskinopt && scn->flexfaceused[geom->objid])) {
for (int v=scn->flexvertadr[geom->objid];
v<scn->flexvertadr[geom->objid]+scn->flexvertnum[geom->objid]; v++) {
glPushMatrix();
glTranslatef(scn->flexvert[3*v], scn->flexvert[3*v+1], scn->flexvert[3*v+2]);
glScalef(size[0], size[0], size[0]);
glCallList(con->baseBuiltin + mjrSPHERE);
glPopMatrix();
}
}
// edge cylinders
if (size[0]>0 && scn->flexedgeopt &&
!(scn->flexskinopt && scn->flexfaceused[geom->objid])) {
for (int e=scn->flexedgeadr[geom->objid];
e<scn->flexedgeadr[geom->objid]+scn->flexedgenum[geom->objid]; e++) {
// get vertices for this edge
float* v1 = scn->flexvert + 3*(scn->flexvertadr[geom->objid]+scn->flexedge[2*e]);
float* v2 = scn->flexvert + 3*(scn->flexvertadr[geom->objid]+scn->flexedge[2*e+1]);
// compute legth and rotation matrix
mjtNum vec[3] = {v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2]};
mjtNum len = mju_normalize3(vec);
mjtNum edgequat[4], edgemat[9];
mju_quatZ2Vec(edgequat, vec);
mju_negQuat(edgequat, edgequat);
mju_quat2Mat(edgemat, edgequat);
mat[0] = (float)edgemat[0];
mat[1] = (float)edgemat[1];
mat[2] = (float)edgemat[2];
mat[4] = (float)edgemat[3];
mat[5] = (float)edgemat[4];
mat[6] = (float)edgemat[5];
mat[8] = (float)edgemat[6];
mat[9] = (float)edgemat[7];
mat[10] = (float)edgemat[8];
// transform and render
glPushMatrix();
glTranslatef((v1[0]+v2[0])*0.5f, (v1[1]+v2[1])*0.5f, (v1[2]+v2[2])*0.5f);
glMultMatrixf(mat);
glScalef(size[0], size[0], (float)(len*0.5));
glCallList(con->baseBuiltin + mjrCYLINDEROPEN);
glPopMatrix();
}
}
// restore texture for faces
if (glIsEnabled(GL_TEXTURE_2D)) {
glEnable(GL_TEXTURE_2D);
}
// face triangles
if (scn->flexfaceused[geom->objid]) {
glEnableClientState(GL_VERTEX_ARRAY);
glEnableClientState(GL_NORMAL_ARRAY);
glVertexPointer(3, GL_FLOAT, 0, scn->flexface + 9*scn->flexfaceadr[geom->objid]);
glNormalPointer(GL_FLOAT, 0, scn->flexnormal + 9*scn->flexfaceadr[geom->objid]);
if (geom->texcoord && geom->texid>=0) {
glEnableClientState(GL_TEXTURE_COORD_ARRAY);
glTexCoordPointer(2, GL_FLOAT, 0, scn->flextexcoord + 6*scn->flexfaceadr[geom->objid]);
}
glDrawArrays(GL_TRIANGLES, 0, 3*scn->flexfaceused[geom->objid]);
glDisableClientState(GL_VERTEX_ARRAY);
glDisableClientState(GL_NORMAL_ARRAY);
if (geom->texcoord && geom->texid>=0) {
glDisableClientState(GL_TEXTURE_COORD_ARRAY);
}
}
break;
case mjGEOM_SKIN: // skin
// vertex positions
glEnableClientState(GL_VERTEX_ARRAY);
src/user/CMakeLists.txt
+2
View File
@@ -15,6 +15,8 @@
set(MUJOCO_USER_SRCS
user_composite.cc
user_composite.h
user_flexcomp.cc
user_flexcomp.h
user_mesh.cc
user_model.cc
user_model.h
src/user/user_flexcomp.cc
+1187
View File
File diff suppressed because it is too large Load Diff
src/user/user_flexcomp.h
+90
View File
@@ -0,0 +1,90 @@
// Copyright 2021 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.
#ifndef MUJOCO_SRC_USER_USER_FLEXCOMP_H_
#define MUJOCO_SRC_USER_USER_FLEXCOMP_H_
#include <string>
#include <vector>
#include <mujoco/mujoco.h>
#include "user/user_model.h"
#include "user/user_objects.h"
typedef enum _mjtFcompType {
mjFCOMPTYPE_GRID = 0,
mjFCOMPTYPE_BOX,
mjFCOMPTYPE_CYLINDER,
mjFCOMPTYPE_ELLIPSOID,
mjFCOMPTYPE_MESH,
mjFCOMPTYPE_GMSH,
mjFCOMPTYPE_DIRECT,
mjNFCOMPTYPES
} mjtFcompType;
class mjCFlexcomp {
public:
mjCFlexcomp(void);
bool Make(mjCModel* model, mjCBody* body, char* error, int error_sz);
bool MakeGrid(char* error, int error_sz);
bool MakeBox(char* error, int error_sz);
bool MakeMesh(mjCModel* model, char* error, int error_sz);
bool MakeGMSH(mjCModel* model, char* error, int error_sz);
void LoadGMSH(mjCModel* model, mjResource* resource);
int GridID(int ix, int iy);
int GridID(int ix, int iy, int iz);
int BoxID(int ix, int iy, int iz);
void BoxProject(double* pos, int ix, int iy, int iz);
// common properties set by user
std::string name; // flex name
mjtFcompType type; // flexcomp type
int count[3]; // grid count in each dimension
double spacing[3]; // spacing between grid elements
double scale[3]; // scaling for mesh and direct
double mass; // total mass of auto-generated bodies
double inertiabox; // size of inertia box for each body
bool equality; // create edge equality constraint
std::string file; // mesh/gmsh file name
// pin specifications
std::vector<int> pinid; // ids of points to pin
std::vector<int> pinrange; // range of ids to pin
std::vector<int> pingrid; // grid coordinates to pin
std::vector<int> pingridrange; // range of grid coordinates to pin
// all other properties
mjCDef def; // local copy, parsed parameters stored here
// pose transform relative to parent body
double pos[3]; // position
double quat[4]; // orientation
mjCAlternative alt; // alternative orientation
// set by user or computed internally
bool rigid; // all vertices are in parent body (all pinned)
bool centered; // all vertex coordinates are (0,0,0) (nothing pinned)
std::vector<mjtNum> point; // flex bodies/vertices
std::vector<bool> pinned; // is point pinned (true: no new body)
std::vector<bool> used; // is point used by any element (false: skip)
std::vector<int> element; // flex elements
std::vector<float> texcoord; // vertex texture coordinates
};
#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_
src/user/user_mesh.cc
+510 -22
View File
@@ -48,6 +48,7 @@
#include <mujoco/mjtnum.h>
#include <mujoco/mjplugin.h>
#include "engine/engine_crossplatform.h"
#include "engine/engine_io.h"
#include "engine/engine_plugin.h"
#include "engine/engine_resource.h"
#include "engine/engine_util_blas.h"
@@ -139,8 +140,8 @@ mjCMesh::mjCMesh(mjCModel* _model, mjCDef* _def) {
mjuu_setvec(boxsz_surface_, 0, 0, 0);
mjuu_setvec(boxsz_volume_, 0, 0, 0);
mjuu_setvec(aabb_, 1e10, 1e10, 1e10);
mjuu_setvec(aabb_+3, -1e10, -1e10, -1e10);
mjuu_setvec(aamm_, 1e10, 1e10, 1e10);
mjuu_setvec(aamm_+3, -1e10, -1e10, -1e10);
nvert_ = 0;
nnormal_ = 0;
ntexcoord_ = 0;
@@ -589,22 +590,22 @@ mjCBoundingVolume mjCMesh::GetBoundingVolume(int faceid) {
node.contype = 1;
node.pos = center_ + 3*faceid;
node.quat = NULL;
mjtNum AABB[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10};
for (int j=0; j<3; j++) {
int vertid = face_[3*faceid+j];
AABB[0] = mjMIN(AABB[0], vert_[3*vertid+0]);
AABB[1] = mjMIN(AABB[1], vert_[3*vertid+1]);
AABB[2] = mjMIN(AABB[2], vert_[3*vertid+2]);
AABB[3] = mjMAX(AABB[3], vert_[3*vertid+0]);
AABB[4] = mjMAX(AABB[4], vert_[3*vertid+1]);
AABB[5] = mjMAX(AABB[5], vert_[3*vertid+2]);
face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]);
face_aamm[1] = mjMIN(face_aamm[1], vert_[3*vertid+1]);
face_aamm[2] = mjMIN(face_aamm[2], vert_[3*vertid+2]);
face_aamm[3] = mjMAX(face_aamm[3], vert_[3*vertid+0]);
face_aamm[4] = mjMAX(face_aamm[4], vert_[3*vertid+1]);
face_aamm[5] = mjMAX(face_aamm[5], vert_[3*vertid+2]);
}
face_aabb_[6*faceid+0] = .5 * (AABB[0] + AABB[3]);
face_aabb_[6*faceid+1] = .5 * (AABB[1] + AABB[4]);
face_aabb_[6*faceid+2] = .5 * (AABB[2] + AABB[5]);
face_aabb_[6*faceid+3] = .5 * (AABB[3] - AABB[0]);
face_aabb_[6*faceid+4] = .5 * (AABB[4] - AABB[1]);
face_aabb_[6*faceid+5] = .5 * (AABB[5] - AABB[2]);
face_aabb_[6*faceid+0] = .5 * (face_aamm[0] + face_aamm[3]);
face_aabb_[6*faceid+1] = .5 * (face_aamm[1] + face_aamm[4]);
face_aabb_[6*faceid+2] = .5 * (face_aamm[2] + face_aamm[5]);
face_aabb_[6*faceid+3] = .5 * (face_aamm[3] - face_aamm[0]);
face_aabb_[6*faceid+4] = .5 * (face_aamm[4] - face_aamm[1]);
face_aabb_[6*faceid+5] = .5 * (face_aamm[5] - face_aamm[2]);
node.aabb = face_aabb_.data() + 6*faceid;
return node;
}
@@ -729,10 +730,10 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
}
}
// use aabb
// use aamm
else {
// find aabb box center
double cen[3] = {(aabb_[0]+aabb_[3])/2, (aabb_[1]+aabb_[4])/2, (aabb_[2]+aabb_[5])/2};
double cen[3] = {(aamm_[0]+aamm_[3])/2, (aamm_[1]+aamm_[4])/2, (aamm_[2]+aamm_[5])/2};
// add box center into meshpos
meshpos[0] += cen[0];
@@ -786,9 +787,9 @@ void mjCMesh::FitGeom(mjCGeom* geom, double* meshpos) {
case mjGEOM_ELLIPSOID:
case mjGEOM_BOX:
geom->size[0] = aabb_[3] - cen[0];
geom->size[1] = aabb_[4] - cen[1];
geom->size[2] = aabb_[5] - cen[2];
geom->size[0] = aamm_[3] - cen[0];
geom->size[1] = aamm_[4] - cen[1];
geom->size[2] = aamm_[5] - cen[2];
break;
default:
@@ -1399,8 +1400,8 @@ void mjCMesh::Process() {
vert_[3*i+j] = (float) res[j];
// axis-aligned bounding box
aabb_[j+0] = mju_min(aabb_[j+0], res[j]);
aabb_[j+3] = mju_max(aabb_[j+3], res[j]);
aamm_[j+0] = mju_min(aamm_[j+0], res[j]);
aamm_[j+3] = mju_max(aamm_[j+3], res[j]);
}
}
for (int i=0; i<nnormal_; i++) {
@@ -2151,3 +2152,490 @@ void mjCSkin::LoadSKN(mjResource* resource) {
throw mjCError(this, "unexpected buffer size in SKN file '%s'", resource->name);
}
}
//------------------ class mjCFlex implementation --------------------------------------------------
// constructor
mjCFlex::mjCFlex(mjCModel* _model) {
// set model
model = _model;
// set contact defaults
contype = 1;
conaffinity = 1;
condim = 3;
priority = 0;
mjuu_setvec(friction, 1, 0.005, 0.0001);
solmix = 1.0;
mj_defaultSolRefImp(solref, solimp);
margin = 0;
gap = 0;
// set other defaults
dim = 2;
radius = 0.005;
internal = true;
flatskin = false;
selfcollide = mjFLEXSELF_AUTO;
activelayers = 1;
group = 0;
edgestiffness = 0;
edgedamping = 0;
material.clear();
rgba[0] = rgba[1] = rgba[2] = 0.5f;
rgba[3] = 1.0f;
// clear internal variables
nvert = 0;
nedge = 0;
nelem = 0;
matid = -1;
rigid = false;
centered = false;
}
// compiler
void mjCFlex::Compile(const mjVFS* vfs) {
// set nelem; check sizes
if (dim<1 || dim>3) {
throw mjCError(this, "dim must be 1, 2 or 3");
}
if (elem.empty()) {
throw mjCError(this, "elem is empty");
}
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 (vert.size() % 3) {
throw mjCError(this, "vert size must be a multiple of 3");
}
nelem = (int)elem.size()/(dim+1);
// set nvert, rigid, centered; check size
if (vert.empty()) {
centered = true;
nvert = (int)vertbody.size();
}
else {
nvert = (int)vert.size()/3;
if (vertbody.size()==1) {
rigid = true;
}
}
if (nvert<dim+1) {
throw mjCError(this, "not enough vertices");
}
// check elem vertex ids
for (int i=0; i<(int)elem.size(); i++) {
if (elem[i]<0 || elem[i]>=nvert) {
throw mjCError(this, "elem vertex id out of range");
}
}
// check texcoord
if (!texcoord.empty() && texcoord.size()!=2*nvert) {
throw mjCError(this, "two texture coordinates per vertex expected");
}
// resolve material name
mjCBase* pmat = model->FindObject(mjOBJ_MATERIAL, material);
if (pmat) {
matid = pmat->id;
} else if (!material.empty()) {
throw mjCError(this, "unkown material '%s' in flex", material.c_str());
}
// resolve body ids
for (int i=0; i<(int)vertbody.size(); i++) {
mjCBase* pbody = model->FindObject(mjOBJ_BODY, vertbody[i]);
if (pbody) {
vertbodyid.push_back(pbody->id);
} else {
throw mjCError(this, "unkown body '%s' in flex", vertbody[i].c_str());
}
}
// process elements
for (int e=0; e<(int)elem.size()/(dim+1); e++) {
// make sorted copy of element
vector<int> el;
el.assign(elem.begin()+e*(dim+1), elem.begin()+(e+1)*(dim+1));
std::sort(el.begin(), el.end());
// check for repeated vertices
for (int k=0; k<dim; k++) {
if (el[k]==el[k+1]) {
throw mjCError(this, "repeated vertex in element");
}
}
// make edges from sorted element
switch (dim) {
case 1: // line
edge.push_back(std::make_pair(el[0], el[1]));
break;
case 2: // triangle
edge.push_back(std::make_pair(el[0], el[1]));
edge.push_back(std::make_pair(el[1], el[2]));
edge.push_back(std::make_pair(el[0], el[2]));
break;
case 3: // tetrahedron
edge.push_back(std::make_pair(el[0], el[1]));
edge.push_back(std::make_pair(el[1], el[2]));
edge.push_back(std::make_pair(el[2], el[3]));
edge.push_back(std::make_pair(el[0], el[2]));
edge.push_back(std::make_pair(el[0], el[3]));
edge.push_back(std::make_pair(el[1], el[3]));
break;
}
}
// sort edges
std::sort(edge.begin(), edge.end());
// remove repeated edges
std::vector<std::pair<int,int>> edge1;
edge1.push_back(edge[0]);
for (int i=1; i<(int)edge.size(); i++) {
if (edge1[edge1.size()-1]!=edge[i]) {
edge1.push_back(edge[i]);
}
}
edge = edge1;
// set size
nedge = (int)edge.size();
// determine rigid if not already set
if (!rigid) {
rigid = true;
for (int i=1; i<(int)vertbodyid.size(); i++) {
if (vertbodyid[i]!=vertbodyid[0]) {
rigid = false;
break;
}
}
}
// determine centered if not already set
if (!centered) {
centered = true;
for (int i=0; i<(int)vert.size(); i++) {
if (vert[i]!=0) {
centered = false;
break;
}
}
}
// compute global vertex positions
vertxpos = vector<mjtNum> (3*nvert);
for (int i=0; i<nvert; i++) {
// get body id, set vertxpos = body.xpos0
int b = rigid ? vertbodyid[0] : vertbodyid[i];
mju_copy3(vertxpos.data()+3*i, model->bodies[b]->xpos0);
// add vertex offset within body if not centered
if (!centered) {
mjtNum offset[3];
mju_rotVecQuat(offset, vert.data()+3*i, model->bodies[b]->xquat0);
mju_addTo3(vertxpos.data()+3*i, offset);
}
}
// reorder tetrahedra so right-handed face orientation is outside
// faces are (0,1,2); (0,2,3); (0,3,1); (1,3,2)
if (dim==3) {
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
mjtNum* v0 = vertxpos.data() + 3*edata[0];
mjtNum* v1 = vertxpos.data() + 3*edata[1];
mjtNum* v2 = vertxpos.data() + 3*edata[2];
mjtNum* v3 = vertxpos.data() + 3*edata[3];
mjtNum v01[3] = {v1[0]-v0[0], v1[1]-v0[1], v1[2]-v0[2]};
mjtNum v02[3] = {v2[0]-v0[0], v2[1]-v0[1], v2[2]-v0[2]};
mjtNum v03[3] = {v3[0]-v0[0], v3[1]-v0[1], v3[2]-v0[2]};
// detect wrong orientation
mjtNum nrm[3];
mju_cross(nrm, v01, v02);
if (mju_dot3(nrm, v03)>0) {
// flip orientation
int tmp = elem[e*(dim+1)+1];
elem[e*(dim+1)+1] = elem[e*(dim+1)+2];
elem[e*(dim+1)+2] = tmp;
}
}
}
// create shell fragments and element-vertex collision pairs
CreateShellPair();
// create bounding volume hierarchy
CreateBVH();
}
// create flex BVH
void mjCFlex::CreateBVH(void) {
// init bounding volume object
mjCBoundingVolume bv;
bv.contype = contype;
bv.conaffinity = conaffinity;
bv.quat = NULL;
// allocate element bounding boxes
vector<mjtNum> elemaabb(6*nelem);
// construct element bounding boxes, add to hierarchy
for (int e=0; e<nelem; e++) {
const int* edata = elem.data() + e*(dim+1);
// skip inactive in 3D
if (dim==3 && elemlayer[e]>=activelayers) {
continue;
}
// compute min and max along each global axis
mjtNum xmin[3], xmax[3];
mju_copy3(xmin, vertxpos.data() + 3*edata[0]);
mju_copy3(xmax, vertxpos.data() + 3*edata[0]);
for (int i=1; i<=dim; i++) {
for (int j=0; j<3; j++) {
xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]);
xmax[j] = mjMAX(xmax[j], vertxpos[3*edata[i]+j]);
}
}
// compute aabb (center, size)
elemaabb[6*e+0] = 0.5*(xmax[0]+xmin[0]);
elemaabb[6*e+1] = 0.5*(xmax[1]+xmin[1]);
elemaabb[6*e+2] = 0.5*(xmax[2]+xmin[2]);
elemaabb[6*e+3] = 0.5*(xmax[0]-xmin[0]) + radius;
elemaabb[6*e+4] = 0.5*(xmax[1]-xmin[1]) + radius;
elemaabb[6*e+5] = 0.5*(xmax[2]-xmin[2]) + radius;
// add bounding volume for this element
bv.id = e;
bv.aabb = elemaabb.data() + 6*e;
bv.pos = bv.aabb;
tree.AddBoundingVolume(bv);
}
// create hierarchy
tree.CreateBVH();
}
// create shells and element-vertex collision pairs
void mjCFlex::CreateShellPair(void) {
vector<vector<int>> fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices]
vector<vector<int>> connectspec; // [elem1, elem2, common sorted frag vertices]
vector<bool> border(nelem, false); // is element on the border
vector<bool> borderfrag(nelem*(dim+1), false); // is fragment on the border
// make fragspec
for (int e=0; e<nelem; e++) {
int n = e*(dim+1);
// element vertices in original (unsorted) order
vector<int> el;
el.assign(elem.begin()+n, elem.begin()+n+dim+1);
// line: 2 vertex fragments
if (dim==1) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[1]);
}
// triangle: 3 edge fragments
else if (dim==2) {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[1]);
fragspec[n+2].push_back(el[2]);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[2]);
fragspec[n+1].push_back(el[0]);
}
// tetrahedron: 4 face fragments
else {
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n].push_back(e);
fragspec[n].push_back(el[0]);
fragspec[n].push_back(el[1]);
fragspec[n].push_back(el[2]);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+2].push_back(e);
fragspec[n+2].push_back(el[0]);
fragspec[n+2].push_back(el[2]);
fragspec[n+2].push_back(el[3]);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+1].push_back(e);
fragspec[n+1].push_back(el[0]);
fragspec[n+1].push_back(el[3]);
fragspec[n+1].push_back(el[1]);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
fragspec[n+3].push_back(e);
fragspec[n+3].push_back(el[1]);
fragspec[n+3].push_back(el[3]);
fragspec[n+3].push_back(el[2]);
}
}
// sort first segment of each fragspec
if (dim>1) {
for (int n=0; n<nelem*(dim+1); n++) {
std::sort(fragspec[n].begin(), fragspec[n].begin()+dim);
}
}
// sort fragspec
std::sort(fragspec.begin(), fragspec.end());
// make border and connectspec, record borderfrag
int cnt = 1;
for (int n=1; n<nelem*(dim+1); n++) {
// extract frag vertices, without elem
vector<int> previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim};
vector<int> current = {fragspec[n].begin(), fragspec[n].begin()+dim};
// same sequential fragments
if (previous==current) {
// found pair of elements connected by common fragment
vector<int> connect;
connect.insert(connect.end(), fragspec[n-1][dim]);
connect.insert(connect.end(), fragspec[n][dim]);
connect.insert(connect.end(), fragspec[n].begin(), fragspec[n].begin()+dim);
connectspec.push_back(connect);
// count same sequential fragments
cnt++;
}
// different sequential fragments
else {
// found border fragment
if (cnt==1) {
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// reset count
cnt = 1;
}
}
// last fragment is border
if (cnt==1) {
int n = nelem*(dim+1);
border[fragspec[n-1][dim]] = true;
borderfrag[n-1] = true;
}
// create shell
for (int i=0; i<(int)borderfrag.size(); i++) {
if (borderfrag[i]) {
// add fragment vertices, in original order
shell.insert(shell.end(), fragspec[i].begin()+dim+1, fragspec[i].end());
}
}
// compute elemlayer (distance from border) via value iteration in 3D
if (dim<3) {
elemlayer = vector<int> (nelem, 0);
}
else {
elemlayer = vector<int> (nelem, nelem+1); // init with greater than max value
for (int e=0; e<nelem; e++) {
if (border[e]) {
elemlayer[e] = 0; // set border elements to 0
}
}
bool change = true;
while (change) { // repeat while changes are happening
change = false;
// process edges of element connectivity graph
for (int i=0; i<(int)connectspec.size(); i++) {
int e1 = connectspec[i][0]; // get element pair for this edge
int e2 = connectspec[i][1];
if (elemlayer[e1]>elemlayer[e2]+1) {
elemlayer[e1] = elemlayer[e2]+1; // better value found for e1: update
change = true;
} else if (elemlayer[e2]>elemlayer[e1]+1) {
elemlayer[e2] = elemlayer[e1]+1; // better value found for e2: update
change = true;
}
}
}
}
// create evpairs in 1D and 2D
if (dim<3) {
// process connected element pairs containing a border element
for (int n=0; n<(int)connectspec.size(); n++) {
if (border[connectspec[n][0]] || border[connectspec[n][1]]) {
// extract common fragment
vector<int> frag = {connectspec[n].begin()+2, connectspec[n].end()};
// process both elements
for (int ei=0; ei<2; ei++) {
const int* edata = elem.data() + connectspec[n][ei]*(dim+1);
// find element vertex that is not in the common fragment
for (int i=0; i<=dim; i++) {
if (frag.end() == std::find(frag.begin(), frag.end(), edata[i])) {
// add ev pair, involving the other element in connectspec
evpair.push_back(connectspec[n][1-ei]);
evpair.push_back(edata[i]);
// one such vertex exists
break;
}
}
}
}
}
}
}
src/user/user_model.cc
+225 -27
View File
@@ -139,6 +139,7 @@ mjCModel::mjCModel() {
//------------------------ private variables
cameras.clear();
lights.clear();
flexes.clear();
meshes.clear();
skins.clear();
hfields.clear();
@@ -185,6 +186,7 @@ mjCModel::~mjCModel() {
delete bodies[0];
// delete objects allocated in mjCModel
for (int i=0; i<flexes.size(); i++) delete flexes[i];
for (int i=0; i<meshes.size(); i++) delete meshes[i];
for (int i=0; i<skins.size(); i++) delete skins[i];
for (int i=0; i<hfields.size(); i++) delete hfields[i];
@@ -204,6 +206,7 @@ mjCModel::~mjCModel() {
for (int i=0; i<defaults.size(); i++) delete defaults[i];
// clear pointer lists created in model construction
flexes.clear();
meshes.clear();
skins.clear();
hfields.clear();
@@ -233,11 +236,14 @@ void mjCModel::Clear(void) {
// sizes set from list lengths
nbody = 0;
nbvh = 0;
nbvhstatic = 0;
nbvhdynamic = 0;
njnt = 0;
ngeom = 0;
nsite = 0;
ncam = 0;
nlight = 0;
nflex = 0;
nmesh = 0;
nskin = 0;
nhfield = 0;
@@ -256,6 +262,13 @@ void mjCModel::Clear(void) {
nv = 0;
nu = 0;
na = 0;
nflexvert = 0;
nflexedge = 0;
nflexelem = 0;
nflexelemdata = 0;
nflexshelldata = 0;
nflexevpair = 0;
nflextexcoord = 0;
nmeshvert = 0;
nmeshnormal = 0;
nmeshtexcoord = 0;
@@ -326,6 +339,12 @@ T* mjCModel::AddObjectDef(vector<T*>& list, string type, mjCDef* def) {
}
// add flex
mjCFlex* mjCModel::AddFlex(void) {
return AddObject(flexes, "flex");
}
// add mesh
mjCMesh* mjCModel::AddMesh(mjCDef* def) {
return AddObjectDef(meshes, "mesh", def);
@@ -443,6 +462,8 @@ int mjCModel::NumObjects(mjtObj type) {
return (int)cameras.size();
case mjOBJ_LIGHT:
return (int)lights.size();
case mjOBJ_FLEX:
return (int)flexes.size();
case mjOBJ_MESH:
return (int)meshes.size();
case mjOBJ_SKIN:
@@ -499,6 +520,8 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) {
return cameras[id];
case mjOBJ_LIGHT:
return lights[id];
case mjOBJ_FLEX:
return flexes[id];
case mjOBJ_MESH:
return meshes[id];
case mjOBJ_SKIN:
@@ -640,6 +663,8 @@ mjCBase* mjCModel::FindObject(mjtObj type, string name) {
return findobject(name, cameras);
case mjOBJ_LIGHT:
return findobject(name, lights);
case mjOBJ_FLEX:
return findobject(name, flexes);
case mjOBJ_MESH:
return findobject(name, meshes);
case mjOBJ_SKIN:
@@ -896,6 +921,7 @@ void mjCModel::SetSizes(void) {
nsite = (int)sites.size();
ncam = (int)cameras.size();
nlight = (int)lights.size();
nflex = (int)flexes.size();
nmesh = (int)meshes.size();
nskin = (int)skins.size();
nhfield = (int)hfields.size();
@@ -928,22 +954,38 @@ void mjCModel::SetSizes(void) {
}
}
// nbvh
// nbvh, nbvhstatic, nbvhdynamic
for (int i=0; i<nbody; i++) {
nbvh += bodies[i]->tree.nbvh;
nbvhstatic += bodies[i]->tree.nbvh;
}
for (int i=0; i<nmesh; i++) {
nbvhstatic += meshes[i]->tree().nbvh;
}
for (int i=0; i<nflex; i++) {
nbvhdynamic += flexes[i]->tree.nbvh;
}
nbvh = nbvhstatic + nbvhdynamic;
// flex counts
for (int i=0; i<nflex; i++) {
nflexvert += flexes[i]->nvert;
nflexedge += flexes[i]->nedge;
nflexelem += flexes[i]->nelem;
nflexelemdata += flexes[i]->nelem * (flexes[i]->dim + 1);
nflexshelldata += (int)flexes[i]->shell.size();
nflexevpair += (int)flexes[i]->evpair.size()/2;
}
// nmeshvert, nmeshface, nmeshtexcoord, nmeshgraph
// mesh counts
for (int i=0; i<nmesh; i++) {
nmeshvert += meshes[i]->nvert();
nmeshnormal += meshes[i]->nnormal();
nmeshface += meshes[i]->nface();
nmeshtexcoord += (meshes[i]->HasTexcoord() ? meshes[i]->ntexcoord() : 0);
nmeshgraph += meshes[i]->szgraph();
nbvh += meshes[i]->tree().nbvh;
}
// nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert
// skin counts
for (int i=0; i<nskin; i++) {
nskinvert += skins[i]->vert.size()/3;
nskintexvert += skins[i]->texcoord.size()/2;
@@ -986,6 +1028,7 @@ void mjCModel::SetSizes(void) {
for (int i=0; i<nsite; i++) nnames += (int)sites[i]->name.length() + 1;
for (int i=0; i<ncam; i++) nnames += (int)cameras[i]->name.length() + 1;
for (int i=0; i<nlight; i++) nnames += (int)lights[i]->name.length() + 1;
for (int i=0; i<nflex; i++) nnames += (int)flexes[i]->name.length() + 1;
for (int i=0; i<nmesh; i++) nnames += (int)meshes[i]->name.length() + 1;
for (int i=0; i<nskin; i++) nnames += (int)skins[i]->name.length() + 1;
for (int i=0; i<nhfield; i++) nnames += (int)hfields[i]->name.length() + 1;
@@ -1271,6 +1314,9 @@ void mjCModel::CopyNames(mjModel* m) {
adr = namelist(lights, adr, m->name_lightadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*lights.size();
adr = namelist(flexes, adr, m->name_flexadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*flexes.size();
adr = namelist(meshes, adr, m->name_meshadr, m->names, map_adr);
map_adr += mjLOAD_MULTIPLE*meshes.size();
@@ -1382,13 +1428,17 @@ void mjCModel::CopyTree(mjModel* m) {
m->body_gravcomp[i] = pb->gravcomp;
copyvec(m->body_user+nuser_body*i, pb->userdata.data(), nuser_body);
m->body_contype[i] = pb->contype;
m->body_conaffinity[i] = pb->conaffinity;
m->body_margin[i] = (mjtNum)pb->margin;
// bounding volume hierarchy
m->body_bvhadr[i] = (!pb->geoms.empty() ? bvh_adr : -1);
m->body_bvhadr[i] = pb->tree.nbvh ? bvh_adr : -1;
m->body_bvhnum[i] = pb->tree.nbvh;
if (pb->tree.nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pb->tree.bvh.data(), 6*pb->tree.nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pb->tree.child.data(), 2*pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pb->tree.nodeid.data(), pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pb->tree.nodeid.data(), pb->tree.nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pb->tree.level.data(), pb->tree.nbvh*sizeof(int));
}
bvh_adr += pb->tree.nbvh;
@@ -1753,7 +1803,8 @@ void mjCModel::CopyTree(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 bonevert_adr, graph_adr, data_adr, bvh_adr=0;
int edge_adr, elem_adr, elemdata_adr, shelldata_adr, evpair_adr;
int bonevert_adr, graph_adr, data_adr, bvh_adr;
// sizes outside call to mj_makeModel
m->nemax = nemax;
@@ -1762,15 +1813,18 @@ void mjCModel::CopyObjects(mjModel* m) {
m->nsensordata = nsensordata;
m->nuserdata = nuserdata;
// find bvh_adr after bodies
bvh_adr = 0;
for (int i=0; i<nbody; i++) {
bvh_adr = mjMAX(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
}
// meshes
vert_adr = 0;
normal_adr = 0;
texcoord_adr = 0;
face_adr = 0;
graph_adr = 0;
for (int i=0; i<nbody; i++) {
bvh_adr = mju_max(bvh_adr, m->body_bvhadr[i] + m->body_bvhnum[i]);
}
for (int i=0; i<nmesh; i++) {
// get pointer
mjCMesh* pme = meshes[i];
@@ -1785,8 +1839,8 @@ void mjCModel::CopyObjects(mjModel* m) {
m->mesh_faceadr[i] = face_adr;
m->mesh_facenum[i] = pme->nface();
m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1);
m->mesh_bvhadr[i] = bvh_adr;
m->mesh_bvhnum[i] = pme->tree().nbvh;
m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1;
copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3);
copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4);
@@ -1804,10 +1858,14 @@ void mjCModel::CopyObjects(mjModel* m) {
if (pme->szgraph()) {
pme->CopyGraph(m->mesh_graph + graph_adr);
}
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pme->tree().child.data(), 2*pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree().level.data(), pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_geomid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int));
// copy bvh data
if (pme->tree().nbvh) {
memcpy(m->bvh_aabb + 6*bvh_adr, pme->tree().bvh.data(), 6*pme->tree().nbvh*sizeof(mjtNum));
memcpy(m->bvh_child + 2*bvh_adr, pme->tree().child.data(), 2*pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pme->tree().level.data(), pme->tree().nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pme->tree().nodeid.data(), pme->tree().nbvh*sizeof(int));
}
// advance counters
vert_adr += pme->nvert();
@@ -1818,6 +1876,127 @@ void mjCModel::CopyObjects(mjModel* m) {
bvh_adr += pme->tree().nbvh;
}
// flexes
vert_adr = 0;
edge_adr = 0;
elem_adr = 0;
elemdata_adr = 0;
shelldata_adr = 0;
evpair_adr = 0;
texcoord_adr = 0;
for (int i=0; i<nflex; i++) {
// get pointer
mjCFlex* pfl = flexes[i];
// set fields: geom-like
m->flex_contype[i] = pfl->contype;
m->flex_conaffinity[i] = pfl->conaffinity;
m->flex_condim[i] = pfl->condim;
m->flex_matid[i] = pfl->matid;
m->flex_group[i] = pfl->group;
m->flex_priority[i] = pfl->priority;
m->flex_solmix[i] = (mjtNum)pfl->solmix;
copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF);
copyvec(m->flex_solimp + mjNIMP * i, pfl->solimp, mjNIMP);
m->flex_radius[i] = (mjtNum)pfl->radius;
copyvec(m->flex_friction + 3 * i, pfl->friction, 3);
m->flex_margin[i] = (mjtNum)pfl->margin;
m->flex_gap[i] = (mjtNum)pfl->gap;
copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4);
// set fields: mesh-like
m->flex_dim[i] = pfl->dim;
m->flex_vertadr[i] = vert_adr;
m->flex_vertnum[i] = pfl->nvert;
m->flex_edgeadr[i] = edge_adr;
m->flex_edgenum[i] = pfl->nedge;
m->flex_elemadr[i] = elem_adr;
m->flex_elemdataadr[i] = elemdata_adr;
m->flex_shellnum[i] = (int)pfl->shell.size()/pfl->dim;
m->flex_shelldataadr[i] = m->flex_shellnum[i] ? shelldata_adr : -1;
if (pfl->evpair.empty()) {
m->flex_evpairadr[i] = -1;
m->flex_evpairnum[i] = 0;
} else {
m->flex_evpairadr[i] = evpair_adr;
m->flex_evpairnum[i] = (int)pfl->evpair.size()/2;
memcpy(m->flex_evpair + 2*evpair_adr, pfl->evpair.data(), pfl->evpair.size()*sizeof(int));
}
if (pfl->texcoord.empty()) {
m->flex_texcoordadr[i] = -1;
} else {
m->flex_texcoordadr[i] = texcoord_adr;
memcpy(m->flex_texcoord + 2*texcoord_adr,
pfl->texcoord.data(), pfl->texcoord.size()*sizeof(float));
}
m->flex_elemnum[i] = pfl->nelem;
memcpy(m->flex_elem + elemdata_adr, pfl->elem.data(), pfl->elem.size()*sizeof(int));
memcpy(m->flex_elemlayer + elem_adr, pfl->elemlayer.data(), pfl->nelem*sizeof(int));
if (m->flex_shellnum[i]) {
memcpy(m->flex_shell + shelldata_adr, pfl->shell.data(), pfl->shell.size()*sizeof(int));
}
m->flex_edgestiffness[i] = (mjtNum)pfl->edgestiffness;
m->flex_edgedamping[i] = (mjtNum)pfl->edgedamping;
m->flex_rigid[i] = pfl->rigid;
m->flex_centered[i] = pfl->centered;
m->flex_internal[i] = pfl->internal;
m->flex_flatskin[i] = pfl->flatskin;
m->flex_selfcollide[i] = pfl->selfcollide;
m->flex_activelayers[i] = pfl->activelayers;
m->flex_bvhnum[i] = pfl->tree.nbvh;
m->flex_bvhadr[i] = pfl->tree.nbvh ? bvh_adr : -1;
// find equality constraint referencing this flex
m->flex_edgeequality[i] = 0;
for (int k=0; k<(int)equalities.size(); k++) {
if (equalities[k]->type==mjEQ_FLEX && equalities[k]->name1==pfl->name) {
m->flex_edgeequality[i] = 1;
break;
}
}
// copy bvh data (flex aabb computed dynamically in mjData)
if (pfl->tree.nbvh) {
memcpy(m->bvh_child + 2*bvh_adr, pfl->tree.child.data(), 2*pfl->tree.nbvh*sizeof(int));
memcpy(m->bvh_depth + bvh_adr, pfl->tree.level.data(), pfl->tree.nbvh*sizeof(int));
memcpy(m->bvh_nodeid + bvh_adr, pfl->tree.nodeid.data(), pfl->tree.nbvh*sizeof(int));
}
// copy or set vert
if (pfl->centered) {
mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert);
}
else {
memcpy(m->flex_vert + 3*vert_adr, pfl->vert.data(), 3*pfl->nvert*sizeof(mjtNum));
}
// copy or set vertbodyid
if (pfl->rigid) {
for (int k=0; k<pfl->nvert; k++) {
m->flex_vertbodyid[vert_adr + k] = pfl->vertbodyid[0];
}
}
else {
memcpy(m->flex_vertbodyid + vert_adr, pfl->vertbodyid.data(), pfl->nvert*sizeof(int));
}
// convert edge pairs to int array
for (int i=0; i<pfl->nedge; i++) {
m->flex_edge[2*(edge_adr+i)] = pfl->edge[i].first;
m->flex_edge[2*(edge_adr+i)+1] = pfl->edge[i].second;
}
// advance counters
vert_adr += pfl->nvert;
edge_adr += pfl->nedge;
elem_adr += pfl->nelem;
elemdata_adr += (pfl->dim+1) * pfl->nelem;
shelldata_adr += (int)pfl->shell.size();
evpair_adr += (int)pfl->evpair.size()/2;
texcoord_adr += (int)pfl->texcoord.size()/2;
bvh_adr += pfl->tree.nbvh;
}
// skins
vert_adr = 0;
face_adr = 0;
@@ -1830,8 +2009,8 @@ void mjCModel::CopyObjects(mjModel* m) {
// set fields
m->skin_matid[i] = psk->matid;
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_group[i] = psk->group;
copyvec(m->skin_rgba+4*i, psk->rgba, 4);
m->skin_inflate[i] = psk->inflate;
m->skin_vertadr[i] = vert_adr;
m->skin_vertnum[i] = psk->vert.size()/3;
@@ -2412,7 +2591,6 @@ void mjCModel::FuseStatic(void) {
//------------------------------- COMPILER ---------------------------------------------------------
// signature comparisons
@@ -2445,13 +2623,25 @@ static void processlist(vector<T*>& list, string defname, bool checkrepeat=true)
// id equals position in array
list[i]->id = i;
}
// compare to all previous names
if (checkrepeat) {
for (int j=0; j<i; j++) {
if (list[i]->name == list[j]->name && list[j]->name != "") {
throw mjCError(list[i], "repeated name in %s array, position %d", defname.c_str(), i);
}
// check for repeated names
if (checkrepeat) {
// created vectors with all names
vector<string> allnames;
for (int i=0; i<(int)list.size(); i++) {
if (!list[i]->name.empty()) {
allnames.push_back(list[i]->name);
}
}
// sort and check for duplicates
if (allnames.size()>1) {
std::sort(allnames.begin(), allnames.end());
auto adjacent = std::adjacent_find(allnames.begin(), allnames.end());
if (adjacent!=allnames.end()) {
string msg = "repeated name '" + *adjacent + "' in " + defname;
throw mjCError(NULL, msg.c_str());
}
}
}
@@ -2549,6 +2739,11 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
throw mjCError(0, "joint found in world body");
}
// check for too many body+flex
if (bodies.size()+flexes.size()>=65534) {
throw mjCError(0, "number of bodies plus flexes must be less than 65534");
}
// append directory separator
if (!meshdir.empty()) {
int n = meshdir.length();
@@ -2578,6 +2773,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
processlist(sites, "site");
processlist(cameras, "camera");
processlist(lights, "light");
processlist(flexes, "flex");
processlist(meshes, "mesh");
processlist(skins, "skin");
processlist(hfields, "hfield");
@@ -2668,6 +2864,7 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// compile all other objects except for keyframes
for (int i=0; i<flexes.size(); i++) flexes[i]->Compile(vfs);
for (int i=0; i<skins.size(); i++) skins[i]->Compile(vfs);
for (int i=0; i<hfields.size(); i++) hfields[i]->Compile(vfs);
for (int i=0; i<textures.size(); i++) textures[i]->Compile(vfs);
@@ -2744,12 +2941,13 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) {
}
// create low-level model
m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, njnt, ngeom, nsite, ncam, nlight,
m = mj_makeModel(nq, nv, nu, na, nbody, nbvh, nbvhstatic, nbvhdynamic, njnt, ngeom, nsite,
ncam, nlight, nflex, nflexvert, nflexedge, nflexelem,
nflexelemdata, nflexshelldata, nflexevpair, nflextexcoord,
nmesh, nmeshvert, nmeshnormal, nmeshtexcoord, nmeshface, nmeshgraph,
nskin, nskinvert, nskintexvert, nskinface, nskinbone, nskinbonevert,
nhfield, nhfielddata, ntex, ntexdata, nmat, npair, nexclude,
neq, ntendon, nwrap, nsensor,
nnumeric, nnumericdata, ntext, ntextdata,
neq, ntendon, nwrap, nsensor, nnumeric, nnumericdata, ntext, ntextdata,
ntuple, ntupledata, nkey, nmocap, nplugin, npluginattr,
nuser_body, nuser_jnt, nuser_geom, nuser_site, nuser_cam,
nuser_tendon, nuser_actuator, nuser_sensor, nnames, npaths);
src/user/user_model.h
+14 -1
View File
@@ -44,6 +44,7 @@ class mjCModel {
friend class mjCBody;
friend class mjCJoint;
friend class mjCGeom;
friend class mjCFlex;
friend class mjCMesh;
friend class mjCSkin;
friend class mjCHField;
@@ -72,6 +73,7 @@ class mjCModel {
//------------------------ API for adding model elements
mjCFlex* AddFlex(void); // flex
mjCMesh* AddMesh(mjCDef* def = 0); // mesh
mjCSkin* AddSkin(void); // skin
mjCHField* AddHField(void); // heightfield
@@ -194,6 +196,7 @@ class mjCModel {
int nsite; // number of sites
int ncam; // number of cameras
int nlight; // number of lights
int nflex; // number of flexes
int nmesh; // number of meshes
int nskin; // number of skins
int nhfield; // number of height fields
@@ -216,7 +219,16 @@ class mjCModel {
int nv; // number of degrees of freedom = dim(qvel)
int nu; // number of actuators/controls
int na; // number of activation variables
int nbvh; // number of boundary volume hierarchies
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 nflexvert; // number of vertices in all flexes
int nflexedge; // number of edges in all flexes
int nflexelem; // number of elements in all flexes
int nflexelemdata; // number of element vertex ids in all flexes
int nflexshelldata; // number of shell fragment vertex ids in all flexes
int nflexevpair; // number of element-vertex pairs in all flexes
int nflextexcoord; // number of vertex texture coordinates in all flexes
int nmeshvert; // number of vertices in all meshes
int nmeshnormal; // number of normals in all meshes
int nmeshtexcoord; // number of texture coordinates in all meshes
@@ -243,6 +255,7 @@ class mjCModel {
//------------------------ object lists
// objects created here
std::vector<mjCFlex*> flexes; // list of flexes
std::vector<mjCMesh*> meshes; // list of meshes
std::vector<mjCSkin*> skins; // list of skins
std::vector<mjCHField*> hfields; // list of height fields
src/user/user_objects.cc
+111 -73
View File
@@ -296,34 +296,39 @@ void mjCBoundingVolumeHierarchy::CreateBVH() {
// compute bounding volume hierarchy
int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements, int lev) {
if (elements.empty()) {
return -1;
}
int nelements = elements.size();
mjtNum AABB[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL};
// inverse transformation
mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]};
// accumulate AAMM over elements
for (int i=0; i<nelements; i++) {
// skip visual objects
if (elements[i].conaffinity==0 && elements[i].contype==0) {
continue;
}
// transform aabb representation
mjtNum aabb[6] = {elements[i].aabb[0] - elements[i].aabb[3],
// transform element aabb to aamm format
mjtNum aamm[6] = {elements[i].aabb[0] - elements[i].aabb[3],
elements[i].aabb[1] - elements[i].aabb[4],
elements[i].aabb[2] - elements[i].aabb[5],
elements[i].aabb[0] + elements[i].aabb[3],
elements[i].aabb[1] + elements[i].aabb[4],
elements[i].aabb[2] + elements[i].aabb[5]};
// update node AABB
// update node AAMM
for (int v=0; v<8; v++) {
mjtNum vert[3], box[3];
vert[0] = (v&1 ? aabb[3] : aabb[0]);
vert[1] = (v&2 ? aabb[4] : aabb[1]);
vert[2] = (v&4 ? aabb[5] : aabb[2]);
vert[0] = (v&1 ? aamm[3] : aamm[0]);
vert[1] = (v&2 ? aamm[4] : aamm[1]);
vert[2] = (v&4 ? aamm[5] : aamm[2]);
// rotate to the body inertial frame
// rotate to the body inertial frame if specified
if (elements[i].quat) {
mju_rotVecQuat(box, vert, elements[i].quat);
box[0] += elements[i].pos[0] - ipos_[0];
@@ -332,20 +337,20 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
mju_rotVecQuat(vert, box, qinv);
}
AABB[0] = mjMIN(AABB[0], vert[0]);
AABB[1] = mjMIN(AABB[1], vert[1]);
AABB[2] = mjMIN(AABB[2], vert[2]);
AABB[3] = mjMAX(AABB[3], vert[0]);
AABB[4] = mjMAX(AABB[4], vert[1]);
AABB[5] = mjMAX(AABB[5], vert[2]);
AAMM[0] = mjMIN(AAMM[0], vert[0]);
AAMM[1] = mjMIN(AAMM[1], vert[1]);
AAMM[2] = mjMIN(AAMM[2], vert[2]);
AAMM[3] = mjMAX(AAMM[3], vert[0]);
AAMM[4] = mjMAX(AAMM[4], vert[1]);
AAMM[5] = mjMAX(AAMM[5], vert[2]);
}
}
// inflate flat AABBs
for (int i=0; i<3; i++) {
if (mju_abs(AABB[i]-AABB[i+3])<mjEPS) {
AABB[i+0] -= mjEPS;
AABB[i+3] += mjEPS;
if (mju_abs(AAMM[i]-AAMM[i+3])<mjEPS) {
AAMM[i+0] -= mjEPS;
AAMM[i+3] += mjEPS;
}
}
@@ -358,10 +363,10 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
// store bounding box of the current node
for (int i=0; i<3; i++) {
bvh.push_back((AABB[3+i] + AABB[i]) / 2);
bvh.push_back((AAMM[3+i] + AAMM[i]) / 2);
}
for (int i=0; i<3; i++) {
bvh.push_back((AABB[3+i] - AABB[i]) / 2);
bvh.push_back((AAMM[3+i] - AAMM[i]) / 2);
}
// leaf node, return
@@ -374,7 +379,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH(std::vector<mjCBoundingVolume>& elements
}
// find longest axis for splitting the bounding box
mjtNum edges[3] = { AABB[3]-AABB[0], AABB[4]-AABB[1], AABB[5]-AABB[2] };
mjtNum edges[3] = { AAMM[3]-AAMM[0], AAMM[4]-AAMM[1], AAMM[5]-AAMM[2] };
int axis = edges[0] > edges[1] ? 0 : 1;
axis = edges[axis] > edges[2] ? axis : 2;
@@ -505,7 +510,7 @@ mjResource* mjCBase::LoadResource(string filename, const mjVFS* vfs) {
if ((r = mju_openVfsResource(cname, vfs)) == nullptr) {
// not in vfs try a provider or fallback to OS filesystem
if ((r = mju_openResource(filename.c_str())) == nullptr) {
throw mjCError(this, "resource not found via provider or OS filesystem: '%s'", cname);
throw mjCError(nullptr, "resource not found via provider or OS filesystem: '%s'", cname);
}
}
return r;
@@ -560,6 +565,12 @@ mjCBody::mjCBody(mjCModel* _model) {
gravcomp = 0;
userdata.clear();
contype = 0;
conaffinity = 0;
margin = 0;
mjuu_zerovec(xpos0, 3);
mjuu_setvec(xquat0, 1, 0, 0, 0);
// clear object lists
bodies.clear();
geoms.clear();
@@ -984,6 +995,15 @@ void mjCBody::Compile(void) {
MakeLocal(geoms[i]->locpos, geoms[i]->locquat, geoms[i]->pos, geoms[i]->quat);
}
// accumulate rbound, contype, conaffinity over geoms
contype = conaffinity = 0;
margin = 0;
for (int i=0; i<geoms.size(); i++) {
contype |= geoms[i]->contype;
conaffinity |= geoms[i]->conaffinity;
margin = mju_max(margin, geoms[i]->margin);
}
// compute bounding volume hierarchy
if (!geoms.empty()) {
tree.Set(ipos, iquat);
@@ -1021,6 +1041,14 @@ void mjCBody::Compile(void) {
throw mjCError(this, "mocap body '%s' is not a fixed child of world", name.c_str());
}
// compute body global pose (no joint transformations in qpos0)
if (id>0) {
mjCBody* par = model->bodies[parentid];
mju_rotVecQuat(xpos0, locpos, par->xquat0);
mju_addTo3(xpos0, par->xpos0);
mju_mulQuat(xquat0, par->xquat0, locquat);
}
// compile all sites
for (int i=0; i<sites.size(); i++) sites[i]->Compile();
@@ -1329,6 +1357,7 @@ double mjCGeom::GetVolume(void) {
case mjGEOM_ELLIPSOID:
return 4*mjPI*size[0]*size[1]*size[2]/3;
case mjGEOM_HFIELD:
case mjGEOM_BOX:
return size[0]*size[1]*size[2]*8;
@@ -1408,6 +1437,7 @@ void mjCGeom::SetInertia(void) {
inertia[2] = mass*(size[0]*size[0]+size[1]*size[1])/5;
return;
case mjGEOM_HFIELD:
case mjGEOM_BOX:
inertia[0] = mass*(size[1]*size[1]+size[2]*size[2])/3;
inertia[1] = mass*(size[0]*size[0]+size[2]*size[2])/3;
@@ -1425,10 +1455,15 @@ void mjCGeom::SetInertia(void) {
// compute radius of bounding sphere
double mjCGeom::GetRBound(void) {
const double* aabb;
const double *aamm, *hsize;
double haabb[3] = {0};
switch (type) {
case mjGEOM_HFIELD:
hsize = model->hfields[hfieldid]->size;
return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] +
mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3]));
case mjGEOM_SPHERE:
return size[0];
@@ -1446,10 +1481,10 @@ double mjCGeom::GetRBound(void) {
case mjGEOM_MESH:
case mjGEOM_SDF:
aabb = model->meshes[meshid]->aabb();
haabb[0] = mju_max(fabs(aabb[0]), fabs(aabb[3]));
haabb[1] = mju_max(fabs(aabb[1]), fabs(aabb[4]));
haabb[2] = mju_max(fabs(aabb[2]), fabs(aabb[5]));
aamm = model->meshes[meshid]->aamm();
haabb[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
haabb[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
haabb[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]);
default:
@@ -1571,68 +1606,63 @@ void mjCGeom::SetFluidCoefs(void) {
// compute bounding box
void mjCGeom::ComputeAABB() {
void mjCGeom::ComputeAABB(void) {
double aamm[6]; // axis-aligned bounding box in (min, max) format
switch (type) {
case mjGEOM_HFIELD:
aamm[0] = -model->hfields[hfieldid]->size[0];
aamm[1] = -model->hfields[hfieldid]->size[1];
aamm[2] = -model->hfields[hfieldid]->size[3];
aamm[3] = model->hfields[hfieldid]->size[0];
aamm[4] = model->hfields[hfieldid]->size[1];
aamm[5] = model->hfields[hfieldid]->size[2];
break;
case mjGEOM_SPHERE:
aabb[3] = aabb[4] = aabb[5] = size[0];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = aamm[5] = size[0];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_CAPSULE:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[0] + size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = size[0];
aamm[5] = size[0] + size[1];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_CYLINDER:
aabb[3] = aabb[4] = size[0];
aabb[5] = size[1];
mjuu_setvec(aabb, -aabb[3], -aabb[4], -aabb[5]);
aamm[3] = aamm[4] = size[0];
aamm[5] = size[1];
mjuu_setvec(aamm, -aamm[3], -aamm[4], -aamm[5]);
break;
case mjGEOM_MESH:
case mjGEOM_SDF:
mjuu_copyvec(aabb, model->meshes[meshid]->aabb(), 6);
mjuu_copyvec(aamm, model->meshes[meshid]->aamm(), 6);
break;
case mjGEOM_PLANE:
aabb[0] = aabb[1] = aabb[2] = -mjMAXVAL;
aabb[3] = aabb[4] = mjMAXVAL;
aabb[5] = 0;
break;
case mjGEOM_HFIELD:
aabb[0] = -size[0];
aabb[1] = -size[1];
aabb[2] = -model->hfields[hfieldid]->size[3];
aabb[3] = size[0];
aabb[4] = size[1];
aabb[5] = model->hfields[hfieldid]->size[2];
aamm[0] = aamm[1] = aamm[2] = -mjMAXVAL;
aamm[3] = aamm[4] = mjMAXVAL;
aamm[5] = 0;
break;
default:
mjuu_copyvec(aabb+3, size, 3);
mjuu_setvec(aabb, -size[0], -size[1], -size[2]);
mjuu_copyvec(aamm+3, size, 3);
mjuu_setvec(aamm, -size[0], -size[1], -size[2]);
break;
}
aabb[0] -= margin;
aabb[1] -= margin;
aabb[2] -= margin;
aabb[3] += margin;
aabb[4] += margin;
aabb[5] += margin;
mjtNum pos[] = {(aabb[3] + aabb[0]) / 2, (aabb[4] + aabb[1]) / 2,
(aabb[5] + aabb[2]) / 2};
mjtNum size[] = {(aabb[3] - aabb[0]) / 2, (aabb[4] - aabb[1]) / 2,
(aabb[5] - aabb[2]) / 2};
// convert aamm to aabb (center, size) format
double pos[] = {(aamm[3] + aamm[0]) / 2, (aamm[4] + aamm[1]) / 2,
(aamm[5] + aamm[2]) / 2};
double size[] = {(aamm[3] - aamm[0]) / 2, (aamm[4] - aamm[1]) / 2,
(aamm[5] - aamm[2]) / 2};
mjuu_copyvec(aabb, pos, 3);
mjuu_copyvec(aabb+3, size, 3);
}
// compiler
void mjCGeom::Compile(void) {
// resize userdata
@@ -1662,9 +1692,9 @@ void mjCGeom::Compile(void) {
throw mjCError(this, "hfield geom '%s' (id = %d) must have valid hfieldid", name.c_str(), id);
}
// plane and hfield only allowed in static bodies
if ((type==mjGEOM_PLANE || type==mjGEOM_HFIELD) && body->weldid!=0) {
throw mjCError(this, "plane and hfield only allowed in static bodies: geom '%s' (id = %d)",
// plane only allowed in static bodies
if (type==mjGEOM_PLANE && body->weldid!=0) {
throw mjCError(this, "plane only allowed in static bodies: geom '%s' (id = %d)",
name.c_str(), id);
}
@@ -1759,12 +1789,13 @@ void mjCGeom::Compile(void) {
if (type==mjGEOM_HFIELD) {
size[0] = model->hfields[hfieldid]->size[0];
size[1] = model->hfields[hfieldid]->size[1];
size[2] = 0.5*(model->hfields[hfieldid]->size[2]+model->hfields[hfieldid]->size[3]);
size[2] = 0.5*(0.5*model->hfields[hfieldid]->size[2] +
model->hfields[hfieldid]->size[3]);
} else if (type==mjGEOM_MESH || type==mjGEOM_SDF) {
const double* aabb = model->meshes[meshid]->aabb();
size[0] = mju_max(fabs(aabb[0]), fabs(aabb[3]));
size[1] = mju_max(fabs(aabb[1]), fabs(aabb[4]));
size[2] = mju_max(fabs(aabb[2]), fabs(aabb[5]));
const double* aamm = model->meshes[meshid]->aamm();
size[0] = mju_max(fabs(aamm[0]), fabs(aamm[3]));
size[1] = mju_max(fabs(aamm[1]), fabs(aamm[4]));
size[2] = mju_max(fabs(aamm[2]), fabs(aamm[5]));
}
for (double s : size) {
@@ -3120,7 +3151,7 @@ void mjCPair::Compile(void) {
// get geom ids and body signature
geom1 = pg1->id;
geom2 = pg2->id;
signature = ((pg1->body->id+1)<<16) + pg2->body->id+1;
signature = ((pg1->body->id)<<16) + pg2->body->id;
// set undefined margin: max
if (!mjuu_defined(margin)) {
@@ -3262,7 +3293,7 @@ void mjCBodyPair::Compile(void) {
// get body ids and body signature
body1 = pb1->id;
body2 = pb2->id;
signature = ((body1+1)<<16) + body2+1;
signature = (body1<<16) + body2;
}
@@ -3310,6 +3341,8 @@ void mjCEquality::Compile(void) {
objtype = mjOBJ_JOINT;
} else if (type==mjEQ_TENDON) {
objtype = mjOBJ_TENDON;
} else if (type==mjEQ_FLEX) {
objtype = mjOBJ_FLEX;
} else {
throw mjCError(this, "invalid type in equality constraint '%s' (id = %d)'", name.c_str(), id);
}
@@ -3330,7 +3363,7 @@ void mjCEquality::Compile(void) {
obj2id = px2->id;
}
// object 2 unspecified: set to -1, except for distance
// object 2 unspecified: set to -1
else {
if (objtype==mjOBJ_GEOM) {
throw mjCError(this, "both geom are required in equality constraint '%s' (id = %d)",
@@ -3346,6 +3379,11 @@ void mjCEquality::Compile(void) {
obj2id = 0;
}
// make sure flex is not rigid
if (type==mjEQ_FLEX && model->flexes[obj1id]->rigid) {
throw mjCError(this, "rigid flex '%s' in equality constraint %d", name1.c_str(), id);
}
// make sure the two objects are different
if (obj1id==obj2id) {
throw mjCError(this, "element '%s' is repeated in equality constraint %d", name1.c_str(), id);
src/user/user_objects.h
+86 -17
View File
@@ -35,8 +35,9 @@ class mjCGeom;
class mjCSite;
class mjCCamera;
class mjCLight;
class mjCMesh;
class mjCSkin;
class mjCFlex; // defined in user_mesh
class mjCMesh; // defined in user_mesh
class mjCSkin; // defined in user_mesh
class mjCTexture;
class mjCMaterial;
class mjCPair;
@@ -50,7 +51,6 @@ class mjCNumeric;
class mjCText;
class mjCTuple;
class mjCDef;
class mjCMesh; // defined in user_mesh
class mjCModel; // defined in user_model
class mjXWriter; // defined in xml_native
class mjXURDF; // defined in xml_urdf
@@ -118,10 +118,6 @@ class mjCAlternative {
//------------------------- class mjCBoundingVolumeHierarchy ---------------------------------------
// bounding volume
@@ -132,7 +128,7 @@ class mjCBoundingVolume {
int id; // object id
int contype; // contact type
int conaffinity; // contact affinity
const mjtNum* aabb; // half-sizes of axis-aligned bounding box
const mjtNum* aabb; // axis-aligned bounding box (center, size)
const mjtNum* pos; // position (set by user or Compile1)
const mjtNum* quat; // orientation (set by user or Compile1)
};
@@ -146,11 +142,11 @@ class mjCBoundingVolumeHierarchy {
int nbvh;
std::vector<mjtNum> bvh; // bounding boxes (nbvh x 6)
std::vector<int> child; // children of each node (nbvh x 2)
std::vector<int> nodeid; // id of the geom contained by the node (nbvh x 1)
std::vector<int> nodeid; // geom of elem id contained by the node (nbvh x 1)
std::vector<int> level; // levels of each node (nbvh x 1)
// make bounding volume hierarchy
void CreateBVH();
void CreateBVH(void);
void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]);
void AddBoundingVolume(const mjCBoundingVolume& bv);
@@ -174,7 +170,7 @@ class mjCBase {
public:
// load resource if found (fallback to OS filesystem)
mjResource* LoadResource(std::string filename, const mjVFS* vfs);
static mjResource* LoadResource(std::string filename, const mjVFS* vfs);
// Get and sanitize content type from raw_text if not empty, otherwise parse
// content type from resource_name; throw on failure
@@ -208,6 +204,7 @@ class mjCBody : public mjCBase {
friend class mjCSite;
friend class mjCCamera;
friend class mjCLight;
friend class mjCFlex;
friend class mjCEquality;
friend class mjCPair;
friend class mjCModel;
@@ -266,6 +263,12 @@ class mjCBody : public mjCBase {
int mocapid; // mocap id, -1: not mocap
bool explicitinertial; // whether to save the body with an explicit inertial clause
int contype; // OR over geom contypes
int conaffinity; // OR over geom conaffinities
double margin; // MAX over geom margins
mjtNum xpos0[3]; // global position in qpos0
mjtNum xquat0[4]; // global orientation in qpos0
// used internally by compiler
int lastdof; // id of last dof
int subtreedofs; // number of dofs in subtree, including self
@@ -389,7 +392,7 @@ class mjCGeom : public mjCBase {
double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid
mjCAlternative alt; // alternative orientation specifications
// variables set by user or 'Compile1'
// variables set by user or 'Compile'
double pos[3]; // position
double quat[4]; // orientation
@@ -406,7 +409,7 @@ class mjCGeom : public mjCBase {
double inertia[3]; // local diagonal inertia
double locpos[3]; // local position
double locquat[4]; // local orientation
double aabb[6]; // half-sizes of axis-aligned bounding box
double aabb[6]; // axis-aligned bounding box (center, size)
mjCBody* body; // geom's body
};
@@ -524,10 +527,75 @@ class mjCLight : public mjCBase {
//------------------------- class mjCFlex ----------------------------------------------------------
// Describes a flex
class mjCFlex: public mjCBase {
friend class mjCDef;
friend class mjCModel;
friend class mjCFlexcomp;
friend class mjCEquality;
friend class mjXWriter;
public:
// contact properties
int contype; // contact type
int conaffinity; // contact affinity
int condim; // contact dimensionality
int priority; // contact priority
double friction[3]; // one-sided friction coefficients: slide, roll, spin
double solmix; // solver mixing for contact pairs
mjtNum solref[mjNREF]; // solver reference
mjtNum solimp[mjNIMP]; // solver impedance
double margin; // margin for contact detection
double gap; // include in solver if dist<margin-gap
// other properties
int dim; // element dimensionality
double radius; // radius around primitive element
bool internal; // enable internal collisions
bool flatskin; // render flex skin with flat shading
int selfcollide; // mode for flex self colllision
int activelayers; // number of active element layers in 3D
int group; // group for visualizatioh
double edgestiffness; // edge stiffness
double edgedamping; // edge damping
std::string material; // name of material used for rendering
float rgba[4]; // rgba when material is omitted
std::vector<std::string> vertbody; // vertex body names
std::vector<mjtNum> vert; // vertex positions
std::vector<int> elem; // element vertex ids
std::vector<float> texcoord; // vertex texture coordinates
private:
mjCFlex(mjCModel* = 0); // constructor
void Compile(const mjVFS* vfs); // compiler
void CreateBVH(void); // create flex BVH
void CreateShellPair(void); // create shells and evpairs
int nvert; // number of verices
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)
std::vector<int> vertbodyid; // vertex 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)
std::vector<int> evpair; // element-vertex pairs
std::vector<mjtNum> vertxpos; // global vertex positions
mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy
};
//------------------------- class mjCMesh ----------------------------------------------------------
// Describes a mesh
class mjCMesh: public mjCBase {
friend class mjCFlexcomp;
public:
mjCMesh(mjCModel* = 0, mjCDef* = 0);
~mjCMesh();
@@ -548,7 +616,7 @@ class mjCMesh: public mjCBase {
// mesh properites computed by Compile
const double* boxsz_volume() const { return boxsz_volume_; }
const double* aabb() const { return aabb_; }
const double* aamm() const { return aamm_; }
// number of vertices, normals, texture coordinates, and faces
int nvert() const { return nvert_; }
@@ -652,7 +720,7 @@ class mjCMesh: public mjCBase {
double quat_[4]; // rotation applied to asset vertices
double boxsz_volume_[3]; // half-sizes of equivalent inertia box (volume)
double boxsz_surface_[3]; // half-sizes of equivalent inertia box (surface)
double aabb_[6]; // axis-aligned bounding box
double aamm_[6]; // axis-aligned bounding box in (min, max) format
double volume_; // volume of the mesh
double surface_; // surface of the mesh
@@ -864,7 +932,7 @@ class mjCPair : public mjCBase {
int geom1; // id of geom1
int geom2; // id of geom2
int signature; // (body1+1)<<16 + body2+1
int signature; // body1<<16 + body2
};
@@ -891,7 +959,7 @@ class mjCBodyPair : public mjCBase {
int body1; // id of body1
int body2; // id of body2
int signature; // (body1+1)<<16 + body2+1
int signature; // body1<<16 + body2
};
@@ -1199,6 +1267,7 @@ class mjCDef {
mjCSite site;
mjCCamera camera;
mjCLight light;
mjCFlex flex;
mjCMesh mesh;
mjCMaterial material;
mjCPair pair;
src/xml/xml_base.h
+1
View File
@@ -70,6 +70,7 @@ extern const mjMap bias_map[];
extern const mjMap stage_map[];
extern const mjMap datatype_map[];
extern const mjMap meshtype_map[];
extern const mjMap flexself_map[];
//---------------------------------- Base XML class ------------------------------------------------
src/xml/xml_native_reader.cc
+298 -11
View File
@@ -34,6 +34,7 @@
#include "engine/engine_util_errmem.h"
#include "engine/engine_util_misc.h"
#include "user/user_composite.h"
#include "user/user_flexcomp.h"
#include "user/user_model.h"
#include "user/user_objects.h"
#include "user/user_util.h"
@@ -77,7 +78,7 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjCPlugin* pp) {
//---------------------------------- MJCF schema ---------------------------------------------------
static const int nMJCF = 204;
static const int nMJCF = 223;
static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"mujoco", "!", "1", "model"},
{"<"},
@@ -92,10 +93,10 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"inttotal", "interval", "tolrange"},
{">"},
{"option", "*", "25",
{"option", "*", "26",
"timestep", "apirate", "impratio", "tolerance", "ls_tolerance", "noslip_tolerance",
"mpr_tolerance", "gravity", "wind", "magnetic", "density", "viscosity",
"o_margin", "o_solref", "o_solimp",
"o_margin", "o_solref", "o_solimp", "o_friction",
"integrator", "cone", "jacobian",
"solver", "iterations", "ls_iterations", "noslip_iterations", "mpr_iterations",
"sdf_iterations", "sdf_initpoints"},
@@ -112,8 +113,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"visual", "*", "0"},
{"<"},
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow", "offwidth",
"offheight", "realtime", "ellipsoidinertia"},
{"global", "?", "10", "fovy", "ipd", "azimuth", "elevation", "linewidth", "glow",
"offwidth", "offheight", "realtime", "ellipsoidinertia"},
{"quality", "?", "5", "shadowsize", "offsamples", "numslices", "numstacks",
"numquads"},
{"headlight", "?", "4", "ambient", "diffuse", "specular", "active"},
@@ -292,6 +293,32 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
{"config", "*", "2", "key", "value"},
{">"},
{">"},
{"flexcomp", "*", "26", "name", "class", "type", "dim", "flatskin",
"count", "spacing", "radius", "rigid", "mass", "inertiabox",
"scale", "file", "point", "element", "texcoord", "material", "rgba", "selfcollide",
"flatskin", "pos", "quat", "axisangle", "xyaxes", "zaxis", "euler"},
{"<"},
{"edge", "?", "5", "equality", "solref", "solimp", "stiffness", "damping"},
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap", },
{"pin", "*", "4", "id", "range", "grid", "gridrange"},
{">"},
{">"},
{"deformable", "*", "0"},
{"<"},
{"flex", "*", "12", "name", "group", "dim", "radius", "material", "rgba", "flatskin",
"selfcollide", "body", "vertex", "element", "texcoord"},
{"<"},
{"contact", "?", "10", "contype", "conaffinity", "condim", "priority",
"friction", "solmix", "solref", "solimp", "margin", "gap"},
{"edge", "?", "2", "stiffness", "damping"},
{">"},
{"skin", "*", "9", "name", "file", "material", "rgba", "inflate",
"vertex", "texcoord", "face", "group"},
{"<"},
{"bone", "*", "5", "body", "bindpos", "bindquat", "vertid", "vertweight"},
{">"},
{">"},
{"contact", "*", "0"},
@@ -311,6 +338,8 @@ static const char* MJCF[nMJCF][mjXATTRNUM] = {
"active", "solref", "solimp"},
{"tendon", "*", "8", "name", "class", "tendon1", "tendon2", "polycoef",
"active", "solref", "solimp"},
{"flex", "*", "6", "name", "class", "flex",
"active", "solref", "solimp"},
{">"},
{"tendon", "*", "0"},
@@ -561,12 +590,13 @@ const mjMap solver_map[solver_sz] = {
// constraint type
const int equality_sz = 5;
const int equality_sz = 6;
const mjMap equality_map[equality_sz] = {
{"connect", mjEQ_CONNECT},
{"weld", mjEQ_WELD},
{"joint", mjEQ_JOINT},
{"tendon", mjEQ_TENDON},
{"flex", mjEQ_FLEX},
{"distance", mjEQ_DISTANCE}
};
@@ -654,7 +684,7 @@ const mjMap datatype_map[datatype_sz] = {
// LR mode
const int lrmode_sz = 4;
const mjMap lrmode_map[datatype_sz] = {
const mjMap lrmode_map[lrmode_sz] = {
{"none", mjLRMODE_NONE},
{"muscle", mjLRMODE_MUSCLE},
{"muscleuser", mjLRMODE_MUSCLEUSER},
@@ -702,12 +732,35 @@ const mjMap tkind_map[2] = {
// mesh type
const mjMap meshtype_map[2] = {
const mjMap meshtype_map[2] = {
{"false", mjVOLUME_MESH},
{"true", mjSHELL_MESH},
};
// flexcomp type
const mjMap fcomp_map[mjNFCOMPTYPES] = {
{"grid", mjFCOMPTYPE_GRID},
{"box", mjFCOMPTYPE_BOX},
{"cylinder", mjFCOMPTYPE_CYLINDER},
{"ellipsoid", mjFCOMPTYPE_ELLIPSOID},
{"mesh", mjFCOMPTYPE_MESH},
{"gmsh", mjFCOMPTYPE_GMSH},
{"direct", mjFCOMPTYPE_DIRECT}
};
// flex selfcollide type
const mjMap flexself_map[5] = {
{"none", mjFLEXSELF_NONE},
{"narrow", mjFLEXSELF_NARROW},
{"bvh", mjFLEXSELF_BVH},
{"sap", mjFLEXSELF_SAP},
{"auto", mjFLEXSELF_AUTO},
};
//---------------------------------- class mjXReader implementation --------------------------------
// constructor
@@ -821,6 +874,11 @@ void mjXReader::Parse(XMLElement* root) {
Contact(section);
}
for (XMLElement* section = root->FirstChildElement("deformable"); section;
section = section->NextSiblingElement("deformable")) {
Deformable(section);
}
for (XMLElement* section = root->FirstChildElement("equality"); section;
section = section->NextSiblingElement("equality")) {
Equality(section);
@@ -959,6 +1017,7 @@ void mjXReader::Option(XMLElement* section, mjOption* opt) {
ReadAttr(section, "o_margin", 1, &opt->o_margin, text);
ReadAttr(section, "o_solref", mjNREF, opt->o_solref, text, false, false);
ReadAttr(section, "o_solimp", mjNIMP, opt->o_solimp, text, false, false);
ReadAttr(section, "o_friction", 5, opt->o_friction, text, false, false);
MapValue(section, "integrator", &opt->integrator, integrator_map, integrator_sz);
MapValue(section, "cone", &opt->cone, cone_map, cone_sz);
@@ -1190,6 +1249,68 @@ void mjXReader::Statistic(XMLElement* section) {
//---------------------------------- one-element parsers -------------------------------------------
// flex element parser
void mjXReader::OneFlex(XMLElement* elem, mjCFlex* pflex) {
string text;
int n;
// read attributes
ReadAttrTxt(elem, "name", pflex->name);
ReadAttr(elem, "radius", 1, &pflex->radius, text);
ReadAttrTxt(elem, "material", pflex->material);
ReadAttr(elem, "rgba", 4, pflex->rgba, text);
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
pflex->flatskin = (n==1);
}
ReadAttrInt(elem, "dim", &pflex->dim);
ReadAttrInt(elem, "group", &pflex->group);
// read data vectors
if (ReadAttrTxt(elem, "body", text, true)) {
String2Vector(text, pflex->vertbody);
}
if (ReadAttrTxt(elem, "vertex", text)) {
String2Vector(text, pflex->vert);
}
if (ReadAttrTxt(elem, "element", text, true)) {
String2Vector(text, pflex->elem);
}
if (ReadAttrTxt(elem, "texcoord", text)) {
String2Vector(text, pflex->texcoord);
}
// contact subelement
XMLElement* cont = elem->FirstChildElement("contact");
if (cont) {
ReadAttrInt(cont, "contype", &pflex->contype);
ReadAttrInt(cont, "conaffinity", &pflex->conaffinity);
ReadAttrInt(cont, "condim", &pflex->condim);
ReadAttrInt(cont, "priority", &pflex->priority);
ReadAttr(cont, "friction", 3, pflex->friction, text, false, false);
ReadAttr(cont, "solmix", 1, &pflex->solmix, text);
ReadAttr(cont, "solref", mjNREF, pflex->solref, text, false, false);
ReadAttr(cont, "solimp", mjNIMP, pflex->solimp, text, false, false);
ReadAttr(cont, "margin", 1, &pflex->margin, text);
ReadAttr(cont, "gap", 1, &pflex->gap, text);
if (MapValue(cont, "internal", &n, bool_map, 2)) {
pflex->internal = (n==1);
}
MapValue(cont, "selfcollide", &pflex->selfcollide, flexself_map, 5);
ReadAttrInt(cont, "activelayers", &pflex->activelayers);
}
// edge subelement
XMLElement* edge = elem->FirstChildElement("edge");
if (edge) {
ReadAttr(edge, "stiffness", 1, &pflex->edgestiffness, text);
ReadAttr(edge, "damping", 1, &pflex->edgedamping, text);
}
GetXMLPos(elem, pflex);
}
// mesh element parser
void mjXReader::OneMesh(XMLElement* elem, mjCMesh* pmesh) {
int n;
@@ -1596,6 +1717,10 @@ void mjXReader::OneEquality(XMLElement* elem, mjCEquality* pequality) {
ReadAttr(elem, "polycoef", 5, pequality->data, text);
break;
case mjEQ_FLEX:
ReadAttrTxt(elem, "flex", pequality->name1, true);
break;
case mjEQ_DISTANCE:
throw mjXError(elem, "support for distance equality constraints was removed in MuJoCo 2.2.2");
break;
@@ -2136,6 +2261,124 @@ void mjXReader::OneComposite(XMLElement* elem, mjCBody* pbody, mjCDef* def) {
// make flexcomp
void mjXReader::OneFlexcomp(XMLElement* elem, mjCBody* pbody) {
string text;
int n;
// create out-of-DOM element
mjCFlexcomp fcomp;
// common properties
ReadAttrTxt(elem, "name", fcomp.name, true);
if (MapValue(elem, "type", &n, fcomp_map, mjNFCOMPTYPES)) {
fcomp.type = (mjtFcompType)n;
}
ReadAttr(elem, "count", 3, fcomp.count, text);
ReadAttr(elem, "spacing", 3, fcomp.spacing, text);
ReadAttr(elem, "scale", 3, fcomp.scale, text);
ReadAttr(elem, "mass", 1, &fcomp.mass, text);
ReadAttr(elem, "inertiabox", 1, &fcomp.inertiabox, text);
ReadAttrTxt(elem, "file", fcomp.file);
ReadAttrTxt(elem, "material", fcomp.def.flex.material);
ReadAttr(elem, "rgba", 4, fcomp.def.flex.rgba, text);
if (MapValue(elem, "flatskin", &n, bool_map, 2)) {
fcomp.def.flex.flatskin = (n==1);
}
ReadAttrInt(elem, "dim", &fcomp.def.flex.dim);
ReadAttr(elem, "radius", 1, &fcomp.def.flex.radius, text);
ReadAttrInt(elem, "group", &fcomp.def.flex.group);
// pose
ReadAttr(elem, "pos", 3, fcomp.pos, text);
ReadAttr(elem, "quat", 4, fcomp.quat, text);
ReadAlternative(elem, fcomp.alt);
// user or internal
if (MapValue(elem, "rigid", &n, bool_map, 2)) {
fcomp.rigid = (n==1);
}
if (ReadAttrTxt(elem, "point", text)){
String2Vector(text, fcomp.point);
}
if (ReadAttrTxt(elem, "element", text)){
String2Vector(text, fcomp.element);
}
if (ReadAttrTxt(elem, "texcoord", text)) {
String2Vector(text, fcomp.texcoord);
}
// edge
XMLElement* edge = elem->FirstChildElement("edge");
if (edge) {
if (MapValue(edge, "equality", &n, bool_map, 2)) {
fcomp.equality = (n==1);
}
ReadAttr(edge, "solref", mjNREF, fcomp.def.equality.solref, text, false, false);
ReadAttr(edge, "solimp", mjNIMP, fcomp.def.equality.solimp, text, false, false);
ReadAttr(edge, "stiffness", 1, &fcomp.def.flex.edgestiffness, text);
ReadAttr(edge, "damping", 1, &fcomp.def.flex.edgedamping, text);
}
// contact
XMLElement* cont = elem->FirstChildElement("contact");
if (cont) {
ReadAttrInt(cont, "contype", &fcomp.def.flex.contype);
ReadAttrInt(cont, "conaffinity", &fcomp.def.flex.conaffinity);
ReadAttrInt(cont, "condim", &fcomp.def.flex.condim);
ReadAttrInt(cont, "priority", &fcomp.def.flex.priority);
ReadAttr(cont, "friction", 3, fcomp.def.flex.friction, text, false, false);
ReadAttr(cont, "solmix", 1, &fcomp.def.flex.solmix, text);
ReadAttr(cont, "solref", mjNREF, fcomp.def.flex.solref, text, false, false);
ReadAttr(cont, "solimp", mjNIMP, fcomp.def.flex.solimp, text, false, false);
ReadAttr(cont, "margin", 1, &fcomp.def.flex.margin, text);
ReadAttr(cont, "gap", 1, &fcomp.def.flex.gap, text);
if (MapValue(cont, "internal", &n, bool_map, 2)) {
fcomp.def.flex.internal = (n==1);
}
MapValue(cont, "selfcollide", &fcomp.def.flex.selfcollide, flexself_map, 5);
ReadAttrInt(cont, "activelayers", &fcomp.def.flex.activelayers);
}
// pin
XMLElement* epin = elem->FirstChildElement("pin");
while (epin) {
// accumulate id, coord, range
vector<int> temp;
if (ReadAttrTxt(epin, "id", text)){
String2Vector(text, temp);
fcomp.pinid.insert(fcomp.pinid.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "range", text)){
String2Vector(text, temp);
fcomp.pinrange.insert(fcomp.pinrange.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "grid", text)){
String2Vector(text, temp);
fcomp.pingrid.insert(fcomp.pingrid.end(), temp.begin(), temp.end());
}
if (ReadAttrTxt(epin, "gridrange", text)){
String2Vector(text, temp);
fcomp.pingridrange.insert(fcomp.pingridrange.end(), temp.begin(), temp.end());
}
// advance
epin = epin->NextSiblingElement("pin");
}
// make flexcomp
char error[200];
bool res = fcomp.Make(pbody->model, pbody, error, 200);
// throw error
if (!res) {
throw mjXError(elem, error);
}
}
// add plugin
void mjXReader::OnePlugin(XMLElement* elem, mjCBase* object) {
object->is_plugin = true;
ReadAttrTxt(elem, "plugin", object->plugin_name);
@@ -2623,9 +2866,9 @@ void mjXReader::Asset(XMLElement* section) {
OneMesh(elem, pmesh);
}
// skin sub-element
// skin sub-element... deprecate ???
else if (name=="skin") {
// create mesh and parse
// create skin and parse
mjCSkin* pskin = model->AddSkin();
OneSkin(elem, pskin);
}
@@ -2771,10 +3014,16 @@ void mjXReader::Body(XMLElement* section, mjCBody* pbody) {
// composite sub-element
else if (name=="composite") {
// create composite and parse
// parse composite
OneComposite(elem, pbody, def);
}
// flexcomp sub-element
else if (name=="flexcomp") {
// parse flexcomp
OneFlexcomp(elem, pbody);
}
// body sub-element
else if (name=="body") {
// read childdef
@@ -2888,6 +3137,44 @@ void mjXReader::Equality(XMLElement* section) {
// deformable section parser
void mjXReader::Deformable(XMLElement* section) {
string name;
XMLElement* elem;
// iterate over child elements
elem = section->FirstChildElement();
while (elem) {
// get sub-element name
name = elem->Value();
// get class if specified, otherwise use default0
mjCDef* def = GetClass(elem);
if (!def) {
def = model->defaults[0];
}
// flex sub-element
if (name=="flex") {
// create flex and parse
mjCFlex* pflex = model->AddFlex();
OneFlex(elem, pflex);
}
// skin sub-element
else if (name=="skin") {
// create skin and parse
mjCSkin* pskin = model->AddSkin();
OneSkin(elem, pskin);
}
// advance to next element
elem = elem->NextSiblingElement();
}
}
// tendon section parser
void mjXReader::Tendon(XMLElement* section) {
string text, text1;
src/xml/xml_native_reader.h
+3
View File
@@ -44,6 +44,7 @@ class mjXReader : public mjXBase {
void Asset(tinyxml2::XMLElement* section); // asset section
void Body(tinyxml2::XMLElement* section, mjCBody* pbody); // body/world section
void Contact(tinyxml2::XMLElement* section); // contact section
void Deformable(tinyxml2::XMLElement* section); // deformable section
void Equality(tinyxml2::XMLElement* section); // equality section
void Tendon(tinyxml2::XMLElement* section); // tendon section
void Actuator(tinyxml2::XMLElement* section); // actuator section
@@ -51,6 +52,7 @@ class mjXReader : public mjXBase {
void Keyframe(tinyxml2::XMLElement* section); // keyframe section
// single element parsers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial);
@@ -64,6 +66,7 @@ class mjXReader : public mjXBase {
void OneTendon(tinyxml2::XMLElement* elem, mjCTendon* ptendon);
void OneActuator(tinyxml2::XMLElement* elem, mjCActuator* pactuator);
void OneComposite(tinyxml2::XMLElement* elem, mjCBody* pbody, mjCDef* def);
void OneFlexcomp(tinyxml2::XMLElement* elem, mjCBody* pbody);
void OnePlugin(tinyxml2::XMLElement* elem, mjCBase* object);
mjXSchema schema; // schema used for validation
src/xml/xml_native_writer.cc
+116 -16
View File
@@ -82,6 +82,74 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) {
//---------------------------------- class mjXWriter: one-element writers --------------------------
// write flex
void mjXWriter::OneFlex(XMLElement* elem, mjCFlex* pflex) {
string text;
mjCFlex defflex;
// common attributes
WriteAttrTxt(elem, "name", pflex->name);
WriteAttr(elem, "radius", 1, &pflex->radius, &defflex.radius);
if (pflex->material != defflex.material) {
WriteAttrTxt(elem, "material", pflex->material);
}
WriteAttr(elem, "rgba", 4, pflex->rgba, defflex.rgba);
WriteAttrKey(elem, "flatskin", bool_map, 2, pflex->flatskin, defflex.flatskin);
WriteAttrInt(elem, "dim", pflex->dim, defflex.dim);
WriteAttrInt(elem, "group", pflex->group, defflex.group);
// data vectors
if (!pflex->vertbody.empty()) {
Vector2String(text, pflex->vertbody);
WriteAttrTxt(elem, "body", text);
}
if (!pflex->vert.empty()) {
Vector2String(text, pflex->vert);
WriteAttrTxt(elem, "vertex", text);
}
if (!pflex->elem.empty()) {
Vector2String(text, pflex->elem);
WriteAttrTxt(elem, "element", text);
}
if (!pflex->texcoord.empty()) {
Vector2String(text, pflex->texcoord);
WriteAttrTxt(elem, "texcoord", text);
}
// contact subelement
XMLElement* cont = InsertEnd(elem, "contact");
WriteAttrInt(cont, "contype", pflex->contype, defflex.contype);
WriteAttrInt(cont, "conaffinity", pflex->conaffinity, defflex.conaffinity);
WriteAttrInt(cont, "condim", pflex->condim, defflex.condim);
WriteAttrInt(cont, "priority", pflex->priority, defflex.priority);
WriteAttr(cont, "friction", 3, pflex->friction, defflex.friction);
WriteAttr(cont, "solmix", 1, &pflex->solmix, &defflex.solmix);
WriteAttr(cont, "solref", mjNREF, pflex->solref, defflex.solref);
WriteAttr(cont, "solimp", mjNIMP, pflex->solimp, defflex.solimp);
WriteAttr(cont, "margin", 1, &pflex->margin, &defflex.margin);
WriteAttr(cont, "gap", 1, &pflex->gap, &defflex.gap);
WriteAttrKey(cont, "internal", bool_map, 2, pflex->internal, defflex.internal);
WriteAttrKey(cont, "selfcollide", flexself_map, 5, pflex->selfcollide, defflex.selfcollide);
WriteAttrInt(cont, "activelayers", pflex->activelayers, defflex.activelayers);
// remove contact is no attributes
if (!cont->FirstAttribute()) {
elem->DeleteChild(cont);
}
// edge subelement
XMLElement* edge = InsertEnd(elem, "edge");
WriteAttr(edge, "stiffness", 1, &pflex->edgestiffness, &defflex.edgestiffness);
WriteAttr(edge, "damping", 1, &pflex->edgedamping, &defflex.edgedamping);
// remove edge if no attributes
if (!edge->FirstAttribute()) {
elem->DeleteChild(edge);
}
}
// write mesh
void mjXWriter::OneMesh(XMLElement* elem, mjCMesh* pmesh, mjCDef* def) {
string text;
@@ -221,12 +289,12 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) {
// special handling of limits
bool range_defined = pjoint->range[0]!=0 || pjoint->range[1]!=0;
bool limited_inferred = def->joint.limited==2 && pjoint->limited==range_defined;
bool limited_inferred = def->joint.limited==2 && pjoint->limited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->joint.limited);
}
bool afrange_defined = pjoint->actfrcrange[0]!=0 || pjoint->actfrcrange[1]!=0;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==afrange_defined;
bool aflimited_inferred = def->joint.actfrclimited==2 && pjoint->actfrclimited==(int)afrange_defined;
if (writingdefaults || !aflimited_inferred) {
WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3,
pjoint->actfrclimited, def->joint.actfrclimited);
@@ -515,6 +583,10 @@ void mjXWriter::OneEquality(XMLElement* elem, mjCEquality* peq, mjCDef* def) {
WriteAttr(elem, "polycoef", 5, peq->data);
break;
case mjEQ_FLEX:
WriteAttrTxt(elem, "flex", peq->name1);
break;
default:
mju_error("mjXWriter: unknown equality type.");
}
@@ -540,7 +612,7 @@ void mjXWriter::OneTendon(XMLElement* elem, mjCTendon* pten, mjCDef* def) {
// special handling of limits
bool range_defined = pten->range[0]!=0 || pten->range[1]!=0;
bool limited_inferred = def->tendon.limited==2 && pten->limited==range_defined;
bool limited_inferred = def->tendon.limited==2 && pten->limited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->tendon.limited);
}
@@ -624,17 +696,17 @@ void mjXWriter::OneActuator(XMLElement* elem, mjCActuator* pact, mjCDef* def) {
// special handling of limits
bool range_defined, limited_inferred;
range_defined = pact->ctrlrange[0]!=0 || pact->ctrlrange[1]!=0;
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==range_defined;
limited_inferred = def->actuator.ctrllimited==2 && pact->ctrllimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->actuator.ctrllimited);
}
range_defined = pact->forcerange[0]!=0 || pact->forcerange[1]!=0;
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==range_defined;
limited_inferred = def->actuator.forcelimited==2 && pact->forcelimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->actuator.forcelimited);
}
range_defined = pact->actrange[0]!=0 || pact->actrange[1]!=0;
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==range_defined;
limited_inferred = def->actuator.actlimited==2 && pact->actlimited==(int)range_defined;
if (writingdefaults || !limited_inferred) {
WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->actuator.actlimited);
}
@@ -750,6 +822,7 @@ string mjXWriter::Write(char *error, size_t error_sz) {
Asset(root);
Body(InsertEnd(root, "worldbody"), model->GetWorld());
Contact(root);
Deformable(root);
Equality(root);
Tendon(root);
Actuator(root);
@@ -819,6 +892,7 @@ void mjXWriter::Option(XMLElement* root) {
WriteAttr(section, "o_margin", 1, &model->option.o_margin, &opt.o_margin);
WriteAttr(section, "o_solref", mjNREF, model->option.o_solref, opt.o_solref);
WriteAttr(section, "o_solimp", mjNIMP, model->option.o_solimp, opt.o_solimp);
WriteAttr(section, "o_friction", 5, model->option.o_friction, opt.o_friction);
WriteAttrKey(section, "integrator", integrator_map, integrator_sz,
model->option.integrator, opt.integrator);
@@ -1261,11 +1335,10 @@ void mjXWriter::Asset(XMLElement* root) {
int ntex = model->NumObjects(mjOBJ_TEXTURE);
int nmat = model->NumObjects(mjOBJ_MATERIAL);
int nmesh = model->NumObjects(mjOBJ_MESH);
int nskin = model->NumObjects(mjOBJ_SKIN);
int nhfield = model->NumObjects(mjOBJ_HFIELD);
// return if empty
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0 && nskin==0) {
if (ntex==0 && nmat==0 && nmesh==0 && nhfield==0) {
return;
}
@@ -1346,14 +1419,6 @@ void mjXWriter::Asset(XMLElement* root) {
}
}
// write skins
for (int i=0; i<nskin; i++) {
// create element and write
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
elem = InsertEnd(section, "skin");
OneSkin(elem, pskin);
}
// write hfields
for (int i=0; i<nhfield; i++) {
// create element
@@ -1513,6 +1578,41 @@ void mjXWriter::Equality(XMLElement* root) {
// deformable section
void mjXWriter::Deformable(XMLElement* root) {
XMLElement* elem;
// get sizes
int nflex = model->NumObjects(mjOBJ_FLEX);
int nskin = model->NumObjects(mjOBJ_SKIN);
// return if empty
if (nflex==0 && nskin==0) {
return;
}
// create section
XMLElement* section = InsertEnd(root, "deformable");
// write flexes
for (int i=0; i<nflex; i++) {
// create element and write
mjCFlex* pflex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i);
elem = InsertEnd(section, "flex");
OneFlex(elem, pflex);
}
// write skins
for (int i=0; i<nskin; i++) {
// create element and write
mjCSkin* pskin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i);
elem = InsertEnd(section, "skin");
OneSkin(elem, pskin);
}
}
// tendon section
void mjXWriter::Tendon(XMLElement* root) {
// skip section if empty
src/xml/xml_native_writer.h
+3 -1
View File
@@ -45,13 +45,15 @@ class mjXWriter : public mjXBase {
void Asset(tinyxml2::XMLElement* root); // asset section
void Body(tinyxml2::XMLElement* elem, mjCBody* body); // body/world section
void Contact(tinyxml2::XMLElement* root); // contact section
void Equality(tinyxml2::XMLElement* root); // equality constraint section
void Deformable(tinyxml2::XMLElement* root); // deformable section
void Equality(tinyxml2::XMLElement* root); // equality section
void Tendon(tinyxml2::XMLElement* root); // tendon section
void Actuator(tinyxml2::XMLElement* root); // actuator section
void Sensor(tinyxml2::XMLElement* root); // sensor section
void Keyframe(tinyxml2::XMLElement* root); // keyframe section
// single element writers, used in defaults and main body
void OneFlex(tinyxml2::XMLElement* elem, mjCFlex* pflex);
void OneMesh(tinyxml2::XMLElement* elem, mjCMesh* pmesh, mjCDef* def);
void OneSkin(tinyxml2::XMLElement* elem, mjCSkin* pskin);
void OneMaterial(tinyxml2::XMLElement* elem, mjCMaterial* pmaterial, mjCDef* def);
src/xml/xml_util.cc
+68
View File
@@ -767,6 +767,42 @@ bool mjXUtil::ReadAttrInt(XMLElement* elem, const char* attr, int* data, bool re
// read vector<string> from string
void mjXUtil::String2Vector(const string& txt, vector<string>& vec) {
stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
string word;
strm >> word;
if (strm.fail()) {
break;
} else {
vec.push_back(word);
}
}
}
// read vector<mjtNum> from string
void mjXUtil::String2Vector(const string& txt, vector<double>& vec) {
stringstream strm(txt);
vec.clear();
while (!strm.eof()) {
double num;
strm >> num;
if (strm.fail()) {
break;
} else {
vec.push_back(num);
}
}
}
// read vector<float> from string
void mjXUtil::String2Vector(const string& txt, vector<float>& vec) {
stringstream strm(txt);
@@ -803,6 +839,38 @@ void mjXUtil::String2Vector(const string& txt, vector<int>& vec) {
// write vector<float> to string
void mjXUtil::Vector2String(string& txt, const vector<string>& vec) {
stringstream strm;
for (size_t i=0; i<vec.size(); i++) {
if (i>0) {
strm << " ";
}
strm << vec[i];
}
txt = strm.str();
}
// write vector<double> to string
void mjXUtil::Vector2String(string& txt, const vector<double>& vec) {
stringstream strm;
for (size_t i=0; i<vec.size(); i++) {
if (i>0) {
strm << " ";
}
strm << vec[i];
}
txt = strm.str();
}
// write vector<float> to string
void mjXUtil::Vector2String(string& txt, const vector<float>& vec) {
stringstream strm;
src/xml/xml_util.h
+12
View File
@@ -160,12 +160,24 @@ class mjXUtil {
static bool ReadAttrInt(tinyxml2::XMLElement* elem, const char* attr, int* data,
bool required = false);
// read vector<string> from string
static void String2Vector(const std::string& txt, std::vector<std::string>& vec);
// read vector<double> from string
static void String2Vector(const std::string& txt, std::vector<double>& vec);
// read vector<float> from string
static void String2Vector(const std::string& txt, std::vector<float>& vec);
// read vector<int> from string
static void String2Vector(const std::string& txt, std::vector<int>& vec);
// write vector<string> to string
static void Vector2String(std::string& txt, const std::vector<std::string>& vec);
// write vector<double> to string
static void Vector2String(std::string& txt, const std::vector<double>& vec);
// write vector<float> to string
static void Vector2String(std::string& txt, const std::vector<float>& vec);
test/engine/engine_collision_driver_test.cc
+27 -4
View File
@@ -41,8 +41,8 @@ static std::vector<GeomPair> colliding_pairs(
const mjModel* model, const mjData* data) {
std::vector<GeomPair> result;
for (int i = 0; i < data->ncon; i++) {
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom1);
std::string geom2 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom2);
std::string geom1 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[0]);
std::string geom2 = mj_id2name(model, mjOBJ_GEOM, data->contact[i].geom[1]);
result.push_back(GeomPair(std::min(geom1, geom2), std::max(geom1, geom2)));
}
std::sort(result.begin(), result.end());
@@ -211,14 +211,37 @@ TEST_F(MjCollisionTest, TestOBB) {
mjtNum mat2[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), true);
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), true);
// rotate by 45 degrees
mat2[0] = 1./mju_sqrt(2.); mat2[1] = -1./mju_sqrt(2.);
mat2[3] = 1./mju_sqrt(2.); mat2[4] = 1./mju_sqrt(2.);
EXPECT_THAT(
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, NULL, NULL, 0), false);
mj_collideOBB(bvh1, bvh2, pos1, mat1, pos2, mat2, 0, NULL, NULL, 0), false);
}
TEST_F(MjCollisionTest, PlaneInBody) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom pos="0 0 0" type="plane" size="1 1 .01"/>
</body>
<body pos="0 0 .0499">
<joint type="slide" axis="0 0 1"/>
<geom size=".05"/>
</body>
</worldbody>
</mujoco>
)";
mjModel* m = LoadModelFromString(xml);
ASSERT_THAT(m, NotNull());
mjData* d = mj_makeData(m);
ASSERT_THAT(d, NotNull());
mj_step(m, d);
mj_deleteData(d);
mj_deleteModel(m);
}
} // namespace
test/user/CMakeLists.txt
+3
View File
@@ -18,6 +18,9 @@ target_link_libraries(user_model_test fixture gmock absl::str_format)
mujoco_test(user_objects_test)
target_link_libraries(user_objects_test fixture gmock)
mujoco_test(user_flex_test)
target_link_libraries(user_flex_test fixture gmock)
mujoco_test(user_mesh_test)
target_link_libraries(
user_mesh_test
test/user/testdata/cube_complete.obj
Vendored
+33
View File
@@ -0,0 +1,33 @@
# cube.obj
#
g cube
v 0.0 0.0 0.0
v 0.0 0.0 1.0
v 0.0 1.0 0.0
v 0.0 1.0 1.0
v 1.0 0.0 0.0
v 1.0 0.0 1.0
v 1.0 1.0 0.0
v 1.0 1.0 1.0
vn 0.0 0.0 1.0
vn 0.0 0.0 -1.0
vn 0.0 1.0 0.0
vn 0.0 -1.0 0.0
vn 1.0 0.0 0.0
vn -1.0 0.0 0.0
f 1//2 7//2 5//2
f 1//2 3//2 7//2
f 1//6 4//6 3//6
f 1//6 2//6 4//6
f 3//3 8//3 7//3
f 3//3 4//3 8//3
f 5//5 7//5 8//5
f 5//5 8//5 6//5
f 1//4 5//4 6//4
f 1//4 6//4 2//4
f 2//1 6//1 8//1
f 2//1 8//1 4//1
test/user/testdata/malformed_flex_nofaces.xml
Vendored
+5
View File
@@ -0,0 +1,5 @@
<mujoco>
<worldbody>
<flexcomp name="test" type="mesh" file="cube.obj"/>
</worldbody>
</mujoco>
test/user/testdata/robot_arm.xml
Vendored
+157
View File
@@ -0,0 +1,157 @@
<mujoco>
<default>
<geom contype='1' conaffinity='1' condim='3' friction='.1 .1' solimp='.95 .98 .0005' solref='0.02 1.1' density='1'/>
<joint type='hinge' armature='0' damping='0'
solimpfriction='.95 .95 0' solreffriction='.02 1' solimplimit='0 .99 .01' solreflimit='.02 1'/>
<site rgba='.5 .5 .5 .3'/>
<general ctrllimited='true' ctrlrange='-0.84 0.84' forcelimited='true'/>
<default class='big_joint'>
<joint frictionloss='.01' armature='1' damping='0.01' range='-3.141592653589793 9.42477796076938' limited='false'/>
<velocity kv='20' forcerange='-3.15 3.15'/>
</default>
<default class='little_joint'>
<joint frictionloss='.01' armature='1' damping='0.01' range='-3.141592653589793 9.42477796076938' limited='false'/>
<velocity kv='20' forcerange='-3.15 3.15'/>
</default>
<default class='finger'>
<joint frictionloss='0.1' armature='.1' axis='0 0 1' limited='true' range='0.15 1.2' damping='.1'/>
<velocity kv='10' ctrlrange='-0.84 0.84' gear='1' forcerange='-1.5 1.5'/>
</default>
<default class='fingertip'>
<geom type='box' pos='0.065 -0.022 0' size='0.02 .005 .011' rgba='1 0 0 1'
quat='1 0 0 -0.235' condim='4'/>
<site type='ellipsoid' pos='0.065 -0.022 0' size='0.02 .005 .011'
quat='1 0 0 -0.235'/>
</default>
<default class='base'>
<geom type='capsule' size='0.0350512 0.0555543' pos='-0.000132925 -0.000516418 0.0820911' quat='0.6951 -0.00714564 0.00738544 0.71884'/>
</default>
<default class='link_1'>
<geom type='capsule' size='0.031417 0.0668352' pos='-3.49946e-05 0.010651 -0.0670012' quat='0.704277 0.0627825 -0.0628 0.70435'/>
</default>
<default class='link_2'>
<geom type='capsule' size='0.0251592 0.240341' pos='0.205004 7.5835e-05 -0.0202812' quat='2.23957e-06 0.707108 5.30416e-06 0.707106'/>
</default>
<default class='link_3'>
<geom type='capsule' size='0.0255647 0.120643' pos='0.0843942 7.48907e-06 -0.0177123' quat='-4.64475e-05 0.692702 -4.11814e-05 0.721224'/>
</default>
<default class='link_4'>
<geom type='capsule' size='0.0257405 0.0289568' pos='0.0101659 -4.64261e-05 -0.0369867' quat='0.965925 1.43712e-05 -0.258823 3.85082e-06'/>
</default>
<default class='link_5'>
<geom type='capsule' size='0.0257405 0.0289568' pos='0.0101659 -4.64261e-05 -0.0369867' quat='0.965925 1.43712e-05 -0.258823 3.85082e-06'/>
</default>
<default class='link_6'>
<geom type='capsule' size='0.0368731 0.0322296' pos='0.00628384 -2.92087e-05 -0.0608681' quat='0.708562 -0.0338601 -0.0358744 0.703923'/>
</default>
<default class='finger_knuckle'>
<geom type='capsule' size='0.0108231 0.0145116' pos='0.0190913 -0.0112192 1.10387e-06' quat='0.475811 -0.475815 0.523071 -0.523068'/>
</default>
</default>
<worldbody>
<body name='b_base' pos='0 0 0.005'>
<site type='sphere' size='.01' name='basesite' pos='0 0 0' rgba='1 0 0 1'/>
<inertial pos='-0.000132925 -0.000516418 0.0820911' quat='0.6951 -0.00714556 0.00738537 0.71884' mass='0.00718278' diaginertia='0.00157301 0.00157269 0.00058831'/>
<geom name='base' class='base' rgba='0 0.4470 0.7410 1'/>
<body name='b_1' pos='0 0 0.1535' quat='0 -1 0 0'>
<inertial pos='-3.49946e-005 0.010651 -0.0670012' quat='0.704277 0.0627825 -0.0628 0.70435' mass='0.00644357' diaginertia='0.00172311 0.00163259 0.00042883'/>
<joint name='joint_1' class='big_joint' axis='0 0 1'/>
<geom name='link_1' class='link_1' rgba='0.8500 0.3250 0.0980 1'/>
<geom name='ring_1' contype='0' conaffinity='0' type='cylinder' size='0.044 0.011' pos='0 0 0' rgba='0.7 0.7 0.72 1'/>
<body name='b_2' pos='0 0 -0.1185' quat='-0.7071067811865475 0.7071067811865475 0.7071067811865475 0.7071067811865475'>
<inertial pos='0.205004 7.5835e-005 -0.0202812' quat='2.24027e-006 0.707108 5.30493e-006 0.707106' mass='0.011691' diaginertia='0.0253426 0.0250512 0.000536357'/>
<joint name='joint_2' class='big_joint' axis='0 0 1'/>
<geom name='link_2' class='link_2' rgba='0.9290 0.6940 0.1250 1'/>
<geom name='ring_2' contype='0' conaffinity='0' type='cylinder' size='0.044 0.011' pos='0 0 0' euler='0 0 0' rgba='0.7 0.7 0.72 1'/>
<body name='b_3' pos='0.41 0 0' quat='0 0 1 0'>
<inertial pos='0.0843942 7.48909e-006 -0.0177123' quat='-4.64469e-005 0.692702 -4.11798e-005 0.721224' mass='0.00673417' diaginertia='0.00420255 0.00409328 0.000303584'/>
<joint name='joint_3' class='big_joint' axis='0 0 1' range='0.6131110819452792 5.673924357222234' limited='true'/>
<geom name='link_3' class='link_3' rgba='0.4940 0.1840 0.5560 1'/>
<geom name='ring_3' contype='0' conaffinity='0' type='cylinder' size='0.044 0.011' pos='0 0 0' euler='0 0 0' rgba='0.7 0.7 0.72 1'/>
<body name='b_4' pos='0.207 0 -0.01125' quat='-0.7071067811865475 0 0.7071067811865475 0'>
<inertial pos='0.0101659 -4.64261e-005 -0.0369867' quat='0.965925 1.91737e-005 -0.258823 -1.40769e-005' mass='0.00221423' diaginertia='0.0001816 0.000174586 9.79319e-005'/>
<joint name='joint_4' class='little_joint' axis='0 0 1'/>
<geom name='link_4' class='link_4' rgba='0.4660 0.6740 0.1880 1'/>
<geom name='ring_4' contype='0' conaffinity='0' type='cylinder' size='0.035 0.009' pos='0 0 0' euler='0 0 0' rgba='0.7 0.7 0.7 1'/>
<body name='b_5' pos='0.037 0 -0.06408' quat='0 0.5 0 -0.8660254037844386'>
<inertial pos='0.0101659 -4.64261e-005 -0.0369867' quat='0.965925 1.91737e-005 -0.258823 -1.40769e-005' mass='0.00221423' diaginertia='0.0001816 0.000174586 9.79319e-005'/>
<joint name='joint_5' class='little_joint' axis='0 0 1'/>
<geom name='ring_5' contype='0' conaffinity='0' type='cylinder' size='0.035 0.009' pos='0 0 0' euler='0 0 0' rgba='0.7 0.7 0.72 1'/>
<geom name='link_5' class='link_5' rgba='0.3010 0.7450 0.9330 1'/>
<body name='b_hand' pos='0.037 0 -0.06408' quat='0 -0.5 0 0.8660254037844386'>
<camera name='cam_hand' pos='0 .015 -.13' fovy='135'/>
<site type='sphere' size='.01' name='wristsite' pos='0 0 0' rgba='1 0 0 1'/>
<site type='sphere' size='.01' name='gripsite' pos='0 0 -.16' rgba='.5 .5 .5 .3'/>
<site type='sphere' size='.01' name='pinchsite' pos='0 0.015 -0.195' rgba='.5 .5 .5 .3'/>
<inertial pos='0.00628384 -2.92087e-005 -0.0608681' quat='0.708562 -0.0338601 -0.0358744 0.703923' mass='0.00547172' diaginertia='0.000759818 0.000676099 0.0004995'/>
<joint name='joint_6' class='little_joint' axis='0 0 1'/>
<geom name='link_6' class='link_6' rgba='0.6350 0.0780 0.1840 1'/>
<geom name='ring_hand' contype='0' conaffinity='0' type='cylinder' size='0.035 0.009' pos='0 0 0' euler='0 0 0' rgba='0.7 0.7 0.72 1'/>
<body name = 'b_palm' pos='0.005 0 -0.11' euler='0 0 0'>
<geom type='cylinder' contype='0' conaffinity='7' size='.0275 0.01' friction='1 1' rgba='0.7 0.7 0.7 1' name='palm'/>
</body>
<body name='b_finger_1' childclass='finger' pos='-0.029 .003 -0.1145' quat='-0.414818 -0.329751 -0.663854 0.52772'>
<inertial pos='0.0485761 -0.000715511 0' quat='0.507589 0.507348 0.492543 0.492294' mass='0.000379077' diaginertia='4.00708e-005 4.00527e-005 2.156e-006'/>
<joint name='joint_finger_1'/>
<geom name='finger_knuckle_1' class='finger_knuckle' rgba='0 0.4470 0.7410 1'/>
<geom class='fingertip' name='finger_tip_1' rgba='0 0.4470 0.7410 1'/>
<site class='fingertip' name='fingertip1'/>
</body>
<body name='b_finger_2' childclass='finger' pos='0.0295 0.0216 -0.115' quat='0.561254 -0.620653 0.321748 0.443014'>
<inertial pos='0.0485761 -0.000715511 0' quat='0.507589 0.507348 0.492543 0.492294' mass='0.000379077' diaginertia='4.00708e-005 4.00527e-005 2.156e-006'/>
<joint name='joint_finger_2'/>
<geom name='finger_knuckle_2' class='finger_knuckle' rgba='0.9290 0.6940 0.1250 1'/>
<geom class='fingertip' name='finger_tip_2' rgba='0.9290 0.6940 0.1250 1'/>
<site class='fingertip' name='fingertip2'/>
</body>
<body name='b_finger_3' childclass='finger' pos='0.0295 -0.0216 -0.1145' quat='0.625248 -0.567602 0.434845 0.312735'>
<inertial pos='0.0485761 -0.000715511 0' quat='0.507589 0.507348 0.492543 0.492294' mass='0.000379077' diaginertia='4.00708e-005 4.00527e-005 2.156e-006'/>
<joint name='joint_finger_3'/>
<geom name='finger_knuckle_3' class='finger_knuckle' rgba='0.8500 0.3250 0.0980 1'/>
<geom class='fingertip' name='finger_tip_3' rgba='0.8500 0.3250 0.0980 1'/>
<site class='fingertip' name='fingertip3'/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</worldbody>
<contact>
<exclude body1='b_base' body2='b_1'/>
<exclude body1='b_finger_1' body2='b_finger_2'/>
<exclude body1='b_finger_1' body2='b_finger_3'/>
<exclude body1='b_finger_2' body2='b_finger_3'/>
</contact>
<sensor>
<touch name="fingertip1" site='fingertip1'/>
<touch name="fingertip2" site='fingertip2'/>
<touch name="fingertip3" site='fingertip3'/>
<force name="wristforce" site='wristsite'/>
<torque name="wristtorque" site='wristsite'/>
<force name="baseforce" site='basesite'/>
<torque name="basetorque" site='basesite'/>
</sensor>
<actuator>
<velocity joint='joint_1' class='big_joint'/>
<velocity joint='joint_2' class='big_joint'/>
<velocity joint='joint_3' class='big_joint'/>
<velocity joint='joint_4' class='little_joint'/>
<velocity joint='joint_5' class='little_joint'/>
<velocity joint='joint_6' class='little_joint'/>
<velocity joint='joint_finger_1' class='finger'/>
<velocity joint='joint_finger_2' class='finger'/>
<velocity joint='joint_finger_3' class='finger'/>
</actuator>
</mujoco>
test/user/user_flex_test.cc
+231
View File
@@ -0,0 +1,231 @@
// Copyright 2021 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.
// Tests for user/user_model.cc.
#include <array>
#include <string>
#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <mujoco/mjmodel.h>
#include <mujoco/mujoco.h>
#include "test/fixture.h"
namespace mujoco {
namespace {
using ::testing::IsNull;
using ::testing::NotNull;
using ::testing::HasSubstr;
using UserFlexTest = MujocoTest;
TEST_F(UserFlexTest, ParentMustHaveName) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'name'"));
}
TEST_F(UserFlexTest, InvalidDim) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" dim="4"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("Invalid dim, must be between 1 and 3"));
}
TEST_F(UserFlexTest, CountTooSmall) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" count="2 2 0"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull());
EXPECT_THAT(error.data(), HasSubstr("Count too small"));
}
TEST_F(UserFlexTest, SpacingGreaterThanGeometry) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" spacing="0.5 0.5 0.5" radius="1"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("Spacing must be larger than geometry size"));
}
TEST_F(UserFlexTest, ScaleMinValue) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" scale="0 1 1"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Scale must be larger than mjMINVAL"));
}
TEST_F(UserFlexTest, MassMinValue) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" mass="0"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("Mass and inertiabox must be larger than mjMINVAL"));
}
TEST_F(UserFlexTest, PointSizeNotMultipleOf3) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" point="0 0 0 0"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Point size must be a multiple of 3"));
}
TEST_F(UserFlexTest, ElementSize) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" point="0 0 0" element="0 1 2" dim="3"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("Element size must be a multiple of dim+1"));
}
TEST_F(UserFlexTest, PointAndElementNotInDirect) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="direct"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Point and element required"));
}
TEST_F(UserFlexTest, UknownFlexCompType) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="unknown"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("invalid keyword: 'unknown'"));
}
TEST_F(UserFlexTest, InvalidPinid) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="direct" point="0 0 0" element="0 1 2">
<pin id="1"/>
</flexcomp>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(),
HasSubstr("element 1 has point id 1, number of points is 1"));
}
TEST_F(UserFlexTest, MeshFileMissing) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<flexcomp name="test" type="mesh"/>
</worldbody>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* m = LoadModelFromString(xml, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("File is required"));
}
TEST_F(UserFlexTest, VertexOrFaceDataMissing) {
const std::string xml_path =
GetTestDataFilePath("user/testdata/malformed_flex_nofaces.xml");
std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
EXPECT_THAT(m, IsNull()) << error.data();
EXPECT_THAT(error.data(), HasSubstr("Vertex and face data required"));
}
TEST_F(UserFlexTest, CreateBVHSuccess) {
const std::string xml_path =
GetTestDataFilePath("user/testdata/robot_arm.xml");
std::array<char, 1024> error;
mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
mjData* d = mj_makeData(m);
EXPECT_THAT(m, NotNull()) << error.data();
mj_step(m, d);
mj_deleteModel(m);
mj_deleteData(d);
}
} // namespace
} // namespace mujoco
test/xml/xml_native_reader_test.cc
+1 -1
View File
@@ -631,7 +631,7 @@ TEST_F(XMLReaderTest, InvalidSkinGroup) {
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(model, IsNull());
EXPECT_THAT(
error.data(),
HasSubstr("skin group must be between 0 and 5\nElement 'skin', line 7"));
test/xml/xml_native_writer_test.cc
+1
View File
@@ -1062,6 +1062,7 @@ TEST_F(PluginTest, WriteReadCompare) {
// if file is meant to fail, skip it
if (absl::StrContains(p.path().string(), "malformed_") ||
absl::StrContains(p.path().string(), "touch_grid") ||
absl::StrContains(p.path().string(), "gmsh_") ||
absl::StrContains(p.path().string(), "cow")) {
continue;
}
unity/Runtime/Bindings/MjBindings.cs
+176 -40
View File
@@ -191,9 +191,10 @@ public enum mjtGeom : int{
mjGEOM_ARROW1 = 101,
mjGEOM_ARROW2 = 102,
mjGEOM_LINE = 103,
mjGEOM_SKIN = 104,
mjGEOM_LABEL = 105,
mjGEOM_TRIANGLE = 106,
mjGEOM_FLEX = 104,
mjGEOM_SKIN = 105,
mjGEOM_LABEL = 106,
mjGEOM_TRIANGLE = 107,
mjGEOM_NONE = 1001,
}
public enum mjtCamLight : int{
@@ -233,7 +234,8 @@ public enum mjtEq : int{
mjEQ_WELD = 1,
mjEQ_JOINT = 2,
mjEQ_TENDON = 3,
mjEQ_DISTANCE = 4,
mjEQ_FLEX = 4,
mjEQ_DISTANCE = 5,
}
public enum mjtWrap : int{
mjWRAP_NONE = 0,
@@ -282,22 +284,23 @@ public enum mjtObj : int{
mjOBJ_SITE = 6,
mjOBJ_CAMERA = 7,
mjOBJ_LIGHT = 8,
mjOBJ_MESH = 9,
mjOBJ_SKIN = 10,
mjOBJ_HFIELD = 11,
mjOBJ_TEXTURE = 12,
mjOBJ_MATERIAL = 13,
mjOBJ_PAIR = 14,
mjOBJ_EXCLUDE = 15,
mjOBJ_EQUALITY = 16,
mjOBJ_TENDON = 17,
mjOBJ_ACTUATOR = 18,
mjOBJ_SENSOR = 19,
mjOBJ_NUMERIC = 20,
mjOBJ_TEXT = 21,
mjOBJ_TUPLE = 22,
mjOBJ_KEY = 23,
mjOBJ_PLUGIN = 24,
mjOBJ_FLEX = 9,
mjOBJ_MESH = 10,
mjOBJ_SKIN = 11,
mjOBJ_HFIELD = 12,
mjOBJ_TEXTURE = 13,
mjOBJ_MATERIAL = 14,
mjOBJ_PAIR = 15,
mjOBJ_EXCLUDE = 16,
mjOBJ_EQUALITY = 17,
mjOBJ_TENDON = 18,
mjOBJ_ACTUATOR = 19,
mjOBJ_SENSOR = 20,
mjOBJ_NUMERIC = 21,
mjOBJ_TEXT = 22,
mjOBJ_TUPLE = 23,
mjOBJ_KEY = 24,
mjOBJ_PLUGIN = 25,
}
public enum mjtConstraint : int{
mjCNSTR_EQUALITY = 0,
@@ -376,6 +379,13 @@ public enum mjtLRMode : int{
mjLRMODE_MUSCLEUSER = 2,
mjLRMODE_ALL = 3,
}
public enum mjtFlexSelf : int{
mjFLEXSELF_NONE = 0,
mjFLEXSELF_NARROW = 1,
mjFLEXSELF_BVH = 2,
mjFLEXSELF_SAP = 3,
mjFLEXSELF_AUTO = 4,
}
public enum mjtPluginCapabilityBit : int{
mjPLUGIN_ACTUATOR = 1,
mjPLUGIN_SENSOR = 2,
@@ -467,13 +477,14 @@ public enum mjtLabel : int{
mjLABEL_TENDON = 7,
mjLABEL_ACTUATOR = 8,
mjLABEL_CONSTRAINT = 9,
mjLABEL_SKIN = 10,
mjLABEL_SELECTION = 11,
mjLABEL_SELPNT = 12,
mjLABEL_CONTACTPOINT = 13,
mjLABEL_CONTACTFORCE = 14,
mjLABEL_ISLAND = 15,
mjNLABEL = 16,
mjLABEL_FLEX = 10,
mjLABEL_SKIN = 11,
mjLABEL_SELECTION = 12,
mjLABEL_SELPNT = 13,
mjLABEL_CONTACTPOINT = 14,
mjLABEL_CONTACTFORCE = 15,
mjLABEL_ISLAND = 16,
mjNLABEL = 17,
}
public enum mjtFrame : int{
mjFRAME_NONE = 0,
@@ -511,10 +522,15 @@ public enum mjtVisFlag : int{
mjVIS_SELECT = 21,
mjVIS_STATIC = 22,
mjVIS_SKIN = 23,
mjVIS_MIDPHASE = 24,
mjVIS_MESHBVH = 25,
mjVIS_SDFITER = 26,
mjNVISFLAG = 27,
mjVIS_FLEXVERT = 24,
mjVIS_FLEXEDGE = 25,
mjVIS_FLEXFACE = 26,
mjVIS_FLEXSKIN = 27,
mjVIS_BODYBVH = 28,
mjVIS_FLEXBVH = 29,
mjVIS_MESHBVH = 30,
mjVIS_SDFITER = 31,
mjNVISFLAG = 32,
}
public enum mjtRndFlag : int{
mjRND_SHADOW = 0,
@@ -553,6 +569,10 @@ public unsafe struct mjContact_ {
public int dim;
public int geom1;
public int geom2;
public fixed int geom[2];
public fixed int flex[2];
public fixed int elem[2];
public fixed int vert[2];
public int exclude;
public int efc_address;
}
@@ -4748,10 +4768,6 @@ public unsafe struct mjData_ {
public fixed int solver_niter[20];
public fixed int solver_nnz[20];
public fixed double solver_fwdinv[2];
public int nbodypair_broad;
public int nbodypair_narrow;
public int ngeompair_mid;
public int ngeompair_narrow;
public int ne;
public int nf;
public int nl;
@@ -4798,13 +4814,20 @@ public unsafe struct mjData_ {
public double* subtree_com;
public double* cdof;
public double* cinert;
public double* flexvert_xpos;
public double* flexelem_aabb;
public int* flexedge_J_rownnz;
public int* flexedge_J_rowadr;
public int* flexedge_J_colind;
public double* flexedge_J;
public double* flexedge_length;
public int* ten_wrapadr;
public int* ten_wrapnum;
public int* ten_J_rownnz;
public int* ten_J_rowadr;
public int* ten_J_colind;
public double* ten_length;
public double* ten_J;
public double* ten_length;
public int* wrap_obj;
public double* wrap_xpos;
public double* actuator_length;
@@ -4814,7 +4837,9 @@ public unsafe struct mjData_ {
public double* qLD;
public double* qLDiagInv;
public double* qLDiagSqrtInv;
public double* bvh_aabb_dyn;
public byte* bvh_active;
public double* flexedge_velocity;
public double* ten_velocity;
public double* actuator_velocity;
public double* cvel;
@@ -4924,6 +4949,7 @@ public unsafe struct mjOption_ {
public double o_margin;
public fixed double o_solref[2];
public fixed double o_solimp[5];
public fixed double o_friction[5];
public int integrator;
public int cone;
public int jacobian;
@@ -5059,11 +5085,21 @@ public unsafe struct mjModel_ {
public int na;
public int nbody;
public int nbvh;
public int nbvhstatic;
public int nbvhdynamic;
public int njnt;
public int ngeom;
public int nsite;
public int ncam;
public int nlight;
public int nflex;
public int nflexvert;
public int nflexedge;
public int nflexelem;
public int nflexelemdata;
public int nflexshelldata;
public int nflexevpair;
public int nflextexcoord;
public int nmesh;
public int nmeshvert;
public int nmeshnormal;
@@ -5148,13 +5184,16 @@ public unsafe struct mjModel_ {
public double* body_inertia;
public double* body_invweight0;
public double* body_gravcomp;
public double* body_margin;
public double* body_user;
public int* body_plugin;
public int* body_contype;
public int* body_conaffinity;
public int* body_bvhadr;
public int* body_bvhnum;
public int* bvh_depth;
public int* bvh_child;
public int* bvh_geomid;
public int* bvh_nodeid;
public double* bvh_aabb;
public int* jnt_type;
public int* jnt_qposadr;
@@ -5251,6 +5290,55 @@ public unsafe struct mjModel_ {
public float* light_ambient;
public float* light_diffuse;
public float* light_specular;
public int* flex_contype;
public int* flex_conaffinity;
public int* flex_condim;
public int* flex_priority;
public double* flex_solmix;
public double* flex_solref;
public double* flex_solimp;
public double* flex_friction;
public double* flex_margin;
public double* flex_gap;
public byte* flex_internal;
public int* flex_selfcollide;
public int* flex_activelayers;
public int* flex_dim;
public int* flex_matid;
public int* flex_group;
public int* flex_vertadr;
public int* flex_vertnum;
public int* flex_edgeadr;
public int* flex_edgenum;
public int* flex_elemadr;
public int* flex_elemnum;
public int* flex_elemdataadr;
public int* flex_shellnum;
public int* flex_shelldataadr;
public int* flex_evpairadr;
public int* flex_evpairnum;
public int* flex_texcoordadr;
public int* flex_vertbodyid;
public int* flex_edge;
public int* flex_elem;
public int* flex_elemlayer;
public int* flex_shell;
public int* flex_evpair;
public double* flex_vert;
public double* flex_xvert0;
public double* flexedge_length0;
public double* flexedge_invweight0;
public double* flex_radius;
public double* flex_edgestiffness;
public double* flex_edgedamping;
public byte* flex_edgeequality;
public byte* flex_rigid;
public byte* flex_centered;
public byte* flex_flatskin;
public int* flex_bvhadr;
public int* flex_bvhnum;
public float* flex_rgba;
public float* flex_texcoord;
public int* mesh_vertadr;
public int* mesh_vertnum;
public int* mesh_faceadr;
@@ -5419,6 +5507,7 @@ public unsafe struct mjModel_ {
public int* name_siteadr;
public int* name_camadr;
public int* name_lightadr;
public int* name_flexadr;
public int* name_meshadr;
public int* name_skinadr;
public int* name_hfieldadr;
@@ -5685,6 +5774,7 @@ public unsafe struct mjuiDef_ {
[StructLayout(LayoutKind.Sequential)]
public unsafe struct mjvPerturb_ {
public int select;
public int flexselect;
public int skinselect;
public int active;
public int active2;
@@ -5770,9 +5860,11 @@ public unsafe struct mjvOption_ {
public fixed byte jointgroup[6];
public fixed byte tendongroup[6];
public fixed byte actuatorgroup[6];
public fixed byte flexgroup[6];
public fixed byte skingroup[6];
public fixed byte flags[27];
public fixed byte flags[32];
public int bvh_depth;
public int flex_layer;
}
[StructLayout(LayoutKind.Sequential)]
@@ -5781,6 +5873,23 @@ public unsafe struct mjvScene_ {
public int ngeom;
public mjvGeom_* geoms;
public int* geomorder;
public int nflex;
public int* flexedgeadr;
public int* flexedgenum;
public int* flexvertadr;
public int* flexvertnum;
public int* flexfaceadr;
public int* flexfacenum;
public int* flexfaceused;
public int* flexedge;
public float* flexvert;
public float* flexface;
public float* flexnormal;
public float* flextexcoord;
public byte flexvertopt;
public byte flexedgeopt;
public byte flexfaceopt;
public byte flexskinopt;
public int nskin;
public int* skinfacenum;
public int* skinvertadr;
@@ -5944,6 +6053,7 @@ public unsafe struct model {
public int na;
public int nbody;
public int nbvh;
public int nbvhstatic;
public int njnt;
public int ngeom;
public int nsite;
@@ -5951,6 +6061,8 @@ public unsafe struct model {
public int nlight;
public int nmesh;
public int nskin;
public int nflex;
public int nflexvert;
public int nskinvert;
public int nskinface;
public int nskinbone;
@@ -5984,7 +6096,7 @@ public unsafe struct model {
public int* body_bvhnum;
public int* bvh_depth;
public int* bvh_child;
public int* bvh_geomid;
public int* bvh_nodeid;
public double* bvh_aabb;
public int* jnt_type;
public int* jnt_bodyid;
@@ -6018,6 +6130,23 @@ public unsafe struct model {
public float* light_ambient;
public float* light_diffuse;
public float* light_specular;
public byte* flex_flatskin;
public int* flex_dim;
public int* flex_matid;
public int* flex_group;
public int* flex_vertadr;
public int* flex_vertnum;
public int* flex_elem;
public int* flex_elemadr;
public int* flex_elemnum;
public int* flex_elemdataadr;
public int* flex_shell;
public int* flex_shellnum;
public int* flex_shelldataadr;
public int* flex_bvhadr;
public int* flex_bvhnum;
public double* flex_radius;
public float* flex_rgba;
public int* mesh_bvhadr;
public int* mesh_bvhnum;
public int* mesh_texcoordadr;
@@ -6139,6 +6268,7 @@ public unsafe struct data {
public int* dof_island;
public int* efc_island;
public int* tendon_efcadr;
public double* flexvert_xpos;
public mjContact_* contact;
public double* efc_force;
}
@@ -6359,6 +6489,9 @@ public static unsafe extern void mj_comPos(mjModel_* m, mjData_* d);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_camlight(mjModel_* m, mjData_* d);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_flex(mjModel_* m, mjData_* d);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mj_tendon(mjModel_* m, mjData_* d);
@@ -6536,6 +6669,9 @@ public static unsafe extern double mj_rayMesh(mjModel_* m, mjData_* d, int geomi
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern double mju_rayGeom(double* pos, double* mat, double* size, double* pnt, double* vec, int geomtype);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern double mju_rayFlex(mjModel_* m, mjData_* d, int flex_layer, byte flg_vert, byte flg_edge, byte flg_face, byte flg_skin, int flexid, double* pnt, double* vec, int* vertid);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern double mju_raySkin(int nface, int nvert, int* face, float* vert, double* pnt, double* vec, int* vertid);
@@ -6594,7 +6730,7 @@ public static unsafe extern void mjv_applyPerturbForce(mjModel_* m, mjData_* d,
public static unsafe extern mjvGLCamera_* mjv_averageCamera(mjvGLCamera_* cam1, mjvGLCamera_* cam2);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern int mjv_select(mjModel_* m, mjData_* d, mjvOption_* vopt, double aspectratio, double relx, double rely, mjvScene_* scn, double* selpnt, int* geomid, int* skinid);
public static unsafe extern int mjv_select(mjModel_* m, mjData_* d, mjvOption_* vopt, double aspectratio, double relx, double rely, mjvScene_* scn, double* selpnt, int* geomid, int* flexid, int* skinid);
[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)]
public static unsafe extern void mjv_defaultOption(mjvOption_* opt);