diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 07012a15..9cc05a09 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -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 diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b5a075f8..b64782f2 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -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 `__ (formerly known as MIME types) of the + `Media Type `_ (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`. 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 `. 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`. 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 ` 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** (*) diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index b301da97..85d5c7b4 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -222,13 +222,13 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`wind` | :ref:`magnetic` | :ref:`density` | :ref:`viscosity` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`o_margin` | :ref:`o_solref` | :ref:`o_solimp` | :ref:`integrator` | | +| | | | :ref:`o_margin` | :ref:`o_solref` | :ref:`o_solimp` | :ref:`o_friction` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | :ref:`iterations` | | +| | | | :ref:`integrator` | :ref:`cone` | :ref:`jacobian` | :ref:`solver` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`mpr_iterations` | :ref:`sdf_iterations` | | +| | | | :ref:`iterations` | :ref:`ls_iterations` | :ref:`noslip_iterations` | :ref:`mpr_iterations` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`sdf_initpoints` | | | | | +| | | | :ref:`sdf_iterations` | :ref:`sdf_initpoints` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| option |br| |_| |L| | | .. table:: | @@ -481,6 +481,104 @@ | | | | :ref:`key` | :ref:`value` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| body |br| |_| |L| | | .. table:: | +| :ref:`flexcomp | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`class` | :ref:`type` | :ref:`dim` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`flatskin` | :ref:`count` | :ref:`spacing` | :ref:`radius` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`rigid` | :ref:`mass` | :ref:`inertiabox` | :ref:`scale` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`file` | :ref:`point` | :ref:`element` | :ref:`texcoord` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`material` | :ref:`rgba` | :ref:`selfcollide` | :ref:`flatskin` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`pos` | :ref:`quat` | :ref:`axisangle` | :ref:`xyaxes` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`zaxis` | :ref:`euler` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flexcomp |br| |_2| |L| | | .. table:: | +| :ref:`edge | ? | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`equality` | :ref:`solref` | :ref:`solimp` | :ref:`stiffness` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`damping` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flexcomp |br| |_2| |L| | | .. table:: | +| :ref:`contact | ? | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`contype` | :ref:`conaffinity` | :ref:`condim` | :ref:`priority` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`friction` | :ref:`solmix` | :ref:`solref` | :ref:`solimp` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`margin` | :ref:`gap` | :ref:`` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flexcomp |br| |_2| |L| | | .. table:: | +| :ref:`pin | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`id` | :ref:`range` | :ref:`grid` | :ref:`gridrange` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| mujoco |br| |L| | | *no attributes* | +| :ref:`deformable` | | | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| deformable |br| |_| |L| | | .. table:: | +| :ref:`flex | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`group` | :ref:`dim` | :ref:`radius` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`material` | :ref:`rgba` | :ref:`flatskin` | :ref:`selfcollide` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body` | :ref:`vertex` | :ref:`element` | :ref:`texcoord` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flex |br| |_2| |L| | | .. table:: | +| :ref:`contact | ? | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`contype` | :ref:`conaffinity` | :ref:`condim` | :ref:`priority` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`friction` | :ref:`solmix` | :ref:`solref` | :ref:`solimp` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`margin` | :ref:`gap` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| flex |br| |_2| |L| | | .. table:: | +| :ref:`edge | ? | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`stiffness` | :ref:`damping` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| deformable |br| |_| |L| | | .. table:: | +| :ref:`skin | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`file` | :ref:`material` | :ref:`rgba` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`inflate` | :ref:`vertex` | :ref:`texcoord` | :ref:`face` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`group` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_2| skin |br| |_2| |L| | | .. table:: | +| :ref:`bone | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`body` | :ref:`bindpos` | :ref:`bindquat` | :ref:`vertid` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`vertweight` | | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | *no attributes* | | :ref:`contact` | | | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ @@ -543,6 +641,15 @@ | | | | :ref:`polycoef` | :ref:`active` | :ref:`solref` | :ref:`solimp` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ +| |_| equality |br| |_| |L| | | .. table:: | +| :ref:`flex | \* | :class: mjcf-attributes | +| ` | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`name` | :ref:`class` | :ref:`flex` | :ref:`active` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`solref` | :ref:`solimp` | | | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | ++------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | mujoco |br| |L| | | *no attributes* | | :ref:`tendon` | | | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ diff --git a/doc/changelog.rst b/doc/changelog.rst index 16a3d108..c1371156 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -179,8 +179,8 @@ General :ref:`Cartesian actuator` 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 `__ (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 `_ (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` and :ref:`integration` (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 diff --git a/doc/includes/references.h b/doc/includes/references.h index 09e29799..1ad876c8 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -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(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); Def(pymodule); Def(pymodule); @@ -581,6 +582,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); // Interaction diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index 85af8ad2..436ba53f 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -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(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); diff --git a/python/mujoco/structs.h b/python/mujoco/structs.h index c49681ab..bb581e9b 100644 --- a/python/mujoco/structs.h +++ b/python/mujoco/structs.h @@ -508,6 +508,10 @@ class MjWrapper : public WrapperBase { X(solreffriction); X(solimp); X(H); + X(geom); + X(flex); + X(elem); + X(vert); #undef X }; @@ -733,6 +737,7 @@ class MjWrapper : public WrapperBase { X(jointgroup); X(tendongroup); X(actuatorgroup); + X(flexgroup); X(skingroup); X(flags); #undef X @@ -754,10 +759,23 @@ class MjWrapper : public WrapperBase { ~MjWrapper() = default; int nskinvert; + int nflexface, nflexedge, nflexvert; #define X(dtype, var) py_array_or_tuple_t 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); diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 1a70c62b..c64b8698 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -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(contacts[0])/nstep); std::printf(" Constraints per step : %.2f\n", static_cast(constraints[0])/nstep); std::printf(" Degrees of freedom : %d\n\n", m->nv); diff --git a/simulate/simulate.cc b/simulate/simulate.cc index 9713cf1d..5a64019f 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -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(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); } diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 5167baa2..a9c47376 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -16,6 +16,7 @@ #include #include +#include #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; diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index eeb92091..158d2eea 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -15,6 +15,7 @@ #include "engine/engine_collision_convex.h" #include +#include #include #include @@ -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; r2) { + // prism height test + if (prism.v[3][2]=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; +} diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 6eed5f3d..3b9b797f 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -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); diff --git a/src/engine/engine_collision_driver.c b/src/engine/engine_collision_driver.c index 7e42f406..b6cf04af 100644 --- a/src/engine/engine_collision_driver.c +++ b/src/engine/engine_collision_driver.c @@ -36,6 +36,7 @@ #include "engine/engine_util_solve.h" #include "engine/engine_util_spatial.h" + // table of pair-wise collision functions mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { /* PLANE HFIELD SPHERE CAPSULE ELLIPSOID CYLINDER BOX MESH SDF */ @@ -52,12 +53,7 @@ mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES] = { -//------------------------------------ static functions -------------------------------------------- - -// test two geoms for collision, apply filters, add to contact list -// flg_user disables filters and uses usermargin -static void collideGeoms(const mjModel* m, mjData* d, - int g1, int g2, int flg_user, mjtNum usermargin); +//------------------------------------ utility functions ------------------------------------------ // move arena pointer back to the end of the contact array static inline void resetArena(mjData* d) { @@ -70,8 +66,10 @@ static inline void resetArena(mjData* d) { #endif } + + // plane to geom_center squared distance, g1 is a plane -static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) { +static mjtNum planeGeomDist(const mjModel* m, mjData* d, int g1, int g2) { mjtNum* mat1 = d->geom_xmat + 9*g1; mjtNum norm[3] = {mat1[2], mat1[5], mat1[8]}; mjtNum dif[3]; @@ -80,39 +78,432 @@ static mjtNum plane_geom(const mjModel* m, mjData* d, int g1, int g2) { return mju_dot3(dif, norm); } -// squared Euclidean distance between 3D vectors -static inline mjtNum squaredDist3(const mjtNum pos1[3], const mjtNum pos2[3]) { - mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; - return dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2]; -} -// bounding-sphere collision -static int collideSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) { - // neither geom is a plane - if (m->geom_rbound[g1] > 0 && m->geom_rbound[g2] > 0) { - mjtNum bound = m->geom_rbound[g1] + m->geom_rbound[g2] + margin; - if (squaredDist3(d->geom_xpos+3*g1, d->geom_xpos+3*g2) > bound*bound) { - return 0; + +// return 1 if body has plane geom, 0 otherwise +static int hasPlane(const mjModel* m, int body) { + int start = m->body_geomadr[body]; + int end = m->body_geomadr[body] + m->body_geomnum[body]; + + // scan geoms belonging to body + int g; + for (g=start; g < end; g++) { + if (m->geom_type[g] == mjGEOM_PLANE) { + return 1; } } - // one geom is a plane - if (m->geom_type[g1] == mjGEOM_PLANE && m->geom_rbound[g2] > 0 - && plane_geom(m, d, g1, g2) > margin+m->geom_rbound[g2]) { - return 0; - } - if (m->geom_type[g2] == mjGEOM_PLANE && m->geom_rbound[g1] > 0 - && plane_geom(m, d, g2, g1) > margin+m->geom_rbound[g1]) { - return 0; - } - return 1; + return 0; } + +// filter contact based on type and affinity +static int filterBitmask(int contype1, int conaffinity1, + int contype2, int conaffinity2) { + return !(contype1 & conaffinity2) && !(contype2 & conaffinity1); +} + + + +// filter contact based on global AABBs +static int filterBox(const mjtNum aabb1[6], const mjtNum aabb2[6], mjtNum margin) { + if (aabb1[0]+aabb1[3]+margin < aabb2[0]-aabb2[3]) return 1; + if (aabb1[1]+aabb1[4]+margin < aabb2[1]-aabb2[4]) return 1; + if (aabb1[2]+aabb1[5]+margin < aabb2[2]-aabb2[5]) return 1; + if (aabb2[0]+aabb2[3]+margin < aabb1[0]-aabb1[3]) return 1; + if (aabb2[1]+aabb2[4]+margin < aabb1[1]-aabb1[4]) return 1; + if (aabb2[2]+aabb2[5]+margin < aabb1[2]-aabb1[5]) return 1; + return 0; +} + + + +// filter contact based sphere-box test, treating sphere as box +static int filterSphereBox(const mjtNum s[3], mjtNum bound, const mjtNum aabb[6]) { + if (s[0]+bound < aabb[0]-aabb[3]) return 1; + if (s[1]+bound < aabb[1]-aabb[4]) return 1; + if (s[2]+bound < aabb[2]-aabb[5]) return 1; + if (s[0]-bound > aabb[0]+aabb[3]) return 1; + if (s[1]-bound > aabb[1]+aabb[4]) return 1; + if (s[2]-bound > aabb[2]+aabb[5]) return 1; + return 0; +} + + + +// filter contact based on bounding sphere test (raw) +static int filterSphere(const mjtNum pos1[3], const mjtNum pos2[3], mjtNum bound) { + mjtNum dif[3] = {pos1[0]-pos2[0], pos1[1]-pos2[1], pos1[2]-pos2[2]}; + mjtNum distsqr = dif[0]*dif[0] + dif[1]*dif[1] + dif[2]*dif[2]; + + return (distsqr > bound*bound); +} + + + +// filter contact based on bounding sphere test +static int mj_filterSphere(const mjModel* m, mjData* d, int g1, int g2, mjtNum margin) { + // neither geom is a plane + if (m->geom_rbound[g1]>0 && m->geom_rbound[g2]>0) { + return filterSphere(d->geom_xpos + 3*g1, d->geom_xpos + 3*g2, + m->geom_rbound[g1] + m->geom_rbound[g2] + margin); + } + + // one geom is a plane + if (m->geom_type[g1]==mjGEOM_PLANE && m->geom_rbound[g2]>0 + && planeGeomDist(m, d, g1, g2) > margin + m->geom_rbound[g2]) { + return 1; + } + if (m->geom_type[g2]==mjGEOM_PLANE && m->geom_rbound[g1]>0 + && planeGeomDist(m, d, g2, g1) > margin + m->geom_rbound[g1]) { + return 1; + } + return 0; +} + + + +// filter body pair: 1- discard, 0- proceed +static int filterBodyPair(int weldbody1, int weldparent1, int weldbody2, + int weldparent2, int dsbl_filterparent) { + // same weldbody check + if (weldbody1 == weldbody2) { + return 1; + } + + // weldparent check + if ((!dsbl_filterparent && weldbody1 != 0 && weldbody2 != 0) && + (weldbody1 == weldparent2 || weldbody2 == weldparent1)) { + return 1; + } + + // all tests passed + return 0; +} + + + +// return 1 if bodyflex can collide, 0 otherwise +static int canCollide(const mjModel* m, int bf) { + if (bfnbody) { + return (m->body_contype[bf] || m->body_conaffinity[bf]); + } else { + int f = bf - m->nbody; + return (m->flex_contype[f] || m->flex_conaffinity[f]); + } +} + + + +// return 1 if two bodyflexes can collide, 0 otherwise +static int canCollide2(const mjModel* m, int bf1, int bf2) { + int nbody = m->nbody; + int contype1 = (bf1body_contype[bf1] : m->flex_contype[bf1-nbody]; + int conaffinity1 = (bf1body_conaffinity[bf1] : m->flex_conaffinity[bf1-nbody]; + int contype2 = (bf2body_contype[bf2] : m->flex_contype[bf2-nbody]; + int conaffinity2 = (bf2body_conaffinity[bf2] : m->flex_conaffinity[bf2-nbody]; + + // opposite of bitmask filter + return (!filterBitmask(contype1, conaffinity1, contype2, conaffinity2)); +} + + + +// return 1 if element is active, 0 otherwise +int mj_isElemActive(const mjModel* m, int f, int e) { + if (m->flex_dim[f]<3) { + return 1; + } else { + return (m->flex_elemlayer[m->flex_elemadr[f]+e] < m->flex_activelayers[f]); + } +} + + + +//----------------------------- collision detection entry point ------------------------------------ + +// compare contact pairs by their geom/elem/vert IDs +quicksortfunc(contactcompare, context, el1, el2) { + const mjModel* m = (const mjModel*) context; + mjContact* con1 = (mjContact*)el1; + mjContact* con2 = (mjContact*)el2; + + // get colliding object ids + int con1_obj1 = con1->geom[0]>=0 ? con1->geom[0] : (con1->elem[0]>=0 ? con1->elem[0] : con1->vert[0]); + int con1_obj2 = con1->geom[1]>=0 ? con1->geom[1] : (con1->elem[1]>=0 ? con1->elem[1] : con1->vert[1]); + int con2_obj1 = con2->geom[0]>=0 ? con2->geom[0] : (con2->elem[0]>=0 ? con2->elem[0] : con2->vert[0]); + int con2_obj2 = con2->geom[1]>=0 ? con2->geom[1] : (con2->elem[1]>=0 ? con2->elem[1] : con2->vert[1]); + + // for geom:geom, reproduce the order of contacts without mj_collideTree + // normally sorted by (g1, g2), but in mj_collideGeoms, g1 and g2 are swapped based on geom_type + // here we undo this swapping for the purpose of sorting - needs to be done for each mjContact + if (con1->geom[0]>=0 && con1->geom[1] && con2->geom[0]>=0 && con2->geom[1]) { + if (m->geom_type[con1_obj1] > m->geom_type[con1_obj2]) { + int tmp = con1_obj1; + con1_obj1 = con1_obj2; + con1_obj2 = tmp; + } + if (m->geom_type[con2_obj1] > m->geom_type[con2_obj2]) { + int tmp = con2_obj1; + con2_obj1 = con2_obj2; + con2_obj2 = tmp; + } + } + if (con1_obj1 < con2_obj1) return -1; + if (con1_obj1 > con2_obj1) return 1; + if (con1_obj2 < con2_obj2) return -1; + if (con1_obj2 > con2_obj2) return 1; + return 0; +} + + + +// main collision function +void mj_collision(const mjModel* m, mjData* d) { + TM_START1; + + int nexclude = m->nexclude, npair = m->npair, nbody = m->nbody; + int nbodyflex = m->nbody + m->nflex; + + // reset the size of the contact array and invalidate efc arrays + d->ncon = 0; + resetArena(d); + mj_clearEfc(d); + + // reset the visualization flags + memset(d->bvh_active, 0, m->nbvh); + + // return if disabled + if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) + || m->nconmax == 0 || nbodyflex < 2) { + return; + } + + mj_markStack(d); + + // broadphase collision detector + TM_START; + int nmaxpairs = (nbodyflex*(nbodyflex - 1))/2; + int* broadphasepair = mj_stackAllocInt(d, nmaxpairs); + int nbfpair = mj_broadphase(m, d, broadphasepair, nmaxpairs); + unsigned int last_signature = -1; + TM_END(mjTIMER_COL_BROAD); + + // midphase collision detector + TM_RESTART; + + // save current narrowphase duration + mjtNum tmNarrow = d->timer[mjTIMER_COL_NARROW].duration; + + // process bodyflex pairs returned by broadphase, merge with predefined geom pairs + int pairadr = 0; + for (int i=0; i>16) & 0xFFFF; + int bf2 = broadphasepair[i] & 0xFFFF; + + // compute signature for this bodyflex pair + unsigned int signature = (bf1<<16) + bf2; + // pairs come sorted by signature, but may not be unique + // if signature is repeated, skip it + if (signature == last_signature) { + continue; + } + last_signature = signature; + + // merge predefined geom pairs + int merged = 0; + int startadr = pairadr; + if (npair) { + // test all predefined pairs for which pair_signature<=signature + while (pairadrpair_signature[pairadr]<=signature) { + if (m->pair_signature[pairadr]==signature) { + merged = 1; + } + mj_collideGeoms(m, d, pairadr++, -1); + } + } + + // apply bitmask filtering at the bodyflex level + if (!canCollide2(m, bf1, bf2)) { + continue; + } + + // handle body pair exclusion + int exadr = 0; + if (nexclude) { + // advance exadr while exclude_signature < signature + while (exadrexclude_signature[exadr]exclude_signature[exadr]==signature) { + continue; + } + } + + // get bodyflex info + int isbody1 = (bf1body_bvhadr[bf1] : m->flex_bvhadr[bf1-nbody]); + int bvh2 = (isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[bf2-nbody]); + + // process bodyflex pair: two single-geom bodies + if (isbody1 && isbody2 && m->body_geomnum[bf1]==1 && m->body_geomnum[bf2]==1) { + mj_collideGeomPair(m, d, m->body_geomadr[bf1], m->body_geomadr[bf2], + merged, startadr, pairadr); + } + + // process bodyflex pair: midphase + else if (!mjDISABLED(mjDSBL_MIDPHASE) && bvh1>=0 && bvh2>=0) { + int ncon_before = d->ncon; + mj_collideTree(m, d, bf1, bf2, merged, startadr, pairadr); + int ncon_after = d->ncon; + + // sort contacts + mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before, + sizeof(mjContact), contactcompare, (void*) m); + } + + // process bodyflex pair: all-to-all + else { + int geomadr_end1 = m->body_geomadr[bf1] + m->body_geomnum[bf1]; + int geomadr_end2 = m->body_geomadr[bf2] + m->body_geomnum[bf2]; + + // body : body + if (isbody1 && isbody2) { + for (int g1=m->body_geomadr[bf1]; g1body_geomadr[bf2]; g2body_geomadr[bf1]; ggeom_contype[g], m->geom_conaffinity[g], + m->flex_contype[f], m->flex_conaffinity[f])) { + continue; + } + + // plane special processing + if (m->geom_type[g]==mjGEOM_PLANE) { + mj_collidePlaneFlex(m, d, g, f); + continue; + } + + // collide geom with flex elements + for (int e=0; eflex_elemnum[f]; e++) { + mj_collideGeomElem(m, d, g, f, e); + } + } + } + + // flex : flex + else { + int f1 = bf1 - nbody; + int f2 = bf2 - nbody; + + // collide elements of two flexes + for (int e1=0; e1flex_elemnum[f1]; e1++) { + for (int e2=0; e2flex_elemnum[f2]; e2++) { + mj_collideElems(m, d, f1, e1, f2, e2); + } + } + } + } + } + + // finish merging predefined geom pairs + if (npair) { + while (pairadrnflex; f++) { + if (!m->flex_rigid[f] && (m->flex_contype[f] & m->flex_conaffinity[f])) { + // internal collisions + if (m->flex_internal[f]) { + mj_collideFlexInternal(m, d, f); + } + + // active element collisions + if (m->flex_selfcollide[f]!=mjFLEXSELF_NONE) { + // element-element: midphase + if (!mjDISABLED(mjDSBL_MIDPHASE) && + m->flex_selfcollide[f]!=mjFLEXSELF_NARROW && + m->flex_bvhadr[f]>=0) { + // select midphase mode + if (m->flex_selfcollide[f]==mjFLEXSELF_BVH || + (m->flex_selfcollide[f]==mjFLEXSELF_AUTO && m->flex_dim[f]==3)) { + mj_collideTree(m, d, nbody+f, nbody+f, 0, 0, 0); + } else { + mj_collideFlexSAP(m, d, f); + } + } + + // element-element: direct + else { + int flex_elemnum = m->flex_elemnum[f]; + for (int e1=0; e1timer[mjTIMER_COL_MID].duration -= (d->timer[mjTIMER_COL_NARROW].duration - tmNarrow); + + // increment narrowphase counter + d->timer[mjTIMER_COL_NARROW].number++; + + mj_freeStack(d); + TM_END1(mjTIMER_POS_COLLISION); +} + + + //------------------------------------ binary tree search ------------------------------------------ +// collision tree node +struct mjCollisionTree_ { + int node1; + int node2; +}; +typedef struct mjCollisionTree_ mjCollisionTree; + + + +// collision tree allocation +static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) { + return (mjCollisionTree*) mj_stackAllocByte( + d, max_stack * sizeof(mjCollisionTree), _Alignof(mjCollisionTree)); +} + + + // checks if the proposed collision pair is already present in pair_geom and calls narrow phase -static void collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged, +void mj_collideGeomPair(const mjModel* m, mjData* d, int g1, int g2, int merged, int startadr, int pairadr) { // merged: make sure geom pair is not repeated if (merged) { @@ -128,20 +519,22 @@ static void collidePair(const mjModel* m, mjData* d, int g1, int g2, int merged, // not found: test if (!found) { - collideGeoms(m, d, g1, g2, 0, 0); + mj_collideGeoms(m, d, g1, g2); } } // not merged: always test else { - collideGeoms(m, d, g1, g2, 0, 0); + mj_collideGeoms(m, d, g1, g2); } } + + // oriented bounding boxes collision (see Gottschalk et al.) 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) { // get infinite dimensions (planes only) mjtByte inf1[3] = {aabb1[3] >= mjMAXVAL, aabb1[4] >= mjMAXVAL, aabb1[5] >= mjMAXVAL}; @@ -163,8 +556,15 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], if (product == NULL) { for (int i=0; i < 2; i++) { // bounding boxes for (int j=0; j < 3; j++) { // axes - mju_rotVecMat(xcenter[i], aabb[i], xmat[i]); - mju_addTo3(xcenter[i], xpos[i]); + if (xmat[i]) { + mju_rotVecMat(xcenter[i], aabb[i], xmat[i]); + } else { + mju_copy3(xcenter[i], aabb[i]); + } + + if (xpos[i]) { + mju_addTo3(xcenter[i], xpos[i]); + } } } } @@ -173,7 +573,11 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], for (int i=0; i < 2; i++) { // bounding boxes for (int j=0; j < 3; j++) { // faces for (int k=0; k < 3; k++) { // world axes - normal[i][j][k] = xmat[i][3*k+j]; + if (xmat[i]) { + normal[i][j][k] = xmat[i][3*k+j]; + } else { + normal[i][j][k] = (j==k); + } } } } @@ -186,7 +590,7 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], for (int l=0; l < 3; l++) { // axes product[18*i + 9*j + 3*k + l] = mju_dot3(normal[i][l], normal[j][k]); } - offset[6*i + 3*j + k] = mju_dot3(xpos[i], normal[j][k]); + offset[6*i + 3*j + k] = xpos[i] ? mju_dot3(xpos[i], normal[j][k]) : 0; } } } @@ -217,7 +621,7 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], } } - if (radius[0]+radius[1] < fabs(proj[1]-proj[0])) { + if (radius[0]+radius[1]+margin < fabs(proj[1]-proj[0])) { return 0; } } @@ -226,64 +630,164 @@ int mj_collideOBB(const mjtNum aabb1[6], const mjtNum aabb2[6], return 1; } -static mjCollisionTree* mj_stackAllocTree(mjData* d, int max_stack) { - return (mjCollisionTree*) mj_stackAllocByte( - d, max_stack * sizeof(mjCollisionTree), _Alignof(mjCollisionTree)); -} -// binary search between two body trees -static void collideTree(const mjModel* m, mjData* d, int b1, int b2, - int merged, int startadr, int pairadr) { - const int bvhadr1 = m->body_bvhadr[b1]; - const int bvhadr2 = m->body_bvhadr[b2]; - const mjtNum* bvh1 = m->bvh_aabb + 6 * bvhadr1; - const mjtNum* bvh2 = m->bvh_aabb + 6 * bvhadr2; - const int* child1 = m->bvh_child + 2 * bvhadr1; - const int* child2 = m->bvh_child + 2 * bvhadr2; + +// binary search between two bodyflex trees +void mj_collideTree(const mjModel* m, mjData* d, int bf1, int bf2, + int merged, int startadr, int pairadr) { + int nbody = m->nbody, nbvhstatic = m->nbvhstatic; + mjtByte isbody1 = (bf1 < nbody); + mjtByte isbody2 = (bf2 < nbody); + int f1 = isbody1 ? -1 : bf1 - nbody; + int f2 = isbody2 ? -1 : bf2 - nbody; + const int bvhadr1 = isbody1 ? m->body_bvhadr[bf1] : m->flex_bvhadr[f1]; + const int bvhadr2 = isbody2 ? m->body_bvhadr[bf2] : m->flex_bvhadr[f2]; + const int* child1 = m->bvh_child + 2*bvhadr1; + const int* child2 = m->bvh_child + 2*bvhadr2; + const mjtNum* bvh1 = isbody1 ? (m->bvh_aabb + 6*bvhadr1) : + (d->bvh_aabb_dyn + 6*(bvhadr1 - nbvhstatic)); + const mjtNum* bvh2 = isbody2 ? (m->bvh_aabb + 6*bvhadr2) : + (d->bvh_aabb_dyn + 6*(bvhadr2 - nbvhstatic)); + + // used with rotated bounding boxes (when bodies are involved) mjtNum product[36]; // 2 bb x 2 bb x 3 axes (body) x 3 axes (world) mjtNum offset[12]; // 2 bb x 2 bb x 3 axes (world) mjtByte initialize = 1; + // bitmask filter for bodyflex pair + if (!canCollide2(m, bf1, bf2)) { + return; + } + mj_markStack(d); // TODO(b/273737633): Store bvh max depths to make this bound tighter. - const int max_stack = m->body_bvhnum[b1] + m->body_bvhnum[b2]; + const int max_stack = (isbody1 ? m->body_bvhnum[bf1] : m->flex_bvhnum[f1]) + + (isbody2 ? m->body_bvhnum[bf2] : m->flex_bvhnum[f2]); mjCollisionTree* stack = mj_stackAllocTree(d, max_stack); int nstack = 1; stack[0].node1 = stack[0].node2 = 0; + // for body:flex, if body has planes, call mj_collidePlaneFlex directly + if (isbody1 && !isbody2 && m->body_weldid[bf1]==0) { + for (int i=m->body_geomadr[bf1]; ibody_geomadr[bf1]+m->body_geomnum[bf1]; i++) { + if (m->geom_type[i]==mjGEOM_PLANE) { + mj_collidePlaneFlex(m, d, i, f2); + } + } + } + + // collide trees while (nstack) { // pop from stack nstack--; int node1 = stack[nstack].node1; int node2 = stack[nstack].node2; - mjtByte isleaf1 = (child1[2*node1] == -1) && (child1[2*node1+1] == -1); - mjtByte isleaf2 = (child2[2*node2] == -1) && (child2[2*node2+1] == -1); - int nodeid1 = m->bvh_geomid[bvhadr1 + node1]; - int nodeid2 = m->bvh_geomid[bvhadr2 + node2]; + mjtByte isleaf1 = (child1[2*node1]<0) && (child1[2*node1+1]<0); + mjtByte isleaf2 = (child2[2*node2]<0) && (child2[2*node2+1]<0); + int nodeid1 = m->bvh_nodeid[bvhadr1 + node1]; + int nodeid2 = m->bvh_nodeid[bvhadr2 + node2]; - // both are leaves - if (isleaf1 && isleaf2 && nodeid1 != -1 && nodeid2 != -1) { - if (collideSphere(m, d, nodeid1, nodeid2, m->geom_margin[nodeid1] + - m->geom_margin[nodeid2])) { - if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, - d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, - d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2, - NULL, NULL, &initialize)) { - collidePair(m, d, nodeid1, nodeid2, merged, startadr, pairadr); - d->bvh_active[node1 + bvhadr1] = 1; - d->bvh_active[node2 + bvhadr2] = 1; - } - } + // SHOULD NOT OCCUR + if ((isleaf1 && nodeid1<0) || (isleaf2 && nodeid2<0)) { + mju_error("BVH leaf has invalid node id"); + } + + // self-collision: avoid repeated pairs + if (bf1==bf2 && node1>node2) { continue; } - // if no intersection at intermediate levels, stop - if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2, - d->xipos + 3*b1, d->ximat + 9*b1, - d->xipos + 3*b2, d->ximat + 9*b2, - product, offset, &initialize)) { - continue; + // body : body + if (isbody1 && isbody2) { + // both are leaves + if (isleaf1 && isleaf2) { + mjtNum margin = mj_assignMargin(m, + mju_max(m->geom_margin[nodeid1], m->geom_margin[nodeid2])); + + if (!mj_filterSphere(m, d, nodeid1, nodeid2, margin)) { + if (mj_collideOBB(m->geom_aabb + 6*nodeid1, m->geom_aabb + 6*nodeid2, + d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, + d->geom_xpos + 3*nodeid2, d->geom_xmat + 9*nodeid2, + margin, NULL, NULL, &initialize)) { + mj_collideGeomPair(m, d, nodeid1, nodeid2, merged, startadr, pairadr); + d->bvh_active[node1 + bvhadr1] = 1; + d->bvh_active[node2 + bvhadr2] = 1; + } + } + continue; + } + + // if no intersection at intermediate levels, stop + mjtNum margin = mj_assignMargin(m, + mju_max(m->body_margin[bf1], m->body_margin[bf2])); + if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2, + d->xipos + 3*bf1, d->ximat + 9*bf1, + d->xipos + 3*bf2, d->ximat + 9*bf2, + margin, product, offset, &initialize)) { + continue; + } + } + + // body : flex + else if (isbody1 && !isbody2) { + // both are leaves + if (isleaf1 && isleaf2) { + mjtNum margin = mj_assignMargin(m, + mju_max(m->geom_margin[nodeid1], m->flex_margin[f2])); + + if (!filterBitmask(m->geom_contype[nodeid1], m->geom_conaffinity[nodeid1], + m->flex_contype[f2], m->flex_conaffinity[f2]) && + !filterSphereBox(d->geom_xpos + 3*nodeid1, m->geom_rbound[nodeid1] + margin, + bvh2 + 6*node2)) { + if (mj_collideOBB(m->geom_aabb + 6*nodeid1, bvh2 + 6*node2, + d->geom_xpos + 3*nodeid1, d->geom_xmat + 9*nodeid1, + NULL, NULL, + margin, NULL, NULL, &initialize)) { + // collide unless geom is plane (plane:flex handled separately) + if (m->geom_type[nodeid1]!=mjGEOM_PLANE) { + mj_collideGeomElem(m, d, nodeid1, f2, nodeid2); + } + d->bvh_active[node1 + bvhadr1] = 1; + d->bvh_active[node2 + bvhadr2] = 1; + } + } + continue; + } + + // if no intersection at intermediate levels, stop + mjtNum margin = mj_assignMargin(m, + mju_max(m->body_margin[bf1], m->flex_margin[f2])); + if (!mj_collideOBB(bvh1 + 6*node1, bvh2 + 6*node2, + d->xipos + 3*bf1, d->ximat + 9*bf1, + NULL, NULL, + margin, product, offset, &initialize)) { + continue; + } + } + + // flex : body SHOULD NOT OCCUR + else if (!isbody1 && isbody2) { + mjERROR("BVH flex : body collision should not occur"); + } + + // flex : flex + else { + // both are leaves + // box filter applied in mj_collideElems, bitmask filter applied earlier + if (isleaf1 && isleaf2) { + mj_collideElems(m, d, f1, nodeid1, f2, nodeid2); + d->bvh_active[node1 + bvhadr1] = 1; + d->bvh_active[node2 + bvhadr2] = 1; + continue; + } + + // if no intersection at intermediate levels, stop + mjtNum margin = mj_assignMargin(m, + mju_max(m->flex_margin[f1], m->flex_margin[f2])); + if (filterBox(bvh1 + 6*node1, bvh2 + 6*node2, margin)) { + continue; + } } d->bvh_active[node1 + bvhadr1] = 1; @@ -353,261 +857,117 @@ static void collideTree(const mjModel* m, mjData* d, int b1, int b2, } -//----------------------------- collision detection entry point ------------------------------------ - -// compare contact pairs by their geom IDs -quicksortfunc(contactcompare, context, el1, el2) { - const mjModel* m = (const mjModel*) context; - mjContact* con1 = (mjContact*)el1; - mjContact* con2 = (mjContact*)el2; - - // reproduce the order contacts without mj_collideTree - // normally sorted by (g1, g2), but in collideGeoms, g1 and g2 are swapped based on geom_type. - // here we undo this swapping for the purpose of sorting - needs to be done for each mjContact - - int con1_g1 = con1->geom1; - int con1_g2 = con1->geom2; - if (m->geom_type[con1_g1] > m->geom_type[con1_g2]) { - int tmp = con1_g1; - con1_g1 = con1_g2; - con1_g2 = tmp; - } - int con2_g1 = con2->geom1; - int con2_g2 = con2->geom2; - if (m->geom_type[con2_g1] > m->geom_type[con2_g2]) { - int tmp = con2_g1; - con2_g1 = con2_g2; - con2_g2 = tmp; - } - - if (con1_g1 < con2_g1) return -1; - if (con1_g1 > con2_g1) return 1; - if (con1_g2 < con2_g2) return -1; - if (con1_g2 > con2_g2) return 1; - return 0; -} - -void mj_collision(const mjModel* m, mjData* d) { - TM_START1; - int g1, g2, merged, b1 = 0, b2 = 0, exadr = 0, pairadr = 0, startadr; - int nexclude = m->nexclude, npair = m->npair; - int *broadphasepair = 0; - - // reset the size of the contact array and invalidate efc arrays - d->ncon = 0; - resetArena(d); - mj_clearEfc(d); - - // reset diagnostics - d->nbodypair_broad = 0; - d->nbodypair_narrow = 0; - d->ngeompair_mid = 0; - d->ngeompair_narrow = 0; - - // reset the visualization flags - memset(d->bvh_active, 0, m->nbvh); - - // return if disabled - if (mjDISABLED(mjDSBL_CONSTRAINT) || mjDISABLED(mjDSBL_CONTACT) - || m->nconmax == 0 || m->nbody < 2) { - return; - } - - mj_markStack(d); - - // broadphase collision detector - TM_START; - int nbodypair = (m->nbody*(m->nbody - 1))/2; - broadphasepair = mj_stackAllocInt(d, nbodypair); - nbodypair = mj_broadphase(m, d, broadphasepair, nbodypair); - unsigned int last_signature = -1; - TM_END(mjTIMER_COL_BROAD); - - // midphase collision detector - TM_RESTART; - - // save current narrowphase duration - mjtNum tmNarrow = d->timer[mjTIMER_COL_NARROW].duration; - - // loop over body pairs - for (int i=0; i < nbodypair; i++) { - // reconstruct body pair ids - b1 = (broadphasepair[i]>>16) & 0xFFFF; - b2 = broadphasepair[i] & 0xFFFF; - - // compute signature for this body pair - unsigned int signature = ((b1+1)<<16) + (b2+1); - // pairs come sorted by signature, but may not be unique - // if signature is repeated, skip it - if (signature == last_signature) { - continue; - } - last_signature = signature; - - // merge predefined pairs - merged = 0; - startadr = pairadr; - if (npair) { - // test all predefined pairs for which pair_signature<=signature - while (pairadr < npair && m->pair_signature[pairadr] <= signature) { - if (m->pair_signature[pairadr] == signature) { - merged = 1; - } - collideGeoms(m, d, pairadr++, -1, 0, 0); - } - } - - // handle exclusion - if (nexclude) { - // advance exadr while exclude_signature < signature - while (exadr < nexclude && m->exclude_signature[exadr] < signature) { - exadr++; - } - - // skip this body pair if its signature is found in exclude array - if (exadr < nexclude && m->exclude_signature[exadr] == signature) { - continue; - } - } - - int ngeompair_narrow_before = d->ngeompair_narrow; - int ngeompair_mid_before = d->ngeompair_mid; - - // test all geom pairs within this body pair - if (m->body_geomnum[b1] && m->body_geomnum[b2]) { - if (!mjDISABLED(mjDSBL_MIDPHASE) && m->body_geomnum[b1]*m->body_geomnum[b2] > 1) { - int ncon_before = d->ncon; - collideTree(m, d, b1, b2, merged, startadr, pairadr); - int ncon_after = d->ncon; - void* context = (void*) m; - mjQUICKSORT(d->contact + ncon_before, ncon_after - ncon_before, - sizeof(mjContact), contactcompare, context); - } else { - for (g1=m->body_geomadr[b1]; g1 < m->body_geomadr[b1]+m->body_geomnum[b1]; g1++) { - for (g2=m->body_geomadr[b2]; g2 < m->body_geomadr[b2]+m->body_geomnum[b2]; g2++) { - collidePair(m, d, g1, g2, merged, startadr, pairadr); - } - } - } - } - if (d->ngeompair_narrow > ngeompair_narrow_before) d->nbodypair_narrow++; - if (d->ngeompair_mid > ngeompair_mid_before) d->nbodypair_broad++; - } - - // finish merging predefined pairs - if (npair) { - while (pairadr < npair) { - collideGeoms(m, d, pairadr++, -1, 0, 0); - } - } - - // end midphase timer - TM_END(mjTIMER_COL_MID); - - // subtract nested narrowphase timing from midphase timer - d->timer[mjTIMER_COL_MID].duration -= (d->timer[mjTIMER_COL_NARROW].duration - tmNarrow); - - // increment narrowphase counter - d->timer[mjTIMER_COL_NARROW].number++; - - mj_freeStack(d); - TM_END1(mjTIMER_POS_COLLISION); -} - - //----------------------------- broad-phase collision detection ------------------------------------ -// helper structure for SAP sorting -struct _mjtBroadphase { - float value; - int body_ismax; -}; -typedef struct _mjtBroadphase mjtBroadphase; +// make AAMM (xmin[3], xmax[3]) for one bodyflex +static void makeAAMM(const mjModel* m, mjData* d, mjtNum* aamm, int bf, const mjtNum* frame) { + // body + if (bfnbody) { + int body = bf; + int body_geomnum = m->body_geomnum[body]; + // process all body geoms (body is collidable, should have geoms) + for (int i=0; ibody_geomadr[body]+i; + mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->geom_margin[geom]; + mjtNum _aamm[6]; -// make AABB for one body -static void makeAABB(const mjModel* m, mjData* d, mjtNum* aabb, int body, const mjtNum* frame) { - int geom; - mjtNum _aabb[6], cen; + // set _aamm for this geom + for (int j=0; j<3; j++) { + mjtNum cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j); + _aamm[j] = cen - m->geom_rbound[geom] - margin; + _aamm[j+3] = cen + m->geom_rbound[geom] + margin; + } - // no geoms attached to body: set to 0 - if (m->body_geomnum[body] == 0) { - mju_zero(aabb, 6); - return; - } - - // process all body geoms - for (int i=0; i < m->body_geomnum[body]; i++) { - // get geom id - geom = m->body_geomadr[body]+i; - - // set _aabb for this geom - for (int j=0; j < 3; j++) { - cen = mju_dot3(d->geom_xpos+3*geom, frame+3*j); - _aabb[2*j] = cen - m->geom_rbound[geom] - m->geom_margin[geom]; - _aabb[2*j+1] = cen + m->geom_rbound[geom] + m->geom_margin[geom]; - } - - // update body aabb - if (i == 0) { - mju_copy(aabb, _aabb, 6); - } else { - for (int j=0; j < 3; j++) { - aabb[2*j] = mju_min(aabb[2*j], _aabb[2*j]); - aabb[2*j+1] = mju_max(aabb[2*j+1], _aabb[2*j+1]); + // update body aamm + if (i==0) { + mju_copy(aamm, _aamm, 6); + } else { + for (int j=0; j<3; j++) { + aamm[j] = mju_min(aamm[j], _aamm[j]); + aamm[j+3] = mju_max(aamm[j+3], _aamm[j+3]); + } } } } -} + // flex + else { + int f = bf - m->nbody; + int flex_vertnum = m->flex_vertnum[f]; + const mjtNum* vbase = d->flexvert_xpos + 3*m->flex_vertadr[f]; + // process flex vertices + for (int i=0; i < flex_vertnum; i++) { + mjtNum v[3]; -// return 1 if body has plane or hfield geom, 0 otherwise -static int has_plane_or_hfield(const mjModel* m, int body) { - int start = m->body_geomadr[body]; - int end = m->body_geomadr[body] + m->body_geomnum[body]; + // compute vertex coordinates in given frame + mju_mulMatVec(v, frame, vbase+3*i, 3, 3); - // scan geoms belonging to body - int g; - for (g=start; g < end; g++) { - if (m->geom_type[g] == mjGEOM_PLANE || m->geom_type[g] == mjGEOM_HFIELD) { - return 1; + // update aamm + if (i==0) { + mju_copy3(aamm, v); + mju_copy3(aamm+3, v); + } else { + for (int j=0; j<3; j++) { + aamm[j] = mju_min(aamm[j], v[j]); + aamm[j+3] = mju_max(aamm[j+3], v[j]); + } + } } - } - return 0; + // correct for flex radius and margin + mjtNum margin = mjENABLED(mjENBL_OVERRIDE) ? 0.5*m->opt.o_margin : m->flex_margin[f]; + mjtNum bound = m->flex_radius[f] + margin; + aamm[0] -= bound; + aamm[1] -= bound; + aamm[2] -= bound; + aamm[3] += bound; + aamm[4] += bound; + aamm[5] += bound; + } } -// filter body pair: 1- discard, 0- proceed -static int body_pair_filter(int weldbody1, int weldparent1, int weldbody2, - int weldparent2, int dsbl_filterparent) { - // same weldbody check - if (weldbody1 == weldbody2) { - return 1; - } - // weldparent check - if ((!dsbl_filterparent && weldbody1 != 0 && weldbody2 != 0) && - (weldbody1 == weldparent2 || weldbody2 == weldparent1)) { - return 1; - } - // all tests passed - return 0; -} - -// add body pair in buffer -static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, int maxpair) { +// add bodyflex pair in buffer; do not filter if m is NULL +static void add_pair(const mjModel* m, int bf1, int bf2, + int* npair, int* pair, int maxpair) { // add pair if there is room in buffer if ((*npair) < maxpair) { - // exlude based on contype and conaffinity - if (m && m->body_geomnum[b1] == 1 && m->body_geomnum[b2] == 1) { - // get contypes and conaffinities - int contype1 = m->geom_contype[m->body_geomadr[b1]]; - int conaffinity1 = m->geom_conaffinity[m->body_geomadr[b1]]; - int contype2 = m->geom_contype[m->body_geomadr[b2]]; - int conaffinity2 = m->geom_conaffinity[m->body_geomadr[b2]]; + // contact filtering if m is not NULL + if (m) { + int nbody = m->nbody; + int contype1, conaffinity1, contype2, conaffinity2; + + // get contype and conaffinity for bodyflex 1 + if (bf1 < nbody) { + int body_geomadr1 = m->body_geomadr[bf1]; + int body_geomnum1 = m->body_geomnum[bf1]; + contype1 = conaffinity1 = 0; + for (int i=body_geomadr1; igeom_contype[i]; + conaffinity1 |= m->geom_conaffinity[i]; + } + } else { + contype1 = m->flex_contype[bf1-nbody]; + conaffinity1 = m->flex_conaffinity[bf1-nbody]; + } + + // get contype and conaffinity for bodyflex 2 + if (bf2 < nbody) { + int body_geomadr2 = m->body_geomadr[bf2]; + int body_geomnum2 = m->body_geomnum[bf2]; + contype2 = conaffinity2 = 0; + for (int i=body_geomadr2; igeom_contype[i]; + conaffinity2 |= m->geom_conaffinity[i]; + } + } else { + contype2 = m->flex_contype[bf2-nbody]; + conaffinity2 = m->flex_conaffinity[bf2-nbody]; + } // compatibility check if (!(contype1 & conaffinity2) && !(contype2 & conaffinity1)) { @@ -616,12 +976,11 @@ static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, in } // add pair - if (b1 < b2) { - pair[*npair] = (b1<<16) + b2; + if (bf1 < bf2) { + pair[*npair] = (bf1<<16) + bf2; } else { - pair[*npair] = (b2<<16) + b1; + pair[*npair] = (bf2<<16) + bf1; } - (*npair)++; } else { mjERROR("broadphase buffer full"); @@ -630,14 +989,25 @@ static void add_pair(const mjModel* m, int b1, int b2, int* npair, int* pair, in -// comparison function for broadphase -quicksortfunc(broadcompare, context, el1, el2) { - mjtBroadphase* b1 = (mjtBroadphase*)el1; - mjtBroadphase* b2 = (mjtBroadphase*)el2; +//----------------------------- general Sweep and Prune algorithm ---------------------------------- - if (b1->value < b2->value) { +// helper structure for SAP sorting +struct _mjtSAP { + float value; + int id_ismax; +}; +typedef struct _mjtSAP mjtSAP; + + + +// comparison function for SAP +quicksortfunc(SAPcompare, context, el1, el2) { + mjtSAP* obj1 = (mjtSAP*)el1; + mjtSAP* obj2 = (mjtSAP*)el2; + + if (obj1->value < obj2->value) { return -1; - } else if (b1->value == b2->value) { + } else if (obj1->value == obj2->value) { return 0; } else { return 1; @@ -646,198 +1016,68 @@ quicksortfunc(broadcompare, context, el1, el2) { -// comparison function for pair sorting -quicksortfunc(paircompare, context, el1, el2) { - int signature1 = *(int*)el1; - int signature2 = *(int*)el2; - - if (signature1 < signature2) { +// given list of axis-aligned bounding boxes in AAMM (xmin[3], xmax[3]) format, +// return list of pairs (i, j) in format (i<<16 + j) that can collide, +// using sweep-and-prune along specified axis (0-2). +static int mj_SAP(mjData* d, const mjtNum* aamm, int n, int axis, int* pair, int maxpair) { + // check inputs + if (n>=0x10000 || axis<0 || axis>2 || maxpair<1) { return -1; - } else if (signature1 == signature2) { - return 0; - } else { - return 1; - } -} - - - -// does body have collidable geoms -static int can_collide(const mjModel* m, int b) { - int g; - - // scan geoms; return if collidable - for (g=0; g < m->body_geomnum[b]; g++) { - int ind = m->body_geomadr[b] + g; - if (m->geom_contype[ind] || m->geom_conaffinity[ind]) { - return 1; - } - } - - // none found - return 0; -} - - - -// broadphase collision detector -int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { - int b1, b2, toremove, cnt, npair = 0, nbody = m->nbody, ngeom = m->ngeom; - mjtNum cov[9], cen[3], dif[3], eigval[3], frame[9], quat[4]; - mjtBroadphase *sortbuf, *activebuf; - mjtNum *aabb; - - int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT); - // world with geoms, and body with plane or hfield, can collide all bodies - for (b1=0; b1 < nbody; b1++) { - // cannot colide - if (!can_collide(m, b1)) { - continue; - } - - // world with geoms, or welded body with plane or hfield - if ((b1 == 0 && m->body_geomnum[b1] > 0) || - (m->body_weldid[b1] == 0 && has_plane_or_hfield(m, b1))) { - int weld1 = 0; - int parent_weld1 = 0; - for (b2=0; b2 < nbody; b2++) { - int weld2 = m->body_weldid[b2]; - int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; - if (!body_pair_filter(weld1, parent_weld1, weld2, parent_weld2, - dsbl_filterparent)) { - add_pair(NULL, b1, b2, &npair, pair, maxpair); - } - } - } - } - - // find center of non-world geoms; return if none - cnt = 0; - mju_zero3(cen); - for (int i=0; i < ngeom; i++) { - if (m->geom_bodyid[i]) { - mju_addTo3(cen, d->geom_xpos+3*i); - cnt++; - } - } - if (cnt == 0) { - return npair; - } else { - for (int i=0; i < 3; i++) { - cen[i] /= cnt; - } - } - - // compute covariance - mju_zero(cov, 9); - for (int i=0; i < ngeom; i++) { - if (m->geom_bodyid[i]) { - mju_sub3(dif, d->geom_xpos+3*i, cen); - mjtNum D00 = dif[0]*dif[0]; - mjtNum D01 = dif[0]*dif[1]; - mjtNum D02 = dif[0]*dif[2]; - mjtNum D11 = dif[1]*dif[1]; - mjtNum D12 = dif[1]*dif[2]; - mjtNum D22 = dif[2]*dif[2]; - cov[0] += D00; - cov[1] += D01; - cov[2] += D02; - cov[3] += D01; - cov[4] += D11; - cov[5] += D12; - cov[6] += D02; - cov[7] += D12; - cov[8] += D22; - } - } - for (int i=0; i < 9; i++) { - cov[i] /= cnt; - } - - // construct covariance-aligned 3D frame - mju_eig3(eigval, frame, quat, cov); - - // allocate AABB; clear world entry (not used) - mj_markStack(d); - aabb = mj_stackAllocNum(d, 6*nbody); - mju_zero(aabb, 6); - - // construct body AABB for the aligned frame, count collidable - int bufcnt = 0; - for (int i=1; i < nbody; i++) { - makeAABB(m, d, aabb+6*i, i, frame); - - if (can_collide(m, i)) { - bufcnt++; - } - } - - // nothing collidable - if (!bufcnt) { - goto endbroad; } // allocate sort buffer - sortbuf = (mjtBroadphase*) mj_stackAllocByte( - d, 2 * bufcnt * sizeof(mjtBroadphase), _Alignof(mjtBroadphase)); - activebuf = (mjtBroadphase*) mj_stackAllocByte( - d, 2 * bufcnt * sizeof(mjtBroadphase), _Alignof(mjtBroadphase)); + mjtSAP* sortbuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); + mjtSAP* activebuf = (mjtSAP*) mj_stackAllocByte(d, 2*n*sizeof(mjtSAP), _Alignof(mjtSAP)); - // init sortbuf with axis0 - int k = 0; - for (int i=1; i < nbody; i++) { - // cannot colide - if (!can_collide(m, i)) { - continue; - } - - // init - sortbuf[2*k].body_ismax = i; - sortbuf[2*k].value = (float)aabb[6*i]; - sortbuf[2*k+1].body_ismax = i + 0x10000; - sortbuf[2*k+1].value = (float)aabb[6*i+1]; - k++; + // init sortbuf with specified axis + for (int i=0; ibody_weldid[b1]; - int weld2 = m->body_weldid[b2]; - int parent_weld1 = m->body_weldid[m->body_parentid[weld1]]; - int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; - - if (body_pair_filter(weld1, parent_weld1, weld2, parent_weld2, - dsbl_filterparent)) { - continue; - } + if (!(sortbuf[i].id_ismax & 0x10000)) { + for (int j=0; j aabb[6*b2+3] || - aabb[6*b1+3] < aabb[6*b2+2] || - aabb[6*b1+4] > aabb[6*b2+5] || - aabb[6*b1+5] < aabb[6*b2+4]) { + if (aamm[6*id1+axisA] > aamm[6*id2+axisA+3] || + aamm[6*id1+axisB] > aamm[6*id2+axisB+3] || + aamm[6*id2+axisA] > aamm[6*id1+axisA+3] || + aamm[6*id2+axisB] > aamm[6*id1+axisB+3]) { continue; } - // add body pair if there is room in buffer - add_pair(m, b1, b2, &npair, pair, maxpair); + // add pair, check buffer size + pair[npair++] = (id1<<16) + id2; + if (npair>=maxpair) { + return maxpair; + } } // add to list @@ -847,11 +1087,11 @@ int mj_broadphase(const mjModel* m, mjData* d, int* pair, int maxpair) { // max value: remove corresponding min value from list else { - toremove = sortbuf[i].body_ismax & 0xFFFF; - for (int j=0; j < cnt; j++) { - if (activebuf[j].body_ismax == toremove) { - if (j < cnt-1) { - memmove(activebuf+j, activebuf+j+1, sizeof(mjtBroadphase)*(cnt-1-j)); + int toremove = sortbuf[i].id_ismax & 0xFFFF; + for (int j=0; jnbody, ngeom = m->ngeom; + int nvert = m->nflexvert, nflex = m->nflex, nbodyflex = m->nbody + m->nflex; + int dsbl_filterparent = mjDISABLED(mjDSBL_FILTERPARENT); + mjtNum cov[9], cen[3], eigval[3], frame[9], quat[4]; + + // init with pairs involving always-colliding bodies + for (int b1=0; b1 < nbody; b1++) { + // cannot colide + if (!canCollide(m, b1)) { + continue; + } + + // b1 is world body with geoms, or world-welded body with plane + if ((b1 == 0 && m->body_geomnum[b1] > 0) || + (m->body_weldid[b1]==0 && hasPlane(m, b1))) { + // add b1:body pairs that are not welded together + for (int b2=0; b2body_weldid[b2]; + int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; + if (filterBodyPair(0, 0, weld2, parent_weld2, dsbl_filterparent)) { + continue; + } + + // add pair + add_pair(m, b1, b2, &npair, bfpair, maxpair); + } + + // add all b1:flex pairs + for (int f=0; fgeom_bodyid[i]) { + mju_addTo3(cen, d->geom_xpos+3*i); + cnt++; + } + } + for (int i=0; i < nvert; i++) { + if (m->flex_vertbodyid[i]) { + mju_addTo3(cen, d->flexvert_xpos+3*i); + cnt++; + } + } + if (cnt==0) { + return npair; + } + mju_scl3(cen, cen, 1.0/cnt); + + // compute covariance + mju_zero(cov, 9); + for (int i=0; i < ngeom; i++) { + if (m->geom_bodyid[i]) { + updateCov(cov, d->geom_xpos+3*i, cen); + } + } + for (int i=0; i < nvert; i++) { + if (m->flex_vertbodyid[i]) { + updateCov(cov, d->flexvert_xpos+3*i, cen); + } + } + mju_scl(cov, cov, 1.0/cnt, 9); + + // construct covariance-aligned 3D frame + mju_eig3(eigval, frame, quat, cov); + + // allocate collidable bodyflex ids, construct list + mj_markStack(d); + int* bfid = mj_stackAllocInt(d, nbodyflex); + int ncollide = 0; + for (int i=1; i> 16]; + int bf2 = bfid[sappair[i] & 0xFFFF]; + + // body pair: prune based on weld filter + if (bf1body_weldid[bf1]; + int weld2 = m->body_weldid[bf2]; + int parent_weld1 = m->body_weldid[m->body_parentid[weld1]]; + int parent_weld2 = m->body_weldid[m->body_parentid[weld2]]; + + if (filterBodyPair(weld1, parent_weld1, weld2, parent_weld2, + dsbl_filterparent)) { + continue; + } + } + + // add bodyflex pair if there is room in buffer + add_pair(m, bf1, bf2, &npair, bfpair, maxpair); + } + endbroad: - // sort pairs by signature + // sort bodyflex pairs by signature if (npair) { - mjQUICKSORT(pair, npair, sizeof(int), paircompare, 0); + mjQUICKSORT(bfpair, npair, sizeof(int), uintcompare, 0); } mj_freeStack(d); @@ -875,14 +1288,163 @@ endbroad: //----------------------------- narrow-phase collision detection ----------------------------------- +// compute contact condim, gap, solref, solimp, friction +static void mj_contactParam(const mjModel* m, int* condim, mjtNum* gap, + mjtNum* solref, mjtNum* solimp, mjtNum* friction, + int g1, int g2, int f1, int f2) { + mjtNum fri[3]; + + // get parameters from geom1 or flex1 + int priority1 = (f1<0) ? m->geom_priority[g1] : m->flex_priority[f1]; + int condim1 = (f1<0) ? m->geom_condim[g1] : m->flex_condim[f1]; + mjtNum gap1 = (f1<0) ? m->geom_gap[g1] : m->flex_gap[f1]; + mjtNum solmix1 = (f1<0) ? m->geom_solmix[g1] : m->flex_solmix[f1]; + const mjtNum* solref1 = (f1<0) ? m->geom_solref+g1*mjNREF : m->flex_solref+f1*mjNREF; + const mjtNum* solimp1 = (f1<0) ? m->geom_solimp+g1*mjNIMP : m->flex_solimp+f1*mjNIMP; + const mjtNum* friction1 = (f1<0) ? m->geom_friction+g1*3 : m->flex_friction+f1*3; + + // get parameters from geom2 or flex2 + int priority2 = (f2<0) ? m->geom_priority[g2] : m->flex_priority[f2]; + int condim2 = (f2<0) ? m->geom_condim[g2] : m->flex_condim[f2]; + mjtNum gap2 = (f2<0) ? m->geom_gap[g2] : m->flex_gap[f2]; + mjtNum solmix2 = (f2<0) ? m->geom_solmix[g2] : m->flex_solmix[f2]; + const mjtNum* solref2 = (f2<0) ? m->geom_solref+g2*mjNREF : m->flex_solref+f2*mjNREF; + const mjtNum* solimp2 = (f2<0) ? m->geom_solimp+g2*mjNIMP : m->flex_solimp+f2*mjNIMP; + const mjtNum* friction2 = (f2<0) ? m->geom_friction+g2*3 : m->flex_friction+f2*3; + + // gap: max + *gap = mju_max(gap1, gap2); + + // different priority: copy from item with higher priority + if (priority1>priority2) { + *condim = condim1; + mju_copy(solref, solref1, mjNREF); + mju_copy(solimp, solimp1, mjNIMP); + mju_copy(fri, friction1, 3); + } + else if (priority1=mjMINVAL && solmix2>=mjMINVAL) { + mix = solmix1 / (solmix1 + solmix2); + } else if (solmix10 && solref2[0]>0) { + for (int i=0; i6 || *condim<1) { + mjERROR("Invalid condim value: %d", *condim); + } +} + + + +// set contact parameters +static void mj_setContact(const mjModel* m, mjContact* con, + int condim, mjtNum includemargin, + const mjtNum* solref, const mjtNum* solreffriction, + const mjtNum* solimp, const mjtNum* friction) { + // set parameters + con->dim = condim; + con->includemargin = includemargin; + mj_assignRef(m, con->solref, solref); + mj_assignRef(m, con->solreffriction, solreffriction); + mj_assignImp(m, con->solimp, solimp); + mj_assignFriction(m, con->friction, friction); + + // exclude in gap + con->exclude = (con->dist >= includemargin); + + // complete frame + mju_makeFrame(con->frame); + + // clear fields that are computed later + con->efc_address = -1; + con->mu = 0; + mju_zero(con->H, 36); + + // set deprecated fields + con->geom1 = con->geom[0]; + con->geom2 = con->geom[1]; +} + + + +// make capsule from two flex vertices +static void mj_makeCapsule(const mjModel* m, mjData* d, int f, const int vid[2], + mjtNum pos[3], mjtNum mat[9], mjtNum size[2]) { + // get vertex positions + mjtNum* v1 = d->flexvert_xpos + 3*(m->flex_vertadr[f] + vid[0]); + mjtNum* v2 = d->flexvert_xpos + 3*(m->flex_vertadr[f] + vid[1]); + + // construct capsule from vertices + mjtNum dif[3] = {v1[0]-v2[0], v1[1]-v2[1], v1[2]-v2[2]}; + size[0] = m->flex_radius[f]; + size[1] = 0.5*mju_normalize3(dif); + + mju_add3(pos, v1, v2); + mju_scl3(pos, pos, 0.5); + + mjtNum quat[4]; + mju_quatZ2Vec(quat, dif); + mju_quat2Mat(mat, quat); +} + + // test two geoms for collision, apply filters, add to contact list -// flg_user disables filters and uses usermargin -static void collideGeoms(const mjModel* m, mjData* d, - int g1, int g2, int flg_user, mjtNum usermargin) { +void mj_collideGeoms(const mjModel* m, mjData* d, int g1, int g2) { TM_START; + int num, type1, type2, condim; - mjtNum margin, gap, mix, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum margin, gap, friction[5], solref[mjNREF], solimp[mjNIMP]; mjtNum solreffriction[mjNREF] = {0}; int ipair = (g2 < 0 ? g1 : -1); @@ -909,8 +1471,8 @@ static void collideGeoms(const mjModel* m, mjData* d, return; } - // apply filters if not predefined pair and not flg_user - if (ipair < 0 && !flg_user) { + // apply filters if not predefined pair + if (ipair < 0) { // user filter if defined if (mjcb_contactfilter) { if (mjcb_contactfilter(m, d, g1, g2)) { @@ -919,49 +1481,24 @@ static void collideGeoms(const mjModel* m, mjData* d, } // otherwise built-in filter - else if (mj_contactFilter(m->geom_contype[g1], m->geom_conaffinity[g1], - m->geom_contype[g2], m->geom_conaffinity[g2])) { + else if (filterBitmask(m->geom_contype[g1], m->geom_conaffinity[g1], + m->geom_contype[g2], m->geom_conaffinity[g2])) { return; } } - // set margin, gap, condim: dynamic + // set margin: dynamic or pair if (ipair < 0) { - // margin and gap: max - margin = mju_max(m->geom_margin[g1], m->geom_margin[g2]); - gap = mju_max(m->geom_gap[g1], m->geom_gap[g2]); - - // condim: priority or max - if (m->geom_priority[g1] != m->geom_priority[g2]) { - int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2); - condim = m->geom_condim[gp]; - } else { - condim = mjMAX(m->geom_condim[g1], m->geom_condim[g2]); - } - } - - // set margin, gap, condim: pair - else { - margin = m->pair_margin[ipair]; - gap = m->pair_gap[ipair]; - condim = m->pair_dim[ipair]; - } - - // adjust margin - if (flg_user) { - margin = usermargin; + margin = mj_assignMargin(m, mju_max(m->geom_margin[g1], m->geom_margin[g2])); } else { - margin = mj_assignMargin(m, margin); + margin = mj_assignMargin(m, m->pair_margin[ipair]); } // bounding sphere filter - if (!collideSphere(m, d, g1, g2, margin)) { + if (mj_filterSphere(m, d, g1, g2, margin)) { return; } - // increment counter of expected collisions - d->ngeompair_mid++; - // allocate mjContact[mjMAXCONPAIR] on the arena mjContact* con = (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); @@ -973,15 +1510,12 @@ static void collideGeoms(const mjModel* m, mjData* d, // call collision detector to generate contacts num = mjCOLLISIONFUNC[type1][type2](m, d, con, g1, g2, margin); - // no contacts from near-phase + // check contacts if (!num) { resetArena(d); return; } - // increment counter of actual collisions - d->ngeompair_narrow++; - // check number of contacts, SHOULD NOT OCCUR if (num > mjMAXCONPAIR) { mjERROR("too many contacts returned by collision function"); @@ -1024,120 +1558,39 @@ static void collideGeoms(const mjModel* m, mjData* d, num = i; } - // set friction, solref, solimp: dynamic + // set condim, gap, solref, solimp, friction: dynamic if (ipair < 0) { - // different priority - if (m->geom_priority[g1] != m->geom_priority[g2]) { - int gp = (m->geom_priority[g1] > m->geom_priority[g2] ? g1 : g2); - - // friction - for (int i=0; i < 3; i++) { - friction[2*i] = m->geom_friction[3*gp+i]; - } - - // reference - mju_copy(solref, m->geom_solref+mjNREF*gp, mjNREF); - - // impedance - mju_copy(solimp, m->geom_solimp+mjNIMP*gp, mjNIMP); - } - - // same priority - else { - // friction: max - for (int i=0; i < 3; i++) { - friction[2*i] = mju_max(m->geom_friction[3*g1+i], m->geom_friction[3*g2+i]); - } - - // solver mix factor - if (m->geom_solmix[g1] >= mjMINVAL && m->geom_solmix[g2] >= mjMINVAL) { - mix = m->geom_solmix[g1] / (m->geom_solmix[g1] + m->geom_solmix[g2]); - } else if (m->geom_solmix[g1] < mjMINVAL && m->geom_solmix[g2] < mjMINVAL) { - mix = 0.5; - } else if (m->geom_solmix[g1] < mjMINVAL) { - mix = 0.0; - } else { - mix = 1.0; - } - - // reference standard: mix - if (m->geom_solref[mjNREF*g1] > 0 && m->geom_solref[mjNREF*g2] > 0) { - for (int i=0; i < mjNREF; i++) { - solref[i] = mix*m->geom_solref[mjNREF*g1+i] + (1-mix)*m->geom_solref[mjNREF*g2+i]; - } - } - - // reference direct: min - else { - for (int i=0; i < mjNREF; i++) { - solref[i] = mju_min(m->geom_solref[mjNREF*g1+i], m->geom_solref[mjNREF*g2+i]); - } - } - - // impedance: mix - mju_scl(solimp, m->geom_solimp+mjNIMP*g1, mix, mjNIMP); - mju_addToScl(solimp, m->geom_solimp+mjNIMP*g2, 1-mix, mjNIMP); - } - - // unpack 5D friction - friction[1] = friction[0]; - friction[3] = friction[4]; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, g1, g2, -1, -1); } - // set friction, solref, solimp: pair + // set condim, gap, solref, solimp, friction: pair else { - // friction - for (int i=0; i < 5; i++) { - friction[i] = m->pair_friction[5*ipair+i]; - } - - // reference, normal direction + condim = m->pair_dim[ipair]; + gap = m->pair_gap[ipair]; mju_copy(solref, m->pair_solref+mjNREF*ipair, mjNREF); + mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); + mju_copy(friction, m->pair_friction+5*ipair, 5); // reference, friction directions if (m->pair_solreffriction[mjNREF*ipair] || m->pair_solreffriction[mjNREF*ipair + 1]) { mju_copy(solreffriction, m->pair_solreffriction+mjNREF*ipair, mjNREF); } - - // impedance - mju_copy(solimp, m->pair_solimp+mjNIMP*ipair, mjNIMP); } - // clamp friction to mjMINMU - for (int i=0; i < 5; i++) { - friction[i] = mju_max(mjMINMU, friction[i]); - } + // add contacts returned by collision detector + for (int i=0; i 6 || condim < 1) { - mjERROR("invalid condim value: %d", i); // SHOULD NOT OCCUR - } - - // set contact data - con[i].dim = condim; - con[i].geom1 = g1; - con[i].geom2 = g2; - con[i].includemargin = margin-gap; - mju_copy(con[i].friction, friction, 5); - mj_assignRef(m, con[i].solref, solref); - mj_assignRef(m, con[i].solreffriction, solreffriction); - mj_assignImp(m, con[i].solimp, solimp); - - // exclude in gap - if (con[i].dist < con[i].includemargin) { - con[i].exclude = 0; - } else { - con[i].exclude = 1; - } - - // complete frame - mju_makeFrame(con[i].frame); - - // clear fields that are computed later - con[i].efc_address = -1; - con[i].mu = 0; - mju_zero(con[i].H, 36); + // set remaining contact parameters + mj_setContact(m, con + i, condim, margin-gap, solref, solreffriction, solimp, friction); } // add to ncon @@ -1152,8 +1605,513 @@ static void collideGeoms(const mjModel* m, mjData* d, -// filter contacts: 1- discard, 0- proceed -int mj_contactFilter(int contype1, int conaffinity1, - int contype2, int conaffinity2) { - return !(contype1 & conaffinity2) && !(contype2 & conaffinity1); +// 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) { + mjContact con; + mjtNum radius = m->flex_radius[f]; + mjtNum* pos = d->geom_xpos + 3*g; + mjtNum* mat = d->geom_xmat + 9*g; + mjtNum nrm[3] = {mat[2], mat[5], mat[8]}; + + // prepare contact parameters (same for all vertices) + mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f])); + int condim; + int flex_vertnum = m->flex_vertnum[f]; + mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solreffriction[mjNREF] = {0}; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + + // collide all flex vertices with plane + for (int i=0; iflexvert_xpos + 3*(m->flex_vertadr[f]+i); + + // distance from plane to vertex + mjtNum dif[3] = {v[0]-pos[0], v[1]-pos[1], v[2]-pos[2]}; + mjtNum dist = mju_dot3(dif, nrm); + + // no contact + if (dist > margin + radius) { + continue; + } + + // create contact + con.dist = dist - radius; + mju_addScl3(con.pos, v, nrm, -con.dist*0.5 - radius); + mju_copy3(con.frame, nrm); + mju_zero3(con.frame+3); + + // set contact ids + con.geom[0] = g; + con.geom[1] = -1; + con.flex[0] = -1; + con.flex[1] = f; + con.elem[0] = -1; + con.elem[1] = -1; + con.vert[0] = -1; + con.vert[1] = i; + + // set remaining contact parameters + mj_setContact(m, &con, condim, margin-gap, solref, solreffriction, solimp, friction); + + // add to mjData, abort if too many contacts + if (mj_addContact(m, d, &con)) { + return; + } + } +} + + + +// test single triangle plane : vertex +static int planeVertex(mjContact* con, const mjtNum* pos, mjtNum rad, + int t0, int t1, int t2, int v) { + // make t0 the origin + mjtNum e1[3], e2[3], ev[3]; + mju_sub3(e1, pos+3*t1, pos+3*t0); + mju_sub3(e2, pos+3*t2, pos+3*t0); + mju_sub3(ev, pos+3*v, pos+3*t0); + + // compute normal + mjtNum nrm[3]; + mju_cross(nrm, e1, e2); + mju_normalize3(nrm); + + // project, check distance + mjtNum dst = mju_dot3(ev, nrm); + if (dst<=-2*rad) { + return 0; + } + + // construct contact + con->dist = -dst-2*rad; + mju_scl3(con->frame, nrm, -1); + mju_addScl3(con->pos, pos+3*v, nrm, -0.5*dst); + con->vert[1] = v; + return 1; +} + + + +// test for internal flex collisions, add to contact list +// ignore margin to avoid permament self-collision +void mj_collideFlexInternal(const mjModel* m, mjData* d, int f) { + int flex_evpairnum = m->flex_evpairnum[f]; + + // predefined element-vertex + for (int i=0; iflex_evpair + 2*m->flex_evpairadr[f] + 2*i; + mj_collideElemVert(m, d, f, ev[0], ev[1]); + } + + // within-element for tetrahedral only + if (m->flex_dim[f]!=3) { + return; + } + + // initialize contact + mjContact con; + con.geom[0] = con.geom[1] = -1; + con.flex[0] = con.flex[1] = f; + con.elem[1] = con.vert[0] = -1; + + // prepare contact parameters + int condim; + int flex_elemnum = m->flex_elemnum[f]; + mjtNum radius = m->flex_radius[f]; + mjtNum gap, solref[mjNREF], solimp[mjNIMP], friction[5]; + mjtNum solreffriction[mjNREF] = {0}; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + condim = 1; + + // process all elements + const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; + for (int e=0; eflex_elem + m->flex_elemdataadr[f] + e*4; + con.elem[0] = e; + + // face (0,1,2) + if (planeVertex(&con, vertxpos, radius, edata[0], edata[1], edata[2], edata[3])) { + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + if (mj_addContact(m, d, &con)) return; + } + + // face (0,2,3) + if (planeVertex(&con, vertxpos, radius, edata[0], edata[2], edata[3], edata[1])) { + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + if (mj_addContact(m, d, &con)) return; + } + + // face (0,3,1) + if (planeVertex(&con, vertxpos, radius, edata[0], edata[3], edata[1], edata[2])) { + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + if (mj_addContact(m, d, &con)) return; + } + + // face (1,3,2) + if (planeVertex(&con, vertxpos, radius, edata[1], edata[3], edata[2], edata[0])) { + mj_setContact(m, &con, condim, 0, solref, solreffriction, solimp, friction); + if (mj_addContact(m, d, &con)) return; + } + } +} + + + +// test active element self-collisions with SAP +// ignore margin to avoid permanent self-collision +void mj_collideFlexSAP(const mjModel* m, mjData* d, int f) { + mj_markStack(d); + + // allocate and construct active element ids + int* elid = mj_stackAllocInt(d, m->flex_elemnum[f]); + int nactive = 0; + int flex_elemnum = m->flex_elemnum[f]; + for (int i=0; iflexelem_aabb + 6*m->flex_elemadr[f]; + for (int i=0; ibvh_aabb_dyn + 6*(m->flex_bvhadr[f] - m->nbvhstatic); + int axis = (bvh[3]>bvh[4] && bvh[3]>bvh[5]) ? 0 : (bvh[4]>bvh[5] ? 1 : 2); + + // call SAP; hard limit on number of pairs to avoid out-of-memory + int maxsappair = mjMIN(nactive*(nactive-1)/2, 1000000); + int* sappair = mj_stackAllocInt(d, maxsappair); + int nsappair = mj_SAP(d, aamm, nactive, axis, sappair, maxsappair); + if (nsappair<0) { + mjERROR("SAP failed"); + } + + // send SAP pairs to nearphase + for (int i=0; i> 16]; + int e2 = elid[sappair[i] & 0xFFFF]; + mj_collideElems(m, d, f, e1, f, e2); + } + + mj_freeStack(d); +} + + + +// 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) { + mjtNum margin = mj_assignMargin(m, mju_max(m->geom_margin[g], m->flex_margin[f])); + int dim = m->flex_dim[f], type = m->geom_type[g]; + int num; + + // bounding sphere test: only if midphase is disabled + if (mjDISABLED(mjDSBL_MIDPHASE)) { + int eglobal = m->flex_elemadr[f] + e; + if (filterSphereBox(d->geom_xpos+3*g, m->geom_rbound[g]+margin, + d->flexelem_aabb+6*eglobal)) { + return; + } + } + + // skip if element has vertices on the same body as geom + int b = m->geom_bodyid[g]; + const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); + const int* bdata = m->flex_vertbodyid + m->flex_vertadr[f]; + for (int i=0; i<=dim; i++) { + if (b==bdata[edata[i]]) { + return; + } + } + + // allocate mjContact[mjMAXCONPAIR] on the arena + mjContact* con = + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return; + } + + // sphere/capsule/box : capsule + if (dim==1 && (type==mjGEOM_SPHERE || type==mjGEOM_CAPSULE || type==mjGEOM_BOX)) { + // make capsule from vertices + mjtNum pos[3], mat[9], size[2]; + mj_makeCapsule(m, d, f, m->flex_elem + m->flex_elemdataadr[f] + e*2, + pos, mat, size); + + // call raw primitive for corresponding geom type + if (type==mjGEOM_SPHERE) { + num = mjraw_SphereCapsule(con, margin, + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, + pos, mat, size); + } + else if (type==mjGEOM_CAPSULE) { + num = mjraw_CapsuleCapsule(con, margin, + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g, + pos, mat, size); + } + else { + num = mjraw_CapsuleBox(con, margin, + pos, mat, size, + d->geom_xpos+3*g, d->geom_xmat+9*g, m->geom_size+3*g); + + // reverse contact normals, since box geom is second + for (int i=0; iflexvert_xpos + 3*m->flex_vertadr[f]; + num = mjraw_SphereTriangle(con, margin, + d->geom_xpos+3*g, m->geom_size[3*g], + vertxpos + 3*edata[0], vertxpos + 3*edata[1], + vertxpos + 3*edata[2], m->flex_radius[f]); + } + + // general geom : elem + else { + num = mjc_ConvexElem(m, d, con, g, -1, -1, -1, f, e, margin); + } + + // check contacts + if (!num) { + resetArena(d); + return; + } + + // get contact parameters + int condim; + mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum solreffriction[mjNREF] = {0}; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, g, -1, -1, f); + + // add contacts + for (int i=0; incon += num; + + // move arena pointer back to the end of the contact array + resetArena(d); +} + + + +// 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) { + TM_START; + mjtNum margin = mj_assignMargin(m, mju_max(m->flex_margin[f1], m->flex_margin[f2])); + int dim1 = m->flex_dim[f1], dim2 = m->flex_dim[f2]; + int num; + + // ignore margin in self-collisions + if (f1==f2) { + margin = 0; + } + + // bounding box filter (not applied in midphase) + if (filterBox(d->flexelem_aabb+6*(m->flex_elemadr[f1]+e1), + d->flexelem_aabb+6*(m->flex_elemadr[f2]+e2), margin)) { + return; + } + + // skip if elements have vertices on the same body + const int* edata1 = m->flex_elem + m->flex_elemdataadr[f1] + e1*(dim1+1); + const int* edata2 = m->flex_elem + m->flex_elemdataadr[f2] + e2*(dim2+1); + const int* bdata1 = m->flex_vertbodyid + m->flex_vertadr[f1]; + const int* bdata2 = m->flex_vertbodyid + m->flex_vertadr[f2]; + for (int i1=0; i1<=dim1; i1++) { + int b1 = bdata1[edata1[i1]]; + for (int i2=0; i2<=dim2; i2++) { + if (b1==bdata2[edata2[i2]]) { + return; + } + } + } + + // allocate mjContact[mjMAXCONPAIR] on the arena + mjContact* con = + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return; + } + + // capsule : capsule + if (dim1==1 && dim2==1) { + // make capsules from vertices + mjtNum pos1[3], mat1[9], size1[2]; + mjtNum pos2[3], mat2[9], size2[2]; + mj_makeCapsule(m, d, f1, m->flex_elem + m->flex_elemdataadr[f1] + e1*2, + pos1, mat1, size1); + mj_makeCapsule(m, d, f2, m->flex_elem + m->flex_elemdataadr[f2] + e2*2, + pos2, mat2, size2); + + // raw primitive + num = mjraw_CapsuleCapsule(con, margin, pos1, mat1, size1, pos2, mat2, size2); + } + + // general convex collision + else { + num = mjc_ConvexElem(m, d, con, -1, f1, e1, -1, f2, e2, margin); + } + + // check contacts + if (!num) { + resetArena(d); + return; + } + + // get contact parameters + int condim; + mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum solreffriction[mjNREF] = {0}; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f1, f2); + + // ignore gap in self collision, since margin is ignored + if (f1==f2) { + gap = 0; + } + + // add contacts + for (int i=0; incon += num; + + // move arena pointer back to the end of the contact array + resetArena(d); + + // add duration without incrementing counter + TM_ADD(mjTIMER_COL_NARROW); +} + + + +// test element and vertex for collision, add to contact list +void mj_collideElemVert(const mjModel* m, mjData* d, int f, int e, int v) { + mjtNum margin = mj_assignMargin(m, m->flex_margin[f]); + mjtNum radius = m->flex_radius[f]; + const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + v); + int dim = m->flex_dim[f]; + const int* edata = m->flex_elem + m->flex_elemdataadr[f] + e*(dim+1); + int num; + + // box-box filter (sphere treated as box) + const mjtNum* aabb = d->flexelem_aabb + 6*(m->flex_elemadr[f] + e); + mjtNum rbound = margin + radius; + if (aabb[0]-aabb[3] > vert[0]+rbound) return; + if (aabb[1]-aabb[4] > vert[1]+rbound) return; + if (aabb[2]-aabb[5] > vert[2]+rbound) return; + if (aabb[0]+aabb[3] < vert[0]-rbound) return; + if (aabb[1]+aabb[4] < vert[1]-rbound) return; + if (aabb[2]+aabb[5] < vert[2]-rbound) return; + + // allocate mjContact[mjMAXCONPAIR] on the arena + mjContact* con = + (mjContact*) mj_arenaAllocByte(d, sizeof(mjContact) * mjMAXCONPAIR, _Alignof(mjContact)); + if (!con) { + mj_warning(d, mjWARN_CONTACTFULL, d->ncon); + return; + } + + // sphere : capsule + if (dim==1) { + mjtNum pos[3], mat[9], size[2]; + mjtNum I[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1}; + mj_makeCapsule(m, d, f, edata, pos, mat, size); + num = mjraw_SphereCapsule(con, 0, vert, I, &radius, pos, mat, size); + } + + // sphere : triangle + else if (dim==2) { + const mjtNum* vertxpos = d->flexvert_xpos + 3*m->flex_vertadr[f]; + num = mjraw_SphereTriangle(con, 0, vert, radius, + vertxpos + 3*edata[0], vertxpos + 3*edata[1], + vertxpos + 3*edata[2], radius); + } + + // sphere : tetrahdron + else { + num = mjc_ConvexElem(m, d, con, -1, f, -1, v, f, e, 0); + } + + // check contacts + if (!num) { + resetArena(d); + return; + } + + // get contact parameters + int condim; + mjtNum gap, friction[5], solref[mjNREF], solimp[mjNIMP]; + mjtNum solreffriction[mjNREF] = {0}; + mj_contactParam(m, &condim, &gap, solref, solimp, friction, -1, -1, f, f); + + // add contacts + for (int i=0; incon += num; + + // move arena pointer back to the end of the contact array + resetArena(d); } diff --git a/src/engine/engine_collision_driver.h b/src/engine/engine_collision_driver.h index 8c84a729..a27f84c7 100644 --- a/src/engine/engine_collision_driver.h +++ b/src/engine/engine_collision_driver.h @@ -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 } diff --git a/src/engine/engine_collision_primitive.c b/src/engine/engine_collision_primitive.c index a56be368..e25c9d37 100644 --- a/src/engine/engine_collision_primitive.c +++ b/src/engine/engine_collision_primitive.c @@ -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]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; +} diff --git a/src/engine/engine_collision_primitive.h b/src/engine/engine_collision_primitive.h index d0089ae1..49946707 100644 --- a/src/engine/engine_collision_primitive.h +++ b/src/engine/engine_collision_primitive.h @@ -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, diff --git a/src/engine/engine_collision_sdf.c b/src/engine/engine_collision_sdf.c index 4e17eebb..f754c9e3 100644 --- a/src/engine/engine_collision_sdf.c +++ b/src/engine/engine_collision_sdf.c @@ -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; diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index b4335e05..ba2bf539 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -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 (vidcontact 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; eflexedge_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; kbody_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; inv, 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; eflex_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) { diff --git a/src/engine/engine_core_constraint.h b/src/engine/engine_core_constraint.h index ce04b120..05c5fd57 100644 --- a/src/engine/engine_core_constraint.h +++ b/src/engine/engine_core_constraint.h @@ -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 ---------------------------------------------- diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index d672d664..ba01e9dd 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -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; ibvh_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; fnflex; 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; iflexvert_xpos+3*i, d->xpos+3*m->flex_vertbodyid[i]); + } + } + + // non-centered: map from local to global + else { + for (int i=vstart; iflexvert_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; fnflex; f++) { + int dim = m->flex_dim[f]; + + // process elements of this flex + for (int e=0; eflex_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; fnflex; 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; ibvh_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; fnflex; 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; eflex_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 } diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index 48776db5..35a78b43 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -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); diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index 265597ff..84c51d47 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -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; fnflex; 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]; eflex_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) { diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index 9433a81b..c68ed24a 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -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); diff --git a/src/engine/engine_inverse.c b/src/engine/engine_inverse.c index 961d46b1..f237b6af 100644 --- a/src/engine/engine_inverse.c +++ b/src/engine/engine_inverse.c @@ -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); } diff --git a/src/engine/engine_io.c b/src/engine/engine_io.c index 19c716df..4d71584e 100644 --- a/src/engine/engine_io.c +++ b/src/engine/engine_io.c @@ -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."; } diff --git a/src/engine/engine_io.h b/src/engine/engine_io.h index d833281a..16554fa7 100644 --- a/src/engine/engine_io.h +++ b/src/engine/engine_io.h @@ -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, diff --git a/src/engine/engine_island.c b/src/engine/engine_island.c index 827bd88d..efa2edea 100644 --- a/src/engine/engine_island.c +++ b/src/engine/engine_island.c @@ -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) { diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index a2ff8560..a5088982 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -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]; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 0446696a..413f866a 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -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; inbvh; 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; inflex; 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); diff --git a/src/engine/engine_ray.c b/src/engine/engine_ray.c index 74bc5b3c..2dc60250 100644 --- a/src/engine/engine_ray.c +++ b/src/engine/engine_ray.c @@ -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; iflex_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]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]; + eflex_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 || solflex_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; vflex_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 || sol1 && (flg_face || flg_skin)) { + for (int e=0; eflex_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= 0 && (x < 0 || sol < x)) { diff --git a/src/engine/engine_ray.h b/src/engine/engine_ray.h index e52fc223..522cef63 100644 --- a/src/engine/engine_ray.h +++ b/src/engine/engine_ray.h @@ -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]); diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 91756051..e7e02bf9 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -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; inflex; 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]; eflex_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; + } + } + } } diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index 542c9c87..438a9ec3 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -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; fnflex; f++) { + for (int i=m->flex_edgeadr[f]; iflex_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]; jflexedge_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; intendon; 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; fnflex; f++) { + for (int e=m->flex_edgeadr[f]; eflex_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; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index ce48d6e8..fa19d68f 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -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; ib2) { + 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; ibody_dofadr[b1] + i; + } + + // copy b2 dofs + for (int i=0; ibody_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; ibody_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; icdof; @@ -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; ibody_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; inflex; + 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; inext_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)); } diff --git a/src/engine/engine_util_errmem.h b/src/engine/engine_util_errmem.h index 60d7b7e1..10623c93 100644 --- a/src/engine/engine_util_errmem.h +++ b/src/engine/engine_util_errmem.h @@ -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]; \ diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 06361e23..3b1496b3 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -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; } diff --git a/src/engine/engine_util_sparse.c b/src/engine/engine_util_sparse.c index 193a1cb0..e86247bd 100644 --- a/src/engine/engine_util_sparse.c +++ b/src/engine/engine_util_sparse.c @@ -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 (++adrsscale = 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; fflex_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; eflex_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; fflexfaceadr[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; diff --git a/src/engine/engine_vis_interact.c b/src/engine/engine_vis_interact.c index c4542204..0c4a14ef 100644 --- a/src/engine/engine_vis_interact.c +++ b/src/engine/engine_vis_interact.c @@ -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; inflex; 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 && (newdistflex_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; } } diff --git a/src/engine/engine_vis_interact.h b/src/engine/engine_vis_interact.h index c78ffa3e..0135f086 100644 --- a/src/engine/engine_vis_interact.h +++ b/src/engine/engine_vis_interact.h @@ -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 } diff --git a/src/engine/engine_vis_state.c b/src/engine/engine_vis_state.c index 90fb58ea..1a0a4f8b 100644 --- a/src/engine/engine_vis_state.c +++ b/src/engine/engine_vis_state.c @@ -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); diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 7964bfed..979b7dc8 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -22,6 +22,7 @@ #include #include #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; inflex; 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; fnflex; f++) { + if (m->flex_bvhnum[f] && + vopt->flexgroup[mjMAX(0, mjMIN(mjNGROUP-1, m->flex_group[f]))]) { + for (int i=m->flex_bvhadr[f]; iflex_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; fnflex; 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; eflex_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; eflex_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; sflex_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; iflex_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; eflex_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; sflex_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; sflex_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); diff --git a/src/engine/engine_vis_visualize.h b/src/engine/engine_vis_visualize.h index bd63a2a8..33e8d2e2 100644 --- a/src/engine/engine_vis_visualize.h +++ b/src/engine/engine_vis_visualize.h @@ -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); diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 526e130c..affc295c 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -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]; + vflexvertadr[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]; + eflexedgeadr[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); diff --git a/src/user/CMakeLists.txt b/src/user/CMakeLists.txt index a6914c9e..e75a0355 100644 --- a/src/user/CMakeLists.txt +++ b/src/user/CMakeLists.txt @@ -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 diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc new file mode 100644 index 00000000..2cea596d --- /dev/null +++ b/src/user/user_flexcomp.cc @@ -0,0 +1,1187 @@ +// 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. + +#include "user/user_flexcomp.h" +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include "cc/array_safety.h" +#include "engine/engine_crossplatform.h" +#include "engine/engine_resource.h" +#include "engine/engine_util_blas.h" +#include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" +#include "engine/engine_util_spatial.h" +#include "user/user_model.h" +#include "user/user_objects.h" +#include "user/user_util.h" + +namespace { +namespace mju = ::mujoco::util; +using std::vector; +using std::string; +using std::stringstream; +} // namespace + +// strncpy with 0, return false +static bool comperr(char* error, const char* msg, int error_sz) { + mju_strncpy(error, msg, error_sz); + return false; +} + +// Read data of type T from a potentially unaligned buffer pointer. +template +static void ReadFromBuffer(T* dst, const char* src) { + std::memcpy(dst, src, sizeof(T)); +} + + +// constructor: set defaults outside mjCDef +mjCFlexcomp::mjCFlexcomp(void) { + type = mjFCOMPTYPE_GRID; + count[0] = count[1] = count[2] = 10; + mjuu_setvec(spacing, 0.02, 0.02, 0.02); + mjuu_setvec(scale, 1, 1, 1); + mass = 1; + inertiabox = 0.005; + equality = true; + mjuu_setvec(pos, 0, 0, 0); + mjuu_setvec(quat, 1, 0, 0, 0); + rigid = false; + centered = false; +} + + + +// make flexcomp object +bool mjCFlexcomp::Make(mjCModel* model, mjCBody* body, char* error, int error_sz) { + bool radial = (type==mjFCOMPTYPE_BOX || + type==mjFCOMPTYPE_CYLINDER || + type==mjFCOMPTYPE_ELLIPSOID); + bool direct = (type==mjFCOMPTYPE_DIRECT || + type==mjFCOMPTYPE_MESH || + type==mjFCOMPTYPE_GMSH); + + // check parent body name + if (body->name.empty()) { + return comperr(error, "Parent body must have name", error_sz); + } + + // check counts + for (int i=0; i<3; i++) { + if (count[i]<1 || (radial && count[i]<2)) { + return comperr(error, "Count too small", error_sz); + } + } + + // check spacing + double minspace = 2*def.flex.radius + def.flex.margin; + if (!direct) { + if (spacing[0]degree, model->euler); + if (alterr) { + return comperr(error, alterr, error_sz); + } + + // type-specific constructor: populate point and element, possibly set dim + bool res; + switch (type) { + case mjFCOMPTYPE_GRID: + res = MakeGrid(error, error_sz); + break; + + case mjFCOMPTYPE_BOX: + case mjFCOMPTYPE_CYLINDER: + case mjFCOMPTYPE_ELLIPSOID: + res = MakeBox(error, error_sz); + break; + + case mjFCOMPTYPE_MESH: + res = MakeMesh(model, error, error_sz); + break; + + case mjFCOMPTYPE_GMSH: + res = MakeGMSH(model, error, error_sz); + break; + + case mjFCOMPTYPE_DIRECT: + res = true; + break; + + default: + return comperr(error, "Uknown flexcomp type", error_sz); + } + if (!res) { + return false; + } + + // get dim and check + int dim = def.flex.dim; + if (dim<1 || dim>3) { + return comperr(error, "Invalid dim, must be between 1 and 3", error_sz); + } + + // force flatskin shading for box, cylinder and 3D grid + if (type==mjFCOMPTYPE_BOX || type==mjFCOMPTYPE_CYLINDER || + (type==mjFCOMPTYPE_GRID && dim==3)) { + def.flex.flatskin = true; + } + + // check pin sizes + if (pinrange.size()%2) { + return comperr(error, "Pin range number must be multiple of 2", error_sz); + } + if (pingrid.size()%dim) { + return comperr(error, "Pin grid number must be multiple of dim", error_sz); + } + if (pingridrange.size()%(2*dim)) { + return comperr(error, "Pin grid range number of must be multiple of 2*dim", error_sz); + } + if (type!=mjFCOMPTYPE_GRID && !(pingrid.empty() && pingridrange.empty())) { + return comperr(error, "Pin grid(range) can only be used with grid type", error_sz); + } + if (dim==1 && !(pingrid.empty() && pingridrange.empty())) { + return comperr(error, "Pin grid(range) cannot be used with dim=1", error_sz); + } + + // require element and point + if (point.empty() || element.empty()) { + return comperr(error, "Point and element required", error_sz); + } + + // check point size + if (point.size()%3) { + return comperr(error, "Point size must be a multiple of 3", error_sz); + } + + // check element size + if (element.size()%(dim+1)) { + return comperr(error, "Element size must be a multiple of dim+1", error_sz); + } + + // get number of points + int npnt = point.size()/3; + + // check elem vertex ids + for (int i=0; i<(int)element.size(); i++) { + if (element[i]<0 || element[i]>=npnt) { + char msg[100]; + snprintf(msg, sizeof(msg), "element %d has point id %d, number of points is %d", i, + element[i], npnt); + return comperr(error, msg, error_sz); + } + } + + // apply scaling for direct types + if (direct && (scale[0]!=1 || scale[1]!=1 || scale[2]!=1)) { + for (int i=0; i(npnt, rigid); + + // handle pins if user did not specify rigid + if (!rigid) { + // process pinid + for (int i=0; i<(int)pinid.size(); i++) { + // check range + if (pinid[i]<0 || pinid[i]>=npnt) { + return comperr(error, "pinid out of range", error_sz); + } + + // set + pinned[pinid[i]] = true; + } + + // process pinrange + for (int i=0; i<(int)pinrange.size(); i+=2) { + // check range + if (pinrange[i]<0 || pinrange[i]>=npnt || + pinrange[i+1]<0 || pinrange[i+1]>=npnt) { + return comperr(error, "pinrange out of range", error_sz); + } + + // set + for (int k=pinrange[i]; k<=pinrange[i+1]; k++) { + pinned[k] = true; + } + } + + // process pingrid + for (int i=0; i<(int)pingrid.size(); i+=dim) { + // check range + for (int k=0; k=count[k]) { + return comperr(error, "pingrid out of range", error_sz); + } + } + + // set + if (dim==2) { + pinned[GridID(pingrid[i], pingrid[i+1])] = true; + } + else if (dim==3) { + pinned[GridID(pingrid[i], pingrid[i+1], pingrid[i+2])] = true; + } + } + + // process pingridrange + for (int i=0; i<(int)pingridrange.size(); i+=2*dim) { + // check range + for (int k=0; k<2*dim; k++) { + if (pingridrange[i+k]<0 || pingridrange[i+k]>=count[k%dim]) { + return comperr(error, "pingridrange out of range", error_sz); + } + } + + // set + if (dim==2) { + for (int ix=pingridrange[i]; ix<=pingridrange[i+2]; ix++) { + for (int iy=pingridrange[i+1]; iy<=pingridrange[i+3]; iy++) { + pinned[GridID(ix, iy)] = true; + } + } + } + else if (dim==3) { + for (int ix=pingridrange[i]; ix<=pingridrange[i+3]; ix++) { + for (int iy=pingridrange[i+1]; iy<=pingridrange[i+4]; iy++) { + for (int iz=pingridrange[i+2]; iz<=pingridrange[i+5]; iz++) { + pinned[GridID(ix, iy, iz)] = true; + } + } + } + } + } + + // center of radial body is always pinned + if (radial) { + pinned[0] = true; + } + + // check if all or none are pinned + bool allpin = true, nopin = true; + for (int i=0; i (npnt, false); + for (int i=0; i<(int)element.size(); i++) { + used[element[i]] = true; + } + + // construct reindex + bool hasunused = false; + std::vector reindex (npnt, 0); + for (int i=0; i (npnt, true); + } + + // create flex, copy parameters + mjCFlex* pf = model->AddFlex(); + int id = pf->id; + *pf = def.flex; + pf->model = model; + pf->id = id; + pf->name = name; + pf->elem = element; + if (!centered) { + pf->vert = point; + } + pf->texcoord = texcoord; + + // rigid: set parent name, nothing else to do + if (rigid) { + pf->vertbody.push_back(body->name); + return true; + } + + // compute body mass and inertia matching specs + double bodymass = mass/npnt; + double bodyinertia = bodymass*(2.0*inertiabox*inertiabox)/3.0; + + // create bodies, construct flex vert and vertbody + for (int i=0; ivertbody.push_back(body->name); + } + + // not pinned: new body + else { + // add new body at vertex coordinates + mjCBody* pb = body->AddBody(); + + // set frame and inertial + pb->pos[0] = point[3*i]; + pb->pos[1] = point[3*i+1]; + pb->pos[2] = point[3*i+2]; + mjuu_zerovec(pb->ipos, 3); + pb->mass = bodymass; + pb->inertia[0] = bodyinertia; + pb->inertia[1] = bodyinertia; + pb->inertia[2] = bodyinertia; + pb->MakeInertialExplicit(); + + // add radial slider + if (radial) { + mjCJoint* jnt = pb->AddJoint(); + + // set properties + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_copyvec(jnt->axis, pb->pos, 3); + mjuu_normvec(jnt->axis, 3); + } + + // add three orthogonal sliders + else { + for (int j=0; j<3; j++) { + // add joint to body + mjCJoint* jnt = pb->AddJoint(); + + // set properties + jnt->type = mjJNT_SLIDE; + mjuu_setvec(jnt->pos, 0, 0, 0); + mjuu_setvec(jnt->axis, 0, 0, 0); + jnt->axis[j] = 1; + } + } + + // construct body name, add to vertbody + char txt[100]; + mju::sprintf_arr(txt, "%s_%d", name.c_str(), i); + pb->name = txt; + pf->vertbody.push_back(pb->name); + + // clear flex vertex coordinates if allocated + if (!centered) { + pf->vert[3*i] = 0; + pf->vert[3*i+1] = 0; + pf->vert[3*i+2] = 0; + } + } + } + + // create edge equality constraint + if (equality) { + mjCEquality *pe = model->AddEquality(&def); + pe->def = model->defaults[0]; + pe->type = mjEQ_FLEX; + pe->active = true; + pe->name1 = name; + } + + return true; +} + + + +// get point id from grid coordinates +int mjCFlexcomp::GridID(int ix, int iy) { + return ix*count[1] + iy; +} +int mjCFlexcomp::GridID(int ix, int iy, int iz) { + return ix*count[1]*count[2] + iy*count[2] + iz; +} + + + +// make grid +bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { + int dim = def.flex.dim; + bool hastex = texcoord.empty(); + + // 1D + if (dim==1) { + for (int ix=0; ix0 && iz0 && iz0 && iy static T* VecToArray(std::vector& vector, bool clear = true){ + if (vector.empty()) + return nullptr; + else { + int n = (int)vector.size(); + T* cvec = (T*) mju_malloc(n*sizeof(T)); + memcpy(cvec, vector.data(), n*sizeof(T)); + if (clear) { + vector.clear(); + } + return cvec; + } +} + + + +// make mesh +bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { + // strip path + if (!file.empty() && model->strippath) { + file = mjuu_strippath(file); + } + + // file is required + if (file.empty()) { + return comperr(error, "File is required", error_sz); + } + + // get extension and check; must be STL, OBJ or MSH + string ext = mjuu_getext(file); + if (strcasecmp(ext.c_str(), ".stl") && + strcasecmp(ext.c_str(), ".obj") && + strcasecmp(ext.c_str(), ".msh")) { + return comperr(error, "Mesh file extension must be stl, obj or msh", error_sz); + } + + // check dim + if (def.flex.dim!=2) { + return comperr(error, "Flex dim must be 2 in for mesh", error_sz); + } + + // load resource + string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file); + mjResource* resource = nullptr; + + try { + resource = mjCBase::LoadResource(filename, 0); + } catch (mjCError err) { + return comperr(error, err.message, error_sz); + } + + // load mesh + mjCMesh mesh; + bool isobj = false; + try { + if (!strcasecmp(ext.c_str(), ".stl")) { + mesh.LoadSTL(resource); + } else if (!strcasecmp(ext.c_str(), ".obj")) { + isobj = true; + mesh.LoadOBJ(resource); + } else { + mesh.LoadMSH(resource); + } + mju_closeResource(resource); + } catch (mjCError err) { + mju_closeResource(resource); + return comperr(error, err.message, error_sz); + } + + // LoadOBJ uses userXXX, extra processing needed + if (isobj) { + // check sizes + if (mesh.uservert().empty() || mesh.userface().empty()) { + return comperr(error, "Vertex and face data required", error_sz); + } + if (mesh.uservert().size()%3) { + return comperr(error, "Vertex data must be multiple of 3", error_sz); + } + if (mesh.userface().size()%3) { + return comperr(error, "Face data must be multiple of 3", error_sz); + } + + // copy vectors and clear + mesh.nvert_ = mesh.uservert_.size()/3; + mesh.nface_ = mesh.userface_.size()/3; + mesh.vert_ = VecToArray(mesh.uservert_, true); + mesh.face_ = VecToArray(mesh.userface_, true); + + // remove repeated vertices (not called in LoadOBJ) + mesh.RemoveRepeated(); + } + + // copy faces + element = vector (mesh.nface()*3); + memcpy(element.data(), mesh.face_, mesh.nface_*3*sizeof(int)); + + // copy vertices, convert from float to mjtNum + point = vector (mesh.nvert()*3); + for (int i=0; istrippath) { + file = mjuu_strippath(file); + } + + // file is required + if (file.empty()) { + return comperr(error, "File is required", error_sz); + } + + // open resource + string filename = mjuu_makefullname(model->modelfiledir, model->meshdir, file); + mjResource* resource = nullptr; + + try { + resource = mjCBase::LoadResource(filename, 0); + } catch (mjCError err) { + return comperr(error, err.message, error_sz); + } + + // try to load, close resource properly + try { + LoadGMSH(model, resource); + mju_closeResource(resource); + } catch (mjCError err) { + mju_closeResource(resource); + return comperr(error, err.message, error_sz); + } catch (...) { + mju_closeResource(resource); + return comperr(error, "exception while reading GMSH file", error_sz); + } + + return true; +} + + + +// load GMSH file from resource +void mjCFlexcomp::LoadGMSH(mjCModel* model, mjResource* resource) { + // get buffer from resource + char* buffer = 0; + int buffer_sz = mju_readResource(resource, (const void**) &buffer); + + // check buffer + if (buffer_sz<0) { + throw mjCError(NULL, "Could not read GMSH file"); + } else if (buffer_sz==0) { + throw mjCError(NULL, "Empty GMSH file"); + } else if (buffer_sz<11 || strncmp(buffer, "$MeshFormat", 11)) { + throw mjCError(NULL, "GMSH file must begin with $MeshFormat"); + } + + // check version, determine ascii or binary + double version; + int binary; + if (sscanf(buffer+11, "%lf %d", &version, &binary) != 2) { + throw mjCError(NULL, "Could not read GMSH file header"); + } + if (mju_round(100*version)!=410) { + throw mjCError(NULL, "Only GMSH file format 4.1 supported"); + } + + // find section begin/end + int nodebegin = findstring(buffer, buffer_sz, "$Nodes"); + int nodeend = findstring(buffer, buffer_sz, "$EndNodes"); + int elembegin = findstring(buffer, buffer_sz, "$Elements"); + int elemend = findstring(buffer, buffer_sz, "$EndElements"); + + // check sections + if (nodebegin<0) { + throw mjCError(NULL, "GMSH file missing $Nodes"); + } + if (nodeend> numEntityBlocks >> numNodes >> minNodeTag >> maxNodeTag; + ss >> entityDim >> entityTag >> parametric >> numNodesInBlock; + if (!ss.good()) { + throw mjCError(NULL, "Error reading Nodes header"); + } + + // require single block + if (numEntityBlocks!=1 || numNodes!=numNodesInBlock) { + throw mjCError(NULL, "All nodes must be in single block"); + } + + // check dimensionality and save + if (entityDim<1 || entityDim>3) { + throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); + } + def.flex.dim = entityDim; + + // read and discard node tags; require range from minNodeTag to maxNodeTag + for (size_t i=0; i> tag; + if (!ss.good()) { + throw mjCError(NULL, "Error reading node tags"); + } + if (tag!=i+minNodeTag) { + throw mjCError(NULL, "Node tags must be sequential"); + } + } + + // read points + point.reserve(3*numNodes); + for (size_t i=0; i<3*numNodes; i++) { + double x; + ss >> x; + if (!ss.good()) { + throw mjCError(NULL, "Error reading node coordinates"); + } + point.push_back(x); + } + } + + // binary nodes + else { + // check header size: 5 size_t, 3 int + if (nodeend-nodebegin < 52) { + throw mjCError(NULL, "Invalid nodes header"); + } + + // read header + ReadFromBuffer(&numEntityBlocks, buffer+nodebegin); + ReadFromBuffer(&numNodes, buffer+nodebegin+8); + ReadFromBuffer(&minNodeTag, buffer+nodebegin+16); + ReadFromBuffer(&maxNodeTag, buffer+nodebegin+24); + ReadFromBuffer(&entityDim, buffer+nodebegin+32); + ReadFromBuffer(&entityTag, buffer+nodebegin+36); + ReadFromBuffer(¶metric, buffer+nodebegin+40); + ReadFromBuffer(&numNodesInBlock, buffer+nodebegin+44); + + // require single block + if (numEntityBlocks!=1 || numNodes!=numNodesInBlock) { + throw mjCError(NULL, "All nodes must be in single block"); + } + + // check dimensionality and save + if (entityDim<1 || entityDim>3) { + throw mjCError(NULL, "Entity must be 1D, 2D or 3D"); + } + def.flex.dim = entityDim; + + // check section byte size + if (nodeend-nodebegin < 52+numNodes*4*8) { + throw mjCError(NULL, "Insufficient byte size of Nodes"); + } + + // check node tags: must range from minNodeTag to maxNodeTag + const char* tagbuffer = buffer + nodebegin + 52; + for (size_t i=0; i> numEntityBlocks >> numElements >> minElementTag >> maxElementTag; + ss >> entityDim >> entityTag >> elementType >> numElementsInBlock; + if (!ss.good()) { + throw mjCError(NULL, "Error reading Elements header"); + } + + // require single block + if (numEntityBlocks!=1 || numElements!=numElementsInBlock) { + throw mjCError(NULL, "All elements must be in single block"); + } + + // dimensionality must be same as nodes + if (entityDim!=def.flex.dim) { + throw mjCError(NULL, "Inconsistent dimensionality in Elements"); + } + + // type must be consistent with dimensionality + if ((entityDim==1 && elementType!=1) || + (entityDim==2 && elementType!=2) || + (entityDim==3 && elementType!=4)) { + throw mjCError(NULL, "Element type inconsistent with dimensionality"); + } + + // read elements, discard tags + element.reserve((entityDim+1)*numElements); + for (size_t i=0; i> tag; + for (int k=0; k<=entityDim; k++) { + ss >> nodeid; + if (!ss.good()) { + throw mjCError(NULL, "Error reading Elements"); + } + element.push_back((int)(nodeid-minNodeTag)); + } + } + } + + // binary elements + else { + // check header size: 5 size_t, 3 int + if (elemend-elembegin < 52) { + throw mjCError(NULL, "Invalid elements header"); + } + + // read header + ReadFromBuffer(&numEntityBlocks, buffer+elembegin); + ReadFromBuffer(&numElements, buffer+elembegin+8); + ReadFromBuffer(&minElementTag, buffer+elembegin+16); + ReadFromBuffer(&maxElementTag, buffer+elembegin+24); + ReadFromBuffer(&entityDim, buffer+elembegin+32); + ReadFromBuffer(&entityTag, buffer+elembegin+36); + ReadFromBuffer(&elementType, buffer+elembegin+40); + ReadFromBuffer(&numElementsInBlock, buffer+elembegin+44); + + // require single block + if (numEntityBlocks!=1 || numElements!=numElementsInBlock) { + throw mjCError(NULL, "All elements must be in single block"); + } + + // dimensionality must be same as nodes + if (entityDim!=def.flex.dim) { + throw mjCError(NULL, "Inconsistent dimensionality in Elements"); + } + + // type must be consistent with dimensionality + if ((entityDim==1 && elementType!=1) || + (entityDim==2 && elementType!=2) || + (entityDim==3 && elementType!=4)) { + throw mjCError(NULL, "Element type inconsistent with dimensionality"); + } + + // check section byte size + if (elemend-elembegin < 52+numElements*(entityDim+2)*8) { + throw mjCError(NULL, "Insufficient byte size of Elements"); + } + + // read elements, discard tags + element.reserve((entityDim+1)*numElements); + const char* elembuffer = buffer + elembegin + 52; + for (size_t i=0; i +#include + +#include +#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 pinid; // ids of points to pin + std::vector pinrange; // range of ids to pin + std::vector pingrid; // grid coordinates to pin + std::vector 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 point; // flex bodies/vertices + std::vector pinned; // is point pinned (true: no new body) + std::vector used; // is point used by any element (false: skip) + std::vector element; // flex elements + std::vector texcoord; // vertex texture coordinates +}; + +#endif // MUJOCO_SRC_USER_USER_FLEXCOMP_H_ diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 6a76c0cd..9d5c7092 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -48,6 +48,7 @@ #include #include #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; iname); } } + + + +//------------------ 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=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 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> 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 (3*nvert); + for (int i=0; ibodies[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; e0) { + // 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 elemaabb(6*nelem); + + // construct element bounding boxes, add to hierarchy + for (int e=0; 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> fragspec(nelem*(dim+1)); // [sorted frag vertices, elem, original frag vertices] + vector> connectspec; // [elem1, elem2, common sorted frag vertices] + vector border(nelem, false); // is element on the border + vector borderfrag(nelem*(dim+1), false); // is fragment on the border + + // make fragspec + for (int e=0; e 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 previous = {fragspec[n-1].begin(), fragspec[n-1].begin()+dim}; + vector current = {fragspec[n].begin(), fragspec[n].begin()+dim}; + + // same sequential fragments + if (previous==current) { + // found pair of elements connected by common fragment + vector 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 (nelem, 0); + } + else { + elemlayer = vector (nelem, nelem+1); // init with greater than max value + for (int e=0; eelemlayer[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 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; + } + } + } + } + } + } +} diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 47f0bb53..db2eecda 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -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& 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; itree.nbvh; + nbvhstatic += bodies[i]->tree.nbvh; + } + for (int i=0; itree().nbvh; + } + for (int i=0; itree.nbvh; + } + nbvh = nbvhstatic + nbvhdynamic; + + // flex counts + for (int i=0; invert; + 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; invert(); 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; ivert.size()/3; nskintexvert += skins[i]->texcoord.size()/2; @@ -986,6 +1028,7 @@ void mjCModel::SetSizes(void) { for (int i=0; iname.length() + 1; for (int i=0; iname.length() + 1; for (int i=0; iname.length() + 1; + for (int i=0; iname.length() + 1; for (int i=0; iname.length() + 1; for (int i=0; iname.length() + 1; for (int i=0; iname.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; ibody_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; ibody_bvhadr[i] + m->body_bvhnum[i]); - } for (int i=0; imesh_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; iflex_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; knvert; 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; inedge; 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& 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; jname == 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 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; iCompile(vfs); for (int i=0; iCompile(vfs); for (int i=0; iCompile(vfs); for (int i=0; iCompile(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); diff --git a/src/user/user_model.h b/src/user/user_model.h index 28d24d96..77d9d718 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -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 flexes; // list of flexes std::vector meshes; // list of meshes std::vector skins; // list of skins std::vector hfields; // list of height fields diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index e7de19bc..a392d13d 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -296,34 +296,39 @@ void mjCBoundingVolumeHierarchy::CreateBVH() { // compute bounding volume hierarchy int mjCBoundingVolumeHierarchy::MakeBVH(std::vector& 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& 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])& 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& 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; icontype; + 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; iCompile(); @@ -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); diff --git a/src/user/user_objects.h b/src/user/user_objects.h index efd07bb3..564a3a88 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -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 bvh; // bounding boxes (nbvh x 6) std::vector child; // children of each node (nbvh x 2) - std::vector nodeid; // id of the geom contained by the node (nbvh x 1) + std::vector nodeid; // geom of elem id contained by the node (nbvh x 1) std::vector 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 vertbody; // vertex body names + std::vector vert; // vertex positions + std::vector elem; // element vertex ids + std::vector 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 vertbodyid; // vertex body ids + std::vector> edge; // edge vertex ids + std::vector shell; // shell fragment vertex ids (dim per fragment) + std::vector elemlayer; // element layer (distance from border) + std::vector evpair; // element-vertex pairs + std::vector 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; diff --git a/src/xml/xml_base.h b/src/xml/xml_base.h index 1b2c5c6b..3e853abf 100644 --- a/src/xml/xml_base.h +++ b/src/xml/xml_base.h @@ -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 ------------------------------------------------ diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 52c511d2..84bc1638 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -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 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; diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index d72c8b29..f1170dcf 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -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 diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 93e35a2a..ba5d3fe9 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -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; iGetObject(mjOBJ_SKIN, i); - elem = InsertEnd(section, "skin"); - OneSkin(elem, pskin); - } - // write hfields for (int i=0; iNumObjects(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; iGetObject(mjOBJ_FLEX, i); + elem = InsertEnd(section, "flex"); + OneFlex(elem, pflex); + } + + // write skins + for (int i=0; iGetObject(mjOBJ_SKIN, i); + elem = InsertEnd(section, "skin"); + OneSkin(elem, pskin); + } +} + + + // tendon section void mjXWriter::Tendon(XMLElement* root) { // skip section if empty diff --git a/src/xml/xml_native_writer.h b/src/xml/xml_native_writer.h index c5da1d87..53a67bf4 100644 --- a/src/xml/xml_native_writer.h +++ b/src/xml/xml_native_writer.h @@ -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); diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index 1b29a852..c3abb1f6 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -767,6 +767,42 @@ bool mjXUtil::ReadAttrInt(XMLElement* elem, const char* attr, int* data, bool re +// read vector from string +void mjXUtil::String2Vector(const string& txt, vector& vec) { + stringstream strm(txt); + vec.clear(); + + while (!strm.eof()) { + string word; + strm >> word; + if (strm.fail()) { + break; + } else { + vec.push_back(word); + } + } +} + + + +// read vector from string +void mjXUtil::String2Vector(const string& txt, vector& vec) { + stringstream strm(txt); + vec.clear(); + + while (!strm.eof()) { + double num; + strm >> num; + if (strm.fail()) { + break; + } else { + vec.push_back(num); + } + } +} + + + // read vector from string void mjXUtil::String2Vector(const string& txt, vector& vec) { stringstream strm(txt); @@ -803,6 +839,38 @@ void mjXUtil::String2Vector(const string& txt, vector& vec) { +// write vector to string +void mjXUtil::Vector2String(string& txt, const vector& vec) { + stringstream strm; + + for (size_t i=0; i0) { + strm << " "; + } + strm << vec[i]; + } + + txt = strm.str(); +} + + + +// write vector to string +void mjXUtil::Vector2String(string& txt, const vector& vec) { + stringstream strm; + + for (size_t i=0; i0) { + strm << " "; + } + strm << vec[i]; + } + + txt = strm.str(); +} + + + // write vector to string void mjXUtil::Vector2String(string& txt, const vector& vec) { stringstream strm; diff --git a/src/xml/xml_util.h b/src/xml/xml_util.h index 622d1722..acf5d288 100644 --- a/src/xml/xml_util.h +++ b/src/xml/xml_util.h @@ -160,12 +160,24 @@ class mjXUtil { static bool ReadAttrInt(tinyxml2::XMLElement* elem, const char* attr, int* data, bool required = false); + // read vector from string + static void String2Vector(const std::string& txt, std::vector& vec); + + // read vector from string + static void String2Vector(const std::string& txt, std::vector& vec); + // read vector from string static void String2Vector(const std::string& txt, std::vector& vec); // read vector from string static void String2Vector(const std::string& txt, std::vector& vec); + // write vector to string + static void Vector2String(std::string& txt, const std::vector& vec); + + // write vector to string + static void Vector2String(std::string& txt, const std::vector& vec); + // write vector to string static void Vector2String(std::string& txt, const std::vector& vec); diff --git a/test/engine/engine_collision_driver_test.cc b/test/engine/engine_collision_driver_test.cc index 82b6eee5..7c6700fd 100644 --- a/test/engine/engine_collision_driver_test.cc +++ b/test/engine/engine_collision_driver_test.cc @@ -41,8 +41,8 @@ static std::vector colliding_pairs( const mjModel* model, const mjData* data) { std::vector 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"( + + + + + + + + + + + + )"; + 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 diff --git a/test/user/CMakeLists.txt b/test/user/CMakeLists.txt index 1672d183..eee664bb 100644 --- a/test/user/CMakeLists.txt +++ b/test/user/CMakeLists.txt @@ -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 diff --git a/test/user/testdata/cube_complete.obj b/test/user/testdata/cube_complete.obj new file mode 100644 index 00000000..ef06c983 --- /dev/null +++ b/test/user/testdata/cube_complete.obj @@ -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 diff --git a/test/user/testdata/malformed_flex_nofaces.xml b/test/user/testdata/malformed_flex_nofaces.xml new file mode 100644 index 00000000..61db94bc --- /dev/null +++ b/test/user/testdata/malformed_flex_nofaces.xml @@ -0,0 +1,5 @@ + + + + + diff --git a/test/user/testdata/robot_arm.xml b/test/user/testdata/robot_arm.xml new file mode 100644 index 00000000..b4004de8 --- /dev/null +++ b/test/user/testdata/robot_arm.xml @@ -0,0 +1,157 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc new file mode 100644 index 00000000..7fe5790c --- /dev/null +++ b/test/user/user_flex_test.cc @@ -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 +#include + +#include +#include +#include +#include +#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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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"( + + + + + + + + )"; + std::array 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"( + + + + + + )"; + std::array 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 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 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 diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index ca485d8e..3506a01d 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -631,7 +631,7 @@ TEST_F(XMLReaderTest, InvalidSkinGroup) { )"; std::array 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")); diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index a90ef15e..664cafb5 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -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; } diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 7469600f..d190ce3a 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -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);