diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml index 874039dd..9277140b 100644 --- a/.github/workflows/build.yml +++ b/.github/workflows/build.yml @@ -148,6 +148,7 @@ jobs: cd build && cmake .. -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF -DCMAKE_INSTALL_PREFIX:STRING=${{ matrix.tmpdir }}/mujoco_install -DMUJOCO_BUILD_EXAMPLES:BOOL=OFF ${{ matrix.cmake_args }} @@ -192,6 +193,7 @@ jobs: cd build && cmake .. -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF -Dmujoco_ROOT:STRING=${{ matrix.tmpdir }}/mujoco_install ${{ matrix.cmake_args }} - name: Build samples @@ -204,6 +206,7 @@ jobs: cd build && cmake .. -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_INTERPROCEDURAL_OPTIMIZATION:BOOL=OFF -Dmujoco_ROOT:STRING=${{ matrix.tmpdir }}/mujoco_install ${{ matrix.cmake_args }} - name: Build simulate diff --git a/README.md b/README.md index 7ce14c2d..06c5f29d 100644 --- a/README.md +++ b/README.md @@ -41,7 +41,7 @@ release can be found in the [changelog] in the latest branch. There are two easy ways to get started with MuJoCo: 1. **Run `simulate` on your machine.** -[This video](https://www.youtube.com/watch?v=0ORsj_E17B0) shows a screen capture +[This video](https://www.youtube.com/watch?v=P83tKA1iz2Y) shows a screen capture of `simulate`, MuJoCo's native interactive viewer. Follow the steps described in the [Getting Started] section of the documentation to get `simulate` running on your machine. diff --git a/cmake/MujocoOptions.cmake b/cmake/MujocoOptions.cmake index 3e3f080f..de146d89 100644 --- a/cmake/MujocoOptions.cmake +++ b/cmake/MujocoOptions.cmake @@ -104,6 +104,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang endif() endif() +if(NOT CMAKE_INTERPROCEDURAL_OPTIMIZATION AND (CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE STREQUAL "Debug")) + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) +endif() + include(MujocoHarden) set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS}) set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS}) diff --git a/doc/APIreference/APIglobals.rst b/doc/APIreference/APIglobals.rst index f1ad2c5b..3fef402d 100644 --- a/doc/APIreference/APIglobals.rst +++ b/doc/APIreference/APIglobals.rst @@ -414,12 +414,6 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr - 50 - The maximum depth of each body and mesh bounding volume hierarchy. If this large limit is exceeded, a warning is raised and ray casting may not be possible. For a balanced hierarchy, this implies 1E15 bounding volumes. - * - ``mjMAXVFS`` - - 200 - - The maximal number of characters in the name of each file in the virtual file system. - * - ``mjMAXVFSNAME`` - - 100 - - The maximal number of characters in the name of each file in the virtual file system. * - ``mjNEQDATA`` - 11 - The maximal number of real-valued parameters used to define each equality constraint. Determines the size of @@ -534,6 +528,14 @@ shown in the table below. Their names are in the format ``mjKEY_XXX``. They corr Macros ^^^^^^ + +.. _mjUSESINGLE: + +mjUSESINGLE +~~~~~~~~~~~ + +Compile-time flag, see :ref:`mjtNum`. + .. _mjDISABLED: mjDISABLED diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 5ee07e6a..db4c797e 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -54,24 +54,28 @@ The two types below are defined in `mjtnum.h `. Fixes :github:issue:`364`. +.. admonition:: Breaking API changes + :class: attention + + 1. Removed deprecated ``mj_makeEmptyFileVFS`` and ``mj_findFileVFS`` functions. The constants ``mjMAXVFS`` and + ``mjMAXVFSNAME`` are also removed as they are no longer needed. + + **Migration:** Use :ref:`mj_addBufferVFS` to copy a buffer into a VFS file directly. + + 2. Calls to :ref:`mj_defaultVFS` may allocate memory inside VFS, and the corresponding + :ref:`mj_deleteVFS` must be called to deallocate any internal allocated memory. + + 3. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and + :ref:`mju_mulMatTVec3`. These function names and argument order are more consistent with the rest of the API. + The older functions have been removed from the Python bindings and will be removed from the C API in the next + release. + +4. The :ref:`VFS` implementation has been rewritten in C++ and is now considerably more efficient in + speed and memory footprint. +5. Added a new API for :doc:`procedural model manipulation`. Fixes :github:issue:`364`. Still missing: - Detailed documentation. - - Python bindings. .. youtube:: ZXBTEIDWHhs :align: right :width: 240px -2. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the +6. Added support for orthographic cameras. This is available for both fixed cameras and the free camera, using the :ref:`camera/orthographic` and :ref:`global/orthographic` attributes, respectively. -3. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. -4. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. -5. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). -6. Deprecated :ref:`mju_rotVecMat` and :ref:`mju_rotVecMatT` in favor of :ref:`mju_mulMatVec3` and - :ref:`mju_mulMatTVec3`. These functions names and argument ordering are more consistent with the rest of the API. +7. Added :ref:`maxhullvert`, the maximum number of vertices in a mesh's convex hull. +8. Added :ref:`mj_setKeyframe` for saving the current state into a model keyframe. +9. Added support for ``ball`` joints in the URDF parser ("spherical" in URDF). +10. Replaced ``mjUSEDOUBLE`` which was previously hard-coded in + `mjtnum.h `__ + with the build-time flag ``mjUSESINGLE``. If this symbol is not defined, MuJoCo will use double-precision floating + point, as usual. If ``mjUSESINGLE`` is defined, MuJoCo will use single-precision floating point. See :ref:`mjtNum`. + + Relatedly, fixed various type errors that prevented building with single-precision. +11. Quaternions in ``mjData->qpos`` and ``mjData->mocap_quat`` are no longer normalized in-place by + :ref:`mj_kinematics`. Instead they are normalized when they are used. After the first step, quaternions in + ``mjData->qpos`` will be normalized. MJX ~~~ -7. Added support for :ref:`elliptic friction cones`. -8. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. -9. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. +12. Added support for :ref:`elliptic friction cones`. +13. Fixed a bug that resulted in less-optimal linesearch solutions for some difficult constraint settings. +14. Fixed a bug in the Newton solver that sometimes resulted in less-optimal gradients. + + +.. youtube:: P83tKA1iz2Y + :align: right + :width: 360px + +Simulate +^^^^^^^^ +15. Added improved tutorial video. +16. Improved the Brownian noise generator. + +|br| |br| |br| |br| + +Python bindings +^^^^^^^^^^^^^^^ +17. Fixed a memory leak when using ``copy.deepcopy()`` on a ``mujoco.MjData`` instance (:github:issue:`1572`). Version 3.1.6 (Jun 3, 2024) --------------------------- @@ -199,10 +239,10 @@ General :at:`ctrlrange` or :at:`actrange` (respectively), according to the range of the transmission target (joint or tendon). See :ref:`position/inheritrange` for details. -2. Deprecated :ref:`mj_makeEmptyFileVFS` in favor of :ref:`mj_addBufferVFS`. :ref:`mjVFS` now computes checksums of +2. Deprecated ``mj_makeEmptyFileVFS`` in favor of :ref:`mj_addBufferVFS`. :ref:`mjVFS` now computes checksums of its internal file buffers. :ref:`mj_addBufferVFS` allocates an empty buffer with a given name in an mjVFS and copies the data buffer into it, combining and replacing the deprecated two-step process of calling - :ref:`mj_makeEmptyFileVFS` followed by a direct copy into the given mjVFS internal file buffer. + ``mj_makeEmptyFileVFS`` followed by a direct copy into the given mjVFS internal file buffer. 3. Added :ref:`mj_angmomMat` which computes the ``3 x nv`` angular momentum matrix :math:`H(q)`, providing the linear mapping from generalized velocities to subtree angular momentum :math:`h = H \dot q`. Contribution by :github:user:`v-r-a`. diff --git a/doc/includes/references.h b/doc/includes/references.h index 9bdb0b1f..257bd64f 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -139,29 +139,29 @@ struct mjData_ { int nplugin; // number of plugin instances // stack pointer - size_t pstack; // first available mjtNum address in stack + size_t pstack; // first available byte in stack size_t pbase; // value of pstack when mj_markStack was last called // arena pointer size_t parena; // first available byte in arena - // memory utilization stats - size_t maxuse_stack; // maximum stack allocation in bytes - size_t maxuse_threadstack[mjMAXTHREAD]; // maximum stack allocation per thread in bytes - size_t maxuse_arena; // maximum arena allocation in bytes - int maxuse_con; // maximum number of contacts - int maxuse_efc; // maximum number of scalar constraints - - // diagnostics - mjWarningStat warning[mjNWARNING]; // warning statistics - mjTimerStat timer[mjNTIMER]; // timer statistics + // memory utilization statistics + size_t maxuse_stack; // maximum stack allocation in bytes + size_t maxuse_threadstack[mjMAXTHREAD]; // maximum stack allocation per thread in bytes + size_t maxuse_arena; // maximum arena allocation in bytes + int maxuse_con; // maximum number of contacts + int maxuse_efc; // maximum number of scalar constraints // solver statistics mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration - int solver_nisland; // number of islands processed by solver - int solver_niter[mjNISLAND]; // number of solver iterations, per island - int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island - mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + int solver_nisland; // number of islands processed by solver + int solver_niter[mjNISLAND]; // number of solver iterations, per island + int solver_nnz[mjNISLAND]; // number of nonzeros in Hessian or efc_AR, per island + mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // diagnostics + mjWarningStat warning[mjNWARNING]; // warning statistics + mjTimerStat timer[mjNTIMER]; // timer statistics // variable sizes int ne; // number of equality constraints @@ -179,8 +179,8 @@ struct mjData_ { //-------------------- end of info header // buffers - void* buffer; // main buffer; all pointers point in it (nbuffer bytes) - void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) + void* buffer; // main buffer; all pointers point in it (nbuffer bytes) + void* arena; // arena+stack buffer (narena bytes) //-------------------- main inputs and outputs of the computation @@ -274,7 +274,7 @@ struct mjData_ { // 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) + mjtByte* bvh_active; // was bounding volume checked for collision (nbvh x 1) //-------------------- POSITION, VELOCITY dependent @@ -344,7 +344,7 @@ struct mjData_ { //-------------------- arena-allocated: POSITION dependent // computed by mj_collision - mjContact* contact; // list of all detected contacts (ncon x 1) + mjContact* contact; // array of all detected contacts (ncon x 1) // computed by mj_makeConstraint int* efc_type; // constraint type (mjtConstraint) (nefc x 1) @@ -394,11 +394,11 @@ struct mjData_ { //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse - mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) - mjtNum* efc_force; // constraint force in constraint space (nefc x 1) - int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) + mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) + mjtNum* efc_force; // constraint force in constraint space (nefc x 1) + int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) - // ThreadPool for multithreaded operations + // thread pool pointer uintptr_t threadpool; }; typedef struct mjData_ mjData; @@ -699,11 +699,7 @@ struct mjLROpt_ { // options for mj_setLengthRange() }; typedef struct mjLROpt_ mjLROpt; struct mjVFS_ { // virtual file system for loading from memory - int nfile; // number of files present - char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path - size_t filesize[mjMAXVFS]; // file size in bytes - void* filedata[mjMAXVFS]; // buffer with file data - uint64_t filestamp[mjMAXVFS]; // checksum of the file data + void* impl_; // internal pointer to VFS memory }; typedef struct mjVFS_ mjVFS; struct mjOption_ { // physics options @@ -1220,7 +1216,7 @@ struct mjModel_ { int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1) // materials - int* mat_texid; // texture id; -1: none (nmat x 1) + int* mat_texid; // indices of textures; -1: none (nmat x mjNTEXMAT) mjtByte* mat_texuniform; // make texture cube uniform (nmat x 1) float* mat_texrepeat; // texture repetition for 2d mapping (nmat x 2) float* mat_emission; // emission (x rgb) (nmat x 1) @@ -1512,92 +1508,97 @@ struct mjrRect_ { // OpenGL rectangle int height; // height (usually buffer height) }; typedef struct mjrRect_ mjrRect; -struct mjrContext_ { // custom OpenGL context +struct mjrContext_ { // custom OpenGL context // parameters copied from mjVisual - float lineWidth; // line width for wireframe rendering - float shadowClip; // clipping radius for directional lights - float shadowScale; // fraction of light cutoff for spot lights - float fogStart; // fog start = stat.extent * vis.map.fogstart - float fogEnd; // fog end = stat.extent * vis.map.fogend - float fogRGBA[4]; // fog rgba - int shadowSize; // size of shadow map texture - int offWidth; // width of offscreen buffer - int offHeight; // height of offscreen buffer - int offSamples; // number of offscreen buffer multisamples + float lineWidth; // line width for wireframe rendering + float shadowClip; // clipping radius for directional lights + float shadowScale; // fraction of light cutoff for spot lights + float fogStart; // fog start = stat.extent * vis.map.fogstart + float fogEnd; // fog end = stat.extent * vis.map.fogend + float fogRGBA[4]; // fog rgba + int shadowSize; // size of shadow map texture + int offWidth; // width of offscreen buffer + int offHeight; // height of offscreen buffer + int offSamples; // number of offscreen buffer multisamples // parameters specified at creation - int fontScale; // font scale - int auxWidth[mjNAUX]; // auxiliary buffer width - int auxHeight[mjNAUX]; // auxiliary buffer height - int auxSamples[mjNAUX]; // auxiliary buffer multisamples + int fontScale; // font scale + int auxWidth[mjNAUX]; // auxiliary buffer width + int auxHeight[mjNAUX]; // auxiliary buffer height + int auxSamples[mjNAUX]; // auxiliary buffer multisamples // offscreen rendering objects - unsigned int offFBO; // offscreen framebuffer object - unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples - unsigned int offColor; // offscreen color buffer - unsigned int offColor_r; // offscreen color buffer for resolving multisamples - unsigned int offDepthStencil; // offscreen depth and stencil buffer - unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for resolving multisamples + unsigned int offFBO; // offscreen framebuffer object + unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples + unsigned int offColor; // offscreen color buffer + unsigned int offColor_r; // offscreen color buffer for resolving multisamples + unsigned int offDepthStencil; // offscreen depth and stencil buffer + unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for multisamples // shadow rendering objects - unsigned int shadowFBO; // shadow map framebuffer object - unsigned int shadowTex; // shadow map texture + unsigned int shadowFBO; // shadow map framebuffer object + unsigned int shadowTex; // shadow map texture // auxiliary buffers - unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object - unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving - unsigned int auxColor[mjNAUX]; // auxiliary color buffer - unsigned int auxColor_r[mjNAUX];// auxiliary color buffer for resolving + unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object + unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving + unsigned int auxColor[mjNAUX]; // auxiliary color buffer + unsigned int auxColor_r[mjNAUX]; // auxiliary color buffer for resolving + + // materials with textures + int mat_texid[mjMAXMATERIAL*mjNTEXMAT]; // material texture ids (-1: no texture) + int mat_texuniform[mjMAXMATERIAL]; // texture repetition for 2d mapping + int mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping // texture objects and info - int ntexture; // number of allocated textures - int textureType[100]; // type of texture (mjtTexture) (ntexture) - unsigned int texture[100]; // texture names + int ntexture; // number of allocated textures + int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) + unsigned int texture[mjMAXTEXTURE]; // texture names // displaylist starting positions - unsigned int basePlane; // all planes from model - unsigned int baseMesh; // all meshes from model - unsigned int baseHField; // all hfields from model - unsigned int baseBuiltin; // all buildin geoms, with quality from model - unsigned int baseFontNormal; // normal font - unsigned int baseFontShadow; // shadow font - unsigned int baseFontBig; // big font + unsigned int basePlane; // all planes from model + unsigned int baseMesh; // all meshes from model + unsigned int baseHField; // all height fields from model + unsigned int baseBuiltin; // all builtin geoms, with quality from model + unsigned int baseFontNormal; // normal font + unsigned int baseFontShadow; // shadow font + unsigned int baseFontBig; // big font // displaylist ranges - int rangePlane; // all planes from model - int rangeMesh; // all meshes from model - int rangeHField; // all hfields from model - int rangeBuiltin; // all builtin geoms, with quality from model - int rangeFont; // all characters in font + int rangePlane; // all planes from model + int rangeMesh; // all meshes from model + int rangeHField; // all hfields from model + int rangeBuiltin; // all builtin geoms, with quality from model + int rangeFont; // all characters in font // skin VBOs - int nskin; // number of skins - unsigned int* skinvertVBO; // skin vertex position VBOs (nskin) - unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin) - unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) - unsigned int* skinfaceVBO; // skin face index VBOs (nskin) + int nskin; // number of skins + unsigned int* skinvertVBO; // skin vertex position VBOs (nskin) + unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin) + unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) + unsigned int* skinfaceVBO; // skin face index VBOs (nskin) // character info - int charWidth[127]; // character widths: normal and shadow - int charWidthBig[127]; // chacarter widths: big - int charHeight; // character heights: normal and shadow - int charHeightBig; // character heights: big + int charWidth[127]; // character widths: normal and shadow + int charWidthBig[127]; // chacarter widths: big + int charHeight; // character heights: normal and shadow + int charHeightBig; // character heights: big // capabilities - int glInitialized; // is OpenGL initialized - int windowAvailable; // is default/window framebuffer available - int windowSamples; // number of samples for default/window framebuffer - int windowStereo; // is stereo available for default/window framebuffer - int windowDoublebuffer; // is default/window framebuffer double buffered + int glInitialized; // is OpenGL initialized + int windowAvailable; // is default/window framebuffer available + int windowSamples; // number of samples for default/window framebuffer + int windowStereo; // is stereo available for default/window framebuffer + int windowDoublebuffer; // is default/window framebuffer double buffered // framebuffer - int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN // pixel output format - int readPixelFormat; // default color pixel format for mjr_readPixels + int readPixelFormat; // default color pixel format for mjr_readPixels // depth output format - int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR }; typedef struct mjrContext_ mjrContext; typedef enum mjtGeomInertia_ { // type of inertia inference @@ -1649,7 +1650,7 @@ typedef struct mjSpec_ { // model specification mjtByte strippath; // automatically strip paths from mesh files mjtByte fitaabb; // meshfit to aabb instead of inertia box mjtByte degree; // angles in radians or degrees - char euler[3]; // sequence for euler rotations + char eulerseq[3]; // sequence for euler rotations mjString* meshdir; // mesh and hfield directory mjString* texturedir; // texture directory mjtByte discardvisual; // discard visual geoms in parser @@ -1743,7 +1744,6 @@ typedef struct mjsFrame_ { // frame specification typedef struct mjsJoint_ { // joint specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtJoint type; // joint type // kinematics @@ -1781,7 +1781,6 @@ typedef struct mjsJoint_ { // joint specification typedef struct mjsGeom_ { // geom specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // classname mjtGeom type; // geom type // frame, size @@ -1828,7 +1827,6 @@ typedef struct mjsGeom_ { // geom specification typedef struct mjsSite_ { // site specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // frame, size double pos[3]; // position @@ -1850,7 +1848,6 @@ typedef struct mjsSite_ { // site specification typedef struct mjsCamera_ { // camera specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // extrinsics double pos[3]; // position @@ -1878,7 +1875,6 @@ typedef struct mjsCamera_ { // camera specification typedef struct mjsLight_ { // light specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // frame double pos[3]; // position @@ -1904,7 +1900,6 @@ typedef struct mjsLight_ { // light specification typedef struct mjsFlex_ { // flex specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // contact properties int contype; // contact type @@ -1943,7 +1938,6 @@ typedef struct mjsFlex_ { // flex specification typedef struct mjsMesh_ { // mesh specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* content_type; // content type of file mjString* file; // mesh file double refpos[3]; // reference position @@ -1974,7 +1968,6 @@ typedef struct mjsHField_ { // height field specification typedef struct mjsSkin_ { // skin specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* file; // skin file mjString* material; // name of material used for rendering float rgba[4]; // rgba when material is omitted @@ -1999,7 +1992,6 @@ typedef struct mjsSkin_ { // skin specification typedef struct mjsTexture_ { // texture specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtTexture type; // texture type // method 1: builtin @@ -2031,7 +2023,6 @@ typedef struct mjsTexture_ { // texture specification typedef struct mjsMaterial_ { // material specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* texture; // name of texture (empty: none) mjtByte texuniform; // make texture cube uniform float texrepeat[2]; // texture repetition for 2D mapping @@ -2047,7 +2038,6 @@ typedef struct mjsMaterial_ { // material specification typedef struct mjsPair_ { // pair specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* geomname1; // name of geom 1 mjString* geomname2; // name of geom 2 @@ -2071,7 +2061,6 @@ typedef struct mjsExclude_ { // exclude specification typedef struct mjsEquality_ { // equality specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtEq type; // constraint type double data[mjNEQDATA]; // type-dependent data mjtByte active; // is equality initially active @@ -2084,7 +2073,6 @@ typedef struct mjsEquality_ { // equality specification typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // stiffness, damping, friction double stiffness; // stiffness coefficient @@ -2118,7 +2106,6 @@ typedef struct mjsWrap_ { // wrapping object specification typedef struct mjsActuator_ { // actuator specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // gain, bias mjtGain gaintype; // gain type @@ -2160,7 +2147,6 @@ typedef struct mjsActuator_ { // actuator specification typedef struct mjsSensor_ { // sensor specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // sensor definition mjtSensor type; // type of sensor @@ -2286,6 +2272,11 @@ typedef enum mjtItem_ { // UI item type mjNITEM // number of item types } mjtItem; +typedef enum mjtSection_ { // UI section state + mjSECT_CLOSED = 0, // closed state (regular section) + mjSECT_OPEN, // open state (regular section) + mjSECT_FIXED // fixed section: always open, no title +} mjtSection; struct mjuiState_ { // mouse and keyboard state // constants set by user int nrect; // number of rectangles used @@ -2333,6 +2324,8 @@ struct mjuiThemeSpacing_ { // UI visualization theme spacing int scroll; // scrollbar width int label; // label width int section; // section gap + int cornersect; // corner radius for section + int cornersep; // corner radius for separator int itemside; // item side gap int itemmid; // item middle gap int itemver; // item vertical gap @@ -2346,9 +2339,14 @@ struct mjuiThemeColor_ { // UI visualization theme color float master[3]; // master background float thumb[3]; // scrollbar thumb float secttitle[3]; // section title + float secttitle2[3]; // section title: bottom color + float secttitlecheck[3]; // section title with checkbox + float secttitlecheck2[3]; // section title with checkbox: bottom color float sectfont[3]; // section font float sectsymbol[3]; // section symbol float sectpane[3]; // section pane + float separator[3]; // separator title + float separator2[3]; // separator title: bottom color float shortcut[3]; // shortcut background float fontactive[3]; // font active float fontinactive[3]; // font inactive @@ -2398,6 +2396,7 @@ struct mjuiItem_ { // UI item void *pdata; // data pointer (type-specific) int sectionid; // id of section containing item int itemid; // id of item within section + int userid; // user-supplied id (for event handling) // type-specific properties union { @@ -2409,20 +2408,23 @@ struct mjuiItem_ { // UI item // internal mjrRect rect; // rectangle occupied by item + int skip; // item skipped due to closed separator }; typedef struct mjuiItem_ mjuiItem; struct mjuiSection_ { // UI section // properties char name[mjMAXUINAME]; // name - int state; // 0: closed, 1: open + int state; // section state (mjtSection) int modifier; // 0: none, 1: control, 2: shift; 4: alt int shortcut; // shortcut key; 0: undefined + int checkbox; // 0: none, 1: hidden, 2: unchecked, 2: checked int nitem; // number of items in use mjuiItem item[mjMAXUIITEM]; // preallocated array of items // internal mjrRect rtitle; // rectangle occupied by title mjrRect rcontent; // rectangle occupied by content + int lastclick; // last mouse click over this section }; typedef struct mjuiSection_ mjuiSection; struct mjUI_ { // entire UI @@ -2441,10 +2443,12 @@ struct mjUI_ { // entire UI int maxheight; // height when all sections open int scroll; // scroll from top of UI - // mouse focus + // mouse focus and count int mousesect; // 0: none, -1: scroll, otherwise 1+section int mouseitem; // item within section int mousehelp; // help button down: print shortcuts + int mouseclicks; // number of mouse clicks over UI + int mousesectcheck; // 0: none, otherwise 1+section // keyboard focus and edit int editsect; // 0: none, otherwise 1+section @@ -2465,6 +2469,7 @@ struct mjuiDef_ { // table passed to mjui_add() int state; // state void* pdata; // pointer to data char other[mjMAXUITEXT]; // string with type-specific properties + int otherint; // int with type-specific properties }; typedef struct mjuiDef_ mjuiDef; typedef enum mjtCatBit_ { // bitflags for mjvGeom category @@ -2635,21 +2640,22 @@ struct mjvGeom_ { // abstract geom int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor int objid; // mujoco object id; -1 for decor int category; // visual category - int texid; // texture id; -1: no texture - int texuniform; // uniform cube mapping + int matid; // material id; -1: no textured material int texcoord; // mesh or flex geom has texture coordinates int segid; // segmentation id; -1: not shown - // OpenGL info - float texrepeat[2]; // texture repetition for 2D mapping + // spatial transform float size[3]; // size parameters float pos[3]; // Cartesian position float mat[9]; // Cartesian orientation + + // material properties float rgba[4]; // color and transparency float emission; // emission coef float specular; // specular coef float shininess; // shininess coef float reflectance; // reflectance coef + char label[100]; // text label // transparency rendering (set internally) @@ -2782,7 +2788,7 @@ struct mjvFigure_ { // abstract 2D figure passed to OpenGL rendere // line data int linepnt[mjMAXLINE]; // number of points in line; (0) disable - float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) + float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) // output from renderer int xaxispixel[2]; // range of x-axis in pixels @@ -3068,10 +3074,8 @@ typedef struct mjvSceneState_ mjvSceneState; void mj_defaultVFS(mjVFS* vfs); int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); -int mj_findFileVFS(const mjVFS* vfs, const char* filename); int mj_deleteFileVFS(mjVFS* vfs, const char* filename); void mj_deleteVFS(mjVFS* vfs); -int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); mjSpec* mj_parseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); mjSpec* mj_parseXMLString(const char* xml, const mjVFS* vfs, char* error, int error_sz); @@ -3525,7 +3529,7 @@ mjsText* mjs_addText(mjSpec* s); mjsTuple* mjs_addTuple(mjSpec* s); mjsKey* mjs_addKey(mjSpec* s); mjsPlugin* mjs_addPlugin(mjSpec* s); -mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id); +mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); mjsMesh* mjs_addMesh(mjSpec* s, mjsDefault* def); mjsHField* mjs_addHField(mjSpec* s); mjsSkin* mjs_addSkin(mjSpec* s); @@ -3536,12 +3540,38 @@ mjsBody* mjs_findBody(mjSpec* s, const char* name); mjsBody* mjs_findChild(mjsBody* body, const char* name); mjsMesh* mjs_findMesh(mjSpec* s, const char* name); mjsFrame* mjs_findFrame(mjSpec* s, const char* name); +mjsKey* mjs_findKeyframe(mjSpec* s, const char* name); mjsDefault* mjs_getDefault(mjsElement* element); mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); mjsDefault* mjs_getSpecDefault(mjSpec* s); int mjs_getId(mjsElement* element); mjsElement* mjs_firstChild(mjsBody* body, mjtObj type); mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child); +mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); +mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); +mjsBody* mjs_asBody(mjsElement* element); +mjsGeom* mjs_asGeom(mjsElement* element); +mjsJoint* mjs_asJoint(mjsElement* element); +mjsSite* mjs_asSite(mjsElement* element); +mjsCamera* mjs_asCamera(mjsElement* element); +mjsLight* mjs_asLight(mjsElement* element); +mjsFrame* mjs_asFrame(mjsElement* element); +mjsActuator* mjs_asActuator(mjsElement* element); +mjsSensor* mjs_asSensor(mjsElement* element); +mjsFlex* mjs_asFlex(mjsElement* element); +mjsPair* mjs_asPair(mjsElement* element); +mjsEquality* mjs_asEquality(mjsElement* element); +mjsExclude* mjs_asExclude(mjsElement* element); +mjsTendon* mjs_asTendon(mjsElement* element); +mjsNumeric* mjs_asNumeric(mjsElement* element); +mjsText* mjs_asText(mjsElement* element); +mjsTuple* mjs_asTuple(mjsElement* element); +mjsKey* mjs_asKey(mjsElement* element); +mjsMesh* mjs_asMesh(mjsElement* element); +mjsHField* mjs_asHField(mjsElement* element); +mjsSkin* mjs_asSkin(mjsElement* element); +mjsTexture* mjs_asTexture(mjsElement* element); +mjsMaterial* mjs_asMaterial(mjsElement* element); void mjs_setString(mjString* dest, const char* text); void mjs_setStringVec(mjStringVec* dest, const char* text); mjtByte mjs_setInStringVec(mjStringVec* dest, int i, const char* text); @@ -3559,7 +3589,6 @@ void mjs_setDefault(mjsElement* element, mjsDefault* def); void mjs_setFrame(mjsElement* dest, mjsFrame* frame); const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); void mjs_defaultSpec(mjSpec* spec); void mjs_defaultOrientation(mjsOrientation* orient); void mjs_defaultBody(mjsBody* body); diff --git a/doc/programming/samples.rst b/doc/programming/samples.rst index 92987a49..40cbd434 100644 --- a/doc/programming/samples.rst +++ b/doc/programming/samples.rst @@ -84,7 +84,7 @@ data plots. The model file can be specified as a command-line argument, or loade functionality. This code sample uses the native UI to render various controls, and provides an illustration of how the new UI framework is intended to be used. Below is a screen-capture of ``simulate`` in action: -.. youtube:: 0ORsj_E17B0 +.. youtube:: P83tKA1iz2Y :width: 95% :align: center @@ -94,7 +94,7 @@ selected object by holding Ctrl and dragging the mouse. Dragging the mouse alone are keyboard shortcuts for pausing the simulation, resetting, and re-loading the model file. The latter functionality is very useful while editing the model in an XML editor. -The code is quite long yet reasonably commented, so it is best to just read it. Here we provide a high-level overview. +The code is long yet reasonably commented, so it is best to just read it. Here we provide a high-level overview. The ``main()`` function initializes both MuJoCo and GLFW, opens a window, and install GLFW callbacks for mouse and keyboard handling. Note that there is no render callback; GLFW puts the user in charge, instead of running a rendering loop behind the scenes. The main loop handles UI events and rendering. The simulation is handled in a background diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index 344c688e..45c8229e 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -167,29 +167,29 @@ struct mjData_ { int nplugin; // number of plugin instances // stack pointer - size_t pstack; // first available mjtNum address in stack + size_t pstack; // first available byte in stack size_t pbase; // value of pstack when mj_markStack was last called // arena pointer size_t parena; // first available byte in arena - // memory utilization stats - size_t maxuse_stack; // maximum stack allocation in bytes - size_t maxuse_threadstack[mjMAXTHREAD]; // maximum stack allocation per thread in bytes - size_t maxuse_arena; // maximum arena allocation in bytes - int maxuse_con; // maximum number of contacts - int maxuse_efc; // maximum number of scalar constraints - - // diagnostics - mjWarningStat warning[mjNWARNING]; // warning statistics - mjTimerStat timer[mjNTIMER]; // timer statistics + // memory utilization statistics + size_t maxuse_stack; // maximum stack allocation in bytes + size_t maxuse_threadstack[mjMAXTHREAD]; // maximum stack allocation per thread in bytes + size_t maxuse_arena; // maximum arena allocation in bytes + int maxuse_con; // maximum number of contacts + int maxuse_efc; // maximum number of scalar constraints // solver statistics mjSolverStat solver[mjNISLAND*mjNSOLVER]; // solver statistics per island, per iteration - int solver_nisland; // number of islands processed by solver - int solver_niter[mjNISLAND]; // number of solver iterations, per island - int solver_nnz[mjNISLAND]; // number of non-zeros in Hessian or efc_AR, per island - mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + int solver_nisland; // number of islands processed by solver + int solver_niter[mjNISLAND]; // number of solver iterations, per island + int solver_nnz[mjNISLAND]; // number of nonzeros in Hessian or efc_AR, per island + mjtNum solver_fwdinv[2]; // forward-inverse comparison: qfrc, efc + + // diagnostics + mjWarningStat warning[mjNWARNING]; // warning statistics + mjTimerStat timer[mjNTIMER]; // timer statistics // variable sizes int ne; // number of equality constraints @@ -207,8 +207,8 @@ struct mjData_ { //-------------------- end of info header // buffers - void* buffer; // main buffer; all pointers point in it (nbuffer bytes) - void* arena; // arena+stack buffer (nstack*sizeof(mjtNum) bytes) + void* buffer; // main buffer; all pointers point in it (nbuffer bytes) + void* arena; // arena+stack buffer (narena bytes) //-------------------- main inputs and outputs of the computation @@ -302,7 +302,7 @@ struct mjData_ { // 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) + mjtByte* bvh_active; // was bounding volume checked for collision (nbvh x 1) //-------------------- POSITION, VELOCITY dependent @@ -372,7 +372,7 @@ struct mjData_ { //-------------------- arena-allocated: POSITION dependent // computed by mj_collision - mjContact* contact; // list of all detected contacts (ncon x 1) + mjContact* contact; // array of all detected contacts (ncon x 1) // computed by mj_makeConstraint int* efc_type; // constraint type (mjtConstraint) (nefc x 1) @@ -422,11 +422,11 @@ struct mjData_ { //-------------------- arena-allocated: POSITION, VELOCITY, CONTROL/ACCELERATION dependent // computed by mj_fwdConstraint/mj_inverse - mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) - mjtNum* efc_force; // constraint force in constraint space (nefc x 1) - int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) + mjtNum* efc_b; // linear cost term: J*qacc_smooth - aref (nefc x 1) + mjtNum* efc_force; // constraint force in constraint space (nefc x 1) + int* efc_state; // constraint state (mjtConstraintState) (nefc x 1) - // ThreadPool for multithreaded operations + // thread pool pointer uintptr_t threadpool; }; typedef struct mjData_ mjData; diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 72999a31..2bafdd13 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -29,8 +29,6 @@ #define mjMAXIMP 0.9999 // maximum constraint impedance #define mjMAXCONPAIR 50 // maximum number of contacts per geom pair #define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth -#define mjMAXVFS 2000 // maximum number of files in virtual file system -#define mjMAXVFSNAME 1000 // maximum filename size in virtual file system //---------------------------------- sizes --------------------------------------------------------- @@ -44,7 +42,7 @@ #define mjNIMP 5 // number of solver impedance parameters #define mjNSOLVER 200 // size of one mjData.solver array #define mjNISLAND 20 // number of mjData.solver arrays - +#define mjNTEXMAT 6 // number of textures per material //---------------------------------- enum types (mjt) ---------------------------------------------- @@ -399,11 +397,7 @@ typedef struct mjLROpt_ mjLROpt; //---------------------------------- mjVFS --------------------------------------------------------- struct mjVFS_ { // virtual file system for loading from memory - int nfile; // number of files present - char filename[mjMAXVFS][mjMAXVFSNAME]; // file name without path - size_t filesize[mjMAXVFS]; // file size in bytes - void* filedata[mjMAXVFS]; // buffer with file data - uint64_t filestamp[mjMAXVFS]; // checksum of the file data + void* impl_; // internal pointer to VFS memory }; typedef struct mjVFS_ mjVFS; @@ -935,7 +929,7 @@ struct mjModel_ { int* tex_pathadr; // address of texture asset path; -1: none (ntex x 1) // materials - int* mat_texid; // texture id; -1: none (nmat x 1) + int* mat_texid; // indices of textures; -1: none (nmat x mjNTEXMAT) mjtByte* mat_texuniform; // make texture cube uniform (nmat x 1) float* mat_texrepeat; // texture repetition for 2d mapping (nmat x 2) float* mat_emission; // emission (x rgb) (nmat x 1) diff --git a/include/mujoco/mjrender.h b/include/mujoco/mjrender.h index 4a4089d7..098cfa0e 100644 --- a/include/mujoco/mjrender.h +++ b/include/mujoco/mjrender.h @@ -15,14 +15,15 @@ #ifndef MUJOCO_MJRENDER_H_ #define MUJOCO_MJRENDER_H_ +#include #if defined(__cplusplus) extern "C" { #endif #define mjNAUX 10 // number of auxiliary buffers -#define mjMAXTEXTURE 1000 // maximum number of textures - +#define mjMAXTEXTURE 100 // maximum number of textures +#define mjMAXMATERIAL 100 // maximum number of materials with textures //---------------------------------- primitive types (mjt) ----------------------------------------- @@ -76,92 +77,97 @@ typedef struct mjrRect_ mjrRect; //---------------------------------- mjrContext ---------------------------------------------------- -struct mjrContext_ { // custom OpenGL context +struct mjrContext_ { // custom OpenGL context // parameters copied from mjVisual - float lineWidth; // line width for wireframe rendering - float shadowClip; // clipping radius for directional lights - float shadowScale; // fraction of light cutoff for spot lights - float fogStart; // fog start = stat.extent * vis.map.fogstart - float fogEnd; // fog end = stat.extent * vis.map.fogend - float fogRGBA[4]; // fog rgba - int shadowSize; // size of shadow map texture - int offWidth; // width of offscreen buffer - int offHeight; // height of offscreen buffer - int offSamples; // number of offscreen buffer multisamples + float lineWidth; // line width for wireframe rendering + float shadowClip; // clipping radius for directional lights + float shadowScale; // fraction of light cutoff for spot lights + float fogStart; // fog start = stat.extent * vis.map.fogstart + float fogEnd; // fog end = stat.extent * vis.map.fogend + float fogRGBA[4]; // fog rgba + int shadowSize; // size of shadow map texture + int offWidth; // width of offscreen buffer + int offHeight; // height of offscreen buffer + int offSamples; // number of offscreen buffer multisamples // parameters specified at creation - int fontScale; // font scale - int auxWidth[mjNAUX]; // auxiliary buffer width - int auxHeight[mjNAUX]; // auxiliary buffer height - int auxSamples[mjNAUX]; // auxiliary buffer multisamples + int fontScale; // font scale + int auxWidth[mjNAUX]; // auxiliary buffer width + int auxHeight[mjNAUX]; // auxiliary buffer height + int auxSamples[mjNAUX]; // auxiliary buffer multisamples // offscreen rendering objects - unsigned int offFBO; // offscreen framebuffer object - unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples - unsigned int offColor; // offscreen color buffer - unsigned int offColor_r; // offscreen color buffer for resolving multisamples - unsigned int offDepthStencil; // offscreen depth and stencil buffer - unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for resolving multisamples + unsigned int offFBO; // offscreen framebuffer object + unsigned int offFBO_r; // offscreen framebuffer for resolving multisamples + unsigned int offColor; // offscreen color buffer + unsigned int offColor_r; // offscreen color buffer for resolving multisamples + unsigned int offDepthStencil; // offscreen depth and stencil buffer + unsigned int offDepthStencil_r; // offscreen depth and stencil buffer for multisamples // shadow rendering objects - unsigned int shadowFBO; // shadow map framebuffer object - unsigned int shadowTex; // shadow map texture + unsigned int shadowFBO; // shadow map framebuffer object + unsigned int shadowTex; // shadow map texture // auxiliary buffers - unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object - unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving - unsigned int auxColor[mjNAUX]; // auxiliary color buffer - unsigned int auxColor_r[mjNAUX];// auxiliary color buffer for resolving + unsigned int auxFBO[mjNAUX]; // auxiliary framebuffer object + unsigned int auxFBO_r[mjNAUX]; // auxiliary framebuffer object for resolving + unsigned int auxColor[mjNAUX]; // auxiliary color buffer + unsigned int auxColor_r[mjNAUX]; // auxiliary color buffer for resolving + + // materials with textures + int mat_texid[mjMAXMATERIAL*mjNTEXMAT]; // material texture ids (-1: no texture) + int mat_texuniform[mjMAXMATERIAL]; // texture repetition for 2d mapping + int mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping // texture objects and info - int ntexture; // number of allocated textures - int textureType[100]; // type of texture (mjtTexture) (ntexture) - unsigned int texture[100]; // texture names + int ntexture; // number of allocated textures + int textureType[mjMAXTEXTURE]; // type of texture (mjtTexture) (ntexture) + unsigned int texture[mjMAXTEXTURE]; // texture names // displaylist starting positions - unsigned int basePlane; // all planes from model - unsigned int baseMesh; // all meshes from model - unsigned int baseHField; // all hfields from model - unsigned int baseBuiltin; // all buildin geoms, with quality from model - unsigned int baseFontNormal; // normal font - unsigned int baseFontShadow; // shadow font - unsigned int baseFontBig; // big font + unsigned int basePlane; // all planes from model + unsigned int baseMesh; // all meshes from model + unsigned int baseHField; // all height fields from model + unsigned int baseBuiltin; // all builtin geoms, with quality from model + unsigned int baseFontNormal; // normal font + unsigned int baseFontShadow; // shadow font + unsigned int baseFontBig; // big font // displaylist ranges - int rangePlane; // all planes from model - int rangeMesh; // all meshes from model - int rangeHField; // all hfields from model - int rangeBuiltin; // all builtin geoms, with quality from model - int rangeFont; // all characters in font + int rangePlane; // all planes from model + int rangeMesh; // all meshes from model + int rangeHField; // all hfields from model + int rangeBuiltin; // all builtin geoms, with quality from model + int rangeFont; // all characters in font // skin VBOs - int nskin; // number of skins - unsigned int* skinvertVBO; // skin vertex position VBOs (nskin) - unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin) - unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) - unsigned int* skinfaceVBO; // skin face index VBOs (nskin) + int nskin; // number of skins + unsigned int* skinvertVBO; // skin vertex position VBOs (nskin) + unsigned int* skinnormalVBO; // skin vertex normal VBOs (nskin) + unsigned int* skintexcoordVBO; // skin vertex texture coordinate VBOs (nskin) + unsigned int* skinfaceVBO; // skin face index VBOs (nskin) // character info - int charWidth[127]; // character widths: normal and shadow - int charWidthBig[127]; // chacarter widths: big - int charHeight; // character heights: normal and shadow - int charHeightBig; // character heights: big + int charWidth[127]; // character widths: normal and shadow + int charWidthBig[127]; // chacarter widths: big + int charHeight; // character heights: normal and shadow + int charHeightBig; // character heights: big // capabilities - int glInitialized; // is OpenGL initialized - int windowAvailable; // is default/window framebuffer available - int windowSamples; // number of samples for default/window framebuffer - int windowStereo; // is stereo available for default/window framebuffer - int windowDoublebuffer; // is default/window framebuffer double buffered + int glInitialized; // is OpenGL initialized + int windowAvailable; // is default/window framebuffer available + int windowSamples; // number of samples for default/window framebuffer + int windowStereo; // is stereo available for default/window framebuffer + int windowDoublebuffer; // is default/window framebuffer double buffered // framebuffer - int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN // pixel output format - int readPixelFormat; // default color pixel format for mjr_readPixels + int readPixelFormat; // default color pixel format for mjr_readPixels // depth output format - int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR }; typedef struct mjrContext_ mjrContext; diff --git a/include/mujoco/mjspec.h b/include/mujoco/mjspec.h index 91f284c7..2b92e819 100644 --- a/include/mujoco/mjspec.h +++ b/include/mujoco/mjspec.h @@ -118,7 +118,7 @@ typedef struct mjSpec_ { // model specification mjtByte strippath; // automatically strip paths from mesh files mjtByte fitaabb; // meshfit to aabb instead of inertia box mjtByte degree; // angles in radians or degrees - char euler[3]; // sequence for euler rotations + char eulerseq[3]; // sequence for euler rotations mjString* meshdir; // mesh and hfield directory mjString* texturedir; // texture directory mjtByte discardvisual; // discard visual geoms in parser @@ -222,7 +222,6 @@ typedef struct mjsFrame_ { // frame specification typedef struct mjsJoint_ { // joint specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtJoint type; // joint type // kinematics @@ -262,7 +261,6 @@ typedef struct mjsJoint_ { // joint specification typedef struct mjsGeom_ { // geom specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // classname mjtGeom type; // geom type // frame, size @@ -311,7 +309,6 @@ typedef struct mjsGeom_ { // geom specification typedef struct mjsSite_ { // site specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // frame, size double pos[3]; // position @@ -335,7 +332,6 @@ typedef struct mjsSite_ { // site specification typedef struct mjsCamera_ { // camera specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // extrinsics double pos[3]; // position @@ -365,7 +361,6 @@ typedef struct mjsCamera_ { // camera specification typedef struct mjsLight_ { // light specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // frame double pos[3]; // position @@ -393,7 +388,6 @@ typedef struct mjsLight_ { // light specification typedef struct mjsFlex_ { // flex specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // contact properties int contype; // contact type @@ -434,7 +428,6 @@ typedef struct mjsFlex_ { // flex specification typedef struct mjsMesh_ { // mesh specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* content_type; // content type of file mjString* file; // mesh file double refpos[3]; // reference position @@ -470,7 +463,6 @@ typedef struct mjsHField_ { // height field specification typedef struct mjsSkin_ { // skin specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* file; // skin file mjString* material; // name of material used for rendering float rgba[4]; // rgba when material is omitted @@ -497,7 +489,6 @@ typedef struct mjsSkin_ { // skin specification typedef struct mjsTexture_ { // texture specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtTexture type; // texture type // method 1: builtin @@ -531,7 +522,6 @@ typedef struct mjsTexture_ { // texture specification typedef struct mjsMaterial_ { // material specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* texture; // name of texture (empty: none) mjtByte texuniform; // make texture cube uniform float texrepeat[2]; // texture repetition for 2D mapping @@ -549,7 +539,6 @@ typedef struct mjsMaterial_ { // material specification typedef struct mjsPair_ { // pair specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjString* geomname1; // name of geom 1 mjString* geomname2; // name of geom 2 @@ -577,7 +566,6 @@ typedef struct mjsExclude_ { // exclude specification typedef struct mjsEquality_ { // equality specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name mjtEq type; // constraint type double data[mjNEQDATA]; // type-dependent data mjtByte active; // is equality initially active @@ -592,7 +580,6 @@ typedef struct mjsEquality_ { // equality specification typedef struct mjsTendon_ { // tendon specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // stiffness, damping, friction double stiffness; // stiffness coefficient @@ -630,7 +617,6 @@ typedef struct mjsWrap_ { // wrapping object specification typedef struct mjsActuator_ { // actuator specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // gain, bias mjtGain gaintype; // gain type @@ -674,7 +660,6 @@ typedef struct mjsActuator_ { // actuator specification typedef struct mjsSensor_ { // sensor specification mjsElement* element; // element type mjString* name; // name - mjString* classname; // class name // sensor definition mjtSensor type; // type of sensor diff --git a/include/mujoco/mjtnum.h b/include/mujoco/mjtnum.h index b9d78ea1..e50b5ff5 100644 --- a/include/mujoco/mjtnum.h +++ b/include/mujoco/mjtnum.h @@ -17,12 +17,8 @@ //---------------------------------- floating-point definition ------------------------------------- -// compile-time configuration options -#define mjUSEDOUBLE // single or double precision for mjtNum - - // floating point data type and minval -#ifdef mjUSEDOUBLE +#ifndef mjUSESINGLE typedef double mjtNum; #define mjMINVAL 1E-15 // minimum value in any denominator #else diff --git a/include/mujoco/mjui.h b/include/mujoco/mjui.h index c6d4c34d..7981d936 100644 --- a/include/mujoco/mjui.h +++ b/include/mujoco/mjui.h @@ -26,6 +26,7 @@ #define mjMAXUIRECT 25 // maximum number of rectangles #define mjSEPCLOSED 1000 // closed state of adjustable separator +#define mjPRESERVE 2000 // preserve section or separator state // key codes matching GLFW (user must remap for other frameworks) @@ -106,6 +107,13 @@ typedef enum mjtItem_ { // UI item type } mjtItem; +typedef enum mjtSection_ { // UI section state + mjSECT_CLOSED = 0, // closed state (regular section) + mjSECT_OPEN, // open state (regular section) + mjSECT_FIXED // fixed section: always open, no title +} mjtSection; + + // predicate function: set enable/disable based on item category typedef int (*mjfItemEnable)(int category, void* data); @@ -163,6 +171,8 @@ struct mjuiThemeSpacing_ { // UI visualization theme spacing int scroll; // scrollbar width int label; // label width int section; // section gap + int cornersect; // corner radius for section + int cornersep; // corner radius for separator int itemside; // item side gap int itemmid; // item middle gap int itemver; // item vertical gap @@ -180,9 +190,14 @@ struct mjuiThemeColor_ { // UI visualization theme color float master[3]; // master background float thumb[3]; // scrollbar thumb float secttitle[3]; // section title + float secttitle2[3]; // section title: bottom color + float secttitlecheck[3]; // section title with checkbox + float secttitlecheck2[3]; // section title with checkbox: bottom color float sectfont[3]; // section font float sectsymbol[3]; // section symbol float sectpane[3]; // section pane + float separator[3]; // separator title + float separator2[3]; // separator title: bottom color float shortcut[3]; // shortcut background float fontactive[3]; // font active float fontinactive[3]; // font inactive @@ -236,6 +251,7 @@ struct mjuiItem_ { // UI item void *pdata; // data pointer (type-specific) int sectionid; // id of section containing item int itemid; // id of item within section + int userid; // user-supplied id (for event handling) // type-specific properties union { @@ -247,6 +263,7 @@ struct mjuiItem_ { // UI item // internal mjrRect rect; // rectangle occupied by item + int skip; // item skipped due to closed separator }; typedef struct mjuiItem_ mjuiItem; @@ -256,15 +273,17 @@ typedef struct mjuiItem_ mjuiItem; struct mjuiSection_ { // UI section // properties char name[mjMAXUINAME]; // name - int state; // 0: closed, 1: open + int state; // section state (mjtSection) int modifier; // 0: none, 1: control, 2: shift; 4: alt int shortcut; // shortcut key; 0: undefined + int checkbox; // 0: none, 1: hidden, 2: unchecked, 2: checked int nitem; // number of items in use mjuiItem item[mjMAXUIITEM]; // preallocated array of items // internal mjrRect rtitle; // rectangle occupied by title mjrRect rcontent; // rectangle occupied by content + int lastclick; // last mouse click over this section }; typedef struct mjuiSection_ mjuiSection; @@ -287,10 +306,12 @@ struct mjUI_ { // entire UI int maxheight; // height when all sections open int scroll; // scroll from top of UI - // mouse focus + // mouse focus and count int mousesect; // 0: none, -1: scroll, otherwise 1+section int mouseitem; // item within section int mousehelp; // help button down: print shortcuts + int mouseclicks; // number of mouse clicks over UI + int mousesectcheck; // 0: none, otherwise 1+section // keyboard focus and edit int editsect; // 0: none, otherwise 1+section @@ -315,6 +336,7 @@ struct mjuiDef_ { // table passed to mjui_add() int state; // state void* pdata; // pointer to data char other[mjMAXUITEXT]; // string with type-specific properties + int otherint; // int with type-specific properties }; typedef struct mjuiDef_ mjuiDef; diff --git a/include/mujoco/mjvisualize.h b/include/mujoco/mjvisualize.h index 0d23e39a..36ae5596 100644 --- a/include/mujoco/mjvisualize.h +++ b/include/mujoco/mjvisualize.h @@ -230,21 +230,22 @@ struct mjvGeom_ { // abstract geom int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor int objid; // mujoco object id; -1 for decor int category; // visual category - int texid; // texture id; -1: no texture - int texuniform; // uniform cube mapping + int matid; // material id; -1: no textured material int texcoord; // mesh or flex geom has texture coordinates int segid; // segmentation id; -1: not shown - // OpenGL info - float texrepeat[2]; // texture repetition for 2D mapping + // spatial transform float size[3]; // size parameters float pos[3]; // Cartesian position float mat[9]; // Cartesian orientation + + // material properties float rgba[4]; // color and transparency float emission; // emission coef float specular; // specular coef float shininess; // shininess coef float reflectance; // reflectance coef + char label[100]; // text label // transparency rendering (set internally) @@ -393,7 +394,7 @@ struct mjvFigure_ { // abstract 2D figure passed to OpenGL rendere // line data int linepnt[mjMAXLINE]; // number of points in line; (0) disable - float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) + float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) // output from renderer int xaxispixel[2]; // range of x-axis in pixels diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index 66a376eb..cb9dc58d 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -413,7 +413,7 @@ X ( int, tex_adr, ntex, 1 ) \ X ( mjtByte, tex_rgb, ntexdata, 1 ) \ XMJV( int, tex_pathadr, ntex, 1 ) \ - XMJV( int, mat_texid, nmat, 1 ) \ + XMJV( int, mat_texid, nmat, mjNTEXMAT ) \ XMJV( mjtByte, mat_texuniform, nmat, 1 ) \ XMJV( float, mat_texrepeat, nmat, 2 ) \ XMJV( float, mat_emission, nmat, 1 ) \ diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 86d48cd7..520ddc40 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -74,7 +74,7 @@ MJAPI extern const char* mjRNDSTRING[mjNRNDFLAG][3]; //---------------------------------- Virtual file system ------------------------------------------- -// Initialize VFS to empty (no deallocation). +// Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS. MJAPI void mj_defaultVFS(mjVFS* vfs); // Add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load. @@ -83,18 +83,12 @@ MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) // Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load. MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); -// Return file index in VFS, or -1 if not found in VFS. -MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); - // Delete file from VFS, return 0: success, -1: not found in VFS. MJAPI int mj_deleteFileVFS(mjVFS* vfs, const char* filename); -// Delete all files from VFS. +// Delete all files from VFS and deallocates VFS internal memory. MJAPI void mj_deleteVFS(mjVFS* vfs); -// deprecated: use mj_copyBufferVFS. -MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); - //---------------------------------- Parse and compile --------------------------------------------- @@ -893,7 +887,7 @@ MJAPI int mjs_isWarning(mjSpec* s); //---------------------------------- Standard math ------------------------------------------------- -#ifdef mjUSEDOUBLE +#if !defined(mjUSESINGLE) #define mju_sqrt sqrt #define mju_exp exp #define mju_sin sin @@ -1498,7 +1492,7 @@ MJAPI mjsKey* mjs_addKey(mjSpec* s); MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); // Add default. -MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id); +MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); //---------------------------------- Assets -------------------------------------------------------- @@ -1536,6 +1530,9 @@ MJAPI mjsMesh* mjs_findMesh(mjSpec* s, const char* name); // Find frame by name. MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); +// Find keyframe by name. +MJAPI mjsKey* mjs_findKeyframe(mjSpec* s, const char* name); + // Get default corresponding to an element. MJAPI mjsDefault* mjs_getDefault(mjsElement* element); @@ -1554,6 +1551,81 @@ MJAPI mjsElement* mjs_firstChild(mjsBody* body, mjtObj type); // Return body's next child of the same type; return NULL if child is last. MJAPI mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child); +// Return spec's first element of selected type. +MJAPI mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); + +// Return spec's next element; return NULL if element is last. +MJAPI mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); + +// Safely cast an element as mjsBody, or return NULL if the element is not an mjsBody. +MJAPI mjsBody* mjs_asBody(mjsElement* element); + +// Safely cast an element as mjsGeom, or return NULL if the element is not an mjsGeom. +MJAPI mjsGeom* mjs_asGeom(mjsElement* element); + +// Safely cast an element as mjsJoint, or return NULL if the element is not an mjsJoint. +MJAPI mjsJoint* mjs_asJoint(mjsElement* element); + +// Safely cast an element as mjsSite, or return NULL if the element is not an mjsSite. +MJAPI mjsSite* mjs_asSite(mjsElement* element); + +// Safely cast an element as mjsCamera, or return NULL if the element is not an mjsCamera. +MJAPI mjsCamera* mjs_asCamera(mjsElement* element); + +// Safely cast an element as mjsLight, or return NULL if the element is not an mjsLight. +MJAPI mjsLight* mjs_asLight(mjsElement* element); + +// Safely cast an element as mjsFrame, or return NULL if the element is not an mjsFrame. +MJAPI mjsFrame* mjs_asFrame(mjsElement* element); + +// Safely cast an element as mjsActuator, or return NULL if the element is not an mjsActuator. +MJAPI mjsActuator* mjs_asActuator(mjsElement* element); + +// Safely cast an element as mjsSensor, or return NULL if the element is not an mjsSensor. +MJAPI mjsSensor* mjs_asSensor(mjsElement* element); + +// Safely cast an element as mjsFlex, or return NULL if the element is not an mjsFlex. +MJAPI mjsFlex* mjs_asFlex(mjsElement* element); + +// Safely cast an element as mjsPair, or return NULL if the element is not an mjsPair. +MJAPI mjsPair* mjs_asPair(mjsElement* element); + +// Safely cast an element as mjsEquality, or return NULL if the element is not an mjsEquality. +MJAPI mjsEquality* mjs_asEquality(mjsElement* element); + +// Safely cast an element as mjsExclude, or return NULL if the element is not an mjsExclude. +MJAPI mjsExclude* mjs_asExclude(mjsElement* element); + +// Safely cast an element as mjsTendon, or return NULL if the element is not an mjsTendon. +MJAPI mjsTendon* mjs_asTendon(mjsElement* element); + +// Safely cast an element as mjsNumeric, or return NULL if the element is not an mjsNumeric. +MJAPI mjsNumeric* mjs_asNumeric(mjsElement* element); + +// Safely cast an element as mjsText, or return NULL if the element is not an mjsText. +MJAPI mjsText* mjs_asText(mjsElement* element); + +// Safely cast an element as mjsTuple, or return NULL if the element is not an mjsTuple. +MJAPI mjsTuple* mjs_asTuple(mjsElement* element); + +// Safely cast an element as mjsKey, or return NULL if the element is not an mjsKey. +MJAPI mjsKey* mjs_asKey(mjsElement* element); + +// Safely cast an element as mjsMesh, or return NULL if the element is not an mjsMesh. +MJAPI mjsMesh* mjs_asMesh(mjsElement* element); + +// Safely cast an element as mjsHField, or return NULL if the element is not an mjsHField. +MJAPI mjsHField* mjs_asHField(mjsElement* element); + +// Safely cast an element as mjsSkin, or return NULL if the element is not an mjsSkin. +MJAPI mjsSkin* mjs_asSkin(mjsElement* element); + +// Safely cast an element as mjsTexture, or return NULL if the element is not an mjsTexture. +MJAPI mjsTexture* mjs_asTexture(mjsElement* element); + +// Safely cast an element as mjsMaterial, or return NULL if the element is not an mjsMaterial. +MJAPI mjsMaterial* mjs_asMaterial(mjsElement* element); + //---------------------------------- Attribute setters --------------------------------------------- @@ -1612,9 +1684,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// Compute quat and diag inertia from full inertia matrix, return error if any. -MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); - //---------------------------------- Element initialization --------------------------------------- diff --git a/introspect/codegen/generate_structs.py b/introspect/codegen/generate_structs.py index 42a5dcd1..7e2038b5 100644 --- a/introspect/codegen/generate_structs.py +++ b/introspect/codegen/generate_structs.py @@ -166,9 +166,15 @@ class MjStructVisitor: elif (node.get('kind') == 'TypedefDecl' and node['type']['qualType'].startswith('struct mj') and node['name'] not in _EXCLUDED): - struct = self._structs[node['type']['qualType']] - self._typedefs[node['name']] = ast_nodes.StructDecl( - name=node['name'], declname=struct.declname, fields=struct.fields) + declname = node['type']['qualType'] + try: + struct = self._structs[declname] + except KeyError: + self._typedefs[node['name']] = ast_nodes.StructDecl( + name=node['name'], declname=declname, fields=()) + else: + self._typedefs[node['name']] = ast_nodes.StructDecl( + name=node['name'], declname=struct.declname, fields=struct.fields) def resolve_all_anonymous(self) -> None: """Replaces anonymous struct placeholders with corresponding decl.""" diff --git a/introspect/enums.py b/introspect/enums.py index 7d22f36f..0542e17e 100644 --- a/introspect/enums.py +++ b/introspect/enums.py @@ -800,4 +800,14 @@ ENUMS: Mapping[str, EnumDecl] = dict([ ('mjNITEM', 14), ]), )), + ('mjtSection', + EnumDecl( + name='mjtSection', + declname='enum mjtSection_', + values=dict([ + ('mjSECT_CLOSED', 0), + ('mjSECT_OPEN', 1), + ('mjSECT_FIXED', 2), + ]), + )), ]) diff --git a/introspect/functions.py b/introspect/functions.py index 0658ba1d..3141750f 100644 --- a/introspect/functions.py +++ b/introspect/functions.py @@ -38,7 +38,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Initialize VFS to empty (no deallocation).', + doc='Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS.', # pylint: disable=line-too-long )), ('mj_addFileVFS', FunctionDecl( @@ -96,26 +96,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Add file to VFS from buffer, return 0: success, 1: full, 2: repeated name, -1: failed to load.', # pylint: disable=line-too-long )), - ('mj_findFileVFS', - FunctionDecl( - name='mj_findFileVFS', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='vfs', - type=PointerType( - inner_type=ValueType(name='mjVFS', is_const=True), - ), - ), - FunctionParameterDecl( - name='filename', - type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - ), - ), - doc='Return file index in VFS, or -1 if not found in VFS.', - )), ('mj_deleteFileVFS', FunctionDecl( name='mj_deleteFileVFS', @@ -148,31 +128,7 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), ), - doc='Delete all files from VFS.', - )), - ('mj_makeEmptyFileVFS', - FunctionDecl( - name='mj_makeEmptyFileVFS', - return_type=ValueType(name='int'), - parameters=( - FunctionParameterDecl( - name='vfs', - type=PointerType( - inner_type=ValueType(name='mjVFS'), - ), - ), - FunctionParameterDecl( - name='filename', - type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - ), - FunctionParameterDecl( - name='filesize', - type=ValueType(name='int'), - ), - ), - doc='deprecated: use mj_copyBufferVFS.', + doc='Delete all files from VFS and deallocates VFS internal memory.', )), ('mj_loadXML', FunctionDecl( @@ -9543,13 +9499,9 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), ), FunctionParameterDecl( - name='parentid', - type=ValueType(name='int'), - ), - FunctionParameterDecl( - name='id', + name='parent', type=PointerType( - inner_type=ValueType(name='int'), + inner_type=ValueType(name='mjsDefault', is_const=True), ), ), ), @@ -9751,6 +9703,28 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Find frame by name.', )), + ('mjs_findKeyframe', + FunctionDecl( + name='mjs_findKeyframe', + return_type=PointerType( + inner_type=ValueType(name='mjsKey'), + ), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), + FunctionParameterDecl( + name='name', + type=PointerType( + inner_type=ValueType(name='char', is_const=True), + ), + ), + ), + doc='Find keyframe by name.', + )), ('mjs_getDefault', FunctionDecl( name='mjs_getDefault', @@ -9861,6 +9835,416 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc="Return body's next child of the same type; return NULL if child is last.", # pylint: disable=line-too-long )), + ('mjs_firstElement', + FunctionDecl( + name='mjs_firstElement', + return_type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), + FunctionParameterDecl( + name='type', + type=ValueType(name='mjtObj'), + ), + ), + doc="Return spec's first element of selected type.", + )), + ('mjs_nextElement', + FunctionDecl( + name='mjs_nextElement', + return_type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + parameters=( + FunctionParameterDecl( + name='s', + type=PointerType( + inner_type=ValueType(name='mjSpec'), + ), + ), + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc="Return spec's next element; return NULL if element is last.", + )), + ('mjs_asBody', + FunctionDecl( + name='mjs_asBody', + return_type=PointerType( + inner_type=ValueType(name='mjsBody'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsBody, or return NULL if the element is not an mjsBody.', # pylint: disable=line-too-long + )), + ('mjs_asGeom', + FunctionDecl( + name='mjs_asGeom', + return_type=PointerType( + inner_type=ValueType(name='mjsGeom'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsGeom, or return NULL if the element is not an mjsGeom.', # pylint: disable=line-too-long + )), + ('mjs_asJoint', + FunctionDecl( + name='mjs_asJoint', + return_type=PointerType( + inner_type=ValueType(name='mjsJoint'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsJoint, or return NULL if the element is not an mjsJoint.', # pylint: disable=line-too-long + )), + ('mjs_asSite', + FunctionDecl( + name='mjs_asSite', + return_type=PointerType( + inner_type=ValueType(name='mjsSite'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsSite, or return NULL if the element is not an mjsSite.', # pylint: disable=line-too-long + )), + ('mjs_asCamera', + FunctionDecl( + name='mjs_asCamera', + return_type=PointerType( + inner_type=ValueType(name='mjsCamera'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsCamera, or return NULL if the element is not an mjsCamera.', # pylint: disable=line-too-long + )), + ('mjs_asLight', + FunctionDecl( + name='mjs_asLight', + return_type=PointerType( + inner_type=ValueType(name='mjsLight'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsLight, or return NULL if the element is not an mjsLight.', # pylint: disable=line-too-long + )), + ('mjs_asFrame', + FunctionDecl( + name='mjs_asFrame', + return_type=PointerType( + inner_type=ValueType(name='mjsFrame'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsFrame, or return NULL if the element is not an mjsFrame.', # pylint: disable=line-too-long + )), + ('mjs_asActuator', + FunctionDecl( + name='mjs_asActuator', + return_type=PointerType( + inner_type=ValueType(name='mjsActuator'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsActuator, or return NULL if the element is not an mjsActuator.', # pylint: disable=line-too-long + )), + ('mjs_asSensor', + FunctionDecl( + name='mjs_asSensor', + return_type=PointerType( + inner_type=ValueType(name='mjsSensor'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsSensor, or return NULL if the element is not an mjsSensor.', # pylint: disable=line-too-long + )), + ('mjs_asFlex', + FunctionDecl( + name='mjs_asFlex', + return_type=PointerType( + inner_type=ValueType(name='mjsFlex'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsFlex, or return NULL if the element is not an mjsFlex.', # pylint: disable=line-too-long + )), + ('mjs_asPair', + FunctionDecl( + name='mjs_asPair', + return_type=PointerType( + inner_type=ValueType(name='mjsPair'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsPair, or return NULL if the element is not an mjsPair.', # pylint: disable=line-too-long + )), + ('mjs_asEquality', + FunctionDecl( + name='mjs_asEquality', + return_type=PointerType( + inner_type=ValueType(name='mjsEquality'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsEquality, or return NULL if the element is not an mjsEquality.', # pylint: disable=line-too-long + )), + ('mjs_asExclude', + FunctionDecl( + name='mjs_asExclude', + return_type=PointerType( + inner_type=ValueType(name='mjsExclude'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsExclude, or return NULL if the element is not an mjsExclude.', # pylint: disable=line-too-long + )), + ('mjs_asTendon', + FunctionDecl( + name='mjs_asTendon', + return_type=PointerType( + inner_type=ValueType(name='mjsTendon'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsTendon, or return NULL if the element is not an mjsTendon.', # pylint: disable=line-too-long + )), + ('mjs_asNumeric', + FunctionDecl( + name='mjs_asNumeric', + return_type=PointerType( + inner_type=ValueType(name='mjsNumeric'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsNumeric, or return NULL if the element is not an mjsNumeric.', # pylint: disable=line-too-long + )), + ('mjs_asText', + FunctionDecl( + name='mjs_asText', + return_type=PointerType( + inner_type=ValueType(name='mjsText'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsText, or return NULL if the element is not an mjsText.', # pylint: disable=line-too-long + )), + ('mjs_asTuple', + FunctionDecl( + name='mjs_asTuple', + return_type=PointerType( + inner_type=ValueType(name='mjsTuple'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsTuple, or return NULL if the element is not an mjsTuple.', # pylint: disable=line-too-long + )), + ('mjs_asKey', + FunctionDecl( + name='mjs_asKey', + return_type=PointerType( + inner_type=ValueType(name='mjsKey'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsKey, or return NULL if the element is not an mjsKey.', # pylint: disable=line-too-long + )), + ('mjs_asMesh', + FunctionDecl( + name='mjs_asMesh', + return_type=PointerType( + inner_type=ValueType(name='mjsMesh'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsMesh, or return NULL if the element is not an mjsMesh.', # pylint: disable=line-too-long + )), + ('mjs_asHField', + FunctionDecl( + name='mjs_asHField', + return_type=PointerType( + inner_type=ValueType(name='mjsHField'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsHField, or return NULL if the element is not an mjsHField.', # pylint: disable=line-too-long + )), + ('mjs_asSkin', + FunctionDecl( + name='mjs_asSkin', + return_type=PointerType( + inner_type=ValueType(name='mjsSkin'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsSkin, or return NULL if the element is not an mjsSkin.', # pylint: disable=line-too-long + )), + ('mjs_asTexture', + FunctionDecl( + name='mjs_asTexture', + return_type=PointerType( + inner_type=ValueType(name='mjsTexture'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsTexture, or return NULL if the element is not an mjsTexture.', # pylint: disable=line-too-long + )), + ('mjs_asMaterial', + FunctionDecl( + name='mjs_asMaterial', + return_type=PointerType( + inner_type=ValueType(name='mjsMaterial'), + ), + parameters=( + FunctionParameterDecl( + name='element', + type=PointerType( + inner_type=ValueType(name='mjsElement'), + ), + ), + ), + doc='Safely cast an element as mjsMaterial, or return NULL if the element is not an mjsMaterial.', # pylint: disable=line-too-long + )), ('mjs_setString', FunctionDecl( name='mjs_setString', @@ -10216,37 +10600,6 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Resolve alternative orientations to quat, return error if any.', )), - ('mjs_fullInertia', - FunctionDecl( - name='mjs_fullInertia', - return_type=PointerType( - inner_type=ValueType(name='char', is_const=True), - ), - parameters=( - FunctionParameterDecl( - name='quat', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(4,), - ), - ), - FunctionParameterDecl( - name='inertia', - type=ArrayType( - inner_type=ValueType(name='double'), - extents=(3,), - ), - ), - FunctionParameterDecl( - name='fullinertia', - type=ArrayType( - inner_type=ValueType(name='double', is_const=True), - extents=(6,), - ), - ), - ), - doc='Compute quat and diag inertia from full inertia matrix, return error if any.', # pylint: disable=line-too-long - )), ('mjs_defaultSpec', FunctionDecl( name='mjs_defaultSpec', diff --git a/introspect/structs.py b/introspect/structs.py index b3e92f77..d5ca63a5 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -91,43 +91,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ declname='struct mjVFS_', fields=( StructFieldDecl( - name='nfile', - type=ValueType(name='int'), - doc='number of files present', - ), - StructFieldDecl( - name='filename', - type=ArrayType( - inner_type=ValueType(name='char'), - extents=(2000, 1000), + name='impl_', + type=PointerType( + inner_type=ValueType(name='void'), ), - doc='file name without path', - ), - StructFieldDecl( - name='filesize', - type=ArrayType( - inner_type=ValueType(name='size_t'), - extents=(2000,), - ), - doc='file size in bytes', - ), - StructFieldDecl( - name='filedata', - type=ArrayType( - inner_type=PointerType( - inner_type=ValueType(name='void'), - ), - extents=(2000,), - ), - doc='buffer with file data', - ), - StructFieldDecl( - name='filestamp', - type=ArrayType( - inner_type=ValueType(name='uint64_t'), - extents=(2000,), - ), - doc='checksum of the file data', + doc='internal pointer to VFS memory', ), ), )), @@ -2950,7 +2918,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='int'), ), - doc='texture id; -1: none (nmat x 1)', + doc='indices of textures; -1: none (nmat x mjNTEXMAT)', # pylint: disable=line-too-long ), StructFieldDecl( name='mat_texuniform', @@ -4183,7 +4151,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='pstack', type=ValueType(name='size_t'), - doc='first available mjtNum address in stack', + doc='first available byte in stack', ), StructFieldDecl( name='pbase', @@ -4223,22 +4191,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='maximum number of scalar constraints', ), - StructFieldDecl( - name='warning', - type=ArrayType( - inner_type=ValueType(name='mjWarningStat'), - extents=(8,), - ), - doc='warning statistics', - ), - StructFieldDecl( - name='timer', - type=ArrayType( - inner_type=ValueType(name='mjTimerStat'), - extents=(15,), - ), - doc='timer statistics', - ), StructFieldDecl( name='solver', type=ArrayType( @@ -4266,7 +4218,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ inner_type=ValueType(name='int'), extents=(20,), ), - doc='number of non-zeros in Hessian or efc_AR, per island', + doc='number of nonzeros in Hessian or efc_AR, per island', ), StructFieldDecl( name='solver_fwdinv', @@ -4276,6 +4228,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='forward-inverse comparison: qfrc, efc', ), + StructFieldDecl( + name='warning', + type=ArrayType( + inner_type=ValueType(name='mjWarningStat'), + extents=(8,), + ), + doc='warning statistics', + ), + StructFieldDecl( + name='timer', + type=ArrayType( + inner_type=ValueType(name='mjTimerStat'), + extents=(15,), + ), + doc='timer statistics', + ), StructFieldDecl( name='ne', type=ValueType(name='int'), @@ -4329,14 +4297,14 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='void'), ), - doc='main buffer; all pointers point in it (nbuffer bytes)', # pylint: disable=line-too-long + doc='main buffer; all pointers point in it (nbuffer bytes)', # pylint: disable=line-too-long ), StructFieldDecl( name='arena', type=PointerType( inner_type=ValueType(name='void'), ), - doc='arena+stack buffer (nstack*sizeof(mjtNum) bytes)', # pylint: disable=line-too-long + doc='arena+stack buffer (narena bytes)', # pylint: disable=line-too-long ), StructFieldDecl( name='qpos', @@ -4756,7 +4724,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtByte'), ), - doc='volume has been added to collisions (nbvh x 1)', # pylint: disable=line-too-long + doc='was bounding volume checked for collision (nbvh x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='flexedge_velocity', @@ -4987,7 +4955,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjContact'), ), - doc='list of all detected contacts (ncon x 1)', # pylint: disable=line-too-long + doc='array of all detected contacts (ncon x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='efc_type', @@ -5239,26 +5207,26 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='linear cost term: J*qacc_smooth - aref (nefc x 1)', # pylint: disable=line-too-long + doc='linear cost term: J*qacc_smooth - aref (nefc x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='efc_force', type=PointerType( inner_type=ValueType(name='mjtNum'), ), - doc='constraint force in constraint space (nefc x 1)', # pylint: disable=line-too-long + doc='constraint force in constraint space (nefc x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='efc_state', type=PointerType( inner_type=ValueType(name='int'), ), - doc='constraint state (mjtConstraintState) (nefc x 1)', # pylint: disable=line-too-long + doc='constraint state (mjtConstraintState) (nefc x 1)', # pylint: disable=line-too-long ), StructFieldDecl( name='threadpool', type=ValueType(name='uintptr_t'), - doc='ThreadPool for multithreaded operations', + doc='thread pool pointer', ), ), )), @@ -5483,14 +5451,9 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='visual category', ), StructFieldDecl( - name='texid', + name='matid', type=ValueType(name='int'), - doc='texture id; -1: no texture', - ), - StructFieldDecl( - name='texuniform', - type=ValueType(name='int'), - doc='uniform cube mapping', + doc='material id; -1: no textured material', ), StructFieldDecl( name='texcoord', @@ -5502,14 +5465,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='segmentation id; -1: not shown', ), - StructFieldDecl( - name='texrepeat', - type=ArrayType( - inner_type=ValueType(name='float'), - extents=(2,), - ), - doc='texture repetition for 2D mapping', - ), StructFieldDecl( name='size', type=ArrayType( @@ -7957,7 +7912,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='offDepthStencil_r', type=ValueType(name='unsigned int'), - doc='offscreen depth and stencil buffer for resolving multisamples', # pylint: disable=line-too-long + doc='offscreen depth and stencil buffer for multisamples', ), StructFieldDecl( name='shadowFBO', @@ -8001,6 +7956,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='auxiliary color buffer for resolving', ), + StructFieldDecl( + name='mat_texid', + type=ArrayType( + inner_type=ValueType(name='int'), + extents=(600,), + ), + doc='material texture ids (-1: no texture)', + ), + StructFieldDecl( + name='mat_texuniform', + type=ArrayType( + inner_type=ValueType(name='int'), + extents=(100,), + ), + doc='texture repetition for 2d mapping', + ), + StructFieldDecl( + name='mat_texrepeat', + type=ArrayType( + inner_type=ValueType(name='int'), + extents=(200,), + ), + doc='texture repetition for 2d mapping', + ), StructFieldDecl( name='ntexture', type=ValueType(name='int'), @@ -8035,12 +8014,12 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='baseHField', type=ValueType(name='unsigned int'), - doc='all hfields from model', + doc='all height fields from model', ), StructFieldDecl( name='baseBuiltin', type=ValueType(name='unsigned int'), - doc='all buildin geoms, with quality from model', + doc='all builtin geoms, with quality from model', ), StructFieldDecl( name='baseFontNormal', @@ -8255,7 +8234,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ doc='angles in radians or degrees', ), StructFieldDecl( - name='euler', + name='eulerseq', type=ArrayType( inner_type=ValueType(name='char'), extents=(3,), @@ -8719,13 +8698,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='type', type=ValueType(name='mjtJoint'), @@ -8893,13 +8865,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='classname', - ), StructFieldDecl( name='type', type=ValueType(name='mjtGeom'), @@ -9108,13 +9073,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='pos', type=ArrayType( @@ -9212,13 +9170,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='pos', type=ArrayType( @@ -9358,13 +9309,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='pos', type=ArrayType( @@ -9483,13 +9427,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='contype', type=ValueType(name='int'), @@ -9665,13 +9602,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='content_type', type=PointerType( @@ -9862,13 +9792,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='file', type=PointerType( @@ -9985,13 +9908,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='type', type=ValueType(name='mjtTexture'), @@ -10121,13 +10037,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='texture', type=PointerType( @@ -10214,13 +10123,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='geomname1', type=PointerType( @@ -10352,13 +10254,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='type', type=ValueType(name='mjtEq'), @@ -10435,13 +10330,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='stiffness', type=ValueType(name='double'), @@ -10596,13 +10484,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='gaintype', type=ValueType(name='mjtGain'), @@ -10793,13 +10674,6 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='name', ), - StructFieldDecl( - name='classname', - type=PointerType( - inner_type=ValueType(name='mjString'), - ), - doc='class name', - ), StructFieldDecl( name='type', type=ValueType(name='mjtSensor'), @@ -11350,6 +11224,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='section gap', ), + StructFieldDecl( + name='cornersect', + type=ValueType(name='int'), + doc='corner radius for section', + ), + StructFieldDecl( + name='cornersep', + type=ValueType(name='int'), + doc='corner radius for separator', + ), StructFieldDecl( name='itemside', type=ValueType(name='int'), @@ -11416,6 +11300,30 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='section title', ), + StructFieldDecl( + name='secttitle2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title: bottom color', + ), + StructFieldDecl( + name='secttitlecheck', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title with checkbox', + ), + StructFieldDecl( + name='secttitlecheck2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='section title with checkbox: bottom color', + ), StructFieldDecl( name='sectfont', type=ArrayType( @@ -11440,6 +11348,22 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='section pane', ), + StructFieldDecl( + name='separator', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='separator title', + ), + StructFieldDecl( + name='separator2', + type=ArrayType( + inner_type=ValueType(name='float'), + extents=(3,), + ), + doc='separator title: bottom color', + ), StructFieldDecl( name='shortcut', type=ArrayType( @@ -11602,6 +11526,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='id of item within section', ), + StructFieldDecl( + name='userid', + type=ValueType(name='int'), + doc='user-supplied id (for event handling)', + ), AnonymousUnionDecl( fields=( StructFieldDecl( @@ -11631,6 +11560,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjrRect'), doc='rectangle occupied by item', ), + StructFieldDecl( + name='skip', + type=ValueType(name='int'), + doc='item skipped due to closed separator', + ), ), )), ('mjuiSection', @@ -11649,7 +11583,7 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='state', type=ValueType(name='int'), - doc='0: closed, 1: open', + doc='section state (mjtSection)', ), StructFieldDecl( name='modifier', @@ -11661,6 +11595,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='shortcut key; 0: undefined', ), + StructFieldDecl( + name='checkbox', + type=ValueType(name='int'), + doc='0: none, 1: hidden, 2: unchecked, 2: checked', + ), StructFieldDecl( name='nitem', type=ValueType(name='int'), @@ -11684,6 +11623,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='mjrRect'), doc='rectangle occupied by content', ), + StructFieldDecl( + name='lastclick', + type=ValueType(name='int'), + doc='last mouse click over this section', + ), ), )), ('mjUI', @@ -11763,6 +11707,16 @@ STRUCTS: Mapping[str, StructDecl] = dict([ type=ValueType(name='int'), doc='help button down: print shortcuts', ), + StructFieldDecl( + name='mouseclicks', + type=ValueType(name='int'), + doc='number of mouse clicks over UI', + ), + StructFieldDecl( + name='mousesectcheck', + type=ValueType(name='int'), + doc='0: none, otherwise 1+section', + ), StructFieldDecl( name='editsect', type=ValueType(name='int'), @@ -11851,6 +11805,11 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='string with type-specific properties', ), + StructFieldDecl( + name='otherint', + type=ValueType(name='int'), + doc='int with type-specific properties', + ), ), )), ]) diff --git a/mjx/requirements.txt b/mjx/requirements.txt index 2a371aae..fd56bbb2 100644 --- a/mjx/requirements.txt +++ b/mjx/requirements.txt @@ -32,9 +32,7 @@ jaxlib==0.4.18; python_version >= '3.9' \ --hash=sha256:f7787a5531d226d6cc9ec2baa7141260bb713435e1cfc053cb9f5cefa9756ac3 \ --hash=sha256:6cb20bbbdafd90e71ad0deb9295519a0175c108c8c557b84fb9fe94f751daee4 \ --hash=sha256:116a0d6aedd3e856b52493d7e392fb1b40952b84fb72448fde1c1ab5687db667 \ - --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 \ - --hash=sha256:2b17b3f05b3bbf8e0ddb85fba339525ac03bac21c9f26d0f83dcea1b1654353e \ - --hash=sha256:b35ec08984e2aa5e96ba3f3f8b88e90dee0283649e037f213dec8e85638fa17d + --hash=sha256:9593ff69f424947567e206f3e356b2a2df55ca68e6d815d5adc6cae308e8f652 pip==23.3.1 \ --hash=sha256:55eb67bb6171d37447e82213be585b75fe2b12b359e993773aca4de9247a052b pytest==7.4.2 \ diff --git a/mjx/training_apg.ipynb b/mjx/training_apg.ipynb index 26879392..4a7192dc 100644 --- a/mjx/training_apg.ipynb +++ b/mjx/training_apg.ipynb @@ -1,39 +1,39 @@ { "cells": [ { - "cell_type": "markdown", - "metadata": { - "id": "MpkYHwCqk7W-" - }, - "source": [ - "![MuJoCo banner](https://raw.githubusercontent.com/google-deepmind/mujoco/main/banner.png)\n", - "\n", - "#

Tutorial

\n", - "\n", - "This notebook provides a tutorial for differentiable physics for policy learning in [**MuJoCo XLA (MJX)**](https://github.com/google-deepmind/mujoco/blob/main/mjx), a JAX-based implementation of MuJoCo.\n", - "\n", - "**A Colab runtime with GPU acceleration is required.** If you're using a CPU-only runtime, you can switch using the menu \"Runtime > Change runtime type\".\n", - "\n", - "\n", - "This notebook was written by [Jing Yuan Luo](https://github.com/Andrew-Luo1).\n", - "\n", - "\u003c!-- Copyright 2021 DeepMind Technologies Limited\n", - "\n", - " Licensed under the Apache License, Version 2.0 (the \"License\");\n", - " you may not use this file except in compliance with the License.\n", - " You may obtain a copy of the License at\n", - "\n", - " http://www.apache.org/licenses/LICENSE-2.0\n", - "\n", - " Unless required by applicable law or agreed to in writing, software\n", - " distributed under the License is distributed on an \"AS IS\" BASIS,\n", - " WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\n", - " See the License for the specific language governing permissions and\n", - " limitations under the License.\n", - "--\u003e" - ] + "cell_type": "markdown", + "metadata": { + "id": "MpkYHwCqk7W-" }, - { + "source": [ + "![MuJoCo banner](https://raw.githubusercontent.com/google-deepmind/mujoco/main/banner.png)\n", + "\n", + "#

Tutorial

\n", + "\n", + "This notebook provides a tutorial for differentiable physics for policy learning in [**MuJoCo XLA (MJX)**](https://github.com/google-deepmind/mujoco/blob/main/mjx), a JAX-based implementation of MuJoCo.\n", + "\n", + "**A Colab runtime with GPU acceleration is required.** If you're using a CPU-only runtime, you can switch using the menu \"Runtime > Change runtime type\".\n", + "\n", + "\n", + "This notebook was written by [Jing Yuan Luo](https://github.com/Andrew-Luo1).\n", + "\n", + "" + ] + }, + { "cell_type": "markdown", "metadata": {}, "source": [ @@ -106,12 +106,97 @@ "## Setup: Imports and installations" ] }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": {}, + "outputs": [], + "source": [ + "# Install MuJoCo, MJX, and Brax\n", + "!pip install mujoco\n", + "!pip install mujoco_mjx\n", + "!pip install brax" + ] + }, + { + "cell_type": "code", + "execution_count": 0, + "metadata": {}, + "outputs": [], + "source": [ + "#@title Check if MuJoCo installation was successful\n", + "\n", + "# Set up GPU rendering.\n", + "from google.colab import files\n", + "import distutils.util\n", + "import os\n", + "import subprocess\n", + "if subprocess.run('nvidia-smi').returncode:\n", + " raise RuntimeError(\n", + " 'Cannot communicate with GPU. '\n", + " 'Make sure you are using a GPU Colab runtime. '\n", + " 'Go to the Runtime menu and select Choose runtime type.')\n", + "\n", + "# Add an ICD config so that glvnd can pick up the Nvidia EGL driver.\n", + "# This is usually installed as part of an Nvidia driver package, but the Colab\n", + "# kernel doesn't install its driver via APT, and as a result the ICD is missing.\n", + "# (https://github.com/NVIDIA/libglvnd/blob/master/src/EGL/icd_enumeration.md)\n", + "NVIDIA_ICD_CONFIG_PATH = '/usr/share/glvnd/egl_vendor.d/10_nvidia.json'\n", + "if not os.path.exists(NVIDIA_ICD_CONFIG_PATH):\n", + " with open(NVIDIA_ICD_CONFIG_PATH, 'w') as f:\n", + " f.write(\"\"\"{\n", + " \"file_format_version\" : \"1.0.0\",\n", + " \"ICD\" : {\n", + " \"library_path\" : \"libEGL_nvidia.so.0\"\n", + " }\n", + "}\n", + "\"\"\")\n", + "\n", + "# Configure MuJoCo to use the EGL rendering backend (requires GPU)\n", + "print('Setting environment variable to use GPU rendering:')\n", + "%env MUJOCO_GL=egl\n", + "\n", + "# Check if installation was succesful.\n", + "try:\n", + " print('Checking that the installation succeeded:')\n", + " import mujoco\n", + " mujoco.MjModel.from_xml_string('')\n", + "except Exception as e:\n", + " raise e from RuntimeError(\n", + " 'Something went wrong during installation. Check the shell output above '\n", + " 'for more information.\\n'\n", + " 'If using a hosted Colab runtime, make sure you enable GPU acceleration '\n", + " 'by going to the Runtime menu and selecting \"Choose runtime type\".')\n", + "\n", + "print('Installation successful.')\n", + "\n", + "# Other imports and helper functions\n", + "import time\n", + "import itertools\n", + "import numpy as np\n", + "\n", + "# Graphics and plotting.\n", + "print('Installing mediapy:')\n", + "!command -v ffmpeg >/dev/null || (apt update && apt install -y ffmpeg)\n", + "!pip install -q mediapy\n", + "import mediapy as media\n", + "import matplotlib.pyplot as plt\n", + "\n", + "# More legible printing from numpy.\n", + "np.set_printoptions(precision=3, suppress=True, linewidth=100)\n", + "\n", + "from IPython.display import clear_output\n", + "clear_output()" + ] + }, { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ + "#@title Import MuJoCo, MJX, and Brax\n", + "\n", "import os\n", "os.environ[\"XLA_PYTHON_CLIENT_MEM_FRACTION\"] = \"0.8\" # 0.9 causes too much lag. \n", "from datetime import datetime\n", @@ -162,7 +247,7 @@ }, { "cell_type": "code", - "execution_count": null, + "execution_count": 0, "metadata": {}, "outputs": [], "source": [ diff --git a/model/hammock/humanoid_body.xml b/model/hammock/humanoid_body.xml index 1254082a..8de9b880 100644 --- a/model/hammock/humanoid_body.xml +++ b/model/hammock/humanoid_body.xml @@ -25,7 +25,7 @@ + rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/> diff --git a/model/humanoid/22_humanoids.xml b/model/humanoid/22_humanoids.xml index b7e6300c..5050941f 100644 --- a/model/humanoid/22_humanoids.xml +++ b/model/humanoid/22_humanoids.xml @@ -21,7 +21,7 @@ + rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/> diff --git a/model/humanoid/humanoid.xml b/model/humanoid/humanoid.xml index b4d8d131..e0129078 100644 --- a/model/humanoid/humanoid.xml +++ b/model/humanoid/humanoid.xml @@ -26,7 +26,7 @@ - + diff --git a/model/humanoid100/humanoid_body.xml b/model/humanoid100/humanoid_body.xml index 8dd51bc3..3dc48b96 100644 --- a/model/humanoid100/humanoid_body.xml +++ b/model/humanoid100/humanoid_body.xml @@ -25,7 +25,7 @@ + rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/> diff --git a/plugin/sdf/bolt.cc b/plugin/sdf/bolt.cc index 4030a164..4f052c06 100644 --- a/plugin/sdf/bolt.cc +++ b/plugin/sdf/bolt.cc @@ -148,7 +148,7 @@ void Bolt::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/bowl.cc b/plugin/sdf/bowl.cc index 55b6515f..2ebbc5ac 100644 --- a/plugin/sdf/bowl.cc +++ b/plugin/sdf/bowl.cc @@ -146,7 +146,7 @@ void Bowl::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/gear.cc b/plugin/sdf/gear.cc index d7b49da3..54288c21 100644 --- a/plugin/sdf/gear.cc +++ b/plugin/sdf/gear.cc @@ -232,7 +232,7 @@ void Gear::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/nut.cc b/plugin/sdf/nut.cc index 448fc4a8..117df406 100644 --- a/plugin/sdf/nut.cc +++ b/plugin/sdf/nut.cc @@ -148,7 +148,7 @@ void Nut::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/sdflib.cc b/plugin/sdf/sdflib.cc index 3985ca5b..1032dbc8 100644 --- a/plugin/sdf/sdflib.cc +++ b/plugin/sdf/sdflib.cc @@ -180,7 +180,7 @@ void SdfLib::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sdf = reinterpret_cast(plugin_data); sdf->Reset(); diff --git a/plugin/sdf/torus.cc b/plugin/sdf/torus.cc index a3c6195c..d48feb51 100644 --- a/plugin/sdf/torus.cc +++ b/plugin/sdf/torus.cc @@ -95,7 +95,7 @@ void Torus::RegisterPlugin() { delete reinterpret_cast(d->plugin_data[instance]); d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { // do nothing }; diff --git a/python/MANIFEST.in b/python/MANIFEST.in index a55f15f1..e7ecd953 100644 --- a/python/MANIFEST.in +++ b/python/MANIFEST.in @@ -1,3 +1,3 @@ include LICENSE *.md -recursive-include mujoco *.h *.cc *.mm CMakeLists.txt *.cmake +recursive-include mujoco *.h *.cc *.cc.inc *.mm CMakeLists.txt *.cmake recursive-include mujoco/mjpython mjpython.* Info.plist diff --git a/python/make_sdist.sh b/python/make_sdist.sh index 1466cb3d..7abe811d 100755 --- a/python/make_sdist.sh +++ b/python/make_sdist.sh @@ -44,6 +44,8 @@ python "${package_dir}"/mujoco/codegen/generate_enum_traits.py > \ mujoco/enum_traits.h python "${package_dir}"/mujoco/codegen/generate_function_traits.py > \ mujoco/function_traits.h +python "${package_dir}"/mujoco/codegen/generate_spec_bindings.py > \ + mujoco/specs.cc.inc export PYTHONPATH="${old_pythonpath}" # Copy over the LICENSE file. diff --git a/python/mujoco/CMakeLists.txt b/python/mujoco/CMakeLists.txt index d31bd903..480865d4 100644 --- a/python/mujoco/CMakeLists.txt +++ b/python/mujoco/CMakeLists.txt @@ -405,6 +405,29 @@ target_link_libraries( structs_header ) +if(NOT EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/specs.cc.inc) + add_custom_command( + OUTPUT specs.cc.inc + COMMAND ${CMAKE_COMMAND} -E env PYTHONPATH=${mujoco_SOURCE_DIR}/mujoco ${Python3_EXECUTABLE} + ${CMAKE_CURRENT_SOURCE_DIR}/codegen/generate_spec_bindings.py > specs.cc.inc + DEPENDS ${CMAKE_CURRENT_SOURCE_DIR}/codegen/generate_spec_bindings.py + ) +endif() + +mujoco_pybind11_module( + _specs + specs.cc + specs.cc.inc +) +target_link_libraries( + _specs + PRIVATE mujoco + Eigen3::Eigen + errors_header + raw + structs_header +) + mujoco_pybind11_module(_simulate simulate.cc) target_link_libraries( _simulate @@ -424,6 +447,7 @@ set(LIBRARIES_FOR_WHEEL "$" "$" "$" + "$" "$" "$" ) @@ -459,6 +483,7 @@ if(MUJOCO_PYTHON_MAKE_WHEEL) _render _rollout _simulate + _specs _structs mujoco ) diff --git a/python/mujoco/__init__.py b/python/mujoco/__init__.py index d9e0e5cf..dd3ad713 100644 --- a/python/mujoco/__init__.py +++ b/python/mujoco/__init__.py @@ -43,6 +43,7 @@ from mujoco._enums import * from mujoco._errors import * from mujoco._functions import * from mujoco._render import * +from mujoco._specs import * from mujoco._structs import * from mujoco.gl_context import * from mujoco.renderer import Renderer diff --git a/python/mujoco/bindings_test.py b/python/mujoco/bindings_test.py index 4ea6b3a9..f6201776 100644 --- a/python/mujoco/bindings_test.py +++ b/python/mujoco/bindings_test.py @@ -409,19 +409,6 @@ class MuJoCoBindingsTest(parameterized.TestCase): np.testing.assert_array_equal(self.model.geom_size[1], [0.5, 0.5, 0.5]) np.testing.assert_array_equal(model_copy.geom_size[1], [0.1, 0.1, 0.1]) - def test_assets_array_filename_too_long(self): - # Longest allowed filename (excluding null byte) - limit = mujoco.mjMAXVFSNAME - 1 - contents = b'' - valid_filename = 'a' * limit - mujoco.MjModel.from_xml_path(valid_filename, {valid_filename: contents}) - invalid_filename = 'a' * (limit + 1) - expected_message = ( - f'Filename length 1000 exceeds 999 character limit: {invalid_filename}') - with self.assertRaisesWithLiteralMatch(ValueError, expected_message): - mujoco.MjModel.from_xml_path(invalid_filename, - {invalid_filename: contents}) - def test_mjdata_can_copy(self): self.data.qpos = [0, 0, 0.1*np.sqrt(2) - 0.001, np.cos(np.pi/8), np.sin(np.pi/8), 0, 0, 0, @@ -856,9 +843,6 @@ Return the current version of MuJoCo as a null-terminated string. Euler integrator, semi-implicit in velocity. """) - def test_int_constant(self): - self.assertEqual(mujoco.mjMAXVFSNAME, 1000) - def test_float_constant(self): self.assertEqual(mujoco.mjMAXVAL, 1e10) self.assertEqual(mujoco.mjMINVAL, 1e-15) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py new file mode 100644 index 00000000..1b4c33cb --- /dev/null +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -0,0 +1,221 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Generates the bindings for the MuJoCo specs.""" + +from collections.abc import Sequence + +from absl import app + +from introspect import ast_nodes +from introspect import structs + + +SCALAR_TYPES = {'int', 'double', 'float', 'mjtByte', 'mjtNum'} + + +def _value_binding_code( + field: ast_nodes.ValueType, classname: str = '', varname: str = '' +) -> str: + """Creates a string that defines Python bindings for a value type.""" + fulltype = field.name + if field.name not in SCALAR_TYPES: + fulltype += '&' + fullvarname = varname + rawclassname = classname.replace('mjs', 'raw::Mjs') + if classname == 'mjSpec': # raw mjSpec has a wrapper + rawclassname = classname.replace('mjS', 'MjS') + fullvarname = 'ptr->' + varname + if field.name.startswith('mjs'): # all other mjs are raw structs + fulltype = field.name.replace('mjs', 'raw::Mjs') + if field.name != 'mjsPlugin' and field.name != 'mjsOrientation': + fulltype = fulltype + '*' # plugin and orientation are pointers + + def_property_args = ( + f'"{varname}"', + f"""[]({rawclassname}& self) -> {fulltype} {{ + return self.{fullvarname}; + }}""", + f"""[]({rawclassname}& self, {fulltype} {varname}) {{ + self.{fullvarname} = {varname}; + }}""", + ) + + if field.name not in SCALAR_TYPES: + def_property_args += ('py::return_value_policy::reference_internal',) + + return f'{classname}.def_property({",".join(def_property_args)});' + + +def _array_binding_code( + field: ast_nodes.ArrayType, classname: str = '', varname: str = '' +) -> str: + """Creates a string that declares Python bindings for an array type.""" + if len(field.extents) > 1: + raise NotImplementedError() + innertype = field.inner_type.decl() + rawclassname = classname.replace('mjs', 'raw::Mjs') + fullvarname = varname + if classname == 'mjSpec': # raw mjSpec has a wrapper + rawclassname = classname.replace('mjS', 'MjS') + fullvarname = 'ptr->' + varname + if innertype == 'double' or innertype == 'mjtNum': + innertype = 'MjDouble' # custom Eigen type + elif innertype == 'float': + innertype = 'MjFloat' # custom Eigen type + elif innertype == 'int': + innertype = 'MjInt' # custom Eigen type + elif innertype == 'char': + # char array special case + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> py::array_t {{ + return py::array_t({field.extents[0]}, self.{fullvarname}); + }}, + []({rawclassname}& self, py::object rhs) {{ + int i = 0; + for (auto val : rhs) {{ + self.{fullvarname}[i++] = py::cast(val); + }} + }}, py::return_value_policy::reference_internal);""" + # all other array types + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> {innertype}{field.extents[0]} {{ + return {innertype}{field.extents[0]}(self.{fullvarname}); + }}, + []({rawclassname}& self, {innertype}Ref{field.extents[0]} {varname}) {{ + {innertype}{field.extents[0]}(self.{fullvarname}) = {varname}; + }}, py::return_value_policy::reference_internal);""" + + +def _ptr_binding_code( + field: ast_nodes.PointerType, classname: str = '', varname: str = '' +) -> str: + """Creates a string that declares Python bindings for a pointer type.""" + vartype = field.inner_type.decl() + rawclassname = classname.replace('mjs', 'raw::Mjs') + fullvarname = varname + if classname == 'mjSpec': # raw mjSpec has a wrapper + rawclassname = classname.replace('mjS', 'MjS') + fullvarname = 'ptr->' + varname + if vartype == 'mjsElement': # this is ignored by the caller + return 'mjsElement' + if vartype.startswith('mjs'): # for structs, use the value case + return _value_binding_code(field.inner_type, classname, varname) + elif vartype == 'mjString': # C++ string -> Python string + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> std::string_view {{ + return *self.{fullvarname}; + }}, + []({rawclassname}& self, std::string_view {varname}) {{ + *(self.{fullvarname}) = {varname}; + }});""" + elif ( # C++ vectors of values -> Python array + vartype == 'mjDoubleVec' + or vartype == 'mjFloatVec' + or vartype == 'mjIntVec' + ): + vartype = vartype.replace('mj', '').replace('Vec', '').lower() + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> py::array_t<{vartype}> {{ + return py::array_t<{vartype}>(self.{fullvarname}->size(), + self.{fullvarname}->data()); + }}, + []({rawclassname}& self, py::object rhs) {{ + self.{fullvarname}->clear(); + self.{fullvarname}->reserve(py::len(rhs)); + for (auto val : rhs) {{ + self.{fullvarname}->push_back(py::cast<{vartype}>(val)); + }} + }}, py::return_value_policy::reference_internal);""" + elif vartype == 'mjStringVec': # C++ vector of strings -> Python list + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> py::list {{ + py::list list; + for (auto val : *self.{fullvarname}) {{ + list.append(val); + }} + return list; + }}, + []({rawclassname}& self, py::object rhs) {{ + self.{fullvarname}->clear(); + self.{fullvarname}->reserve(py::len(rhs)); + for (auto val : rhs) {{ + self.{fullvarname}->push_back(py::cast(val)); + }} + }}, py::return_value_policy::reference_internal);""" + elif 'VecVec' in vartype: # C++ vector of vectors -> Python list of lists + vartype = vartype.replace('mj', '').replace('VecVec', '').lower() + return f"""\ + {classname}.def_property( + "{varname}", + []({rawclassname}& self) -> py::list {{ + py::list list; + for (auto inner_vec : *self.{fullvarname}) {{ + py::list inner_list; + for (auto val : inner_vec) {{ + inner_list.append(val); + }} + list.append(inner_list); + }} + return list; + }}, + []({rawclassname}& self, py::object rhs) {{ + self.{fullvarname}->clear(); + self.{fullvarname}->reserve(py::len(rhs)); + for (auto inner_list : rhs) {{ + auto inner_vec = py::cast>(inner_list); + self.{fullvarname}->push_back(inner_vec); + }} + }}, py::return_value_policy::reference_internal);""" + + raise NotImplementedError() + + +def _binding_code(field: ast_nodes.StructFieldDecl, key: str) -> str: + if isinstance(field.type, ast_nodes.ValueType): + return _value_binding_code(field.type, key, field.name) + elif isinstance(field.type, ast_nodes.PointerType): + return _ptr_binding_code(field.type, key, field.name) + elif isinstance(field.type, ast_nodes.ArrayType): + return _array_binding_code(field.type, key, field.name) + return '' + + +def generate() -> None: + for key in structs.STRUCTS.keys(): + if (key.startswith('mjs') or key == 'mjSpec') and key != 'mjsElement': + print('\n // ' + key) + for field in structs.STRUCTS[key].fields: + code = _binding_code(field, key) + if code != 'mjsElement': + print(code) + + +def main(argv: Sequence[str]) -> None: + if len(argv) > 1: + raise app.UsageError('Too many command-line arguments.') + generate() + +if __name__ == '__main__': + app.run(main) diff --git a/python/mujoco/constants.cc b/python/mujoco/constants.cc index b8327c75..0f0f9ff6 100644 --- a/python/mujoco/constants.cc +++ b/python/mujoco/constants.cc @@ -57,8 +57,6 @@ PYBIND11_MODULE(_constants, pymodule) { X(mjMINIMP); X(mjMAXIMP); X(mjMAXCONPAIR); - X(mjMAXVFS); - X(mjMAXVFSNAME); X(mjNEQDATA); X(mjNDYN); X(mjNGAIN); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index 7582ac70..86beb5cd 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include #include @@ -29,6 +28,7 @@ #include "functions.h" #include "private.h" #include "raw.h" +#include "structs.h" #include #include #include @@ -108,9 +108,10 @@ PYBIND11_MODULE(_functions, pymodule) { // Skipped: mj_copyModel (have MjModel.__copy__, memory managed by MjModel) pymodule.def( "mj_saveModel", - [](const MjModelWrapper& m, const std::optional& filename = std::nullopt, - std::optional< - Eigen::Ref>> buffer = std::nullopt) { + [](const MjModelWrapper& m, + const std::optional& filename = std::nullopt, + std::optional>> + buffer = std::nullopt) { void* buffer_ptr = nullptr; int buffer_sz = 0; if (buffer.has_value()) { @@ -122,8 +123,7 @@ PYBIND11_MODULE(_functions, pymodule) { buffer_ptr, buffer_sz); }, py::arg("m"), py::arg_v("filename", std::nullopt), - py::arg_v("buffer", std::nullopt), - py::doc(traits::mj_saveModel::doc), + py::arg_v("buffer", std::nullopt), py::doc(traits::mj_saveModel::doc), py::call_guard()); // Skipped: mj_loadModel (have MjModel.from_binary_path) // Skipped: mj_deleteModel (have MjModel.__del__) @@ -700,8 +700,8 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); - Def(pymodule); - Def(pymodule); + // skipped: mju_rotVecMat + // skipped: mju_rotVecMatT Def(pymodule); Def(pymodule); Def(pymodule); diff --git a/python/mujoco/indexer_xmacro.h b/python/mujoco/indexer_xmacro.h index b604f008..0d0b529f 100644 --- a/python/mujoco/indexer_xmacro.h +++ b/python/mujoco/indexer_xmacro.h @@ -168,7 +168,7 @@ X( float, light_, specular, nlight, 3 ) #define MJMODEL_MATERIAL \ - X( int, mat_, texid, nmat, 1 ) \ + X( int, mat_, texid, nmat, mjNTEXMAT ) \ X( mjtByte, mat_, texuniform, nmat, 1 ) \ X( float, mat_, texrepeat, nmat, 2 ) \ X( float, mat_, emission, nmat, 1 ) \ diff --git a/python/mujoco/memory_leak_test.py b/python/mujoco/memory_leak_test.py new file mode 100644 index 00000000..47fae759 --- /dev/null +++ b/python/mujoco/memory_leak_test.py @@ -0,0 +1,69 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Test that copying mujoco.MjData multiple times doesn't leak memory.""" + +import copy +import textwrap + +from absl.testing import absltest +import mujoco + + +class MemoryLeakTest(absltest.TestCase): + + # Regression test for https://github.com/google-deepmind/mujoco/issues/1572 + def test_deepcopy_mjdata_leak(self): + # MuJoCo model with textures that take up significant memory. + model_xml = textwrap.dedent(""" + + + + + + + + + + + """) + limit = self._memory_limit(4 * 2**30) + try: + model = mujoco.MjModel.from_xml_string(model_xml) + data = mujoco.MjData(model) + total = 0 + for _ in range(1000): + mujoco.mj_step(model, data) + last_data = copy.deepcopy(data) + total += last_data.time + finally: + self._memory_limit(limit) + + def _memory_limit(self, limit_in_bytes: int) -> int: + """Limits max memory usage, and returns previous limit.""" + soft = -1 + try: + import resource # pylint: disable=g-import-not-at-top + soft, hard = resource.getrlimit(resource.RLIMIT_AS) + resource.setrlimit(resource.RLIMIT_AS, (limit_in_bytes, hard)) + except (ImportError, ValueError): + # On Windows or systems where setting resource limits fails, do nothing. + pass + return soft + + +if __name__ == '__main__': + absltest.main() diff --git a/python/mujoco/raw.h b/python/mujoco/raw.h index 5aa146f4..3d481a12 100644 --- a/python/mujoco/raw.h +++ b/python/mujoco/raw.h @@ -18,6 +18,7 @@ #include #include #include +#include #include // Type aliases for MuJoCo C structs to allow us refer to consistently refer @@ -28,6 +29,35 @@ using MjContact = ::mjContact; using MjData = ::mjData; using MjLROpt = ::mjLROpt; using MjModel = ::mjModel; +using MjSpec = ::mjSpec; +using MjsElement = ::mjsElement; +using MjsOrientation = ::mjsOrientation; +using MjsPlugin = ::mjsPlugin; +using MjsBody = ::mjsBody; +using MjsFrame = ::mjsFrame; +using MjsJoint = ::mjsJoint; +using MjsGeom = ::mjsGeom; +using MjsSite = ::mjsSite; +using MjsCamera = ::mjsCamera; +using MjsLight = ::mjsLight; +using MjsFlex = ::mjsFlex; +using MjsMesh = ::mjsMesh; +using MjsHField = ::mjsHField; +using MjsSkin = ::mjsSkin; +using MjsTexture = ::mjsTexture; +using MjsMaterial = ::mjsMaterial; +using MjsPair = ::mjsPair; +using MjsExclude = ::mjsExclude; +using MjsEquality = ::mjsEquality; +using MjsTendon = ::mjsTendon; +using MjsWrap = ::mjsWrap; +using MjsActuator = ::mjsActuator; +using MjsSensor = ::mjsSensor; +using MjsNumeric = ::mjsNumeric; +using MjsText = ::mjsText; +using MjsTuple = ::mjsTuple; +using MjsKey = ::mjsKey; +using MjsDefault = ::mjsDefault; using MjOption = ::mjOption; using MjSolverStat = ::mjSolverStat; using MjStatistic = ::mjStatistic; diff --git a/python/mujoco/render.cc b/python/mujoco/render.cc index 8be1fc69..c621cd0e 100644 --- a/python/mujoco/render.cc +++ b/python/mujoco/render.cc @@ -54,6 +54,9 @@ class MjWrapper : public WrapperBase { X(auxFBO_r); X(auxColor); X(auxColor_r); + X(mat_texid); + X(mat_texuniform); + X(mat_texrepeat); X(textureType); X(texture); X(skinvertVBO); @@ -95,6 +98,9 @@ MjrContextWrapper::MjWrapper() X(auxFBO_r), X(auxColor), X(auxColor_r), + X(mat_texid), + X(mat_texuniform), + X(mat_texrepeat), X(textureType), X(texture), X_SKIN(skinvertVBO), @@ -119,6 +125,9 @@ MjrContextWrapper::MjWrapper(const MjModelWrapper& model, int fontscale) X(auxFBO_r), X(auxColor), X(auxColor_r), + X(mat_texid), + X(mat_texuniform), + X(mat_texrepeat), X(textureType), X(texture), X_SKIN(skinvertVBO), @@ -238,6 +247,9 @@ PYBIND11_MODULE(_render, pymodule) { X(auxFBO_r); X(auxColor); X(auxColor_r); + X(mat_texid); + X(mat_texuniform); + X(mat_texrepeat); X(textureType); X(texture); X(skinvertVBO); diff --git a/python/mujoco/simulate.cc b/python/mujoco/simulate.cc index 35532daa..0ddd5982 100644 --- a/python/mujoco/simulate.cc +++ b/python/mujoco/simulate.cc @@ -123,6 +123,9 @@ class SimulateWrapper { mujoco::Simulate* simulate() { return simulate_; } + py::object GetModel() const { return m_; } + py::object GetData() const { return d_; } + private: mujoco::Simulate* simulate_; std::atomic_int destroyed_ = 0; @@ -222,6 +225,8 @@ PYBIND11_MODULE(_simulate, pymodule) { .def("lock", GetIfNotNull(&mujoco::Simulate::mtx), py::call_guard(), py::return_value_policy::reference_internal) + .def_property_readonly("m", &SimulateWrapper::GetModel) + .def_property_readonly("d", &SimulateWrapper::GetData) .def_property_readonly("ctrl_noise_std", GetIfNotNull(&mujoco::Simulate::ctrl_noise_std), py::call_guard()) diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc new file mode 100644 index 00000000..872761c7 --- /dev/null +++ b/python/mujoco/specs.cc @@ -0,0 +1,954 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include // IWYU pragma: keep +#include +#include +#include +#include // IWYU pragma: keep +#include // IWYU pragma: keep + +#include +#include +#include // IWYU pragma: keep +#include +#include "errors.h" +#include "indexers.h" // IWYU pragma: keep +#include "raw.h" +#include "structs.h" // IWYU pragma: keep +#include +#include +#include +#include +#include +#include + +namespace py = ::pybind11; + +namespace mujoco::python { +using MjInt2 = Eigen::Map; +using MjInt3 = Eigen::Map; +using MjFloat2 = Eigen::Map; +using MjFloat3 = Eigen::Map; +using MjFloat4 = Eigen::Map; +using MjDouble2 = Eigen::Map; +using MjDouble3 = Eigen::Map; +using MjDouble4 = Eigen::Map; +using MjDouble5 = Eigen::Map>; +using MjDouble6 = Eigen::Map>; +using MjDouble10 = Eigen::Map>; +using MjDouble11 = Eigen::Map>; +using MjDoubleVec = Eigen::Map; + +using MjIntRef2 = Eigen::Ref; +using MjIntRef3 = Eigen::Ref; +using MjFloatRef2 = Eigen::Ref; +using MjFloatRef3 = Eigen::Ref; +using MjFloatRef4 = Eigen::Ref; +using MjDoubleRef2 = Eigen::Ref; +using MjDoubleRef3 = Eigen::Ref; +using MjDoubleRef4 = Eigen::Ref; +using MjDoubleRef5 = Eigen::Ref>; +using MjDoubleRef6 = Eigen::Ref>; +using MjDoubleRef10 = Eigen::Ref>; +using MjDoubleRef11 = Eigen::Ref>; +using MjDoubleRefVec = Eigen::Ref; + +struct MjSpec { + MjSpec() : ptr(mj_makeSpec()) {} + ~MjSpec() { mj_deleteSpec(ptr); } + raw::MjSpec* ptr; +}; + +PYBIND11_MODULE(_specs, m) { + auto structs_m = py::module::import("mujoco._structs"); + py::function mjmodel_from_spec_ptr = + structs_m.attr("MjModel").attr("_from_spec_ptr"); + py::function mjmodel_mjdata_from_spec_ptr = + structs_m.attr("_recompile_spec_addr"); + + py::class_ mjSpec(m, "MjSpec"); + py::class_ mjsElement(m, "MjsElement"); + py::class_ mjsDefault(m, "MjsDefault"); + py::class_ mjsBody(m, "MjsBody"); + py::class_ mjsFrame(m, "MjsFrame"); + py::class_ mjsGeom(m, "MjsGeom"); + py::class_ mjsJoint(m, "MjsJoint"); + py::class_ mjsLight(m, "MjsLight"); + py::class_ mjsMaterial(m, "MjsMaterial"); + py::class_ mjsSite(m, "MjsSite"); + py::class_ mjsMesh(m, "MjsMesh"); + py::class_ mjsSkin(m, "MjsSkin"); + py::class_ mjsTexture(m, "MjsTexture"); + py::class_ mjsText(m, "MjsText"); + py::class_ mjsTuple(m, "MjsTuple"); + py::class_ mjsCamera(m, "MjsCamera"); + py::class_ mjsFlex(m, "MjsFlex"); + py::class_ mjsHField(m, "MjsHField"); + py::class_ mjsKey(m, "MjsKey"); + py::class_ mjsNumeric(m, "MjsNumeric"); + py::class_ mjsPair(m, "MjsPair"); + py::class_ mjsExclude(m, "MjsExclude"); + py::class_ mjsEquality(m, "MjsEquality"); + py::class_ mjsTendon(m, "MjsTendon"); + py::class_ mjsSensor(m, "MjsSensor"); + py::class_ mjsActuator(m, "MjsActuator"); + py::class_ mjsPlugin(m, "MjsPlugin"); + py::class_ mjsOrientation(m, "MjsOrientation"); + py::class_ mjsWrap(m, "MjsWrap"); + + // ============================= MJSPEC ===================================== + mjSpec.def(py::init<>()); + mjSpec.def("recompile", [mjmodel_mjdata_from_spec_ptr]( + const MjSpec& self, py::object m, py::object d) { + return mjmodel_mjdata_from_spec_ptr(reinterpret_cast(self.ptr), + m, d); + }); + mjSpec.def( + "copy", + [](const MjSpec& self) -> raw::MjSpec* { return mj_copySpec(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "worldbody", + [](MjSpec& self) -> raw::MjsBody* { + return mjs_findBody(self.ptr, "world"); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_body", + [](MjSpec& self, std::string& name) -> raw::MjsBody* { + return mjs_findBody(self.ptr, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_mesh", + [](MjSpec& self, std::string& name) -> raw::MjsMesh* { + return mjs_findMesh(self.ptr, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_frame", + [](MjSpec& self, std::string& name) -> raw::MjsFrame* { + return mjs_findFrame(self.ptr, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_keyframe", + [](MjSpec& self, std::string& name) -> raw::MjsKey* { + return mjs_findKeyframe(self.ptr, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "find_default", + [](MjSpec& self, std::string& classname) -> raw::MjsDefault* { + return mjs_findDefault(self.ptr, classname.c_str()); + }, + py::return_value_policy::reference_internal); + mjSpec.def("compile", [mjmodel_from_spec_ptr](MjSpec& self) { + return mjmodel_from_spec_ptr(reinterpret_cast(self.ptr)); + }); + mjSpec.def( + "copy_back", + [](MjSpec& self, raw::MjModel& model) { + return mj_copyBack(self.ptr, &model); + }, + py::return_value_policy::reference_internal); + mjSpec.def("to_xml", [](MjSpec& self) -> std::string { + int size = mj_saveXMLString(self.ptr, nullptr, 0, nullptr, 0); + std::unique_ptr buf(new char[size + 1]); + std::array err; + buf[0] = '\0'; + err[0] = '\0'; + mj_saveXMLString(self.ptr, buf.get(), size + 1, err.data(), err.size()); + if (err[0] != '\0') { + throw FatalError(std::string(err.data())); + } + return std::string(buf.get()); + }); + mjSpec.def("from_file", [](MjSpec& self, std::string& filename) -> void { + std::array err; + err[0] = '\0'; + mj_deleteSpec(self.ptr); + self.ptr = mj_parseXML(filename.c_str(), 0, err.data(), err.size()); + if (!self.ptr) { + throw FatalError(std::string(err.data())); + } + }); + mjSpec.def("from_string", [](MjSpec& self, std::string& xml) -> void { + std::array err; + err[0] = '\0'; + mj_deleteSpec(self.ptr); + self.ptr = mj_parseXMLString(xml.c_str(), 0, err.data(), err.size()); + if (!self.ptr) { + throw FatalError(std::string(err.data())); + } + }); + mjSpec.def( + "add_default", + [](MjSpec* spec, std::string& classname, + raw::MjsDefault* parent) -> raw::MjsDefault* { + return mjs_addDefault(spec->ptr, classname.c_str(), parent); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "default", + [](MjSpec& self) -> raw::MjsDefault* { + return mjs_getSpecDefault(self.ptr); + }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_material", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsMaterial* { + return mjs_addMaterial(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_mesh", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsMesh* { + return mjs_addMesh(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_skin", + [](MjSpec& self) -> raw::MjsSkin* { return mjs_addSkin(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_texture", + [](MjSpec& self) -> raw::MjsTexture* { return mjs_addTexture(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_text", + [](MjSpec& self) -> raw::MjsText* { return mjs_addText(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_tuple", + [](MjSpec& self) -> raw::MjsTuple* { return mjs_addTuple(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_flex", + [](MjSpec& self) -> raw::MjsFlex* { return mjs_addFlex(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_hfield", + [](MjSpec& self) -> raw::MjsHField* { return mjs_addHField(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_key", + [](MjSpec& self) -> raw::MjsKey* { return mjs_addKey(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_numeric", + [](MjSpec& self) -> raw::MjsNumeric* { return mjs_addNumeric(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_pair", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsPair* { + return mjs_addPair(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_exclude", + [](MjSpec& self) -> raw::MjsExclude* { return mjs_addExclude(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_equality", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsEquality* { + return mjs_addEquality(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_tendon", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsTendon* { + return mjs_addTendon(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_sensor", + [](MjSpec& self) -> raw::MjsSensor* { return mjs_addSensor(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def( + "add_actuator", + [](MjSpec& self, raw::MjsDefault* default_) -> raw::MjsActuator* { + return mjs_addActuator(self.ptr, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjSpec.def( + "add_plugin", + [](MjSpec& self) -> raw::MjsPlugin* { return mjs_addPlugin(self.ptr); }, + py::return_value_policy::reference_internal); + mjSpec.def("detach_body", [](MjSpec& self, raw::MjsBody& body) { + mjs_detachBody(self.ptr, &body); + }); + mjSpec.def_property_readonly( + "actuators", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_ACTUATOR); + while (el) { + list.append(mjs_asActuator(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "sensors", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_SENSOR); + while (el) { + list.append(mjs_asSensor(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "flexes", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_FLEX); + while (el) { + list.append(mjs_asFlex(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "pairs", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_PAIR); + while (el) { + list.append(mjs_asPair(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "equality", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_EQUALITY); + while (el) { + list.append(mjs_asEquality(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "excludes", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_EXCLUDE); + while (el) { + list.append(mjs_asExclude(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "tendons", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_TENDON); + while (el) { + list.append(mjs_asTendon(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "numeric", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_NUMERIC); + while (el) { + list.append(mjs_asNumeric(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "text", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_TEXT); + while (el) { + list.append(mjs_asText(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "tuple", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_TUPLE); + while (el) { + list.append(mjs_asTuple(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "key", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_KEY); + while (el) { + list.append(mjs_asKey(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "mesh", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_MESH); + while (el) { + list.append(mjs_asMesh(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "hfield", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_HFIELD); + while (el) { + list.append(mjs_asHField(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "skin", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_SKIN); + while (el) { + list.append(mjs_asSkin(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "texture", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_TEXTURE); + while (el) { + list.append(mjs_asTexture(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + mjSpec.def_property_readonly( + "material", + [](MjSpec& self) -> py::list { + py::list list; + raw::MjsElement* el = mjs_firstElement(self.ptr, mjOBJ_MATERIAL); + while (el) { + list.append(mjs_asMaterial(el)); + el = mjs_nextElement(self.ptr, el); + } + return list; + }, + py::return_value_policy::reference_internal); + + // ============================= MJSBODY ===================================== + mjsBody.def_property_readonly( + "id", [](raw::MjsBody& self) -> int { return mjs_getId(self.element); }); + mjsBody.def("delete", [](raw::MjsBody& self) { mjs_delete(self.element); }); + mjsBody.def( + "add_body", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsBody* { + return mjs_addBody(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_frame", + [](raw::MjsBody& self, raw::MjsFrame* parentframe_) -> raw::MjsFrame* { + return mjs_addFrame(&self, parentframe_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_geom", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsGeom* { + return mjs_addGeom(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_joint", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsJoint* { + return mjs_addJoint(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_freejoint", + [](raw::MjsBody& self) -> raw::MjsJoint* { + return mjs_addFreeJoint(&self); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "add_light", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsLight* { + return mjs_addLight(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_site", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsSite* { + return mjs_addSite(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def( + "add_camera", + [](raw::MjsBody& self, raw::MjsDefault* default_) -> raw::MjsCamera* { + return mjs_addCamera(&self, default_); + }, + py::arg_v("default", nullptr), + py::return_value_policy::reference_internal); + mjsBody.def("set_frame", + [](raw::MjsBody& self, raw::MjsFrame& frame) -> void { + mjs_setFrame(self.element, &frame); + }); + mjsBody.def("set_default", + [](raw::MjsBody& self, raw::MjsDefault& default_) -> void { + mjs_setDefault(self.element, &default_); + }); + mjsBody.def( + "default", + [](raw::MjsBody& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "find_child", + [](raw::MjsBody& self, std::string& name) -> raw::MjsBody* { + return mjs_findChild(&self, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_body", + [](raw::MjsBody& self) -> raw::MjsBody* { + return mjs_asBody(mjs_firstChild(&self, mjOBJ_BODY)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_body", + [](raw::MjsBody& self, raw::MjsBody& child) -> raw::MjsBody* { + return mjs_asBody(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_camera", + [](raw::MjsBody& self) -> raw::MjsCamera* { + return mjs_asCamera(mjs_firstChild(&self, mjOBJ_CAMERA)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_camera", + [](raw::MjsBody& self, raw::MjsCamera& child) -> raw::MjsCamera* { + return mjs_asCamera(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_light", + [](raw::MjsBody& self) -> raw::MjsLight* { + return mjs_asLight(mjs_firstChild(&self, mjOBJ_LIGHT)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_light", + [](raw::MjsBody& self, raw::MjsLight& child) -> raw::MjsLight* { + return mjs_asLight(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_joint", + [](raw::MjsBody& self) -> raw::MjsJoint* { + return mjs_asJoint(mjs_firstChild(&self, mjOBJ_JOINT)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_joint", + [](raw::MjsBody& self, raw::MjsJoint& child) -> raw::MjsJoint* { + return mjs_asJoint(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_geom", + [](raw::MjsBody& self) -> raw::MjsGeom* { + return mjs_asGeom(mjs_firstChild(&self, mjOBJ_GEOM)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_geom", + [](raw::MjsBody& self, raw::MjsGeom& child) -> raw::MjsGeom* { + return mjs_asGeom(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_site", + [](raw::MjsBody& self) -> raw::MjsSite* { + return mjs_asSite(mjs_firstChild(&self, mjOBJ_SITE)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_site", + [](raw::MjsBody& self, raw::MjsSite& child) -> raw::MjsSite* { + return mjs_asSite(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "first_frame", + [](raw::MjsBody& self) -> raw::MjsFrame* { + return mjs_asFrame(mjs_firstChild(&self, mjOBJ_FRAME)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "next_frame", + [](raw::MjsBody& self, raw::MjsFrame& child) -> raw::MjsFrame* { + return mjs_asFrame(mjs_nextChild(&self, child.element)); + }, + py::return_value_policy::reference_internal); + mjsBody.def( + "spec", + [](raw::MjsBody& self) -> raw::MjSpec* { return mjs_getSpec(&self); }, + py::return_value_policy::reference_internal); + mjsBody.def("attach_frame", + [](raw::MjsBody& self, raw::MjsFrame& frame, std::string& prefix, + std::string& suffix) -> void { + mjs_attachFrame(&self, &frame, prefix.c_str(), suffix.c_str()); + }); + + // ============================= MJSFRAME ==================================== + mjsFrame.def_property_readonly( + "id", [](raw::MjsFrame& self) -> int { return mjs_getId(self.element); }); + mjsFrame.def("delete", [](raw::MjsFrame& self) { mjs_delete(self.element); }); + mjsFrame.def("set_frame", [](raw::MjsFrame& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsFrame.def("attach_body", [](raw::MjsFrame& self, raw::MjsBody& body, + std::string& prefix, std::string& suffix) { + mjs_attachBody(&self, &body, prefix.c_str(), suffix.c_str()); + }); + + // ============================= MJSGEOM ===================================== + mjsGeom.def_property_readonly( + "id", [](raw::MjsGeom& self) -> int { return mjs_getId(self.element); }); + mjsGeom.def("delete", [](raw::MjsGeom& self) { mjs_delete(self.element); }); + mjsGeom.def("set_frame", [](raw::MjsGeom& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsGeom.def("set_default", [](raw::MjsGeom& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsGeom.def( + "default", + [](raw::MjsGeom& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSJOINT ==================================== + mjsJoint.def_property_readonly( + "id", [](raw::MjsJoint& self) -> int { return mjs_getId(self.element); }); + mjsJoint.def("delete", [](raw::MjsJoint& self) { mjs_delete(self.element); }); + mjsJoint.def("set_frame", [](raw::MjsJoint& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsJoint.def("set_default", [](raw::MjsJoint& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsJoint.def( + "default", + [](raw::MjsJoint& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSSITE ===================================== + mjsSite.def_property_readonly( + "id", [](raw::MjsSite& self) -> int { return mjs_getId(self.element); }); + mjsSite.def("delete", [](raw::MjsSite& self) { mjs_delete(self.element); }); + mjsSite.def("set_frame", [](raw::MjsSite& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsSite.def("set_default", [](raw::MjsSite& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsSite.def( + "default", + [](raw::MjsSite& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSCAMERA =================================== + mjsCamera.def_property_readonly("id", [](raw::MjsCamera& self) -> int { + return mjs_getId(self.element); + }); + mjsCamera.def("delete", + [](raw::MjsCamera& self) { mjs_delete(self.element); }); + mjsCamera.def("set_frame", [](raw::MjsCamera& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsCamera.def("set_default", [](raw::MjsCamera& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsCamera.def( + "default", + [](raw::MjsCamera& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSLIGHT ==================================== + mjsLight.def_property_readonly( + "id", [](raw::MjsLight& self) -> int { return mjs_getId(self.element); }); + mjsLight.def("delete", [](raw::MjsLight& self) { mjs_delete(self.element); }); + mjsLight.def("set_frame", [](raw::MjsLight& self, raw::MjsFrame& frame) { + mjs_setFrame(self.element, &frame); + }); + mjsLight.def("set_default", [](raw::MjsLight& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsLight.def( + "default", + [](raw::MjsLight& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSMATERIAL ================================= + mjsMaterial.def_property_readonly("id", [](raw::MjsMaterial& self) -> int { + return mjs_getId(self.element); + }); + mjsMaterial.def("delete", + [](raw::MjsMaterial& self) { mjs_delete(self.element); }); + mjsMaterial.def("set_default", + [](raw::MjsMaterial& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsMaterial.def( + "default", + [](raw::MjsMaterial& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSMESH ===================================== + mjsMesh.def_property_readonly( + "id", [](raw::MjsMesh& self) -> int { return mjs_getId(self.element); }); + mjsMesh.def("delete", [](raw::MjsMesh& self) { mjs_delete(self.element); }); + mjsMesh.def("set_default", [](raw::MjsMesh& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsMesh.def( + "default", + [](raw::MjsMesh& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSPAIR ===================================== + mjsPair.def_property_readonly( + "id", [](raw::MjsPair& self) -> int { return mjs_getId(self.element); }); + mjsPair.def("delete", [](raw::MjsPair& self) { mjs_delete(self.element); }); + mjsPair.def("set_default", [](raw::MjsPair& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsPair.def( + "default", + [](raw::MjsPair& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSEQUAL ==================================== + mjsEquality.def_property_readonly("id", [](raw::MjsEquality& self) -> int { + return mjs_getId(self.element); + }); + mjsEquality.def("delete", + [](raw::MjsEquality& self) { mjs_delete(self.element); }); + mjsEquality.def("set_default", + [](raw::MjsEquality& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsEquality.def( + "default", + [](raw::MjsEquality& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSACTUATOR ================================= + mjsActuator.def_property_readonly("id", [](raw::MjsActuator& self) -> int { + return mjs_getId(self.element); + }); + mjsActuator.def("delete", + [](raw::MjsActuator& self) { mjs_delete(self.element); }); + mjsActuator.def("set_default", + [](raw::MjsActuator& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsActuator.def( + "default", + [](raw::MjsActuator& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSTENDON =================================== + mjsTendon.def_property_readonly("id", [](raw::MjsTendon& self) -> int { + return mjs_getId(self.element); + }); + mjsTendon.def("delete", + [](raw::MjsTendon& self) { mjs_delete(self.element); }); + mjsTendon.def("set_default", [](raw::MjsTendon& self, raw::MjsDefault& def) { + mjs_setDefault(self.element, &def); + }); + mjsTendon.def( + "default", + [](raw::MjsTendon& self) -> raw::MjsDefault* { + return mjs_getDefault(self.element); + }, + py::return_value_policy::reference_internal); + mjsTendon.def( + "wrap_site", + [](raw::MjsTendon& self, std::string& name) { + return mjs_wrapSite(&self, name.c_str()); + }, + py::return_value_policy::reference_internal); + mjsTendon.def( + "wrap_geom", + [](raw::MjsTendon& self, std::string& name, std::string& sidesite) { + return mjs_wrapGeom(&self, name.c_str(), sidesite.c_str()); + }, + py::return_value_policy::reference_internal); + mjsTendon.def( + "wrap_joint", + [](raw::MjsTendon& self, std::string& name, double coef) { + return mjs_wrapJoint(&self, name.c_str(), coef); + }, + py::return_value_policy::reference_internal); + mjsTendon.def( + "wrap_pulley", + [](raw::MjsTendon& self, double divisor) { + return mjs_wrapPulley(&self, divisor); + }, + py::return_value_policy::reference_internal); + + // ============================= MJSSENSOR =================================== + mjsSensor.def_property_readonly("id", [](raw::MjsSensor& self) -> int { + return mjs_getId(self.element); + }); + mjsSensor.def("delete", + [](raw::MjsSensor& self) { mjs_delete(self.element); }); + + // ============================= MJSFLEX ===================================== + mjsFlex.def_property_readonly( + "id", [](raw::MjsFlex& self) -> int { return mjs_getId(self.element); }); + mjsFlex.def("delete", [](raw::MjsFlex& self) { mjs_delete(self.element); }); + + // ============================= MJSHFIELD =================================== + mjsHField.def_property_readonly("id", [](raw::MjsHField& self) -> int { + return mjs_getId(self.element); + }); + mjsHField.def("delete", + [](raw::MjsHField& self) { mjs_delete(self.element); }); + + // ============================= MJSSKIN ===================================== + mjsSkin.def_property_readonly( + "id", [](raw::MjsSkin& self) -> int { return mjs_getId(self.element); }, + py::return_value_policy::reference_internal); + mjsSkin.def("delete", [](raw::MjsSkin& self) { mjs_delete(self.element); }); + + // ============================= MJSTEXTURE ================================== + mjsTexture.def_property_readonly("id", [](raw::MjsTexture& self) -> int { + return mjs_getId(self.element); + }); + mjsTexture.def("delete", + [](raw::MjsTexture& self) { mjs_delete(self.element); }); + + // ============================= MJSKEY ====================================== + mjsKey.def_property_readonly( + "id", [](raw::MjsKey& self) -> int { return mjs_getId(self.element); }); + mjsKey.def("delete", [](raw::MjsKey& self) { mjs_delete(self.element); }); + + // ============================= MJSTEXT ===================================== + mjsText.def_property_readonly( + "id", [](raw::MjsText& self) -> int { return mjs_getId(self.element); }); + mjsText.def("delete", [](raw::MjsText& self) { mjs_delete(self.element); }); + + // ============================= MJSNUMERIC ================================== + mjsNumeric.def_property_readonly("id", [](raw::MjsNumeric& self) -> int { + return mjs_getId(self.element); + }); + mjsNumeric.def("delete", + [](raw::MjsNumeric& self) { mjs_delete(self.element); }); + + // ============================= MJSEXCLUDE ================================== + mjsExclude.def_property_readonly("id", [](raw::MjsExclude& self) -> int { + return mjs_getId(self.element); + }); + mjsExclude.def("delete", + [](raw::MjsExclude& self) { mjs_delete(self.element); }); + + // ============================= MJSTUPLE ==================================== + mjsTuple.def_property_readonly( + "id", [](raw::MjsTuple& self) -> int { return mjs_getId(self.element); }); + mjsTuple.def("delete", [](raw::MjsTuple& self) { mjs_delete(self.element); }); + + // ============================= MJSPLUGIN =================================== + mjsPlugin.def_property_readonly("id", [](raw::MjsPlugin& self) -> int { + return mjs_getId(self.instance); + }); + mjsPlugin.def("delete", + [](raw::MjsPlugin& self) { mjs_delete(self.instance); }); + +#include "specs.cc.inc" +} // PYBIND11_MODULE // NOLINT +} // namespace mujoco::python diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py new file mode 100644 index 00000000..9c36d728 --- /dev/null +++ b/python/mujoco/specs_test.py @@ -0,0 +1,237 @@ +# Copyright 2024 DeepMind Technologies Limited +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ============================================================================== +"""Tests for mjSpec bindings.""" + +import inspect +import textwrap + +from absl.testing import absltest +import mujoco +import numpy as np + + +def get_linenumber(): + cf = inspect.currentframe() + return cf.f_back.f_lineno + + +class SpecsTest(absltest.TestCase): + + def test_basic(self): + # Create a spec. + spec = mujoco.MjSpec() + + # Check that euler sequence order is set correctly. + self.assertEqual(spec.eulerseq[0], ord('x')) + spec.eulerseq = ['z', 'y', 'x'] + self.assertEqual(spec.eulerseq[0], ord('z')) + + # Add a body, check that it has default orientation. + body = spec.worldbody.add_body() + self.assertEqual(body.name, '') + np.testing.assert_array_equal(body.quat, [1, 0, 0, 0]) + + # Change the name of the body and read it back twice. + body.name = 'foobar' + self.assertEqual(body.name, 'foobar') + body.name = 'baz' + self.assertEqual(body.name, 'baz') + + # Change the position of the body and read it back. + body.pos = [1, 2, 3] + np.testing.assert_array_equal(body.pos, [1, 2, 3]) + self.assertEqual(body.pos.shape, (3,)) + + # Change the orientation of the body and read it back. + body.quat = [0, 1, 0, 0] + np.testing.assert_array_equal(body.quat, [0, 1, 0, 0]) + self.assertEqual(body.quat.shape, (4,)) + + # Add a site to the body with user data and read it back. + site = body.add_site() + site.name = 'sitename' + site.userdata = [1, 2, 3, 4, 5, 6] + self.assertEqual(site.name, 'sitename') + np.testing.assert_array_equal(site.userdata, [1, 2, 3, 4, 5, 6]) + + # Check that the site and body have no id before compilation. + self.assertEqual(body.id, -1) + self.assertEqual(site.id, -1) + + # Compile the spec and check for expected values in the model. + model = spec.compile() + self.assertEqual(spec.worldbody.id, 0) + self.assertEqual(body.id, 1) + self.assertEqual(site.id, 0) + self.assertEqual(model.nbody, 2) # 2 bodies, including the world body + np.testing.assert_array_equal(model.body_pos[1], [1, 2, 3]) + np.testing.assert_array_equal(model.body_quat[1], [0, 1, 0, 0]) + self.assertEqual(model.nsite, 1) + self.assertEqual(model.nuser_site, 6) + np.testing.assert_array_equal(model.site_user[0], [1, 2, 3, 4, 5, 6]) + + self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + + + + + + + + + + + + """),) + + def test_compile_errors_with_line_info(self): + spec = mujoco.MjSpec() + + added_on_line = get_linenumber() + 1 + geom = spec.worldbody.add_geom() + geom.name = 'MyGeom' + geom.info = f'geom added on line {added_on_line}' + + # Try to compile, get error. + expected_error = ( + 'Error: size 0 must be positive in geom\n' + + f'Element name \'MyGeom\', id 0, geom added on line {added_on_line}' + ) + with self.assertRaisesRegex(ValueError, expected_error): + spec.compile() + + def test_recompile(self): + # Create a spec. + spec = mujoco.MjSpec() + + # Add movable body1. + body1 = spec.worldbody.add_body() + geom = body1.add_geom() + geom.size[0] = 1 + geom.pos = [1, 1, 0] + joint = body1.add_joint() + joint.type = mujoco.mjtJoint.mjJNT_BALL + + # Compile model, make data. + model = spec.compile() + data = mujoco.MjData(model) + + # Simulate for 1 second. + while data.time < 1: + mujoco.mj_step(model, data) + + # Add movable body2. + body2 = spec.worldbody.add_body() + body2.pos[1] = 3 + geom = body2.add_geom() + geom.size[0] = 1 + geom.pos = [0, 1, 0] + joint = body2.add_joint() + joint.type = mujoco.mjtJoint.mjJNT_BALL + + # Recompile model and data while maintaining the state. + model_new, data_new = spec.recompile(model, data) + + # Check that the state is preserved. + np.testing.assert_array_equal(model_new.body_pos[1], model.body_pos[1]) + np.testing.assert_array_equal(data_new.qpos[:4], data.qpos) + np.testing.assert_array_equal(data_new.qvel[:3], data.qvel) + + def test_uncompiled_spec_cannot_be_written(self): + spec = mujoco.MjSpec() + + # Cannot write XML of an uncompiled spec. + expected_error = 'XML Write error: Only compiled model can be written' + with self.assertRaisesWithLiteralMatch(mujoco.FatalError, expected_error): + spec.to_xml() + + def test_modelname_default_class(self): + spec = mujoco.MjSpec() + spec.modelname = 'test' + + main = spec.default() + main.geom.size[0] = 2 + + def1 = spec.add_default('def1', main) + def1.geom.size[0] = 3 + + spec.worldbody.add_geom(def1) + spec.worldbody.add_geom(main) + + spec.compile() + self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + + + + + + + + + + + + + + + + """)) + spec = mujoco.MjSpec() + spec.modelname = 'test' + + main = spec.default() + main.geom.size[0] = 2 + + def1 = spec.add_default('def1', main) + def1.geom.size[0] = 3 + + spec.worldbody.add_geom(def1) + spec.worldbody.add_geom(main) + + spec.compile() + self.assertEqual(spec.to_xml(), textwrap.dedent("""\ + + + + + + + + + + + + + + + + """)) + + def test_element_list(self): + spec = mujoco.MjSpec() + sensor1 = spec.add_sensor() + sensor2 = spec.add_sensor() + sensor3 = spec.add_sensor() + sensor1.name = 'sensor1' + sensor2.name = 'sensor2' + sensor3.name = 'sensor3' + self.assertLen(spec.sensors, 3) + self.assertEqual(spec.sensors[0].name, 'sensor1') + self.assertEqual(spec.sensors[1].name, 'sensor2') + self.assertEqual(spec.sensors[2].name, 'sensor3') + + +if __name__ == '__main__': + absltest.main() diff --git a/python/mujoco/structs.cc b/python/mujoco/structs.cc index a52c8335..c70e28cc 100644 --- a/python/mujoco/structs.cc +++ b/python/mujoco/structs.cc @@ -28,6 +28,7 @@ #include #include #include +#include #include #include #include @@ -316,24 +317,22 @@ static raw::MjModel* LoadModelFileImpl( const std::string& filename, const std::vector& assets, LoadFunc&& loadfunc) { - std::unique_ptr vfs(nullptr, [](mjVFS*){}); + mjVFS vfs; + mjVFS* vfs_ptr = nullptr; if (!assets.empty()) { - // mjVFS should be allocated on the heap, because it's ~2MB - vfs = decltype(vfs)(new mjVFS, [](mjVFS* vfs) { - mj_deleteVFS(vfs); - delete vfs; - }); - mj_defaultVFS(vfs.get()); + mj_defaultVFS(&vfs); + vfs_ptr = &vfs; for (const auto& asset : assets) { const int vfs_error = InterceptMjErrors(mj_addBufferVFS)( - vfs.get(), asset.name, asset.content, asset.content_size); + vfs_ptr, asset.name, asset.content, asset.content_size); if (vfs_error) { throw py::value_error("assets dict is too big"); } } } - raw::MjModel* model = loadfunc(filename.c_str(), vfs.get()); + raw::MjModel* model = loadfunc(filename.c_str(), vfs_ptr); + mj_deleteVFS(vfs_ptr); if (model && !model->buffer) { mj_deleteModel(model); model = nullptr; @@ -350,12 +349,6 @@ ConvertAssetsDict( std::vector out; if (assets.has_value()) { for (const auto& [name, content] : *assets) { - if (name.length() >= mjMAXVFSNAME) { - std::ostringstream error; - error << "Filename length " << name.length() << " exceeds " - << mjMAXVFSNAME - 1 << " character limit: " << name; - throw py::value_error(error.str()); - } out.emplace_back(name.c_str(), PYBIND11_BYTES_AS_STRING(content.ptr()), py::len(content)); } @@ -430,6 +423,27 @@ MjModelWrapper MjModelWrapper::LoadXML( return MjModelWrapper(model); } +MjModelWrapper MjModelWrapper::CompileSpec(raw::MjSpec* spec) { + auto m = mj_compile(spec, nullptr); + if (!m || mjs_isWarning(spec)) { + throw py::value_error(mjs_getError(spec)); + } + return MjModelWrapper(m); +} + +py::tuple RecompileSpec(raw::MjSpec* spec, const MjModelWrapper& old_m, + const MjDataWrapper& old_d) { + raw::MjModel* m = static_cast(mju_malloc(sizeof(mjModel))); + m->buffer = nullptr; + raw::MjData* d = mj_copyData(nullptr, old_m.get(), old_d.get()); + mj_recompile(spec, nullptr, m, d); + + py::object m_pyobj = py::cast((MjModelWrapper(m))); + py::object d_pyobj = + py::cast((MjDataWrapper(py::cast(m_pyobj), d))); + return py::make_tuple(m_pyobj, d_pyobj); +} + namespace { // A byte at the start of serialized mjModel structs, which can be incremented // when we change the serialization logic to reject pickles from an unsupported @@ -1056,7 +1070,6 @@ MjvGLCameraWrapper::MjWrapper(const MjvGLCameraWrapper& other) #define X(var) var(InitPyArray(ptr_->var, owner_)) MjvGeomWrapper::MjWrapper() : WrapperBase(new raw::MjvGeom{}), - X(texrepeat), X(size), X(pos), mat([this]() { @@ -1069,7 +1082,6 @@ MjvGeomWrapper::MjWrapper() MjvGeomWrapper::MjWrapper(raw::MjvGeom* ptr, py::handle owner) : WrapperBase(ptr, owner), - X(texrepeat), X(size), X(pos), mat([this]() { @@ -1557,6 +1569,11 @@ PYBIND11_MODULE(_structs, m) { py::arg("xml"), py::arg_v("assets", py::none()), py::doc( R"(Loads an MjModel from an XML string and an optional assets dictionary.)")); + mjModel.def_static( + "_from_spec_ptr", [](uintptr_t addr) { + return MjModelWrapper::CompileSpec( + reinterpret_cast(addr)); + }); mjModel.def_static( "from_xml_path", &MjModelWrapper::LoadXMLFile, py::arg("filename"), py::arg_v("assets", py::none()), @@ -1907,9 +1924,11 @@ This is useful for example when the MJB is not available as a file on disk.)")); mjData.def("__copy__", [](const MjDataWrapper& other) { return MjDataWrapper(other); }); - mjData.def("__deepcopy__", [](const MjDataWrapper& other, py::dict) { - MjModelWrapper* model_copy = new MjModelWrapper(other.model()); - return MjDataWrapper(other, model_copy); + mjData.def("__deepcopy__", [](const MjDataWrapper& other, py::dict memo) { + // Use copy.deepcopy(model) to make a model that Python is aware of. + py::object new_model_py = + py::cast(other.model()).attr("__deepcopy__")(memo); + return MjDataWrapper(other, new_model_py.cast()); }); mjData.def(py::pickle( [](const MjDataWrapper& d) { // __getstate__ @@ -2185,8 +2204,7 @@ This is useful for example when the MJB is not available as a file on disk.)")); X(objtype); X(objid); X(category); - X(texid); - X(texuniform); + X(matid); X(texcoord); X(segid); X(emission); @@ -2199,7 +2217,6 @@ This is useful for example when the MJB is not available as a file on disk.)")); #undef X #define X(var) DefinePyArray(mjvGeom, #var, &MjvGeomWrapper::var) - X(texrepeat); X(size); X(pos); X(mat); @@ -2404,5 +2421,12 @@ This is useful for example when the MJB is not available as a file on disk.)")); }, py::arg("cam1"), py::arg("cam2"), py::doc(python_traits::mjv_averageCamera::doc)); + + m.def( + "_recompile_spec_addr", + [](uintptr_t spec_addr, const MjModelWrapper& m, const MjDataWrapper& d) { + return RecompileSpec(reinterpret_cast(spec_addr), m, d); + } + ); } // PYBIND11_MODULE NOLINT(readability/fn_size) } // namespace mujoco::python::_impl diff --git a/python/mujoco/structs.h b/python/mujoco/structs.h index f27e17ff..70e399ce 100644 --- a/python/mujoco/structs.h +++ b/python/mujoco/structs.h @@ -462,6 +462,10 @@ class MjWrapper : public WrapperBase { public: MjWrapper(const MjWrapper&); MjWrapper(MjWrapper&&); + + // Takes ownership of the raw mjModel pointer. + explicit MjWrapper(raw::MjModel* ptr); + ~MjWrapper(); MjModelIndexer& indexer() { return indexer_; } @@ -485,6 +489,8 @@ class MjWrapper : public WrapperBase { const std::optional< std::unordered_map>& assets); + static MjWrapper CompileSpec(raw::MjSpec* spec); + static constexpr char kFromRawPointer[] = "__MUJOCO_STRUCTS_MJMODELWRAPPER_LOOKUP"; static MjWrapper* FromRawPointer(raw::MjModel* m) noexcept; @@ -502,8 +508,6 @@ class MjWrapper : public WrapperBase { pybind11::bytes paths_bytes; protected: - explicit MjWrapper(raw::MjModel* ptr); - MjModelIndexer indexer_; }; @@ -591,8 +595,14 @@ class MjWrapper: public WrapperBase { explicit MjWrapper(MjModelWrapper* model); MjWrapper(const MjWrapper& other); MjWrapper(MjWrapper&&); + // Used for deepcopy MjWrapper(const MjWrapper& other, MjModelWrapper* model); + + // Internal constructor which takes ownership of given mjData pointer. + // Used for deserialization and recompile. + explicit MjWrapper(MjModelWrapper* model, raw::MjData* d); + ~MjWrapper(); const MjModelWrapper& model() const { return *model_; } @@ -622,9 +632,6 @@ class MjWrapper: public WrapperBase { py_array_or_tuple_t energy; protected: - // Internal constructor which takes ownership of given mjData pointer. - // Used for deserialization. - explicit MjWrapper(MjModelWrapper* model, raw::MjData* d); raw::MjData* Copy() const; // A reference to the model that was used to create this mjData. @@ -727,7 +734,7 @@ class MjWrapper : public WrapperBase { py_array_or_tuple_t< \ std::remove_all_extents_t> \ var - X(texrepeat); + X(matid); X(size); X(pos); X(mat); diff --git a/python/mujoco/usd/exporter.py b/python/mujoco/usd/exporter.py index 25cd6747..3ed8ae70 100644 --- a/python/mujoco/usd/exporter.py +++ b/python/mujoco/usd/exporter.py @@ -247,7 +247,11 @@ class USDExporter: assert geom_name not in self.geom_names - texture_file = self.texture_files[geom.texid] if geom.texid != -1 else None + texture_file = ( + self.texture_files[self.model.mat_texid[mujoco.mjNTEXMAT*geom.matid]] + if geom.matid != -1 + else None + ) # handling meshes in our scene if geom.type == mujoco.mjtGeom.mjGEOM_MESH: diff --git a/python/mujoco/viewer.py b/python/mujoco/viewer.py index 4bed131b..28c55e44 100644 --- a/python/mujoco/viewer.py +++ b/python/mujoco/viewer.py @@ -94,6 +94,20 @@ class Handle: def user_scn(self): return self._user_scn + @property + def m(self): + sim = self._sim() + if sim is not None: + return sim.m + return None + + @property + def d(self): + sim = self._sim() + if sim is not None: + return sim.d + return None + def close(self): sim = self._sim() if sim is not None: diff --git a/python/setup.py b/python/setup.py index bc1934bb..7807c410 100644 --- a/python/setup.py +++ b/python/setup.py @@ -347,6 +347,7 @@ setuptools.setup( CMakeExtension('mujoco._render'), CMakeExtension('mujoco._rollout'), CMakeExtension('mujoco._simulate'), + CMakeExtension('mujoco._specs'), CMakeExtension('mujoco._structs'), ], scripts=[ diff --git a/python/tutorial.ipynb b/python/tutorial.ipynb index 1e2528bc..36f76de5 100644 --- a/python/tutorial.ipynb +++ b/python/tutorial.ipynb @@ -420,8 +420,8 @@ "data = mujoco.MjData(model)\n", "\n", "# Make renderer, render and show the pixels\n", - "renderer = mujoco.Renderer(model)\n", - "media.show_image(renderer.render())" + "with mujoco.Renderer(model) as renderer:\n", + " media.show_image(renderer.render())" ] }, { @@ -445,10 +445,11 @@ }, "outputs": [], "source": [ - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data)\n", + "with mujoco.Renderer(model) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data)\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -480,11 +481,11 @@ "model = mujoco.MjModel.from_xml_string(xml)\n", "data = mujoco.MjData(model)\n", "\n", - "renderer = mujoco.Renderer(model)\n", - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data)\n", + "with mujoco.Renderer(model) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data)\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -508,9 +509,10 @@ "source": [ "# Run this cell multiple times for different colors\n", "model.geom('red_box').rgba[:3] = np.random.rand(3)\n", - "renderer.update_scene(data)\n", + "with mujoco.Renderer(model) as renderer:\n", + " renderer.update_scene(data)\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -540,12 +542,13 @@ "# Simulate and display video.\n", "frames = []\n", "mujoco.mj_resetData(model, data) # Reset state and time.\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data)\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] @@ -583,7 +586,6 @@ "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(xml)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model)\n", "\n", "# enable joint visualization option:\n", "scene_option = mujoco.MjvOption()\n", @@ -595,12 +597,13 @@ "# Simulate and display video.\n", "frames = []\n", "mujoco.mj_resetData(model, data)\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data, scene_option=scene_option)\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data, scene_option=scene_option)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] @@ -651,12 +654,13 @@ "# Simulate and display video.\n", "frames = []\n", "mujoco.mj_resetData(model, data)\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data, scene_option=scene_option)\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data, scene_option=scene_option)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=60)" ] @@ -760,12 +764,12 @@ "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(tippe_top)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model)\n", "\n", "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data, camera=\"closeup\")\n", + "with mujoco.Renderer(model) as renderer:\n", + " renderer.update_scene(data, camera=\"closeup\")\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -821,12 +825,13 @@ "# Simulate and display video.\n", "frames = []\n", "mujoco.mj_resetDataKeyframe(model, data, 0) # Reset the state to keyframe 0\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data, \"closeup\")\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data, \"closeup\")\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] @@ -939,12 +944,14 @@ "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(chaotic_pendulum)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model, 480, 640)\n", + "height = 480\n", + "width = 640\n", "\n", - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data, camera=\"fixed\")\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data, camera=\"fixed\")\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -970,7 +977,8 @@ "framerate = 30 # Hz\n", "n_frames = int(n_seconds * framerate)\n", "frames = []\n", - "renderer = mujoco.Renderer(model, 240, 320)\n", + "height = 240\n", + "width = 320\n", "\n", "# set initial state\n", "mujoco.mj_resetData(model, data)\n", @@ -981,17 +989,18 @@ "sim_time = 0\n", "render_time = 0\n", "n_steps = 0\n", - "for i in range(n_frames):\n", - " while data.time * framerate < i:\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " for i in range(n_frames):\n", + " while data.time * framerate < i:\n", + " tic = time.time()\n", + " mujoco.mj_step(model, data)\n", + " sim_time += time.time() - tic\n", + " n_steps += 1\n", " tic = time.time()\n", - " mujoco.mj_step(model, data)\n", - " sim_time += time.time() - tic\n", - " n_steps += 1\n", - " tic = time.time()\n", - " renderer.update_scene(data, \"fixed\")\n", - " frame = renderer.render()\n", - " render_time += time.time() - tic\n", - " frames.append(frame)\n", + " renderer.update_scene(data, \"fixed\")\n", + " frame = renderer.render()\n", + " render_time += time.time() - tic\n", + " frames.append(frame)\n", "\n", "# print timing and play video\n", "step_time = 1e6*sim_time/n_steps\n", @@ -1233,12 +1242,14 @@ "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(free_body_MJCF)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model, 400, 600)\n", + "height = 400\n", + "width = 600\n", "\n", - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data, \"fixed\")\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data, \"fixed\")\n", "\n", - "media.show_image(renderer.render())" + " media.show_image(renderer.render())" ] }, { @@ -1262,7 +1273,6 @@ "height = 240\n", "width = 320\n", "frames = []\n", - "renderer = mujoco.Renderer(model, height, width)\n", "\n", "# visualize contact frames and forces, make body transparent\n", "options = mujoco.MjvOption()\n", @@ -1282,12 +1292,13 @@ "data.qvel[3:6] = 5*np.random.randn(3)\n", "\n", "# Simulate and display video.\n", - "for i in range(n_frames):\n", - " while data.time < i/120.0: #1/4x real time\n", - " mujoco.mj_step(model, data)\n", - " renderer.update_scene(data, \"track\", options)\n", - " frame = renderer.render()\n", - " frames.append(frame)\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " for i in range(n_frames):\n", + " while data.time < i/120.0: #1/4x real time\n", + " mujoco.mj_step(model, data)\n", + " renderer.update_scene(data, \"track\", options)\n", + " frame = renderer.render()\n", + " frames.append(frame)\n", "\n", "media.show_video(frames, fps=30)" ] @@ -1440,16 +1451,16 @@ "# load\n", "model = mujoco.MjModel.from_xml_string(MJCF)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model, height, width)\n", "\n", "# Simulate and display video.\n", - "mujoco.mj_resetData(model, data)\n", - "for i in range(n_frames):\n", - " while data.time < i/30.0:\n", - " mujoco.mj_step(model, data)\n", - " renderer.update_scene(data, \"y\")\n", - " frame = renderer.render()\n", - " frames.append(frame)\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " mujoco.mj_resetData(model, data)\n", + " for i in range(n_frames):\n", + " while data.time < i/30.0:\n", + " mujoco.mj_step(model, data)\n", + " renderer.update_scene(data, \"y\")\n", + " frame = renderer.render()\n", + " frames.append(frame)\n", "\n", "media.show_video(frames, fps=30)" ] @@ -1520,12 +1531,14 @@ "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(MJCF)\n", "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model, 480, 480)\n", + "height = 480\n", + "width = 480\n", "\n", - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data, \"fixed\")\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data, \"fixed\")\n", "\n", - "media.show_image(renderer.render())\n" + " media.show_image(renderer.render())" ] }, { @@ -1553,21 +1566,20 @@ "times = []\n", "sensordata = []\n", "\n", - "renderer = mujoco.Renderer(model, height, width)\n", - "\n", "# constant actuator signal\n", "mujoco.mj_resetData(model, data)\n", "data.ctrl = 20\n", "\n", "# Simulate and display video.\n", - "for i in range(n_frames):\n", - " while data.time < i/fps:\n", - " mujoco.mj_step(model, data)\n", - " times.append(data.time)\n", - " sensordata.append(data.sensor('accelerometer').data.copy())\n", - " renderer.update_scene(data, \"fixed\")\n", - " frame = renderer.render()\n", - " frames.append(frame)\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " for i in range(n_frames):\n", + " while data.time < i/fps:\n", + " mujoco.mj_step(model, data)\n", + " times.append(data.time)\n", + " sensordata.append(data.sensor('accelerometer').data.copy())\n", + " renderer.update_scene(data, \"fixed\")\n", + " frame = renderer.render()\n", + " frames.append(frame)\n", "\n", "media.show_video(frames, fps=fps)" ] @@ -1644,12 +1656,12 @@ "\n", "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(xml)\n", - "renderer = mujoco.Renderer(model)\n", "data = mujoco.MjData(model)\n", "\n", - "mujoco.mj_forward(model, data)\n", - "renderer.update_scene(data)\n", - "media.show_image(renderer.render())" + "with mujoco.Renderer(model) as renderer:\n", + " mujoco.mj_forward(model, data)\n", + " renderer.update_scene(data)\n", + " media.show_image(renderer.render())" ] }, { @@ -1664,9 +1676,10 @@ "\n", "scene_option.frame = mujoco.mjtFrame.mjFRAME_GEOM\n", "scene_option.flags[mujoco.mjtVisFlag.mjVIS_TRANSPARENT] = True\n", - "renderer.update_scene(data, scene_option=scene_option)\n", - "frame = renderer.render()\n", - "media.show_image(frame)" + "with mujoco.Renderer(model) as renderer:\n", + " renderer.update_scene(data, scene_option=scene_option)\n", + " frame = renderer.render()\n", + " media.show_image(frame)" ] }, { @@ -1679,25 +1692,24 @@ "source": [ "#@title Depth rendering {vertical-output: true}\n", "\n", - "# update renderer to render depth\n", - "renderer.enable_depth_rendering()\n", + "with mujoco.Renderer(model) as renderer:\n", + " # update renderer to render depth\n", + " renderer.enable_depth_rendering()\n", "\n", - "# reset the scene\n", - "renderer.update_scene(data)\n", + " # reset the scene\n", + " renderer.update_scene(data)\n", "\n", - "# depth is a float array, in meters.\n", - "depth = renderer.render()\n", + " # depth is a float array, in meters.\n", + " depth = renderer.render()\n", "\n", - "# Shift nearest values to the origin.\n", - "depth -= depth.min()\n", - "# Scale by 2 mean distances of near rays.\n", - "depth /= 2*depth[depth <= 1].mean()\n", - "# Scale to [0, 255]\n", - "pixels = 255*np.clip(depth, 0, 1)\n", + " # Shift nearest values to the origin.\n", + " depth -= depth.min()\n", + " # Scale by 2 mean distances of near rays.\n", + " depth /= 2*depth[depth <= 1].mean()\n", + " # Scale to [0, 255]\n", + " pixels = 255*np.clip(depth, 0, 1)\n", "\n", - "media.show_image(pixels.astype(np.uint8))\n", - "\n", - "renderer.disable_depth_rendering()" + " media.show_image(pixels.astype(np.uint8))" ] }, { @@ -1710,25 +1722,26 @@ "source": [ "#@title Segmentation rendering {vertical-output: true}\n", "\n", - "# update renderer to render segmentation\n", - "renderer.enable_segmentation_rendering()\n", + "with mujoco.Renderer(model) as renderer:\n", + " renderer.disable_depth_rendering()\n", "\n", - "# reset the scene\n", - "renderer.update_scene(data)\n", + " # update renderer to render segmentation\n", + " renderer.enable_segmentation_rendering()\n", "\n", - "seg = renderer.render()\n", + " # reset the scene\n", + " renderer.update_scene(data)\n", "\n", - "# Display the contents of the first channel, which contains object\n", - "# IDs. The second channel, seg[:, :, 1], contains object types.\n", - "geom_ids = seg[:, :, 0]\n", - "# Infinity is mapped to -1\n", - "geom_ids = geom_ids.astype(np.float64) + 1\n", - "# Scale to [0, 1]\n", - "geom_ids = geom_ids / geom_ids.max()\n", - "pixels = 255*geom_ids\n", - "media.show_image(pixels.astype(np.uint8))\n", + " seg = renderer.render()\n", "\n", - "renderer.disable_segmentation_rendering()" + " # Display the contents of the first channel, which contains object\n", + " # IDs. The second channel, seg[:, :, 1], contains object types.\n", + " geom_ids = seg[:, :, 0]\n", + " # Infinity is mapped to -1\n", + " geom_ids = geom_ids.astype(np.float64) + 1\n", + " # Scale to [0, 1]\n", + " geom_ids = geom_ids / geom_ids.max()\n", + " pixels = 255*geom_ids\n", + " media.show_image(pixels.astype(np.uint8))" ] }, { @@ -1792,37 +1805,39 @@ "source": [ "#@title Project from world to camera coordinates {vertical-output: true}\n", "\n", - "# reset the scene\n", - "renderer.update_scene(data)\n", + "with mujoco.Renderer(model) as renderer:\n", + " renderer.disable_segmentation_rendering()\n", + " # reset the scene\n", + " renderer.update_scene(data)\n", "\n", - "# Get the world coordinates of the box corners\n", - "box_pos = data.geom_xpos[model.geom('red_box').id]\n", - "box_mat = data.geom_xmat[model.geom('red_box').id].reshape(3, 3)\n", - "box_size = model.geom_size[model.geom('red_box').id]\n", - "offsets = np.array([-1, 1]) * box_size[:, None]\n", - "xyz_local = np.stack(list(itertools.product(*offsets))).T\n", - "xyz_global = box_pos[:, None] + box_mat @ xyz_local\n", + " # Get the world coordinates of the box corners\n", + " box_pos = data.geom_xpos[model.geom('red_box').id]\n", + " box_mat = data.geom_xmat[model.geom('red_box').id].reshape(3, 3)\n", + " box_size = model.geom_size[model.geom('red_box').id]\n", + " offsets = np.array([-1, 1]) * box_size[:, None]\n", + " xyz_local = np.stack(list(itertools.product(*offsets))).T\n", + " xyz_global = box_pos[:, None] + box_mat @ xyz_local\n", "\n", - "# Camera matrices multiply homogenous [x, y, z, 1] vectors.\n", - "corners_homogeneous = np.ones((4, xyz_global.shape[1]), dtype=float)\n", - "corners_homogeneous[:3, :] = xyz_global\n", + " # Camera matrices multiply homogenous [x, y, z, 1] vectors.\n", + " corners_homogeneous = np.ones((4, xyz_global.shape[1]), dtype=float)\n", + " corners_homogeneous[:3, :] = xyz_global\n", "\n", - "# Get the camera matrix.\n", - "m = compute_camera_matrix(renderer, data)\n", + " # Get the camera matrix.\n", + " m = compute_camera_matrix(renderer, data)\n", "\n", - "# Project world coordinates into pixel space. See:\n", - "# https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula\n", - "xs, ys, s = m @ corners_homogeneous\n", - "# x and y are in the pixel coordinate system.\n", - "x = xs / s\n", - "y = ys / s\n", + " # Project world coordinates into pixel space. See:\n", + " # https://en.wikipedia.org/wiki/3D_projection#Mathematical_formula\n", + " xs, ys, s = m @ corners_homogeneous\n", + " # x and y are in the pixel coordinate system.\n", + " x = xs / s\n", + " y = ys / s\n", "\n", - "# Render the camera view and overlay the projected corner coordinates.\n", - "pixels = renderer.render()\n", - "fig, ax = plt.subplots(1, 1)\n", - "ax.imshow(pixels)\n", - "ax.plot(x, y, '+', c='w')\n", - "ax.set_axis_off()" + " # Render the camera view and overlay the projected corner coordinates.\n", + " pixels = renderer.render()\n", + " fig, ax = plt.subplots(1, 1)\n", + " ax.imshow(pixels)\n", + " ax.plot(x, y, '+', c='w')\n", + " ax.set_axis_off()" ] }, { @@ -1894,17 +1909,18 @@ "mujoco.mj_resetData(model, data)\n", "mujoco.mj_forward(model, data)\n", "\n", - "while data.time < duration:\n", - " # append data to the traces\n", - " positions.append(data.geom_xpos[data.geom(\"green_sphere\").id].copy())\n", - " times.append(data.time)\n", - " speeds.append(get_geom_speed(model, data, \"green_sphere\"))\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data)\n", - " modify_scene(renderer.scene)\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model) as renderer:\n", + " while data.time < duration:\n", + " # append data to the traces\n", + " positions.append(data.geom_xpos[data.geom(\"green_sphere\").id].copy())\n", + " times.append(data.time)\n", + " speeds.append(get_geom_speed(model, data, \"green_sphere\"))\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data)\n", + " modify_scene(renderer.scene)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] @@ -2018,8 +2034,7 @@ "\n", "\"\"\"\n", "model = mujoco.MjModel.from_xml_string(dominos_xml)\n", - "data = mujoco.MjData(model)\n", - "renderer = mujoco.Renderer(model, height=1024, width=1440)" + "data = mujoco.MjData(model)\n" ] }, { @@ -2035,16 +2050,19 @@ "\n", "duration = 2.5 # (seconds)\n", "framerate = 60 # (Hz)\n", + "height = 1024\n", + "width = 1440\n", "\n", "# Simulate and display video.\n", "frames = []\n", "mujoco.mj_resetData(model, data) # Reset state and time.\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate:\n", - " renderer.update_scene(data, camera='top')\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate:\n", + " renderer.update_scene(data, camera='top')\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] @@ -2061,6 +2079,8 @@ "#@title Render from moving camera\n", "\n", "duration = 3 # (seconds)\n", + "height = 1024\n", + "width = 1440\n", "\n", "# find time when box is thrown (speed > 2cm/s)\n", "throw_time = 0.0\n", @@ -2115,13 +2135,14 @@ "slowdown = 4 # 4x slow-down\n", "mujoco.mj_resetData(model, data)\n", "frames = []\n", - "while data.time < duration:\n", - " mujoco.mj_step(model, data)\n", - " if len(frames) < data.time * framerate * slowdown:\n", - " cam.distance, cam.azimuth, cam.elevation, cam.lookat = cam_motion()\n", - " renderer.update_scene(data, cam)\n", - " pixels = renderer.render()\n", - " frames.append(pixels)\n", + "with mujoco.Renderer(model, height, width) as renderer:\n", + " while data.time < duration:\n", + " mujoco.mj_step(model, data)\n", + " if len(frames) < data.time * framerate * slowdown:\n", + " cam.distance, cam.azimuth, cam.elevation, cam.lookat = cam_motion()\n", + " renderer.update_scene(data, cam)\n", + " pixels = renderer.render()\n", + " frames.append(pixels)\n", "\n", "media.show_video(frames, fps=framerate)" ] diff --git a/sample/Makefile b/sample/Makefile deleted file mode 100644 index 3f2949d2..00000000 --- a/sample/Makefile +++ /dev/null @@ -1,12 +0,0 @@ -# This Makefile assumes that you have GLFW libraries and headers installed on, -# which is commonly available through your distro's package manager. -# On Debian and Ubuntu, GLFW can be installed via `apt install libglfw3-dev`. - -COMMON=-O2 -I../include -L../lib -std=c++17 -pthread -Wl,-no-as-needed -Wl,-rpath,'$$ORIGIN'/../lib - -.PHONY: all -all: - $(CXX) $(COMMON) testspeed.cc -lmujoco -o ../bin/testspeed - $(CXX) $(COMMON) compile.cc -lmujoco -o ../bin/compile - $(CXX) $(COMMON) basic.cc -lmujoco -lglfw -o ../bin/basic - $(CXX) $(COMMON) record.cc -lmujoco -lglfw -o ../bin/record diff --git a/sample/Makefile.macos b/sample/Makefile.macos deleted file mode 100644 index 07cd830e..00000000 --- a/sample/Makefile.macos +++ /dev/null @@ -1,18 +0,0 @@ -# This Makefile assumes that GLFW is installed via Homebrew. -# If your setup is different, you will need to set GLFWROOT manually. - -# This Makefile also assumes that MuJoCo.app is present in /Applications. - -GLFWROOT?=$(shell brew --prefix) -MUJOCOPATH?=/Applications/MuJoCo.app/Contents/Frameworks - -CFLAGS=-O2 -F$(MUJOCOPATH) -I$(GLFWROOT)/include -pthread -CXXFLAGS=$(CFLAGS) -std=c++17 -stdlib=libc++ -ALLFLAGS=$(CXXFLAGS) -L$(GLFWROOT)/lib -Wl,-rpath,$(MUJOCOPATH) - -.PHONY: all -all: - clang++ $(ALLFLAGS) testspeed.cc -framework mujoco -o testspeed - clang++ $(ALLFLAGS) compile.cc -framework mujoco -o compile - clang++ $(ALLFLAGS) basic.cc -framework mujoco -lglfw -o basic - clang++ $(ALLFLAGS) record.cc -framework mujoco -lglfw -o record diff --git a/sample/Makefile.windows b/sample/Makefile.windows deleted file mode 100644 index 9aa858c3..00000000 --- a/sample/Makefile.windows +++ /dev/null @@ -1,17 +0,0 @@ -# A copy of the GLFW library is required to build some sample programs. -# If this is not already installed on your system, download the WIN64 archive -# from https://github.com/glfw/glfw/releases, and copy files as follows: -# - Copy the entire `include/GLFW` subdirectory to `mujoco/include/GLFW`. -# - Copy glfw3dll.lib from the subdirectory corresponding into your compiler -# choice to `mujoco/lib/glfw3dll.lib`. -# - Copy glfw3.dll from the same subdirectory into `mujoco/bin/glfw3.dll`. - -COMMON=/O2 /MT /EHsc /arch:AVX /I../include /Fe../bin/ - -.PHONY: all -all: - cl $(COMMON) testspeed.cc ../lib/mujoco.lib - cl $(COMMON) compile.cc ../lib/mujoco.lib - cl $(COMMON) basic.cc ../lib/glfw3dll.lib ../lib/mujoco.lib - cl $(COMMON) record.cc ../lib/glfw3dll.lib ../lib/mujoco.lib - del *.obj diff --git a/sample/cmake/SampleOptions.cmake b/sample/cmake/SampleOptions.cmake index 3e3f080f..de146d89 100644 --- a/sample/cmake/SampleOptions.cmake +++ b/sample/cmake/SampleOptions.cmake @@ -104,6 +104,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang endif() endif() +if(NOT CMAKE_INTERPROCEDURAL_OPTIMIZATION AND (CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE STREQUAL "Debug")) + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) +endif() + include(MujocoHarden) set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS}) set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS}) diff --git a/sample/testspeed.cc b/sample/testspeed.cc index 66dabf3a..09757888 100644 --- a/sample/testspeed.cc +++ b/sample/testspeed.cc @@ -34,18 +34,17 @@ mjData* d[maxthread]; // per-thread statistics int contacts[maxthread]; int constraints[maxthread]; -double simtime[maxthread]; - +mjtNum simtime[maxthread]; // timer -std::chrono::steady_clock::time_point tm_start; mjtNum gettm(void) { - std::chrono::duration elapsed; - elapsed = std::chrono::steady_clock::now() - tm_start; + using std::chrono::steady_clock; + using Microseconds = std::chrono::duration; + static steady_clock::time_point tm_start = steady_clock::now(); + auto elapsed = Microseconds(steady_clock::now() - tm_start); return elapsed.count(); } - // deallocate and print message int finish(const char* msg = NULL, mjModel* m = NULL) { // deallocate model @@ -87,7 +86,7 @@ void simulate(int id, int nstep, mjtNum* ctrl) { constraints[id] = 0; // run and time - double start = gettm(); + mjtNum start = gettm(); for (int i=0; i < nstep; i++) { // inject pseudo-random control noise mju_copy(d[id]->ctrl, ctrl + i*m->nu, m->nu); @@ -126,7 +125,7 @@ int main(int argc, char** argv) { // read arguments int nstep = 10000, nthread = 0, npoolthread = 0; // inject small noise by default, to avoid fixed contact state - mjtNum ctrlnoise = 0.01; + double ctrlnoise = 0.01; if (argc > 2 && (std::sscanf(argv[2], "%d", &nstep) != 1 || nstep <= 0)) { return finish("Invalid nstep argument"); } @@ -149,7 +148,7 @@ int main(int argc, char** argv) { // get filename, determine file type std::string filename(argv[1]); - bool binary = (filename.find(".mjb") != std::string::npos); + bool binary = (filename.find(".mjb") != std::string::npos); // NOLINT // load model char error[1000] = "Could not load binary model"; @@ -191,8 +190,11 @@ int main(int argc, char** argv) { nstep, nthread > 1 ? " per thread" : "", m->opt.timestep); + if (sizeof(mjtNum) == 4) { + std::printf(", using single-precision"); + } if (npoolthread > 1) { - std::printf(", using %d threads for engine-internal threadpool", npoolthread); + std::printf(", using %d threads", npoolthread); } std::printf("...\n\n"); diff --git a/simulate/Makefile b/simulate/Makefile deleted file mode 100644 index f0834cbe..00000000 --- a/simulate/Makefile +++ /dev/null @@ -1,11 +0,0 @@ -# This Makefile assumes that you have GLFW libraries and headers installed on, -# which is commonly available through your distro's package manager. -# On Debian and Ubuntu, GLFW can be installed via `apt install libglfw3-dev`. - -COMMON=-O2 -I../include -L../lib -pthread -Wl,-no-as-needed -Wl,-rpath,'$$ORIGIN'/../lib - -all: - $(CXX) $(COMMON) -std=c++17 -c simulate.cc - $(CC) $(COMMON) -std=c11 -c uitools.c - $(CXX) $(COMMON) -std=c++17 main.cc simulate.o uitools.o -lmujoco -lglfw -o ../bin/simulate - rm uitools.o simulate.o diff --git a/simulate/Makefile.macos b/simulate/Makefile.macos deleted file mode 100644 index ae401473..00000000 --- a/simulate/Makefile.macos +++ /dev/null @@ -1,18 +0,0 @@ -# This Makefile assumes that GLFW is installed via Homebrew. -# If your setup is different, you will need to set GLFWROOT manually. - -# This Makefile also assumes that MuJoCo.app is present in /Applications. - -GLFWROOT?=$(shell brew --prefix) -MUJOCOPATH?=/Applications/MuJoCo.app/Contents/Frameworks - -CFLAGS=-O2 -F$(MUJOCOPATH) -I$(GLFWROOT)/include -pthread -CXXFLAGS=$(CFLAGS) -std=c++17 -stdlib=libc++ -ALLFLAGS=$(CXXFLAGS) -L$(GLFWROOT)/lib -Wl,-rpath,$(MUJOCOPATH) - -all: - clang++ $(CXXFLAGS) -c macos_gui.mm - clang++ $(CXXFLAGS) -c simulate.cc - clang $(CFLAGS) -std=c11 -c uitools.c - clang++ $(CXXFLAGS) main.cc macos_gui.o simulate.o uitools.o -framework mujoco -framework Cocoa -lglfw -o simulate - rm *.o diff --git a/simulate/Makefile.windows b/simulate/Makefile.windows deleted file mode 100644 index 957929e4..00000000 --- a/simulate/Makefile.windows +++ /dev/null @@ -1,13 +0,0 @@ -# A copy of the GLFW library is required to build some sample programs. -# If this is not already installed on your system, download the WIN64 archive -# from https://github.com/glfw/glfw/releases, and copy files as follows: -# - Copy the entire `include/GLFW` subdirectory to `mujoco/include/GLFW`. -# - Copy glfw3dll.lib from the subdirectory corresponding into your compiler -# choice to `mujoco/lib/glfw3dll.lib`. -# - Copy glfw3.dll from the same subdirectory into `mujoco/bin/glfw3.dll`. - -COMMON=/O2 /MT /EHsc /arch:AVX /I../include /Fe../bin/ - -all: - cl $(COMMON) main.cc simulate.cc uitools.c ../lib/glfw3dll.lib ../lib/mujoco.lib - del *.obj diff --git a/simulate/cmake/SimulateOptions.cmake b/simulate/cmake/SimulateOptions.cmake index 3e3f080f..de146d89 100644 --- a/simulate/cmake/SimulateOptions.cmake +++ b/simulate/cmake/SimulateOptions.cmake @@ -104,6 +104,10 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" OR (CMAKE_CXX_COMPILER_ID MATCHES "Clang endif() endif() +if(NOT CMAKE_INTERPROCEDURAL_OPTIMIZATION AND (CMAKE_BUILD_TYPE AND NOT CMAKE_BUILD_TYPE STREQUAL "Debug")) + set(CMAKE_INTERPROCEDURAL_OPTIMIZATION ON) +endif() + include(MujocoHarden) set(EXTRA_COMPILE_OPTIONS ${EXTRA_COMPILE_OPTIONS} ${MUJOCO_HARDEN_COMPILE_OPTIONS}) set(EXTRA_LINK_OPTIONS ${EXTRA_LINK_OPTIONS} ${MUJOCO_HARDEN_LINK_OPTIONS}) diff --git a/simulate/main.cc b/simulate/main.cc index 30139f8f..397a83c9 100644 --- a/simulate/main.cc +++ b/simulate/main.cc @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include #include #include @@ -334,7 +335,7 @@ void PhysicsLoop(mj::Simulate& sim) { // misalignment condition: distance from target sim time is bigger than syncmisalign bool misaligned = - mju_abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign; + std::abs(Seconds(elapsedCPU).count()/slowdown - elapsedSim) > syncMisalign; // out-of-sync (for any reason): reset sync times, step if (elapsedSim < 0 || elapsedCPU.count() < 0 || syncCPU.time_since_epoch().count() == 0 || diff --git a/simulate/simulate.cc b/simulate/simulate.cc index a86c7343..17ae8db6 100644 --- a/simulate/simulate.cc +++ b/simulate/simulate.cc @@ -117,13 +117,13 @@ enum { // file section of UI const mjuiDef defFile[] = { - {mjITEM_SECTION, "File", 1, nullptr, "AF"}, - {mjITEM_BUTTON, "Save xml", 2, nullptr, ""}, - {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""}, - {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"}, - {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"}, - {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"}, - {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"}, + {mjITEM_SECTION, "File", mjPRESERVE, nullptr, "AF"}, + {mjITEM_BUTTON, "Save xml", 2, nullptr, ""}, + {mjITEM_BUTTON, "Save mjb", 2, nullptr, ""}, + {mjITEM_BUTTON, "Print model", 2, nullptr, "CM"}, + {mjITEM_BUTTON, "Print data", 2, nullptr, "CD"}, + {mjITEM_BUTTON, "Quit", 1, nullptr, "CQ"}, + {mjITEM_BUTTON, "Screenshot", 2, nullptr, "CP"}, {mjITEM_END} }; @@ -674,15 +674,15 @@ void UpdateWatch(mj::Simulate* sim, const mjModel* m, const mjData* d) { //---------------------------------- UI construction ----------------------------------------------- // make physics section of UI -void MakePhysicsSection(mj::Simulate* sim, int oldstate) { +void MakePhysicsSection(mj::Simulate* sim) { mjOption* opt = sim->is_passive_ ? &sim->scnstate_.model.opt : &sim->m_->opt; mjuiDef defPhysics[] = { - {mjITEM_SECTION, "Physics", oldstate, nullptr, "AP"}, + {mjITEM_SECTION, "Physics", mjPRESERVE, nullptr, "AP"}, {mjITEM_SELECT, "Integrator", 2, &(opt->integrator), "Euler\nRK4\nimplicit\nimplicitfast"}, {mjITEM_SELECT, "Cone", 2, &(opt->cone), "Pyramidal\nElliptic"}, {mjITEM_SELECT, "Jacobian", 2, &(opt->jacobian), "Dense\nSparse\nAuto"}, {mjITEM_SELECT, "Solver", 2, &(opt->solver), "PGS\nCG\nNewton"}, - {mjITEM_SEPARATOR, "Algorithmic Parameters", 1}, + {mjITEM_SEPARATOR, "Algorithmic Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Timestep", 2, &(opt->timestep), "1 0 1"}, {mjITEM_EDITINT, "Iterations", 2, &(opt->iterations), "1 0 1000"}, {mjITEM_EDITNUM, "Tolerance", 2, &(opt->tolerance), "1 0 1"}, @@ -695,22 +695,22 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_EDITNUM, "API Rate", 2, &(opt->apirate), "1 0 1000"}, {mjITEM_EDITINT, "SDF Iter", 2, &(opt->sdf_iterations), "1 1 20"}, {mjITEM_EDITINT, "SDF Init", 2, &(opt->sdf_initpoints), "1 1 100"}, - {mjITEM_SEPARATOR, "Physical Parameters", 1}, + {mjITEM_SEPARATOR, "Physical Parameters", mjPRESERVE}, {mjITEM_EDITNUM, "Gravity", 2, opt->gravity, "3"}, {mjITEM_EDITNUM, "Wind", 2, opt->wind, "3"}, {mjITEM_EDITNUM, "Magnetic", 2, opt->magnetic, "3"}, {mjITEM_EDITNUM, "Density", 2, &(opt->density), "1"}, {mjITEM_EDITNUM, "Viscosity", 2, &(opt->viscosity), "1"}, {mjITEM_EDITNUM, "Imp Ratio", 2, &(opt->impratio), "1"}, - {mjITEM_SEPARATOR, "Disable Flags", 1}, + {mjITEM_SEPARATOR, "Disable Flags", mjPRESERVE}, {mjITEM_END} }; mjuiDef defEnableFlags[] = { - {mjITEM_SEPARATOR, "Enable Flags", 1}, + {mjITEM_SEPARATOR, "Enable Flags", mjPRESERVE}, {mjITEM_END} }; mjuiDef defOverride[] = { - {mjITEM_SEPARATOR, "Contact Override", 1}, + {mjITEM_SEPARATOR, "Contact Override", mjPRESERVE}, {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"}, @@ -718,7 +718,7 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { {mjITEM_END} }; mjuiDef defDisableActuator[] = { - {mjITEM_SEPARATOR, "Actuator Group Enable", 1}, + {mjITEM_SEPARATOR, "Actuator Group Enable", mjPRESERVE}, {mjITEM_CHECKBYTE, "Act Group 0", 2, sim->enableactuator+0, ""}, {mjITEM_CHECKBYTE, "Act Group 1", 2, sim->enableactuator+1, ""}, {mjITEM_CHECKBYTE, "Act Group 2", 2, sim->enableactuator+2, ""}, @@ -752,57 +752,39 @@ void MakePhysicsSection(mj::Simulate* sim, int oldstate) { // add actuator group enable/disable mjui_add(&sim->ui0, defDisableActuator); + + // make some subsections closed by default + for (int i=0; i < sim->ui0.sect[SECT_PHYSICS].nitem; i++) { + mjuiItem* it = sim->ui0.sect[SECT_PHYSICS].item + i; + + // close less useful subsections + if (it->type == mjITEM_SEPARATOR) { + if (mju::strcmp_arr(it->name, "Actuator Group Enable") && + mju::strcmp_arr(it->name, "Contact Override") && + mju::strcmp_arr(it->name, "Physical Parameters")) { + it->state = mjSEPCLOSED+1; + } + } + } } // make rendering section of UI -void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) { +void MakeRenderingSection(mj::Simulate* sim, const mjModel* m) { mjuiDef defRendering[] = { - { - mjITEM_SECTION, - "Rendering", - oldstate, - nullptr, - "AR" - }, - { - mjITEM_SELECT, - "Camera", - 2, - &(sim->camera), - "Free\nTracking" - }, - { - mjITEM_SELECT, - "Label", - 2, - &(sim->opt.label), + {mjITEM_SECTION, "Rendering", mjPRESERVE, nullptr, "AR"}, + {mjITEM_SELECT, "Camera", 2, &(sim->camera), "Free\nTracking"}, + {mjITEM_SELECT, "Label", 2, &(sim->opt.label), "None\nBody\nJoint\nGeom\nSite\nCamera\nLight\nTendon\n" "Actuator\nConstraint\nFlex\nSkin\nSelection\nSel Pnt\nContact\nForce\nIsland" }, - { - mjITEM_SELECT, - "Frame", - 2, - &(sim->opt.frame), + {mjITEM_SELECT, "Frame", 2, &(sim->opt.frame), "None\nBody\nGeom\nSite\nCamera\nLight\nContact\nWorld" }, - { - mjITEM_BUTTON, - "Copy camera", - 2, - nullptr, - "" - }, - { - mjITEM_SEPARATOR, - "Model Elements", - 1 - }, - { - mjITEM_END - } + {mjITEM_BUTTON, "Copy camera", 2, nullptr, ""}, + {mjITEM_SEPARATOR, "Model Elements", 1}, + {mjITEM_END} }; mjuiDef defOpenGL[] = { {mjITEM_SEPARATOR, "OpenGL Effects", 1}, @@ -876,18 +858,18 @@ void MakeRenderingSection(mj::Simulate* sim, const mjModel* m, int oldstate) { } // make visualization section of UI -void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) { +void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m) { mjStatistic* stat = sim->is_passive_ ? &sim->scnstate_.model.stat : &sim->m_->stat; mjVisual* vis = sim->is_passive_ ? &sim->scnstate_.model.vis : &sim->m_->vis; mjuiDef defVisualization[] = { - {mjITEM_SECTION, "Visualization", oldstate, nullptr, "AV"}, + {mjITEM_SECTION, "Visualization", mjPRESERVE, nullptr, "AV"}, {mjITEM_SEPARATOR, "Headlight", 1}, {mjITEM_RADIO, "Active", 5, &(vis->headlight.active), "Off\nOn"}, {mjITEM_EDITFLOAT, "Ambient", 2, &(vis->headlight.ambient), "3"}, {mjITEM_EDITFLOAT, "Diffuse", 2, &(vis->headlight.diffuse), "3"}, {mjITEM_EDITFLOAT, "Specular", 2, &(vis->headlight.specular), "3"}, - {mjITEM_SEPARATOR, "Free Camera", 1}, + {mjITEM_SEPARATOR, "Free Camera", 1}, {mjITEM_RADIO, "Orthographic", 2, &(vis->global.orthographic), "No\nYes"}, {mjITEM_EDITFLOAT, "Field of view", 2, &(vis->global.fovy), "1"}, {mjITEM_EDITNUM, "Center", 2, &(stat->center), "3"}, @@ -911,8 +893,8 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) {mjITEM_EDITFLOAT, "Haze", 2, &(vis->map.haze), "1"}, {mjITEM_EDITFLOAT, "Shadow clip", 2, &(vis->map.shadowclip), "1"}, {mjITEM_EDITFLOAT, "Shadow scale", 2, &(vis->map.shadowscale), "1"}, - {mjITEM_SEPARATOR, "Scale", 1}, - {mjITEM_EDITNUM, "All [meansize]", 2, &(stat->meansize), "1"}, + {mjITEM_SEPARATOR, "Scale", mjPRESERVE}, + {mjITEM_EDITNUM, "All (meansize)", 2, &(stat->meansize), "1"}, {mjITEM_EDITFLOAT, "Force width", 2, &(vis->scale.forcewidth), "1"}, {mjITEM_EDITFLOAT, "Contact width", 2, &(vis->scale.contactwidth), "1"}, {mjITEM_EDITFLOAT, "Contact height", 2, &(vis->scale.contactheight), "1"}, @@ -929,17 +911,43 @@ void MakeVisualizationSection(mj::Simulate* sim, const mjModel* m, int oldstate) {mjITEM_EDITFLOAT, "Frame width", 2, &(vis->scale.framewidth), "1"}, {mjITEM_EDITFLOAT, "Constraint", 2, &(vis->scale.constraint), "1"}, {mjITEM_EDITFLOAT, "Slider-crank", 2, &(vis->scale.slidercrank), "1"}, + {mjITEM_SEPARATOR, "RGBA", mjPRESERVE}, + {mjITEM_EDITFLOAT, "fog", 2, &(vis->rgba.fog), "4"}, + {mjITEM_EDITFLOAT, "haze", 2, &(vis->rgba.haze), "4"}, + {mjITEM_EDITFLOAT, "force", 2, &(vis->rgba.force), "4"}, + {mjITEM_EDITFLOAT, "inertia", 2, &(vis->rgba.inertia), "4"}, + {mjITEM_EDITFLOAT, "joint", 2, &(vis->rgba.joint), "4"}, + {mjITEM_EDITFLOAT, "actuator", 2, &(vis->rgba.actuator), "4"}, + {mjITEM_EDITFLOAT, "actnegative", 2, &(vis->rgba.actuatornegative), "4"}, + {mjITEM_EDITFLOAT, "actpositive", 2, &(vis->rgba.actuatorpositive), "4"}, + {mjITEM_EDITFLOAT, "com", 2, &(vis->rgba.com), "4"}, + {mjITEM_EDITFLOAT, "camera", 2, &(vis->rgba.camera), "4"}, + {mjITEM_EDITFLOAT, "light", 2, &(vis->rgba.light), "4"}, + {mjITEM_EDITFLOAT, "selectpoint", 2, &(vis->rgba.selectpoint), "4"}, + {mjITEM_EDITFLOAT, "connect", 2, &(vis->rgba.connect), "4"}, + {mjITEM_EDITFLOAT, "contactpoint", 2, &(vis->rgba.contactpoint), "4"}, + {mjITEM_EDITFLOAT, "contactforce", 2, &(vis->rgba.contactforce), "4"}, + {mjITEM_EDITFLOAT, "contactfriction", 2, &(vis->rgba.contactfriction), "4"}, + {mjITEM_EDITFLOAT, "contacttorque", 2, &(vis->rgba.contacttorque), "4"}, + {mjITEM_EDITFLOAT, "contactgap", 2, &(vis->rgba.contactgap), "4"}, + {mjITEM_EDITFLOAT, "rangefinder", 2, &(vis->rgba.rangefinder), "4"}, + {mjITEM_EDITFLOAT, "constraint", 2, &(vis->rgba.constraint), "4"}, + {mjITEM_EDITFLOAT, "slidercrank", 2, &(vis->rgba.slidercrank), "4"}, + {mjITEM_EDITFLOAT, "crankbroken", 2, &(vis->rgba.crankbroken), "4"}, + {mjITEM_EDITFLOAT, "frustum", 2, &(vis->rgba.frustum), "4"}, + {mjITEM_EDITFLOAT, "bv", 2, &(vis->rgba.bv), "4"}, + {mjITEM_EDITFLOAT, "bvactive", 2, &(vis->rgba.bvactive), "4"}, {mjITEM_END} }; - // add rendering standard + // add visualization section mjui_add(&sim->ui0, defVisualization); } // make group section of UI -void MakeGroupSection(mj::Simulate* sim, int oldstate) { +void MakeGroupSection(mj::Simulate* sim) { mjuiDef defGroup[] = { - {mjITEM_SECTION, "Group enable", oldstate, nullptr, "AG"}, + {mjITEM_SECTION, "Group enable", mjPRESERVE, nullptr, "AG"}, {mjITEM_SEPARATOR, "Geom groups", 1}, {mjITEM_CHECKBYTE, "Geom 0", 2, sim->opt.geomgroup, " 0"}, {mjITEM_CHECKBYTE, "Geom 1", 2, sim->opt.geomgroup+1, " 1"}, @@ -997,9 +1005,9 @@ void MakeGroupSection(mj::Simulate* sim, int oldstate) { } // make joint section of UI -void MakeJointSection(mj::Simulate* sim, int oldstate) { +void MakeJointSection(mj::Simulate* sim) { mjuiDef defJoint[] = { - {mjITEM_SECTION, "Joint", oldstate, nullptr, "AJ"}, + {mjITEM_SECTION, "Joint", mjPRESERVE, nullptr, "AJ"}, {mjITEM_END} }; mjuiDef defSlider[] = { @@ -1050,9 +1058,9 @@ void MakeJointSection(mj::Simulate* sim, int oldstate) { } // make control section of UI -void MakeControlSection(mj::Simulate* sim, int oldstate) { +void MakeControlSection(mj::Simulate* sim) { mjuiDef defControl[] = { - {mjITEM_SECTION, "Control", oldstate, nullptr, "AC"}, + {mjITEM_SECTION, "Control", mjPRESERVE, nullptr, "AC"}, {mjITEM_BUTTON, "Clear all", 2}, {mjITEM_END} }; @@ -1107,35 +1115,17 @@ void MakeControlSection(mj::Simulate* sim, int oldstate) { // make model-dependent UI sections void MakeUiSections(mj::Simulate* sim, const mjModel* m, const mjData* d) { - // get section open-close state, UI 0 - int oldstate0[NSECT0]; - for (int i=0; iui0.nsect>i) { - oldstate0[i] = sim->ui0.sect[i].state; - } - } - - // get section open-close state, UI 1 - int oldstate1[NSECT1]; - for (int i=0; iui1.nsect>i) { - oldstate1[i] = sim->ui1.sect[i].state; - } - } - // clear model-dependent sections of UI sim->ui0.nsect = SECT_PHYSICS; sim->ui1.nsect = 0; // make - MakePhysicsSection(sim, oldstate0[SECT_PHYSICS]); - MakeRenderingSection(sim, m, oldstate0[SECT_RENDERING]); - MakeVisualizationSection(sim, m, oldstate0[SECT_VISUALIZATION]); - MakeGroupSection(sim, oldstate0[SECT_GROUP]); - MakeJointSection(sim, oldstate1[SECT_JOINT]); - MakeControlSection(sim, oldstate1[SECT_CONTROL]); + MakePhysicsSection(sim); + MakeRenderingSection(sim, m); + MakeVisualizationSection(sim, m); + MakeGroupSection(sim); + MakeJointSection(sim); + MakeControlSection(sim); } //---------------------------------- utility functions --------------------------------------------- @@ -1165,7 +1155,9 @@ void CopyPose(mj::Simulate* sim, const mjModel* m, const mjData* d) { // millisecond timer, for MuJoCo built-in profiler mjtNum Timer() { - return Milliseconds(mj::Simulate::Clock::now().time_since_epoch()).count(); + static auto start = mj::Simulate::Clock::now(); + auto elapsed = Milliseconds(mj::Simulate::Clock::now() - start); + return elapsed.count(); } // clear all times @@ -1263,7 +1255,7 @@ int ComputeFontScale(const mj::PlatformUIAdapter& platform_ui) { fs = 150; } fs = mju_round(fs * 0.02) * 50; - fs = mjMIN(250, mjMAX(100, fs)); + fs = mjMIN(300, mjMAX(100, fs)); return fs; } @@ -1320,9 +1312,22 @@ void UiLayout(mjuiState* state) { rect[3].height = rect[0].height; } +// modify UI void UiModify(mjUI* ui, mjuiState* state, mjrContext* con) { mjui_resize(ui, con); - mjr_addAux(ui->auxid, ui->width, ui->maxheight, ui->spacing.samples, con); + + // remake aux buffer only if missing or different + int id = ui->auxid; + if (con->auxFBO[id] == 0 || + con->auxFBO_r[id] == 0 || + con->auxColor[id] == 0 || + con->auxColor_r[id] == 0 || + con->auxWidth[id] != ui->width || + con->auxHeight[id] != ui->maxheight || + con->auxSamples[id] != ui->spacing.samples) { + mjr_addAux(id, ui->width, ui->maxheight, ui->spacing.samples, con); + } + UiLayout(state); mjui_update(-1, -1, ui, state, con); } @@ -1502,7 +1507,7 @@ void UiEvent(mjuiState* state) { // remake joint section if joint group changed if (it->name[0]=='J' && it->name[1]=='o') { sim->ui1.nsect = SECT_JOINT; - MakeJointSection(sim, sim->ui1.sect[SECT_JOINT].state); + MakeJointSection(sim); sim->ui1.nsect = NSECT1; UiModify(&sim->ui1, state, &sim->platform_ui->mjr_context()); } @@ -2455,7 +2460,7 @@ void Simulate::Render() { if (pending_.ui_remake_ctrl) { if (this->ui1_enable && this->ui1.sect[SECT_CONTROL].state) { this->ui1.nsect = SECT_CONTROL; - MakeControlSection(this, this->ui1.sect[SECT_CONTROL].state); + MakeControlSection(this); this->ui1.nsect = NSECT1; UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); } @@ -2643,12 +2648,15 @@ void Simulate::RenderLoop() { this->platform_ui->SetEventCallback(UiEvent); this->platform_ui->SetLayoutCallback(UiLayout); - // populate uis with standard sections + // populate uis with standard sections, open some sections initially this->ui0.userdata = this; this->ui1.userdata = this; mjui_add(&this->ui0, defFile); mjui_add(&this->ui0, this->def_option); mjui_add(&this->ui0, this->def_simulation); + this->ui0.sect[0].state = 1; + this->ui0.sect[1].state = 1; + this->ui0.sect[2].state = 1; mjui_add(&this->ui0, this->def_watch); UiModify(&this->ui0, &this->uistate, &this->platform_ui->mjr_context()); UiModify(&this->ui1, &this->uistate, &this->platform_ui->mjr_context()); diff --git a/simulate/simulate.h b/simulate/simulate.h index 5c385a74..bffacb37 100644 --- a/simulate/simulate.h +++ b/simulate/simulate.h @@ -265,7 +265,7 @@ class Simulate { // Constant arrays needed for the option section of UI and the UI interface // TODO setting the size here is not ideal const mjuiDef def_option[13] = { - {mjITEM_SECTION, "Option", 1, nullptr, "AO"}, + {mjITEM_SECTION, "Option", mjPRESERVE, nullptr, "AO"}, {mjITEM_CHECKINT, "Help", 2, &this->help, " #290"}, {mjITEM_CHECKINT, "Info", 2, &this->info, " #291"}, {mjITEM_CHECKINT, "Profiler", 2, &this->profiler, " #292"}, @@ -287,7 +287,7 @@ class Simulate { // simulation section of UI const mjuiDef def_simulation[14] = { - {mjITEM_SECTION, "Simulation", 1, nullptr, "AS"}, + {mjITEM_SECTION, "Simulation", mjPRESERVE, nullptr, "AS"}, {mjITEM_RADIO, "", 5, &this->run, "Pause\nRun"}, {mjITEM_BUTTON, "Reset", 2, nullptr, " #259"}, {mjITEM_BUTTON, "Reload", 5, nullptr, "CL"}, @@ -306,7 +306,7 @@ class Simulate { // watch section of UI const mjuiDef def_watch[5] = { - {mjITEM_SECTION, "Watch", 0, nullptr, "AW"}, + {mjITEM_SECTION, "Watch", mjPRESERVE, nullptr, "AW"}, {mjITEM_EDITTXT, "Field", 2, this->field, "qpos"}, {mjITEM_EDITINT, "Index", 2, &this->index, "1"}, {mjITEM_STATIC, "Value", 2, nullptr, " "}, diff --git a/src/engine/CMakeLists.txt b/src/engine/CMakeLists.txt index 0aacf637..b1ec5d23 100644 --- a/src/engine/CMakeLists.txt +++ b/src/engine/CMakeLists.txt @@ -21,6 +21,8 @@ set(MUJOCO_ENGINE_SRCS engine_collision_convex.h engine_collision_driver.c engine_collision_driver.h + engine_collision_gjk.c + engine_collision_gjk.h engine_collision_primitive.c engine_collision_primitive.h engine_collision_sdf.c @@ -45,6 +47,8 @@ set(MUJOCO_ENGINE_SRCS engine_io.c engine_io.h engine_macro.h + engine_name.c + engine_name.h engine_passive.c engine_passive.h engine_plugin.cc diff --git a/src/engine/engine_collision_box.c b/src/engine/engine_collision_box.c index 51471332..17bbecbe 100644 --- a/src/engine/engine_collision_box.c +++ b/src/engine/engine_collision_box.c @@ -806,8 +806,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 if (q2) { mju_mulMatMatT3(r, rotmore, rot); - // mju_rotVecMat(p,pos12,rotmore); - // mju_rotVecMat(tmp1,size2,rotmore); + // mju_mulMatVec3(p,rotmore,pos12); + // mju_mulMatVec3(tmp1,rotmore,size2); rotaxis(p, pos12); rotaxis(tmp1, size2); @@ -818,8 +818,8 @@ int mjc_BoxBox(const mjModel* M, const mjData* D, mjContact* con, int g1, int g2 rotmatx(r, rot); - // mju_rotVecMat(p,pos21,rotmore); - // mju_rotVecMat(tmp1,size1,rotmore); + // mju_mulMatVec3(p,rotmore,pos21); + // mju_mulMatVec3(tmp1,rotmore,size1); rotaxis(p, pos21); rotaxis(tmp1, size1); @@ -1074,8 +1074,8 @@ edgeedge: f2 = -1; } - // mju_rotVecMat(p,pos21,rotmore); - // mju_rotVecMat(rnorm,clnorm,rotmore); + // mju_mulMatVec3(p,rotmore,pos21); + // mju_mulMatVec3(rnorm,rotmore,clnorm); rotaxis(p, pos21); rotaxis(rnorm, clnorm); diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 54f72ad5..5ba14c02 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -34,25 +34,31 @@ // 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; + mjc_center(center->v, (const mjCCDObj*) obj); +} + + + +// center function for convex collision algorithms +void mjc_center(mjtNum res[3], const mjCCDObj *obj) { + int g = obj->geom; + int f = obj->flex; + int e = obj->elem; + int v = obj->vert; // return geom position if (g >= 0) { - mju_copy3(center->v, ccd->data->geom_xpos + 3*g); + mju_copy3(res, obj->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)); + mju_copy3(res, obj->data->flexelem_aabb + 6*(obj->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)); + mju_copy3(res, obj->data->flexvert_xpos + 3*(obj->model->flex_vertadr[f]+v)); } } @@ -60,21 +66,25 @@ void mjccd_center(const void *obj, ccd_vec3_t *center) { // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { - const mjtCCD* ccd = (const mjtCCD*)obj; - const mjModel* m = ccd->model; - const mjData* d = ccd->data; - int g = ccd->geom; + mjc_support(vec->v, (mjCCDObj*) obj, _dir->v); +} + + + +// support function for convex collision algorithms +void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { + const mjModel* m = obj->model; + const mjData* d = obj->data; + int g = obj->geom; //-------------------------- flex element or vertex ----------------------------- if (g < 0) { - int f = ccd->flex; + int f = obj->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; + if (obj->elem >= 0) { + int e = obj->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]; @@ -92,14 +102,14 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // add radius and margin/2 - mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*ccd->margin); + mju_addToScl3(res, dir, m->flex_radius[f] + 0.5*obj->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); + const mjtNum* vert = d->flexvert_xpos + 3*(m->flex_vertadr[f] + obj->vert); + mju_addScl3(res, vert, dir, m->flex_radius[f] + 0.5*obj->margin); return; } } @@ -111,30 +121,29 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjtNum tmp, vdot; const mjtNum* size = m->geom_size+3*g; // geom sizes - mjtNum dir[3]; // direction in geom local frame - mjtNum res[3]; // result in geom local frame + mjtNum local_dir[3]; // direction in geom local frame // rotate dir to geom local frame - mju_mulMatTVec3(dir, d->geom_xmat+9*g, _dir->v); + mju_mulMatTVec3(local_dir, d->geom_xmat+9*g, dir); // compute result according to geom type switch ((mjtGeom) m->geom_type[g]) { case mjGEOM_SPHERE: - mju_scl3(res, dir, size[0]); + mju_scl3(res, local_dir, size[0]); break; case mjGEOM_CAPSULE: // start with sphere - mju_scl3(res, dir, size[0]); + mju_scl3(res, local_dir, size[0]); // add cylinder contribution - res[2] += mju_sign(dir[2]) * size[1]; + res[2] += mju_sign(local_dir[2]) * size[1]; break; case mjGEOM_ELLIPSOID: // find support point on unit sphere: scale dir by ellipsoid sizes and renormalize for (int i=0; i < 3; i++) { - res[i] = dir[i] * size[i]; + res[i] = local_dir[i] * size[i]; } mju_normalize3(res); @@ -146,21 +155,21 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { case mjGEOM_CYLINDER: // set result in XY plane: support on circle - tmp = mju_sqrt(dir[0]*dir[0] + dir[1]*dir[1]); + tmp = mju_sqrt(local_dir[0]*local_dir[0] + local_dir[1]*local_dir[1]); if (tmp > mjMINVAL) { - res[0] = dir[0]/tmp*size[0]; - res[1] = dir[1]/tmp*size[0]; + res[0] = local_dir[0]/tmp*size[0]; + res[1] = local_dir[1]/tmp*size[0]; } else { res[0] = res[1] = 0; } // set result in Z direction - res[2] = mju_sign(dir[2]) * size[1]; + res[2] = mju_sign(local_dir[2]) * size[1]; break; case mjGEOM_BOX: for (int i=0; i < 3; i++) { - res[i] = mju_sign(dir[i]) * size[i]; + res[i] = mju_sign(local_dir[i]) * size[i]; } break; @@ -176,9 +185,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // search all vertices, find best for (int i=0; i < m->mesh_vertnum[m->geom_dataid[g]]; i++) { // vdot = dot(vertex, dir) - vdot = dir[0] * (mjtNum)vertdata[3*i] + - dir[1] * (mjtNum)vertdata[3*i+1] + - dir[2] * (mjtNum)vertdata[3*i+2]; + vdot = local_dir[0] * (mjtNum)vertdata[3*i] + + local_dir[1] * (mjtNum)vertdata[3*i+1] + + local_dir[2] * (mjtNum)vertdata[3*i+2]; // update best if (vdot > tmp) { @@ -188,7 +197,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // record best vertex index, in globalid format - ((mjtCCD*)ccd)->meshindex = ibest; + obj->meshindex = ibest; } // hill-climb using graph data @@ -202,9 +211,9 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { // init with first vertex in convex hull ibest = 0; - tmp = dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + - dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + - dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; + tmp = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[0]] + + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[0]+1] + + local_dir[2] * (mjtNum)vertdata[3*vert_globalid[0]+2]; // hill-climb until no change change = 1; @@ -213,10 +222,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { change = 0; int i = vert_edgeadr[ibest]; while ((locid=edge_localid[i]) >= 0) { - // vdot = dot(vertex, dir) - vdot = dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + - dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + - dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; + // vdot = dot(vertex, local_dir) + vdot = local_dir[0] * (mjtNum)vertdata[3*vert_globalid[locid]] + + local_dir[1] * (mjtNum)vertdata[3*vert_globalid[locid]+1] + + local_dir[2] * (mjtNum)vertdata[3*vert_globalid[locid]+2]; // update best if (vdot > tmp) { @@ -231,7 +240,7 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { } // record best vertex index, in locid format - ((mjtCCD*)ccd)->meshindex = ibest; + obj->meshindex = ibest; // map best index to globalid ibest = vert_globalid[ibest]; @@ -255,16 +264,16 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjERROR("ccd support function is undefined for geom type %d", m->geom_type[g]); } - // add dir*margin/2 to result + // add local_dir*margin/2 to result for (int i=0; i < 3; i++) { - res[i] += dir[i] * ccd->margin/2; + res[i] += local_dir[i] * obj->margin/2; } // rotate result to global frame - mju_mulMatVec3(vec->v, d->geom_xmat+9*g, res); + mju_mulMatVec3(res, d->geom_xmat+9*g, res); // add geom position - mju_addTo3(vec->v, d->geom_xpos+3*g); + mju_addTo3(res, d->geom_xpos+3*g); } @@ -280,7 +289,7 @@ static void mjc_initCCD(ccd_t* ccd, const mjModel* m) { // find single convex-convex collision, using libccd -static int mjc_MPRIteration(mjtCCD* obj1, mjtCCD* obj2, const ccd_t* ccd, +static int mjc_MPRIteration(mjCCDObj* obj1, mjCCDObj* obj2, const ccd_t* ccd, const mjModel* m, const mjData* d, mjContact* con, mjtNum margin) { ccd_vec3_t dir, pos; @@ -351,8 +360,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, -1, -1, -1, margin, {1, 0, 0, 0}}; - mjtCCD obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -480,7 +489,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, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; // get support point in -normal direction ccdVec3Set(&dir, -mat1[2], -mat1[5], -mat1[8]); @@ -661,7 +670,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjtCCD obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}}; + mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -1094,8 +1103,8 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in 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}}; + mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1142,7 +1151,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // 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}}; + mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; ccd_t ccd; //------------------------------------- AABB computation, box-box test diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 3b9b797f..24f38648 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -23,8 +23,10 @@ #include +#include #include #include +#include #define mjGETINFO_HFIELD \ const mjtNum* pos1 = d->geom_xpos + 3*g1; \ @@ -38,8 +40,8 @@ extern "C" { #endif -// ccd general object type -struct _mjtCCD { +// internal object type for convex collision detection +struct _mjCCDObj { const mjModel* model; const mjData* data; int geom; @@ -49,14 +51,19 @@ struct _mjtCCD { int vert; mjtNum margin; mjtNum rotate[4]; + mjtNum x0[3]; // initial guess of the witness point }; -typedef struct _mjtCCD mjtCCD; +typedef struct _mjCCDObj mjCCDObj; +// support function for convex collision algorithms +MJAPI void mjc_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]); + +// center function for convex collision algorithms +MJAPI void mjc_center(mjtNum res[3], const mjCCDObj *obj); // ccd support function void mjccd_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec); - // pairwise geom collision functions using ccd int mjc_PlaneConvex (const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin); diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c new file mode 100644 index 00000000..381b6a36 --- /dev/null +++ b/src/engine/engine_collision_gjk.c @@ -0,0 +1,475 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "engine/engine_collision_gjk.h" + +#include + +#include +#include +#include "engine/engine_util_blas.h" +#include "engine/engine_util_spatial.h" +#include "engine/engine_collision_convex.h" + +// Computes the shortest distance between the origin and an n-simplex (n <= 3) and returns the +// barycentric coordinates of the closest point in the simplex. This is the so called distance +// sub-algorithm of the original 1988 GJK algorithm. +// +// We have adapted the Signed Volume method for our approach from the paper: +// Improving the GJK Algorithm for Faster and More Reliable Distance Queries Between Two +// Convex Objects, Montanari et al, ToG 2017. +static void signedVolume(mjtNum lambda[4], const mjtNum simplex[12], int n); + +// these internal functions compute the barycentric coordinates of the closest point +// to the origin in the n-simplex, where n = 3, 2, 1 respectively +static void S3D(mjtNum lambda[4], const mjtNum simplex[12]); +static void S2D(mjtNum lambda[3], const mjtNum simplex[9]); +static void S1D(mjtNum lambda[2], const mjtNum simplex[6]); + +// helper function to compute the support point in the Minkowski difference +static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[3]); + +// linear algebra utility functions +static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]); +static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n); + +// returns the distance between the two objects. The witness points are +// recoverable from x_0 in obj1 and obj2. +mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2) { + mjtNum simplex[12]; // our current simplex with max 4 vertices due to only 3 dimensions + int n = 0; // number of vertices in the simplex + mjtNum x_k[3]; // the kth approximation point with initial value x_0 + + // segregated simplices and points for the two objects to recover witness points + mjtNum simplex1[12], simplex2[12]; + mjtNum* x1_k = obj1->x0; + mjtNum* x2_k = obj2->x0; + mju_sub3(x_k, x1_k, x2_k); + + int N = config->max_iterations; + for (size_t k = 0; k < N; k++) { + mjtNum s1[3], s2[3]; // the support points in obj1 and obj2 + mjtNum s_k[3]; // the kth support point of Minkowski difference + mjtNum lambda[4]; // barycentric coordinates for x_k + + // compute the kth support point + support(s1, s2, obj1, obj2, x_k); + mju_sub3(s_k, s1, s2); + + // the stopping criteria relies on the Frank-Wolfe duality gap given by + // f(x_k) - f(x_min) <= < grad f(x_k), (x_k - s_k) > + mjtNum diff[3]; + mju_sub3(diff, x_k, s_k); + if (2*mju_dot3(x_k, diff) < config->tolerance) { + return mju_norm3(x_k); + } + + // TODO(kylebayes): signedVolume has been written to assume the first vertex is the latest + // support to be added. Once the logic has been updated, then this hack should be removed. + for (int i = n; i > 0; i--) { + // shift the simplex vertices to the right + mju_copy3(simplex + 3*i, simplex + 3*(i-1)); + mju_copy3(simplex1 + 3*i, simplex1 + 3*(i-1)); + mju_copy3(simplex2 + 3*i, simplex2 + 3*(i-1)); + } + + // copy new support point into the simplex + mju_copy3(simplex, s_k); + + // copy new support point into the individual simplexes + mju_copy3(simplex1, s1); + mju_copy3(simplex2, s2); + + // run the distance subalgorithm to compute the barycentric coordinates + // of the closest point to the origin in the simplex + signedVolume(lambda, simplex, ++n); + lincomb(x_k, lambda, simplex, 4); + + // compute the approximate witness points + lincomb(x1_k, lambda, simplex1, 4); + lincomb(x2_k, lambda, simplex2, 4); + + // for lambda[i] == 0, remove the ith vertex from the simplex + n = 0; + for (int i = 0; i < 4; i++) { + if (lambda[i] == 0) continue; + // recover simplex for the two objects + mju_copy3(simplex1 + 3*n, simplex1 + 3*i); + mju_copy3(simplex2 + 3*n, simplex2 + 3*i); + + // simplex in Minkowski difference + mju_copy3(simplex + 3*n++, simplex + 3*i); + } + } + return mju_norm3(x_k); +} + + + +// computes the support points in obj1 and obj2 for the kth approximation point +static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, + const mjtNum x_k[3]) { + mjtNum dir[3], dir_neg[3]; + mju_copy3(dir_neg, x_k); + mju_normalize3(dir_neg); // mjc_support assumes a normalized direction + mju_scl3(dir, dir_neg, -1); + + // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) + mjc_support(s1, obj1, dir); + mjc_support(s2, obj2, dir_neg); +} + + + +// linear combination of n 3D vectors: +// res = coef[0]*v[0] + ... + coef[n-1]*v[3*(n-1)] +static void lincomb(mjtNum res[3], const mjtNum* coef, const mjtNum* v, int n) { + mju_zero3(res); + for (int i = 0; i < n; i++) { + if (coef[i] == 0) continue; + res[0] += coef[i] * v[3*i + 0]; + res[1] += coef[i] * v[3*i + 1]; + res[2] += coef[i] * v[3*i + 2]; + } +} + + + +// returns determinant of the 3x3 matrix with columns v1, v2, v3 +static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { + mjtNum temp[3]; + mju_cross(temp, v2, v3); + return mju_dot3(v1, temp); +} + + + +// returns true only when a and b are both strictly positive or both strictly negative +static int compareSigns(mjtNum a, mjtNum b) { + if (a > 0 && b > 0) return 1; + if (a < 0 && b < 0) return 1; + return 0; +} + + + +// computes the barycentric coordinates of the closest point to the origin in the n-simplex +void signedVolume(mjtNum lambda[4], const mjtNum simplex[12], int n) { + int r = n - 1; // spatial dimension of the simplex + mju_zero4(lambda); + + if (r == 3) { + S3D(lambda, simplex); + } else if (r == 2) { + S2D(lambda, simplex); + } else if (r == 1) { + S1D(lambda, simplex); + } else { + lambda[0] = 1; + } +} + + + +static void S3D(mjtNum lambda[4], const mjtNum simplex[12]) { + // the four vertices of the 3-simplex that correspond to 4 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + const mjtNum* s3 = simplex + 6; + const mjtNum* s4 = simplex + 9; + + // the matrix M is given by + // [[ s1_x, s2_x, s3_x, s4_x ], + // [ s1_y, s2_y, s3_y, s4_y ], + // [ s1_z, s2_z, s3_z, s4_z ], + // [ 1, 1, 1, 1 ]] + // we want to solve M*lambda = P, where P = [p_x, p_y, p_z, 1] with [p_x, p_y, p_z] is the + // origin projected onto the simplex + + // compute cofactors to find det(M) + mjtNum C41 = -det3(s2, s3, s4); + mjtNum C42 = det3(s1, s3, s4); + mjtNum C43 = -det3(s1, s2, s4); + mjtNum C44 = det3(s1, s2, s3); + + // note that m_det = 6*SignVol(simplex) with C4i corresponding to the volume of the 3-simplex + // with vertices {s1, s2, s3, 0} - si + mjtNum m_det = C41 + C42 + C43 + C44; + + int comp1 = compareSigns(m_det, C41), + comp2 = compareSigns(m_det, C42), + comp3 = compareSigns(m_det, C43), + comp4 = compareSigns(m_det, C44); + + // if all signs are the same then the origin is inside the simplex + if (comp1 && comp2 && comp3 && comp4) { + lambda[0] = C41 / m_det; + lambda[1] = C42 / m_det; + lambda[2] = C43 / m_det; + lambda[3] = C44 / m_det; + return; + } + + // find the smallest distance, and use the corresponding barycentric coordinates + mjtNum dist = mjMAXVAL; + + if (!comp2) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s3); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + lambda[0] = lambda_2d[0]; + lambda[1] = 0; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + dist = d; + } + + if (!comp3) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_2d[0]; + lambda[1] = lambda_2d[1]; + lambda[2] = 0; + lambda[3] = lambda_2d[2]; + dist = d; + } + } + + if (!comp4) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + mju_copy3(verts + 6, s3); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_2d[0]; + lambda[1] = lambda_2d[1]; + lambda[2] = lambda_2d[2]; + lambda[3] = 0; + dist = d; + } + } + + if (!comp1) { + mjtNum lambda_2d[3], verts[9], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + mju_copy3(verts + 6, s4); + S2D(lambda_2d, verts); + lincomb(x, lambda_2d, verts, 3); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = 0; + lambda[1] = lambda_2d[0]; + lambda[2] = lambda_2d[1]; + lambda[3] = lambda_2d[2]; + dist = d; + } + } +} + + + +static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { + // the three vertices of the 2-simplex that correspond to 3 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + const mjtNum* s3 = simplex + 6; + + // compute normal + mjtNum diff1[3], diff2[3], n[3]; + mju_sub3(diff1, s2, s1); + mju_sub3(diff2, s3, s1); + mju_cross(n, diff1, diff2); + + // project origin + mjtNum p_o[3]; + mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n)); + + mjtNum mu_max = 0; + + // Below are the minors M_i4 of the matrix M given by + // [[ s1_x, s2_x, s3_x, s4_x ], + // [ s1_y, s2_y, s3_y, s4_y ], + // [ s1_z, s2_z, s3_z, s4_z ], + // [ 1, 1, 1, 1 ]] + mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1]; + mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0]; + mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0]; + + // exclude one of the axes with the largest projection of the simplex using the computed minors + mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2]; + mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34); + if (mu1 >= mu2 && mu1 >= mu3) { + mu_max = mu1; + s1_2D[0] = s1[1]; + s1_2D[1] = s1[2]; + + s2_2D[0] = s2[1]; + s2_2D[1] = s2[2]; + + s3_2D[0] = s3[1]; + s3_2D[1] = s3[2]; + + p_o_2D[0] = p_o[1]; + p_o_2D[1] = p_o[2]; + } else if (mu2 >= mu3) { + mu_max = mu2; + s1_2D[0] = s1[0]; + s1_2D[1] = s1[2]; + + s2_2D[0] = s2[0]; + s2_2D[1] = s2[2]; + + s3_2D[0] = s3[0]; + s3_2D[1] = s3[2]; + + p_o_2D[0] = p_o[0]; + p_o_2D[1] = p_o[2]; + } else { + mu_max = mu3; + s1_2D[0] = s1[0]; + s1_2D[1] = s1[1]; + + s2_2D[0] = s2[0]; + s2_2D[1] = s2[1]; + + s3_2D[0] = s3[0]; + s3_2D[1] = s3[1]; + + p_o_2D[0] = p_o[0]; + p_o_2D[1] = p_o[1]; + } + + // substitute p_o as a vertex in simplex + mjtNum C1 = p_o_2D[0]*s2_2D[1] + p_o_2D[1]*s3_2D[0] + s2_2D[0]*s3_2D[1] + - p_o_2D[0]*s3_2D[1] - p_o_2D[1]*s2_2D[0] - s3_2D[0]*s2_2D[1]; + + mjtNum C2 = p_o_2D[0]*s3_2D[1] + p_o_2D[1]*s1_2D[0] + s3_2D[0]*s1_2D[1] + - p_o_2D[0]*s1_2D[1] - p_o_2D[1]*s3_2D[0] - s1_2D[0]*s3_2D[1]; + + mjtNum C3 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] + - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; + + int comp1 = compareSigns(mu_max, C1), + comp2 = compareSigns(mu_max, C2), + comp3 = compareSigns(mu_max, C3); + + // inside the simplex + if (comp1 && comp2 && comp3) { + lambda[0] = C1 / mu_max; + lambda[1] = C2 / mu_max; + lambda[2] = C3 / mu_max; + return; + } + + // find the smallest distance, and use the corresponding barycentric coordinates + mjtNum dist = mjMAXVAL; + + if (!comp2) { + mjtNum lambda_1d[2], verts[6], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s3); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + lambda[0] = lambda_1d[0]; + lambda[1] = 0; + lambda[2] = lambda_1d[1]; + dist = d; + } + + if (!comp3) { + mjtNum lambda_1d[2], verts[6], x[3]; + mju_copy3(verts, s1); + mju_copy3(verts + 3, s2); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[0] = lambda_1d[0]; + lambda[1] = lambda_1d[1]; + lambda[2] = 0; + dist = d; + } + } + + if (!comp1) { + mjtNum lambda_1d[2], verts[6], x[3]; + mju_copy3(verts, s2); + mju_copy3(verts + 3, s3); + S1D(lambda_1d, verts); + lincomb(x, lambda_1d, verts, 2); + mjtNum d = mju_norm3(x); + if (d < dist) { + lambda[1] = lambda_1d[0]; + lambda[2] = lambda_1d[1]; + lambda[0] = 0; + dist = d; + } + } +} + + + +static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) { + // the two vertices of the 1-simplex correspond to 2 support points + const mjtNum* s1 = simplex; + const mjtNum* s2 = simplex + 3; + + // find projection of origin onto the 1-simplex: + // p_o = s2 - / * (s2 - s1) + mjtNum p_o[3]; + mjtNum diff[3]; + mju_sub3(diff, s2, s1); + mjtNum temp1 = mju_dot3(s2, diff); + mjtNum temp2 = mju_dot3(diff, diff); + mju_addScl3(p_o, s2, diff, - temp1 / temp2); + + // find the axis with the largest projection "shadow" of the simplex + mjtNum mu_max = 0; + int index; + for (int i = 0; i < 3; i++) { + mjtNum mu = s1[i] - s2[i]; + if (mju_abs(mu) >= mju_abs(mu_max)) { + mu_max = mu; + index = i; + } + } + + mjtNum C1 = p_o[index] - s2[index]; + mjtNum C2 = s1[index] - p_o[index]; + + // inside the simplex + if (compareSigns(mu_max, C1) && compareSigns(mu_max, C2)) { + lambda[0] = C1 / mu_max; + lambda[1] = C2 / mu_max; + } else { + lambda[0] = 1; + lambda[1] = 0; + } +} diff --git a/src/engine/engine_collision_gjk.h b/src/engine/engine_collision_gjk.h new file mode 100644 index 00000000..ead02eba --- /dev/null +++ b/src/engine/engine_collision_gjk.h @@ -0,0 +1,41 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ + +#include +#include +#include "engine/engine_collision_convex.h" + +#ifdef __cplusplus +extern "C" { +#endif + +// internal configuration for convex collision detection +struct _mjCCDConfig { + int max_iterations; + mjtNum tolerance; +}; +typedef struct _mjCCDConfig mjCCDConfig; + +// Returns the distance between the two objects. The witness points are +// recoverable from x_0 in obj1 and obj2. +MJAPI mjtNum mj_gjk(const mjCCDConfig* config, mjCCDObj* obj1, mjCCDObj* obj2); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_COLLISION_GJK_H_ diff --git a/src/engine/engine_core_constraint.c b/src/engine/engine_core_constraint.c index 6e47d384..6cb5c26a 100644 --- a/src/engine/engine_core_constraint.c +++ b/src/engine/engine_core_constraint.c @@ -35,9 +35,9 @@ #endif #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX - #endif // defined(__AVX__) && defined(mjUSEDOUBLE) + #endif // defined(__AVX__) && !defined(mjUSESINGLE) #endif // mjUSEPLATFORMSIMD @@ -818,7 +818,10 @@ void mj_instantiateLimit(const mjModel* m, mjData* d) { // BALL joint else if (m->jnt_type[i] == mjJNT_BALL) { // convert joint quaternion to axis-angle - mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1); + int adr = m->jnt_qposadr[i]; + mjtNum quat[4] = {d->qpos[adr], d->qpos[adr+1], d->qpos[adr+2], d->qpos[adr+3]}; + mju_normalize4(quat); + mju_quat2Vel(angleAxis, quat, 1); // get rotation angle, normalize value = mju_normalize3(angleAxis); @@ -1767,7 +1770,10 @@ static int mj_nl(const mjModel* m, const mjData* d, int *nnz) { } else if (m->jnt_type[i] == mjJNT_BALL) { mjtNum angleAxis[3]; - mju_quat2Vel(angleAxis, d->qpos+m->jnt_qposadr[i], 1); + int adr = m->jnt_qposadr[i]; + mjtNum quat[4] = {d->qpos[adr], d->qpos[adr+1], d->qpos[adr+2], d->qpos[adr+3]}; + mju_normalize4(quat); + mju_quat2Vel(angleAxis, quat, 1); value = mju_normalize3(angleAxis); dist = mju_max(m->jnt_range[2*i], m->jnt_range[2*i+1]) - value; if (dist < margin) { diff --git a/src/engine/engine_core_smooth.c b/src/engine/engine_core_smooth.c index 3dfbb09b..4016df97 100644 --- a/src/engine/engine_core_smooth.c +++ b/src/engine/engine_core_smooth.c @@ -44,14 +44,6 @@ void mj_kinematics(const mjModel* m, mjData* d) { d->xmat[0] = d->xmat[4] = d->xmat[8] = 1; d->ximat[0] = d->ximat[4] = d->ximat[8] = 1; - // normalize all quaternions in qpos - mj_normalizeQuat(m, d->qpos); - - // normalize mocap quaternions - for (int i=0; i < m->nmocap; i++) { - mju_normalize4(d->mocap_quat+4*i); - } - // compute global cartesian positions and orientations of all bodies for (int i=1; i < m->nbody; i++) { mjtNum xpos[3], xquat[4]; @@ -66,6 +58,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { // copy pos and quat from qpos mju_copy3(xpos, d->qpos+qadr); mju_copy4(xquat, d->qpos+qadr+3); + mju_normalize4(xquat); // assign xanchor and xaxis mju_copy3(d->xanchor+3*jntadr, xpos); @@ -77,10 +70,12 @@ void mj_kinematics(const mjModel* m, mjData* d) { int pid = m->body_parentid[i]; // get body pos and quat: from model or mocap - mjtNum *bodypos, *bodyquat; + mjtNum *bodypos, *bodyquat, quat[4]; if (m->body_mocapid[i] >= 0) { bodypos = d->mocap_pos + 3*m->body_mocapid[i]; - bodyquat = d->mocap_quat + 4*m->body_mocapid[i]; + mju_copy4(quat, d->mocap_quat + 4*m->body_mocapid[i]); + mju_normalize4(quat); + bodyquat = quat; } else { bodypos = m->body_pos+3*i; bodyquat = m->body_quat+4*i; @@ -125,6 +120,7 @@ void mj_kinematics(const mjModel* m, mjData* d) { mjtNum qloc[4]; if (jtype == mjJNT_BALL) { mju_copy4(qloc, d->qpos+qadr); + mju_normalize4(qloc); } else { mju_axisAngle2Quat(qloc, m->jnt_axis+3*jid, d->qpos[qadr] - m->qpos0[qadr]); } @@ -890,13 +886,15 @@ void mj_transmission(const mjModel* m, mjData* d) { int j = m->jnt_qposadr[id]; // axis: expmap representation of quaternion - mju_quat2Vel(axis, d->qpos+j, 1); + mju_copy4(quat, d->qpos+j); + mju_normalize4(quat); + mju_quat2Vel(axis, quat, 1); // gearAxis: rotate to parent frame if necessary if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear); } else { - mju_negQuat(quat, d->qpos+j); + mju_negQuat(quat, quat); mju_rotVecQuat(gearAxis, gear, quat); } @@ -923,12 +921,15 @@ void mj_transmission(const mjModel* m, mjData* d) { // axis: expmap representation of quaternion mju_quat2Vel(axis, d->qpos+j+3, 1); + mju_copy4(quat, d->qpos+j+3); + mju_normalize4(quat); + mju_quat2Vel(axis, quat, 1); // gearAxis: rotate to world frame if necessary if (m->actuator_trntype[i] == mjTRN_JOINT) { mju_copy3(gearAxis, gear+3); } else { - mju_negQuat(quat, d->qpos+j+3); + mju_negQuat(quat, quat); mju_rotVecQuat(gearAxis, gear+3, quat); } diff --git a/src/engine/engine_name.c b/src/engine/engine_name.c new file mode 100644 index 00000000..013ae8ad --- /dev/null +++ b/src/engine/engine_name.c @@ -0,0 +1,284 @@ +// 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 "engine/engine_name.h" + +#include +#include + +#include +#include "engine/engine_crossplatform.h" +#include "engine/engine_io.h" + + + +//-------------------------- name functions -------------------------------------------------------- +// get number of objects and name addresses for given object type +static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) { + int num = -1; + // map address starts at the end, subtract with explicit switch fallthrough below + *mapadr = m->nnames_map; + + // get address list and size for object type + switch (type) { + case mjOBJ_BODY: + case mjOBJ_XBODY: + *mapadr -= mjLOAD_MULTIPLE*m->nbody; + *padr = m->name_bodyadr; + num = m->nbody; + mjFALLTHROUGH; + + case mjOBJ_JOINT: + *mapadr -= mjLOAD_MULTIPLE*m->njnt; + if (num < 0) { + *padr = m->name_jntadr; + num = m->njnt; + } + mjFALLTHROUGH; + + case mjOBJ_GEOM: + *mapadr -= mjLOAD_MULTIPLE*m->ngeom; + if (num < 0) { + *padr = m->name_geomadr; + num = m->ngeom; + } + mjFALLTHROUGH; + + case mjOBJ_SITE: + *mapadr -= mjLOAD_MULTIPLE*m->nsite; + if (num < 0) { + *padr = m->name_siteadr; + num = m->nsite; + } + mjFALLTHROUGH; + + case mjOBJ_CAMERA: + *mapadr -= mjLOAD_MULTIPLE*m->ncam; + if (num < 0) { + *padr = m->name_camadr; + num = m->ncam; + } + mjFALLTHROUGH; + + case mjOBJ_LIGHT: + *mapadr -= mjLOAD_MULTIPLE*m->nlight; + if (num < 0) { + *padr = m->name_lightadr; + num = m->nlight; + } + mjFALLTHROUGH; + + case mjOBJ_FLEX: + *mapadr -= mjLOAD_MULTIPLE*m->nflex; + if (num < 0) { + *padr = m->name_flexadr; + num = m->nflex; + } + mjFALLTHROUGH; + + case mjOBJ_MESH: + *mapadr -= mjLOAD_MULTIPLE*m->nmesh; + if (num < 0) { + *padr = m->name_meshadr; + num = m->nmesh; + } + mjFALLTHROUGH; + + case mjOBJ_SKIN: + *mapadr -= mjLOAD_MULTIPLE*m->nskin; + if (num < 0) { + *padr = m->name_skinadr; + num = m->nskin; + } + mjFALLTHROUGH; + + case mjOBJ_HFIELD: + *mapadr -= mjLOAD_MULTIPLE*m->nhfield; + if (num < 0) { + *padr = m->name_hfieldadr; + num = m->nhfield; + } + mjFALLTHROUGH; + + case mjOBJ_TEXTURE: + *mapadr -= mjLOAD_MULTIPLE*m->ntex; + if (num < 0) { + *padr = m->name_texadr; + num = m->ntex; + } + mjFALLTHROUGH; + + case mjOBJ_MATERIAL: + *mapadr -= mjLOAD_MULTIPLE*m->nmat; + if (num < 0) { + *padr = m->name_matadr; + num = m->nmat; + } + mjFALLTHROUGH; + + case mjOBJ_PAIR: + *mapadr -= mjLOAD_MULTIPLE*m->npair; + if (num < 0) { + *padr = m->name_pairadr; + num = m->npair; + } + mjFALLTHROUGH; + + case mjOBJ_EXCLUDE: + *mapadr -= mjLOAD_MULTIPLE*m->nexclude; + if (num < 0) { + *padr = m->name_excludeadr; + num = m->nexclude; + } + mjFALLTHROUGH; + + case mjOBJ_EQUALITY: + *mapadr -= mjLOAD_MULTIPLE*m->neq; + if (num < 0) { + *padr = m->name_eqadr; + num = m->neq; + } + mjFALLTHROUGH; + + case mjOBJ_TENDON: + *mapadr -= mjLOAD_MULTIPLE*m->ntendon; + if (num < 0) { + *padr = m->name_tendonadr; + num = m->ntendon; + } + mjFALLTHROUGH; + + case mjOBJ_ACTUATOR: + *mapadr -= mjLOAD_MULTIPLE*m->nu; + if (num < 0) { + *padr = m->name_actuatoradr; + num = m->nu; + } + mjFALLTHROUGH; + + case mjOBJ_SENSOR: + *mapadr -= mjLOAD_MULTIPLE*m->nsensor; + if (num < 0) { + *padr = m->name_sensoradr; + num = m->nsensor; + } + mjFALLTHROUGH; + + case mjOBJ_NUMERIC: + *mapadr -= mjLOAD_MULTIPLE*m->nnumeric; + if (num < 0) { + *padr = m->name_numericadr; + num = m->nnumeric; + } + mjFALLTHROUGH; + + case mjOBJ_TEXT: + *mapadr -= mjLOAD_MULTIPLE*m->ntext; + if (num < 0) { + *padr = m->name_textadr; + num = m->ntext; + } + mjFALLTHROUGH; + + case mjOBJ_TUPLE: + *mapadr -= mjLOAD_MULTIPLE*m->ntuple; + if (num < 0) { + *padr = m->name_tupleadr; + num = m->ntuple; + } + mjFALLTHROUGH; + + case mjOBJ_KEY: + *mapadr -= mjLOAD_MULTIPLE*m->nkey; + if (num < 0) { + *padr = m->name_keyadr; + num = m->nkey; + } + mjFALLTHROUGH; + + case mjOBJ_PLUGIN: + *mapadr -= mjLOAD_MULTIPLE*m->nplugin; + if (num < 0) { + *padr = m->name_pluginadr; + num = m->nplugin; + } + mjFALLTHROUGH; + + default: + if (num < 0) { + *padr = 0; + num = 0; + } + } + + return num; +} + +// get string hash, see http://www.cse.yorku.ca/~oz/hash.html +uint64_t mj_hashString(const char* s, uint64_t n) { + uint64_t h = 5381; + int c; + while ((c = *s++)) { + h = ((h << 5) + h) ^ c; + } + return h % n; +} + +// get id of object with the specified mjtObj type and name, +// returns -1 if id not found +int mj_name2id(const mjModel* m, int type, const char* name) { + int mapadr; + int* adr = 0; + + // get number of objects and name addresses + int num = mjLOAD_MULTIPLE*_getnumadr(m, type, &adr, &mapadr); + + // search + if (num) { // look up at hash address + uint64_t hash = mj_hashString(name, num); + uint64_t i = hash; + + do { + int j = m->names_map[mapadr + i]; + if (j < 0) { + return -1; + } + + if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) { + return j; + } + if ((++i) == num)i = 0; + } while (i != hash); + } + return -1; +} + + + +// get name of object with the specified mjtObj type and id, +// returns NULL if name not found +const char* mj_id2name(const mjModel* m, int type, int id) { + int mapadr; + int* adr = 0; + + // get number of objects and name addresses + int num = _getnumadr(m, type, &adr, &mapadr); + + // id is in [0, num) and the found name is not the empty string "\0" + if (id >= 0 && id < num && m->names[adr[id]]) { + return m->names+adr[id]; + } + + return NULL; +} diff --git a/src/engine/engine_name.h b/src/engine/engine_name.h new file mode 100644 index 00000000..3bb98b2c --- /dev/null +++ b/src/engine/engine_name.h @@ -0,0 +1,41 @@ +// Copyright 2021 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef MUJOCO_SRC_ENGINE_ENGINE_NAME_H_ +#define MUJOCO_SRC_ENGINE_ENGINE_NAME_H_ + +#include + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//-------------------------- name functions -------------------------------------------------------- + +// get string hash, see http://www.cse.yorku.ca/~oz/hash.html +uint64_t mj_hashString(const char* s, uint64_t n); + +// get id of object with the specified mjtObj type and name, returns -1 if id not found +MJAPI int mj_name2id(const mjModel* m, int type, const char* name); + +// get name of object with the specified mjtObj type and id, returns NULL if name not found +MJAPI const char* mj_id2name(const mjModel* m, int type, int id); +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_SRC_ENGINE_ENGINE_NAME_H_ diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 8412f8f5..c66bb7e4 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -64,9 +64,11 @@ static void mj_springdamper(const mjModel* m, mjData* d) { case mjJNT_BALL: { - mjtNum dif[3]; // convert quatertion difference into angular "velocity" - mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr); + mjtNum dif[3], quat[4]; + mju_copy4(quat, d->qpos+padr); + mju_normalize4(quat); + mju_subQuat(dif, quat, m->qpos_spring + padr); // apply torque d->qfrc_spring[dadr+0] = -stiffness*dif[0]; diff --git a/src/engine/engine_print.c b/src/engine/engine_print.c index 8fee7ebf..0504c768 100644 --- a/src/engine/engine_print.c +++ b/src/engine/engine_print.c @@ -25,6 +25,7 @@ #include #include "engine/engine_core_constraint.h" #include "engine/engine_io.h" +#include "engine/engine_name.h" #include "engine/engine_support.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" diff --git a/src/engine/engine_resource.c b/src/engine/engine_resource.c index 107bf3b4..1668811c 100644 --- a/src/engine/engine_resource.c +++ b/src/engine/engine_resource.c @@ -14,6 +14,7 @@ #include "engine/engine_resource.h" +#include #include #include #include @@ -41,7 +42,6 @@ static void* _fileToMemory(FILE* fp, const char* filename, size_t* filesize); // file buffer used internally for the OS filesystem typedef struct { - FILE* fp; // file handle int is_read; // set to nonzero if buffer was read into uint8_t* buffer; // raw bytes from file size_t nbuffer; // size of buffer in bytes @@ -100,20 +100,16 @@ mjResource* mju_openResource(const char* name, char* error, size_t error_sz) { spec->is_read = 0; spec->buffer = NULL; spec->nbuffer = 0; - spec->fp = fopen(name, "rb"); - if (!spec->fp) { - if (error) { - snprintf(error, error_sz, - "resource not found via provider or OS filesystem: '%s'", name); - } - mju_closeResource(resource); - return NULL; - } struct stat file_stat; + errno = 0; if (stat(name, &file_stat) == 0) { memcpy(&spec->mtime, &file_stat.st_mtime, sizeof(time_t)); } else { - memset(&spec->mtime, 0, sizeof(time_t)); + if (error) { + snprintf(error, error_sz, "Error opening file '%s': %s", name, strerror(errno)); + } + mju_closeResource(resource); + return NULL; } mju_encodeBase64(resource->timestamp, (uint8_t*) &spec->mtime, sizeof(time_t)); return resource; @@ -157,17 +153,12 @@ int mju_readResource(mjResource* resource, const void** buffer) { return resource->provider->read(resource, buffer); } - // if provider is NULL, then OS filesystem is used file_spec* spec = (file_spec*) resource->data; - if (!spec->fp && !spec->is_read) { - mjERROR("internal error FILE pointer undefined"); // should not occur - } // only read once from file if (!spec->is_read) { - spec->buffer = _fileToMemory(spec->fp, resource->name, &(spec->nbuffer)); - spec->fp = NULL; // closed by _fileToMemory + spec->buffer = mju_fileToMemory(resource->name, &(spec->nbuffer)); spec->is_read = 1; } *buffer = spec->buffer; diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 9fe2c2c3..415472dc 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -281,6 +281,7 @@ void mj_sensorPos(const mjModel* m, mjData* d) { case mjSENS_BALLQUAT: // ballquat mju_copy4(d->sensordata+adr, d->qpos+m->jnt_qposadr[objid]); + mju_normalize4(d->sensordata+adr); break; case mjSENS_JOINTLIMITPOS: // jointlimitpos @@ -899,7 +900,7 @@ void mj_sensorAcc(const mjModel* m, mjData* d) { // position-dependent energy (potential) void mj_energyPos(const mjModel* m, mjData* d) { int padr; - mjtNum dif[3], stiffness; + mjtNum dif[3], quat[4], stiffness; // disabled: clear and return if (!mjENABLED(mjENBL_ENERGY)) { @@ -923,7 +924,9 @@ void mj_energyPos(const mjModel* m, mjData* d) { switch ((mjtJoint) m->jnt_type[i]) { case mjJNT_FREE: - mju_sub3(dif, d->qpos+padr, m->qpos_spring+padr); + mju_copy4(quat, d->qpos+padr); + mju_normalize4(quat); + mju_sub3(dif, quat, m->qpos_spring+padr); d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif); // continue with rotations @@ -932,6 +935,8 @@ void mj_energyPos(const mjModel* m, mjData* d) { case mjJNT_BALL: // covert quatertion difference into angular "velocity" + mju_copy4(quat, d->qpos+padr); + mju_normalize4(quat); mju_subQuat(dif, d->qpos + padr, m->qpos_spring + padr); d->energy[0] += 0.5*stiffness*mju_dot3(dif, dif); break; diff --git a/src/engine/engine_support.c b/src/engine/engine_support.c index 526a1334..09a5373f 100644 --- a/src/engine/engine_support.c +++ b/src/engine/engine_support.c @@ -15,7 +15,6 @@ #include "engine/engine_support.h" #include -#include #include #include @@ -31,7 +30,7 @@ #include "engine/engine_util_spatial.h" #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX #include "immintrin.h" #endif @@ -868,269 +867,6 @@ void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body) { -//-------------------------- name functions -------------------------------------------------------- - -// get number of objects and name addresses for given object type -static int _getnumadr(const mjModel* m, mjtObj type, int** padr, int* mapadr) { - int num = -1; - // map address starts at the end, subtract with explicit switch fallthrough below - *mapadr = m->nnames_map; - - // get address list and size for object type - switch (type) { - case mjOBJ_BODY: - case mjOBJ_XBODY: - *mapadr -= mjLOAD_MULTIPLE*m->nbody; - *padr = m->name_bodyadr; - num = m->nbody; - mjFALLTHROUGH; - - case mjOBJ_JOINT: - *mapadr -= mjLOAD_MULTIPLE*m->njnt; - if (num < 0) { - *padr = m->name_jntadr; - num = m->njnt; - } - mjFALLTHROUGH; - - case mjOBJ_GEOM: - *mapadr -= mjLOAD_MULTIPLE*m->ngeom; - if (num < 0) { - *padr = m->name_geomadr; - num = m->ngeom; - } - mjFALLTHROUGH; - - case mjOBJ_SITE: - *mapadr -= mjLOAD_MULTIPLE*m->nsite; - if (num < 0) { - *padr = m->name_siteadr; - num = m->nsite; - } - mjFALLTHROUGH; - - case mjOBJ_CAMERA: - *mapadr -= mjLOAD_MULTIPLE*m->ncam; - if (num < 0) { - *padr = m->name_camadr; - num = m->ncam; - } - mjFALLTHROUGH; - - case mjOBJ_LIGHT: - *mapadr -= mjLOAD_MULTIPLE*m->nlight; - if (num < 0) { - *padr = m->name_lightadr; - num = m->nlight; - } - mjFALLTHROUGH; - - case mjOBJ_FLEX: - *mapadr -= mjLOAD_MULTIPLE*m->nflex; - if (num < 0) { - *padr = m->name_flexadr; - num = m->nflex; - } - mjFALLTHROUGH; - - case mjOBJ_MESH: - *mapadr -= mjLOAD_MULTIPLE*m->nmesh; - if (num < 0) { - *padr = m->name_meshadr; - num = m->nmesh; - } - mjFALLTHROUGH; - - case mjOBJ_SKIN: - *mapadr -= mjLOAD_MULTIPLE*m->nskin; - if (num < 0) { - *padr = m->name_skinadr; - num = m->nskin; - } - mjFALLTHROUGH; - - case mjOBJ_HFIELD: - *mapadr -= mjLOAD_MULTIPLE*m->nhfield; - if (num < 0) { - *padr = m->name_hfieldadr; - num = m->nhfield; - } - mjFALLTHROUGH; - - case mjOBJ_TEXTURE: - *mapadr -= mjLOAD_MULTIPLE*m->ntex; - if (num < 0) { - *padr = m->name_texadr; - num = m->ntex; - } - mjFALLTHROUGH; - - case mjOBJ_MATERIAL: - *mapadr -= mjLOAD_MULTIPLE*m->nmat; - if (num < 0) { - *padr = m->name_matadr; - num = m->nmat; - } - mjFALLTHROUGH; - - case mjOBJ_PAIR: - *mapadr -= mjLOAD_MULTIPLE*m->npair; - if (num < 0) { - *padr = m->name_pairadr; - num = m->npair; - } - mjFALLTHROUGH; - - case mjOBJ_EXCLUDE: - *mapadr -= mjLOAD_MULTIPLE*m->nexclude; - if (num < 0) { - *padr = m->name_excludeadr; - num = m->nexclude; - } - mjFALLTHROUGH; - - case mjOBJ_EQUALITY: - *mapadr -= mjLOAD_MULTIPLE*m->neq; - if (num < 0) { - *padr = m->name_eqadr; - num = m->neq; - } - mjFALLTHROUGH; - - case mjOBJ_TENDON: - *mapadr -= mjLOAD_MULTIPLE*m->ntendon; - if (num < 0) { - *padr = m->name_tendonadr; - num = m->ntendon; - } - mjFALLTHROUGH; - - case mjOBJ_ACTUATOR: - *mapadr -= mjLOAD_MULTIPLE*m->nu; - if (num < 0) { - *padr = m->name_actuatoradr; - num = m->nu; - } - mjFALLTHROUGH; - - case mjOBJ_SENSOR: - *mapadr -= mjLOAD_MULTIPLE*m->nsensor; - if (num < 0) { - *padr = m->name_sensoradr; - num = m->nsensor; - } - mjFALLTHROUGH; - - case mjOBJ_NUMERIC: - *mapadr -= mjLOAD_MULTIPLE*m->nnumeric; - if (num < 0) { - *padr = m->name_numericadr; - num = m->nnumeric; - } - mjFALLTHROUGH; - - case mjOBJ_TEXT: - *mapadr -= mjLOAD_MULTIPLE*m->ntext; - if (num < 0) { - *padr = m->name_textadr; - num = m->ntext; - } - mjFALLTHROUGH; - - case mjOBJ_TUPLE: - *mapadr -= mjLOAD_MULTIPLE*m->ntuple; - if (num < 0) { - *padr = m->name_tupleadr; - num = m->ntuple; - } - mjFALLTHROUGH; - - case mjOBJ_KEY: - *mapadr -= mjLOAD_MULTIPLE*m->nkey; - if (num < 0) { - *padr = m->name_keyadr; - num = m->nkey; - } - mjFALLTHROUGH; - - case mjOBJ_PLUGIN: - *mapadr -= mjLOAD_MULTIPLE*m->nplugin; - if (num < 0) { - *padr = m->name_pluginadr; - num = m->nplugin; - } - mjFALLTHROUGH; - - default: - if (num < 0) { - *padr = 0; - num = 0; - } - } - - return num; -} - -// get string hash, see http://www.cse.yorku.ca/~oz/hash.html -uint64_t mj_hashString(const char* s, uint64_t n) { - uint64_t h = 5381; - int c; - while ((c = *s++)) { - h = ((h << 5) + h) ^ c; - } - return h % n; -} - -// get id of object with the specified mjtObj type and name, -// returns -1 if id not found -int mj_name2id(const mjModel* m, int type, const char* name) { - int mapadr; - int* adr = 0; - - // get number of objects and name addresses - int num = mjLOAD_MULTIPLE*_getnumadr(m, type, &adr, &mapadr); - - // search - if (num) { // look up at hash address - uint64_t hash = mj_hashString(name, num); - uint64_t i = hash; - - do { - int j = m->names_map[mapadr + i]; - if (j < 0) { - return -1; - } - - if (!strncmp(name, m->names+adr[j], m->nnames-adr[j])) { - return j; - } - if ((++i) == num)i = 0; - } while (i != hash); - } - return -1; -} - - - -// get name of object with the specified mjtObj type and id, -// returns NULL if name not found -const char* mj_id2name(const mjModel* m, int type, int id) { - int mapadr; - int* adr = 0; - - // get number of objects and name addresses - int num = _getnumadr(m, type, &adr, &mapadr); - - // id is in [0, num) and the found name is not the empty string "\0" - if (id >= 0 && id < num && m->names[adr[id]]) { - return m->names+adr[id]; - } - - return NULL; -} - - - //-------------------------- inertia functions ----------------------------------------------------- // convert sparse inertia matrix M into full matrix diff --git a/src/engine/engine_support.h b/src/engine/engine_support.h index ae3df31c..df1c8e02 100644 --- a/src/engine/engine_support.h +++ b/src/engine/engine_support.h @@ -15,8 +15,6 @@ #ifndef MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_ #define MUJOCO_SRC_ENGINE_ENGINE_SUPPORT_H_ -#include - #include #include #include @@ -113,18 +111,6 @@ int mj_jacSum(const mjModel* m, mjData* d, int* chain, MJAPI void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body); -//-------------------------- name functions -------------------------------------------------------- - -// get string hash, see http://www.cse.yorku.ca/~oz/hash.html -uint64_t mj_hashString(const char* s, uint64_t n); - -// get id of object with the specified mjtObj type and name, returns -1 if id not found -MJAPI int mj_name2id(const mjModel* m, int type, const char* name); - -// get name of object with the specified mjtObj type and id, returns NULL if name not found -MJAPI const char* mj_id2name(const mjModel* m, int type, int id); - - //-------------------------- inertia functions ----------------------------------------------------- // convert sparse inertia matrix M into full matrix diff --git a/src/engine/engine_util_blas.c b/src/engine/engine_util_blas.c index bb920b86..77ed4194 100644 --- a/src/engine/engine_util_blas.c +++ b/src/engine/engine_util_blas.c @@ -19,7 +19,7 @@ #include #ifdef mjUSEPLATFORMSIMD - #if defined(__AVX__) && defined(mjUSEDOUBLE) + #if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX #include "immintrin.h" #endif @@ -609,7 +609,7 @@ void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n) { void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n) { int i = 0; -#if defined(__AVX__) && defined(mjUSEAVX) && defined(mjUSEDOUBLE) +#if defined(__AVX__) && defined(mjUSEAVX) && !defined(mjUSESINGLE) int n_4 = n - 4; // vector part diff --git a/src/engine/engine_util_blas.h b/src/engine/engine_util_blas.h index eaac6360..882c9783 100644 --- a/src/engine/engine_util_blas.h +++ b/src/engine/engine_util_blas.h @@ -26,7 +26,7 @@ extern "C" { //------------------------------ standard library functions ---------------------------------------- -#ifdef mjUSEDOUBLE +#if !defined(mjUSESINGLE) #define mju_sqrt sqrt #define mju_exp exp #define mju_sin sin @@ -59,7 +59,7 @@ extern "C" { #define mju_log10 log10f #define mju_floor floorf #define mju_ceil ceilf -#endif +#endif // !defined(mjUSESINGLE) //------------------------------ 3D vector and matrix-vector operations ---------------------------- diff --git a/src/engine/engine_util_misc.c b/src/engine/engine_util_misc.c index 3564e9b1..7f716b39 100644 --- a/src/engine/engine_util_misc.c +++ b/src/engine/engine_util_misc.c @@ -1407,51 +1407,6 @@ char* mju_strncpy(char *dst, const char *src, int n) { -// assemble full filename from directory and filename, return 0 on success -int mju_makefullname(char* full, size_t nfull, const char* dir, const char* file) { - int dirlen = (!dir) ? 0 : strlen(dir); - int filelen = (!file) ? 0 : strlen(file); - char* filepos = full + dirlen; - - // missing filename - if (!filelen) { - return -1; - } - - // no directory then just copy filename over - if (!dirlen) { - // make sure full has space - if (filelen >= nfull) { - return -1; - } - strcpy(full, file); - return 0; - } - - // make sure full has space - if (dirlen + filelen >= nfull) { - return -1; - } - - // dir doesn't end with a slash - if (dir[dirlen - 1] != '\\' && dir[dirlen - 1] != '/') { - // need extra space for forward slash - if ((dirlen + filelen + 1) >= nfull) { - return -1; - } - - // add forward slash - *filepos++ = '/'; - } - - // copy directory and file over - memcpy(full, dir, sizeof(char) * dirlen); - strcpy(filepos, file); - return 0; -} - - - // sigmoid function over 0<=x<=1 using quintic polynomial mjtNum mju_sigmoid(mjtNum x) { // fast return diff --git a/src/engine/engine_util_misc.h b/src/engine/engine_util_misc.h index 21bc3f83..117d2325 100644 --- a/src/engine/engine_util_misc.h +++ b/src/engine/engine_util_misc.h @@ -164,10 +164,6 @@ MJAPI mjtNum mju_Halton(int index, int base); // call strncpy, then set dst[n-1] = 0 MJAPI char* mju_strncpy(char *dst, const char *src, int n); -// assemble full filename from directory and filename, return 0 on success -MJAPI int mju_makefullname(char* full, size_t nfull, - const char* dir, const char* file); - // sigmoid function over 0<=x<=1 using quintic polynomial MJAPI mjtNum mju_sigmoid(mjtNum x); diff --git a/src/engine/engine_util_sparse_avx.h b/src/engine/engine_util_sparse_avx.h index f962975c..c7f2f219 100644 --- a/src/engine/engine_util_sparse_avx.h +++ b/src/engine/engine_util_sparse_avx.h @@ -16,7 +16,7 @@ #define MUJOCO_SRC_ENGINE_ENGINE_UTIL_SPARSE_AVX_H_ #ifdef mjUSEPLATFORMSIMD -#if defined(__AVX__) && defined(mjUSEDOUBLE) +#if defined(__AVX__) && !defined(mjUSESINGLE) #define mjUSEAVX @@ -315,7 +315,7 @@ int mju_compare_avx(const int* vec1, const int* vec2, int n) { return !memcmp(vec1+i, vec2+i, (n-i)*sizeof(int)); } -#endif // defined(__AVX__) && defined(mjUSEDOUBLE) +#endif // defined(__AVX__) && !defined(mjUSESINGLE) #endif // mjUSEPLATFORMSIMD diff --git a/src/engine/engine_util_spatial.c b/src/engine/engine_util_spatial.c index 60c4af9d..39cc30b9 100644 --- a/src/engine/engine_util_spatial.c +++ b/src/engine/engine_util_spatial.c @@ -247,8 +247,8 @@ void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale) { mju_copy3(tmp, vel); angle = scale * mju_normalize3(tmp); mju_axisAngle2Quat(qrot, tmp, angle); - mju_mulQuat(quat, quat, qrot); mju_normalize4(quat); + mju_mulQuat(quat, quat, qrot); } diff --git a/src/engine/engine_vis_visualize.c b/src/engine/engine_vis_visualize.c index 66177c4f..acae774e 100644 --- a/src/engine/engine_vis_visualize.c +++ b/src/engine/engine_vis_visualize.c @@ -23,6 +23,7 @@ #include #include "engine/engine_array_safety.h" #include "engine/engine_io.h" +#include "engine/engine_name.h" #include "engine/engine_plugin.h" #include "engine/engine_support.h" #include "engine/engine_util_blas.h" @@ -301,21 +302,13 @@ static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* const mjtByte* flags) { // set material properties if given if (matid >= 0) { - f2f(geom->texrepeat, m->mat_texrepeat + 2*matid, 2); f2f(geom->rgba, m->mat_rgba + 4*matid, 4); - geom->texuniform = m->mat_texuniform[matid]; geom->emission = m->mat_emission[matid]; geom->specular = m->mat_specular[matid]; geom->shininess = m->mat_shininess[matid]; geom->reflectance = m->mat_reflectance[matid]; } - // otherwise clear texrepeat - else { - geom->texrepeat[0] = 0; - geom->texrepeat[1] = 0; - } - // use rgba if different from default, or no material given if (rgba[0] != 0.5f || rgba[1] != 0.5f || rgba[2] != 0.5f || rgba[3] != 1.0f || matid < 0) { f2f(geom->rgba, rgba, 4); @@ -323,7 +316,7 @@ static void setMaterial(const mjModel* m, mjvGeom* geom, int matid, const float* // set texture if (flags[mjVIS_TEXTURE] && matid >= 0) { - geom->texid = m->mat_texid[matid]; + geom->matid = matid; } // scale alpha for dynamic geoms only @@ -457,11 +450,8 @@ void mjv_initGeom(mjvGeom* geom, int type, const mjtNum* size, // set defaults that cannot be assigned via this function geom->dataid = -1; - geom->texid = -1; - geom->texuniform = 0; + geom->matid = -1; geom->texcoord = 0; - geom->texrepeat[0] = 1; - geom->texrepeat[1] = 1; geom->emission = 0; geom->specular = 0.5; geom->shininess = 0.5; @@ -570,7 +560,7 @@ void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* vopt, thisgeom->texcoord = 1; } else { - thisgeom->texid = -1; + thisgeom->matid = -1; } // glow flex if selected diff --git a/src/render/render_context.c b/src/render/render_context.c index ec349b2a..a4f0a4a8 100644 --- a/src/render/render_context.c +++ b/src/render/render_context.c @@ -1294,6 +1294,43 @@ static void makeFont(mjrContext* con, int fontscale) { } } +// make materials, just for those that have textures +static void makeMaterial(const mjModel* m, mjrContext* con) { + memset(con->mat_texid, -1, sizeof(con->mat_texid)); + memset(con->mat_texuniform, 0, sizeof(con->mat_texuniform)); + memset(con->mat_texrepeat, 0, sizeof(con->mat_texrepeat)); + if (!m->nmat || !m->ntex) { + return; + } + + if (m->nmat >= mjMAXMATERIAL-1) { + mju_error("Maximum number of materials is %d", mjMAXMATERIAL); + } + for (int i=0; i < m->nmat; i++) { + if (m->mat_texid[i*mjNTEXMAT] >= 0) { + for (int j=0; j < mjNTEXMAT; j++) { + con->mat_texid[i*mjNTEXMAT + j] = m->mat_texid[i*mjNTEXMAT + j]; + } + con->mat_texuniform[i] = m->mat_texuniform[i]; + con->mat_texrepeat[2*i] = m->mat_texrepeat[2*i]; + con->mat_texrepeat[2*i+1] = m->mat_texrepeat[2*i+1]; + } + } + // find skybox texture + for (int i=0; i < m->ntex; i++) { + if (m->tex_type[i] == mjTEXTURE_SKYBOX) { + if (m->nmat >= mjMAXMATERIAL-2) { + mju_error("With skybox, maximum number of materials is %d", mjMAXMATERIAL); + } + con->mat_texid[mjNTEXMAT * (mjMAXMATERIAL-1)] = i; + for (int j=1; j < mjNTEXMAT; j++) { + con->mat_texid[mjNTEXMAT * (mjMAXMATERIAL-1) + j] = -1; + } + + break; + } + } +} // make textures @@ -1585,6 +1622,7 @@ void mjr_makeContext_offSize(const mjModel* m, mjrContext* con, int fontscale, // make everything makeOff(con); makeShadow(m, con); + makeMaterial(m, con); makeTexture(m, con); makePlane(m, con); makeMesh(m, con); diff --git a/src/render/render_gl3.c b/src/render/render_gl3.c index 4591c4bb..598161fb 100644 --- a/src/render/render_gl3.c +++ b/src/render/render_gl3.c @@ -61,6 +61,10 @@ enum { // enable/disable texture mapping static void settexture(int type, int state, const mjrContext* con, const mjvGeom* geom) { float plane[4], scl[2]; + int texid = -1; + if (geom) { + texid = (geom->matid == -1) ? -1 : con->mat_texid[mjNTEXMAT * geom->matid]; + } // shadow if (type == mjtexSHADOW) { @@ -92,7 +96,7 @@ static void settexture(int type, int state, const mjrContext* con, const mjvGeom if (state) { glActiveTexture(GL_TEXTURE0); glEnable(GL_TEXTURE_2D); - glBindTexture(GL_TEXTURE_2D, con->texture[geom->texid]); + glBindTexture(GL_TEXTURE_2D, con->texture[texid]); } // disable @@ -103,18 +107,18 @@ static void settexture(int type, int state, const mjrContext* con, const mjvGeom } // 2D - else if (type == mjtexREGULAR && con->textureType[geom->texid] == mjTEXTURE_2D) { + else if (type == mjtexREGULAR && con->textureType[texid] == mjTEXTURE_2D) { // enable if (state) { glActiveTexture(GL_TEXTURE0); glEnable(GL_TEXTURE_2D); glEnable(GL_TEXTURE_GEN_S); glEnable(GL_TEXTURE_GEN_T); - glBindTexture(GL_TEXTURE_2D, con->texture[geom->texid]); + glBindTexture(GL_TEXTURE_2D, con->texture[texid]); // determine scaling, adjust for pre-scaled geoms - scl[0] = geom->texrepeat[0]; - scl[1] = geom->texrepeat[1]; + scl[0] = con->mat_texrepeat[geom->matid*2]; + scl[1] = con->mat_texrepeat[geom->matid*2+1]; if (geom->dataid >= 0) { if (geom->size[0] > 0) { scl[0] = scl[0] / mju_max(mjMINVAL, geom->size[0]); @@ -126,7 +130,7 @@ static void settexture(int type, int state, const mjrContext* con, const mjvGeom } // uniform: repeat relative to spatial units rather than object - if (geom->texuniform) { + if (con->mat_texuniform[geom->matid]) { if (geom->size[0] > 0) { scl[0] = scl[0] * geom->size[0]; } @@ -161,15 +165,15 @@ static void settexture(int type, int state, const mjrContext* con, const mjvGeom glEnable(GL_TEXTURE_GEN_S); glEnable(GL_TEXTURE_GEN_T); glEnable(GL_TEXTURE_GEN_R); - glBindTexture(GL_TEXTURE_CUBE_MAP, con->texture[geom->texid]); + glBindTexture(GL_TEXTURE_CUBE_MAP, con->texture[texid]); // set mapping : cube if (type == mjtexREGULAR) { - mjr_setf4(plane, geom->texuniform ? geom->size[0] : 1, 0, 0, 0); + mjr_setf4(plane, con->mat_texuniform[geom->matid] ? geom->size[0] : 1, 0, 0, 0); glTexGenfv(GL_S, GL_OBJECT_PLANE, plane); - mjr_setf4(plane, 0, geom->texuniform ? geom->size[1] : 1, 0, 0); + mjr_setf4(plane, 0, con->mat_texuniform[geom->matid] ? geom->size[1] : 1, 0, 0); glTexGenfv(GL_T, GL_OBJECT_PLANE, plane); - mjr_setf4(plane, 0, 0, geom->texuniform ? geom->size[2] : 1, 0); + mjr_setf4(plane, 0, 0, con->mat_texuniform[geom->matid] ? geom->size[2] : 1, 0); glTexGenfv(GL_R, GL_OBJECT_PLANE, plane); } @@ -232,7 +236,7 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, behind = isBehind(headpos, geom->pos, geom->mat); // enable texture in normal and shadowmap mode - if (geom->texid >= 0 && con->ntexture > 0 && + if (geom->matid >= 0 && (mode == mjrRND_NORMAL || mode == mjrRND_SHADOWMAP)) { settexture(mjtexREGULAR, 1, con, geom); } @@ -515,14 +519,14 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, 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) { + if (geom->texcoord && geom->matid>=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) { + if (geom->texcoord && geom->matid>=0) { glDisableClientState(GL_TEXTURE_COORD_ARRAY); } } @@ -568,7 +572,7 @@ static void renderGeom(const mjvGeom* geom, int mode, const float* headpos, glPopMatrix(); // disable texture if enabled - if (geom->texid >= 0 && con->ntexture > 0 && + if (geom->matid >= 0 && (mode == mjrRND_NORMAL || mode == mjrRND_SHADOWMAP)) { settexture(mjtexREGULAR, 0, con, geom); } @@ -1268,7 +1272,7 @@ void mjr_render(mjrRect viewport, mjvScene* scn, const mjrContext* con) { if (con->textureType[i] == mjTEXTURE_SKYBOX) { // save first skybox texture id in tempgeom memset(&tempgeom, 0, sizeof(mjvGeom)); - tempgeom.texid = i; + tempgeom.matid = mjMAXMATERIAL - 1; // modify settings glDisable(GL_LIGHTING); diff --git a/src/ui/ui_main.c b/src/ui/ui_main.c index 523d9cca..6ccf1421 100644 --- a/src/ui/ui_main.c +++ b/src/ui/ui_main.c @@ -14,6 +14,7 @@ #include "ui/ui_main.h" +#include #include #include @@ -26,142 +27,165 @@ // theme spacing 0 : tight static const mjuiThemeSpacing themeSpacing0 = { - 270, // int total; - 15, // int scroll; - 120, // int label; - 8, // int section; - 4, // int itemside; - 4, // int itemmid; - 4, // int itemver; - 8, // int texthor; - 4, // int textver; - 30, // int linescroll; - 4 // int samples; + 270, // total + 15, // scroll + 120, // label + 8, // section + 6, // cornersect + 6, // cornersep + 4, // itemside + 4, // itemmid + 4, // itemver + 8, // texthor + 4, // textver + 30, // linescroll + 4 // samples }; // theme spacing 1 : wide static const mjuiThemeSpacing themeSpacing1 = { - 310, // int total; - 15, // int scroll; - 120, // int label; - 10, // int section; - 7, // int itemside; - 7, // int itemmid; - 7, // int itemver; - 10, // int texthor; - 5, // int textver; - 30, // int linescroll; - 4 // int samples; + 310, // total + 15, // scroll + 120, // label + 10, // section + 10, // cornersect + 10, // cornersep + 7, // itemside + 7, // itemmid + 7, // itemver + 10, // texthor + 5, // textver + 30, // linescroll + 4 // samples }; // theme color 0 : default static const mjuiThemeColor themeColor0 = { - {0.25, 0.25, 0.25}, // float master[3]; - {0.12, 0.12, 0.12}, // float thumb[3]; - {0.6, 0.2, 0.2}, // float secttitle[3]; - {1.0, 1.0, 1.0}, // float sectfont[3]; - {0.7, 0.7, 0.7}, // float sectsymbol[3]; - {0.1, 0.1, 0.1}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {1.0, 1.0, 1.0}, // float fontactive[3]; - {0.5, 0.5, 0.5}, // float fontinactive[3]; - {0.3, 0.3, 0.3}, // float decorinactive[3]; - {0.4, 0.4, 0.4}, // float decorinactive2[3]; - {0.6, 0.4, 0.4}, // float button[3]; - {0.4, 0.4, 0.7}, // float check[3]; - {0.4, 0.6, 0.4}, // float radio[3]; - {0.4, 0.6, 0.6}, // float select[3]; - {0.2, 0.3, 0.3}, // float select2[3]; - {0.3, 0.2, 0.3}, // float slider[3]; - {0.6, 0.4, 0.6}, // float slider2[3]; - {0.6, 0.6, 0.4}, // float edit[3]; - {0.7, 0.0, 0.0}, // float edit2[3]; - {0.9, 0.9, 0.9} // float cursor[3]; + {0.25, 0.25, 0.25}, // master + {0.12, 0.12, 0.12}, // thumb + {0.6, 0.2, 0.2}, // secttitle + {0.1, 0.1, 0.1}, // secttitle2 + {0.45, 0.17, 0.17}, // secttitlecheck + {0.45, 0.17, 0.17}, // secttitlecheck2 + {1.0, 1.0, 1.0}, // sectfont + {0.7, 0.7, 0.7}, // sectsymbol + {0.1, 0.1, 0.1}, // sectpane + {0.25, 0.25, 0.25}, // separator + {0.1, 0.1, 0.1}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {1.0, 1.0, 1.0}, // fontactive + {0.5, 0.5, 0.5}, // fontinactive + {0.3, 0.3, 0.3}, // decorinactive + {0.4, 0.4, 0.4}, // decorinactive2 + {0.6, 0.4, 0.4}, // button + {0.4, 0.4, 0.7}, // check + {0.4, 0.6, 0.4}, // radio + {0.4, 0.6, 0.6}, // select + {0.2, 0.3, 0.3}, // select2 + {0.3, 0.2, 0.3}, // slider + {0.6, 0.4, 0.6}, // slider2 + {0.6, 0.6, 0.4}, // edit + {0.7, 0.0, 0.0}, // edit2 + {0.9, 0.9, 0.9} // cursor }; // theme color 1 : orange static const mjuiThemeColor themeColor1 = { - {0.2, 0.2, 0.2}, // float master[3]; - {0.12, 0.12, 0.12}, // float thumb[3]; - {0.3, 0.3, 0.3}, // float secttitle[3]; - {0.8, 0.8, 0.8}, // float sectfont[3]; - {0.7, 0.7, 0.7}, // float sectsymbol[3]; - {0.15, 0.15, 0.15}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {0.9, 0.9, 0.9}, // float fontactive[3]; - {0.5, 0.5, 0.5}, // float fontinactive[3]; - {0.2, 0.2, 0.2}, // float decorinactive[3]; - {0.25, 0.25, 0.25}, // float decorinactive2[3]; - {0.6, 0.4, 0.2}, // float button[3]; - {0.6, 0.4, 0.2}, // float check[3]; - {0.6, 0.4, 0.2}, // float radio[3]; - {0.6, 0.4, 0.2}, // float select[3]; - {0.3, 0.2, 0.1}, // float select2[3]; - {0.2, 0.2, 0.2}, // float slider[3]; - {0.6, 0.4, 0.2}, // float slider2[3]; - {0.6, 0.4, 0.2}, // float edit[3]; - {0.7, 0.0, 0.0}, // float edit2[3]; - {0.9, 0.9, 0.9} // float cursor[3]; + {0.2, 0.2, 0.2}, // master + {0.12, 0.12, 0.12}, // thumb + {0.3, 0.3, 0.3}, // secttitle + {0.15, 0.15, 0.15}, // secttitle2 + {0.25, 0.25, 0.25}, // secttitlecheck + {0.25, 0.25, 0.25}, // secttitlecheck2 + {0.8, 0.8, 0.8}, // sectfont + {0.7, 0.7, 0.7}, // sectsymbol + {0.15, 0.15, 0.15}, // sectpane + {0.2, 0.2, 0.2}, // separator + {0.15, 0.15, 0.15}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {0.9, 0.9, 0.9}, // fontactive + {0.5, 0.5, 0.5}, // fontinactive + {0.2, 0.2, 0.2}, // decorinactive + {0.25, 0.25, 0.25}, // decorinactive2 + {0.6, 0.4, 0.2}, // button + {0.6, 0.4, 0.2}, // check + {0.6, 0.4, 0.2}, // radio + {0.6, 0.4, 0.2}, // select + {0.3, 0.2, 0.1}, // select2 + {0.2, 0.2, 0.2}, // slider + {0.6, 0.4, 0.2}, // slider2 + {0.6, 0.4, 0.2}, // edit + {0.7, 0.0, 0.0}, // edit2 + {0.9, 0.9, 0.9} // cursor }; // theme color 2 : white static const mjuiThemeColor themeColor2 = { - {0.9, 0.9, 0.9}, // float master[3]; - {0.7, 0.7, 0.7}, // float thumb[3]; - {0.8, 0.8, 0.8}, // float secttitle[3]; - {0.0, 0.0, 0.8}, // float sectfont[3]; - {0.0, 0.0, 0.8}, // float sectsymbol[3]; - {1.0, 1.0, 1.0}, // float sectpane[3]; - {0.0, 1.0, 1.0}, // float shortcut[3]; - {0.0, 0.0, 0.0}, // float fontactive[3]; - {0.7, 0.7, 0.7}, // float fontinactive[3]; - {0.95, 0.95, 0.95}, // float decorinactive[3]; - {0.9, 0.9, 0.9}, // float decorinactive2[3]; - {0.8, 0.8, 0.8}, // float button[3]; - {0.8, 0.8, 0.8}, // float check[3]; - {0.8, 0.8, 0.8}, // float radio[3]; - {0.8, 0.8, 0.8}, // float select[3]; - {0.9, 0.9, 0.9}, // float select2[3]; - {0.95, 0.95, 0.95}, // float slider[3]; - {0.8, 0.8, 0.8}, // float slider2[3]; - {0.8, 0.8, 0.8}, // float edit[3]; - {1.0, 0.3, 0.3}, // float edit2[3]; - {0.2, 0.2, 0.2} // float cursor[3]; + {0.9, 0.9, 0.9}, // master + {0.7, 0.7, 0.7}, // thumb + {0.8, 0.8, 0.8}, // secttitle + {1.0, 1.0, 1.0}, // secttitle2 + {0.95, 0.95, 0.95}, // secttitlecheck + {0.95, 0.95, 0.95}, // secttitlecheck2 + {0.0, 0.0, 0.8}, // sectfont + {0.0, 0.0, 0.8}, // sectsymbol + {1.0, 1.0, 1.0}, // sectpane + {0.9, 0.9, 0.9}, // separator + {1.0, 1.0, 1.0}, // separator2 + {0.0, 1.0, 1.0}, // shortcut + {0.0, 0.0, 0.0}, // fontactive + {0.7, 0.7, 0.7}, // fontinactive + {0.95, 0.95, 0.95}, // decorinactive + {0.9, 0.9, 0.9}, // decorinactive2 + {0.8, 0.8, 0.8}, // button + {0.8, 0.8, 0.8}, // check + {0.8, 0.8, 0.8}, // radio + {0.8, 0.8, 0.8}, // select + {0.9, 0.9, 0.9}, // select2 + {0.95, 0.95, 0.95}, // slider + {0.8, 0.8, 0.8}, // slider2 + {0.8, 0.8, 0.8}, // edit + {1.0, 0.3, 0.3}, // edit2 + {0.2, 0.2, 0.2} // cursor }; // theme color 3 : black static const mjuiThemeColor themeColor3 = { - {0.15, 0.15, 0.15}, // float master[3]; - {0.3, 0.3, 0.3}, // float thumb[3]; - {0.25, 0.25, 0.25}, // float secttitle[3]; - {1.0, 0.3, 0.3}, // float sectfont[3]; - {1.0, 0.3, 0.3}, // float sectsymbol[3]; - {0.0, 0.0, 0.0}, // float sectpane[3]; - {0.0, 0.0, 1.0}, // float shortcut[3]; - {1.0, 1.0, 1.0}, // float fontactive[3]; - {0.4, 0.4, 0.4}, // float fontinactive[3]; - {0.1, 0.1, 0.1}, // float decorinactive[3]; - {0.15, 0.15, 0.15}, // float decorinactive2[3]; - {0.3, 0.3, 0.3}, // float button[3]; - {0.3, 0.3, 0.3}, // float check[3]; - {0.3, 0.3, 0.3}, // float radio[3]; - {0.3, 0.3, 0.3}, // float select[3]; - {0.15, 0.15, 0.15}, // float select2[3]; - {0.15, 0.15, 0.15}, // float slider[3]; - {0.3, 0.3, 0.3}, // float slider2[3]; - {0.3, 0.3, 0.3}, // float edit[3]; - {0.8, 0.2, 0.2}, // float edit2[3]; - {0.8, 0.8, 0.8} // float cursor[3]; + {0.15, 0.15, 0.15}, // master + {0.3, 0.3, 0.3}, // thumb + {0.25, 0.25, 0.25}, // secttitle + {0.0, 0.0, 0.0}, // secttitle2 + {0.2, 0.2, 0.2}, // secttitlecheck + {0.2, 0.2, 0.2}, // secttitlecheck2 + {1.0, 0.3, 0.3}, // sectfont + {1.0, 0.3, 0.3}, // sectsymbol + {0.0, 0.0, 0.0}, // sectpane + {0.15, 0.15, 0.15}, // separator + {0.0, 0.0, 0.0}, // separator2 + {0.0, 0.0, 1.0}, // shortcut + {1.0, 1.0, 1.0}, // fontactive + {0.4, 0.4, 0.4}, // fontinactive + {0.1, 0.1, 0.1}, // decorinactive + {0.15, 0.15, 0.15}, // decorinactive2 + {0.3, 0.3, 0.3}, // button + {0.3, 0.3, 0.3}, // check + {0.3, 0.3, 0.3}, // radio + {0.3, 0.3, 0.3}, // select + {0.15, 0.15, 0.15}, // select2 + {0.15, 0.15, 0.15}, // slider + {0.3, 0.3, 0.3}, // slider2 + {0.3, 0.3, 0.3}, // edit + {0.8, 0.2, 0.2}, // edit2 + {0.8, 0.8, 0.8} // cursor }; - //------------------------------------ Utility functions ------------------------------------------- // scale from abstract pixels to framebuffer units @@ -172,7 +196,7 @@ static int SCL(int sz, const mjrContext* con) { // init OpenGL -static void initOpenGL(const mjUI* ui, const mjrContext* con) { +static void initOpenGL(const mjrRect* r, const mjrContext* con) { // set OpenGL options glDisable(GL_NORMALIZE); glDisable(GL_DEPTH_TEST); @@ -186,12 +210,12 @@ static void initOpenGL(const mjUI* ui, const mjrContext* con) { // standard 2D projection, in framebuffer units glMatrixMode(GL_PROJECTION); glLoadIdentity(); - glOrtho(0, ui->width, 0, ui->height, -1, 1); + glOrtho(0, r->width, 0, r->height, -1, 1); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); // set viewport - glViewport(0, 0, ui->width, ui->height); + glViewport(r->left, r->bottom, r->width, r->height); } @@ -277,10 +301,72 @@ static void drawrectangle(mjrRect rect, const float* rgb, const float* rgbback, +// round corners of rectangle +static void roundcorner(mjrRect rect, int flg_skipbottom, int flg_separator, + const mjUI* ui, const mjrContext* con) { + // get rounding from theme, exit if disabled + int cornerspec = flg_separator ? ui->spacing.cornersep : ui->spacing.cornersect; + if (cornerspec == 0) { + return; + } + + // quarter-circle divisions and radius + int ndivide = 10; + double radius = cornerspec * 0.01 * con->fontScale; + + // draw fans in the four corners, optionally skip bottom corners + for (int ic = (flg_skipbottom ? 2 : 0); ic < 4; ++ic) { + // set corner + double corner[2]; + switch (ic) { + case 0: // bottom-left + corner[0] = rect.left; + corner[1] = rect.bottom; + break; + + case 1: // bottom-right + corner[0] = rect.left + rect.width; + corner[1] = rect.bottom; + break; + + case 2: // top-right + corner[0] = rect.left + rect.width; + corner[1] = rect.bottom + rect.height; + break; + + default: // top-left + corner[0] = rect.left; + corner[1] = rect.bottom + rect.height; + } + + // orient fan to point inside + double angle = ic * 0.5 * mjPI; + + // compute circle center: opposite to corner + double center[2]; + center[0] = corner[0] + mju_sqrt(2.0) * radius * cos(angle + 0.25 * mjPI); + center[1] = corner[1] + mju_sqrt(2.0) * radius * sin(angle + 0.25 * mjPI); + + // fill with erase color, start trinagle_fan from corner + glColor3fv(flg_separator ? ui->color.sectpane : ui->color.master); + glBegin(GL_TRIANGLE_FAN); + glVertex2d(corner[0], corner[1]); + + // compute vertices of quarter-circle + for (int i = 0; i <= ndivide; i++) { + double a = angle + mjPI + 0.5 * mjPI * (double)i / (double)ndivide; + glVertex2d(center[0] + radius * cos(a), center[1] + radius * sin(a)); + } + glEnd(); + } +} + + + // draw oval static void drawoval(mjrRect rect, const float* rgb, const float* rgbback, const mjrContext* con) { - const int ndivide = 20; + const int ndivide = 15; // require horizontal if (rect.height > rect.width) { @@ -334,8 +420,9 @@ static void drawoval(mjrRect rect, const float* rgb, const float* rgbback, -// draw section open/closed symbol: section -static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, +// draw open/closed symbol in title +// type: 0- section, 1- section with checkbox, 2- separator +static void drawsymbol(mjrRect rect, int flg_open, int type, const mjUI* ui, const mjrContext* con) { // size and center int texthor = SCL(ui->spacing.texthor, con); @@ -344,7 +431,7 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, int d = mju_round(con->charHeight*0.33); // separator size - if (flg_sep) { + if (type == 2) { d = mju_round(con->charHeight*0.28); } @@ -360,7 +447,7 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, // closed else { - // solid + // solid outside glColor3fv(ui->color.sectsymbol); glBegin(GL_TRIANGLES); glVertex2i(cx, cy-d); @@ -368,23 +455,36 @@ static void drawsymbol(mjrRect rect, int flg_open, int flg_sep, glVertex2i(cx-2*d, cy); glEnd(); - // empty - double margin = con->fontScale * 0.015; - double u = 0.5*sqrt(5.0)*margin; - double y = d - u - 0.5*margin; - if (flg_sep) { + // set color for inside + switch (type) { + case 0: // section glColor3f( - (ui->color.master[0] + ui->color.sectpane[0]) * 0.5, - (ui->color.master[1] + ui->color.sectpane[1]) * 0.5, - (ui->color.master[2] + ui->color.sectpane[2]) * 0.5 - ); - } else { + (ui->color.secttitle[0] + ui->color.secttitle2[0]) * 0.5, + (ui->color.secttitle[1] + ui->color.secttitle2[1]) * 0.5, + (ui->color.secttitle[2] + ui->color.secttitle2[2]) * 0.5 + ); + break; + + case 1: // section with checkbox glColor3f( - (ui->color.secttitle[0] + ui->color.sectpane[0]) * 0.5, - (ui->color.secttitle[1] + ui->color.sectpane[1]) * 0.5, - (ui->color.secttitle[2] + ui->color.sectpane[2]) * 0.5 - ); + (ui->color.secttitlecheck[0] + ui->color.secttitlecheck2[0]) * 0.5, + (ui->color.secttitlecheck[1] + ui->color.secttitlecheck2[1]) * 0.5, + (ui->color.secttitlecheck[2] + ui->color.secttitlecheck2[2]) * 0.5 + ); + break; + + case 2: // separator + glColor3f( + (ui->color.separator[0] + ui->color.separator2[0]) * 0.5, + (ui->color.separator[1] + ui->color.separator2[1]) * 0.5, + (ui->color.separator[2] + ui->color.separator2[2]) * 0.5 + ); } + + // draw inside + double margin = con->fontScale * 0.015; + double u = 0.5 * sqrt(5.0) * margin; + double y = d - u - 0.5 * margin; glBegin(GL_TRIANGLES); glVertex2d(cx-margin, cy-y); glVertex2d(cx-margin, cy+y); @@ -636,6 +736,7 @@ static int insideoval(int x, int y, mjrRect r) { // find mouse location in UI; y already inverted // sect: -1: thumb, -2: slider down, -3: slider up, positive: 1+section // item: -1: section title or scroll, non-negative: item number +// item: -2 in checkbox on section title static void findmouse(const mjUI* ui, const mjuiState* ins, const mjrContext* con, int* sect, int* item) { // clear @@ -689,6 +790,16 @@ static void findmouse(const mjUI* ui, const mjuiState* ins, const mjrContext* co if (s->state < 2 && inside(x, y, s->rtitle)) { *sect = n+1; *item = -1; + + // in checkbox + if (s->checkbox > 0) { + mjrRect rcheck = s->rtitle; + rcheck.width = mjMIN(rcheck.height, rcheck.width); + if (inside(x, y, rcheck)) { + *item = -2; + } + } + return; } @@ -1152,18 +1263,32 @@ void mjui_add(mjUI* ui, const mjuiDef* def) { if (strlen(def[n].name) >= mjMAXUINAME-1) { mju_error("mjui_add: section name too long"); } - if (def[n].state < 0 || def[n].state > 2) { + if (def[n].state != mjSECT_CLOSED && def[n].state != mjSECT_OPEN && + def[n].state != mjSECT_FIXED && def[n].state != mjPRESERVE) { mju_error("mjui_add: invalid section state"); } - // add section, clear + // add section, save state ui->nsect++; mjuiSection* se = ui->sect + (ui->nsect-1); - memset(se, 0, sizeof(mjuiSection)); + int oldstate = se->state; - // copy data + // clear, but preserve item states + int itemstate[mjMAXUIITEM]; + for (int i = 0; i < mjMAXUIITEM; ++i) { + itemstate[i] = se->item[i].state; + } + memset(se, 0, sizeof(mjuiSection)); + for (int i = 0; i < mjMAXUIITEM; ++i) { + se->item[i].state = itemstate[i]; + } + + // set or restore section state + se->state = (def[n].state == mjPRESERVE ? oldstate : def[n].state); + + // copy remaining data mjSTRNCPY(se->name, def[n].name); - se->state = def[n].state; + se->checkbox = def[n].otherint; parseshortcut(def[n].other, &(se->modifier), &(se->shortcut)); } @@ -1191,18 +1316,28 @@ void mjui_add(mjUI* ui, const mjuiDef* def) { mju_error("mjui_add: invalid item state"); } - // add item, clear + // add item, save state, clear se->nitem++; mjuiItem* it = se->item + (se->nitem-1); + int oldstate = it->state; memset(it, 0, sizeof(mjuiItem)); + // set or restore state for collapsible separator, copy state for others + if (def[n].type == mjITEM_SEPARATOR && def[n].state == mjPRESERVE) { + // mjSEPCLOSED makes separator collapsible + it->state = (oldstate < mjSEPCLOSED ? mjSEPCLOSED : oldstate); + } + else { + it->state = def[n].state; + } + // copy common data it->type = def[n].type; - it->state = def[n].state; it->pdata = def[n].pdata; mjSTRNCPY(it->name, def[n].name); it->sectionid = ui->nsect - 1; it->itemid = se->nitem - 1; + it->userid = def[n].otherint; // data pointer check if (it->type > mjITEM_BUTTON && it->pdata == 0) { @@ -1343,10 +1478,38 @@ void mjui_addToSection(mjUI* ui, int sect, const mjuiDef* def) { -// Compute UI sizes. -void mjui_resize(mjUI* ui, const mjrContext* con) { +// set item skip flags within section, but not in pass 0 +static void setitemskip(mjuiSection* s, int pass) { + int skip = 0; + + // process section items + for (int i = 0; i < s->nitem; ++i) { + mjuiItem* it = s->item + i; + + // pass 0: nothing is skipped + if (pass == 0) { + it->skip = 0; + continue; + } + + // item is a separator: update skip state for subsequent items + if (it->type == mjITEM_SEPARATOR) { + skip = (it->state == mjSEPCLOSED); + } + + // item is not a separator: set skip state + else { + it->skip = skip; + } + } +} + + + +// Compute UI sizes: internal fuction, may be called twice per resize +static void tryresize(mjUI* ui, const mjrContext* con) { // scale theme sizes - int w_master = SCL(ui->spacing.total, con); + int w_master = SCL(ui->spacing.total, con); int w_scroll = SCL(ui->spacing.scroll, con); int g_section = SCL(ui->spacing.section, con); int g_itemside = SCL(ui->spacing.itemside, con); @@ -1361,183 +1524,279 @@ void mjui_resize(mjUI* ui, const mjrContext* con) { // column width int colwidth = (w_master - w_scroll - 2*g_section - 2*g_itemside - g_itemmid)/2; - // init UI sizes - int height = 0; - int maxheight = 0; + // pass 0 includes skipped items, pass 1 does not + int Height, MaxHeight; + for (int pass = 0; pass < 2; ++pass) { + // init UI heights + int height = 0; + int maxheight = 0; - // process sections - int skip; - for (int n=0; n < ui->nsect; n++) { - // vertical padding before section + // process sections + for (int n = 0; n < ui->nsect; n++) { + // vertical padding before section + height += g_section; + maxheight += g_section; + + // get section pointer + mjuiSection* s = ui->sect + n; + + // set item skip flags for section, depending on pass + setitemskip(s, pass); + + // title rectangle + s->rtitle.left = g_section; + s->rtitle.width = w_master - w_scroll - 2 * g_section; + if (s->state == mjSECT_FIXED) { // fixed section: no title + s->rtitle.bottom = height; + s->rtitle.height = 0; + } + else { // regular section with title + s->rtitle.bottom = height + textheight; + s->rtitle.height = textheight; + } + + // count title height + height += s->rtitle.height; + maxheight += s->rtitle.height; + + // init content rectangle + s->rcontent.left = s->rtitle.left; + s->rcontent.width = s->rtitle.width; + s->rcontent.height = 0; + s->rcontent.bottom = 0; + + // process items within section + for (int i = 0; i < s->nitem; i++) { + // get item pointer, clear rectangle + mjuiItem* it = s->item + i; + memset(&it->rect, 0, sizeof(mjrRect)); + + // item is skipped: nothing to do + if (it->skip) { + continue; + } + + // vertical padding before item + s->rcontent.height += it->type == mjITEM_SEPARATOR ? g_section : g_itemver; + + // packed pair of items + if (i < s->nitem - 1 && s->item[i + 1].type == it->type && + (it->type == mjITEM_BUTTON || + it->type == mjITEM_CHECKINT || + it->type == mjITEM_CHECKBYTE)) { + // get next item pointer + mjuiItem* it1 = s->item + (i + 1); + + // this item rectangle + it->rect.left = s->rcontent.left + g_itemside; + it->rect.width = colwidth; + it->rect.height = textheight; + + // next item rectangle (set bottom here) + it1->rect.left = s->rcontent.left + g_itemside + colwidth + g_itemmid; + it1->rect.width = colwidth; + it1->rect.height = textheight; + it1->rect.bottom = height + s->rcontent.height + it->rect.height; + + // advance + i++; + } + + // single-line item + else { + // common left border (except for labeled controls at the end) + it->rect.left = s->rcontent.left + g_itemside; + + // static + if (it->type == mjITEM_STATIC) { + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = (con->charHeight + g_textver) * it->multi.nelem; + } + + // single column + else if (it->type == mjITEM_BUTTON || + it->type == mjITEM_CHECKINT || + it->type == mjITEM_CHECKBYTE) { + it->rect.width = colwidth; + it->rect.height = textheight; + } + + // radio + else if (it->type == mjITEM_RADIO) { + int ncol = ui->radiocol ? ui->radiocol : 2; + int nrow = (it->multi.nelem - 1) / ncol + 1; + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = textheight * nrow; + } + + // separator, select, slider, edit, radioline + else { + it->rect.width = s->rcontent.width - 2 * g_itemside; + it->rect.height = textheight; + } + + // add room for label + if (it->name[0] && + (it->type >= mjITEM_RADIO || + it->type >= mjITEM_RADIOLINE || + it->type == mjITEM_STATIC)) { + it->rect.left = s->rcontent.left + g_itemside + g_label; + it->rect.width = s->rcontent.width - (2 * g_itemside + g_label); + } + } + + // set bottom, count height + it->rect.bottom = height + s->rcontent.height + it->rect.height; + s->rcontent.height += it->rect.height; + } + + // vertical padding after last item, compute bottom + s->rcontent.height += g_itemver; + s->rcontent.bottom = height + s->rcontent.height; + + // count content height + if (s->state != mjSECT_CLOSED) { + height += s->rcontent.height; + } + maxheight += s->rcontent.height; + } + + // vertical padding after last section height += g_section; maxheight += g_section; - // get section pointer - mjuiSection* s = ui->sect + n; - - // title rectangle - s->rtitle.left = g_section; - s->rtitle.width = w_master - w_scroll - 2*g_section; - if (s->state < 2) { - s->rtitle.bottom = height + textheight; - s->rtitle.height = textheight; - } else { - s->rtitle.bottom = height; - s->rtitle.height = 0; + // save data: maxheight from pass 0, height from pass 1 + if (pass == 0) { + MaxHeight = maxheight; } - - // count title height - height += s->rtitle.height; - maxheight += s->rtitle.height; - - // init content rectangle - s->rcontent.left = s->rtitle.left; - s->rcontent.width = s->rtitle.width; - s->rcontent.height = 0; - - // process items within section - for (int i=0; i < s->nitem; i++) { - // get item pointer - mjuiItem* it = s->item + i; - - // save section rcontent - mjrRect oldcontent = s->rcontent; - - // determine skip (collapsed separator before item) - skip = 0; - if (i > 0 && it->type != mjITEM_SEPARATOR) { - for (int k=i-1; k >= 0; k--) { - if (s->item[k].type == mjITEM_SEPARATOR) { - // collapsed state: skip items below it - if (s->item[k].state == mjSEPCLOSED) { - skip = 1; - } - - break; - } - } - } - - // vertical padding before item - s->rcontent.height += it->type == mjITEM_SEPARATOR ? g_section : g_itemver; - - // packed pair of items - if (i < s->nitem-1 && s->item[i+1].type == it->type && - (it->type == mjITEM_BUTTON || - it->type == mjITEM_CHECKINT || - it->type == mjITEM_CHECKBYTE)) { - // get next item pointer - mjuiItem* it1 = s->item + (i+1); - - // this item rectangle - it->rect.left = s->rcontent.left + g_itemside; - it->rect.width = colwidth; - it->rect.height = textheight; - - // next item rectangle (set bottom here) - it1->rect.left = s->rcontent.left + g_itemside + colwidth + g_itemmid; - it1->rect.width = colwidth; - it1->rect.height = textheight; - it1->rect.bottom = height + s->rcontent.height + it->rect.height; - - // skip second item in pair - if (skip) { - it1->rect.width = 0; - it1->rect.height = 0; - } - - // advance - i++; - } - - // single-line item - else { - // common left border (except for labeled controls at the end) - it->rect.left = s->rcontent.left + g_itemside; - - // static - if (it->type == mjITEM_STATIC) { - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = (con->charHeight+g_textver)*it->multi.nelem; - } - - // single column - else if (it->type == mjITEM_BUTTON || - it->type == mjITEM_CHECKINT || - it->type == mjITEM_CHECKBYTE) { - it->rect.width = colwidth; - it->rect.height = textheight; - } - - // radio - else if (it->type == mjITEM_RADIO) { - int ncol = ui->radiocol ? ui->radiocol : 2; - int nrow = (it->multi.nelem-1)/ncol + 1; - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = textheight*nrow; - } - - // separator, select, slider, edit, radioline - else { - it->rect.width = s->rcontent.width - 2*g_itemside; - it->rect.height = textheight; - } - - // add room for label - if (it->name[0] && - (it->type >= mjITEM_RADIO || - it->type >= mjITEM_RADIOLINE || - it->type == mjITEM_STATIC)) { - it->rect.left = s->rcontent.left + g_itemside + g_label; - it->rect.width = s->rcontent.width - (2*g_itemside + g_label); - } - } - - // set bottom, count height - it->rect.bottom = height + s->rcontent.height + it->rect.height; - s->rcontent.height += it->rect.height; - - // skip item - if (skip) { - maxheight += s->rcontent.height - oldcontent.height; - s->rcontent = oldcontent; - it->rect.width = 0; - it->rect.height = 0; - } + else { + Height = height; } - - // vertical padding after last item, compute bottom - s->rcontent.height += g_itemver; - s->rcontent.bottom = height + s->rcontent.height; - - // count content height - if (s->state) { - height += s->rcontent.height; - } - maxheight += s->rcontent.height; } - // vertical padding after last section - height += g_section; - maxheight += g_section; - // invert bottom for all sections and items for (int n=0; n < ui->nsect; n++) { // section mjuiSection* s = ui->sect + n; - s->rtitle.bottom = height - s->rtitle.bottom; - s->rcontent.bottom = height - s->rcontent.bottom; + s->rtitle.bottom = Height - s->rtitle.bottom; + s->rcontent.bottom = Height - s->rcontent.bottom; // items for (int i=0; i < s->nitem; i++) { - s->item[i].rect.bottom = height - s->item[i].rect.bottom; + s->item[i].rect.bottom = Height - s->item[i].rect.bottom; } } // assign UI sizes ui->width = w_master; - ui->height = height; - ui->maxheight = maxheight; + ui->height = Height; + ui->maxheight = MaxHeight; +} + + + +// insertion sort of groups of ints: increasing order of leading int +static void insertionsortgroup(int* list, int num, int stride) { + // allocate buffer of 10 ints, cannot handle more + if (stride > 10) { + mju_error("insertionsortgroup cannot handle stride greater than 10"); + } + int x[10]; + + for (int i = 1; i < num; i++) { + memcpy(x, list + i * stride, sizeof(int) * stride); + + int j = i - 1; + while (j >= 0 && list[j * stride] > x[0]) { + memcpy(list + (j + 1) * stride, list + j * stride, sizeof(int) * stride); + j--; + } + + memcpy(list + (j + 1) * stride, x, sizeof(int) * stride); + } +} + + + +// Compute UI sizes. +void mjui_resize(mjUI* ui, const mjrContext* con) { + // get maximum buffer size allowed by OpenGL driver + int maxBufferSize = 0; + glGetIntegerv(GL_MAX_RENDERBUFFER_SIZE, &maxBufferSize); + + // USED FOR TESTING OF SMALL BUFFER SIZES + // maxBufferSize = 3000; + + // resize with current section states, clamp maxheight + tryresize(ui, con); + ui->maxheight = mjMIN(ui->maxheight, maxBufferSize); + + // if height is too large, close some sections + if (ui->height > ui->maxheight) { + // init new height with section gaps + int hnew = (ui->nsect + 1) * SCL(ui->spacing.section, con); + + // add titles of regular sections and contents of fixed sections + for (int n = 0; n < ui->nsect; ++n) { + if (ui->sect[n].state == mjSECT_FIXED) { + hnew += ui->sect[n].rcontent.height; + } + else { + hnew += ui->sect[n].rtitle.height; + } + } + + // if fixed height is too big, nothing we can do + if (hnew > ui->maxheight) { + mju_error("fixed section height already too big, closing sections cannot help"); + } + + // sort open sections by lastclick + int nopen = 0; + int sortbuf[2 * mjMAXUISECT] = { 0 }; + for (int n = 0; n < ui->nsect; ++n) { + if (ui->sect[n].state == mjSECT_OPEN) { + sortbuf[2 * nopen] = ui->sect[n].lastclick; + sortbuf[2 * nopen + 1] = n; + ++nopen; + } + } + insertionsortgroup(sortbuf, nopen, 2); + + // nothing is open; SHOULD NOT OCCUR + if (nopen == 0) { + mju_error("internal error: expected some sections to be open"); + } + + // keep most recent sections: as many as can fit in maxheight + for (int i = nopen - 1; i >= 0; --i) { + // section fits: add height + if (hnew + ui->sect[sortbuf[2 * i + 1]].rcontent.height <= ui->maxheight) { + hnew += ui->sect[sortbuf[2 * i + 1]].rcontent.height; + } + + // section does not fit: mark for closing + else { + sortbuf[2 * i] = -1; + } + } + + // close sections that were marked + for (int i = 0; i < nopen; ++i) { + if (sortbuf[2 * i] == -1) { + ui->sect[sortbuf[2 * i + 1]].state = mjSECT_CLOSED; + } + } + + // resize with new section states, clamp maxheight again + tryresize(ui, con); + ui->maxheight = mjMIN(ui->maxheight, maxBufferSize); + + // make sure tryresize did what we expected; SHOULD NOT OCCUR + if (ui->height != hnew) { + mju_error("internal error: tryresize produced unexpeced ui height"); + } + } } @@ -1653,7 +1912,8 @@ void mjui_update(int section, int item, const mjUI* ui, // start rendering mjr_setAux(ui->auxid, con); - initOpenGL(ui, con); + mjrRect rgl = { 0, 0, ui->width, ui->height }; + initOpenGL(&rgl, con); // all sections: clear background if (section < 0) { @@ -1684,34 +1944,81 @@ void mjui_update(int section, int item, const mjUI* ui, // redraw section title and pane if (section < 0 || item < 0) { - // title shown - if (s->state < 2) { - // interpolated rectangle - r = s->rtitle; - glBegin(GL_QUADS); - glColor3fv(ui->color.sectpane); - glVertex2i(r.left, r.bottom); - glVertex2i(r.left+r.width, r.bottom); - glColor3fv(ui->color.secttitle); - glVertex2i(r.left+r.width, r.bottom+r.height); - glVertex2i(r.left, r.bottom+r.height); - glEnd(); + r = s->rtitle; - // symbol and text - drawsymbol(s->rtitle, s->state, 0, ui, con); - drawtext(s->name, s->rtitle.left+g_texthor, s->rtitle.bottom+g_textver, - 2*maxwidth, ui->color.sectfont, con); + // title shown + if (s->state != mjSECT_FIXED) { + // section without checkbox + if (s->checkbox == 0) { + // interpolated rectangle + glBegin(GL_QUADS); + glColor3fv(ui->color.secttitle2); + glVertex2i(r.left, r.bottom); + glVertex2i(r.left + r.width, r.bottom); + glColor3fv(ui->color.secttitle); + glVertex2i(r.left + r.width, r.bottom + r.height); + glVertex2i(r.left, r.bottom + r.height); + glEnd(); + + // symbol and text + drawsymbol(r, s->state, 0, ui, con); + drawtext(s->name, r.left + g_texthor, + r.bottom + g_textver, 2 * maxwidth, + ui->color.sectfont, con); + } + + // section with checkbox + else { + glBegin(GL_QUADS); + glColor3fv(ui->color.secttitlecheck2); + glVertex2i(r.left, r.bottom); + glVertex2i(r.left + r.width, r.bottom); + glColor3fv(ui->color.secttitlecheck); + glVertex2i(r.left + r.width, r.bottom + r.height); + glVertex2i(r.left, r.bottom + r.height); + glEnd(); + + // symbol and text with offset + drawsymbol(r, s->state, 1, ui, con); + drawtext(s->name, r.left + r.height, + r.bottom + g_textver, 2 * maxwidth - r.height, + ui->color.sectfont, con); + + // draw checkmark as specified + if (s->checkbox > 1) { + int cgap = r.height / 4; + mjrRect cr = { r.left + cgap, r.bottom + cgap, r.height - 2 * cgap, r.height - 2 * cgap }; + float rgb[3] = { + 0.5f * (ui->color.secttitlecheck[0] + ui->color.secttitlecheck2[0]), + 0.5f * (ui->color.secttitlecheck[1] + ui->color.secttitlecheck2[1]), + 0.5f * (ui->color.secttitlecheck[2] + ui->color.secttitlecheck2[2]) + }; + drawrectangle(cr, ui->color.sectsymbol, + s->checkbox == 2 ? rgb : NULL, con); + } + } // shortcut if (ui->mousehelp && s->shortcut) { - shortcuthelp(s->rtitle, s->modifier, s->shortcut, ui, con); + shortcuthelp(r, s->modifier, s->shortcut, ui, con); } } // content pane, active only - if (s->state) { + if (s->state != mjSECT_CLOSED) { drawrectangle(s->rcontent, ui->color.sectpane, NULL, con); } + + // round corners + mjrRect rround = s->rtitle; + if (s->state == mjSECT_FIXED) { + rround = s->rcontent; + } + else if (s->state == mjSECT_OPEN) { + rround.bottom = s->rcontent.bottom; + rround.height = s->rtitle.height + s->rcontent.height; + } + roundcorner(rround, 0, 0, ui, con); } // closed: skip items @@ -1754,10 +2061,10 @@ void mjui_update(int section, int item, const mjUI* ui, // background r = it->rect; glBegin(GL_QUADS); - glColor3fv(ui->color.sectpane); + glColor3fv(ui->color.separator2); glVertex2i(r.left, r.bottom); glVertex2i(r.left+r.width, r.bottom); - glColor3fv(ui->color.master); + glColor3fv(ui->color.separator); glVertex2i(r.left+r.width, r.bottom+r.height); glVertex2i(r.left, r.bottom+r.height); glEnd(); @@ -1768,13 +2075,12 @@ void mjui_update(int section, int item, const mjUI* ui, it->rect.bottom+g_textver, it->rect.width-2*g_texthor, ui->color.sectfont, con); - // symbol - if (it->state == mjSEPCLOSED+1) { - drawsymbol(it->rect, 1, 1, ui, con); - } else if (it->state == mjSEPCLOSED) { - drawsymbol(it->rect, 0, 1, ui, con); + // symbol and round corners for collapsible + if (it->state >= mjSEPCLOSED) { + int flg_open = (it->state == mjSEPCLOSED + 1); + drawsymbol(it->rect, flg_open, 2, ui, con); + roundcorner(it->rect, flg_open, 1, ui, con); } - break; case mjITEM_STATIC: @@ -1945,7 +2251,7 @@ void mjui_update(int section, int item, const mjUI* ui, it->rect.width-2*g_texthor, rgbfont, con); } - // draw tracking at the end + // draw tracking in mjui_render() break; case mjITEM_SLIDERINT: @@ -2062,46 +2368,6 @@ void mjui_update(int section, int item, const mjUI* ui, } } - // select tracking - if (ui->mousesect > 0 && ui->mouseitem >= 0) { - // get item pointer - const mjuiItem* it = ui->sect[ui->mousesect-1].item + ui->mouseitem; - - // proceed if select type - if (it->type == mjITEM_SELECT) { - // margin - r = it->rect; - r.left -= g_itemside; - r.width += 2*g_itemside; - r.height = it->multi.nelem * cellheight + g_itemside; - r.bottom -= r.height; - drawrectangle(r, ui->color.sectpane, NULL, con); - - // box - r = it->rect; - r.height = it->multi.nelem * cellheight; - r.bottom -= r.height; - drawrectangle(r, ui->color.select2, NULL, con); - - // hightlight row under mouse - int k = findselect(it, ui, state, con); - if (k >= 0) { - mjrRect r1 = r; - r1.bottom = r.bottom + (it->multi.nelem-1-k)*cellheight; - r1.height = cellheight; - drawrectangle(r1, ui->color.select, NULL, con); - } - - // values - for (int k=0; k < it->multi.nelem; k++) { - drawtext(it->multi.name[k], - r.left+g_texthor, - r.bottom+g_textver+(it->multi.nelem-1-k)*cellheight, - r.width-2*g_texthor, ui->color.fontactive, con); - } - } - } - // stop rendering mjr_restoreBuffer(con); } @@ -2114,6 +2380,11 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { mjuiItem* it; ui->editchanged = NULL; + // count mouse clicks over UI + if (state->type == mjEVENT_PRESS) { + ++ui->mouseclicks; + } + // non-left mouse events: handle shortcut help if ((state->type == mjEVENT_PRESS || state->type == mjEVENT_MOVE || state->type == mjEVENT_RELEASE) && state->button != mjBUTTON_LEFT) { @@ -2139,6 +2410,11 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { it_cur = ui->sect[sect_cur-1].item + item_cur; } + // update section lastclick + if (sect_cur > 0 && state->type == mjEVENT_PRESS) { + ui->sect[sect_cur - 1].lastclick = ui->mouseclicks; + } + // get recorded mouse section and item int sect_rec = ui->mousesect; int item_rec = -1; @@ -2242,19 +2518,33 @@ mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con) { // section title else if (sect_cur > 0 && item_cur < 0) { - // double-click: make all sections like this + mjuiSection* se = ui->sect + sect_cur - 1; + + // handle section checkbox + if (item_cur == -2 && se->checkbox > 0) { + ui->mousesectcheck = sect_cur; + return NULL; // leave it to user, because sections may interact + } + else { + ui->mousesectcheck = 0; + } + + // double-click: make all sections like this (exclude fixed) if (state->doubleclick) { for (int i=0; i < ui->nsect; i++) { - if (ui->sect[i].state < 2 && ui->sect[sect_cur-1].state < 2) { - ui->sect[i].state = ui->sect[sect_cur-1].state; + if (ui->sect[i].state != mjSECT_FIXED && se->state != mjSECT_FIXED) { + ui->sect[i].state = se->state; } } } - // single click: toggle section state + // single click: toggle section state (exclude fixed) else { - if (ui->sect[sect_cur-1].state < 2) { - ui->sect[sect_cur-1].state = 1 - ui->sect[sect_cur-1].state; + if (se->state == mjSECT_OPEN) { + se->state = mjSECT_CLOSED; + } + else if (se->state == mjSECT_CLOSED) { + se->state = mjSECT_OPEN; } } @@ -2607,7 +2897,7 @@ void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con) { mjr_blitAux(ui->auxid, raux, rect.left, rect.bottom + mjMAX(0, rect.height - ui->height + ui->scroll), con); - // draw scrollbar on top if needed + // draw scrollbar over blit if needed if (ui->height > rect.height) { // construct rectangles mjrRect bar; @@ -2618,4 +2908,60 @@ void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con) { mjr_rectangle(thumb, ui->color.thumb[0], ui->color.thumb[1], ui->color.thumb[2], 1); } + + // draw selection box tracking over blit if needed + if (ui->mousesect > 0 && ui->mouseitem >= 0) { + // get item pointer + const mjuiItem* it = ui->sect[ui->mousesect-1].item + ui->mouseitem; + + // proceed if select type + if (it->type == mjITEM_SELECT) { + // get relevant sizes + int g_texthor = SCL(ui->spacing.texthor, con); + int g_textver = SCL(ui->spacing.textver, con); + int g_itemside = SCL(ui->spacing.itemside, con); + int cellheight = con->charHeight + 2 * g_textver; + int offset = mjMAX(0, rect.height - ui->height + ui->scroll) - + mjMAX(0, ui->height - ui->scroll - rect.height); + + // margin + mjrRect r = it->rect; + r.left -= g_itemside; + r.width += 2*g_itemside; + r.height = it->multi.nelem * cellheight + g_itemside; + r.bottom -= r.height; + r.bottom += offset; + r.left += rect.left; + mjr_rectangle(r, ui->color.sectpane[0], + ui->color.sectpane[1], ui->color.sectpane[2], 1); + + // box + r = it->rect; + r.height = it->multi.nelem * cellheight; + r.bottom -= r.height; + r.bottom += offset; + r.left += rect.left; + mjr_rectangle(r, ui->color.select2[0], + ui->color.select2[1], ui->color.select2[2], 1); + + // hightlight row under mouse + int k = findselect(it, ui, state, con); + if (k >= 0) { + mjrRect r1 = r; + r1.bottom = r.bottom + (it->multi.nelem-1-k)*cellheight; + r1.height = cellheight; + mjr_rectangle(r1, ui->color.select[0], + ui->color.select[1], ui->color.select[2], 1); + } + + // text values + initOpenGL(&rect, con); + for (int k=0; k < it->multi.nelem; k++) { + drawtext(it->multi.name[k], + r.left+g_texthor - rect.left, + r.bottom+g_textver+(it->multi.nelem-1-k)*cellheight, + r.width-2*g_texthor, ui->color.fontactive, con); + } + } + } } diff --git a/src/user/CMakeLists.txt b/src/user/CMakeLists.txt index c5d02994..c73b1133 100644 --- a/src/user/CMakeLists.txt +++ b/src/user/CMakeLists.txt @@ -29,7 +29,7 @@ set(MUJOCO_USER_SRCS user_objects.h user_util.cc user_util.h - user_vfs.c + user_vfs.cc user_vfs.h ) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 2efa3856..dfef94a5 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -437,10 +437,11 @@ mjsPlugin* mjs_addPlugin(mjSpec* s) { // add default to model -mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id) { +mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent) { mjCModel* modelC = static_cast(s->element); - *id = (int)modelC->NumDefaults(); - mjCDef* def = modelC->AddDefault(classname, parentid); + mjCDef* parentC = parent ? static_cast(parent->element) : + static_cast(s->element)->Defaults(0); + mjCDef* def = modelC->AddDefault(classname, parentC); if (def) { return &def->spec; } else { @@ -529,6 +530,15 @@ mjsFrame* mjs_findFrame(mjSpec* s, const char* name) { +// find keyframe by name +mjsKey* mjs_findKeyframe(mjSpec* s, const char* name) { + mjCModel* model = static_cast(s->element); + mjCKey* key = (mjCKey*)model->FindObject(mjOBJ_KEY, std::string(name)); + return key ? &(static_cast(key)->spec) : nullptr; +} + + + // set frame void mjs_setFrame(mjsElement* dest, mjsFrame* frame) { if (!frame) { @@ -580,6 +590,183 @@ mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child) { +// return spec's first element of selected type +mjsElement* mjs_firstElement(mjSpec* s, mjtObj type) { + mjCModel* modelC = static_cast(s->element); + return modelC->NextObject(NULL, type); +} + + + +// return spec's next element; return NULL if element is last +mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element) { + mjCModel* modelC = static_cast(s->element); + return modelC->NextObject(element); +} + + + +// return body given mjsElement +mjsBody* mjs_asBody(mjsElement* element) { + return element->elemtype == mjOBJ_BODY ? &(static_cast(element)->spec) : nullptr; +} + + + +// return geom given mjsElement +mjsGeom* mjs_asGeom(mjsElement* element) { + return element->elemtype == mjOBJ_GEOM ? &(static_cast(element)->spec) : nullptr; +} + + + +// return joint given mjsElement +mjsJoint* mjs_asJoint(mjsElement* element) { + return element->elemtype == mjOBJ_JOINT ? &(static_cast(element)->spec) : nullptr; +} + + + +// Return site given mjsElement +mjsSite* mjs_asSite(mjsElement* element) { + return element->elemtype == mjOBJ_SITE ? &(static_cast(element)->spec) : nullptr; +} + + + +// return camera given mjsElement +mjsCamera* mjs_asCamera(mjsElement* element) { + return element->elemtype == mjOBJ_CAMERA ? &(static_cast(element)->spec) : nullptr; +} + + + +// return light given mjsElement +mjsLight* mjs_asLight(mjsElement* element) { + return element->elemtype == mjOBJ_LIGHT ? &(static_cast(element)->spec) : nullptr; +} + + + +// return frame given mjsElement +mjsFrame* mjs_asFrame(mjsElement* element) { + return element->elemtype == mjOBJ_FRAME ? &(static_cast(element)->spec) : nullptr; +} + + + +// return actuator given mjsElement +mjsActuator* mjs_asActuator(mjsElement* element) { + return element->elemtype == mjOBJ_ACTUATOR ? &(static_cast(element)->spec) : nullptr; +} + + + +// return sensor given mjsElement +mjsSensor* mjs_asSensor(mjsElement* element) { + return element->elemtype == mjOBJ_SENSOR ? &(static_cast(element)->spec) : nullptr; +} + + + +// return flex given mjsElement +mjsFlex* mjs_asFlex(mjsElement* element) { + return element->elemtype == mjOBJ_FLEX ? &(static_cast(element)->spec) : nullptr; +} + + + +// return pair given mjsElement +mjsPair* mjs_asPair(mjsElement* element) { + return element->elemtype == mjOBJ_PAIR ? &(static_cast(element)->spec) : nullptr; +} + + + +// return equality given mjsElement +mjsEquality* mjs_asEquality(mjsElement* element) { + return element->elemtype == mjOBJ_EQUALITY ? &(static_cast(element)->spec) : nullptr; +} + + + +// return exclude given mjsElement +mjsExclude* mjs_asExclude(mjsElement* element) { + return element->elemtype == mjOBJ_EXCLUDE ? &(static_cast(element)->spec) : nullptr; +} + + + +// return tendon given mjsElement +mjsTendon* mjs_asTendon(mjsElement* element) { + return element->elemtype == mjOBJ_TENDON ? &(static_cast(element)->spec) : nullptr; +} + + + +// return numeric given mjsElement +mjsNumeric* mjs_asNumeric(mjsElement* element) { + return element->elemtype == mjOBJ_NUMERIC ? &(static_cast(element)->spec) : nullptr; +} + + + +// return text given mjsElement +mjsText* mjs_asText(mjsElement* element) { + return element->elemtype == mjOBJ_TEXT ? &(static_cast(element)->spec) : nullptr; +} + + + +// return tuple given mjsElement +mjsTuple* mjs_asTuple(mjsElement* element) { + return element->elemtype == mjOBJ_TUPLE ? &(static_cast(element)->spec) : nullptr; +} + + + +// return key given mjsElement +mjsKey* mjs_asKey(mjsElement* element) { + return element->elemtype == mjOBJ_KEY ? &(static_cast(element)->spec) : nullptr; +} + + + +// return mesh given mjsElement +mjsMesh* mjs_asMesh(mjsElement* element) { + return element->elemtype == mjOBJ_MESH ? &(static_cast(element)->spec) : nullptr; +} + + + +// return hfield given mjsElement +mjsHField* mjs_asHField(mjsElement* element) { + return element->elemtype == mjOBJ_HFIELD ? &(static_cast(element)->spec) : nullptr; +} + + + +// return skin given mjsElement +mjsSkin* mjs_asSkin(mjsElement* element) { + return element->elemtype == mjOBJ_SKIN ? &(static_cast(element)->spec) : nullptr; +} + + + +// return texture given mjsElement +mjsTexture* mjs_asTexture(mjsElement* element) { + return element->elemtype == mjOBJ_TEXTURE ? &(static_cast(element)->spec) : nullptr; +} + + + +// return material given mjsElement +mjsMaterial* mjs_asMaterial(mjsElement* element) { + return element->elemtype == mjOBJ_MATERIAL ? &(static_cast(element)->spec) : nullptr; +} + + + // set string void mjs_setString(mjString* dest, const char* text) { std::string* str = static_cast(dest); @@ -696,13 +883,6 @@ void mjs_setActivePlugins(mjSpec* s, void* activeplugins) { -// compute full inertia -const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { - return FullInertia(quat, inertia, fullinertia); -} - - - // -------------------------- GLOBAL ASSET CACHE ------------------------------- void mj_setCacheSize(mjCache cache, std::size_t size) { diff --git a/src/user/user_api.h b/src/user/user_api.h index d7c03f0f..7c870ae2 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -156,7 +156,7 @@ MJAPI mjsKey* mjs_addKey(mjSpec* s); MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); // Add default. -MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, int parentid, int* id); +MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); //---------------------------------- Add assets ---------------------------------------------------- @@ -194,6 +194,9 @@ MJAPI mjsMesh* mjs_findMesh(mjSpec* s, const char* name); // Find frame by name. MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); +// Find keyframe by name. +MJAPI mjsKey* mjs_findKeyframe(mjSpec* s, const char* name); + // Get default corresponding to an element. MJAPI mjsDefault* mjs_getDefault(mjsElement* element); @@ -215,6 +218,81 @@ MJAPI mjsElement* mjs_firstChild(mjsBody* body, mjtObj type); // Return body's next child of the same type; return NULL if child is last. MJAPI mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child); +// Return spec's first element of selected type. +MJAPI mjsElement* mjs_firstElement(mjSpec* s, mjtObj type); + +// Return spec's next element; return NULL if element is last. +MJAPI mjsElement* mjs_nextElement(mjSpec* s, mjsElement* element); + +// Safely cast an element as mjsBody, or return NULL if the element is not an mjsBody. +MJAPI mjsBody* mjs_asBody(mjsElement* element); + +// Safely cast an element as mjsGeom, or return NULL if the element is not an mjsGeom. +MJAPI mjsGeom* mjs_asGeom(mjsElement* element); + +// Safely cast an element as mjsJoint, or return NULL if the element is not an mjsJoint. +MJAPI mjsJoint* mjs_asJoint(mjsElement* element); + +// Safely cast an element as mjsSite, or return NULL if the element is not an mjsSite. +MJAPI mjsSite* mjs_asSite(mjsElement* element); + +// Safely cast an element as mjsCamera, or return NULL if the element is not an mjsCamera. +MJAPI mjsCamera* mjs_asCamera(mjsElement* element); + +// Safely cast an element as mjsLight, or return NULL if the element is not an mjsLight. +MJAPI mjsLight* mjs_asLight(mjsElement* element); + +// Safely cast an element as mjsFrame, or return NULL if the element is not an mjsFrame. +MJAPI mjsFrame* mjs_asFrame(mjsElement* element); + +// Safely cast an element as mjsActuator, or return NULL if the element is not an mjsActuator. +MJAPI mjsActuator* mjs_asActuator(mjsElement* element); + +// Safely cast an element as mjsSensor, or return NULL if the element is not an mjsSensor. +MJAPI mjsSensor* mjs_asSensor(mjsElement* element); + +// Safely cast an element as mjsFlex, or return NULL if the element is not an mjsFlex. +MJAPI mjsFlex* mjs_asFlex(mjsElement* element); + +// Safely cast an element as mjsPair, or return NULL if the element is not an mjsPair. +MJAPI mjsPair* mjs_asPair(mjsElement* element); + +// Safely cast an element as mjsEquality, or return NULL if the element is not an mjsEquality. +MJAPI mjsEquality* mjs_asEquality(mjsElement* element); + +// Safely cast an element as mjsExclude, or return NULL if the element is not an mjsExclude. +MJAPI mjsExclude* mjs_asExclude(mjsElement* element); + +// Safely cast an element as mjsTendon, or return NULL if the element is not an mjsTendon. +MJAPI mjsTendon* mjs_asTendon(mjsElement* element); + +// Safely cast an element as mjsNumeric, or return NULL if the element is not an mjsNumeric. +MJAPI mjsNumeric* mjs_asNumeric(mjsElement* element); + +// Safely cast an element as mjsText, or return NULL if the element is not an mjsText. +MJAPI mjsText* mjs_asText(mjsElement* element); + +// Safely cast an element as mjsTuple, or return NULL if the element is not an mjsTuple. +MJAPI mjsTuple* mjs_asTuple(mjsElement* element); + +// Safely cast an element as mjsKey, or return NULL if the element is not an mjsKey. +MJAPI mjsKey* mjs_asKey(mjsElement* element); + +// Safely cast an element as mjsMesh, or return NULL if the element is not an mjsMesh. +MJAPI mjsMesh* mjs_asMesh(mjsElement* element); + +// Safely cast an element as mjsHField, or return NULL if the element is not an mjsHField. +MJAPI mjsHField* mjs_asHField(mjsElement* element); + +// Safely cast an element as mjsSkin, or return NULL if the element is not an mjsSkin. +MJAPI mjsSkin* mjs_asSkin(mjsElement* element); + +// Safely cast an element as mjsTexture, or return NULL if the element is not an mjsTexture. +MJAPI mjsTexture* mjs_asTexture(mjsElement* element); + +// Safely cast an element as mjsMaterial, or return NULL if the element is not an mjsMaterial. +MJAPI mjsMaterial* mjs_asMaterial(mjsElement* element); + //---------------------------------- Attribute setters --------------------------------------------- @@ -273,9 +351,6 @@ MJAPI void mjs_setFrame(mjsElement* dest, mjsFrame* frame); MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, const mjsOrientation* orientation); -// Compute quat and diag inertia from full inertia matrix, return error if any. -MJAPI const char* mjs_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]); - //---------------------------------- Initialization ----------------------------------------------- diff --git a/src/user/user_composite.cc b/src/user/user_composite.cc index 3ff3c280..891ce675 100644 --- a/src/user/user_composite.cc +++ b/src/user/user_composite.cc @@ -31,7 +31,6 @@ #include #include "cc/array_safety.h" #include "engine/engine_io.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_model.h" @@ -428,12 +427,12 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int } // compute volume - std::vector volume(uservert.size()/3); - mjtNum t = 1; + std::vector volume(uservert.size()/3); + double thickness = 1; if (dim == 2 && plugin.active) { try { mjCPlugin* pplugin = static_cast(plugin.instance); - t = std::stod(pplugin->config_attribs["thickness"], nullptr); + thickness = std::stod(pplugin->config_attribs["thickness"], nullptr); } catch (const std::invalid_argument& e) { return comperr(error, "Invalid thickness attribute", error_sz); } @@ -441,9 +440,9 @@ bool mjCComposite::MakeParticle(mjCModel* model, mjsBody* body, char* error, int if (!userface.empty()) { face = mjXUtil::String2Vector(userface); for (int j=0; jquat, this_quat, 4); } else { mjuu_setvec(body->pos, length_prev, 0, 0); - mjtNum negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; + double negquat[4] = {prev_quat[0], -prev_quat[1], -prev_quat[2], -prev_quat[3]}; mjuu_mulquat(dquat, negquat, this_quat); mjuu_copyvec(body->quat, dquat, 4); } @@ -1091,7 +1091,7 @@ void mjCComposite::BoxProject(double* pos) { // cylinder else if (type==mjCOMPTYPE_CYLINDER) { - double L0 = mju_max(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -1156,6 +1156,7 @@ bool mjCComposite::MakeBox(mjCModel* model, mjsBody* body, char* error, int erro BoxProject(b->pos); // reorient body + b->alt.type = mjORIENTATION_ZAXIS; mjuu_copyvec(b->alt.zaxis, b->pos, 3); mjuu_normvec(b->alt.zaxis, 3); diff --git a/src/user/user_composite.h b/src/user/user_composite.h index 88bccedc..aa16f136 100644 --- a/src/user/user_composite.h +++ b/src/user/user_composite.h @@ -107,7 +107,7 @@ class mjCComposite { std::string initial; // root boundary type std::vector uservert; // user-specified vertex positions std::string userface; // connectivity - mjtNum size[3]; // rope size (meaning depends on the shape) + double size[3]; // rope size (meaning depends on the shape) mjtCompShape curve[3]; // geometric shape // body names used in the skin @@ -137,7 +137,7 @@ class mjCComposite { private: mjsBody* AddRopeBody(mjCModel* model, mjsBody* body, int ix, int ix1); - mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, mjtNum normal[3], mjtNum prev_quat[4]); + mjsBody* AddCableBody(mjCModel* model, mjsBody* body, int ix, double normal[3], double prev_quat[4]); // temporary skin vectors void CopyIntoSkin(mjsSkin* skin); diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index 663c8b6e..a669a550 100644 --- a/src/user/user_flexcomp.cc +++ b/src/user/user_flexcomp.cc @@ -14,6 +14,7 @@ #include #include +#include #include #include #include @@ -26,13 +27,11 @@ #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_flexcomp.h" #include #include "user/user_model.h" @@ -147,7 +146,7 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // compute orientation - const char* alterr = mjs_resolveOrientation(quat, model->spec.degree, model->spec.euler, &alt); + const char* alterr = mjs_resolveOrientation(quat, model->spec.degree, model->spec.eulerseq, &alt); if (alterr) { return comperr(error, alterr, error_sz); } @@ -250,12 +249,9 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) { } // apply pose transform to points - mjtNum posn[3], quatn[4]; - mju_d2n(posn, pos, 3); - mju_d2n(quatn, quat, 4); for (int i=0; i < npnt; i++) { - mjtNum newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; - mju_trnVecPose(newp, posn, quatn, oldp); + double newp[3], oldp[3] = {point[3*i], point[3*i+1], point[3*i+2]}; + mjuu_trnVecPose(newp, pos, quat, oldp); point[3*i] = newp[0]; point[3*i+1] = newp[1]; point[3*i+2] = newp[2]; @@ -570,7 +566,7 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { int quad2tri[2][3] = {{0, 1, 2}, {0, 2, 3}}; // add point - mjtNum pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), + double pos[2] = {spacing[0]*(ix - 0.5*(count[0]-1)), spacing[1]*(iy - 0.5*(count[1]-1))}; point.push_back(pos[0]); point.push_back(pos[1]); @@ -578,8 +574,8 @@ bool mjCFlexcomp::MakeGrid(char* error, int error_sz) { // add texture coordinates, if not specified explicitly if (!hastex) { - texcoord.push_back(ix/(mjtNum)mjMAX(count[0]-1, 1)); - texcoord.push_back(iy/(mjtNum)mjMAX(count[1]-1, 1)); + texcoord.push_back(ix/(double)std::max(count[0]-1, 1)); + texcoord.push_back(iy/(double)std::max(count[1]-1, 1)); } // flip triangles if radial projection is requested @@ -713,7 +709,7 @@ void mjCFlexcomp::BoxProject(double* pos, int ix, int iy, int iz) { // cylinder else if (type==mjFCOMPTYPE_CYLINDER) { - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -749,8 +745,8 @@ bool mjCFlexcomp::MakeSquare(char* error, int error_sz) { }; for (int i=0; i < point.size()/3; i++) { - mjtNum* pos = point.data() + i*3; - double L0 = mjMAX(mju_abs(pos[0]), mju_abs(pos[1])); + double* pos = point.data() + i*3; + double L0 = std::max(std::abs(pos[0]), std::abs(pos[1])); mjuu_normvec(pos, 2); pos[0] *= size[0]*L0; pos[1] *= size[1]*L0; @@ -904,7 +900,7 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { } // load resource - std::string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir), + std::string filename = mjuu_combinePaths(mjs_getString(model->spec.modelfiledir), mjs_getString(model->spec.meshdir), file); mjResource* resource = nullptr; @@ -952,10 +948,10 @@ bool mjCFlexcomp::MakeMesh(mjCModel* model, char* error, int error_sz) { // copy faces element = mesh.face_; - // copy vertices, convert from float to mjtNum - point = vector (mesh.nvert()*3); + // copy vertices, convert from float to double + point = vector (mesh.nvert()*3); for (int i=0; i < mesh.nvert()*3; i++) { - point[i] = (mjtNum) mesh.vert_[i]; + point[i] = (double) mesh.vert_[i]; } return true; @@ -1003,7 +999,7 @@ bool mjCFlexcomp::MakeGMSH(mjCModel* model, char* error, int error_sz) { } // open resource - std::string filename = mjuu_makefullname(mjs_getString(model->spec.modelfiledir), + std::string filename = mjuu_combinePaths(mjs_getString(model->spec.modelfiledir), mjs_getString(model->spec.meshdir), file); mjResource* resource = nullptr; diff --git a/src/user/user_flexcomp.h b/src/user/user_flexcomp.h index a22780aa..d090022b 100644 --- a/src/user/user_flexcomp.h +++ b/src/user/user_flexcomp.h @@ -89,7 +89,7 @@ class mjCFlexcomp { // 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 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 diff --git a/src/user/user_init.c b/src/user/user_init.c index 864c7ec1..e7a1f885 100644 --- a/src/user/user_init.c +++ b/src/user/user_init.c @@ -36,9 +36,9 @@ void mjs_defaultSpec(mjSpec* spec) { spec->autolimits = 1; spec->settotalmass = -1; spec->degree = 1; - spec->euler[0] = 'x'; - spec->euler[1] = 'y'; - spec->euler[2] = 'z'; + spec->eulerseq[0] = 'x'; + spec->eulerseq[1] = 'y'; + spec->eulerseq[2] = 'z'; spec->convexhull = 1; spec->usethread = 1; spec->inertiafromgeom = mjINERTIAFROMGEOM_AUTO; diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 3d09d181..450174c0 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -57,11 +57,7 @@ #include "engine/engine_io.h" #include "engine/engine_plugin.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_solve.h" -#include "engine/engine_util_spatial.h" #include "user/user_cache.h" #include "user/user_model.h" #include "user/user_objects.h" @@ -77,7 +73,7 @@ using std::string; using std::vector; // compute triangle area, surface normal, center -static mjtNum _triangle(mjtNum* normal, mjtNum* center, +static double _triangle(double* normal, double* center, const float* v1, const float* v2, const float* v3) { // center if (center) { @@ -89,10 +85,10 @@ static mjtNum _triangle(mjtNum* normal, mjtNum* center, // normal = (v2-v1) cross (v3-v1) double b[3] = { v2[0]-v1[0], v2[1]-v1[1], v2[2]-v1[2] }; double c[3] = { v3[0]-v1[0], v3[1]-v1[1], v3[2]-v1[2] }; - mju_cross(normal, b, c); + mjuu_crossvec(normal, b, c); // get length - double len = mju_norm3(normal); + double len = sqrt(mjuu_dot3(normal, normal)); // ignore small faces if (len(this); spec.name = &name; - spec.classname = &classname; spec.file = &spec_file_; spec.content_type = &spec_content_type_; spec.uservert = &spec_vert_; @@ -296,7 +291,7 @@ void mjCMesh::LoadSDF() { pplugin->sdf_aabb(aabb, attributes.data()); mjtNum total = aabb[3] + aabb[4] + aabb[5]; - const mjtNum n = 300; + const double n = 300; int nx, ny, nz; nx = floor(n / total * aabb[3]) + 1; ny = floor(n / total * aabb[4]) + 1; @@ -373,7 +368,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { throw mjCError(this, "unsupported content type: '%s'", asset_type.c_str()); } - string filename = mjuu_makefullname(model->modelfiledir_, model->meshdir_, file_); + string filename = mjuu_combinePaths(model->modelfiledir_, model->meshdir_, file_); mjResource* resource = LoadResource(filename, vfs); try { @@ -456,7 +451,7 @@ void mjCMesh::Compile(const mjVFS* vfs) { int v0 = face_[3*i+0]; int v1 = face_[3*i+1]; int v2 = face_[3*i+2]; - mjtNum normal[3]; + double normal[3]; float* vtx = vert_.data(); if (_triangle(normal, nullptr, vtx+3*v0, vtx+3*v1, vtx+3*v2)>sqrt(mjMINVAL)) { halfedge_.push_back(std::pair(v0, v1)); @@ -552,7 +547,7 @@ void mjCMesh::SetBoundingVolume(int faceid) { node->contype = 1; node->pos = center_ + 3*faceid; node->quat = NULL; - mjtNum face_aamm[6] = {1E+10, 1E+10, 1E+10, -1E+10, -1E+10, -1E+10}; + double 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]; face_aamm[0] = mjMIN(face_aamm[0], vert_[3*vertid+0]); @@ -1263,15 +1258,15 @@ void mjCMesh::ApplyTransformations() { // rotate if (refquat[0]!=1 || refquat[1]!=0 || refquat[2]!=0 || refquat[3]!=0) { // prepare rotation - mjtNum quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; - mjtNum mat[9]; - mju_normalize4(quat); - mju_quat2Mat(mat, quat); + double quat[4] = {refquat[0], refquat[1], refquat[2], refquat[3]}; + double mat[9]; + mjuu_normvec(quat, 4); + mjuu_quat2mat(mat, quat); // process vertices for (int i=0; i < nvert(); i++) { - mjtNum p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; - mju_mulMatTVec3(p1, mat, p0); + double p1[3], p0[3] = {vert_[3*i], vert_[3*i+1], vert_[3*i+2]}; + mjuu_mulvecmatT(p1, p0, mat); vert_[3*i] = (float) p1[0]; vert_[3*i+1] = (float) p1[1]; vert_[3*i+2] = (float) p1[2]; @@ -1279,8 +1274,8 @@ void mjCMesh::ApplyTransformations() { // process normals for (int i=0; i < nnormal(); i++) { - mjtNum n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; - mju_mulMatTVec3(n1, mat, n0); + double n1[3], n0[3] = {normal_[3*i], normal_[3*i+1], normal_[3*i+2]}; + mjuu_mulvecmatT(n1, n0, mat); normal_[3*i] = (float) n1[0]; normal_[3*i+1] = (float) n1[1]; normal_[3*i+2] = (float) n1[2]; @@ -1449,13 +1444,13 @@ void mjCMesh::Process() { inert[5] = -P[5]; // get quaternion and diagonal inertia - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { inert[0], inert[3], inert[4], inert[3], inert[1], inert[5], inert[4], inert[5], inert[2] }; - mju_eig3(eigval, eigvec, quattmp, full); + mjuu_eig3(eigval, eigvec, quattmp, full); // check eigval - SHOULD NOT OCCUR if (eigval[2]<=0) { @@ -1479,12 +1474,12 @@ void mjCMesh::Process() { // if volume was valid, copy volume quat to shell and stop, // otherwise use shell quat for coordinate transformations if (type==mjINERTIA_SHELL && validvolume_>0) { - mju_copy4(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME)); + mjuu_copyvec(GetQuatPtr(type), GetQuatPtr(mjINERTIA_VOLUME), 4); continue; } // rotate vertices and normals into axis-aligned frame - mju_copy4(GetQuatPtr(type), quattmp); + mjuu_copyvec(GetQuatPtr(type), quattmp, 4); double neg[4] = {quattmp[0], -quattmp[1], -quattmp[2], -quattmp[3]}; double mat[9]; mjuu_quat2mat(mat, neg); @@ -1798,16 +1793,16 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // add normal to each vertex with weight = area for (int j=0; j<3; j++) { @@ -1833,25 +1828,25 @@ void mjCMesh::MakeNormal(void) { } // get triangle edges - mjtNum vec01[3], vec02[3]; + double vec01[3], vec02[3]; for (int j=0; j<3; j++) { vec01[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[0]+j]; vec02[j] = vert_[3*vertid[2]+j] - vert_[3*vertid[0]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, vec01, vec02); - mjtNum area = mju_normalize3(nrm); + double nrm[3]; + mjuu_crossvec(nrm, vec01, vec02); + double area = mjuu_normvec(nrm, 3); // compare to vertex normal, subtract contribution if dot product too small for (int j=0; j<3; j++) { // normalized vertex normal - mjtNum vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; - mju_normalize3(vnrm); + double vnrm[3] = {normal_[3*vertid[j]], normal_[3*vertid[j]+1], normal_[3*vertid[j]+2]}; + mjuu_normvec(vnrm, 3); // dot too small: remove - if (mju_dot3(nrm, vnrm)<0.8) { + if (mjuu_dot3(nrm, vnrm)<0.8) { for (int k=0; k<3; k++) { nremove[3*vertid[j]+k] += nrm[k]*area; } @@ -1901,28 +1896,28 @@ void mjCMesh::MakeCenter(void) { int* vertid = face_.data() + 3*i; // get triangle edges - mjtNum a[3], b[3]; + double a[3], b[3]; for (int j=0; j<3; j++) { a[j] = vert_[3*vertid[0]+j] - vert_[3*vertid[2]+j]; b[j] = vert_[3*vertid[1]+j] - vert_[3*vertid[2]+j]; } // compute face normal - mjtNum nrm[3]; - mju_cross(nrm, a, b); + double nrm[3]; + mjuu_crossvec(nrm, a, b); // compute circumradius - mjtNum norm_a_2 = mju_dot3(a, a); - mjtNum norm_b_2 = mju_dot3(b, b); - mjtNum area = mju_norm3(nrm); + double norm_a_2 = mjuu_dot3(a, a); + double norm_b_2 = mjuu_dot3(b, b); + double area = mjuu_normvec(nrm, 3); // compute circumcenter - mjtNum res[3], vec[3] = { + double res[3], vec[3] = { norm_a_2 * b[0] - norm_b_2 * a[0], norm_a_2 * b[1] - norm_b_2 * a[1], norm_a_2 * b[2] - norm_b_2 * a[2] }; - mju_cross(res, vec, nrm); + mjuu_crossvec(res, vec, nrm); center_[3*i+0] = res[0]/(2*area*area) + vert_[3*vertid[2]+0]; center_[3*i+1] = res[1]/(2*area*area) + vert_[3*vertid[2]+1]; center_[3*i+2] = res[2]/(2*area*area) + vert_[3*vertid[2]+2]; @@ -1986,7 +1981,6 @@ mjCSkin& mjCSkin::operator=(const mjCSkin& other) { void mjCSkin::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.file = &spec_file_; spec.material = &spec_material_; spec.vert = &spec_vert_; @@ -2097,7 +2091,7 @@ void mjCSkin::Compile(const mjVFS* vfs) { throw mjCError(this, "Unknown skin file type: %s", file_.c_str()); } - string filename = mjuu_makefullname(model->modelfiledir_, model->meshdir_, file_); + string filename = mjuu_combinePaths(model->modelfiledir_, model->meshdir_, file_); mjResource* resource = LoadResource(filename, vfs); try { @@ -2200,13 +2194,13 @@ void mjCSkin::Compile(const mjVFS* vfs) { // normalize bindquat for (int i=0; i < nbone; i++) { - mjtNum quat[4] = { - (mjtNum)bindquat_[4*i], - (mjtNum)bindquat_[4*i+1], - (mjtNum)bindquat_[4*i+2], - (mjtNum)bindquat_[4*i+3] + double quat[4] = { + (double)bindquat_[4*i], + (double)bindquat_[4*i+1], + (double)bindquat_[4*i+2], + (double)bindquat_[4*i+3] }; - mju_normalize4(quat); + mjuu_normvec(quat, 4); bindquat_[4*i] = (float) quat[0]; bindquat_[4*i+1] = (float) quat[1]; @@ -2398,7 +2392,6 @@ mjCFlex& mjCFlex::operator=(const mjCFlex& other) { void mjCFlex::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.material = &spec_material_; spec.vertbody = &spec_vertbody_; spec.vert = &spec_vert_; @@ -2562,17 +2555,17 @@ void mjCFlex::Compile(const mjVFS* vfs) { } // compute global vertex positions - vertxpos = vector (3*nvert); + vertxpos = vector (3*nvert); for (int i=0; i < nvert; i++) { // get body id, set vertxpos = body.xpos0 int b = rigid ? vertbodyid[0] : vertbodyid[i]; - mju_copy3(vertxpos.data()+3*i, model->Bodies()[b]->xpos0); + mjuu_copyvec(vertxpos.data()+3*i, model->Bodies()[b]->xpos0, 3); // 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); + double offset[3]; + mjuu_rotVecQuat(offset, vert_.data()+3*i, model->Bodies()[b]->xquat0); + mjuu_addtovec(vertxpos.data()+3*i, offset, 3); } } @@ -2581,18 +2574,18 @@ void mjCFlex::Compile(const mjVFS* vfs) { if (dim==3) { for (int e=0; e0) { + double nrm[3]; + mjuu_crossvec(nrm, v01, v02); + if (mjuu_dot3(nrm, v03)>0) { // flip orientation int tmp = elem_[e*(dim+1)+1]; elem_[e*(dim+1)+1] = elem_[e*(dim+1)+2]; @@ -2672,9 +2665,9 @@ void mjCFlex::CreateBVH(void) { } // 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]); + double xmin[3], xmax[3]; + mjuu_copyvec(xmin, vertxpos.data() + 3*edata[0], 3); + mjuu_copyvec(xmax, vertxpos.data() + 3*edata[0], 3); for (int i=1; i <= dim; i++) { for (int j=0; j<3; j++) { xmin[j] = mjMIN(xmin[j], vertxpos[3*edata[i]+j]); diff --git a/src/user/user_model.cc b/src/user/user_model.cc index a8a0d77c..c1b1b3ae 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -15,6 +15,7 @@ #include "user/user_model.h" #include +#include #include #include #include @@ -35,10 +36,10 @@ #include "cc/array_safety.h" #include "engine/engine_forward.h" #include "engine/engine_io.h" +#include "engine/engine_name.h" #include "engine/engine_plugin.h" #include "engine/engine_setconst.h" #include "engine/engine_support.h" -#include "engine/engine_util_blas.h" #include "engine/engine_util_errmem.h" #include "engine/engine_util_misc.h" #include "user/user_api.h" @@ -69,15 +70,6 @@ using std::vector; #endif -// copy real-valued vector -template -static void copyvec(T1* dest, T2* src, int n) { - for (int i=0; iname = "main"; // world body mjCBody* world = new mjCBody(this); @@ -713,6 +706,70 @@ mjCBase* mjCModel::GetObject(mjtObj type, int id) { +template +static mjsElement* GetNext(std::vector& list, mjsElement* child) { + // TODO: use id for direct indexing instead of a loop + for (unsigned int i = 0; i < list.size()-1; i++) { + if (list[i]->spec.element == child) { + return list[i+1]->spec.element; + } + } + return nullptr; +} + + + +// next object of specified type +mjsElement* mjCModel::NextObject(mjsElement* object, mjtObj type) { + if (type == mjOBJ_UNKNOWN) { + if (!object) { + throw mjCError(NULL, "type must be specified if no element is given"); + } else { + type = object->elemtype; + } + } else if (object && object->elemtype != type) { + throw mjCError(NULL, "element is not of requested type"); + } + + switch (type) { + case mjOBJ_ACTUATOR: + return object ? GetNext(actuators_, object) : actuators_[0]; + case mjOBJ_SENSOR: + return object ? GetNext(sensors_, object) : sensors_[0]; + case mjOBJ_FLEX: + return object ? GetNext(flexes_, object) : flexes_[0]; + case mjOBJ_PAIR: + return object ? GetNext(pairs_, object) : pairs_[0]; + case mjOBJ_EXCLUDE: + return object ? GetNext(excludes_, object) : excludes_[0]; + case mjOBJ_EQUALITY: + return object ? GetNext(equalities_, object) : equalities_[0]; + case mjOBJ_TENDON: + return object ? GetNext(tendons_, object) : tendons_[0]; + case mjOBJ_NUMERIC: + return object ? GetNext(numerics_, object) : numerics_[0]; + case mjOBJ_TEXT: + return object ? GetNext(texts_, object) : texts_[0]; + case mjOBJ_TUPLE: + return object ? GetNext(tuples_, object) : tuples_[0]; + case mjOBJ_KEY: + return object ? GetNext(keys_, object) : keys_[0]; + case mjOBJ_MESH: + return object ? GetNext(meshes_, object) : meshes_[0]; + case mjOBJ_HFIELD: + return object ? GetNext(hfields_, object) : hfields_[0]; + case mjOBJ_SKIN: + return object ? GetNext(skins_, object) : skins_[0]; + case mjOBJ_TEXTURE: + return object ? GetNext(textures_, object) : textures_[0]; + case mjOBJ_MATERIAL: + return object ? GetNext(materials_, object) : materials_[0]; + default: + return nullptr; + } +} + + //------------------------ API FOR ACCESS TO PRIVATE VARIABLES ------------------------------------- // compiled flag @@ -749,7 +806,9 @@ mjCDef* mjCModel::FindDefault(string name) { // add default class to array -mjCDef* mjCModel::AddDefault(string name, int parentid) { +mjCDef* mjCModel::AddDefault(string name, mjCDef* parent) { + int parentid = parent ? parent->id : 0; + // check for repeated name int thisid = (int)defaults_.size(); for (int i=0; iid = thisid; // initialize contents if (parentid>=0 && parentid(std::vector& elements) { elements.clear(); } +template <> +void mjCModel::DeleteAll(std::vector& elements) { + for (mjCKey* element : elements) { + delete element; + } + elements.clear(); +} + // set nuser fields void mjCModel::SetNuser() { if (nuser_body == -1) { @@ -1434,11 +1502,11 @@ void mjCModel::AutoSpringDamper(mjModel* m) { for (int i=0; idof_invweight0[adr+i]; } - inertia = ((mjtNum)ndim) / mju_max(mjMINVAL, inertia); + inertia = ((mjtNum)ndim) / std::max(mjMINVAL, inertia); // compute stiffness and damping (same as solref computation) - mjtNum stiffness = inertia / mju_max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); - mjtNum damping = 2 * inertia / mju_max(mjMINVAL, timeconst); + mjtNum stiffness = inertia / std::max(mjMINVAL, timeconst*timeconst*dampratio*dampratio); + mjtNum damping = 2 * inertia / std::max(mjMINVAL, timeconst); // assign m->jnt_stiffness[n] = stiffness; @@ -1771,14 +1839,14 @@ void mjCModel::CopyTree(mjModel* m) { m->body_dofadr[i] = (pb->dofnum ? dofadr : -1); m->body_geomnum[i] = (int)pb->geoms.size(); m->body_geomadr[i] = (!pb->geoms.empty() ? pb->geoms[0]->id : -1); - copyvec(m->body_pos+3*i, pb->pos, 3); - copyvec(m->body_quat+4*i, pb->quat, 4); - copyvec(m->body_ipos+3*i, pb->ipos, 3); - copyvec(m->body_iquat+4*i, pb->iquat, 4); + mjuu_copyvec(m->body_pos+3*i, pb->pos, 3); + mjuu_copyvec(m->body_quat+4*i, pb->quat, 4); + mjuu_copyvec(m->body_ipos+3*i, pb->ipos, 3); + mjuu_copyvec(m->body_iquat+4*i, pb->iquat, 4); m->body_mass[i] = (mjtNum)pb->mass; - copyvec(m->body_inertia+3*i, pb->inertia, 3); + mjuu_copyvec(m->body_inertia+3*i, pb->inertia, 3); m->body_gravcomp[i] = pb->gravcomp; - copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); + mjuu_copyvec(m->body_user+nuser_body*i, pb->get_userdata().data(), nuser_body); m->body_contype[i] = pb->contype; m->body_conaffinity[i] = pb->conaffinity; @@ -1854,23 +1922,23 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; - copyvec(m->jnt_pos+3*jid, pj->pos, 3); - copyvec(m->jnt_axis+3*jid, pj->axis, 3); + mjuu_copyvec(m->jnt_pos+3*jid, pj->pos, 3); + mjuu_copyvec(m->jnt_axis+3*jid, pj->axis, 3); m->jnt_stiffness[jid] = (mjtNum)pj->stiffness; - copyvec(m->jnt_range+2*jid, pj->range, 2); - copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); - copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); - copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); + mjuu_copyvec(m->jnt_range+2*jid, pj->range, 2); + mjuu_copyvec(m->jnt_actfrcrange+2*jid, pj->actfrcrange, 2); + mjuu_copyvec(m->jnt_solref+mjNREF*jid, pj->solref_limit, mjNREF); + mjuu_copyvec(m->jnt_solimp+mjNIMP*jid, pj->solimp_limit, mjNIMP); m->jnt_margin[jid] = (mjtNum)pj->margin; - copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); + mjuu_copyvec(m->jnt_user+nuser_jnt*jid, pj->get_userdata().data(), nuser_jnt); // not simple if: rotation already found, or pos not zero, or mis-aligned axis if (rotfound || !IsNullPose(m->jnt_pos+3*jid, NULL) || ((pj->type==mjJNT_HINGE || pj->type==mjJNT_SLIDE) && - ((mju_abs(pj->axis[0])>mjEPS) + - (mju_abs(pj->axis[1])>mjEPS) + - (mju_abs(pj->axis[2])>mjEPS)) > 1)) { + ((std::abs(pj->axis[0])>mjEPS) + + (std::abs(pj->axis[1])>mjEPS) + + (std::abs(pj->axis[2])>mjEPS)) > 1)) { m->body_simple[i] = 0; } @@ -1882,9 +1950,9 @@ void mjCModel::CopyTree(mjModel* m) { // set qpos0 and qpos_spring, check type switch (pj->type) { case mjJNT_FREE: - copyvec(m->qpos0+qposadr, pb->pos, 3); - copyvec(m->qpos0+qposadr+3, pb->quat, 4); - mju_copy(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); + mjuu_copyvec(m->qpos0+qposadr, pb->pos, 3); + mjuu_copyvec(m->qpos0+qposadr+3, pb->quat, 4); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 7); break; case mjJNT_BALL: @@ -1892,7 +1960,7 @@ void mjCModel::CopyTree(mjModel* m) { m->qpos0[qposadr+1] = 0; m->qpos0[qposadr+2] = 0; m->qpos0[qposadr+3] = 0; - mju_copy4(m->qpos_spring+qposadr, m->qpos0+qposadr); + mjuu_copyvec(m->qpos_spring+qposadr, m->qpos0+qposadr, 4); break; case mjJNT_SLIDE: @@ -1910,8 +1978,8 @@ void mjCModel::CopyTree(mjModel* m) { // set attributes m->dof_bodyid[dofadr] = pb->id; m->dof_jntid[dofadr] = jid; - copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); - copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); + mjuu_copyvec(m->dof_solref+mjNREF*dofadr, pj->solref_friction, mjNREF); + mjuu_copyvec(m->dof_solimp+mjNIMP*dofadr, pj->solimp_friction, mjNIMP); m->dof_frictionloss[dofadr] = (mjtNum)pj->frictionloss; m->dof_armature[dofadr] = (mjtNum)pj->armature; m->dof_damping[dofadr] = (mjtNum)pj->damping; @@ -1962,19 +2030,19 @@ void mjCModel::CopyTree(mjModel* m) { m->geom_matid[gid] = pg->matid; m->geom_group[gid] = pg->group; m->geom_priority[gid] = pg->priority; - copyvec(m->geom_size+3*gid, pg->size, 3); - copyvec(m->geom_aabb+6*gid, pg->aabb, 6); - copyvec(m->geom_pos+3*gid, pg->pos, 3); - copyvec(m->geom_quat+4*gid, pg->quat, 4); - copyvec(m->geom_friction+3*gid, pg->friction, 3); + mjuu_copyvec(m->geom_size+3*gid, pg->size, 3); + mjuu_copyvec(m->geom_aabb+6*gid, pg->aabb, 6); + mjuu_copyvec(m->geom_pos+3*gid, pg->pos, 3); + mjuu_copyvec(m->geom_quat+4*gid, pg->quat, 4); + mjuu_copyvec(m->geom_friction+3*gid, pg->friction, 3); m->geom_solmix[gid] = (mjtNum)pg->solmix; - copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); - copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); + mjuu_copyvec(m->geom_solref+mjNREF*gid, pg->solref, mjNREF); + mjuu_copyvec(m->geom_solimp+mjNIMP*gid, pg->solimp, mjNIMP); m->geom_margin[gid] = (mjtNum)pg->margin; m->geom_gap[gid] = (mjtNum)pg->gap; - copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); - copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); - copyvec(m->geom_rgba+4*gid, pg->rgba, 4); + mjuu_copyvec(m->geom_fluid+mjNFLUID*gid, pg->fluid, mjNFLUID); + mjuu_copyvec(m->geom_user+nuser_geom*gid, pg->get_userdata().data(), nuser_geom); + mjuu_copyvec(m->geom_rgba+4*gid, pg->rgba, 4); // determine sameframe if (IsNullPose(m->geom_pos+3*gid, m->geom_quat+4*gid)) { @@ -2006,11 +2074,11 @@ void mjCModel::CopyTree(mjModel* m) { m->site_bodyid[sid] = ps->body->id; m->site_matid[sid] = ps->matid; m->site_group[sid] = ps->group; - copyvec(m->site_size+3*sid, ps->size, 3); - copyvec(m->site_pos+3*sid, ps->pos, 3); - copyvec(m->site_quat+4*sid, ps->quat, 4); - copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); - copyvec(m->site_rgba+4*sid, ps->rgba, 4); + mjuu_copyvec(m->site_size+3*sid, ps->size, 3); + mjuu_copyvec(m->site_pos+3*sid, ps->pos, 3); + mjuu_copyvec(m->site_quat+4*sid, ps->quat, 4); + mjuu_copyvec(m->site_user+nuser_site*sid, ps->userdata_.data(), nuser_site); + mjuu_copyvec(m->site_rgba+4*sid, ps->rgba, 4); // determine sameframe if (IsNullPose(m->site_pos+3*sid, m->site_quat+4*sid)) { @@ -2038,15 +2106,15 @@ void mjCModel::CopyTree(mjModel* m) { m->cam_bodyid[cid] = pc->body->id; m->cam_mode[cid] = pc->mode; m->cam_targetbodyid[cid] = pc->targetbodyid; - copyvec(m->cam_pos+3*cid, pc->pos, 3); - copyvec(m->cam_quat+4*cid, pc->quat, 4); + mjuu_copyvec(m->cam_pos+3*cid, pc->pos, 3); + mjuu_copyvec(m->cam_quat+4*cid, pc->quat, 4); m->cam_orthographic[cid] = pc->orthographic; m->cam_fovy[cid] = (mjtNum)pc->fovy; m->cam_ipd[cid] = (mjtNum)pc->ipd; - copyvec(m->cam_resolution+2*cid, pc->resolution, 2); - copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); - copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); - copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); + mjuu_copyvec(m->cam_resolution+2*cid, pc->resolution, 2); + mjuu_copyvec(m->cam_sensorsize+2*cid, pc->sensor_size, 2); + mjuu_copyvec(m->cam_intrinsic+4*cid, pc->intrinsic, 4); + mjuu_copyvec(m->cam_user+nuser_cam*cid, pc->get_userdata().data(), nuser_cam); } // loop over lights for this body @@ -2062,15 +2130,15 @@ void mjCModel::CopyTree(mjModel* m) { m->light_directional[lid] = (mjtByte)pl->directional; m->light_castshadow[lid] = (mjtByte)pl->castshadow; m->light_active[lid] = (mjtByte)pl->active; - copyvec(m->light_pos+3*lid, pl->pos, 3); - copyvec(m->light_dir+3*lid, pl->dir, 3); + mjuu_copyvec(m->light_pos+3*lid, pl->pos, 3); + mjuu_copyvec(m->light_dir+3*lid, pl->dir, 3); m->light_bulbradius[lid] = pl->bulbradius; - copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); + mjuu_copyvec(m->light_attenuation+3*lid, pl->attenuation, 3); m->light_cutoff[lid] = pl->cutoff; m->light_exponent[lid] = pl->exponent; - copyvec(m->light_ambient+3*lid, pl->ambient, 3); - copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); - copyvec(m->light_specular+3*lid, pl->specular, 3); + mjuu_copyvec(m->light_ambient+3*lid, pl->ambient, 3); + mjuu_copyvec(m->light_diffuse+3*lid, pl->diffuse, 3); + mjuu_copyvec(m->light_specular+3*lid, pl->specular, 3); } } @@ -2210,9 +2278,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->mesh_graphadr[i] = (pme->szgraph() ? graph_adr : -1); m->mesh_bvhnum[i] = pme->tree().nbvh; m->mesh_bvhadr[i] = pme->tree().nbvh ? bvh_adr : -1; - copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); - copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); - copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); + mjuu_copyvec(&m->mesh_scale[3 * i], pme->get_scale(), 3); + mjuu_copyvec(&m->mesh_pos[3 * i], pme->GetOffsetPosPtr(), 3); + mjuu_copyvec(&m->mesh_quat[4 * i], pme->GetOffsetQuatPtr(), 4); // copy vertices, normals, faces, texcoords, aux data pme->CopyVert(m->mesh_vert + 3*vert_adr); @@ -2268,13 +2336,13 @@ void mjCModel::CopyObjects(mjModel* m) { 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); + mjuu_copyvec(m->flex_solref + mjNREF * i, pfl->solref, mjNREF); + mjuu_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); + mjuu_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); + mjuu_copyvec(m->flex_rgba + 4 * i, pfl->rgba, 4); // set fields: mesh-like m->flex_dim[i] = pfl->dim; @@ -2338,7 +2406,7 @@ void mjCModel::CopyObjects(mjModel* m) { // copy or set vert if (pfl->centered) { - mju_zero(m->flex_vert + 3*vert_adr, 3*pfl->nvert); + mjuu_zerovec(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)); @@ -2393,7 +2461,7 @@ void mjCModel::CopyObjects(mjModel* m) { // set fields m->skin_matid[i] = psk->matid; m->skin_group[i] = psk->group; - copyvec(m->skin_rgba+4*i, psk->rgba, 4); + mjuu_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->get_vert().size()/3; @@ -2448,7 +2516,7 @@ void mjCModel::CopyObjects(mjModel* m) { mjCHField* phf = hfields_[i]; // set fields - copyvec(m->hfield_size+4*i, phf->size, 4); + mjuu_copyvec(m->hfield_size+4*i, phf->size, 4); m->hfield_nrow[i] = phf->nrow; m->hfield_ncol[i] = phf->ncol; m->hfield_adr[i] = data_adr; @@ -2485,16 +2553,19 @@ void mjCModel::CopyObjects(mjModel* m) { mjCMaterial* pmat = materials_[i]; // set fields - m->mat_texid[i] = pmat->texid; + m->mat_texid[mjNTEXMAT*i] = pmat->texid; + for (int j=1; jmat_texid[mjNTEXMAT*i+j] = -1; + } m->mat_texuniform[i] = pmat->texuniform; - copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); + mjuu_copyvec(m->mat_texrepeat+2*i, pmat->texrepeat, 2); m->mat_emission[i] = pmat->emission; m->mat_specular[i] = pmat->specular; m->mat_shininess[i] = pmat->shininess; m->mat_reflectance[i] = pmat->reflectance; m->mat_metallic[i] = pmat->metallic; m->mat_roughness[i] = pmat->roughness; - copyvec(m->mat_rgba+4*i, pmat->rgba, 4); + mjuu_copyvec(m->mat_rgba+4*i, pmat->rgba, 4); } // geom pairs to include @@ -2503,12 +2574,12 @@ void mjCModel::CopyObjects(mjModel* m) { m->pair_geom1[i] = pairs_[i]->geom1->id; m->pair_geom2[i] = pairs_[i]->geom2->id; m->pair_signature[i] = pairs_[i]->signature; - copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); - copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); - copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); + mjuu_copyvec(m->pair_solref+mjNREF*i, pairs_[i]->solref, mjNREF); + mjuu_copyvec(m->pair_solreffriction+mjNREF*i, pairs_[i]->solreffriction, mjNREF); + mjuu_copyvec(m->pair_solimp+mjNIMP*i, pairs_[i]->solimp, mjNIMP); m->pair_margin[i] = (mjtNum)pairs_[i]->margin; m->pair_gap[i] = (mjtNum)pairs_[i]->gap; - copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); + mjuu_copyvec(m->pair_friction+5*i, pairs_[i]->friction, 5); } // body pairs to exclude @@ -2526,9 +2597,9 @@ void mjCModel::CopyObjects(mjModel* m) { m->eq_obj1id[i] = peq->obj1id; m->eq_obj2id[i] = peq->obj2id; m->eq_active0[i] = peq->active; - copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); - copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); - copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); + mjuu_copyvec(m->eq_solref+mjNREF*i, peq->solref, mjNREF); + mjuu_copyvec(m->eq_solimp+mjNIMP*i, peq->solimp, mjNIMP); + mjuu_copyvec(m->eq_data+mjNEQDATA*i, peq->data, mjNEQDATA); } // tendons and wraps @@ -2544,10 +2615,10 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_group[i] = pte->group; m->tendon_limited[i] = (mjtByte)pte->is_limited(); m->tendon_width[i] = (mjtNum)pte->width; - copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); - copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); - copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); - copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); + mjuu_copyvec(m->tendon_solref_lim+mjNREF*i, pte->solref_limit, mjNREF); + mjuu_copyvec(m->tendon_solimp_lim+mjNIMP*i, pte->solimp_limit, mjNIMP); + mjuu_copyvec(m->tendon_solref_fri+mjNREF*i, pte->solref_friction, mjNREF); + mjuu_copyvec(m->tendon_solimp_fri+mjNIMP*i, pte->solimp_friction, mjNIMP); m->tendon_range[2*i] = (mjtNum)pte->range[0]; m->tendon_range[2*i+1] = (mjtNum)pte->range[1]; m->tendon_margin[i] = (mjtNum)pte->margin; @@ -2556,8 +2627,8 @@ void mjCModel::CopyObjects(mjModel* m) { m->tendon_frictionloss[i] = (mjtNum)pte->frictionloss; m->tendon_lengthspring[2*i] = (mjtNum)pte->springlength[0]; m->tendon_lengthspring[2*i+1] = (mjtNum)pte->springlength[1]; - copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); - copyvec(m->tendon_rgba+4*i, pte->rgba, 4); + mjuu_copyvec(m->tendon_user+nuser_tendon*i, pte->get_userdata().data(), nuser_tendon); + mjuu_copyvec(m->tendon_rgba+4*i, pte->rgba, 4); // set wraps for (int j=0; j<(int)pte->path.size(); j++) { @@ -2597,15 +2668,15 @@ void mjCModel::CopyObjects(mjModel* m) { m->actuator_actlimited[i] = (mjtByte)pac->is_actlimited(); m->actuator_actearly[i] = pac->actearly; m->actuator_cranklength[i] = (mjtNum)pac->cranklength; - copyvec(m->actuator_gear + 6*i, pac->gear, 6); - copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); - copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); - copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); - copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); - copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); - copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); - copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); - copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); + mjuu_copyvec(m->actuator_gear + 6*i, pac->gear, 6); + mjuu_copyvec(m->actuator_dynprm + mjNDYN*i, pac->dynprm, mjNDYN); + mjuu_copyvec(m->actuator_gainprm + mjNGAIN*i, pac->gainprm, mjNGAIN); + mjuu_copyvec(m->actuator_biasprm + mjNBIAS*i, pac->biasprm, mjNBIAS); + mjuu_copyvec(m->actuator_ctrlrange + 2*i, pac->ctrlrange, 2); + mjuu_copyvec(m->actuator_forcerange + 2*i, pac->forcerange, 2); + mjuu_copyvec(m->actuator_actrange + 2*i, pac->actrange, 2); + mjuu_copyvec(m->actuator_lengthrange + 2*i, pac->lengthrange, 2); + mjuu_copyvec(m->actuator_user+nuser_actuator*i, pac->get_userdata().data(), nuser_actuator); } // sensors @@ -2625,7 +2696,7 @@ void mjCModel::CopyObjects(mjModel* m) { m->sensor_dim[i] = psen->dim; m->sensor_cutoff[i] = (mjtNum)psen->cutoff; m->sensor_noise[i] = (mjtNum)psen->noise; - copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); + mjuu_copyvec(m->sensor_user+nuser_sensor*i, psen->get_userdata().data(), nuser_sensor); // calculate address and advance m->sensor_adr[i] = adr; @@ -2690,34 +2761,34 @@ void mjCModel::CopyObjects(mjModel* m) { for (int i=0; ikey_time[i] = (mjtNum)keys_[i]->time; - copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); - copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); + mjuu_copyvec(m->key_qpos+i*nq, keys_[i]->qpos_.data(), nq); + mjuu_copyvec(m->key_qvel+i*nv, keys_[i]->qvel_.data(), nv); if (na) { - copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); + mjuu_copyvec(m->key_act+i*na, keys_[i]->act_.data(), na); } if (nmocap) { - copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); - copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); + mjuu_copyvec(m->key_mpos + i*3*nmocap, keys_[i]->mpos_.data(), 3*nmocap); + mjuu_copyvec(m->key_mquat + i*4*nmocap, keys_[i]->mquat_.data(), 4*nmocap); } // normalize quaternions in m->key_qpos for (int j=0; jnjnt; j++) { if (m->jnt_type[j]==mjJNT_BALL || m->jnt_type[j]==mjJNT_FREE) { - mju_normalize4(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE)); + mjuu_normvec(m->key_qpos+i*nq+m->jnt_qposadr[j]+3*(m->jnt_type[j]==mjJNT_FREE), 4); } } // normalize quaternions in m->key_mquat for (int j=0; jkey_mquat+i*4*nmocap+4*j); + mjuu_normvec(m->key_mquat+i*4*nmocap+4*j, 4); } - copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); + mjuu_copyvec(m->key_ctrl+i*nu, keys_[i]->ctrl_.data(), nu); } // save qpos0 in user model (to recognize changed key_qpos in write) qpos0.resize(nq); - mju_copy(qpos0.data(), m->qpos0, nq); + mjuu_copyvec(qpos0.data(), m->qpos0, nq); } @@ -2727,17 +2798,17 @@ void mjCModel::SaveState(const mjData* d) { for (auto joint : joints_) { switch (joint->type) { case mjJNT_FREE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 7); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 6); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 7); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 6); break; case mjJNT_BALL: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 4); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 3); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 4); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(joint->qpos, d->qpos + joint->qposadr_, 1); - mju_copy(joint->qvel, d->qvel + joint->dofadr_, 1); + mjuu_copyvec(joint->qpos, d->qpos + joint->qposadr_, 1); + mjuu_copyvec(joint->qvel, d->qvel + joint->dofadr_, 1); break; } } @@ -2745,7 +2816,7 @@ void mjCModel::SaveState(const mjData* d) { for (auto actuator : actuators_) { if (actuator->actadr_ != -1) { actuator->act.assign(actuator->actnum_, 0); - mju_copy(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); + mjuu_copyvec(actuator->act.data(), d->act + actuator->actadr_, actuator->actnum_); } } } @@ -2767,24 +2838,24 @@ void mjCModel::RestoreState(const mjModel* m, mjData** dest) { } switch (joint->type) { case mjJNT_FREE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 7); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 6); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 7); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 6); break; case mjJNT_BALL: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 4); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 3); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 4); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 3); break; case mjJNT_HINGE: case mjJNT_SLIDE: - mju_copy(d->qpos + joint->qposadr_, joint->qpos, 1); - mju_copy(d->qvel + joint->dofadr_, joint->qvel, 1); + mjuu_copyvec(d->qpos + joint->qposadr_, joint->qpos, 1); + mjuu_copyvec(d->qvel + joint->dofadr_, joint->qvel, 1); break; } } for (auto actuator : actuators_) { if (mjuu_defined(actuator->act[0])) { - mju_copy(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); + mjuu_copyvec(d->act + actuator->actadr_, actuator->act.data(), actuator->actnum_); } } } @@ -2930,7 +3001,7 @@ void mjCModel::FuseStatic(void) { // compute principal axes of inertia mjuu_copyvec(par->fullinertia, toti, 6); - const char* err1 = FullInertia(par->iquat, par->inertia, par->fullinertia); + const char* err1 = mjuu_fullInertia(par->iquat, par->inertia, par->fullinertia); if (err1) { throw mjCError(NULL, "error '%s' in fusing static body inertias", err1); } @@ -3185,9 +3256,6 @@ mjModel* mjCModel::Compile(const mjVFS* vfs, mjModel** m) { errInfo = mjCError(); warningtext[0] = 0; - // init random number generator, to make textures reproducible - srand(123); - try { if (setjmp(error_jmp_buf) != 0) { // TryCompile resulted in an mju_error which was converted to a longjmp. @@ -3566,14 +3634,14 @@ void mjCModel::TryCompile(mjModel*& m, mjData*& d, const mjVFS* vfs) { meaninertia_auto = m->stat.meaninertia; meanmass_auto = m->stat.meanmass; meansize_auto = m->stat.meansize; - copyvec(center_auto, m->stat.center, 3); + mjuu_copyvec(center_auto, m->stat.center, 3); // override model statistics if defined by user if (mjuu_defined(stat.extent)) m->stat.extent = (mjtNum)stat.extent; if (mjuu_defined(stat.meaninertia)) m->stat.meaninertia = (mjtNum)stat.meaninertia; if (mjuu_defined(stat.meanmass)) m->stat.meanmass = (mjtNum)stat.meanmass; if (mjuu_defined(stat.meansize)) m->stat.meansize = (mjtNum)stat.meansize; - if (mjuu_defined(stat.center[0])) copyvec(m->stat.center, stat.center, 3); + if (mjuu_defined(stat.center[0])) mjuu_copyvec(m->stat.center, stat.center, 3); // assert that model has valid references const char* validationerr = mj_validateReferences(m); @@ -3652,15 +3720,15 @@ bool mjCModel::CopyBack(const mjModel* m) { if (m->stat.center[0] != center_auto[0] || m->stat.center[1] != center_auto[1] || m->stat.center[2] != center_auto[2]) { - mju_copy3(stat.center, m->stat.center); + mjuu_copyvec(stat.center, m->stat.center, 3); } // qpos0, qpos_spring for (int i=0; itype) { case mjJNT_FREE: - copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); - copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->pos, m->qpos0+m->jnt_qposadr[i], 3); + mjuu_copyvec(bodies_[m->jnt_bodyid[i]]->quat, m->qpos0+m->jnt_qposadr[i]+3, 4); break; case mjJNT_SLIDE: @@ -3674,22 +3742,22 @@ bool mjCModel::CopyBack(const mjModel* m) { break; } } - mju_copy(qpos0.data(), m->qpos0, m->nq); + mjuu_copyvec(qpos0.data(), m->qpos0, m->nq); // body mjCBody* pb; for (int i=0; ipos, m->body_pos+3*i, 3); - copyvec(pb->quat, m->body_quat+4*i, 4); - copyvec(pb->ipos, m->body_ipos+3*i, 3); - copyvec(pb->iquat, m->body_iquat+4*i, 4); + mjuu_copyvec(pb->pos, m->body_pos+3*i, 3); + mjuu_copyvec(pb->quat, m->body_quat+4*i, 4); + mjuu_copyvec(pb->ipos, m->body_ipos+3*i, 3); + mjuu_copyvec(pb->iquat, m->body_iquat+4*i, 4); pb->mass = (double)m->body_mass[i]; - copyvec(pb->inertia, m->body_inertia+3*i, 3); + mjuu_copyvec(pb->inertia, m->body_inertia+3*i, 3); if (nuser_body) { - copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); + mjuu_copyvec(pb->userdata_.data(), m->body_user + nuser_body*i, nuser_body); } } @@ -3699,22 +3767,22 @@ bool mjCModel::CopyBack(const mjModel* m) { pj = joints_[i]; // joint data - copyvec(pj->pos, m->jnt_pos+3*i, 3); - copyvec(pj->axis, m->jnt_axis+3*i, 3); + mjuu_copyvec(pj->pos, m->jnt_pos+3*i, 3); + mjuu_copyvec(pj->axis, m->jnt_axis+3*i, 3); pj->stiffness = (double)m->jnt_stiffness[i]; - copyvec(pj->range, m->jnt_range+2*i, 2); - copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); - copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pj->range, m->jnt_range+2*i, 2); + mjuu_copyvec(pj->solref_limit, m->jnt_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pj->solimp_limit, m->jnt_solimp+mjNIMP*i, mjNIMP); pj->margin = (double)m->jnt_margin[i]; if (nuser_jnt) { - copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); + mjuu_copyvec(pj->userdata_.data(), m->jnt_user + nuser_jnt*i, nuser_jnt); } // dof data int j = m->jnt_dofadr[i]; - copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); - copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); + mjuu_copyvec(pj->solref_friction, m->dof_solref+mjNREF*j, mjNREF); + mjuu_copyvec(pj->solimp_friction, m->dof_solimp+mjNIMP*j, mjNIMP); pj->armature = (double)m->dof_armature[j]; pj->damping = (double)m->dof_damping[j]; pj->frictionloss = (double)m->dof_frictionloss[j]; @@ -3725,19 +3793,19 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; isize, m->geom_size+3*i, 3); - copyvec(pg->pos, m->geom_pos+3*i, 3); - copyvec(pg->quat, m->geom_quat+4*i, 4); - copyvec(pg->friction, m->geom_friction+3*i, 3); - copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); - copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); - copyvec(pg->rgba, m->geom_rgba+4*i, 4); + mjuu_copyvec(pg->size, m->geom_size+3*i, 3); + mjuu_copyvec(pg->pos, m->geom_pos+3*i, 3); + mjuu_copyvec(pg->quat, m->geom_quat+4*i, 4); + mjuu_copyvec(pg->friction, m->geom_friction+3*i, 3); + mjuu_copyvec(pg->solref, m->geom_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pg->solimp, m->geom_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pg->rgba, m->geom_rgba+4*i, 4); pg->solmix = (double)m->geom_solmix[i]; pg->margin = (double)m->geom_margin[i]; pg->gap = (double)m->geom_gap[i]; if (nuser_geom) { - copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); + mjuu_copyvec(pg->userdata_.data(), m->geom_user + nuser_geom*i, nuser_geom); } } @@ -3745,8 +3813,8 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCMesh* pm; for (int i=0; iGetOffsetPosPtr(), m->mesh_pos+3*i, 3); - copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); + mjuu_copyvec(pm->GetOffsetPosPtr(), m->mesh_pos+3*i, 3); + mjuu_copyvec(pm->GetOffsetQuatPtr(), m->mesh_quat+4*i, 4); } // heightfield @@ -3762,84 +3830,84 @@ bool mjCModel::CopyBack(const mjModel* m) { // copy back in reverse row order for (int j=0; jsize, m->site_size + 3 * i, 3); - copyvec(sites_[i]->pos, m->site_pos+3*i, 3); - copyvec(sites_[i]->quat, m->site_quat+4*i, 4); - copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); + mjuu_copyvec(sites_[i]->size, m->site_size + 3 * i, 3); + mjuu_copyvec(sites_[i]->pos, m->site_pos+3*i, 3); + mjuu_copyvec(sites_[i]->quat, m->site_quat+4*i, 4); + mjuu_copyvec(sites_[i]->rgba, m->site_rgba+4*i, 4); if (nuser_site) { - copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); + mjuu_copyvec(sites_[i]->userdata_.data(), m->site_user + nuser_site*i, nuser_site); } } // cameras for (int i=0; ipos, m->cam_pos+3*i, 3); - copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); + mjuu_copyvec(cameras_[i]->pos, m->cam_pos+3*i, 3); + mjuu_copyvec(cameras_[i]->quat, m->cam_quat+4*i, 4); cameras_[i]->fovy = (double)m->cam_fovy[i]; cameras_[i]->ipd = (double)m->cam_ipd[i]; - copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); - copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); + mjuu_copyvec(cameras_[i]->resolution, m->cam_resolution+2*i, 2); + mjuu_copyvec(cameras_[i]->intrinsic, m->cam_intrinsic+4*i, 4); if (nuser_cam) { - copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); + mjuu_copyvec(cameras_[i]->userdata_.data(), m->cam_user + nuser_cam*i, nuser_cam); } } // lights for (int i=0; ipos, m->light_pos+3*i, 3); - copyvec(lights_[i]->dir, m->light_dir+3*i, 3); - copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); + mjuu_copyvec(lights_[i]->pos, m->light_pos+3*i, 3); + mjuu_copyvec(lights_[i]->dir, m->light_dir+3*i, 3); + mjuu_copyvec(lights_[i]->attenuation, m->light_attenuation+3*i, 3); lights_[i]->cutoff = m->light_cutoff[i]; lights_[i]->exponent = m->light_exponent[i]; - copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); - copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); - copyvec(lights_[i]->specular, m->light_specular+3*i, 3); + mjuu_copyvec(lights_[i]->ambient, m->light_ambient+3*i, 3); + mjuu_copyvec(lights_[i]->diffuse, m->light_diffuse+3*i, 3); + mjuu_copyvec(lights_[i]->specular, m->light_specular+3*i, 3); } // materials for (int i=0; itexrepeat, m->mat_texrepeat+2*i, 2); + mjuu_copyvec(materials_[i]->texrepeat, m->mat_texrepeat+2*i, 2); materials_[i]->emission = m->mat_emission[i]; materials_[i]->specular = m->mat_specular[i]; materials_[i]->shininess = m->mat_shininess[i]; materials_[i]->reflectance = m->mat_reflectance[i]; - copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); + mjuu_copyvec(materials_[i]->rgba, m->mat_rgba+4*i, 4); } // pairs for (int i=0; isolref, m->pair_solref+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); - copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(pairs_[i]->solref, m->pair_solref+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solreffriction, m->pair_solreffriction+mjNREF*i, mjNREF); + mjuu_copyvec(pairs_[i]->solimp, m->pair_solimp+mjNIMP*i, mjNIMP); pairs_[i]->margin = (double)m->pair_margin[i]; pairs_[i]->gap = (double)m->pair_gap[i]; - copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); + mjuu_copyvec(pairs_[i]->friction, m->pair_friction+5*i, 5); } // equality constraints for (int i=0; idata, m->eq_data+mjNEQDATA*i, mjNEQDATA); - copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); - copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); + mjuu_copyvec(equalities_[i]->data, m->eq_data+mjNEQDATA*i, mjNEQDATA); + mjuu_copyvec(equalities_[i]->solref, m->eq_solref+mjNREF*i, mjNREF); + mjuu_copyvec(equalities_[i]->solimp, m->eq_solimp+mjNIMP*i, mjNIMP); } // tendons for (int i=0; irange, m->tendon_range+2*i, 2); - copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); - copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); - copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); + mjuu_copyvec(tendons_[i]->range, m->tendon_range+2*i, 2); + mjuu_copyvec(tendons_[i]->solref_limit, m->tendon_solref_lim+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_limit, m->tendon_solimp_lim+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->solref_friction, m->tendon_solref_fri+mjNREF*i, mjNREF); + mjuu_copyvec(tendons_[i]->solimp_friction, m->tendon_solimp_fri+mjNIMP*i, mjNIMP); + mjuu_copyvec(tendons_[i]->rgba, m->tendon_rgba+4*i, 4); tendons_[i]->width = (double)m->tendon_width[i]; tendons_[i]->margin = (double)m->tendon_margin[i]; tendons_[i]->stiffness = (double)m->tendon_stiffness[i]; @@ -3847,7 +3915,7 @@ bool mjCModel::CopyBack(const mjModel* m) { tendons_[i]->frictionloss = (double)m->tendon_frictionloss[i]; if (nuser_tendon) { - copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); + mjuu_copyvec(tendons_[i]->userdata_.data(), m->tendon_user + nuser_tendon*i, nuser_tendon); } } @@ -3856,18 +3924,18 @@ bool mjCModel::CopyBack(const mjModel* m) { for (int i=0; idynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); - copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); - copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); - copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); - copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); - copyvec(pa->actrange, m->actuator_actrange+2*i, 2); - copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); - copyvec(pa->gear, m->actuator_gear+6*i, 6); + mjuu_copyvec(pa->dynprm, m->actuator_dynprm+i*mjNDYN, mjNDYN); + mjuu_copyvec(pa->gainprm, m->actuator_gainprm+i*mjNGAIN, mjNGAIN); + mjuu_copyvec(pa->biasprm, m->actuator_biasprm+i*mjNBIAS, mjNBIAS); + mjuu_copyvec(pa->ctrlrange, m->actuator_ctrlrange+2*i, 2); + mjuu_copyvec(pa->forcerange, m->actuator_forcerange+2*i, 2); + mjuu_copyvec(pa->actrange, m->actuator_actrange+2*i, 2); + mjuu_copyvec(pa->lengthrange, m->actuator_lengthrange+2*i, 2); + mjuu_copyvec(pa->gear, m->actuator_gear+6*i, 6); pa->cranklength = (double)m->actuator_cranklength[i]; if (nuser_actuator) { - copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); + mjuu_copyvec(pa->userdata_.data(), m->actuator_user + nuser_actuator*i, nuser_actuator); } } @@ -3877,7 +3945,7 @@ bool mjCModel::CopyBack(const mjModel* m) { sensors_[i]->noise = (double)m->sensor_noise[i]; if (nuser_sensor) { - copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); + mjuu_copyvec(sensors_[i]->userdata_.data(), m->sensor_user + nuser_sensor*i, nuser_sensor); } } @@ -3900,17 +3968,17 @@ bool mjCModel::CopyBack(const mjModel* m) { mjCKey* pk = keys_[i]; pk->time = (double)m->key_time[i]; - copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); - copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); + mjuu_copyvec(pk->qpos_.data(), m->key_qpos + i*nq, nq); + mjuu_copyvec(pk->qvel_.data(), m->key_qvel + i*nv, nv); if (na) { - copyvec(pk->act_.data(), m->key_act + i*na, na); + mjuu_copyvec(pk->act_.data(), m->key_act + i*na, na); } if (nmocap) { - copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); - copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); + mjuu_copyvec(pk->mpos_.data(), m->key_mpos + i*3*nmocap, 3*nmocap); + mjuu_copyvec(pk->mquat_.data(), m->key_mquat + i*4*nmocap, 4*nmocap); } if (nu) { - copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); + mjuu_copyvec(pk->ctrl_.data(), m->key_ctrl + i*nu, nu); } } diff --git a/src/user/user_model.h b/src/user/user_model.h index 5d2ee204..06a52b8f 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -203,17 +203,18 @@ class mjCModel : public mjCModel_, private mjSpec { // API for access to model elements (outside tree) int NumObjects(mjtObj type); // number of objects in specified list mjCBase* GetObject(mjtObj type, int id); // pointer to specified object + mjsElement* NextObject(mjsElement* object, mjtObj type = mjOBJ_UNKNOWN); // next object of specified type // API for access to other variables - bool IsCompiled() const; // is model already compiled - const mjCError& GetError() const; // get reference of error object - mjCBody* GetWorld(); // pointer to world body - mjCDef* FindDefault(std::string name); // find defaults class name - mjCDef* AddDefault(std::string name, int parentid); // add defaults class to array - mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name - mjCBody* FindBody(mjCBody* body, std::string name); // find body given name - mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name - bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose + bool IsCompiled() const; // is model already compiled + const mjCError& GetError() const; // get reference of error object + mjCBody* GetWorld(); // pointer to world body + mjCDef* FindDefault(std::string name); // find defaults class name + mjCDef* AddDefault(std::string name, mjCDef* parent = nullptr); // add defaults class to array + mjCBase* FindObject(mjtObj type, std::string name) const; // find object given type and name + mjCBody* FindBody(mjCBody* body, std::string name); // find body given name + mjCFrame* FindFrame(mjCBody* body, std::string name) const; // find frame given name + bool IsNullPose(const mjtNum* pos, const mjtNum* quat) const; // detect null pose void SetActivePlugins(const std::vector>&& active_plugins) { active_plugins_ = std::move(active_plugins); } diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 223d951e..6cbfa6e4 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -19,11 +19,11 @@ #include #include #include -#include #include #include #include #include +#include #include #include #include @@ -37,14 +37,7 @@ #include #include "cc/array_safety.h" #include "engine/engine_resource.h" -#include "engine/engine_io.h" #include "engine/engine_passive.h" -#include "engine/engine_plugin.h" -#include "engine/engine_util_blas.h" -#include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_solve.h" -#include "engine/engine_util_spatial.h" #include #include "user/user_api.h" #include "user/user_cache.h" @@ -98,42 +91,6 @@ static bool islimited(int limited, const double range[2]) { return false; } -// compute frame quat and diagonal inertia from full inertia matrix, return error if any -const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { - if (!mjuu_defined(fullinertia[0])) { - return nullptr; - } - - mjtNum eigval[3], eigvec[9], quattmp[4]; - mjtNum full[9] = { - fullinertia[0], fullinertia[3], fullinertia[4], - fullinertia[3], fullinertia[1], fullinertia[5], - fullinertia[4], fullinertia[5], fullinertia[2] - }; - - mju_eig3(eigval, eigvec, quattmp, full); - - // check mimimal eigenvalue - if (eigval[2] elements; elements.reserve(bvleaf_.size()); - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; for (int i = 0; i < bvleaf_.size(); i++) { if (bvleaf_[i].conaffinity || bvleaf_[i].contype) { BVElement element; element.e = &bvleaf_[i]; - mjtNum vert[3] = {element.e->pos[0] - ipos_[0], + double vert[3] = {element.e->pos[0] - ipos_[0], element.e->pos[1] - ipos_[1], element.e->pos[2] - ipos_[2]}; - mju_rotVecQuat(element.lpos, vert, qinv); + mjuu_rotVecQuat(element.lpos, vert, qinv); elements.push_back(std::move(element)); } } @@ -448,15 +405,15 @@ int mjCBoundingVolumeHierarchy::MakeBVH( if (nelements == 0) { return -1; } - mjtNum AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; + double AAMM[6] = {mjMAXVAL, mjMAXVAL, mjMAXVAL, -mjMAXVAL, -mjMAXVAL, -mjMAXVAL}; // inverse transformation - mjtNum qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; + double qinv[4] = {iquat_[0], -iquat_[1], -iquat_[2], -iquat_[3]}; // accumulate AAMM over elements for (auto element = elements_begin; element != elements_end; ++element) { // transform element aabb to aamm format - mjtNum aamm[6] = {element->e->aabb[0] - element->e->aabb[3], + double aamm[6] = {element->e->aabb[0] - element->e->aabb[3], element->e->aabb[1] - element->e->aabb[4], element->e->aabb[2] - element->e->aabb[5], element->e->aabb[0] + element->e->aabb[3], @@ -465,32 +422,32 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // update node AAMM for (int v=0; v<8; v++) { - mjtNum vert[3], box[3]; + double vert[3], box[3]; vert[0] = (v&1 ? aamm[3] : aamm[0]); vert[1] = (v&2 ? aamm[4] : aamm[1]); vert[2] = (v&4 ? aamm[5] : aamm[2]); // rotate to the body inertial frame if specified if (element->e->quat) { - mju_rotVecQuat(box, vert, element->e->quat); + mjuu_rotVecQuat(box, vert, element->e->quat); box[0] += element->e->pos[0] - ipos_[0]; box[1] += element->e->pos[1] - ipos_[1]; box[2] += element->e->pos[2] - ipos_[2]; - mju_rotVecQuat(vert, box, qinv); + mjuu_rotVecQuat(vert, box, qinv); } - 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]); + AAMM[0] = std::min(AAMM[0], vert[0]); + AAMM[1] = std::min(AAMM[1], vert[1]); + AAMM[2] = std::min(AAMM[2], vert[2]); + AAMM[3] = std::max(AAMM[3], vert[0]); + AAMM[4] = std::max(AAMM[4], vert[1]); + AAMM[5] = std::max(AAMM[5], vert[2]); } } // inflate flat AABBs for (int i=0; i<3; i++) { - if (mju_abs(AAMM[i]-AAMM[i+3])= edges[0] + mjEPS) axis = 1; if (edges[2] >= edges[axis] + mjEPS) axis = 2; @@ -560,6 +517,7 @@ int mjCBoundingVolumeHierarchy::MakeBVH( // constructor mjCDef::mjCDef() { name.clear(); + id = 0; parentid = -1; childid.clear(); mjs_defaultJoint(&joint_.spec); @@ -902,6 +860,9 @@ mjCBody& mjCBody::operator+=(const mjCFrame& other) { // attach referencing elements *model += *other.model; + // (b/350784262) delete keyframes + model->DeleteAll(model->keys_); + // clear namespace and return body other.model->prefix.clear(); other.model->suffix.clear(); @@ -1340,7 +1301,7 @@ void mjCBody::GeomFrame(void) { } // check for small mass - if (massdegree, model->euler, alt); + const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); if (err) { throw mjCError(this, "error '%s' in frame alternative", err); } // check and process orientation alternatives for inertia - const char* ierr = FullInertia(iquat, inertia, this->fullinertia); + const char* ierr = mjuu_fullInertia(iquat, inertia, this->fullinertia); if (ierr) { throw mjCError(this, "error '%s' in inertia alternative", ierr); } @@ -1476,10 +1437,10 @@ void mjCBody::Compile(void) { // check and correct mass and inertia if (id>0) { // fix minimum - mass = mju_max(mass, model->boundmass); - inertia[0] = mju_max(inertia[0], model->boundinertia); - inertia[1] = mju_max(inertia[1], model->boundinertia); - inertia[2] = mju_max(inertia[2], model->boundinertia); + mass = std::max(mass, model->boundmass); + inertia[0] = std::max(inertia[0], model->boundinertia); + inertia[1] = std::max(inertia[1], model->boundinertia); + inertia[2] = std::max(inertia[2], model->boundinertia); // check for negative values if (mass<0 || inertia[0]<0 || inertia[1]<0 ||inertia[2]<0) { @@ -1509,7 +1470,7 @@ void mjCBody::Compile(void) { for (int i=0; icontype; conaffinity |= geoms[i]->conaffinity; - margin = mju_max(margin, geoms[i]->margin); + margin = std::max(margin, geoms[i]->margin); } // compute bounding volume hierarchy @@ -1546,9 +1507,9 @@ void mjCBody::Compile(void) { // compute body global pose (no joint transformations in qpos0) if (id>0) { mjCBody* par = model->Bodies()[parentid]; - mju_rotVecQuat(xpos0, pos, par->xquat0); - mju_addTo3(xpos0, par->xpos0); - mju_mulQuat(xquat0, par->xquat0, quat); + mjuu_rotVecQuat(xpos0, pos, par->xquat0); + mjuu_addtovec(xpos0, par->xpos0, 3); + mjuu_mulquat(xquat0, par->xquat0, quat); } // compile all sites @@ -1640,6 +1601,9 @@ mjCFrame& mjCFrame::operator+=(const mjCBody& other) { // attach referencing elements *model += *other.model; + // (b/350784262) delete keyframes + model->DeleteAll(model->keys_); + // clear suffixes and return other.model->suffix.clear(); other.model->prefix.clear(); @@ -1680,8 +1644,8 @@ void mjCFrame::PointToLocal() { void mjCFrame::CopyFromSpec() { *static_cast(this) = spec; - mju_copy3(pos, spec.pos); - mju_copy4(quat, spec.quat); + mjuu_copyvec(pos, spec.pos, 3); + mjuu_copyvec(quat, spec.quat, 4); } @@ -1692,7 +1656,7 @@ void mjCFrame::Compile() { } CopyFromSpec(); - const char* err = ResolveOrientation(quat, model->spec.degree, model->spec.euler, alt); + const char* err = ResolveOrientation(quat, model->spec.degree, model->spec.eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -1728,6 +1692,7 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -1768,7 +1733,6 @@ bool mjCJoint::is_actfrclimited() const { return islimited(actfrclimited, actfrc void mjCJoint::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.userdata = &spec_userdata_; spec.info = &info; } @@ -1936,6 +1900,7 @@ mjCGeom::mjCGeom(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -1969,7 +1934,6 @@ void mjCGeom::PointToLocal(void) { spec.element = static_cast(this); spec.name = &name; spec.info = &info; - spec.classname = &classname; spec.userdata = &spec_userdata_; spec.material = &spec_material_; spec.meshname = &spec_meshname_; @@ -2147,7 +2111,7 @@ double mjCGeom::GetRBound(void) { case mjGEOM_HFIELD: hsize = hfield->size; return sqrt(hsize[0]*hsize[0] + hsize[1]*hsize[1] + - mjMAX(hsize[2]*hsize[2], hsize[3]*hsize[3])); + std::max(hsize[2]*hsize[2], hsize[3]*hsize[3])); case mjGEOM_SPHERE: return size[0]; @@ -2159,7 +2123,7 @@ double mjCGeom::GetRBound(void) { return sqrt(size[0]*size[0]+size[1]*size[1]); case mjGEOM_ELLIPSOID: - return mju_max(mju_max(size[0], size[1]), size[2]); + return std::max(std::max(size[0], size[1]), size[2]); case mjGEOM_BOX: return sqrt(size[0]*size[0]+size[1]*size[1]+size[2]*size[2]); @@ -2167,9 +2131,9 @@ double mjCGeom::GetRBound(void) { case mjGEOM_MESH: case mjGEOM_SDF: aamm = mesh->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])); + haabb[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + haabb[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + haabb[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); return sqrt(haabb[0]*haabb[0] + haabb[1]*haabb[1] + haabb[2]*haabb[2]); default: @@ -2270,18 +2234,21 @@ void mjCGeom::SetFluidCoefs(void) { // coefficients of virtual moment of inertia. Note: if (kz-ky) in numerator // is negative, also the denom is negative. Abs both and clip to MINVAL const auto pow2 = [](const double val) { return val * val; }; - const double Ixfac = pow2(dy*dy - dz*dz) * std::fabs(kz - ky) / std::max( - mjMINVAL, std::fabs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); - const double Iyfac = pow2(dz*dz - dx*dx) * std::fabs(kx - kz) / std::max( - mjMINVAL, std::fabs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); - const double Izfac = pow2(dx*dx - dy*dy) * std::fabs(ky - kx) / std::max( - mjMINVAL, std::fabs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); + const double Ixfac = pow2(dy*dy - dz*dz) * std::abs(kz - ky) / std::max( + mjEPS, std::abs(2*(dy*dy - dz*dz) + (dy*dy + dz*dz)*(ky - kz))); + const double Iyfac = pow2(dz*dz - dx*dx) * std::abs(kx - kz) / std::max( + mjEPS, std::abs(2*(dz*dz - dx*dx) + (dz*dz + dx*dx)*(kz - kx))); + const double Izfac = pow2(dx*dx - dy*dy) * std::abs(ky - kx) / std::max( + mjEPS, std::abs(2*(dx*dx - dy*dy) + (dx*dx + dy*dy)*(kx - ky))); - const mjtNum virtual_mass[3] = { - volume * kx / std::max(mjMINVAL, 2-kx), - volume * ky / std::max(mjMINVAL, 2-ky), - volume * kz / std::max(mjMINVAL, 2-kz)}; - const mjtNum virtual_inertia[3] = {volume*Ixfac/5, volume*Iyfac/5, volume*Izfac/5}; + mjtNum virtual_mass[3]; + virtual_mass[0] = volume * kx / std::max(mjEPS, 2-kx); + virtual_mass[1] = volume * ky / std::max(mjEPS, 2-ky); + virtual_mass[2] = volume * kz / std::max(mjEPS, 2-kz); + mjtNum virtual_inertia[3]; + virtual_inertia[0] = volume*Ixfac/5; + virtual_inertia[1] = volume*Iyfac/5; + virtual_inertia[2] = volume*Izfac/5; writeFluidGeomInteraction(fluid, &fluid_ellipsoid, &fluid_coefs[0], &fluid_coefs[1], &fluid_coefs[2], @@ -2433,7 +2400,7 @@ void mjCGeom::Compile(void) { // not 'fromto': try alternative else { - const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); + const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in geom %d", err, id); } @@ -2477,9 +2444,9 @@ void mjCGeom::Compile(void) { size[2] = 0.25 * hfield->size[2] + 0.5 * hfield->size[3]; } else if (type==mjGEOM_MESH || type==mjGEOM_SDF) { const double* aamm = mesh->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])); + size[0] = std::max(std::abs(aamm[0]), std::abs(aamm[3])); + size[1] = std::max(std::abs(aamm[1]), std::abs(aamm[4])); + size[2] = std::max(std::abs(aamm[2]), std::abs(aamm[5])); } for (double s : size) { @@ -2497,7 +2464,7 @@ void mjCGeom::Compile(void) { if (mass==0) { mass_ = 0; density = 0; - } else if (GetVolume()>mjMINVAL) { + } else if (GetVolume()>mjEPS) { mass_ = mass; density = mass / GetVolume(); SetInertia(); @@ -2576,6 +2543,7 @@ mjCSite::mjCSite(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; } @@ -2602,7 +2570,6 @@ void mjCSite::PointToLocal() { spec.element = static_cast(this); spec.name = &name; spec.info = &info; - spec.classname = &classname; spec.material = &spec_material_; spec.userdata = &spec_userdata_; } @@ -2682,7 +2649,7 @@ void mjCSite::Compile(void) { // alternative orientation else { - const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); + const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in site %d", err, id); } @@ -2722,6 +2689,7 @@ mjCCamera::mjCCamera(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -2753,7 +2721,6 @@ mjCCamera& mjCCamera::operator=(const mjCCamera& other) { void mjCCamera::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.userdata = &spec_userdata_; spec.targetbody = &spec_targetbody_; spec.info = &info; @@ -2793,7 +2760,7 @@ void mjCCamera::Compile(void) { userdata_.resize(model->nuser_cam); // process orientation specifications - const char* err = ResolveOrientation(quat, model->degree, model->euler, alt); + const char* err = ResolveOrientation(quat, model->degree, model->eulerseq, alt); if (err) { throw mjCError(this, "orientation specification error '%s' in camera %d", err, id); } @@ -2847,7 +2814,7 @@ void mjCCamera::Compile(void) { intrinsic[3] = principal_pixel[1] / pixel_density[1] + principal_length[1]; // fovy with principal point at (0, 0) - fovy = mju_atan2((float)sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; + fovy = std::atan2(sensor_size[1]/2, intrinsic[1]) * 360.0 / mjPI; } else { intrinsic[0] = model->visual.map.znear; intrinsic[1] = model->visual.map.znear; @@ -2876,6 +2843,7 @@ mjCLight::mjCLight(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; PointToLocal(); CopyFromSpec(); @@ -2904,7 +2872,6 @@ mjCLight& mjCLight::operator=(const mjCLight& other) { void mjCLight::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.targetbody = &spec_targetbody_; spec.info = &info; } @@ -2942,7 +2909,7 @@ void mjCLight::Compile(void) { } // normalize direction, make sure it is not zero - if (mjuu_normvec(dir, 3)modelfiledir_, model->meshdir_, file_); + string filename = mjuu_combinePaths(model->modelfiledir_, model->meshdir_, file_); mjResource* resource = LoadResource(filename, vfs); try { @@ -3168,15 +3135,15 @@ void mjCHField::Compile(const mjVFS* vfs) { // set elevation data to [0-1] range float emin = 1E+10, emax = -1E+10; for (int i = 0; iemax) { throw mjCError(this, "invalid data range in hfield '%s'", file_.c_str()); } for (int i=0; imjMINVAL) { + if (emax-emin>mjEPS) { data[i] /= (emax - emin); } } @@ -3233,7 +3200,6 @@ mjCTexture& mjCTexture::operator=(const mjCTexture& other) { void mjCTexture::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.file = &spec_file_; spec.content_type = &spec_content_type_; spec.cubefiles = &spec_cubefiles_; @@ -3267,9 +3233,15 @@ mjCTexture::~mjCTexture() { // insert random dots static void randomdot(unsigned char* rgb, const double* markrgb, int width, int height, double probability) { + // make distribution using fixed seed + std::mt19937_64 rng; + rng.seed(42); + std::uniform_real_distribution dist(0, 1); + + // sample for (int r=0; rmodelfiledir_, model->texturedir_, cubefiles_[i]); + string filename = mjuu_combinePaths(model->modelfiledir_, model->texturedir_, cubefiles_[i]); // load PNG or custom unsigned int w, h; @@ -3819,14 +3791,18 @@ void mjCTexture::Compile(const mjVFS* vfs) { // builtin if (builtin != mjBUILTIN_NONE) { - // check size - if (width<1 || height<1) { - throw mjCError(this, "Invalid width or height of builtin texture"); + // check width + if (width<1) { + throw mjCError(this, "Invalid width of builtin texture"); } // adjust height of cube texture if (type != mjTEXTURE_2D) { height = 6*width; + } else { + if (height<1) { + throw mjCError(this, "Invalid height of builtin texture"); + } } // allocate data @@ -3851,7 +3827,7 @@ void mjCTexture::Compile(const mjVFS* vfs) { } // make filename - string filename = mjuu_makefullname(model->modelfiledir_, model->texturedir_, file_); + string filename = mjuu_combinePaths(model->modelfiledir_, model->texturedir_, file_); // dispatch if (type==mjTEXTURE_2D) { @@ -3912,6 +3888,7 @@ mjCMaterial::mjCMaterial(mjCModel* _model, mjCDef* _def) { model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; PointToLocal(); @@ -3942,7 +3919,6 @@ mjCMaterial& mjCMaterial::operator=(const mjCMaterial& other) { void mjCMaterial::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.texture = &spec_texture_; spec.info = &info; } @@ -3999,6 +3975,7 @@ mjCPair::mjCPair(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -4032,7 +4009,6 @@ mjCPair& mjCPair::operator=(const mjCPair& other) { void mjCPair::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.geomname1 = &spec_geomname1_; spec.geomname2 = &spec_geomname2_; spec.info = &info; @@ -4122,12 +4098,12 @@ void mjCPair::Compile(void) { // set undefined margin: max if (!mjuu_defined(margin)) { - margin = mjMAX(geom1->margin, geom2->margin); + margin = std::max(geom1->margin, geom2->margin); } // set undefined gap: max if (!mjuu_defined(gap)) { - gap = mjMAX(geom1->gap, geom2->gap); + gap = std::max(geom1->gap, geom2->gap); } // set undefined condim, friction, solref, solimp: different priority @@ -4165,23 +4141,23 @@ void mjCPair::Compile(void) { else { // condim: max if (condim<0) { - condim = mjMAX(geom1->condim, geom2->condim); + condim = std::max(geom1->condim, geom2->condim); } // friction: max if (!mjuu_defined(friction[0])) { - friction[0] = friction[1] = mju_max(geom1->friction[0], geom2->friction[0]); - friction[2] = mju_max(geom1->friction[1], geom2->friction[1]); - friction[3] = friction[4] = mju_max(geom1->friction[2], geom2->friction[2]); + friction[0] = friction[1] = std::max(geom1->friction[0], geom2->friction[0]); + friction[2] = std::max(geom1->friction[1], geom2->friction[1]); + friction[3] = friction[4] = std::max(geom1->friction[2], geom2->friction[2]); } // solver mix factor double mix; - if (geom1->solmix>=mjMINVAL && geom2->solmix>=mjMINVAL) { + if (geom1->solmix>=mjEPS && geom2->solmix>=mjEPS) { mix = geom1->solmix / (geom1->solmix + geom2->solmix); - } else if (geom1->solmixsolmixsolmixsolmixsolmixsolmixsolref[i], geom2->solref[i]); + solref[i] = std::min(geom1->solref[i], geom2->solref[i]); } } } @@ -4360,6 +4336,7 @@ mjCEquality::mjCEquality(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -4391,7 +4368,6 @@ mjCEquality& mjCEquality::operator=(const mjCEquality& other) { void mjCEquality::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.name1 = &spec_name1_; spec.name2 = &spec_name2_; spec.info = &info; @@ -4522,6 +4498,7 @@ mjCTendon::mjCTendon(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // point to local PointToLocal(); @@ -4560,7 +4537,6 @@ bool mjCTendon::is_limited() const { return islimited(limited, range); } void mjCTendon::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.material = &spec_material_; spec.userdata = &spec_userdata_; spec.info = &info; @@ -5000,6 +4976,7 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { // set model, def model = _model; def = (_def ? _def : (_model ? _model->Defaults(0) : 0)); + classname = def ? def->name : ""; // in case this actuator is not compiled CopyFromSpec(); @@ -5041,7 +5018,6 @@ bool mjCActuator::is_actlimited() const { return islimited(actlimited, actrange) void mjCActuator::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.userdata = &spec_userdata_; spec.target = &spec_target_; spec.refsite = &spec_refsite_; @@ -5364,7 +5340,6 @@ mjCSensor& mjCSensor::operator=(const mjCSensor& other) { void mjCSensor::PointToLocal() { spec.element = static_cast(this); spec.name = &name; - spec.classname = &classname; spec.userdata = &spec_userdata_; spec.objname = &spec_objname_; spec.refname = &spec_refname_; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 2135421d..e6545e57 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -86,8 +86,6 @@ const char* ResolveOrientation(double* quat, // set frame quat const char* sequence, // euler sequence format: "xyz" const mjsOrientation& orient); -// compute frame quat and diagonal inertia from full inertia matrix, return error if any -const char* FullInertia(double quat[4], double inertia[3], const double fullinertia[6]); //------------------------- class mjCBoundingVolumeHierarchy --------------------------------------- @@ -98,9 +96,9 @@ class mjCBoundingVolume { int contype; // contact type int conaffinity; // contact affinity - 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) + const double* aabb; // axis-aligned bounding box (center, size) + const double* pos; // position (set by user or Compile1) + const double* quat; // orientation (set by user or Compile1) const int* GetId() const { if (id_) return id_; else return &idval_; } void SetId(const int* id) { id_ = id; } @@ -134,7 +132,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { // make bounding volume hierarchy void CreateBVH(void); - void Set(mjtNum ipos_element[3], mjtNum iquat_element[4]); + void Set(double ipos_element[3], double iquat_element[4]); void AllocateBoundingVolumes(int nleaf); void RemoveInactiveVolumes(int nmax); mjCBoundingVolume* GetBoundingVolume(int id); @@ -144,7 +142,7 @@ class mjCBoundingVolumeHierarchy : public mjCBoundingVolumeHierarchy_ { struct BVElement { const mjCBoundingVolume* e; // position of the element in the BVH axes - mjtNum lpos[3]; + double lpos[3]; }; struct BVElementCompare { @@ -231,8 +229,8 @@ class mjCBody_ : public mjCBase { 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 + double xpos0[3]; // global position in qpos0 + double xquat0[4]; // global orientation in qpos0 // used internally by compiler int lastdof; // id of last dof @@ -302,7 +300,6 @@ class mjCBody : public mjCBody_, private mjsBody { // inherited using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -364,7 +361,6 @@ class mjCFrame : public mjCFrame_, private mjsFrame { mjsFrame spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); @@ -411,7 +407,6 @@ class mjCJoint : public mjCJoint_, private mjsJoint { mjsJoint spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); @@ -512,7 +507,6 @@ class mjCGeom : public mjCGeom_, private mjsGeom { void NameSpace(const mjCModel* m); // inherited - using mjCBase::classname; using mjCBase::info; }; @@ -550,7 +544,6 @@ class mjCSite : public mjCSite_, private mjsSite { // use strings from mjCBase rather than mjStrings from mjsSite using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -593,7 +586,6 @@ class mjCCamera : public mjCCamera_, private mjsCamera { mjsCamera spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -633,7 +625,6 @@ class mjCLight : public mjCLight_, private mjsLight { mjsLight spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -664,20 +655,20 @@ class mjCFlex_ : public mjCBase { 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 + std::vector vertxpos; // global vertex positions mjCBoundingVolumeHierarchy tree; // bounding volume hierarchy - std::vector elemaabb_; // element bounding volume + std::vector elemaabb_; // element bounding volume // variable-size data std::vector vertbody_; // vertex body names - std::vector vert_; // vertex positions + std::vector vert_; // vertex positions std::vector elem_; // element vertex ids std::vector texcoord_; // vertex texture coordinates std::string material_; // name of material used for rendering std::string spec_material_; std::vector spec_vertbody_; - std::vector spec_vert_; + std::vector spec_vert_; std::vector spec_elem_; std::vector spec_texcoord_; }; @@ -696,7 +687,6 @@ class mjCFlex: public mjCFlex_, private mjsFlex { mjsFlex spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); @@ -796,7 +786,6 @@ class mjCMesh: public mjCMesh_, private mjsMesh { mjsMesh spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); @@ -927,7 +916,6 @@ class mjCSkin: public mjCSkin_, private mjsSkin { mjsSkin spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; const std::string& get_file() const { return file_; } @@ -1029,7 +1017,6 @@ class mjCTexture : public mjCTexture_, private mjsTexture { mjsTexture spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(void); @@ -1084,7 +1071,6 @@ class mjCMaterial : public mjCMaterial_, private mjsMaterial { mjsMaterial spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(); @@ -1125,7 +1111,6 @@ class mjCPair : public mjCPair_, private mjsPair { mjsPair spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(); @@ -1222,7 +1207,6 @@ class mjCEquality : public mjCEquality_, private mjsEquality { mjsEquality spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void CopyFromSpec(); @@ -1263,7 +1247,6 @@ class mjCTendon : public mjCTendon_, private mjsTendon { mjsTendon spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; void set_material(std::string _material) { material_ = _material; } @@ -1396,7 +1379,6 @@ class mjCActuator : public mjCActuator_, private mjsActuator { mjsActuator spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -1455,7 +1437,6 @@ class mjCSensor : public mjCSensor_, private mjsSensor { mjsSensor spec; using mjCBase::name; - using mjCBase::classname; using mjCBase::info; // used by mjXWriter and mjCModel @@ -1653,6 +1634,7 @@ class mjCDef : public mjsElement { // identifiers std::string name; // class name + int id; // id of this default int parentid; // id of parent class std::vector childid; // ids of child classes diff --git a/src/user/user_util.cc b/src/user/user_util.cc index a2a91ee9..ed3a7158 100644 --- a/src/user/user_util.cc +++ b/src/user/user_util.cc @@ -17,30 +17,21 @@ #include #include #include -#include #include #include #include -#include #include #include "engine/engine_crossplatform.h" -#include "engine/engine_util_misc.h" -#include "engine/engine_util_spatial.h" - -using std::isnan; -using std::string; -using std::numeric_limits; - // check if numeric variable is defined -bool mjuu_defined(const double num) { - return !isnan(num); +bool mjuu_defined(double num) { + return !std::isnan(num); } // compute address of M[g1][g2] where M is triangular n-by-n -int mjuu_matadr(int g1, int g2, const int n) { +int mjuu_matadr(int g1, int g2, int n) { if (g1<0 || g2<0 || g1>=n || g2>=n) { return -1; } @@ -56,13 +47,13 @@ int mjuu_matadr(int g1, int g2, const int n) { // set 4D vector -void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w) { +void mjuu_setvec(double* dest, double x, double y, double z, double w) { dest[0] = x; dest[1] = y; dest[2] = z; dest[3] = w; } -void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w) { +void mjuu_setvec(float* dest, double x, double y, double z, double w) { dest[0] = (float)x; dest[1] = (float)y; dest[2] = (float)z; @@ -71,12 +62,12 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z, co // set 3D vector -void mjuu_setvec(double* dest, const double x, const double y, const double z) { +void mjuu_setvec(double* dest, double x, double y, double z) { dest[0] = x; dest[1] = y; dest[2] = z; } -void mjuu_setvec(float* dest, const double x, const double y, const double z) { +void mjuu_setvec(float* dest, double x, double y, double z) { dest[0] = (float)x; dest[1] = (float)y; dest[2] = (float)z; @@ -84,35 +75,32 @@ void mjuu_setvec(float* dest, const double x, const double y, const double z) { // set 2D vector -void mjuu_setvec(double* dest, const double x, const double y) { +void mjuu_setvec(double* dest, double x, double y) { dest[0] = x; dest[1] = y; } -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n) { +// add to double array +void mjuu_addtovec(double* dest, const double* src, int n) { for (int i=0; i mjEPS) { + for (int i=0; i mjEPS) { + for (int i=0; i std::abs(D[2]) && std::abs(D[1]) > std::abs(D[5])) { + rk = 0; // row + ck = 1; // column + rotk = 2; // rotation axis + } else if (std::abs(D[2]) > std::abs(D[5])) { + rk = 0; + ck = 2; + rotk = 1; + } else { + rk = 1; + ck = 2; + rotk = 0; + } + + // terminate if max off-diagonal element too small + if (std::abs(D[3*rk+ck]) < kEigEPS) { + break; + } + + // 2x2 symmetric Schur decomposition + tau = (D[4*ck]-D[4*rk])/(2*D[3*rk+ck]); + if (tau >= 0) { + t = 1.0/(tau + sqrt(1 + tau*tau)); + } else { + t = -1.0/(-tau + sqrt(1 + tau*tau)); + } + c = 1.0/sqrt(1 + t*t); + + // terminate if cosine too close to 1 + if (c > 1.0-kEigEPS) { + break; + } + + // express rotation as quaternion + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[rotk+1] = (tau >= 0 ? -sqrt(0.5-0.5*c) : sqrt(0.5-0.5*c)); + if (rotk == 1) { + tmp[rotk+1] = -tmp[rotk+1]; + } + tmp[0] = sqrt(1.0 - tmp[rotk+1]*tmp[rotk+1]); + mjuu_normvec(tmp, 4); + + // accumulate quaternion rotation + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + + // sort eigenvalues in decreasing order (bubblesort: 0, 1, 0) + for (int j=0; j < 3; j++) { + int j1 = j%2; // lead index + + // only swap if the eigenvalues are different + if (eigval[j1]+kEigEPS < eigval[j1+1]) { + // swap eigenvalues + t = eigval[j1]; + eigval[j1] = eigval[j1+1]; + eigval[j1+1] = t; + + // rotate quaternion + tmp[0] = 0.707106781186548; // cos(pi/4) = sin(pi/4) + tmp[1] = tmp[2] = tmp[3] = 0; + tmp[(j1+2)%3+1] = tmp[0]; + mjuu_mulquat(quat, quat, tmp); + mjuu_normvec(quat, 4); + } + } + + // recompute eigvec + mjuu_quat2mat(eigvec, quat); + + return iter; +} + +// transform vector by pose +void mjuu_trnVecPose(double res[3], const double pos[3], const double quat[4], + const double vec[3]) { + // res = quat*vec + pos + mjuu_rotVecQuat(res, vec, quat); + res[0] += pos[0]; + res[1] += pos[1]; + res[2] += pos[2]; +} + // strip directory from filename -string mjuu_strippath(string filename) { +std::string mjuu_strippath(std::string filename) { // find last pathsymbol size_t start = filename.find_last_of("/\\"); // no path found: return original - if (start==string::npos) { + if (start==std::string::npos) { return filename; } @@ -542,13 +744,46 @@ string mjuu_strippath(string filename) { } +// compute frame quat and diagonal inertia from full inertia matrix, return error if any +const char* mjuu_fullInertia(double quat[4], double inertia[3], const double fullinertia[6]) { + if (!mjuu_defined(fullinertia[0])) { + return nullptr; + } + + double eigval[3], eigvec[9], quattmp[4]; + double full[9] = { + fullinertia[0], fullinertia[3], fullinertia[4], + fullinertia[3], fullinertia[1], fullinertia[5], + fullinertia[4], fullinertia[5], fullinertia[2] + }; + + mjuu_eig3(eigval, eigvec, quattmp, full); + + // check mimimal eigenvalue + if (eigval[2] 0 && path1[n - 1] != '\\' && path1[n - 1] != '/') { + return path1 + "/" + path2; } - - // default - return filedir + meshdir + filename; + return path1 + path2; } + +// assemble three file paths +std::string mjuu_combinePaths(const std::string& path1, const std::string& path2, + const std::string& path3) { + return mjuu_combinePaths(path1, mjuu_combinePaths(path2, path3)); +} + + + // return true if the text is in a valid content type format: // {type}/{subtype}[;{parameter}={value}] static bool mjuu_isValidContentType(std::string_view text) { diff --git a/src/user/user_util.h b/src/user/user_util.h index 29ff5dd8..38932964 100644 --- a/src/user/user_util.h +++ b/src/user/user_util.h @@ -20,40 +20,48 @@ #include -const double mjEPS = 1E-14; // minimum value in various calculations -const double mjMINMASS = 1E-6; // minimum mass allowed +const double mjEPS = 1E-14; // minimum value in various calculations +const double mjMINMASS = 1E-6; // minimum mass allowed // check if numeric variable is defined: !_isnan(num) -bool mjuu_defined(const double num); +bool mjuu_defined(double num); // compute linear address of M[g1][g2] where M is triangular n-by-n // return -1 if inputs are invalid -int mjuu_matadr(int g1, int g2, const int n); +int mjuu_matadr(int g1, int g2, int n); // set 4D vector -void mjuu_setvec(double* dest, const double x, const double y, const double z, const double w); -void mjuu_setvec(float* dest, const double x, const double y, const double z, const double w); +void mjuu_setvec(double* dest, double x, double y, double z, double w); +void mjuu_setvec(float* dest, double x, double y, double z, double w); // set 3D vector -void mjuu_setvec(double* dest, const double x, const double y, const double z); -void mjuu_setvec(float* dest, const double x, const double y, const double z); +void mjuu_setvec(double* dest, double x, double y, double z); +void mjuu_setvec(float* dest, double x, double y, double z); // set 2D vector -void mjuu_setvec(double* dest, const double x, const double y); +void mjuu_setvec(double* dest, double x, double y); -// copy double array -void mjuu_copyvec(double* dest, const double* src, const int n); +// copy real-valued vector +template +void mjuu_copyvec(T1* dest, const T2* src, int n) { + for (int i=0; i mjuu_parseContentTypeAttrType(std::string_view text); diff --git a/src/user/user_vfs.c b/src/user/user_vfs.c deleted file mode 100644 index c4448bf4..00000000 --- a/src/user/user_vfs.c +++ /dev/null @@ -1,420 +0,0 @@ -// 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_vfs.h" - -#include -#include -#include -#include - -#include "engine/engine_array_safety.h" -#include "engine/engine_resource.h" -#include "engine/engine_util_errmem.h" -#include "engine/engine_util_misc.h" - -// strip path prefix from filename -static void vfs_strippath(char* newname, const char* oldname) { - int sz = strlen(oldname); - - // find last delimiter - int i; - for (i=sz-1; i >= 0; i--) { - if (oldname[i] == '\\' || oldname[i] == '/') { - break; - } - } - - // check resulting length - if (sz-(i+1) >= mjMAXVFSNAME) { - mjERROR("filename too long"); - } - if (sz-(i+1) <= 0) { - mjERROR("empty filename"); - } - - // copy - mju_strncpy(newname, oldname+i+1, mjMAXVFSNAME); - - // make lowercase - for (int j=strlen(newname)-1; j >= 0; j--) { - if (newname[j] >= 'A' && newname[j] <= 'Z') { - newname[j] = (char)(((int)newname[j]) +'a' - 'A'); - } - } -} - - - -// copies data into a buffer and produces a hash of the data -static uint64_t vfs_memcpy(void* dest, const void* restrict src, size_t n) { - uint64_t hash = 0xcbf29ce484222325; // magic number - uint64_t prime = 0x100000001b3; // magic prime - const uint8_t* bytes = (uint8_t*) src; - uint8_t* buffer = (uint8_t*) dest; - for (size_t i = 0; i < n; i++) { - buffer[i] = bytes[i]; - - // do FNV-1 hash - hash |= bytes[i]; - hash *= prime; - } - return hash; -} - - - -// VFS hash function implemented using the FNV-1 hash -static uint64_t vfs_hash(const void* restrict buffer, size_t n) { - uint64_t hash = 0xcbf29ce484222325; // magic number - uint64_t prime = 0x100000001b3; // magic prime - const uint8_t* bytes = (uint8_t*) buffer; - for (size_t i = 0; i < n; i++) { - hash |= bytes[i]; - hash *= prime; - } - return hash; -} - - - -// initialize to empty (no deallocation) -void mj_defaultVFS(mjVFS* vfs) { - memset(vfs, 0, sizeof(mjVFS)); -} - - - -// add file to VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load -int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) { - // check vfs size - if (vfs->nfile >= mjMAXVFS-1) { - return 1; - } - - // make full name - char fullname[1000]; - if (mju_makefullname(fullname, sizeof(fullname), directory, filename)) { - return -1; - } - - // strip path - char newname[mjMAXVFSNAME]; - vfs_strippath(newname, filename); - - // check for repeated name - for (int i=0; i < vfs->nfile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { - return 2; - } - } - - // assign name - mjSTRNCPY(vfs->filename[vfs->nfile], newname); - - // allocate and read - size_t filesize = 0; - vfs->filedata[vfs->nfile] = mju_fileToMemory(fullname, &filesize); - if (!vfs->filedata[vfs->nfile]) { - return -1; - } - - // assign size, count, and checksum - vfs->filestamp[vfs->nfile] = vfs_hash(vfs->filedata[vfs->nfile], filesize); - vfs->filesize[vfs->nfile] = filesize; - vfs->nfile++; - - return 0; -} - - - -// make empty file in VFS, return 0: success, 1: full, 2: repeated name -int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize) { - // check vfs size - if (vfs->nfile >= mjMAXVFS-1) { - return 1; - } - - // check filesize - if (filesize <= 0) { - mjERROR("expects positive filesize"); - } - - // strip path - char newname[mjMAXVFSNAME]; - vfs_strippath(newname, filename); - - // check for repeated name - for (int i=0; i < vfs->nfile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { - return 2; - } - } - - // assign name - mjSTRNCPY(vfs->filename[vfs->nfile], newname); - - // allocate and clear - vfs->filedata[vfs->nfile] = mju_malloc(filesize); - if (!vfs->filedata[vfs->nfile]) { - mjERROR("could not allocate memory"); - } - memset(vfs->filedata[vfs->nfile], 0, filesize); - vfs->filestamp[vfs->nfile] = 0; - - // assign size and count - vfs->filesize[vfs->nfile] = filesize; - vfs->nfile++; - - return 0; -} - - - -// add file from buffer into VFS -int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer) { - if (!vfs || !buffer || !name) { - mjERROR("null pointer"); - } - - if (vfs->nfile >= mjMAXVFS-1) { - return 1; - } - - // check buffer size - if (nbuffer <= 0) { - mjERROR("expects positive buffer size"); - } - - // strip path - char newname[mjMAXVFSNAME]; - vfs_strippath(newname, name); - - // check for repeated name - for (int i=0; i < vfs->nfile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { - return 2; - } - } - - // assign name - mjSTRNCPY(vfs->filename[vfs->nfile], newname); - - // allocate and clear - vfs->filedata[vfs->nfile] = mju_malloc(nbuffer); - if (!vfs->filedata[vfs->nfile]) { - mjERROR("could not allocate memory"); - } - vfs->filestamp[vfs->nfile] = vfs_memcpy(vfs->filedata[vfs->nfile], buffer, nbuffer); - - // assign size and count - vfs->filesize[vfs->nfile] = nbuffer; - vfs->nfile++; - - return 0; -} - - - -// return file index in VFS, or -1 if not found in VFS -int mj_findFileVFS(const mjVFS* vfs, const char* filename) { - // strip path - char newname[mjMAXVFSNAME]; - vfs_strippath(newname, filename); - // find specific file - for (int i=0; i < vfs->nfile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { - return i; - } - } - - return -1; -} - - - -// delete file from VFS, return 0: success, -1: not found in VFS -int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { - // strip path - char newname[mjMAXVFSNAME]; - vfs_strippath(newname, filename); - - // find specified file - for (int i=0; i < vfs->nfile; i++) { - if (strncmp(newname, vfs->filename[i], mjMAXVFSNAME) == 0) { - // free buffer - mju_free(vfs->filedata[i]); - - // scroll remaining files forward - for (int j=i; j < vfs->nfile-1; j++) { - mjSTRNCPY(vfs->filename[j], vfs->filename[j+1]); - vfs->filesize[j] = vfs->filesize[j+1]; - vfs->filedata[j] = vfs->filedata[j+1]; - } - - // set last to 0, for style - vfs->filename[vfs->nfile-1][0] = 0; - vfs->filesize[vfs->nfile-1] = 0; - vfs->filedata[vfs->nfile-1] = NULL; - - // decrease counter - vfs->nfile--; - return 0; - } - } - - return -1; -} - - - -// delete all files from VFS -void mj_deleteVFS(mjVFS* vfs) { - for (int i=0; i < vfs->nfile; i++) { - mju_free(vfs->filedata[i]); - } - - memset(vfs, 0, sizeof(mjVFS)); -} - - - -// open callback for the VFS resource provider -static int vfs_open_callback(mjResource* resource) { - if (!resource || !resource->name || !resource->data) { - return 0; - } - - const mjVFS* vfs = (const mjVFS*) resource->data; - int i = mj_findFileVFS(vfs, resource->name); - resource->timestamp[0] = '\0'; - if (i >= 0 && vfs->filestamp[i]) { - mju_encodeBase64(resource->timestamp, (uint8_t*) &vfs->filestamp[i], - sizeof(uint64_t)); - } - return i >= 0; -} - - - -// read callback for the VFS resource provider -static int vfs_read_callback(mjResource* resource, const void** buffer) { - if (!resource || !resource->name || !resource->data) { - *buffer = NULL; - return -1; - } - - const mjVFS* vfs = (const mjVFS*) resource->data; - int i = mj_findFileVFS(vfs, resource->name); - if (i < 0) { - *buffer = NULL; - return -1; - } - - *buffer = vfs->filedata[i]; - return vfs->filesize[i]; -} - - - -// close callback for the VFS resource provider -static void vfs_close_callback(mjResource* resource) { -} - - - -// getdir callback for the VFS resource provider -static void vfs_getdir_callback(mjResource* resource, const char** dir, int* ndir) { - if (resource) { - *dir = resource->name; - *ndir = mju_dirnamelen(resource->name); - } else { - *dir = NULL; - *ndir = 0; - } -} - - -// modified callback for the VFS resource provider -// return > 0 if modified and 0 if unmodified -static int vfs_modified_callback(const mjResource* resource, const char* timestamp) { - uint64_t filestamp; - if (mju_isValidBase64(timestamp) > sizeof(uint64_t)) { - return 2; // error (assume modified) - } - - mju_decodeBase64((uint8_t*) &filestamp, timestamp); - if (!filestamp) return 3; // no hash (assume modified) - - if (resource) { - const mjVFS* vfs = (const mjVFS*) resource->data; - int i = mj_findFileVFS(vfs, resource->name); - if (i < 0) return 4; // missing file (assume modified) - if (!vfs->filestamp[i]) return 5; // missing filestamp (assume modified) - - if (vfs->filestamp[i] == filestamp) { - return 0; // unmodified - } - } - return 1; // modified -} - -// open VFS resource -mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) { - if (vfs == NULL) { - return NULL; - } - - // VFS provider - static struct mjpResourceProvider provider = { - .prefix = NULL, - .data = NULL, - .open = &vfs_open_callback, - .read = &vfs_read_callback, - .close = &vfs_close_callback, - .getdir = &vfs_getdir_callback, - .modified = &vfs_modified_callback, - }; - - // create resource - mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource)); - if (resource == NULL) { - mjERROR("could not allocate memory"); - return NULL; - } - - // clear out resource - memset(resource, 0, sizeof(mjResource)); - - // copy name - resource->name = mju_malloc(sizeof(char) * (strlen(name) + 1)); - if (resource->name == NULL) { - mju_closeResource(resource); - mjERROR("could not allocate memory"); - return NULL; - } - memcpy(resource->name, name, sizeof(char) * (strlen(name) + 1)); - resource->data = (void*) vfs; - - // open resource - resource->provider = &provider; - if (provider.open(resource)) { - return resource; - } - - // not found in VFS - mju_closeResource(resource); - return NULL; -} diff --git a/src/user/user_vfs.cc b/src/user/user_vfs.cc new file mode 100644 index 00000000..f2b05eb0 --- /dev/null +++ b/src/user/user_vfs.cc @@ -0,0 +1,322 @@ +// 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_vfs.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "engine/engine_resource.h" +#include "engine/engine_util_errmem.h" +#include "engine/engine_util_misc.h" +#include "user/user_util.h" + +namespace { + +// internal struct for VFS files +struct VFSFile { + std::string filename; + std::unique_ptr> filedata; + std::size_t filesize; + uint64_t filestamp; +}; + +// internal container class for VFS +class VFS { + public: + // returns true if the file exists in the VFS + bool HasFile(const std::string& filename) const; + + // returns inserted mjuuVFSFile if the file was added successfully. This class + // assumes ownership of the buffer and will free it when the VFS is deleted. + VFSFile* AddFile(const std::string& filename, void* buffer, + std::size_t nbuffer, uint64_t filestamp); + + // returns the internal file struct for the given filename + const VFSFile* GetFile(const std::string& filename) const; + + // deletes file from VFS, return 0: success, -1: not found + int DeleteFile(const std::string& filename); + + private: + std::unordered_map files_; +}; + +// returns the internal VFS class pointer from the VFS C struct +inline VFS* GetVFSImpl(const mjVFS* vfs) { + return vfs->impl_ ? static_cast(vfs->impl_) : nullptr; +} + +// strip path prefix from filename and make lowercase +std::string StripPath(const char* name) { + std::string newname = mjuu_strippath(name); + + // make lowercase + std::transform(newname.begin(), newname.end(), newname.begin(), + [](unsigned char c) { return std::tolower(c); }); + return newname; +} + +// copies data into a buffer and produces a hash of the data +uint64_t vfs_memcpy(void* dest, const void* src, size_t n) { + uint64_t hash = 0xcbf29ce484222325; // magic number + uint64_t prime = 0x100000001b3; // magic prime + const uint8_t* bytes = (uint8_t*) src; + uint8_t* buffer = (uint8_t*) dest; + for (size_t i = 0; i < n; i++) { + buffer[i] = bytes[i]; + + // do FNV-1 hash + hash |= bytes[i]; + hash *= prime; + } + return hash; +} + +// VFS hash function implemented using the FNV-1 hash +uint64_t vfs_hash(const void* buffer, size_t n) { + uint64_t hash = 0xcbf29ce484222325; // magic number + uint64_t prime = 0x100000001b3; // magic prime + const uint8_t* bytes = (uint8_t*) buffer; + for (size_t i = 0; i < n; i++) { + hash |= bytes[i]; + hash *= prime; + } + return hash; +} + +bool VFS::HasFile(const std::string& filename) const { + return files_.find(filename) != files_.end(); +} + +VFSFile* VFS::AddFile(const std::string& filename, void* buffer, + std::size_t nbuffer, uint64_t filestamp) { + auto [it, inserted] = files_.insert({filename, VFSFile()}); + if (!inserted) { + return nullptr; // repeated name + } + it->second.filename = filename; + it->second.filedata = std::unique_ptr( + buffer, [](void* b) { mju_free(b); }); // corresponding to mju_malloc + it->second.filesize = nbuffer; + it->second.filestamp = filestamp; + return &(it->second); +} + +const VFSFile* VFS::GetFile(const std::string& filename) const { + auto it = files_.find(filename); + if (it == files_.end()) { + return nullptr; + } + return &it->second; +} + +int VFS::DeleteFile(const std::string& filename) { + auto it = files_.find(filename); + if (it == files_.end()) { + return -1; + } + files_.erase(it); + return 0; +} + +// open callback for the VFS resource provider +int Open(mjResource* resource) { + if (!resource || !resource->name || !resource->data) { + return 0; + } + + const mjVFS* vfs = (const mjVFS*) resource->data; + const VFS* cvfs = GetVFSImpl(vfs); + const VFSFile* file = cvfs->GetFile(StripPath(resource->name)); + if (file == nullptr) { + return 0; + } + + resource->timestamp[0] = '\0'; + if (file->filestamp) { + mju_encodeBase64(resource->timestamp, (uint8_t*) &file->filestamp, + sizeof(uint64_t)); + } + return 1; +} + +// read callback for the VFS resource provider +int Read(mjResource* resource, const void** buffer) { + if (!resource || !resource->name || !resource->data) { + *buffer = nullptr; + return -1; + } + + const VFS* vfs = GetVFSImpl(static_cast(resource->data)); + const VFSFile* file = vfs->GetFile(StripPath(resource->name)); + if (file == nullptr) { + *buffer = nullptr; + return -1; + } + + *buffer = file->filedata.get(); + return file->filesize; +} + +// close callback for the VFS resource provider +void Close(mjResource* resource) { +} + +// getdir callback for the VFS resource provider +void GetDir(mjResource* resource, const char** dir, int* ndir) { + *dir = (resource) ? resource->name : nullptr; + *ndir = (resource) ? mju_dirnamelen(resource->name) : 0; +} + +// modified callback for the VFS resource provider +// return > 0 if modified and 0 if unmodified +int Modified(const mjResource* resource, const char* timestamp) { + uint64_t filestamp; + if (mju_isValidBase64(timestamp) > sizeof(uint64_t)) { + return 2; // error (assume modified) + } + + mju_decodeBase64((uint8_t*) &filestamp, timestamp); + if (!filestamp) return 3; // no hash (assume modified) + + if (resource) { + const VFS* cvfs = GetVFSImpl(static_cast(resource->data)); + const VFSFile* file = cvfs->GetFile(StripPath(resource->name)); + if (file == nullptr) return 4; // missing file (assume modified) + if (!file->filestamp) return 5; // missing filestamp (assume modified) + + if (file->filestamp == filestamp) { + return 0; // unmodified + } + } + return 1; // modified +} + +} // namespace + +// initialize to empty (no deallocation) +void mj_defaultVFS(mjVFS* vfs) { + vfs->impl_ = new VFS(); +} + +// add file to VFS, return 0: success, 2: repeated name, -1: failed to load +int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename) { + VFS* cvfs = GetVFSImpl(vfs); + + // make full name + std::string fullname = mjuu_combinePaths(directory, filename); + + // strip path + std::string newname = StripPath(filename); + + // check beforehand for repeated name, to avoid reading file into memory + if (cvfs->HasFile(newname)) { + return 2; + } + + // allocate and read + size_t nbuffer = 0; + void* buffer = mju_fileToMemory(fullname.c_str(), &nbuffer); + if (buffer == nullptr) { + return -1; + } + + if (!cvfs->AddFile(newname, buffer, nbuffer, vfs_hash(buffer, nbuffer))) { + mju_free(buffer); + return 2; // AddFile failed, SHOULD NOT OCCUR + } + return 0; +} + +// add file from buffer into VFS +int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, + int nbuffer) { + VFS* cvfs = GetVFSImpl(vfs); + + // allocate and clear + void* inbuffer = mju_malloc(nbuffer); + if (buffer == nullptr) { + mjERROR("could not allocate memory"); + } + VFSFile* file; + if (!(file = cvfs->AddFile(StripPath(name), inbuffer, nbuffer, 0))) { + mju_free(inbuffer); + return 2; // AddFile failed, repeated name + } + file->filestamp = vfs_memcpy(inbuffer, buffer, nbuffer); + return 0; +} + +// delete file from VFS, return 0: success, -1: not found in VFS +int mj_deleteFileVFS(mjVFS* vfs, const char* filename) { + VFS* cvfs = GetVFSImpl(vfs); + return cvfs->DeleteFile(StripPath(filename)); +} + +// delete all files from VFS +void mj_deleteVFS(mjVFS* vfs) { + if (vfs) { + delete GetVFSImpl(vfs); + } +} + +// open VFS resource +mjResource* mju_openVfsResource(const char* name, const mjVFS* vfs) { + if (vfs == nullptr) { + return nullptr; + } + + // VFS provider + static struct mjpResourceProvider provider = { nullptr, &Open, &Read, &Close, + &GetDir, &Modified, nullptr }; + + // create resource + mjResource* resource = (mjResource*) mju_malloc(sizeof(mjResource)); + if (resource == nullptr) { + mjERROR("could not allocate memory"); + return nullptr; + } + + // clear out resource + memset(resource, 0, sizeof(mjResource)); + + // copy name + std::size_t n = std::strlen(name); + resource->name = (char*) mju_malloc(sizeof(char) * (n + 1)); + if (resource->name == nullptr) { + mju_closeResource(resource); + mjERROR("could not allocate memory"); + return nullptr; + } + std::memcpy(resource->name, name, sizeof(char) * (n + 1)); + resource->data = (void*) vfs; + + // open resource + resource->provider = &provider; + if (provider.open(resource)) { + return resource; + } + + // not found in VFS + mju_closeResource(resource); + return nullptr; +} diff --git a/src/user/user_vfs.h b/src/user/user_vfs.h index 80a7fcef..202136fd 100644 --- a/src/user/user_vfs.h +++ b/src/user/user_vfs.h @@ -27,17 +27,17 @@ extern "C" { #endif -// initialize to empty (no deallocation) +// Initialize an empty VFS, mj_deleteVFS must be called to deallocate the VFS MJAPI void mj_defaultVFS(mjVFS* vfs); -// add file to VFS, return 0: success, 1: full, 2: repeated name, -1: not found on disk +// add file to VFS, return 0: success, 2: repeated name, -1: not found on disk MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); -// deprecated: use mj_copyBufferVFS +// deprecated: use mj_addBufferVFS MJAPI int mj_makeEmptyFileVFS(mjVFS* vfs, const char* filename, int filesize); -// add file from buffer into VFS, return 0: success, 1: full, 2: repeated name, -1: failed to load -MJAPI int mj_copyBufferVFS(mjVFS* vfs, const char* filename, const void* buffer, int nbuffer); +// add file from buffer into VFS, return 0: success, 2: repeated name, -1: failed to load +MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* filename, const void* buffer, int nbuffer); // return file index in VFS, or -1 if not found in VFS MJAPI int mj_findFileVFS(const mjVFS* vfs, const char* filename); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 0e05d98c..d5f20190 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -29,6 +29,7 @@ #include "tinyxml2.h" +#include #include #include #include @@ -862,7 +863,7 @@ void mjXReader::Parse(XMLElement* root) { readingdefaults = true; for (XMLElement* section = FirstChildElement(root, "default"); section; section = NextSiblingElement(section, "default")) { - Default(section, -1); + Default(section, nullptr); } readingdefaults = false; @@ -958,7 +959,7 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { if (text.size()!=3) { throw mjXError(section, "euler format must have length 3"); } - memcpy(spec->euler, text.c_str(), 3); + memcpy(spec->eulerseq, text.c_str(), 3); } if (ReadAttrTxt(section, "assetdir", text)) { mjs_setString(spec->meshdir, text.c_str()); @@ -1288,16 +1289,13 @@ void mjXReader::Statistic(XMLElement* section) { // flex element parser void mjXReader::OneFlex(XMLElement* elem, mjsFlex* pflex) { - string text, name, classname, material; + string text, name, material; int n; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pflex->name, name.c_str()); } - if (ReadAttrTxt(elem, "classname", classname)) { - mjs_setString(pflex->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "material", material)) { mjs_setString(pflex->material, material.c_str()); } @@ -1363,15 +1361,12 @@ void mjXReader::OneFlex(XMLElement* elem, mjsFlex* pflex) { // mesh element parser void mjXReader::OneMesh(XMLElement* elem, mjsMesh* pmesh) { int n; - string text, name, classname, content_type; + string text, name, content_type; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pmesh->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pmesh->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "content_type", content_type)) { mjs_setString(pmesh->content_type, content_type.c_str()); } @@ -1525,16 +1520,13 @@ void mjXReader::OneSkin(XMLElement* elem, mjsSkin* pskin) { // material element parser void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* pmat) { - string text, name, classname, texture; + string text, name, texture; int n; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pmat->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pmat->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "texture", texture)) { mjs_setString(pmat->texture, texture.c_str()); } @@ -1558,7 +1550,7 @@ void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* pmat) { // joint element parser void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) { - string text, name, classname; + string text, name; std::vector userdata; int n; @@ -1566,9 +1558,6 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) { if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pjoint->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pjoint->classname, classname.c_str()); - } if (MapValue(elem, "type", &n, joint_map, joint_sz)) { pjoint->type = (mjtJoint)n; } @@ -1608,7 +1597,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) { // geom element parser void mjXReader::OneGeom(XMLElement* elem, mjsGeom* pgeom) { - string text, name, classname; + string text, name; std::vector userdata; std::string hfieldname, meshname, material; int n; @@ -1617,9 +1606,6 @@ void mjXReader::OneGeom(XMLElement* elem, mjsGeom* pgeom) { if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pgeom->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pgeom->classname, classname.c_str()); - } if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) { pgeom->type = (mjtGeom)n; } @@ -1684,7 +1670,7 @@ void mjXReader::OneGeom(XMLElement* elem, mjsGeom* pgeom) { // site element parser void mjXReader::OneSite(XMLElement* elem, mjsSite* site) { int n; - string text, name, classname; + string text, name; std::vector userdata; std::string material; @@ -1692,9 +1678,6 @@ void mjXReader::OneSite(XMLElement* elem, mjsSite* site) { if (ReadAttrTxt(elem, "name", name)) { mjs_setString(site->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(site->classname, classname.c_str()); - } if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) { site->type = (mjtGeom)n; } @@ -1721,16 +1704,13 @@ void mjXReader::OneSite(XMLElement* elem, mjsSite* site) { // camera element parser void mjXReader::OneCamera(XMLElement* elem, mjsCamera* pcam) { int n; - string text, name, classname, targetbody; + string text, name, targetbody; std::vector userdata; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pcam->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pcam->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "target", targetbody)) { mjs_setString(pcam->targetbody, targetbody.c_str()); } @@ -1779,15 +1759,12 @@ void mjXReader::OneCamera(XMLElement* elem, mjsCamera* pcam) { // light element parser void mjXReader::OneLight(XMLElement* elem, mjsLight* plight) { int n; - string text, name, classname, targetbody; + string text, name, targetbody; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(plight->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(plight->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "target", targetbody)) { mjs_setString(plight->targetbody, targetbody.c_str()); } @@ -1821,13 +1798,10 @@ void mjXReader::OneLight(XMLElement* elem, mjsLight* plight) { // pair element parser void mjXReader::OnePair(XMLElement* elem, mjsPair* ppair) { - string text, name, classname, geomname1, geomname2; + string text, name, geomname1, geomname2; // regular only if (!readingdefaults) { - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(ppair->classname, classname.c_str()); - } if (ReadAttrTxt(elem, "geom1", geomname1)) { mjs_setString(ppair->geomname1, geomname1.c_str()); } @@ -1857,7 +1831,7 @@ void mjXReader::OnePair(XMLElement* elem, mjsPair* ppair) { // equality element parser void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) { int n; - string text, name1, name2, name, classname; + string text, name1, name2, name; // read type (bad keywords already detected by schema) text = elem->Value(); @@ -1868,9 +1842,6 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) { if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pequality->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pequality->classname, classname.c_str()); - }; switch (pequality->type) { case mjEQ_CONNECT: @@ -1934,16 +1905,13 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) { // tendon element parser void mjXReader::OneTendon(XMLElement* elem, mjsTendon* pten) { - string text, name, classname, material; + string text, name, material; std::vector userdata; // read attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pten->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pten->classname, classname.c_str()); - } ReadAttrInt(elem, "group", &pten->group); if (ReadAttrTxt(elem, "material", material)) { mjs_setString(pten->material, material.c_str()); @@ -1978,15 +1946,12 @@ void mjXReader::OneTendon(XMLElement* elem, mjsTendon* pten) { // actuator element parser void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { - string text, type, name, classname, target, slidersite, refsite; + string text, type, name, target, slidersite, refsite; // common attributes if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pact->name, name.c_str()); } - if (ReadAttrTxt(elem, "class", classname)) { - mjs_setString(pact->classname, classname.c_str()); - } ReadAttrInt(elem, "group", &pact->group); MapValue(elem, "ctrllimited", &pact->ctrllimited, TFAuto_map, 3); MapValue(elem, "forcelimited", &pact->forcelimited, TFAuto_map, 3); @@ -2560,7 +2525,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* pbody) { fcomp.rigid = (n==1); } if (ReadAttrTxt(elem, "point", text)){ - fcomp.point = String2Vector(text); + fcomp.point = String2Vector(text); } if (ReadAttrTxt(elem, "element", text)){ fcomp.element = String2Vector(text); @@ -2667,31 +2632,28 @@ void mjXReader::OnePlugin(XMLElement* elem, mjsPlugin* plugin) { //------------------ MJCF-specific sections -------------------------------------------------------- // default section parser -void mjXReader::Default(XMLElement* section, int parentid) { +void mjXReader::Default(XMLElement* section, const mjsDefault* def) { XMLElement* elem; string text, name; - mjsDefault* def; - int thisid; - // create new default, except at top level (already added in mjCModel ctor) + // create new default, except at top level (already added in mjCModel constructor) text.clear(); ReadAttrTxt(section, "class", text); if (text.empty()) { - if (parentid>=0) { + if (def) { throw mjXError(section, "empty class name"); - } else { - text = "main"; } } - if (parentid>=0) { - def = mjs_addDefault(model, text.c_str(), parentid, &thisid); + if (def) { + def = mjs_addDefault(model, text.c_str(), def); if (!def) { throw mjXError(section, "repeated default class name"); } } else { - thisid = 0; def = mjs_getSpecDefault(model); - mjs_setString(def->name, text.c_str()); + if (!text.empty() && text != "main") { + throw mjXError(section, "top-level default class 'main' cannot be renamed"); + } } // iterate over elements other than nested defaults @@ -2755,7 +2717,7 @@ void mjXReader::Default(XMLElement* section, int parentid) { // read default if (name=="default") { - Default(elem, thisid); + Default(elem, def); } // advance @@ -3431,7 +3393,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { alt.type = mjORIENTATION_EULER; mjuu_copyvec(alt.euler, euler, 3); double rotation[4] = {1, 0, 0, 0}; - mjs_resolveOrientation(rotation, model->degree, model->euler, &alt); + mjs_resolveOrientation(rotation, model->degree, model->eulerseq, &alt); // read childdef mjsDefault* childdef = 0; @@ -3462,7 +3424,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { alt.euler[0] = i*euler[0]; alt.euler[1] = i*euler[1]; alt.euler[2] = i*euler[2]; - mjs_resolveOrientation(quat, model->degree, model->euler, &alt); + mjs_resolveOrientation(quat, model->degree, model->eulerseq, &alt); mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]); // process suffix @@ -3497,6 +3459,9 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame) { mjs_setString(pchild->info, std::string("line " + std::to_string(elem->GetLineNum())).c_str()); + // set default from class or childclass + mjs_setDefault(pchild->element, childdef ? childdef : def); + // read attributes std::string name, childclass; if (ReadAttrTxt(elem, "name", name)) { @@ -4155,56 +4120,36 @@ mjsDefault* mjXReader::GetClass(XMLElement* section) { return def; } - - - -// return true if c is a directory path separator (i.e. '/' or '\' on windows) -static bool IsSeperator(char c) { - return c == '/' || c == '\\'; -} - void mjXReader::SetModelFileDir(std::string modelfiledir) { modelfiledir_ = modelfiledir; - if (!modelfiledir_.empty() && !IsSeperator(modelfiledir_.back())) { - modelfiledir_.append("/"); - } } void mjXReader::SetAssetDir(std::string assetdir) { assetdir_ = assetdir; - if (!assetdir_.empty() && !IsSeperator(assetdir_.back())) { - assetdir_.append("/"); - } } void mjXReader::SetMeshDir(std::string meshdir) { meshdir_ = meshdir; - if (!meshdir_.empty() && !IsSeperator(meshdir_.back())) { - meshdir_.append("/"); - } } void mjXReader::SetTextureDir(std::string texturedir) { texturedir_ = texturedir; - if (!texturedir_.empty() && !IsSeperator(texturedir_.back())) { - texturedir_.append("/"); - } } std::string mjXReader::AssetDir() const { - return modelfiledir_ + assetdir_; + return mjuu_combinePaths(modelfiledir_, assetdir_); } std::string mjXReader::MeshDir() const { if (meshdir_.empty()) { return AssetDir(); } - return modelfiledir_ + meshdir_; + return mjuu_combinePaths(modelfiledir_, meshdir_); } std::string mjXReader::TextureDir() const { if (texturedir_.empty()) { return AssetDir(); } - return modelfiledir_ + texturedir_; + return mjuu_combinePaths(modelfiledir_, texturedir_); } diff --git a/src/xml/xml_native_reader.h b/src/xml/xml_native_reader.h index aa20841a..f2049bec 100644 --- a/src/xml/xml_native_reader.h +++ b/src/xml/xml_native_reader.h @@ -48,7 +48,7 @@ class mjXReader : public mjXBase { private: // XML section specific to MJCF - void Default(tinyxml2::XMLElement* section, int parentid); // default section + void Default(tinyxml2::XMLElement* section, const mjsDefault* def); // default section void Extension(tinyxml2::XMLElement* section); // extension section void Custom(tinyxml2::XMLElement* section); // custom section void Visual(tinyxml2::XMLElement* section); // visual section diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index c0f07e10..7b3bf798 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -14,9 +14,11 @@ #include "xml/xml_native_writer.h" +#include #include #include #include +#include #include #include @@ -69,7 +71,45 @@ static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) { mjCopyError(error, doc.ErrorStr(), error_sz); return ""; } - return string(stream.CStr()); + std::string str = string(stream.CStr()); + + // top level sections + std::array sections = { + "", "name); - WriteAttrTxt(elem, "class", pmesh->classname); + if (pmesh->classname != "main") { + WriteAttrTxt(elem, "class", pmesh->classname); + } WriteAttrTxt(elem, "content_type", pmesh->get_content_type()); WriteAttrTxt(elem, "file", pmesh->get_file()); @@ -256,7 +298,9 @@ void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* pmat, mjCDef* d // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pmat->name); - WriteAttrTxt(elem, "class", pmat->classname); + if (pmat->classname != "main") { + WriteAttrTxt(elem, "class", pmat->classname); + } } // defaults and regular @@ -277,13 +321,16 @@ void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* pmat, mjCDef* d // write joint -void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def) { +void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def, + std::string_view classname) { double zero = 0; // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pjoint->name); - WriteAttrTxt(elem, "class", pjoint->classname); + if (classname != pjoint->classname && pjoint->classname != "main") { + WriteAttrTxt(elem, "class", pjoint->classname); + } if (pjoint->type != mjJNT_FREE) { WriteAttr(elem, "pos", 3, pjoint->pos); } @@ -325,17 +372,18 @@ void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def) } } - - // write geom -void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def) { +void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def, + std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; double mass = 0; // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pgeom->name); - WriteAttrTxt(elem, "class", pgeom->classname); + if (classname != pgeom->classname && pgeom->classname != "main") { + WriteAttrTxt(elem, "class", pgeom->classname); + } if (mjGEOMINFO[pgeom->type]) { WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->Geom().size); } @@ -420,16 +468,17 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def) { } } - - // write site -void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def) { +void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def, + std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", psite->name); - WriteAttrTxt(elem, "class", psite->classname); + if (classname != psite->classname && psite->classname != "main") { + WriteAttrTxt(elem, "class", psite->classname); + } WriteAttr(elem, "pos", 3, psite->pos); WriteAttr(elem, "quat", 4, psite->quat, unitq); if (mjGEOMINFO[psite->type]) { @@ -455,16 +504,17 @@ void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def) { } } - - // write camera -void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def) { +void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def, + std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pcam->name); - WriteAttrTxt(elem, "class", pcam->classname); + if (classname != pcam->classname && pcam->classname != "main") { + WriteAttrTxt(elem, "class", pcam->classname); + } WriteAttrTxt(elem, "target", pcam->get_targetbody()); WriteAttr(elem, "pos", 3, pcam->pos); WriteAttr(elem, "quat", 4, pcam->quat, unitq); @@ -495,14 +545,15 @@ void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def) } } - - // write light -void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def) { +void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def, + std::string_view classname) { // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", plight->name); - WriteAttrTxt(elem, "class", plight->classname); + if (classname != plight->classname && plight->classname != "main") { + WriteAttrTxt(elem, "class", plight->classname); + } WriteAttrTxt(elem, "target", plight->get_targetbody()); WriteAttr(elem, "pos", 3, plight->pos); WriteAttr(elem, "dir", 3, plight->dir); @@ -522,13 +573,13 @@ void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def) WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->Light().mode); } - - // write pair void mjXWriter::OnePair(XMLElement* elem, const mjCPair* ppair, mjCDef* def) { // regular if (!writingdefaults) { - WriteAttrTxt(elem, "class", ppair->classname); + if (ppair->classname != "main") { + WriteAttrTxt(elem, "class", ppair->classname); + } WriteAttrTxt(elem, "geom1", ppair->get_geomname1()); WriteAttrTxt(elem, "geom2", ppair->get_geomname2()); } @@ -552,7 +603,9 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* peq, mjCDef* de // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", peq->name); - WriteAttrTxt(elem, "class", peq->classname); + if (peq->classname != "main") { + WriteAttrTxt(elem, "class", peq->classname); + } switch (peq->type) { case mjEQ_CONNECT: @@ -605,7 +658,9 @@ void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* pten, mjCDef* def) // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pten->name); - WriteAttrTxt(elem, "class", pten->classname); + if (pten->classname != "main") { + WriteAttrTxt(elem, "class", pten->classname); + } } // defaults and regular @@ -652,7 +707,9 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* d // regular if (!writingdefaults) { WriteAttrTxt(elem, "name", pact->name); - WriteAttrTxt(elem, "class", pact->classname); + if (pact->classname != "main") { + WriteAttrTxt(elem, "class", pact->classname); + } // transmission target switch (pact->trntype) { @@ -1143,7 +1200,9 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) { // create section, write class name section = InsertEnd(root, "default"); - WriteAttrTxt(section, "class", def->name); + if (def->name != "main") { + WriteAttrTxt(section, "class", def->name); + } // mesh elem = InsertEnd(section, "mesh"); @@ -1474,7 +1533,7 @@ XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) { // recursive body and frame writer -void mjXWriter::Body(XMLElement* elem, mjCBody* body) { +void mjXWriter::Body(XMLElement* elem, mjCBody* body, std::string_view childclass) { double unitq[4] = {1, 0, 0, 0}; if (!body) { @@ -1516,31 +1575,51 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { // write joints for (int i=0; ijoints.size(); i++) { XMLElement* celem = OneFrame(elem, body->joints[i]->frame); - OneJoint(InsertEnd(celem, "joint"), body->joints[i], body->joints[i]->def); + std::string classname = body->joints[i]->frame && !body->joints[i]->frame->classname.empty() + ? body->joints[i]->frame->classname + : body->classname; + OneJoint(InsertEnd(celem, "joint"), body->joints[i], body->joints[i]->def, + classname.empty() ? childclass : classname); } // write geoms for (int i=0; igeoms.size(); i++) { XMLElement* celem = OneFrame(elem, body->geoms[i]->frame); - OneGeom(InsertEnd(celem, "geom"), body->geoms[i], body->geoms[i]->def); + std::string classname = body->geoms[i]->frame && !body->geoms[i]->frame->classname.empty() + ? body->geoms[i]->frame->classname + : body->classname; + OneGeom(InsertEnd(celem, "geom"), body->geoms[i], body->geoms[i]->def, + classname.empty() ? childclass : classname); } // write sites for (int i=0; isites.size(); i++) { XMLElement* celem = OneFrame(elem, body->sites[i]->frame); - OneSite(InsertEnd(celem, "site"), body->sites[i], body->sites[i]->def); + std::string classname = body->sites[i]->frame && !body->sites[i]->frame->classname.empty() + ? body->sites[i]->frame->classname + : body->classname; + OneSite(InsertEnd(celem, "site"), body->sites[i], body->sites[i]->def, + classname.empty() ? childclass : classname); } // write cameras for (int i=0; icameras.size(); i++) { XMLElement* celem = OneFrame(elem, body->cameras[i]->frame); - OneCamera(InsertEnd(celem, "camera"), body->cameras[i], body->cameras[i]->def); + std::string classname = body->cameras[i]->frame && !body->cameras[i]->frame->classname.empty() + ? body->cameras[i]->frame->classname + : body->classname; + OneCamera(InsertEnd(celem, "camera"), body->cameras[i], body->cameras[i]->def, + classname.empty() ? childclass : classname); } // write lights for (int i=0; ilights.size(); i++) { XMLElement* celem = OneFrame(elem, body->lights[i]->frame); - OneLight(InsertEnd(celem, "light"), body->lights[i], body->lights[i]->def); + std::string classname = body->lights[i]->frame && !body->lights[i]->frame->classname.empty() + ? body->lights[i]->frame->classname + : body->classname; + OneLight(InsertEnd(celem, "light"), body->lights[i], body->lights[i]->def, + classname.empty() ? childclass : classname); } // write plugin @@ -1551,7 +1630,10 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body) { // write child bodies recursively for (int i=0; ibodies.size(); i++) { XMLElement* celem = OneFrame(elem, body->bodies[i]->frame); - Body(InsertEnd(celem, "body"), body->bodies[i]); + std::string classname = body->bodies[i]->frame && !body->bodies[i]->frame->classname.empty() + ? body->bodies[i]->frame->classname + : body->classname; + Body(InsertEnd(celem, "body"), body->bodies[i], classname.empty() ? childclass : classname); } } diff --git a/src/xml/xml_native_writer.h b/src/xml/xml_native_writer.h index c6791acd..f0d6ccec 100644 --- a/src/xml/xml_native_writer.h +++ b/src/xml/xml_native_writer.h @@ -17,6 +17,7 @@ #include #include +#include #include #include "user/user_objects.h" @@ -49,7 +50,6 @@ class mjXWriter : public mjXBase { void Extension(tinyxml2::XMLElement* root); // extension section void Custom(tinyxml2::XMLElement* root); // custom section 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 Deformable(tinyxml2::XMLElement* root); // deformable section void Equality(tinyxml2::XMLElement* root); // equality section @@ -58,16 +58,24 @@ class mjXWriter : public mjXBase { void Sensor(tinyxml2::XMLElement* root); // sensor section void Keyframe(tinyxml2::XMLElement* root); // keyframe section + // body/world section + void Body(tinyxml2::XMLElement* elem, mjCBody* body, std::string_view childclass = ""); + // single element writers, used in defaults and main body void OneFlex(tinyxml2::XMLElement* elem, const mjCFlex* pflex); void OneMesh(tinyxml2::XMLElement* elem, const mjCMesh* pmesh, mjCDef* def); void OneSkin(tinyxml2::XMLElement* elem, const mjCSkin* pskin); void OneMaterial(tinyxml2::XMLElement* elem, const mjCMaterial* pmaterial, mjCDef* def); - void OneJoint(tinyxml2::XMLElement* elem, const mjCJoint* pjoint, mjCDef* def); - void OneGeom(tinyxml2::XMLElement* elem, const mjCGeom* pgeom, mjCDef* def); - void OneSite(tinyxml2::XMLElement* elem, const mjCSite* psite, mjCDef* def); - void OneCamera(tinyxml2::XMLElement* elem, const mjCCamera* pcamera, mjCDef* def); - void OneLight(tinyxml2::XMLElement* elem, const mjCLight* plight, mjCDef* def); + void OneJoint(tinyxml2::XMLElement* elem, const mjCJoint* pjoint, mjCDef* def, + std::string_view classname = ""); + void OneGeom(tinyxml2::XMLElement* elem, const mjCGeom* pgeom, mjCDef* def, + std::string_view classname = ""); + void OneSite(tinyxml2::XMLElement* elem, const mjCSite* psite, mjCDef* def, + std::string_view classname = ""); + void OneCamera(tinyxml2::XMLElement* elem, const mjCCamera* pcamera, + mjCDef* def, std::string_view classname = ""); + void OneLight(tinyxml2::XMLElement* elem, const mjCLight* plight, mjCDef* def, + std::string_view classname = ""); void OnePair(tinyxml2::XMLElement* elem, const mjCPair* ppair, mjCDef* def); void OneEquality(tinyxml2::XMLElement* elem, const mjCEquality* pequality, mjCDef* def); void OneTendon(tinyxml2::XMLElement* elem, const mjCTendon* ptendon, mjCDef* def); diff --git a/src/xml/xml_urdf.cc b/src/xml/xml_urdf.cc index e7d27d88..628388e1 100644 --- a/src/xml/xml_urdf.cc +++ b/src/xml/xml_urdf.cc @@ -274,7 +274,7 @@ void mjXURDF::Body(XMLElement* body_elem) { // lquat = rotation from specified to default (joint/body) inertial frame double lquat[4] = {1, 0, 0, 0}; double tmpquat[4] = {1, 0, 0, 0}; - const char* altres = mjs_fullInertia(lquat, pbody->inertia, pbody->fullinertia); + const char* altres = mjuu_fullInertia(lquat, pbody->inertia, pbody->fullinertia); // inertia are sometimes 0 in URDF files: ignore error in altres, fix later (void) altres; diff --git a/src/xml/xml_util.cc b/src/xml/xml_util.cc index eec0dd39..6cc1f4fd 100644 --- a/src/xml/xml_util.cc +++ b/src/xml/xml_util.cc @@ -168,7 +168,7 @@ static std::string ResolveFilePath(XMLElement* e, std::string filename, // TODO(kylebayes): We first look in the base model directory for files to // remain backwards compatible. - std::string full_filename = dir + filename; + std::string full_filename = mjuu_combinePaths(dir, filename); mjResource *resource = mju_openResource(full_filename.c_str(), nullptr, 0); if (resource != nullptr) { mju_closeResource(resource); @@ -185,7 +185,7 @@ static std::string ResolveFilePath(XMLElement* e, std::string filename, break; } } - return path + filename; + return mjuu_combinePaths(path, filename); } // constructor diff --git a/test/benchmark/testdata/humanoid_body.xml b/test/benchmark/testdata/humanoid_body.xml index c3cc25c9..96b4d2f6 100644 --- a/test/benchmark/testdata/humanoid_body.xml +++ b/test/benchmark/testdata/humanoid_body.xml @@ -25,7 +25,7 @@ + rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/> diff --git a/test/engine/CMakeLists.txt b/test/engine/CMakeLists.txt index f35bb8b6..b844e061 100644 --- a/test/engine/CMakeLists.txt +++ b/test/engine/CMakeLists.txt @@ -21,6 +21,9 @@ target_link_libraries(engine_collision_convex_test fixture gmock) mujoco_test(engine_collision_driver_test) target_link_libraries(engine_collision_driver_test fixture gmock) +mujoco_test(engine_collision_gjk_test) +target_link_libraries(engine_collision_gjk_test fixture gmock ccd) + mujoco_test(engine_core_constraint_test) target_link_libraries(engine_core_constraint_test fixture gmock) diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc new file mode 100644 index 00000000..22cc7297 --- /dev/null +++ b/test/engine/engine_collision_gjk_test.cc @@ -0,0 +1,202 @@ +// Copyright 2024 DeepMind Technologies Limited +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Tests for engine/engine_collision_gjk.c. + +#include "src/engine/engine_collision_gjk.h" + +#include + +#include "src/engine/engine_collision_convex.h" +#include +#include +#include "test/fixture.h" +#include +#include + +namespace mujoco { +namespace { + +using ::testing::NotNull; +using ::testing::ElementsAre; + +constexpr mjtNum kTolerance = 1e-6; +constexpr int kMaxIterations = 1000; + +static mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], + mjtNum x2[3]) { + mjCCDConfig config = {kMaxIterations, kTolerance}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjc_center(obj1.x0, &obj1); + mjc_center(obj2.x0, &obj2); + mjtNum dist = mj_gjk(&config, &obj1, &obj2); + if (x1 != nullptr) mju_copy3(x1, obj1.x0); + if (x2 != nullptr) mju_copy3(x2, obj2.x0); + return dist; +} + +using MjGjkTest = MujocoTest; + +TEST_F(MjGjkTest, SphereSphereIntersect) { + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_EQ(dist, 0); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, SphereSphere) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum x1[3], x2[3]; + mjtNum dist = run_gjk(model, data, geom1, geom2, x1, x2); + + EXPECT_EQ(dist, 1); + EXPECT_THAT(x1, ElementsAre(-.5, 0, 0)); + EXPECT_THAT(x2, ElementsAre(.5, 0, 0)); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, BoxBox) { + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_EQ(dist, 1); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, EllipsoidEllipsoid) { + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_NEAR(dist, 0.7542, .0001); + mj_deleteData(data); + mj_deleteModel(model); +} + +TEST_F(MjGjkTest, CapsuleCapsule) { + static constexpr char xml[] = R"( + + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_NEAR(dist, 0.4765, .0001); + mj_deleteData(data); + mj_deleteModel(model); +} + +} // namespace +} // namespace mujoco diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 2b9a4702..85ecf60a 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -223,11 +223,23 @@ TEST_F(DerivativeTest, StepSkip) { mjINT_IMPLICITFAST}) { model->opt.integrator = integrator; - // reset, take 20 steps, save initial state + // reset, take 20 steps mj_resetData(model, data); for (int i=0; i < 20; i++) { mj_step(model, data); } + + // denormalize the quat, just to see that it doesn't make a difference + for (int j=0; j < model->njnt; j++) { + if (model->jnt_type[j] == mjJNT_BALL) { + int adr = model->jnt_qposadr[j]; + for (int k=0; k < 4; k++) { + data->qpos[adr + k] *= 8; + } + } + } + + // save state std::vector qpos = AsVector(data->qpos, nq); std::vector qvel = AsVector(data->qvel, nv); @@ -279,7 +291,6 @@ TEST_F(DerivativeTest, StepSkip) { mj_deleteModel(model); } - // Analytic transition matrices for linear dynamical system xn = A*x + B*u // given modified mass matrix H (`data->qH`) and // Ac = H^-1 [diag(-stiffness) diag(-damping)] diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 41d58a71..6c269b77 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -27,11 +27,16 @@ #include #include #include +#include #include "src/cc/array_safety.h" #include "src/engine/engine_callback.h" #include "src/engine/engine_io.h" #include "test/fixture.h" +#ifdef MEMORY_SANITIZER + #include +#endif + namespace mujoco { namespace { @@ -607,6 +612,147 @@ TEST_F(ForwardTest, eq_active) { mj_deleteModel(model); } +// test that normalized and denormalized quats give the same result +TEST_F(ForwardTest, NormalizeQuats) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + + mjData* data_u = mj_makeData(model); + + // we'll compare all the memory, so unpoison it first + #ifdef MEMORY_SANITIZER + __msan_unpoison(data_u->buffer, data_u->nbuffer); + __msan_unpoison(data_u->arena, data_u->narena); + #endif + + // set quats to denormalized values, non-zero velocities + for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i; + for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i; + + // copy data and normalize quats + mjData* data_n = mj_copyData(nullptr, model, data_u); + mj_normalizeQuat(model, data_n->qpos); + + // call forward, expect quats to be untouched + mj_forward(model, data_u); + for (int i = 3; i < model->nq; i++) { + EXPECT_EQ(data_u->qpos[i], (mjtNum)i); + } + + // expect that the ball joint limit is active + EXPECT_EQ(data_u->nl, 1); + + // step both models + mj_step(model, data_u); + mj_step(model, data_n); + + // expect everything to match + MJDATA_POINTERS_PREAMBLE(model) + #define X(type, name, nr, nc) \ + for (int i = 0; i < model->nr; i++) \ + for (int j = 0; j < nc; j++) \ + EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]); + MJDATA_POINTERS; + #undef X + + // repeat the above with RK4 integrator + model->opt.integrator = mjINT_RK4; + + // reset data, unpoison + mj_resetData(model, data_u); + #ifdef MEMORY_SANITIZER + __msan_unpoison(data_u->buffer, data_u->nbuffer); + __msan_unpoison(data_u->arena, data_u->narena); + #endif + + // set quats to un-normalized values, non-zero velocities + for (int i = 3; i < model->nq; i++) data_u->qpos[i] = i; + for (int i = 0; i < model->nv; i++) data_u->qvel[i] = 0.1*i; + + // copy data and normalize quats + mj_copyData(data_n, model, data_u); + mj_normalizeQuat(model, data_n->qpos); + + // step both models + mj_step(model, data_u); + mj_step(model, data_n); + + // expect everything to match + #define X(type, name, nr, nc) \ + for (int i = 0; i < model->nr; i++) \ + for (int j = 0; j < nc; j++) \ + EXPECT_EQ(data_n->name[i*nc+j], data_u->name[i*nc+j]); + MJDATA_POINTERS; + #undef X + + mj_deleteData(data_n); + mj_deleteData(data_u); + mj_deleteModel(model); +} + +// test that normalized and denormalized quats give the same result +TEST_F(ForwardTest, MocapQuats) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + // expect mocap_quat to be normalized (by the compiler) + for (int i = 0; i < 4; i++) { + EXPECT_EQ(data->mocap_quat[i], 0.5); + EXPECT_EQ(data->xquat[4+i], 0.5); + } + + // write denormalized quats to mocap_quat, call forward again + for (int i = 0; i < 4; i++) { + data->mocap_quat[i] = 1; + } + mj_forward(model, data); + + // expect mocap_quat to remain denormalized, but xquat to be normalized + for (int i = 0; i < 4; i++) { + EXPECT_EQ(data->mocap_quat[i], 1); + EXPECT_EQ(data->xquat[4+i], 0.5); + } + + mj_deleteData(data); + mj_deleteModel(model); +} + // user defined 2nd-order activation dynamics: frequency-controlled oscillator // note that scalar mjcb_act_dyn callbacks are expected to return act_dot, but // since we have a vector output we write into act_dot directly diff --git a/test/engine/engine_plugin_test.cc b/test/engine/engine_plugin_test.cc index 53ae1553..3a0c639f 100644 --- a/test/engine/engine_plugin_test.cc +++ b/test/engine/engine_plugin_test.cc @@ -240,7 +240,7 @@ int RegisterSensorPlugin() { TestSensor::DestroyCount()++; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto sensor = reinterpret_cast(plugin_data); sensor->Reset(); @@ -283,7 +283,7 @@ int RegisterActuatorPlugin() { TestActuator::DestroyCount()++; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto actuator = reinterpret_cast(plugin_data); actuator->Reset(); @@ -338,7 +338,7 @@ int RegisterPassivePlugin() { d->plugin_data[instance] = 0; }; - plugin.reset = +[](const mjModel* m, double* plugin_state, void* plugin_data, + plugin.reset = +[](const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance) { auto passive = reinterpret_cast(plugin_data); passive->Reset(); diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 0667c321..005136bd 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -207,15 +207,11 @@ TEST_F(RelativeFrameSensorTest, ReferencePosMatQuat) { } mj_forward(model, data); - // note that in the loop above the quat is unnormalized, but that's ok, - // quaternions are automatically normalized in place: - EXPECT_NEAR(mju_norm(data->qpos+3, 4), 1.0, tol); - // get values from relative sensors after moving the object std::vector actual_values(data->sensordata+nsensordata/2, data->sensordata+nsensordata); - // object and reference have moved together, we expect values to not unchange + // object and reference have moved together, we expect values to not change EXPECT_THAT(actual_values, Pointwise(DoubleNear(tol), expected_values)); mj_deleteData(data); diff --git a/test/engine/engine_support_test.cc b/test/engine/engine_support_test.cc index c18dc7c0..93e69bfa 100644 --- a/test/engine/engine_support_test.cc +++ b/test/engine/engine_support_test.cc @@ -546,13 +546,13 @@ TEST_F(AddMTest, DenseSameAsSparse) { } // dense zero matrix - std::vector dst_sparse = std::vector(nv * nv, 0.0); + std::vector dst_sparse(nv * nv, 0.0); // sparse zero matrix - std::vector dst_dense = std::vector(nv * nv, 0.0); - std::vector rownnz = std::vector(nv, nv); - std::vector rowadr = std::vector(nv, 0); - std::vector colind = std::vector(nv * nv, 0); + std::vector dst_dense(nv * nv, 0.0); + std::vector rownnz(nv, nv); + std::vector rowadr(nv, 0); + std::vector colind(nv * nv, 0); // set sparse structure for (int i = 0; i < nv; i++) { diff --git a/test/engine/engine_util_blas_test.cc b/test/engine/engine_util_blas_test.cc index e2d354d9..706c4d17 100644 --- a/test/engine/engine_util_blas_test.cc +++ b/test/engine/engine_util_blas_test.cc @@ -16,6 +16,8 @@ #include "src/engine/engine_util_blas.h" +#include + #include #include #include @@ -128,5 +130,88 @@ TEST_F(EngineUtilBlasTest, MjuMulMat3) { } } +// utility: random quaternion, normally distributed +void RandomQuat(mjtNum quat[4], int seed) { + // make distribution using seed + std::mt19937_64 rng; + rng.seed(seed); + std::normal_distribution dist(0, 1); + + // sample + for (int i = 0; i < 4; i++) { + quat[i] = dist(rng); + } +} + +TEST_F(EngineUtilBlasTest, Normalize4IsIdempotent) { + for (int i = 0; i < 10000; ++i) { + // make random quaternion and normalize it + mjtNum quat[4]; + RandomQuat(quat, i); + mju_normalize4(quat); + + // normalize again + mjtNum quat_renormalized[4] = {quat[0], quat[1], quat[2], quat[3]}; + mju_normalize4(quat_renormalized); + + // expect equality + EXPECT_EQ(quat[0], quat_renormalized[0]); + EXPECT_EQ(quat[1], quat_renormalized[1]); + EXPECT_EQ(quat[2], quat_renormalized[2]); + EXPECT_EQ(quat[3], quat_renormalized[3]); + } +} + +TEST_F(EngineUtilBlasTest, Normalize4EdgeCases) { + // zero quat normalizes to unit quat + mjtNum quat[4] = {0}; + mjtNum norm = mju_normalize4(quat); + EXPECT_EQ(norm, 0); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // small quat normalizes regularly + quat[0] = 0; + quat[1] = mjMINVAL; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, mjMINVAL); + EXPECT_EQ(quat[0], 0); + EXPECT_EQ(quat[1], 1); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // tiny quat normalizes to unit quat + quat[0] = 0; + quat[1] = mjMINVAL/2; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, mjMINVAL/2); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // near-unit quat is normalized + quat[0] = 1 + mjMINVAL; + quat[1] = 0; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, 1 + mjMINVAL); + EXPECT_EQ(quat[0], 1); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); + + // very-near-unit quat is untouched + quat[0] = 1 + mjMINVAL/2; + quat[1] = 0; + norm = mju_normalize4(quat); + EXPECT_EQ(norm, 1 + mjMINVAL/2); + EXPECT_EQ(quat[0], 1 + mjMINVAL/2); + EXPECT_EQ(quat[1], 0); + EXPECT_EQ(quat[2], 0); + EXPECT_EQ(quat[3], 0); +} + } // namespace } // namespace mujoco diff --git a/test/engine/engine_util_misc_test.cc b/test/engine/engine_util_misc_test.cc index 63632349..aca400a5 100644 --- a/test/engine/engine_util_misc_test.cc +++ b/test/engine/engine_util_misc_test.cc @@ -148,92 +148,6 @@ TEST_F(MujocoTest, MuscleGainLength) { EXPECT_EQ(mju_muscleGainLength(2.0, lmin, lmax), 0); } -TEST_F(MujocoTest, mju_makefullname) { - char buffer[1000]; - constexpr char path[] = "engine/testdata/"; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - ASSERT_THAT(buffer, StrEq("engine/testdata/file")); - EXPECT_THAT(n, 0); -} - -TEST_F(MujocoTest, mju_makefullname2) { - char buffer[1000]; - constexpr char path[] = "engine\\testdata\\"; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - ASSERT_THAT(buffer, StrEq("engine\\testdata\\file")); - EXPECT_THAT(n, 0); -} - - -TEST_F(MujocoTest, mju_makefullname_missingSlash) { - char buffer[1000]; - constexpr char path[] = "engine/testdata"; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - ASSERT_THAT(buffer, StrEq("engine/testdata/file")); - EXPECT_THAT(n, 0); -} - -TEST_F(MujocoTest, mju_makefullname_withoutDir) { - char buffer[1000]; - constexpr char *path = NULL; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - ASSERT_THAT(buffer, StrEq("file")); - EXPECT_THAT(n, 0); -} - -TEST_F(MujocoTest, mju_makefullname_withoutDir2) { - char buffer[1000]; - constexpr char path[] = ""; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - ASSERT_THAT(buffer, StrEq("file")); - EXPECT_THAT(n, 0); -} - -TEST_F(MujocoTest, mju_makefullname_error) { - char buffer[1000]; - constexpr char path[] = "engine/testdata"; - constexpr char *file = NULL; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - EXPECT_THAT(n, Ne(0)); -} - -TEST_F(MujocoTest, mju_makefullname_error2) { - char buffer[1000]; - constexpr char path[] = "engine/testdata"; - constexpr char file[] = ""; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - EXPECT_THAT(n, Ne(0)); -} - -TEST_F(MujocoTest, mju_makefullname_error3) { - char buffer[20]; - constexpr char path[] = "engine/testdata/"; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - EXPECT_THAT(n, Ne(0)); -} - -TEST_F(MujocoTest, mju_makefullname_error4) { - char buffer[20]; - constexpr char path[] = "engine/testdata"; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - EXPECT_THAT(n, Ne(0)); -} - -TEST_F(MujocoTest, mju_makefullname_error5) { - char buffer[4]; - constexpr char path[] = ""; - constexpr char file[] = "file"; - int n = mju_makefullname(buffer, sizeof(buffer), path, file); - EXPECT_THAT(n, Ne(0)); -} - // --------------------------------- Base64 ------------------------------------ using Base64Test = MujocoTest; diff --git a/test/engine/engine_util_spatial_test.cc b/test/engine/engine_util_spatial_test.cc index 159db210..678252f0 100644 --- a/test/engine/engine_util_spatial_test.cc +++ b/test/engine/engine_util_spatial_test.cc @@ -166,38 +166,38 @@ TEST_F(Euler2QuatTest, BadSeqLength) { } TEST_F(Euler2QuatTest, Euler2Quat) { - double quat[4] = {0}; - double tol = 1e-14; + mjtNum quat[4] = {0}; + mjtNum tol = 1e-14; char seq[] = "xyz"; - double euler[3] = {mjPI, 0, 0}; - double expected[4] = {0, 1, 0, 0}; + mjtNum euler[3] = {mjPI, 0, 0}; + mjtNum expected[4] = {0, 1, 0, 0}; mju_euler2Quat(quat, euler, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected)); euler[1] = mjPI; - double expected2[4] = {0, 0, 0, 1}; + mjtNum expected2[4] = {0, 0, 0, 1}; mju_euler2Quat(quat, euler, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected2)); char seq2[] = "XYZ"; - double expected3[4] = {0, 0, 0, -1}; + mjtNum expected3[4] = {0, 0, 0, -1}; mju_euler2Quat(quat, euler, seq2); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected3)); - double euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; - double expected4[4] = {-1, 0, 0, 0}; + mjtNum euler2[3] = {2*mjPI, 2*mjPI, 2*mjPI}; + mjtNum expected4[4] = {-1, 0, 0, 0}; mju_euler2Quat(quat, euler2, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); mju_euler2Quat(quat, euler2, seq2); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected4)); - double euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; - double expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; + mjtNum euler3[3] = {mjPI/2, mjPI/2, mjPI/2}; + mjtNum expected5[4] = {0, mju_sqrt(.5), 0, mju_sqrt(.5)}; mju_euler2Quat(quat, euler3, seq); EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected5)); mju_euler2Quat(quat, euler3, seq2); - double expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; + mjtNum expected6[4] = {mju_sqrt(.5), 0, mju_sqrt(.5), 0}; EXPECT_THAT(quat, Pointwise(DoubleNear(tol), expected6)); } diff --git a/test/engine/testdata/actuation/actuator_group_disable.xml b/test/engine/testdata/actuation/actuator_group_disable.xml index b5f2632d..ab87d416 100644 --- a/test/engine/testdata/actuation/actuator_group_disable.xml +++ b/test/engine/testdata/actuation/actuator_group_disable.xml @@ -10,7 +10,7 @@ - + diff --git a/test/engine/testdata/collision_driver/humanoid.xml b/test/engine/testdata/collision_driver/humanoid.xml index 324d3fd2..b858631c 100644 --- a/test/engine/testdata/collision_driver/humanoid.xml +++ b/test/engine/testdata/collision_driver/humanoid.xml @@ -26,7 +26,7 @@ - + diff --git a/test/engine/testdata/hammock/humanoid_body.xml b/test/engine/testdata/hammock/humanoid_body.xml index 1254082a..8de9b880 100644 --- a/test/engine/testdata/hammock/humanoid_body.xml +++ b/test/engine/testdata/hammock/humanoid_body.xml @@ -25,7 +25,7 @@ + rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1"/> diff --git a/test/fixture.cc b/test/fixture.cc index c3c1df28..4e231408 100644 --- a/test/fixture.cc +++ b/test/fixture.cc @@ -177,6 +177,17 @@ std::vector GetCtrlNoise(const mjModel* m, int nsteps, return ctrl; } +template +auto Compare(T val1, T val2); + +auto Compare(char val1, char val2) { + return val1 != val2; +} + +auto Compare(unsigned char val1, unsigned char val2) { + return val1 != val2; +} + // The maximum spacing between a normalised floating point number x and an // adjacent normalised number is 2 epsilon |x|; a factor 10 is added accounting // for losses during non-idempotent operations such as vector normalizations. @@ -185,13 +196,13 @@ auto Compare(T val1, T val2) { using ReturnType = std::conditional_t, float, double>; ReturnType error; - if (mju_abs(val1) <= 1 || mju_abs(val2) <= 1) { + if (std::abs(val1) <= 1 || std::abs(val2) <= 1) { // Absolute precision for small numbers - error = mju_abs(val1-val2); + error = std::abs(val1-val2); } else { // Relative precision for larger numbers - ReturnType magnitude = mju_abs(val1) + mju_abs(val2); - error = mju_abs(val1/magnitude - val2/magnitude) / magnitude; + ReturnType magnitude = std::abs(val1) + std::abs(val2); + error = std::abs(val1/magnitude - val2/magnitude) / magnitude; } ReturnType safety_factor = 200; return error < safety_factor * std::numeric_limits::epsilon() @@ -214,18 +225,21 @@ mjtNum CompareModel(const mjModel* m1, const mjModel* m2, #undef X if (maxdif > 0) return maxdif; - // compare arrays -#define X(type, name, nr, nc) \ - for (int r = 0; r < m1->nr; r++) { \ - for (int c = 0; c < nc; c++) { \ - dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ - if (dif > maxdif) { \ - maxdif = dif; \ - field = #name; \ - field += " row: " + std::to_string(r); \ - field += " col: " + std::to_string(c); \ - } \ - } \ + // compare arrays, apart from bvh-related ones, as those are sensitive to + // numerical differences when meshes are perfectly symmetric. +#define X(type, name, nr, nc) \ + if (strncmp(#name, "bvh_", 4)) { \ + for (int r = 0; r < m1->nr; r++) { \ + for (int c = 0; c < nc; c++) { \ + dif = Compare(m1->name[r * nc + c], m2->name[r * nc + c]); \ + if (dif > maxdif) { \ + maxdif = dif; \ + field = #name; \ + field += " row: " + std::to_string(r); \ + field += " col: " + std::to_string(c); \ + } \ + } \ + } \ } // NOLINT MJMODEL_POINTERS #undef X diff --git a/test/user/user_mesh_test.cc b/test/user/user_mesh_test.cc index cf62423d..8e8d04ff 100644 --- a/test/user/user_mesh_test.cc +++ b/test/user/user_mesh_test.cc @@ -16,6 +16,8 @@ #include #include +#include +#include #include #include @@ -127,7 +129,8 @@ TEST_F(MjCMeshTest, LoadMSHWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadOBJWithVFS) { @@ -152,7 +155,8 @@ TEST_F(MjCMeshTest, LoadOBJWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadSTLWithVFS) { @@ -177,7 +181,8 @@ TEST_F(MjCMeshTest, LoadSTLWithVFS) { // should fallback to OS filesystem mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } // ------------- test content_type attributes ---------------------------------- @@ -204,7 +209,8 @@ TEST_F(MjCMeshTest, LoadMSHWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadOBJWithContentType) { @@ -229,7 +235,8 @@ TEST_F(MjCMeshTest, LoadOBJWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadSTLWithContentType) { @@ -254,7 +261,8 @@ TEST_F(MjCMeshTest, LoadSTLWithContentType) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { @@ -280,6 +288,7 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'model/unknown'")); + mj_deleteVFS(vfs.get()); } TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { @@ -304,12 +313,13 @@ TEST_F(MjCMeshTest, LoadMSHWithContentTypeParam) { // should try opening the file (not found obviously) mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); - EXPECT_THAT(error, HasSubstr("resource not found via provider or OS filesystem")); + EXPECT_THAT(error, HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } -// ------------- test vertex de-duplication (STL) ------------------------------ +// ------------- test vertex deduplication (STL) ------------------------------ -TEST_F(MjCMeshTest, DeDuplicateSTLVertices) { +TEST_F(MjCMeshTest, DeduplicateSTLVertices) { const std::string xml_path = GetTestDataFilePath(kDuplicateVerticesPath); char error[1024]; size_t error_sz = 1024; @@ -420,7 +430,8 @@ TEST_F(MjCMeshTest, MaxHullVert) { } TEST_F(MjCMeshTest, MaxHullVertDefault) { - const std::string xml_path = GetTestDataFilePath(kTorusDefaultMaxhullVertPath); + const std::string xml_path = + GetTestDataFilePath(kTorusDefaultMaxhullVertPath); std::array error; mjModel* model = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size()); ASSERT_GT(model->ngeom, 0); @@ -465,8 +476,9 @@ TEST_F(MjCMeshTest, SmallInertiaLoads) { )"; - mjModel* model = LoadModelFromString(xml); - ASSERT_THAT(model, NotNull()); + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); mj_deleteModel(model); } @@ -792,15 +804,16 @@ TEST_F(MjCMeshTest, ExactConcaveInertia) { mjtNum d_cube = .5 - model->body_ipos[5]; mjtNum d_hole = .55 - model->body_ipos[5]; mjtNum I1 = I_cube - m_hole*(.8*.8 + .8*.8)/12; - mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; - EXPECT_LE(fabs(model->body_mass[1] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[2] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[3] - m_concave_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[4] - m_concave_cube), max_abs_err); + mjtNum I2 = I_cube - m_hole*(.8*.8 + .9*.9)/12 + + m_cube*d_cube*d_cube - m_hole*d_hole*d_hole; + EXPECT_LE(mju_abs(model->body_mass[1] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[2] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[3] - m_concave_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[4] - m_concave_cube), max_abs_err); for (int i = 3; i < 15; i += 3) { - EXPECT_LE(fabs(model->body_inertia[i] - I1), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[i+1] - I2), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[i+2] - I2), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i] - I1), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i+1] - I2), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i+2] - I2), max_abs_err); } mj_deleteModel(model); } @@ -812,10 +825,10 @@ TEST_F(MjCMeshTest, ExactConvexInertia) { // https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_solid_cube = 1.; mjtNum I_solid_cube = 1./6. * m_solid_cube; - EXPECT_LE(fabs(model->body_mass[1] - m_solid_cube), max_abs_err); - EXPECT_LE(fabs(model->body_mass[2] - m_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[1] - m_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[2] - m_solid_cube), max_abs_err); for (int i = 3; i < 9; i++) { - EXPECT_LE(fabs(model->body_inertia[i] - I_solid_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[i] - I_solid_cube), max_abs_err); } mj_deleteModel(model); } @@ -827,10 +840,10 @@ TEST_F(MjCMeshTest, ExactShellInertia) { // see https://en.wikipedia.org/wiki/List_of_moments_of_inertia mjtNum m_hollow_cube = 6.; mjtNum I_hollow_cube = 5./18. * m_hollow_cube; - EXPECT_LE(fabs(model->body_mass[1] - m_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[3] - I_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[4] - I_hollow_cube), max_abs_err); - EXPECT_LE(fabs(model->body_inertia[5] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_mass[1] - m_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[3] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[4] - I_hollow_cube), max_abs_err); + EXPECT_LE(mju_abs(model->body_inertia[5] - I_hollow_cube), max_abs_err); mj_deleteModel(model); } @@ -860,8 +873,8 @@ TEST_F(MjCMeshTest, MeshPosQuat) { // Apply the inverted mesh_pos and inverted mesh_quat to the geom's pos and // quat. It should match the originally specified values. - double recovered_pos[3]; - double recovered_quat[4]; + mjtNum recovered_pos[3]; + mjtNum recovered_quat[4]; mju_mulPose(recovered_pos, recovered_quat, &model->geom_pos[0], &model->geom_quat[0], inverse_mesh_pos, inverse_mesh_quat); diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index 22b46ec0..d067d81a 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -283,7 +283,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { mj_step(m_fuse, d_fuse); mj_step(m_no_fuse, d_no_fuse); - EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 1e-17)) + EXPECT_THAT(d_fuse->qvel[0], DoubleNear(d_no_fuse->qvel[0], 2e-17)) << "Velocity should be the same after 1 step"; EXPECT_NE(d_fuse->qvel[0], 0); diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index bb4e7e3e..f7c90528 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -69,7 +69,8 @@ TEST_F(VfsTest, HFieldPngWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, HFieldCustomWithVFS) { @@ -96,7 +97,8 @@ TEST_F(VfsTest, HFieldCustomWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, TexturePngWithVFS) { @@ -124,7 +126,8 @@ TEST_F(VfsTest, TexturePngWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(VfsTest, TextureCustomWithVFS) { @@ -152,7 +155,8 @@ TEST_F(VfsTest, TextureCustomWithVFS) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } // ------------------------ test content_type attribute ------------------------ @@ -184,7 +188,8 @@ TEST_F(ContentTypeTest, HFieldPngWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, HFieldCustomWithContentType) { @@ -212,7 +217,8 @@ TEST_F(ContentTypeTest, HFieldCustomWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, HFieldWithContentTypeError) { @@ -240,6 +246,7 @@ TEST_F(ContentTypeTest, HFieldWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TexturePngWithContentType) { @@ -267,7 +274,8 @@ TEST_F(ContentTypeTest, TexturePngWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureCustomWithContentType) { @@ -296,7 +304,8 @@ TEST_F(ContentTypeTest, TextureCustomWithContentType) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, - HasSubstr("resource not found via provider or OS filesystem")); + HasSubstr("Error opening file")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureWithContentTypeError) { @@ -325,6 +334,7 @@ TEST_F(ContentTypeTest, TextureWithContentTypeError) { mjModel* model = LoadModelFromString(xml, error, error_sz, vfs.get()); EXPECT_THAT(model, IsNull()); EXPECT_THAT(error, HasSubstr("unsupported content type: 'image/jpeg'")); + mj_deleteVFS(vfs.get()); } TEST_F(ContentTypeTest, TextureLoadPng) { @@ -370,7 +380,7 @@ TEST_F(ContentTypeTest, TextureLoadPng) { EXPECT_THAT(model, NotNull()); mj_deleteModel(model); - mj_deleteFileVFS(vfs.get(), filename); + mj_deleteVFS(vfs.get()); } // ------------------------ test keyframes ------------------------------------- @@ -805,6 +815,26 @@ TEST_F(MjCHFieldTest, PngMap) { mj_deleteModel(model); } +// ------------- test textures ------------------------------------------------- + +using MjCTextureTest = MujocoTest; + +TEST_F(MjCTextureTest, TexturesLoad) { + static constexpr char xml[] = R"( + + + + + + )"; + std::array error; + mjModel* m = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(m, NotNull()) << error.data(); + + mj_deleteModel(m); +} + // ------------- test quaternion normalization---------------------------------- using QuatNorm = MujocoTest; diff --git a/test/user/user_vfs_test.cc b/test/user/user_vfs_test.cc index cc3ce120..269accea 100644 --- a/test/user/user_vfs_test.cc +++ b/test/user/user_vfs_test.cc @@ -14,7 +14,6 @@ #include #include -#include #include #include @@ -28,9 +27,16 @@ namespace mujoco { namespace { using ::testing::NotNull; -using EngineVfsTest = MujocoTest; +using UserVfsTest = MujocoTest; -TEST_F(EngineVfsTest, AddFile) { +static bool HasFile(const mjVFS* vfs, const std::string& filename) { + mjResource* resource = mju_openVfsResource(filename.c_str(), vfs); + bool result = resource != nullptr; + mju_closeResource(resource); + return result; +} + +TEST_F(UserVfsTest, AddFile) { constexpr char path[] = "engine/testdata/actuation/"; const std::string dir = GetTestDataFilePath(path); std::string file1 = "activation.xml"; @@ -49,54 +55,159 @@ TEST_F(EngineVfsTest, AddFile) { ASSERT_THAT(fp3, NotNull()) << "Input file3 missing."; std::fclose(fp3); - auto vfs = std::make_unique(); - mj_defaultVFS(vfs.get()); + mjVFS vfs; + mj_defaultVFS(&vfs); + EXPECT_FALSE(HasFile(&vfs, file1)); + EXPECT_THAT(mj_addFileVFS(&vfs, dir.c_str(), file1.c_str()), 0); + EXPECT_TRUE(HasFile(&vfs, file1)); - EXPECT_THAT(vfs->nfile, 0); - EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file1.c_str()), 0); - EXPECT_THAT(vfs->nfile, 1); - EXPECT_THAT(vfs->filename[0], file1); + EXPECT_THAT(mj_addFileVFS(&vfs, dir.c_str(), file2.c_str()), 0); + EXPECT_TRUE(HasFile(&vfs, file1)); + EXPECT_TRUE(HasFile(&vfs, file2)); - EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file2.c_str()), 0); - EXPECT_THAT(vfs->nfile, 2); - EXPECT_THAT(vfs->filename[0], file1); - EXPECT_THAT(vfs->filename[1], file2); + EXPECT_THAT(mj_addFileVFS(&vfs, dir.c_str(), file3.c_str()), 0); + EXPECT_TRUE(HasFile(&vfs, file1.c_str())); + EXPECT_TRUE(HasFile(&vfs, file2.c_str())); + EXPECT_TRUE(HasFile(&vfs, file3.c_str())); - EXPECT_THAT(mj_addFileVFS(vfs.get(), dir.c_str(), file3.c_str()), 0); - EXPECT_THAT(vfs->nfile, 3); - EXPECT_THAT(vfs->filename[0], file1); - EXPECT_THAT(vfs->filename[1], file2); - EXPECT_THAT(vfs->filename[2], file3); + mj_deleteFileVFS(&vfs, file1.c_str()); + EXPECT_FALSE(HasFile(&vfs, file1.c_str())); + EXPECT_TRUE(HasFile(&vfs, file2.c_str())); + EXPECT_TRUE(HasFile(&vfs, file3.c_str())); - mj_deleteFileVFS(vfs.get(), file1.c_str()); - EXPECT_THAT(vfs->nfile, 2); - EXPECT_THAT(vfs->filename[0], file2); - EXPECT_THAT(vfs->filename[1], file3); - mj_deleteFileVFS(vfs.get(), file3.c_str()); - EXPECT_THAT(vfs->nfile, 1); - EXPECT_THAT(vfs->filename[0], file2); + mj_deleteFileVFS(&vfs, file3.c_str()); + EXPECT_FALSE(HasFile(&vfs, file1.c_str())); + EXPECT_TRUE(HasFile(&vfs, file2.c_str())); + EXPECT_FALSE(HasFile(&vfs, file3.c_str())); - mj_deleteFileVFS(vfs.get(), file2.c_str()); - EXPECT_THAT(vfs->nfile, 0); + mj_deleteFileVFS(&vfs, file2.c_str()); + EXPECT_FALSE(HasFile(&vfs, file1.c_str())); + EXPECT_FALSE(HasFile(&vfs, file2.c_str())); + EXPECT_FALSE(HasFile(&vfs, file3.c_str())); - mj_deleteVFS(vfs.get()); + mj_deleteVFS(&vfs); } -TEST_F(EngineVfsTest, AddBuffer) { - auto vfs = std::make_unique(); - mj_defaultVFS(vfs.get()); +TEST_F(UserVfsTest, AddFileStripPath) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file1 = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file1.c_str()); + + EXPECT_TRUE(HasFile(&vfs, file1)); + EXPECT_TRUE(HasFile(&vfs, dir + file1)); + EXPECT_TRUE(HasFile(&vfs, "some/dir/" + file1)); + EXPECT_TRUE(HasFile(&vfs, "some/dir\\" + file1)); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, AddFileRepeat) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file1 = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file1.c_str()); + + EXPECT_TRUE(HasFile(&vfs, file1)); + EXPECT_THAT(mj_addFileVFS(&vfs, "dir/", file1.c_str()), 2); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, DeleteFile) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file1 = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file1.c_str()); + + EXPECT_TRUE(HasFile(&vfs, file1)); + EXPECT_THAT(mj_deleteFileVFS(&vfs, file1.c_str()), 0); + EXPECT_FALSE(HasFile(&vfs, file1)); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, DeleteFileStripPath) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file = "activation.xml"; + std::string fileUpper = "Activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file.c_str()); + + EXPECT_TRUE(HasFile(&vfs, file)); + EXPECT_THAT(mj_deleteFileVFS(&vfs, ("dir\\" + fileUpper).c_str()), 0); + EXPECT_FALSE(HasFile(&vfs, file)); + + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, DeleteFileRepeat) { + mjVFS vfs; + mj_defaultVFS(&vfs); + + constexpr char path[] = "engine/testdata/actuation/"; + const std::string dir = GetTestDataFilePath(path); + std::string file = "activation.xml"; + mj_addFileVFS(&vfs, dir.c_str(), file.c_str()); + + EXPECT_TRUE(HasFile(&vfs, file)); + EXPECT_THAT(mj_deleteFileVFS(&vfs, file.c_str()), 0); + EXPECT_FALSE(HasFile(&vfs, file)); + EXPECT_THAT(mj_deleteFileVFS(&vfs, file.c_str()), -1); + + mj_deleteVFS(&vfs); +} + + +TEST_F(UserVfsTest, AddBuffer) { + mjVFS vfs; + mj_defaultVFS(&vfs); std::string buffer = ""; - mj_addBufferVFS(vfs.get(), "model", static_cast(buffer.c_str()), + mj_addBufferVFS(&vfs, "model", static_cast(buffer.c_str()), buffer.size()); std::array error; - mjModel* model = mj_loadXML("model", vfs.get(), error.data(), error.size()); + mjModel* model = mj_loadXML("model", &vfs, error.data(), error.size()); EXPECT_THAT(model, NotNull()); mj_deleteModel(model); - mj_deleteVFS(vfs.get()); + mj_deleteVFS(&vfs); } -TEST_F(EngineVfsTest, Timestamps) { +TEST_F(UserVfsTest, AddBufferRepeat) { + mjVFS vfs; + mj_defaultVFS(&vfs); + std::string buffer = ""; + const void* ptr = static_cast(buffer.c_str()); + mj_addBufferVFS(&vfs, "model", ptr, buffer.size()); + int result = mj_addBufferVFS(&vfs, "model", ptr, buffer.size()); + EXPECT_EQ(result, 2); + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, BufferStripPath) { + mjVFS vfs; + mj_defaultVFS(&vfs); + std::string buffer = ""; + const void* ptr = static_cast(buffer.c_str()); + mj_addBufferVFS(&vfs, "dir/model", ptr, buffer.size()); + EXPECT_TRUE(HasFile(&vfs, "MODEL")); + EXPECT_TRUE(HasFile(&vfs, "dir\\model")); + mj_deleteVFS(&vfs); +} + +TEST_F(UserVfsTest, Timestamps) { static constexpr char cube[] = R"( v -0.500000 -0.500000 0.500000 v 0.500000 -0.500000 0.500000 @@ -107,11 +218,11 @@ TEST_F(EngineVfsTest, Timestamps) { v -0.500000 -0.500000 -0.500000 v 0.500000 -0.500000 -0.500000)"; - auto vfs = std::make_unique(); - mj_defaultVFS(vfs.get()); - mj_addBufferVFS(vfs.get(), "cube.obj", cube, sizeof(cube)); + mjVFS vfs; + mj_defaultVFS(&vfs); + mj_addBufferVFS(&vfs, "cube.obj", cube, sizeof(cube)); - mjResource* resource = mju_openVfsResource("cube.obj", vfs.get()); + mjResource* resource = mju_openVfsResource("cube.obj", &vfs); // same timestamps EXPECT_EQ(mju_isModifiedResource(resource, resource->timestamp), 0); @@ -120,7 +231,7 @@ TEST_F(EngineVfsTest, Timestamps) { EXPECT_EQ(mju_isModifiedResource(resource, "QQ=="), 1); mju_closeResource(resource); - mj_deleteVFS(vfs.get()); + mj_deleteVFS(&vfs); } } // namespace diff --git a/test/xml/xml_api_test.cc b/test/xml/xml_api_test.cc index 8c5041a2..9b5b241c 100644 --- a/test/xml/xml_api_test.cc +++ b/test/xml/xml_api_test.cc @@ -124,8 +124,8 @@ TEST_F(MujocoTest, SaveXmlShortString) { std::array out; EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), - error.data(), error.size()), 272); - EXPECT_STREQ(error.data(), "Output string too short, should be at least 273"); + error.data(), error.size()), 273); + EXPECT_STREQ(error.data(), "Output string too short, should be at least 274"); mj_deleteSpec(spec); mj_deleteModel(model); @@ -139,7 +139,7 @@ TEST_F(MujocoTest, SaveXml) { mjModel* model = mj_compile(spec, 0); EXPECT_THAT(model, NotNull()) << "Failed to compile model: " << error.data(); - std::array out; + std::array out; EXPECT_THAT(mj_saveXMLString(spec, out.data(), out.size(), error.data(), error.size()), 0) << error.data(); diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 1d13f7d1..06a3268b 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -464,6 +464,39 @@ TEST_F(XMLReaderTest, InvalidDoubleOrientation) { } } +TEST_F(XMLReaderTest, ClassOverridesChildclass) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_EQ(model->geom_size[3*0], 2); + EXPECT_EQ(model->geom_size[3*1], 3); + EXPECT_EQ(model->geom_size[3*2], 2); + EXPECT_EQ(model->geom_size[3*3], 3); + mj_deleteModel(model); +} + TEST_F(XMLReaderTest, RepeatedDefaultName) { static constexpr char xml[] = R"( @@ -511,6 +544,45 @@ TEST_F(XMLReaderTest, InvalidDefaultClassName) { HasSubstr("Element 'geom'"), HasSubstr("line 10"))); } +TEST_F(XMLReaderTest, InvalidTopDefaultClassName) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()) << error.data(); + EXPECT_THAT(error.data(), + HasSubstr("top-level default class 'main' cannot be renamed")); +} + +TEST_F(XMLReaderTest, ValidTopDefaultClassName) { + static constexpr char xml[] = R"( + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + mj_deleteModel(model); +} + // ------------------------ test including ------------------------------------- // tiny RGB 2 x 3 PNG file @@ -834,6 +906,47 @@ TEST_F(XMLReaderTest, IncludeAbsoluteTest) { mj_deleteModel(model); } +TEST_F(XMLReaderTest, IncludeAbsoluteMeshDirTest) { + static constexpr char xml[] = R"( + + + + )"; + static constexpr char assets[] = R"( + + + + + + + )"; + + static constexpr char cube[] = R"( + v -0.500000 -0.500000 0.500000 + v 0.500000 -0.500000 0.500000 + v -0.500000 0.500000 0.500000 + v 0.500000 0.500000 0.500000 + v -0.500000 0.500000 -0.500000 + v 0.500000 0.500000 -0.500000 + v -0.500000 -0.500000 -0.500000 + v 0.500000 -0.500000 -0.500000)"; + + MockFilesystem fs("IncludeAbsoluteMeshDirTest"); + fs.AddFile("/assets/cube.obj", (const unsigned char*) cube, sizeof(cube)); + fs.AddFile("assets.xml", (const unsigned char*) assets, + sizeof(assets)); + fs.AddFile("model.xml", (const unsigned char*) xml, sizeof(xml)); + std::string modelpath = fs.FullPath("model.xml"); + + std::array error; + // loading the file should be successful + mjModel* model = mj_loadXML(modelpath.c_str(), nullptr, + error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mj_deleteModel(model); +} + TEST_F(XMLReaderTest, ParsePolycoef) { static constexpr char xml[] = R"( diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index bcb8c68b..96134397 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -727,8 +727,10 @@ TEST_F(XMLWriterTest, WritesActuatorDefaults) { TEST_F(XMLWriterTest, WritesFrameDefaults) { static constexpr char xml[] = R"( - - + + + + @@ -1302,9 +1304,13 @@ TEST_F(XMLWriterTest, WriteReadCompare) { ASSERT_THAT(error.data(), HasSubstr("file")) << error.data() << " from " << xml.c_str(); } else { - // for a particularly difficult example, relax the tolerance - mjtNum tol = - absl::StrContains(p.path().string(), "belt.xml") ? 1e-13 : 0; + mjtNum tol = 0; + + // for particularly sensitive models, relax the tolerance + if (absl::StrContains(p.path().string(), "belt.xml") || + absl::StrContains(p.path().string(), "cable.xml")) { + tol = 1e-13; + } // compare and delete std::string field = ""; diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 2e791f25..215834ba 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -41,8 +41,6 @@ public const double mjMINIMP = 0.0001; public const double mjMAXIMP = 0.9999; public const int mjMAXCONPAIR = 50; public const int mjMAXTREEDEPTH = 50; -public const int mjMAXVFS = 2000; -public const int mjMAXVFSNAME = 1000; public const int mjNEQDATA = 11; public const int mjNDYN = 10; public const int mjNGAIN = 10; @@ -52,16 +50,17 @@ public const int mjNREF = 2; public const int mjNIMP = 5; public const int mjNSOLVER = 200; public const int mjNISLAND = 20; +public const int mjNTEXMAT = 6; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJPLUGIN_H_ = true; public const bool mjEXTERNC = true; public const bool THIRD_PARTY_MUJOCO_MJRENDER_H_ = true; public const int mjNAUX = 10; -public const int mjMAXTEXTURE = 1000; +public const int mjMAXTEXTURE = 100; +public const int mjMAXMATERIAL = 100; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJSPEC_H_ = true; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTHREAD_H_ = true; public const int mjMAXTHREAD = 128; public const bool THIRD_PARTY_MUJOCO_INCLUDE_MJTNUM_H_ = true; -public const bool mjUSEDOUBLE = true; public const double mjMINVAL = 1e-15; public const bool THIRD_PARTY_MUJOCO_MJUI_H_ = true; public const int mjMAXUISECT = 10; @@ -72,6 +71,7 @@ public const int mjMAXUIMULTI = 35; public const int mjMAXUIEDIT = 7; public const int mjMAXUIRECT = 25; public const int mjSEPCLOSED = 1000; +public const int mjPRESERVE = 2000; public const int mjKEY_ESCAPE = 256; public const int mjKEY_ENTER = 257; public const int mjKEY_TAB = 258; @@ -483,6 +483,11 @@ public enum mjtEvent : int{ mjEVENT_REDRAW = 7, mjEVENT_FILESDROP = 8, } +public enum mjtSection : int{ + mjSECT_CLOSED = 0, + mjSECT_OPEN = 1, + mjSECT_FIXED = 2, +} public enum mjtCatBit : int{ mjCAT_STATIC = 1, mjCAT_DYNAMIC = 2, @@ -782,29 +787,6 @@ public unsafe struct mjData_ { public UIntPtr maxuse_arena; public int maxuse_con; public int maxuse_efc; - public mjWarningStat_ warning0; - public mjWarningStat_ warning1; - public mjWarningStat_ warning2; - public mjWarningStat_ warning3; - public mjWarningStat_ warning4; - public mjWarningStat_ warning5; - public mjWarningStat_ warning6; - public mjWarningStat_ warning7; - public mjTimerStat_ timer0; - public mjTimerStat_ timer1; - public mjTimerStat_ timer2; - public mjTimerStat_ timer3; - public mjTimerStat_ timer4; - public mjTimerStat_ timer5; - public mjTimerStat_ timer6; - public mjTimerStat_ timer7; - public mjTimerStat_ timer8; - public mjTimerStat_ timer9; - public mjTimerStat_ timer10; - public mjTimerStat_ timer11; - public mjTimerStat_ timer12; - public mjTimerStat_ timer13; - public mjTimerStat_ timer14; public mjSolverStat_ solver0; public mjSolverStat_ solver1; public mjSolverStat_ solver2; @@ -4809,6 +4791,29 @@ public unsafe struct mjData_ { public fixed int solver_niter[20]; public fixed int solver_nnz[20]; public fixed double solver_fwdinv[2]; + public mjWarningStat_ warning0; + public mjWarningStat_ warning1; + public mjWarningStat_ warning2; + public mjWarningStat_ warning3; + public mjWarningStat_ warning4; + public mjWarningStat_ warning5; + public mjWarningStat_ warning6; + public mjWarningStat_ warning7; + public mjTimerStat_ timer0; + public mjTimerStat_ timer1; + public mjTimerStat_ timer2; + public mjTimerStat_ timer3; + public mjTimerStat_ timer4; + public mjTimerStat_ timer5; + public mjTimerStat_ timer6; + public mjTimerStat_ timer7; + public mjTimerStat_ timer8; + public mjTimerStat_ timer9; + public mjTimerStat_ timer10; + public mjTimerStat_ timer11; + public mjTimerStat_ timer12; + public mjTimerStat_ timer13; + public mjTimerStat_ timer14; public int ne; public int nf; public int nl; @@ -4971,11 +4976,7 @@ public unsafe struct mjLROpt_ { [StructLayout(LayoutKind.Sequential)] public unsafe struct _mjVFS { - public int nfile; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000 * 1000)] public char[] filename; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UIntPtr[] filesize; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public IntPtr[] filedata; - [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2000)] public UInt64[] filestamp; + public void* impl_; } [StructLayout(LayoutKind.Sequential)] @@ -5629,6 +5630,9 @@ public unsafe struct mjrContext_ { public fixed uint auxFBO_r[10]; public fixed uint auxColor[10]; public fixed uint auxColor_r[10]; + public fixed int mat_texid[600]; + public fixed int mat_texuniform[100]; + public fixed int mat_texrepeat[200]; public int ntexture; public fixed int textureType[100]; public fixed uint texture[100]; @@ -5723,6 +5727,8 @@ public unsafe struct mjuiThemeSpacing_ { public int scroll; public int label; public int section; + public int cornersect; + public int cornersep; public int itemside; public int itemmid; public int itemver; @@ -5737,9 +5743,14 @@ public unsafe struct mjuiThemeColor_ { public fixed float master[3]; public fixed float thumb[3]; public fixed float secttitle[3]; + public fixed float secttitle2[3]; + public fixed float secttitlecheck[3]; + public fixed float secttitlecheck2[3]; public fixed float sectfont[3]; public fixed float sectsymbol[3]; public fixed float sectpane[3]; + public fixed float separator[3]; + public fixed float separator2[3]; public fixed float shortcut[3]; public fixed float fontactive[3]; public fixed float fontinactive[3]; @@ -5787,9 +5798,11 @@ public unsafe struct mjuiSection_ { public int state; public int modifier; public int shortcut; + public int checkbox; public int nitem; public mjrRect_ rtitle; public mjrRect_ rcontent; + public int lastclick; } [StructLayout(LayoutKind.Sequential)] @@ -5808,6 +5821,8 @@ public unsafe struct mjUI_ { public int mousesect; public int mouseitem; public int mousehelp; + public int mouseclicks; + public int mousesectcheck; public int editsect; public int edititem; public int editcursor; @@ -5834,6 +5849,7 @@ public unsafe struct mjuiDef_ { public int state; public void* pdata; public fixed sbyte other[300]; + public int otherint; } [StructLayout(LayoutKind.Sequential)] @@ -5884,11 +5900,9 @@ public unsafe struct mjvGeom_ { public int objtype; public int objid; public int category; - public int texid; - public int texuniform; + public int matid; public int texcoord; public int segid; - public fixed float texrepeat[2]; public fixed float size[3]; public fixed float pos[3]; public fixed float mat[9]; @@ -6377,18 +6391,12 @@ public static unsafe extern int mj_addFileVFS(void* vfs, [MarshalAs(UnmanagedTyp [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_addBufferVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string name, void* buffer, int nbuffer); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern int mj_findFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern int mj_deleteFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_deleteVFS(void* vfs); -[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] -public static unsafe extern int mj_makeEmptyFileVFS(void* vfs, [MarshalAs(UnmanagedType.LPStr)]string filename, int filesize); - [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern mjModel_* mj_loadXML([MarshalAs(UnmanagedType.LPStr)]string filename, void* vfs, StringBuilder error, int error_sz); diff --git a/unity/Runtime/Tools/MjVfs.cs b/unity/Runtime/Tools/MjVfs.cs index a52da4b2..802d53f9 100644 --- a/unity/Runtime/Tools/MjVfs.cs +++ b/unity/Runtime/Tools/MjVfs.cs @@ -40,11 +40,6 @@ public sealed class MjVfs : IDisposable { } } - // Number of files added to the filesystem. - public int FilesCount { - get { return Data.nfile; } - } - // Adds a new file to the virtual filesystem. public unsafe void AddFile(string filename, string contents) { var contents_bytes = Encoding.UTF8.GetBytes(contents); @@ -59,20 +54,8 @@ public sealed class MjVfs : IDisposable { } } - // Searches the VFS for the specified file and returns its index. - // The index then can be used to retrieve the file contents from Data.filedata array. - public unsafe int FindFile(string filename) { - return mj_findFileVFS(_unmanagedVfs.ToPointer(), filename); - } - // Loads a model from the specified file. - // The file is assumed to be located in the filesystem. If it's not found, the method will throw - // an ArgumentException. public unsafe MujocoLib.mjModel_* LoadXML(string filename) { - if (FindFile(filename) < 0) { - throw new ArgumentException($"File {filename} was not added to the VFS."); - } - var errorBuf = new StringBuilder(1024); MujocoLib.mjModel_* model = MujocoLib.mj_loadXML( filename, _unmanagedVfs.ToPointer(), errorBuf, errorBuf.Capacity); diff --git a/unity/Tests/Editor/Tools/MjVFSEditorTests.cs b/unity/Tests/Editor/Tools/MjVFSEditorTests.cs index b43ea404..238a494b 100644 --- a/unity/Tests/Editor/Tools/MjVFSEditorTests.cs +++ b/unity/Tests/Editor/Tools/MjVFSEditorTests.cs @@ -40,14 +40,7 @@ namespace Mujoco { var filename = "filename"; var contents = "contents"; _vfs.AddFile(filename, contents); - Assert.That(_vfs.FilesCount, Is.EqualTo(1)); - } - - [Test] - public unsafe void LocatingFileUsingMujocoLib() { - var filename = "filename"; - _vfs.AddFile(filename, "contents"); - Assert.That(_vfs.FindFile(filename), Is.EqualTo(0)); + Assert.That(() => { _vfs.AddFile(filename, contents); }, Throws.Exception); // duplicate file } } }