From 556001035b6aefb28ed277a20f538d083669b3e6 Mon Sep 17 00:00:00 2001 From: kanarus Date: Sat, 8 Nov 2025 10:58:34 +0900 Subject: [PATCH 1/3] refactor: vendor MuJoCo's `include` directory in rusty_mujoco --- .github/workflows/CI.yml | 1 + Cargo.toml | 4 +- LICENSE | 201 ++- README.md | 2 +- build.rs | 12 +- vendor/include/mujoco/mjdata.h | 472 +++++++ vendor/include/mujoco/mjexport.h | 47 + vendor/include/mujoco/mjmacro.h | 38 + vendor/include/mujoco/mjmodel.h | 1169 +++++++++++++++++ vendor/include/mujoco/mjplugin.h | 175 +++ vendor/include/mujoco/mjrender.h | 177 +++ vendor/include/mujoco/mjsan.h | 58 + vendor/include/mujoco/mjspec.h | 787 ++++++++++++ vendor/include/mujoco/mjthread.h | 42 + vendor/include/mujoco/mjtnum.h | 37 + vendor/include/mujoco/mjui.h | 345 +++++ vendor/include/mujoco/mjvisualize.h | 694 ++++++++++ vendor/include/mujoco/mjxmacro.h | 814 ++++++++++++ vendor/include/mujoco/mujoco.h | 1806 +++++++++++++++++++++++++++ 19 files changed, 6847 insertions(+), 34 deletions(-) create mode 100644 vendor/include/mujoco/mjdata.h create mode 100644 vendor/include/mujoco/mjexport.h create mode 100644 vendor/include/mujoco/mjmacro.h create mode 100644 vendor/include/mujoco/mjmodel.h create mode 100644 vendor/include/mujoco/mjplugin.h create mode 100644 vendor/include/mujoco/mjrender.h create mode 100644 vendor/include/mujoco/mjsan.h create mode 100644 vendor/include/mujoco/mjspec.h create mode 100644 vendor/include/mujoco/mjthread.h create mode 100644 vendor/include/mujoco/mjtnum.h create mode 100644 vendor/include/mujoco/mjui.h create mode 100644 vendor/include/mujoco/mjvisualize.h create mode 100644 vendor/include/mujoco/mjxmacro.h create mode 100644 vendor/include/mujoco/mujoco.h diff --git a/.github/workflows/CI.yml b/.github/workflows/CI.yml index 51032ae..dc21793 100644 --- a/.github/workflows/CI.yml +++ b/.github/workflows/CI.yml @@ -52,6 +52,7 @@ jobs: run: | cargo build cargo build --features bindgen + git diff --exit-code ./src/bindgen.rs || (echo "bindgen.rs changed after build with bindgen feature."; exit 1) test: strategy: diff --git a/Cargo.toml b/Cargo.toml index 899390b..758f3d7 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,13 +1,13 @@ [package] name = "rusty_mujoco" -version = "0.1.1" +version = "0.2.0" edition = "2024" authors = ["kanarus "] documentation = "https://docs.rs/rusty_mujoco" homepage = "https://crates.io/crates/rusty_mujoco" repository = "https://github.com/rust-control/rusty_mujoco" readme = "README.md" -license = "MIT" +license = "Apache-2.0" description = "Rust bindings for the MuJoCo physics simulator" keywords = ["mujoco", "rl", "ml", "physics", "robotics"] categories = ["api-bindings", "science::robotics", "simulation"] diff --git a/LICENSE b/LICENSE index f434fac..d9a10c0 100644 --- a/LICENSE +++ b/LICENSE @@ -1,25 +1,176 @@ -Copyright (c) 2025 kanarus - -Permission is hereby granted, free of charge, to any -person obtaining a copy of this software and associated -documentation files (the "Software"), to deal in the -Software without restriction, including without -limitation the rights to use, copy, modify, merge, -publish, distribute, sublicense, and/or sell copies of -the Software, and to permit persons to whom the Software -is furnished to do so, subject to the following -conditions: - -The above copyright notice and this permission notice -shall be included in all copies or substantial portions -of the Software. - -THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF -ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED -TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A -PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT -SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY -CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION -OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR -IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER -DEALINGS IN THE SOFTWARE. \ No newline at end of file + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS diff --git a/README.md b/README.md index 99adf9f..0a182b8 100644 --- a/README.md +++ b/README.md @@ -133,4 +133,4 @@ and [examples/README.md](./examples/README.md) for the description. ## License -rusty_mujoco is licensed under [MIT LICENSE](https://github.com/rust-control/rusty_mujoco/blob/main/LICENSE). +rusty_mujoco is licensed under [Apache Lisense, Version 2.0)](https://github.com/rust-control/rusty_mujoco/blob/main/LICENSE). diff --git a/build.rs b/build.rs index f8cff2b..1d110d3 100644 --- a/build.rs +++ b/build.rs @@ -9,11 +9,11 @@ fn main() { println!("cargo:rustc-link-lib=dylib=mujoco"); #[cfg(feature = "bindgen")] - bindgen(mujoco_dir); + bindgen(); } #[cfg(feature = "bindgen")] -fn bindgen(mujoco_dir: impl AsRef) { +fn bindgen() { #[derive(Debug)] struct TrimUnderscoreCallbacks; impl bindgen::callbacks::ParseCallbacks for TrimUnderscoreCallbacks { @@ -61,9 +61,9 @@ fn bindgen(mujoco_dir: impl AsRef) { "`cargo fmt` is not available; This build script can't continue without it." ); - let mujoco_dir = mujoco_dir.as_ref(); - let mujoco_include = mujoco_dir.join("include").to_str().unwrap().to_owned(); - let mujoco_include_mujoco = mujoco_dir.join("include").join("mujoco").to_str().unwrap().to_owned(); + let vendor_dir = std::path::Path::new(env!("CARGO_MANIFEST_DIR")).join("vendor"); + let vendor_include = vendor_dir.join("include").to_str().unwrap().to_owned(); + let vendor_include_mujoco = vendor_dir.join("include").join("mujoco").to_str().unwrap().to_owned(); let src_dir = std::path::Path::new(env!("CARGO_MANIFEST_DIR")).join("src"); let bindgen_h = src_dir.join("bindgen.h").to_str().unwrap().to_owned(); @@ -74,7 +74,7 @@ fn bindgen(mujoco_dir: impl AsRef) { let mut bindings = Vec::new(); bindgen::builder() .header(bindgen_h) - .clang_args([format!("-I{mujoco_include}"), format!("-I{mujoco_include_mujoco}")]) + .clang_args([format!("-I{vendor_include}"), format!("-I{vendor_include_mujoco}")]) .use_core() .raw_line("#![allow(unused, non_camel_case_types, non_snake_case, non_upper_case_globals)]") .respect_cxx_access_specs(false) diff --git a/vendor/include/mujoco/mjdata.h b/vendor/include/mujoco/mjdata.h new file mode 100644 index 0000000..d6f9877 --- /dev/null +++ b/vendor/include/mujoco/mjdata.h @@ -0,0 +1,472 @@ +// 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_MJDATA_H_ +#define MUJOCO_MJDATA_H_ + +#include +#include + +#include +#include +#include + +//---------------------------------- primitive types (mjt) ----------------------------------------- + +typedef enum mjtState_ { // state elements + mjSTATE_TIME = 1<<0, // time + mjSTATE_QPOS = 1<<1, // position + mjSTATE_QVEL = 1<<2, // velocity + mjSTATE_ACT = 1<<3, // actuator activation + mjSTATE_WARMSTART = 1<<4, // acceleration used for warmstart + mjSTATE_CTRL = 1<<5, // control + mjSTATE_QFRC_APPLIED = 1<<6, // applied generalized force + mjSTATE_XFRC_APPLIED = 1<<7, // applied Cartesian force/torque + mjSTATE_EQ_ACTIVE = 1<<8, // enable/disable constraints + mjSTATE_MOCAP_POS = 1<<9, // positions of mocap bodies + mjSTATE_MOCAP_QUAT = 1<<10, // orientations of mocap bodies + mjSTATE_USERDATA = 1<<11, // user data + mjSTATE_PLUGIN = 1<<12, // plugin state + + mjNSTATE = 13, // number of state elements + + // convenience values for commonly used state specifications + mjSTATE_PHYSICS = mjSTATE_QPOS | mjSTATE_QVEL | mjSTATE_ACT, + mjSTATE_FULLPHYSICS = mjSTATE_TIME | mjSTATE_PHYSICS | mjSTATE_PLUGIN, + mjSTATE_USER = mjSTATE_CTRL | mjSTATE_QFRC_APPLIED | mjSTATE_XFRC_APPLIED | + mjSTATE_EQ_ACTIVE | mjSTATE_MOCAP_POS | mjSTATE_MOCAP_QUAT | + mjSTATE_USERDATA, + mjSTATE_INTEGRATION = mjSTATE_FULLPHYSICS | mjSTATE_USER | mjSTATE_WARMSTART +} mjtState; + + +typedef enum mjtWarning_ { // warning types + mjWARN_INERTIA = 0, // (near) singular inertia matrix + mjWARN_CONTACTFULL, // too many contacts in contact list + mjWARN_CNSTRFULL, // too many constraints + mjWARN_VGEOMFULL, // too many visual geoms + mjWARN_BADQPOS, // bad number in qpos + mjWARN_BADQVEL, // bad number in qvel + mjWARN_BADQACC, // bad number in qacc + mjWARN_BADCTRL, // bad number in ctrl + + mjNWARNING // number of warnings +} mjtWarning; + + +typedef enum mjtTimer_ { // internal timers + // main api + mjTIMER_STEP = 0, // step + mjTIMER_FORWARD, // forward + mjTIMER_INVERSE, // inverse + + // breakdown of step/forward + mjTIMER_POSITION, // fwdPosition + mjTIMER_VELOCITY, // fwdVelocity + mjTIMER_ACTUATION, // fwdActuation + mjTIMER_CONSTRAINT, // fwdConstraint + mjTIMER_ADVANCE, // mj_Euler, mj_implicit + + // breakdown of fwdPosition + mjTIMER_POS_KINEMATICS, // kinematics, com, tendon, transmission + mjTIMER_POS_INERTIA, // inertia computations + mjTIMER_POS_COLLISION, // collision detection + mjTIMER_POS_MAKE, // make constraints + mjTIMER_POS_PROJECT, // project constraints + + // breakdown of mj_collision + mjTIMER_COL_BROAD, // broadphase + mjTIMER_COL_NARROW, // narrowphase + + mjNTIMER // number of timers +} mjtTimer; + + +//---------------------------------- mjContact ----------------------------------------------------- + +struct mjContact_ { // result of collision detection functions + // contact parameters set by near-phase collision function + mjtNum dist; // distance between nearest points; neg: penetration + mjtNum pos[3]; // position of contact point: midpoint between geoms + mjtNum frame[9]; // normal is in [0-2], points from geom[0] to geom[1] + + // contact parameters set by mj_collideGeoms + mjtNum includemargin; // include if distplugin, required for deletion (nplugin x 1) + uintptr_t* plugin_data; // pointer to plugin-managed data structure (nplugin x 1) + + //-------------------- POSITION dependent + + // computed by mj_fwdPosition/mj_kinematics + mjtNum* xpos; // Cartesian position of body frame (nbody x 3) + mjtNum* xquat; // Cartesian orientation of body frame (nbody x 4) + mjtNum* xmat; // Cartesian orientation of body frame (nbody x 9) + mjtNum* xipos; // Cartesian position of body com (nbody x 3) + mjtNum* ximat; // Cartesian orientation of body inertia (nbody x 9) + mjtNum* xanchor; // Cartesian position of joint anchor (njnt x 3) + mjtNum* xaxis; // Cartesian joint axis (njnt x 3) + mjtNum* geom_xpos; // Cartesian geom position (ngeom x 3) + mjtNum* geom_xmat; // Cartesian geom orientation (ngeom x 9) + mjtNum* site_xpos; // Cartesian site position (nsite x 3) + mjtNum* site_xmat; // Cartesian site orientation (nsite x 9) + mjtNum* cam_xpos; // Cartesian camera position (ncam x 3) + mjtNum* cam_xmat; // Cartesian camera orientation (ncam x 9) + mjtNum* light_xpos; // Cartesian light position (nlight x 3) + mjtNum* light_xdir; // Cartesian light direction (nlight x 3) + + // computed by mj_fwdPosition/mj_comPos + mjtNum* subtree_com; // center of mass of each subtree (nbody x 3) + mjtNum* cdof; // com-based motion axis of each dof (rot:lin) (nv x 6) + mjtNum* cinert; // com-based body inertia and mass (nbody x 10) + + // computed by mj_fwdPosition/mj_flex + mjtNum* flexvert_xpos; // Cartesian flex vertex positions (nflexvert x 3) + mjtNum* flexelem_aabb; // flex element bounding boxes (center, size) (nflexelem x 6) + int* flexedge_J_rownnz; // number of non-zeros in Jacobian row (nflexedge x 1) + int* flexedge_J_rowadr; // row start address in colind array (nflexedge x 1) + int* flexedge_J_colind; // column indices in sparse Jacobian (nflexedge x nv) + mjtNum* flexedge_J; // flex edge Jacobian (nflexedge x nv) + mjtNum* flexedge_length; // flex edge lengths (nflexedge x 1) + + // computed by mj_fwdPosition/mj_tendon + int* ten_wrapadr; // start address of tendon's path (ntendon x 1) + int* ten_wrapnum; // number of wrap points in path (ntendon x 1) + int* ten_J_rownnz; // number of non-zeros in Jacobian row (ntendon x 1) + int* ten_J_rowadr; // row start address in colind array (ntendon x 1) + int* ten_J_colind; // column indices in sparse Jacobian (ntendon x nv) + mjtNum* ten_J; // tendon Jacobian (ntendon x nv) + mjtNum* ten_length; // tendon lengths (ntendon x 1) + int* wrap_obj; // geom id; -1: site; -2: pulley (nwrap x 2) + mjtNum* wrap_xpos; // Cartesian 3D points in all paths (nwrap x 6) + + // computed by mj_fwdPosition/mj_transmission + mjtNum* actuator_length; // actuator lengths (nu x 1) + int* moment_rownnz; // number of non-zeros in actuator_moment row (nu x 1) + int* moment_rowadr; // row start address in colind array (nu x 1) + int* moment_colind; // column indices in sparse Jacobian (nJmom x 1) + mjtNum* actuator_moment; // actuator moments (nJmom x 1) + + // computed by mj_fwdPosition/mj_crb + mjtNum* crb; // com-based composite inertia and mass (nbody x 10) + mjtNum* qM; // total inertia (sparse) (nM x 1) + + // computed by mj_fwdPosition/mj_factorM + mjtNum* qLD; // L'*D*L factorization of M (sparse) (nM x 1) + mjtNum* qLDiagInv; // 1/diag(D) (nv x 1) + + // computed by mj_collisionTree + mjtNum* bvh_aabb_dyn; // global bounding box (center, size) (nbvhdynamic x 6) + mjtByte* bvh_active; // was bounding volume checked for collision (nbvh x 1) + + //-------------------- POSITION, VELOCITY dependent + + // computed by mj_fwdVelocity + mjtNum* flexedge_velocity; // flex edge velocities (nflexedge x 1) + mjtNum* ten_velocity; // tendon velocities (ntendon x 1) + mjtNum* actuator_velocity; // actuator velocities (nu x 1) + + // computed by mj_fwdVelocity/mj_comVel + mjtNum* cvel; // com-based velocity (rot:lin) (nbody x 6) + mjtNum* cdof_dot; // time-derivative of cdof (rot:lin) (nv x 6) + + // computed by mj_fwdVelocity/mj_rne (without acceleration) + mjtNum* qfrc_bias; // C(qpos,qvel) (nv x 1) + + // computed by mj_fwdVelocity/mj_passive + mjtNum* qfrc_spring; // passive spring force (nv x 1) + mjtNum* qfrc_damper; // passive damper force (nv x 1) + mjtNum* qfrc_gravcomp; // passive gravity compensation force (nv x 1) + mjtNum* qfrc_fluid; // passive fluid force (nv x 1) + mjtNum* qfrc_passive; // total passive force (nv x 1) + + // computed by mj_sensorVel/mj_subtreeVel if needed + mjtNum* subtree_linvel; // linear velocity of subtree com (nbody x 3) + mjtNum* subtree_angmom; // angular momentum about subtree com (nbody x 3) + + // computed by mj_Euler or mj_implicit + mjtNum* qH; // L'*D*L factorization of modified M (nM x 1) + mjtNum* qHDiagInv; // 1/diag(D) of modified M (nv x 1) + + // computed by mj_resetData + int* B_rownnz; // body-dof: non-zeros in each row (nbody x 1) + int* B_rowadr; // body-dof: address of each row in B_colind (nbody x 1) + int* B_colind; // body-dof: column indices of non-zeros (nB x 1) + int* M_rownnz; // inertia: non-zeros in each row (nv x 1) + int* M_rowadr; // inertia: address of each row in M_colind (nv x 1) + int* M_colind; // inertia: column indices of non-zeros (nM x 1) + int* mapM2M; // index mapping from M (legacy) to M (CSR) (nM x 1) + int* C_rownnz; // reduced dof-dof: non-zeros in each row (nv x 1) + int* C_rowadr; // reduced dof-dof: address of each row in C_colind (nv x 1) + int* C_colind; // reduced dof-dof: column indices of non-zeros (nC x 1) + int* mapM2C; // index mapping from M to C (nC x 1) + int* D_rownnz; // dof-dof: non-zeros in each row (nv x 1) + int* D_rowadr; // dof-dof: address of each row in D_colind (nv x 1) + int* D_diag; // dof-dof: index of diagonal element (nv x 1) + int* D_colind; // dof-dof: column indices of non-zeros (nD x 1) + int* mapM2D; // index mapping from M to D (nD x 1) + int* mapD2M; // index mapping from D to M (nM x 1) + + // computed by mj_implicit/mj_derivative + mjtNum* qDeriv; // d (passive + actuator - bias) / d qvel (nD x 1) + + // computed by mj_implicit/mju_factorLUSparse + mjtNum* qLU; // sparse LU of (qM - dt*qDeriv) (nD x 1) + + //-------------------- POSITION, VELOCITY, CONTROL/ACCELERATION dependent + + // computed by mj_fwdActuation + mjtNum* actuator_force; // actuator force in actuation space (nu x 1) + mjtNum* qfrc_actuator; // actuator force (nv x 1) + + // computed by mj_fwdAcceleration + mjtNum* qfrc_smooth; // net unconstrained force (nv x 1) + mjtNum* qacc_smooth; // unconstrained acceleration (nv x 1) + + // computed by mj_fwdConstraint/mj_inverse + mjtNum* qfrc_constraint; // constraint force (nv x 1) + + // computed by mj_inverse + mjtNum* qfrc_inverse; // net external force; should equal: + // qfrc_applied + J'*xfrc_applied + qfrc_actuator (nv x 1) + + // computed by mj_sensorAcc/mj_rnePostConstraint if needed; rotation:translation format + mjtNum* cacc; // com-based acceleration (nbody x 6) + mjtNum* cfrc_int; // com-based interaction force with parent (nbody x 6) + mjtNum* cfrc_ext; // com-based external force on body (nbody x 6) + + //-------------------- arena-allocated: POSITION dependent + + // computed by mj_collision + mjContact* contact; // array of all detected contacts (ncon x 1) + + // computed by mj_makeConstraint + int* efc_type; // constraint type (mjtConstraint) (nefc x 1) + int* efc_id; // id of object of specified type (nefc x 1) + int* efc_J_rownnz; // number of non-zeros in constraint Jacobian row (nefc x 1) + int* efc_J_rowadr; // row start address in colind array (nefc x 1) + int* efc_J_rowsuper; // number of subsequent rows in supernode (nefc x 1) + int* efc_J_colind; // column indices in constraint Jacobian (nJ x 1) + int* efc_JT_rownnz; // number of non-zeros in constraint Jacobian row T (nv x 1) + int* efc_JT_rowadr; // row start address in colind array T (nv x 1) + int* efc_JT_rowsuper; // number of subsequent rows in supernode T (nv x 1) + int* efc_JT_colind; // column indices in constraint Jacobian T (nJ x 1) + mjtNum* efc_J; // constraint Jacobian (nJ x 1) + mjtNum* efc_JT; // constraint Jacobian transposed (nJ x 1) + mjtNum* efc_pos; // constraint position (equality, contact) (nefc x 1) + mjtNum* efc_margin; // inclusion margin (contact) (nefc x 1) + mjtNum* efc_frictionloss; // frictionloss (friction) (nefc x 1) + mjtNum* efc_diagApprox; // approximation to diagonal of A (nefc x 1) + mjtNum* efc_KBIP; // stiffness, damping, impedance, imp' (nefc x 4) + mjtNum* efc_D; // constraint mass (nefc x 1) + mjtNum* efc_R; // inverse constraint mass (nefc x 1) + int* tendon_efcadr; // first efc address involving tendon; -1: none (ntendon x 1) + + // computed by mj_island + int* dof_island; // island id of this dof; -1: none (nv x 1) + int* island_dofnum; // number of dofs in island (nisland x 1) + int* island_dofadr; // start address in island_dofind (nisland x 1) + int* island_dofind; // island dof indices; -1: none (nv x 1) + int* dof_islandind; // dof island indices; -1: none (nv x 1) + int* efc_island; // island id of this constraint (nefc x 1) + int* island_efcnum; // number of constraints in island (nisland x 1) + int* island_efcadr; // start address in island_efcind (nisland x 1) + int* island_efcind; // island constraint indices (nefc x 1) + + // computed by mj_projectConstraint (PGS solver) + int* efc_AR_rownnz; // number of non-zeros in AR (nefc x 1) + int* efc_AR_rowadr; // row start address in colind array (nefc x 1) + int* efc_AR_colind; // column indices in sparse AR (nA x 1) + mjtNum* efc_AR; // J*inv(M)*J' + R (nA x 1) + + //-------------------- arena-allocated: POSITION, VELOCITY dependent + + // computed by mj_fwdVelocity/mj_referenceConstraint + mjtNum* efc_vel; // velocity in constraint space: J*qvel (nefc x 1) + mjtNum* efc_aref; // reference pseudo-acceleration (nefc x 1) + + //-------------------- 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) + + // thread pool pointer + uintptr_t threadpool; + + // compilation signature + uint64_t signature; // also held by the mjSpec that compiled the model +}; +typedef struct mjData_ mjData; + + +//---------------------------------- callback function types --------------------------------------- + +// generic MuJoCo function +typedef void (*mjfGeneric)(const mjModel* m, mjData* d); + +// contact filter: 1- discard, 0- collide +typedef int (*mjfConFilt)(const mjModel* m, mjData* d, int geom1, int geom2); + +// sensor simulation +typedef void (*mjfSensor)(const mjModel* m, mjData* d, int stage); + +// timer +typedef mjtNum (*mjfTime)(void); + +// actuator dynamics, gain, bias +typedef mjtNum (*mjfAct)(const mjModel* m, const mjData* d, int id); + +// collision detection +typedef int (*mjfCollision)(const mjModel* m, const mjData* d, + mjContact* con, int g1, int g2, mjtNum margin); + +#endif // MUJOCO_MJDATA_H_ diff --git a/vendor/include/mujoco/mjexport.h b/vendor/include/mujoco/mjexport.h new file mode 100644 index 0000000..1194e03 --- /dev/null +++ b/vendor/include/mujoco/mjexport.h @@ -0,0 +1,47 @@ +// 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_MJEXPORT_H_ +#define MUJOCO_MJEXPORT_H_ + +#if defined _WIN32 || defined __CYGWIN__ + #define MUJOCO_HELPER_DLL_IMPORT __declspec(dllimport) + #define MUJOCO_HELPER_DLL_EXPORT __declspec(dllexport) + #define MUJOCO_HELPER_DLL_LOCAL +#else + #if __GNUC__ >= 4 + #define MUJOCO_HELPER_DLL_IMPORT __attribute__ ((visibility ("default"))) + #define MUJOCO_HELPER_DLL_EXPORT __attribute__ ((visibility ("default"))) + #define MUJOCO_HELPER_DLL_LOCAL __attribute__ ((visibility ("hidden"))) + #else + #define MUJOCO_HELPER_DLL_IMPORT + #define MUJOCO_HELPER_DLL_EXPORT + #define MUJOCO_HELPER_DLL_LOCAL + #endif +#endif + +#ifdef MJ_STATIC + // static library + #define MJAPI + #define MJLOCAL +#else + #ifdef MUJOCO_DLL_EXPORTS + #define MJAPI MUJOCO_HELPER_DLL_EXPORT + #else + #define MJAPI MUJOCO_HELPER_DLL_IMPORT + #endif + #define MJLOCAL MUJOCO_HELPER_DLL_LOCAL +#endif + +#endif // MUJOCO_MJEXPORT_H_ diff --git a/vendor/include/mujoco/mjmacro.h b/vendor/include/mujoco/mjmacro.h new file mode 100644 index 0000000..a50c117 --- /dev/null +++ b/vendor/include/mujoco/mjmacro.h @@ -0,0 +1,38 @@ +// Copyright 2023 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_MJMACRO_H_ +#define MUJOCO_MJMACRO_H_ + +// max and min (use only for primitive types) +#define mjMAX(a, b) (((a) > (b)) ? (a) : (b)) +#define mjMIN(a, b) (((a) < (b)) ? (a) : (b)) + +// return current value of mjOption enable/disable flags +#define mjDISABLED(x) (m->opt.disableflags & (x)) +#define mjENABLED(x) (m->opt.enableflags & (x)) + +// is actuator disabled +#define mjACTUATORDISABLED(i) (m->opt.disableactuator & (1 << m->actuator_group[i])) + +// annotation for functions that accept printf-like variadic arguments +#ifndef mjPRINTFLIKE + #if defined(__GNUC__) + #define mjPRINTFLIKE(n, m) __attribute__((format(printf, n, m))) + #else + #define mjPRINTFLIKE(n, m) + #endif +#endif + +#endif // MUJOCO_MJMACRO_H_ diff --git a/vendor/include/mujoco/mjmodel.h b/vendor/include/mujoco/mjmodel.h new file mode 100644 index 0000000..b6bdbfc --- /dev/null +++ b/vendor/include/mujoco/mjmodel.h @@ -0,0 +1,1169 @@ +// 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_MJMODEL_H_ +#define MUJOCO_MJMODEL_H_ + +#include +#include + + +#include + +// global constants +#define mjPI 3.14159265358979323846 +#define mjMAXVAL 1E+10 // maximum value in qpos, qvel, qacc +#define mjMINMU 1E-5 // minimum friction coefficient +#define mjMINIMP 0.0001 // minimum constraint impedance +#define mjMAXIMP 0.9999 // maximum constraint impedance +#define mjMAXCONPAIR 50 // maximum number of contacts per geom pair +#define mjMAXTREEDEPTH 50 // maximum bounding volume hierarchy depth +#define mjMAXFLEXNODES 27 // maximum number of flex nodes + + +//---------------------------------- sizes --------------------------------------------------------- + +#define mjNEQDATA 11 // number of eq_data fields +#define mjNDYN 10 // number of actuator dynamics parameters +#define mjNGAIN 10 // number of actuator gain parameters +#define mjNBIAS 10 // number of actuator bias parameters +#define mjNFLUID 12 // number of fluid interaction parameters +#define mjNREF 2 // number of solver reference parameters +#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 + + +//---------------------------------- enum types (mjt) ---------------------------------------------- + +typedef enum mjtDisableBit_ { // disable default feature bitflags + mjDSBL_CONSTRAINT = 1<<0, // entire constraint solver + mjDSBL_EQUALITY = 1<<1, // equality constraints + mjDSBL_FRICTIONLOSS = 1<<2, // joint and tendon frictionloss constraints + mjDSBL_LIMIT = 1<<3, // joint and tendon limit constraints + mjDSBL_CONTACT = 1<<4, // contact constraints + mjDSBL_PASSIVE = 1<<5, // passive forces + mjDSBL_GRAVITY = 1<<6, // gravitational forces + mjDSBL_CLAMPCTRL = 1<<7, // clamp control to specified range + mjDSBL_WARMSTART = 1<<8, // warmstart constraint solver + mjDSBL_FILTERPARENT = 1<<9, // remove collisions with parent body + mjDSBL_ACTUATION = 1<<10, // apply actuation forces + mjDSBL_REFSAFE = 1<<11, // integrator safety: make ref[0]>=2*timestep + mjDSBL_SENSOR = 1<<12, // sensors + mjDSBL_MIDPHASE = 1<<13, // mid-phase collision filtering + mjDSBL_EULERDAMP = 1<<14, // implicit integration of joint damping in Euler integrator + mjDSBL_AUTORESET = 1<<15, // automatic reset when numerical issues are detected + mjDSBL_NATIVECCD = 1<<16, // native convex collision detection + + mjNDISABLE = 17 // number of disable flags +} mjtDisableBit; + + +typedef enum mjtEnableBit_ { // enable optional feature bitflags + mjENBL_OVERRIDE = 1<<0, // override contact parameters + mjENBL_ENERGY = 1<<1, // energy computation + mjENBL_FWDINV = 1<<2, // record solver statistics + mjENBL_INVDISCRETE = 1<<3, // discrete-time inverse dynamics + // experimental features: + mjENBL_MULTICCD = 1<<4, // multi-point convex collision detection + mjENBL_ISLAND = 1<<5, // constraint island discovery + + mjNENABLE = 6 // number of enable flags +} mjtEnableBit; + + +typedef enum mjtJoint_ { // type of degree of freedom + mjJNT_FREE = 0, // global position and orientation (quat) (7) + mjJNT_BALL, // orientation (quat) relative to parent (4) + mjJNT_SLIDE, // sliding distance along body-fixed axis (1) + mjJNT_HINGE // rotation angle (rad) around body-fixed axis (1) +} mjtJoint; + + +typedef enum mjtGeom_ { // type of geometric shape + // regular geom types + mjGEOM_PLANE = 0, // plane + mjGEOM_HFIELD, // height field + mjGEOM_SPHERE, // sphere + mjGEOM_CAPSULE, // capsule + mjGEOM_ELLIPSOID, // ellipsoid + mjGEOM_CYLINDER, // cylinder + mjGEOM_BOX, // box + mjGEOM_MESH, // mesh + mjGEOM_SDF, // signed distance field + + mjNGEOMTYPES, // number of regular geom types + + // rendering-only geom types: not used in mjModel, not counted in mjNGEOMTYPES + mjGEOM_ARROW = 100, // arrow + mjGEOM_ARROW1, // arrow without wedges + mjGEOM_ARROW2, // arrow in both directions + mjGEOM_LINE, // line + mjGEOM_LINEBOX, // box with line edges + mjGEOM_FLEX, // flex + mjGEOM_SKIN, // skin + mjGEOM_LABEL, // text label + mjGEOM_TRIANGLE, // triangle + + mjGEOM_NONE = 1001 // missing geom type +} mjtGeom; + + +typedef enum mjtCamLight_ { // tracking mode for camera and light + mjCAMLIGHT_FIXED = 0, // pos and rot fixed in body + mjCAMLIGHT_TRACK, // pos tracks body, rot fixed in global + mjCAMLIGHT_TRACKCOM, // pos tracks subtree com, rot fixed in body + mjCAMLIGHT_TARGETBODY, // pos fixed in body, rot tracks target body + mjCAMLIGHT_TARGETBODYCOM // pos fixed in body, rot tracks target subtree com +} mjtCamLight; + + +typedef enum mjtTexture_ { // type of texture + mjTEXTURE_2D = 0, // 2d texture, suitable for planes and hfields + mjTEXTURE_CUBE, // cube texture, suitable for all other geom types + mjTEXTURE_SKYBOX // cube texture used as skybox +} mjtTexture; + + +typedef enum mjtTextureRole_ { // role of texture map in rendering + mjTEXROLE_USER = 0, // unspecified + mjTEXROLE_RGB, // base color (albedo) + mjTEXROLE_OCCLUSION, // ambient occlusion + mjTEXROLE_ROUGHNESS, // roughness + mjTEXROLE_METALLIC, // metallic + mjTEXROLE_NORMAL, // normal (bump) map + mjTEXROLE_OPACITY, // transperancy + mjTEXROLE_EMISSIVE, // light emission + mjTEXROLE_RGBA, // base color, opacity + mjTEXROLE_ORM, // occlusion, roughness, metallic + mjNTEXROLE +} mjtTextureRole; + + +typedef enum mjtIntegrator_ { // integrator mode + mjINT_EULER = 0, // semi-implicit Euler + mjINT_RK4, // 4th-order Runge Kutta + mjINT_IMPLICIT, // implicit in velocity + mjINT_IMPLICITFAST // implicit in velocity, no rne derivative +} mjtIntegrator; + + +typedef enum mjtCone_ { // type of friction cone + mjCONE_PYRAMIDAL = 0, // pyramidal + mjCONE_ELLIPTIC // elliptic +} mjtCone; + + +typedef enum mjtJacobian_ { // type of constraint Jacobian + mjJAC_DENSE = 0, // dense + mjJAC_SPARSE, // sparse + mjJAC_AUTO // dense if nv<60, sparse otherwise +} mjtJacobian; + + +typedef enum mjtSolver_ { // constraint solver algorithm + mjSOL_PGS = 0, // PGS (dual) + mjSOL_CG, // CG (primal) + mjSOL_NEWTON // Newton (primal) +} mjtSolver; + + +typedef enum mjtEq_ { // type of equality constraint + mjEQ_CONNECT = 0, // connect two bodies at a point (ball joint) + mjEQ_WELD, // fix relative position and orientation of two bodies + mjEQ_JOINT, // couple the values of two scalar joints with cubic + mjEQ_TENDON, // couple the lengths of two tendons with cubic + mjEQ_FLEX, // fix all edge lengths of a flex + mjEQ_DISTANCE // unsupported, will cause an error if used +} mjtEq; + + +typedef enum mjtWrap_ { // type of tendon wrap object + mjWRAP_NONE = 0, // null object + mjWRAP_JOINT, // constant moment arm + mjWRAP_PULLEY, // pulley used to split tendon + mjWRAP_SITE, // pass through site + mjWRAP_SPHERE, // wrap around sphere + mjWRAP_CYLINDER // wrap around (infinite) cylinder +} mjtWrap; + + +typedef enum mjtTrn_ { // type of actuator transmission + mjTRN_JOINT = 0, // force on joint + mjTRN_JOINTINPARENT, // force on joint, expressed in parent frame + mjTRN_SLIDERCRANK, // force via slider-crank linkage + mjTRN_TENDON, // force on tendon + mjTRN_SITE, // force on site + mjTRN_BODY, // adhesion force on a body's geoms + + mjTRN_UNDEFINED = 1000 // undefined transmission type +} mjtTrn; + + +typedef enum mjtDyn_ { // type of actuator dynamics + mjDYN_NONE = 0, // no internal dynamics; ctrl specifies force + mjDYN_INTEGRATOR, // integrator: da/dt = u + mjDYN_FILTER, // linear filter: da/dt = (u-a) / tau + mjDYN_FILTEREXACT, // linear filter: da/dt = (u-a) / tau, with exact integration + mjDYN_MUSCLE, // piece-wise linear filter with two time constants + mjDYN_USER // user-defined dynamics type +} mjtDyn; + + +typedef enum mjtGain_ { // type of actuator gain + mjGAIN_FIXED = 0, // fixed gain + mjGAIN_AFFINE, // const + kp*length + kv*velocity + mjGAIN_MUSCLE, // muscle FLV curve computed by mju_muscleGain() + mjGAIN_USER // user-defined gain type +} mjtGain; + + +typedef enum mjtBias_ { // type of actuator bias + mjBIAS_NONE = 0, // no bias + mjBIAS_AFFINE, // const + kp*length + kv*velocity + mjBIAS_MUSCLE, // muscle passive force computed by mju_muscleBias() + mjBIAS_USER // user-defined bias type +} mjtBias; + + +typedef enum mjtObj_ { // type of MujoCo object + mjOBJ_UNKNOWN = 0, // unknown object type + mjOBJ_BODY, // body + mjOBJ_XBODY, // body, used to access regular frame instead of i-frame + mjOBJ_JOINT, // joint + mjOBJ_DOF, // dof + mjOBJ_GEOM, // geom + mjOBJ_SITE, // site + mjOBJ_CAMERA, // camera + mjOBJ_LIGHT, // light + mjOBJ_FLEX, // flex + mjOBJ_MESH, // mesh + mjOBJ_SKIN, // skin + mjOBJ_HFIELD, // heightfield + mjOBJ_TEXTURE, // texture + mjOBJ_MATERIAL, // material for rendering + mjOBJ_PAIR, // geom pair to include + mjOBJ_EXCLUDE, // body pair to exclude + mjOBJ_EQUALITY, // equality constraint + mjOBJ_TENDON, // tendon + mjOBJ_ACTUATOR, // actuator + mjOBJ_SENSOR, // sensor + mjOBJ_NUMERIC, // numeric + mjOBJ_TEXT, // text + mjOBJ_TUPLE, // tuple + mjOBJ_KEY, // keyframe + mjOBJ_PLUGIN, // plugin instance + + mjNOBJECT, // number of object types + + // meta elements, do not appear in mjModel + mjOBJ_FRAME = 100, // frame + mjOBJ_DEFAULT, // default + mjOBJ_MODEL // entire model + +} mjtObj; + + +typedef enum mjtConstraint_ { // type of constraint + mjCNSTR_EQUALITY = 0, // equality constraint + mjCNSTR_FRICTION_DOF, // dof friction + mjCNSTR_FRICTION_TENDON, // tendon friction + mjCNSTR_LIMIT_JOINT, // joint limit + mjCNSTR_LIMIT_TENDON, // tendon limit + mjCNSTR_CONTACT_FRICTIONLESS, // frictionless contact + mjCNSTR_CONTACT_PYRAMIDAL, // frictional contact, pyramidal friction cone + mjCNSTR_CONTACT_ELLIPTIC // frictional contact, elliptic friction cone +} mjtConstraint; + + +typedef enum mjtConstraintState_ { // constraint state + mjCNSTRSTATE_SATISFIED = 0, // constraint satisfied, zero cost (limit, contact) + mjCNSTRSTATE_QUADRATIC, // quadratic cost (equality, friction, limit, contact) + mjCNSTRSTATE_LINEARNEG, // linear cost, negative side (friction) + mjCNSTRSTATE_LINEARPOS, // linear cost, positive side (friction) + mjCNSTRSTATE_CONE // squared distance to cone cost (elliptic contact) +} mjtConstraintState; + + +typedef enum mjtSensor_ { // type of sensor + // common robotic sensors, attached to a site + mjSENS_TOUCH = 0, // scalar contact normal forces summed over sensor zone + mjSENS_ACCELEROMETER, // 3D linear acceleration, in local frame + mjSENS_VELOCIMETER, // 3D linear velocity, in local frame + mjSENS_GYRO, // 3D angular velocity, in local frame + mjSENS_FORCE, // 3D force between site's body and its parent body + mjSENS_TORQUE, // 3D torque between site's body and its parent body + mjSENS_MAGNETOMETER, // 3D magnetometer + mjSENS_RANGEFINDER, // scalar distance to nearest geom or site along z-axis + mjSENS_CAMPROJECTION, // pixel coordinates of a site in the camera image + + // sensors related to scalar joints, tendons, actuators + mjSENS_JOINTPOS, // scalar joint position (hinge and slide only) + mjSENS_JOINTVEL, // scalar joint velocity (hinge and slide only) + mjSENS_TENDONPOS, // scalar tendon position + mjSENS_TENDONVEL, // scalar tendon velocity + mjSENS_ACTUATORPOS, // scalar actuator position + mjSENS_ACTUATORVEL, // scalar actuator velocity + mjSENS_ACTUATORFRC, // scalar actuator force + mjSENS_JOINTACTFRC, // scalar actuator force, measured at the joint + mjSENS_TENDONACTFRC, // scalar actuator force, measured at the tendon + + // sensors related to ball joints + mjSENS_BALLQUAT, // 4D ball joint quaternion + mjSENS_BALLANGVEL, // 3D ball joint angular velocity + + // joint and tendon limit sensors, in constraint space + mjSENS_JOINTLIMITPOS, // joint limit distance-margin + mjSENS_JOINTLIMITVEL, // joint limit velocity + mjSENS_JOINTLIMITFRC, // joint limit force + mjSENS_TENDONLIMITPOS, // tendon limit distance-margin + mjSENS_TENDONLIMITVEL, // tendon limit velocity + mjSENS_TENDONLIMITFRC, // tendon limit force + + // sensors attached to an object with spatial frame: (x)body, geom, site, camera + mjSENS_FRAMEPOS, // 3D position + mjSENS_FRAMEQUAT, // 4D unit quaternion orientation + mjSENS_FRAMEXAXIS, // 3D unit vector: x-axis of object's frame + mjSENS_FRAMEYAXIS, // 3D unit vector: y-axis of object's frame + mjSENS_FRAMEZAXIS, // 3D unit vector: z-axis of object's frame + mjSENS_FRAMELINVEL, // 3D linear velocity + mjSENS_FRAMEANGVEL, // 3D angular velocity + mjSENS_FRAMELINACC, // 3D linear acceleration + mjSENS_FRAMEANGACC, // 3D angular acceleration + + // sensors related to kinematic subtrees; attached to a body (which is the subtree root) + mjSENS_SUBTREECOM, // 3D center of mass of subtree + mjSENS_SUBTREELINVEL, // 3D linear velocity of subtree + mjSENS_SUBTREEANGMOM, // 3D angular momentum of subtree + + // sensors for geometric distance; attached to geoms or bodies + mjSENS_GEOMDIST, // signed distance between two geoms + mjSENS_GEOMNORMAL, // normal direction between two geoms + mjSENS_GEOMFROMTO, // segment between two geoms + + // global sensors + mjSENS_E_POTENTIAL, // potential energy + mjSENS_E_KINETIC, // kinetic energy + mjSENS_CLOCK, // simulation time + + // plugin-controlled sensors + mjSENS_PLUGIN, // plugin-controlled + + // user-defined sensor + mjSENS_USER // sensor data provided by mjcb_sensor callback +} mjtSensor; + + +typedef enum mjtStage_ { // computation stage + mjSTAGE_NONE = 0, // no computations + mjSTAGE_POS, // position-dependent computations + mjSTAGE_VEL, // velocity-dependent computations + mjSTAGE_ACC // acceleration/force-dependent computations +} mjtStage; + + +typedef enum mjtDataType_ { // data type for sensors + mjDATATYPE_REAL = 0, // real values, no constraints + mjDATATYPE_POSITIVE, // positive values; 0 or negative: inactive + mjDATATYPE_AXIS, // 3D unit vector + mjDATATYPE_QUATERNION // unit quaternion +} mjtDataType; + + +typedef enum mjtSameFrame_ { // frame alignment of bodies with their children + mjSAMEFRAME_NONE = 0, // no alignment + mjSAMEFRAME_BODY, // frame is same as body frame + mjSAMEFRAME_INERTIA, // frame is same as inertial frame + mjSAMEFRAME_BODYROT, // frame orientation is same as body orientation + mjSAMEFRAME_INERTIAROT // frame orientation is same as inertia orientation +} mjtSameFrame; + + +typedef enum mjtLRMode_ { // mode for actuator length range computation + mjLRMODE_NONE = 0, // do not process any actuators + mjLRMODE_MUSCLE, // process muscle actuators + mjLRMODE_MUSCLEUSER, // process muscle and user actuators + mjLRMODE_ALL // process all actuators +} mjtLRMode; + + +typedef enum mjtFlexSelf_ { // mode for flex selfcollide + mjFLEXSELF_NONE = 0, // no self-collisions + mjFLEXSELF_NARROW, // skip midphase, go directly to narrowphase + mjFLEXSELF_BVH, // use BVH in midphase (if midphase enabled) + mjFLEXSELF_SAP, // use SAP in midphase + mjFLEXSELF_AUTO // choose between BVH and SAP automatically +} mjtFlexSelf; + + +//---------------------------------- mjLROpt ------------------------------------------------------- + +struct mjLROpt_ { // options for mj_setLengthRange() + // flags + int mode; // which actuators to process (mjtLRMode) + int useexisting; // use existing length range if available + int uselimit; // use joint and tendon limits if available + + // algorithm parameters + mjtNum accel; // target acceleration used to compute force + mjtNum maxforce; // maximum force; 0: no limit + mjtNum timeconst; // time constant for velocity reduction; min 0.01 + mjtNum timestep; // simulation timestep; 0: use mjOption.timestep + mjtNum inttotal; // total simulation time interval + mjtNum interval; // evaluation time interval (at the end) + mjtNum tolrange; // convergence tolerance (relative to range) +}; +typedef struct mjLROpt_ mjLROpt; + + +//---------------------------------- mjVFS --------------------------------------------------------- + +struct mjVFS_ { // virtual file system for loading from memory + void* impl_; // internal pointer to VFS memory +}; +typedef struct mjVFS_ mjVFS; + +//---------------------------------- mjOption ------------------------------------------------------ + +struct mjOption_ { // physics options + // timing parameters + mjtNum timestep; // timestep + mjtNum apirate; // update rate for remote API (Hz) + + // solver parameters + mjtNum impratio; // ratio of friction-to-normal contact impedance + mjtNum tolerance; // main solver tolerance + mjtNum ls_tolerance; // CG/Newton linesearch tolerance + mjtNum noslip_tolerance; // noslip solver tolerance + mjtNum ccd_tolerance; // convex collision solver tolerance + + // physical constants + mjtNum gravity[3]; // gravitational acceleration + mjtNum wind[3]; // wind (for lift, drag and viscosity) + mjtNum magnetic[3]; // global magnetic flux + mjtNum density; // density of medium + mjtNum viscosity; // viscosity of medium + + // override contact solver parameters (if enabled) + mjtNum o_margin; // margin + mjtNum o_solref[mjNREF]; // solref + mjtNum o_solimp[mjNIMP]; // solimp + mjtNum o_friction[5]; // friction + + // discrete settings + int integrator; // integration mode (mjtIntegrator) + int cone; // type of friction cone (mjtCone) + int jacobian; // type of Jacobian (mjtJacobian) + int solver; // solver algorithm (mjtSolver) + int iterations; // maximum number of main solver iterations + int ls_iterations; // maximum number of CG/Newton linesearch iterations + int noslip_iterations; // maximum number of noslip solver iterations + int ccd_iterations; // maximum number of convex collision solver iterations + int disableflags; // bit flags for disabling standard features + int enableflags; // bit flags for enabling optional features + int disableactuator; // bit flags for disabling actuators by group id + + // sdf collision settings + int sdf_initpoints; // number of starting points for gradient descent + int sdf_iterations; // max number of iterations for gradient descent +}; +typedef struct mjOption_ mjOption; + + +//---------------------------------- mjVisual ------------------------------------------------------ + +struct mjVisual_ { // visualization options + struct { // global parameters + int orthographic; // is the free camera orthographic (0: no, 1: yes) + float fovy; // y field-of-view of free camera (orthographic ? length : degree) + float ipd; // inter-pupilary distance for free camera + float azimuth; // initial azimuth of free camera (degrees) + float elevation; // initial elevation of free camera (degrees) + float linewidth; // line width for wireframe and ray rendering + float glow; // glow coefficient for selected body + float realtime; // initial real-time factor (1: real time) + int offwidth; // width of offscreen buffer + int offheight; // height of offscreen buffer + int ellipsoidinertia; // geom for inertia visualization (0: box, 1: ellipsoid) + int bvactive; // visualize active bounding volumes (0: no, 1: yes) + } global; + + struct { // rendering quality + int shadowsize; // size of shadowmap texture + int offsamples; // number of multisamples for offscreen rendering + int numslices; // number of slices for builtin geom drawing + int numstacks; // number of stacks for builtin geom drawing + int numquads; // number of quads for box rendering + } quality; + + struct { // head light + float ambient[3]; // ambient rgb (alpha=1) + float diffuse[3]; // diffuse rgb (alpha=1) + float specular[3]; // specular rgb (alpha=1) + int active; // is headlight active + } headlight; + + struct { // mapping + float stiffness; // mouse perturbation stiffness (space->force) + float stiffnessrot; // mouse perturbation stiffness (space->torque) + float force; // from force units to space units + float torque; // from torque units to space units + float alpha; // scale geom alphas when transparency is enabled + float fogstart; // OpenGL fog starts at fogstart * mjModel.stat.extent + float fogend; // OpenGL fog ends at fogend * mjModel.stat.extent + float znear; // near clipping plane = znear * mjModel.stat.extent + float zfar; // far clipping plane = zfar * mjModel.stat.extent + float haze; // haze ratio + float shadowclip; // directional light: shadowclip * mjModel.stat.extent + float shadowscale; // spot light: shadowscale * light.cutoff + float actuatortendon; // scale tendon width + } map; + + struct { // scale of decor elements relative to mean body size + float forcewidth; // width of force arrow + float contactwidth; // contact width + float contactheight; // contact height + float connect; // autoconnect capsule width + float com; // com radius + float camera; // camera object + float light; // light object + float selectpoint; // selection point + float jointlength; // joint length + float jointwidth; // joint width + float actuatorlength; // actuator length + float actuatorwidth; // actuator width + float framelength; // bodyframe axis length + float framewidth; // bodyframe axis width + float constraint; // constraint width + float slidercrank; // slidercrank width + float frustum; // frustum zfar plane + } scale; + + struct { // color of decor elements + float fog[4]; // fog + float haze[4]; // haze + float force[4]; // external force + float inertia[4]; // inertia box + float joint[4]; // joint + float actuator[4]; // actuator, neutral + float actuatornegative[4]; // actuator, negative limit + float actuatorpositive[4]; // actuator, positive limit + float com[4]; // center of mass + float camera[4]; // camera object + float light[4]; // light object + float selectpoint[4]; // selection point + float connect[4]; // auto connect + float contactpoint[4]; // contact point + float contactforce[4]; // contact force + float contactfriction[4]; // contact friction force + float contacttorque[4]; // contact torque + float contactgap[4]; // contact point in gap + float rangefinder[4]; // rangefinder ray + float constraint[4]; // constraint + float slidercrank[4]; // slidercrank + float crankbroken[4]; // used when crank must be stretched/broken + float frustum[4]; // camera frustum + float bv[4]; // bounding volume + float bvactive[4]; // active bounding volume + } rgba; +}; +typedef struct mjVisual_ mjVisual; + + +//---------------------------------- mjStatistic --------------------------------------------------- + +struct mjStatistic_ { // model statistics (in qpos0) + mjtNum meaninertia; // mean diagonal inertia + mjtNum meanmass; // mean body mass + mjtNum meansize; // mean body size + mjtNum extent; // spatial extent + mjtNum center[3]; // center of model +}; +typedef struct mjStatistic_ mjStatistic; + + +//---------------------------------- mjModel ------------------------------------------------------- + +struct mjModel_ { + // ------------------------------- sizes + + // sizes needed at mjModel construction + int nq; // number of generalized coordinates = dim(qpos) + int nv; // number of degrees of freedom = dim(qvel) + int nu; // number of actuators/controls = dim(ctrl) + int na; // number of activation states = dim(act) + int nbody; // number of bodies + int nbvh; // number of total bounding volumes in all bodies + int nbvhstatic; // number of static bounding volumes (aabb stored in mjModel) + int nbvhdynamic; // number of dynamic bounding volumes (aabb stored in mjData) + int njnt; // number of joints + int ngeom; // number of geoms + int nsite; // number of sites + int ncam; // number of cameras + int nlight; // number of lights + int nflex; // number of flexes + int nflexnode; // number of dofs in all flexes + int nflexvert; // number of vertices in all flexes + int nflexedge; // number of edges in all flexes + int nflexelem; // number of elements in all flexes + int nflexelemdata; // number of element vertex ids in all flexes + int nflexelemedge; // number of element edge ids in all flexes + int nflexshelldata; // number of shell fragment vertex ids in all flexes + int nflexevpair; // number of element-vertex pairs in all flexes + int nflextexcoord; // number of vertices with texture coordinates + int nmesh; // number of meshes + int nmeshvert; // number of vertices in all meshes + int nmeshnormal; // number of normals in all meshes + int nmeshtexcoord; // number of texcoords in all meshes + int nmeshface; // number of triangular faces in all meshes + int nmeshgraph; // number of ints in mesh auxiliary data + int nmeshpoly; // number of polygons in all meshes + int nmeshpolyvert; // number of vertices in all polygons + int nmeshpolymap; // number of polygons in vertex map + int nskin; // number of skins + int nskinvert; // number of vertices in all skins + int nskintexvert; // number of vertiex with texcoords in all skins + int nskinface; // number of triangular faces in all skins + int nskinbone; // number of bones in all skins + int nskinbonevert; // number of vertices in all skin bones + int nhfield; // number of heightfields + int nhfielddata; // number of data points in all heightfields + int ntex; // number of textures + int ntexdata; // number of bytes in texture rgb data + int nmat; // number of materials + int npair; // number of predefined geom pairs + int nexclude; // number of excluded geom pairs + int neq; // number of equality constraints + int ntendon; // number of tendons + int nwrap; // number of wrap objects in all tendon paths + int nsensor; // number of sensors + int nnumeric; // number of numeric custom fields + int nnumericdata; // number of mjtNums in all numeric fields + int ntext; // number of text custom fields + int ntextdata; // number of mjtBytes in all text fields + int ntuple; // number of tuple custom fields + int ntupledata; // number of objects in all tuple fields + int nkey; // number of keyframes + int nmocap; // number of mocap bodies + int nplugin; // number of plugin instances + int npluginattr; // number of chars in all plugin config attributes + int nuser_body; // number of mjtNums in body_user + int nuser_jnt; // number of mjtNums in jnt_user + int nuser_geom; // number of mjtNums in geom_user + int nuser_site; // number of mjtNums in site_user + int nuser_cam; // number of mjtNums in cam_user + int nuser_tendon; // number of mjtNums in tendon_user + int nuser_actuator; // number of mjtNums in actuator_user + int nuser_sensor; // number of mjtNums in sensor_user + int nnames; // number of chars in all names + int npaths; // number of chars in all paths + + // sizes set after mjModel construction + int nnames_map; // number of slots in the names hash map + int nM; // number of non-zeros in sparse inertia matrix + int nB; // number of non-zeros in sparse body-dof matrix + int nC; // number of non-zeros in sparse reduced dof-dof matrix + int nD; // number of non-zeros in sparse dof-dof matrix + int nJmom; // number of non-zeros in sparse actuator_moment matrix + int ntree; // number of kinematic trees under world body + int ngravcomp; // number of bodies with nonzero gravcomp + int nemax; // number of potential equality-constraint rows + int njmax; // number of available rows in constraint Jacobian (legacy) + int nconmax; // number of potential contacts in contact list (legacy) + int nuserdata; // number of mjtNums reserved for the user + int nsensordata; // number of mjtNums in sensor data vector + int npluginstate; // number of mjtNums in plugin state vector + + size_t narena; // number of bytes in the mjData arena (inclusive of stack) + size_t nbuffer; // number of bytes in buffer + + // ------------------------------- options and statistics + + mjOption opt; // physics options + mjVisual vis; // visualization options + mjStatistic stat; // model statistics + + // ------------------------------- buffers + + // main buffer + void* buffer; // main buffer; all pointers point in it (nbuffer) + + // default generalized coordinates + mjtNum* qpos0; // qpos values at default pose (nq x 1) + mjtNum* qpos_spring; // reference pose for springs (nq x 1) + + // bodies + int* body_parentid; // id of body's parent (nbody x 1) + int* body_rootid; // id of root above body (nbody x 1) + int* body_weldid; // id of body that this body is welded to (nbody x 1) + int* body_mocapid; // id of mocap data; -1: none (nbody x 1) + int* body_jntnum; // number of joints for this body (nbody x 1) + int* body_jntadr; // start addr of joints; -1: no joints (nbody x 1) + int* body_dofnum; // number of motion degrees of freedom (nbody x 1) + int* body_dofadr; // start addr of dofs; -1: no dofs (nbody x 1) + int* body_treeid; // id of body's kinematic tree; -1: static (nbody x 1) + int* body_geomnum; // number of geoms (nbody x 1) + int* body_geomadr; // start addr of geoms; -1: no geoms (nbody x 1) + mjtByte* body_simple; // 1: diag M; 2: diag M, sliders only (nbody x 1) + mjtByte* body_sameframe; // same frame as inertia (mjtSameframe) (nbody x 1) + mjtNum* body_pos; // position offset rel. to parent body (nbody x 3) + mjtNum* body_quat; // orientation offset rel. to parent body (nbody x 4) + mjtNum* body_ipos; // local position of center of mass (nbody x 3) + mjtNum* body_iquat; // local orientation of inertia ellipsoid (nbody x 4) + mjtNum* body_mass; // mass (nbody x 1) + mjtNum* body_subtreemass; // mass of subtree starting at this body (nbody x 1) + mjtNum* body_inertia; // diagonal inertia in ipos/iquat frame (nbody x 3) + mjtNum* body_invweight0; // mean inv inert in qpos0 (trn, rot) (nbody x 2) + mjtNum* body_gravcomp; // antigravity force, units of body weight (nbody x 1) + mjtNum* body_margin; // MAX over all geom margins (nbody x 1) + mjtNum* body_user; // user data (nbody x nuser_body) + int* body_plugin; // plugin instance id; -1: not in use (nbody x 1) + int* body_contype; // OR over all geom contypes (nbody x 1) + int* body_conaffinity; // OR over all geom conaffinities (nbody x 1) + int* body_bvhadr; // address of bvh root (nbody x 1) + int* body_bvhnum; // number of bounding volumes (nbody x 1) + + // bounding volume hierarchy + int* bvh_depth; // depth in the bounding volume hierarchy (nbvh x 1) + int* bvh_child; // left and right children in tree (nbvh x 2) + int* bvh_nodeid; // geom or elem id of node; -1: non-leaf (nbvh x 1) + mjtNum* bvh_aabb; // local bounding box (center, size) (nbvhstatic x 6) + + // joints + int* jnt_type; // type of joint (mjtJoint) (njnt x 1) + int* jnt_qposadr; // start addr in 'qpos' for joint's data (njnt x 1) + int* jnt_dofadr; // start addr in 'qvel' for joint's data (njnt x 1) + int* jnt_bodyid; // id of joint's body (njnt x 1) + int* jnt_group; // group for visibility (njnt x 1) + mjtByte* jnt_limited; // does joint have limits (njnt x 1) + mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1) + mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (njnt x 1) + mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF) + mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) + mjtNum* jnt_pos; // local anchor position (njnt x 3) + mjtNum* jnt_axis; // local joint axis (njnt x 3) + mjtNum* jnt_stiffness; // stiffness coefficient (njnt x 1) + mjtNum* jnt_range; // joint limits (njnt x 2) + mjtNum* jnt_actfrcrange; // range of total actuator force (njnt x 2) + mjtNum* jnt_margin; // min distance for limit detection (njnt x 1) + mjtNum* jnt_user; // user data (njnt x nuser_jnt) + + // dofs + int* dof_bodyid; // id of dof's body (nv x 1) + int* dof_jntid; // id of dof's joint (nv x 1) + int* dof_parentid; // id of dof's parent; -1: none (nv x 1) + int* dof_treeid; // id of dof's kinematic tree (nv x 1) + int* dof_Madr; // dof address in M-diagonal (nv x 1) + int* dof_simplenum; // number of consecutive simple dofs (nv x 1) + mjtNum* dof_solref; // constraint solver reference:frictionloss (nv x mjNREF) + mjtNum* dof_solimp; // constraint solver impedance:frictionloss (nv x mjNIMP) + mjtNum* dof_frictionloss; // dof friction loss (nv x 1) + mjtNum* dof_armature; // dof armature inertia/mass (nv x 1) + mjtNum* dof_damping; // damping coefficient (nv x 1) + mjtNum* dof_invweight0; // diag. inverse inertia in qpos0 (nv x 1) + mjtNum* dof_M0; // diag. inertia in qpos0 (nv x 1) + + // geoms + int* geom_type; // geometric type (mjtGeom) (ngeom x 1) + int* geom_contype; // geom contact type (ngeom x 1) + int* geom_conaffinity; // geom contact affinity (ngeom x 1) + int* geom_condim; // contact dimensionality (1, 3, 4, 6) (ngeom x 1) + int* geom_bodyid; // id of geom's body (ngeom x 1) + int* geom_dataid; // id of geom's mesh/hfield; -1: none (ngeom x 1) + int* geom_matid; // material id for rendering; -1: none (ngeom x 1) + int* geom_group; // group for visibility (ngeom x 1) + int* geom_priority; // geom contact priority (ngeom x 1) + int* geom_plugin; // plugin instance id; -1: not in use (ngeom x 1) + mjtByte* geom_sameframe; // same frame as body (mjtSameframe) (ngeom x 1) + mjtNum* geom_solmix; // mixing coef for solref/imp in geom pair (ngeom x 1) + mjtNum* geom_solref; // constraint solver reference: contact (ngeom x mjNREF) + mjtNum* geom_solimp; // constraint solver impedance: contact (ngeom x mjNIMP) + mjtNum* geom_size; // geom-specific size parameters (ngeom x 3) + mjtNum* geom_aabb; // bounding box, (center, size) (ngeom x 6) + mjtNum* geom_rbound; // radius of bounding sphere (ngeom x 1) + mjtNum* geom_pos; // local position offset rel. to body (ngeom x 3) + mjtNum* geom_quat; // local orientation offset rel. to body (ngeom x 4) + mjtNum* geom_friction; // friction for (slide, spin, roll) (ngeom x 3) + mjtNum* geom_margin; // detect contact if dist +#include +#include +#include + + +//---------------------------------- Resource Provider --------------------------------------------- + +struct mjResource_ { + char* name; // name of resource (filename, etc) + void* data; // opaque data pointer + char timestamp[512]; // timestamp of the resource + const struct mjpResourceProvider* provider; // pointer to the provider +}; +typedef struct mjResource_ mjResource; + +// callback for opening a resource, returns zero on failure +typedef int (*mjfOpenResource)(mjResource* resource); + +// callback for reading a resource +// return number of bytes stored in buffer, return -1 if error +typedef int (*mjfReadResource)(mjResource* resource, const void** buffer); + +// callback for closing a resource (responsible for freeing any allocated memory) +typedef void (*mjfCloseResource)(mjResource* resource); + +// callback for returning the directory of a resource +// sets dir to directory string with ndir being size of directory string +typedef void (*mjfGetResourceDir)(mjResource* resource, const char** dir, int* ndir); + +// callback for checking if the current resource was modified from the time +// specified by the timestamp +// returns 0 if the resource's timestamp matches the provided timestamp +// returns > 0 if the resource is younger than the given timestamp +// returns < 0 if the resource is older than the given timestamp +typedef int (*mjfResourceModified)(const mjResource* resource, const char* timestamp); + +// struct describing a single resource provider +struct mjpResourceProvider { + const char* prefix; // prefix for match against a resource name + mjfOpenResource open; // opening callback + mjfReadResource read; // reading callback + mjfCloseResource close; // closing callback + mjfGetResourceDir getdir; // get directory callback (optional) + mjfResourceModified modified; // resource modified callback (optional) + void* data; // opaque data pointer (resource invariant) +}; +typedef struct mjpResourceProvider mjpResourceProvider; + + +//---------------------------------- Plugins ------------------------------------------------------- + +typedef enum mjtPluginCapabilityBit_ { + mjPLUGIN_ACTUATOR = 1<<0, // actuator forces + mjPLUGIN_SENSOR = 1<<1, // sensor measurements + mjPLUGIN_PASSIVE = 1<<2, // passive forces + mjPLUGIN_SDF = 1<<3, // signed distance fields +} mjtPluginCapabilityBit; + +struct mjpPlugin_ { + const char* name; // globally unique name identifying the plugin + + int nattribute; // number of configuration attributes + const char* const* attributes; // name of configuration attributes + + int capabilityflags; // plugin capabilities: bitfield of mjtPluginCapabilityBit + int needstage; // sensor computation stage (mjtStage) + + // number of mjtNums needed to store the state of a plugin instance (required) + int (*nstate)(const mjModel* m, int instance); + + // dimension of the specified sensor's output (required only for sensor plugins) + int (*nsensordata)(const mjModel* m, int instance, int sensor_id); + + // called when a new mjData is being created (required), returns 0 on success or -1 on failure + int (*init)(const mjModel* m, mjData* d, int instance); + + // called when an mjData is being freed (optional) + void (*destroy)(mjData* d, int instance); + + // called when an mjData is being copied (optional) + void (*copy)(mjData* dest, const mjModel* m, const mjData* src, int instance); + + // called when an mjData is being reset (required) + void (*reset)(const mjModel* m, mjtNum* plugin_state, void* plugin_data, int instance); + + // called when the plugin needs to update its outputs (required) + void (*compute)(const mjModel* m, mjData* d, int instance, int capability_bit); + + // called when time integration occurs (optional) + void (*advance)(const mjModel* m, mjData* d, int instance); + + // called by mjv_updateScene (optional) + void (*visualize)(const mjModel*m, mjData* d, const mjvOption* opt, mjvScene* scn, int instance); + + // methods specific to actuators (optional) + + // updates the actuator plugin's entries in act_dot + // called after native act_dot is computed and before the compute callback + void (*actuator_act_dot)(const mjModel* m, mjData* d, int instance); + + // methods specific to signed distance fields (optional) + + // signed distance from the surface + mjtNum (*sdf_distance)(const mjtNum point[3], const mjData* d, int instance); + + // gradient of distance with respect to local coordinates + void (*sdf_gradient)(mjtNum gradient[3], const mjtNum point[3], const mjData* d, int instance); + + // called during compilation for marching cubes + mjtNum (*sdf_staticdistance)(const mjtNum point[3], const mjtNum* attributes); + + // convert attributes and provide defaults if not present + void (*sdf_attribute)(mjtNum attribute[], const char* name[], const char* value[]); + + // bounding box of implicit surface + void (*sdf_aabb)(mjtNum aabb[6], const mjtNum* attributes); +}; +typedef struct mjpPlugin_ mjpPlugin; + +#if defined(__has_attribute) + + #if __has_attribute(constructor) + #define mjPLUGIN_LIB_INIT __attribute__((constructor)) static void _mjplugin_init(void) + #endif // __has_attribute(constructor) + +#elif defined(_MSC_VER) + + #ifndef mjDLLMAIN + #define mjDLLMAIN DllMain + #endif + + #if !defined(mjEXTERNC) + #if defined(__cplusplus) + #define mjEXTERNC extern "C" + #else + #define mjEXTERNC + #endif // defined(__cplusplus) + #endif // !defined(mjEXTERNC) + + // NOLINTBEGIN(runtime/int) + #define mjPLUGIN_LIB_INIT \ + static void _mjplugin_dllmain(void); \ + mjEXTERNC int __stdcall mjDLLMAIN(void* hinst, unsigned long reason, void* reserved) { \ + if (reason == 1) { \ + _mjplugin_dllmain(); \ + } \ + return 1; \ + } \ + static void _mjplugin_dllmain(void) + // NOLINTEND(runtime/int) + +#endif // defined(_MSC_VER) + +// function pointer type for mj_loadAllPluginLibraries callback +typedef void (*mjfPluginLibraryLoadCallback)(const char* filename, int first, int count); + +#endif // MUJOCO_INCLUDE_MJPLUGIN_H_ diff --git a/vendor/include/mujoco/mjrender.h b/vendor/include/mujoco/mjrender.h new file mode 100644 index 0000000..9a8ed1b --- /dev/null +++ b/vendor/include/mujoco/mjrender.h @@ -0,0 +1,177 @@ +// 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_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 mjMAXMATERIAL 1000 // maximum number of materials with textures + +//---------------------------------- primitive types (mjt) ----------------------------------------- + +typedef enum mjtGridPos_ { // grid position for overlay + mjGRID_TOPLEFT = 0, // top left + mjGRID_TOPRIGHT, // top right + mjGRID_BOTTOMLEFT, // bottom left + mjGRID_BOTTOMRIGHT, // bottom right + mjGRID_TOP, // top center + mjGRID_BOTTOM, // bottom center + mjGRID_LEFT, // left center + mjGRID_RIGHT // right center +} mjtGridPos; + + +typedef enum mjtFramebuffer_ { // OpenGL framebuffer option + mjFB_WINDOW = 0, // default/window buffer + mjFB_OFFSCREEN // offscreen buffer +} mjtFramebuffer; + +typedef enum mjtDepthMap_ { // depth mapping for `mjr_readPixels` + mjDEPTH_ZERONEAR = 0, // standard depth map; 0: znear, 1: zfar + mjDEPTH_ZEROFAR = 1 // reversed depth map; 1: znear, 0: zfar +} mjtDepthMap; + +typedef enum mjtFontScale_ { // font scale, used at context creation + mjFONTSCALE_50 = 50, // 50% scale, suitable for low-res rendering + mjFONTSCALE_100 = 100, // normal scale, suitable in the absence of DPI scaling + mjFONTSCALE_150 = 150, // 150% scale + mjFONTSCALE_200 = 200, // 200% scale + mjFONTSCALE_250 = 250, // 250% scale + mjFONTSCALE_300 = 300 // 300% scale +} mjtFontScale; + + +typedef enum mjtFont_ { // font type, used at each text operation + mjFONT_NORMAL = 0, // normal font + mjFONT_SHADOW, // normal font with shadow (for higher contrast) + mjFONT_BIG // big font (for user alerts) +} mjtFont; + + +struct mjrRect_ { // OpenGL rectangle + int left; // left (usually 0) + int bottom; // bottom (usually 0) + int width; // width (usually buffer width) + int height; // height (usually buffer height) +}; +typedef struct mjrRect_ mjrRect; + + +//---------------------------------- mjrContext ---------------------------------------------------- + +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 + + // 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 + + // 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 multisamples + + // shadow rendering objects + 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 + + // materials with textures + int mat_texid[mjMAXMATERIAL*mjNTEXROLE]; // material texture ids (-1: no texture) + int mat_texuniform[mjMAXMATERIAL]; // uniform cube mapping + float mat_texrepeat[mjMAXMATERIAL*2]; // texture repetition for 2d mapping + + // texture objects and info + 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 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 + + // 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) + + // 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 + + // 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 + + // framebuffer + int currentBuffer; // currently active framebuffer: mjFB_WINDOW or mjFB_OFFSCREEN + + // pixel output format + int readPixelFormat; // default color pixel format for mjr_readPixels + + // depth output format + int readDepthMap; // depth mapping: mjDEPTH_ZERONEAR or mjDEPTH_ZEROFAR +}; +typedef struct mjrContext_ mjrContext; + +#if defined(__cplusplus) +} +#endif +#endif // MUJOCO_MJRENDER_H_ diff --git a/vendor/include/mujoco/mjsan.h b/vendor/include/mujoco/mjsan.h new file mode 100644 index 0000000..4d45997 --- /dev/null +++ b/vendor/include/mujoco/mjsan.h @@ -0,0 +1,58 @@ +// 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_INCLUDE_MJSAN_H_ +#define MUJOCO_INCLUDE_MJSAN_H_ + +// Include asan interface header, or provide stubs for poison/unpoison macros when not using asan. +#ifdef ADDRESS_SANITIZER + #include +#elif defined(_MSC_VER) + #define ASAN_POISON_MEMORY_REGION(addr, size) + #define ASAN_UNPOISON_MEMORY_REGION(addr, size) +#else + #define ASAN_POISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) + #define ASAN_UNPOISON_MEMORY_REGION(addr, size) ((void)(addr), (void)(size)) +#endif + +// When built and run under address sanitizer (asan), mj_markStack and mj_freeStack are instrumented +// to detect leakage of mjData stack frames. When the compiler inlines several callees that call +// into mark/free into the same function, this instrumentation requires that the compiler retains +// separate mark/free calls for each original callee. The memory-clobbered asm blocks act as a +// barrier to prevent mark/free calls from being combined under optimization. +#ifdef ADDRESS_SANITIZER +#ifdef __cplusplus +extern "C" { +#endif + +void mj__markStack(mjData*) __attribute__((noinline)); +static inline void mj_markStack(mjData* d) __attribute__((always_inline)) { + asm volatile("" ::: "memory"); + mj__markStack(d); + asm volatile("" ::: "memory"); +} + +void mj__freeStack(mjData*) __attribute__((noinline)); +static inline void mj_freeStack(mjData* d) __attribute__((always_inline)) { + asm volatile("" ::: "memory"); + mj__freeStack(d); + asm volatile("" ::: "memory"); +} + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // ADDRESS_SANITIZER + +#endif // MUJOCO_INCLUDE_MJSAN_H_ diff --git a/vendor/include/mujoco/mjspec.h b/vendor/include/mujoco/mjspec.h new file mode 100644 index 0000000..3202d9d --- /dev/null +++ b/vendor/include/mujoco/mjspec.h @@ -0,0 +1,787 @@ +// 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_INCLUDE_MJSPEC_H_ +#define MUJOCO_INCLUDE_MJSPEC_H_ + +#include +#include +#include + + +// this is a C-API +#ifdef __cplusplus +#include +#include +#include +#include + +extern "C" { +#endif + +//-------------------------------- handles to strings and arrays ----------------------------------- + +#ifdef __cplusplus + // C++: defined to be compatible with corresponding std types + using mjString = std::string; + using mjStringVec = std::vector; + using mjIntVec = std::vector; + using mjIntVecVec = std::vector>; + using mjFloatVec = std::vector; + using mjFloatVecVec = std::vector>; + using mjDoubleVec = std::vector; + using mjByteVec = std::vector; +#else + // C: opaque types + typedef void mjString; + typedef void mjStringVec; + typedef void mjIntVec; + typedef void mjIntVecVec; + typedef void mjFloatVec; + typedef void mjFloatVecVec; + typedef void mjDoubleVec; + typedef void mjByteVec; +#endif + + +//-------------------------------- enum types (mjt) ------------------------------------------------ + +typedef enum mjtGeomInertia_ { // type of inertia inference + mjINERTIA_VOLUME = 0, // mass distributed in the volume + mjINERTIA_SHELL, // mass distributed on the surface +} mjtGeomInertia; + + +typedef enum mjtMeshInertia_ { // type of mesh inertia + mjMESH_INERTIA_CONVEX = 0, // convex mesh inertia + mjMESH_INERTIA_EXACT, // exact mesh inertia + mjMESH_INERTIA_LEGACY, // legacy mesh inertia + mjMESH_INERTIA_SHELL // shell mesh inertia +} mjtMeshInertia; + + +typedef enum mjtBuiltin_ { // type of built-in procedural texture + mjBUILTIN_NONE = 0, // no built-in texture + mjBUILTIN_GRADIENT, // gradient: rgb1->rgb2 + mjBUILTIN_CHECKER, // checker pattern: rgb1, rgb2 + mjBUILTIN_FLAT // 2d: rgb1; cube: rgb1-up, rgb2-side, rgb3-down +} mjtBuiltin; + + +typedef enum mjtMark_ { // mark type for procedural textures + mjMARK_NONE = 0, // no mark + mjMARK_EDGE, // edges + mjMARK_CROSS, // cross + mjMARK_RANDOM // random dots +} mjtMark; + + +typedef enum mjtLimited_ { // type of limit specification + mjLIMITED_FALSE = 0, // not limited + mjLIMITED_TRUE, // limited + mjLIMITED_AUTO, // limited inferred from presence of range +} mjtLimited; + +typedef enum mjtAlignFree_ { // whether to align free joints with the inertial frame + mjALIGNFREE_FALSE = 0, // don't align + mjALIGNFREE_TRUE, // align + mjALIGNFREE_AUTO, // respect the global compiler flag +} mjtAlignFree; + + +typedef enum mjtInertiaFromGeom_ { // whether to infer body inertias from child geoms + mjINERTIAFROMGEOM_FALSE = 0, // do not use; inertial element required + mjINERTIAFROMGEOM_TRUE, // always use; overwrite inertial element + mjINERTIAFROMGEOM_AUTO // use only if inertial element is missing +} mjtInertiaFromGeom; + + +typedef enum mjtOrientation_ { // type of orientation specifier + mjORIENTATION_QUAT = 0, // quaternion + mjORIENTATION_AXISANGLE, // axis and angle + mjORIENTATION_XYAXES, // x and y axes + mjORIENTATION_ZAXIS, // z axis (minimal rotation) + mjORIENTATION_EULER, // Euler angles +} mjtOrientation; + + +//-------------------------------- attribute structs (mjs) ----------------------------------------- + +typedef struct mjsElement_ { // element type, do not modify + mjtObj elemtype; // element type + uint64_t signature; // compilation signature +} mjsElement; + + +typedef struct mjsCompiler_ { // compiler options + mjtByte autolimits; // infer "limited" attribute based on range + double boundmass; // enforce minimum body mass + double boundinertia; // enforce minimum body diagonal inertia + double settotalmass; // rescale masses and inertias; <=0: ignore + mjtByte balanceinertia; // automatically impose A + B >= C rule + mjtByte fitaabb; // meshfit to aabb instead of inertia box + mjtByte degree; // angles in radians or degrees + char eulerseq[3]; // sequence for euler rotations + mjtByte discardvisual; // discard visual geoms in parser + mjtByte usethread; // use multiple threads to speed up compiler + mjtByte fusestatic; // fuse static bodies with parent + int inertiafromgeom; // use geom inertias (mjtInertiaFromGeom) + int inertiagrouprange[2]; // range of geom groups used to compute inertia + mjtByte saveinertial; // save explicit inertial clause for all bodies to XML + int alignfree; // align free joints with inertial frame + mjLROpt LRopt; // options for lengthrange computation +} mjsCompiler; + + +typedef struct mjSpec_ { // model specification + mjsElement* element; // element type + mjString* modelname; // model name + + // compiler data + mjsCompiler compiler; // compiler options + mjtByte strippath; // automatically strip paths from mesh files + mjString* meshdir; // mesh and hfield directory + mjString* texturedir; // texture directory + + // engine data + mjOption option; // physics options + mjVisual visual; // visual options + mjStatistic stat; // statistics override (if defined) + + // sizes + size_t memory; // number of bytes in arena+stack memory + int nemax; // max number of equality constraints + int nuserdata; // number of mjtNums in userdata + int nuser_body; // number of mjtNums in body_user + int nuser_jnt; // number of mjtNums in jnt_user + int nuser_geom; // number of mjtNums in geom_user + int nuser_site; // number of mjtNums in site_user + int nuser_cam; // number of mjtNums in cam_user + int nuser_tendon; // number of mjtNums in tendon_user + int nuser_actuator; // number of mjtNums in actuator_user + int nuser_sensor; // number of mjtNums in sensor_user + int nkey; // number of keyframes + int njmax; // (deprecated) max number of constraints + int nconmax; // (deprecated) max number of detected contacts + size_t nstack; // (deprecated) number of mjtNums in mjData stack + + // global data + mjString* comment; // comment at top of XML + mjString* modelfiledir; // path to model file + + // other + mjtByte hasImplicitPluginElem; // already encountered an implicit plugin sensor/actuator +} mjSpec; + + +typedef struct mjsOrientation_ { // alternative orientation specifiers + mjtOrientation type; // active orientation specifier + double axisangle[4]; // axis and angle + double xyaxes[6]; // x and y axes + double zaxis[3]; // z axis (minimal rotation) + double euler[3]; // Euler angles +} mjsOrientation; + + +typedef struct mjsPlugin_ { // plugin specification + mjsElement* element; // element type + mjString* name; // instance name + mjString* plugin_name; // plugin name + mjtByte active; // is the plugin active + mjString* info; // message appended to compiler errors +} mjsPlugin; + + +typedef struct mjsBody_ { // body specification + mjsElement* element; // element type + mjString* name; // name + mjString* childclass; // childclass name + + // body frame + double pos[3]; // frame position + double quat[4]; // frame orientation + mjsOrientation alt; // frame alternative orientation + + // inertial frame + double mass; // mass + double ipos[3]; // inertial frame position + double iquat[4]; // inertial frame orientation + double inertia[3]; // diagonal inertia (in i-frame) + mjsOrientation ialt; // inertial frame alternative orientation + double fullinertia[6]; // non-axis-aligned inertia matrix + + // other + mjtByte mocap; // is this a mocap body + double gravcomp; // gravity compensation + mjDoubleVec* userdata; // user data + mjtByte explicitinertial; // whether to save the body with explicit inertial clause + mjsPlugin plugin; // passive force plugin + mjString* info; // message appended to compiler errors +} mjsBody; + + +typedef struct mjsFrame_ { // frame specification + mjsElement* element; // element type + mjString* name; // name + mjString* childclass; // childclass name + double pos[3]; // position + double quat[4]; // orientation + mjsOrientation alt; // alternative orientation + mjString* info; // message appended to compiler errors +} mjsFrame; + + +typedef struct mjsJoint_ { // joint specification + mjsElement* element; // element type + mjString* name; // name + mjtJoint type; // joint type + + // kinematics + double pos[3]; // anchor position + double axis[3]; // joint axis + double ref; // value at reference configuration: qpos0 + int align; // align free joint with body com (mjtAlignFree) + + // stiffness + double stiffness; // stiffness coefficient + double springref; // spring reference value: qpos_spring + double springdamper[2]; // timeconst, dampratio + + // limits + int limited; // does joint have limits (mjtLimited) + double range[2]; // joint limits + double margin; // margin value for joint limit detection + mjtNum solref_limit[mjNREF]; // solver reference: joint limits + mjtNum solimp_limit[mjNIMP]; // solver impedance: joint limits + int actfrclimited; // are actuator forces on joint limited (mjtLimited) + double actfrcrange[2]; // actuator force limits + + // dof properties + double armature; // armature inertia (mass for slider) + double damping; // damping coefficient + double frictionloss; // friction loss + mjtNum solref_friction[mjNREF]; // solver reference: dof friction + mjtNum solimp_friction[mjNIMP]; // solver impedance: dof friction + + // other + int group; // group + mjtByte actgravcomp; // is gravcomp force applied via actuators + mjDoubleVec* userdata; // user data + mjString* info; // message appended to compiler errors +} mjsJoint; + + +typedef struct mjsGeom_ { // geom specification + mjsElement* element; // element type + mjString* name; // name + mjtGeom type; // geom type + + // frame, size + double pos[3]; // position + double quat[4]; // orientation + mjsOrientation alt; // alternative orientation + double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid + double size[3]; // type-specific size + + // contact related + int contype; // contact type + int conaffinity; // contact affinity + int condim; // contact dimensionality + int priority; // contact priority + double friction[3]; // one-sided friction coefficients: slide, roll, spin + double solmix; // solver mixing for contact pairs + mjtNum solref[mjNREF]; // solver reference + mjtNum solimp[mjNIMP]; // solver impedance + double margin; // margin for contact detection + double gap; // include in solver if dist < margin-gap + + // inertia inference + double mass; // used to compute density + double density; // used to compute mass and inertia from volume or surface + mjtGeomInertia typeinertia; // selects between surface and volume inertia + + // fluid forces + mjtNum fluid_ellipsoid; // whether ellipsoid-fluid model is active + mjtNum fluid_coefs[5]; // ellipsoid-fluid interaction coefs + + // visual + mjString* material; // name of material + float rgba[4]; // rgba when material is omitted + int group; // group + + // other + mjString* hfieldname; // heightfield attached to geom + mjString* meshname; // mesh attached to geom + double fitscale; // scale mesh uniformly + mjDoubleVec* userdata; // user data + mjsPlugin plugin; // sdf plugin + mjString* info; // message appended to compiler errors +} mjsGeom; + + +typedef struct mjsSite_ { // site specification + mjsElement* element; // element type + mjString* name; // name + + // frame, size + double pos[3]; // position + double quat[4]; // orientation + mjsOrientation alt; // alternative orientation + double fromto[6]; // alternative for capsule, cylinder, box, ellipsoid + double size[3]; // geom size + + // visual + mjtGeom type; // geom type + mjString* material; // name of material + int group; // group + float rgba[4]; // rgba when material is omitted + + // other + mjDoubleVec* userdata; // user data + mjString* info; // message appended to compiler errors +} mjsSite; + + +typedef struct mjsCamera_ { // camera specification + mjsElement* element; // element type + mjString* name; // name + + // extrinsics + double pos[3]; // position + double quat[4]; // orientation + mjsOrientation alt; // alternative orientation + mjtCamLight mode; // tracking mode + mjString* targetbody; // target body for tracking/targeting + + // intrinsics + int orthographic; // is camera orthographic + double fovy; // y-field of view + double ipd; // inter-pupilary distance + float intrinsic[4]; // camera intrinsics (length) + float sensor_size[2]; // sensor size (length) + float resolution[2]; // resolution (pixel) + float focal_length[2]; // focal length (length) + float focal_pixel[2]; // focal length (pixel) + float principal_length[2]; // principal point (length) + float principal_pixel[2]; // principal point (pixel) + + // other + mjDoubleVec* userdata; // user data + mjString* info; // message appended to compiler errors +} mjsCamera; + + +typedef struct mjsLight_ { // light specification + mjsElement* element; // element type + mjString* name; // name + + // frame + double pos[3]; // position + double dir[3]; // direction + mjtCamLight mode; // tracking mode + mjString* targetbody; // target body for targeting + + // intrinsics + mjtByte active; // is light active + mjtByte directional; // is light directional or spot + mjtByte castshadow; // does light cast shadows + double bulbradius; // bulb radius, for soft shadows + float attenuation[3]; // OpenGL attenuation (quadratic model) + float cutoff; // OpenGL cutoff + float exponent; // OpenGL exponent + float ambient[3]; // ambient color + float diffuse[3]; // diffuse color + float specular[3]; // specular color + + // other + mjString* info; // message appended to compiler errorsx +} mjsLight; + + +typedef struct mjsFlex_ { // flex specification + mjsElement* element; // element type + mjString* name; // name + + // contact properties + int contype; // contact type + int conaffinity; // contact affinity + int condim; // contact dimensionality + int priority; // contact priority + double friction[3]; // one-sided friction coefficients: slide, roll, spin + double solmix; // solver mixing for contact pairs + mjtNum solref[mjNREF]; // solver reference + mjtNum solimp[mjNIMP]; // solver impedance + double margin; // margin for contact detection + double gap; // include in solver if dist + +#define mjMAXUISECT 10 // maximum number of sections +#define mjMAXUIITEM 200 // maximum number of items per section +#define mjMAXUITEXT 300 // maximum number of chars in edittext and other +#define mjMAXUINAME 40 // maximum number of chars in name +#define mjMAXUIMULTI 35 // maximum number of radio/select items in group +#define mjMAXUIEDIT 7 // maximum number of elements in edit list +#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) +#define mjKEY_ESCAPE 256 +#define mjKEY_ENTER 257 +#define mjKEY_TAB 258 +#define mjKEY_BACKSPACE 259 +#define mjKEY_INSERT 260 +#define mjKEY_DELETE 261 +#define mjKEY_RIGHT 262 +#define mjKEY_LEFT 263 +#define mjKEY_DOWN 264 +#define mjKEY_UP 265 +#define mjKEY_PAGE_UP 266 +#define mjKEY_PAGE_DOWN 267 +#define mjKEY_HOME 268 +#define mjKEY_END 269 +#define mjKEY_F1 290 +#define mjKEY_F2 291 +#define mjKEY_F3 292 +#define mjKEY_F4 293 +#define mjKEY_F5 294 +#define mjKEY_F6 295 +#define mjKEY_F7 296 +#define mjKEY_F8 297 +#define mjKEY_F9 298 +#define mjKEY_F10 299 +#define mjKEY_F11 300 +#define mjKEY_F12 301 +#define mjKEY_NUMPAD_0 320 +#define mjKEY_NUMPAD_9 329 + + +//---------------------------------- primitive types (mjt) ----------------------------------------- + +typedef enum mjtButton_ { // mouse button + mjBUTTON_NONE = 0, // no button + mjBUTTON_LEFT, // left button + mjBUTTON_RIGHT, // right button + mjBUTTON_MIDDLE // middle button +} mjtButton; + + +typedef enum mjtEvent_ { // mouse and keyboard event type + mjEVENT_NONE = 0, // no event + mjEVENT_MOVE, // mouse move + mjEVENT_PRESS, // mouse button press + mjEVENT_RELEASE, // mouse button release + mjEVENT_SCROLL, // scroll + mjEVENT_KEY, // key press + mjEVENT_RESIZE, // resize + mjEVENT_REDRAW, // redraw + mjEVENT_FILESDROP // files drop +} mjtEvent; + + +typedef enum mjtItem_ { // UI item type + mjITEM_END = -2, // end of definition list (not an item) + mjITEM_SECTION = -1, // section (not an item) + mjITEM_SEPARATOR = 0, // separator + mjITEM_STATIC, // static text + mjITEM_BUTTON, // button + + // the rest have data pointer + mjITEM_CHECKINT, // check box, int value + mjITEM_CHECKBYTE, // check box, mjtByte value + mjITEM_RADIO, // radio group + mjITEM_RADIOLINE, // radio group, single line + mjITEM_SELECT, // selection box + mjITEM_SLIDERINT, // slider, int value + mjITEM_SLIDERNUM, // slider, mjtNum value + mjITEM_EDITINT, // editable array, int values + mjITEM_EDITNUM, // editable array, mjtNum values + mjITEM_EDITFLOAT, // editable array, float values + mjITEM_EDITTXT, // editable text + + 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; + + +// predicate function: set enable/disable based on item category +typedef int (*mjfItemEnable)(int category, void* data); + + +//---------------------------------- mjuiState ----------------------------------------------------- + +struct mjuiState_ { // mouse and keyboard state + // constants set by user + int nrect; // number of rectangles used + mjrRect rect[mjMAXUIRECT]; // rectangles (index 0: entire window) + void* userdata; // pointer to user data (for callbacks) + + // event type + int type; // (type mjtEvent) + + // mouse buttons + int left; // is left button down + int right; // is right button down + int middle; // is middle button down + int doubleclick; // is last press a double click + int button; // which button was pressed (mjtButton) + double buttontime; // time of last button press + + // mouse position + double x; // x position + double y; // y position + double dx; // x displacement + double dy; // y displacement + double sx; // x scroll + double sy; // y scroll + + // keyboard + int control; // is control down + int shift; // is shift down + int alt; // is alt down + int key; // which key was pressed + double keytime; // time of last key press + + // rectangle ownership and dragging + int mouserect; // which rectangle contains mouse + int dragrect; // which rectangle is dragged with mouse + int dragbutton; // which button started drag (mjtButton) + + // files dropping (only valid when type == mjEVENT_FILESDROP) + int dropcount; // number of files dropped + const char** droppaths; // paths to files dropped +}; +typedef struct mjuiState_ mjuiState; + + +//---------------------------------- mjuiThemeSpacing ---------------------------------------------- + +struct mjuiThemeSpacing_ { // UI visualization theme spacing + int total; // total width + 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 + int texthor; // text horizontal gap + int textver; // text vertical gap + int linescroll; // number of pixels to scroll + int samples; // number of multisamples +}; +typedef struct mjuiThemeSpacing_ mjuiThemeSpacing; + + +//---------------------------------- mjuiThemeColor ------------------------------------------------ + +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 secttitleuncheck[3]; // section title with unchecked box + float secttitleuncheck2[3]; // section title with unchecked box: bottom color + float secttitlecheck[3]; // section title with checked box + float secttitlecheck2[3]; // section title with checked box: 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 + float decorinactive[3]; // decor inactive + float decorinactive2[3]; // inactive slider color 2 + float button[3]; // button + float check[3]; // check + float radio[3]; // radio + float select[3]; // select + float select2[3]; // select pane + float slider[3]; // slider + float slider2[3]; // slider color 2 + float edit[3]; // edit + float edit2[3]; // edit invalid + float cursor[3]; // edit cursor +}; +typedef struct mjuiThemeColor_ mjuiThemeColor; + + +//---------------------------------- mjuiItem ------------------------------------------------------ + +struct mjuiItemSingle_ { // check and button-related + int modifier; // 0: none, 1: control, 2: shift; 4: alt + int shortcut; // shortcut key; 0: undefined +}; + + +struct mjuiItemMulti_ { // static, radio and select-related + int nelem; // number of elements in group + char name[mjMAXUIMULTI][mjMAXUINAME]; // element names +}; + + +struct mjuiItemSlider_ { // slider-related + double range[2]; // slider range + double divisions; // number of range divisions +}; + + +struct mjuiItemEdit_ { // edit-related + int nelem; // number of elements in list + double range[mjMAXUIEDIT][2]; // element range (min>=max: ignore) +}; + + +struct mjuiItem_ { // UI item + // common properties + int type; // type (mjtItem) + char name[mjMAXUINAME]; // name + int state; // 0: disable, 1: enable, 2+: use predicate + 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 { + struct mjuiItemSingle_ single; // check and button + struct mjuiItemMulti_ multi; // static, radio and select + struct mjuiItemSlider_ slider; // slider + struct mjuiItemEdit_ edit; // edit + }; + + // internal + mjrRect rect; // rectangle occupied by item + int skip; // item skipped due to closed separator +}; +typedef struct mjuiItem_ mjuiItem; + + +//---------------------------------- mjuiSection --------------------------------------------------- + +struct mjuiSection_ { // UI section + // properties + char name[mjMAXUINAME]; // name + 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: 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; + + +//---------------------------------- mjUI ---------------------------------------------------------- + +struct mjUI_ { // entire UI + // constants set by user + mjuiThemeSpacing spacing; // UI theme spacing + mjuiThemeColor color; // UI theme color + mjfItemEnable predicate; // callback to set item state programmatically + void* userdata; // pointer to user data (passed to predicate) + int rectid; // index of this ui rectangle in mjuiState + int auxid; // aux buffer index of this ui + int radiocol; // number of radio columns (0 defaults to 2) + + // UI sizes (framebuffer units) + int width; // width + int height; // current height + int maxheight; // height when all sections open + int scroll; // scroll from top of UI + + // 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 + int edititem; // item within section + int editcursor; // cursor position + int editscroll; // horizontal scroll + char edittext[mjMAXUITEXT]; // current text + mjuiItem* editchanged; // pointer to changed edit in last mjui_event + + // sections + int nsect; // number of sections in use + mjuiSection sect[mjMAXUISECT]; // preallocated array of sections +}; +typedef struct mjUI_ mjUI; + + +//---------------------------------- mjuiDef ------------------------------------------------------- + +struct mjuiDef_ { // table passed to mjui_add() + int type; // type (mjtItem); -1: section + char name[mjMAXUINAME]; // name + 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; + +#endif // MUJOCO_MJUI_H_ diff --git a/vendor/include/mujoco/mjvisualize.h b/vendor/include/mujoco/mjvisualize.h new file mode 100644 index 0000000..0a757cf --- /dev/null +++ b/vendor/include/mujoco/mjvisualize.h @@ -0,0 +1,694 @@ +// 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_MJVISUALIZE_H_ +#define MUJOCO_MJVISUALIZE_H_ + +#include +#include +#include + + +#define mjNGROUP 6 // number of geom, site, joint, skin groups with visflags +#define mjMAXLIGHT 100 // maximum number of lights in a scene +#define mjMAXOVERLAY 500 // maximum number of characters in overlay text +#define mjMAXLINE 100 // maximum number of lines per plot +#define mjMAXLINEPNT 1000 // maximum number points per line +#define mjMAXPLANEGRID 200 // maximum number of grid divisions for plane + + +//---------------------------------- primitive types (mjt) ----------------------------------------- + +typedef enum mjtCatBit_ { // bitflags for mjvGeom category + mjCAT_STATIC = 1, // model elements in body 0 + mjCAT_DYNAMIC = 2, // model elements in all other bodies + mjCAT_DECOR = 4, // decorative geoms + mjCAT_ALL = 7 // select all categories +} mjtCatBit; + + +typedef enum mjtMouse_ { // mouse interaction mode + mjMOUSE_NONE = 0, // no action + mjMOUSE_ROTATE_V, // rotate, vertical plane + mjMOUSE_ROTATE_H, // rotate, horizontal plane + mjMOUSE_MOVE_V, // move, vertical plane + mjMOUSE_MOVE_H, // move, horizontal plane + mjMOUSE_ZOOM, // zoom + mjMOUSE_SELECT // selection +} mjtMouse; + + +typedef enum mjtPertBit_ { // mouse perturbations + mjPERT_TRANSLATE = 1, // translation + mjPERT_ROTATE = 2 // rotation +} mjtPertBit; + + +typedef enum mjtCamera_ { // abstract camera type + mjCAMERA_FREE = 0, // free camera + mjCAMERA_TRACKING, // tracking camera; uses trackbodyid + mjCAMERA_FIXED, // fixed camera; uses fixedcamid + mjCAMERA_USER // user is responsible for setting OpenGL camera +} mjtCamera; + + +typedef enum mjtLabel_ { // object labeling + mjLABEL_NONE = 0, // nothing + mjLABEL_BODY, // body labels + mjLABEL_JOINT, // joint labels + mjLABEL_GEOM, // geom labels + mjLABEL_SITE, // site labels + mjLABEL_CAMERA, // camera labels + mjLABEL_LIGHT, // light labels + mjLABEL_TENDON, // tendon labels + mjLABEL_ACTUATOR, // actuator labels + mjLABEL_CONSTRAINT, // constraint labels + mjLABEL_FLEX, // flex labels + mjLABEL_SKIN, // skin labels + mjLABEL_SELECTION, // selected object + mjLABEL_SELPNT, // coordinates of selection point + mjLABEL_CONTACTPOINT, // contact information + mjLABEL_CONTACTFORCE, // magnitude of contact force + mjLABEL_ISLAND, // id of island + + mjNLABEL // number of label types +} mjtLabel; + + +typedef enum mjtFrame_ { // frame visualization + mjFRAME_NONE = 0, // no frames + mjFRAME_BODY, // body frames + mjFRAME_GEOM, // geom frames + mjFRAME_SITE, // site frames + mjFRAME_CAMERA, // camera frames + mjFRAME_LIGHT, // light frames + mjFRAME_CONTACT, // contact frames + mjFRAME_WORLD, // world frame + + mjNFRAME // number of visualization frames +} mjtFrame; + + +typedef enum mjtVisFlag_ { // flags enabling model element visualization + mjVIS_CONVEXHULL = 0, // mesh convex hull + mjVIS_TEXTURE, // textures + mjVIS_JOINT, // joints + mjVIS_CAMERA, // cameras + mjVIS_ACTUATOR, // actuators + mjVIS_ACTIVATION, // activations + mjVIS_LIGHT, // lights + mjVIS_TENDON, // tendons + mjVIS_RANGEFINDER, // rangefinder sensors + mjVIS_CONSTRAINT, // point constraints + mjVIS_INERTIA, // equivalent inertia boxes + mjVIS_SCLINERTIA, // scale equivalent inertia boxes with mass + mjVIS_PERTFORCE, // perturbation force + mjVIS_PERTOBJ, // perturbation object + mjVIS_CONTACTPOINT, // contact points + mjVIS_ISLAND, // constraint islands + mjVIS_CONTACTFORCE, // contact force + mjVIS_CONTACTSPLIT, // split contact force into normal and tangent + mjVIS_TRANSPARENT, // make dynamic geoms more transparent + mjVIS_AUTOCONNECT, // auto connect joints and body coms + mjVIS_COM, // center of mass + mjVIS_SELECT, // selection point + mjVIS_STATIC, // static bodies + mjVIS_SKIN, // skin + mjVIS_FLEXVERT, // flex vertices + mjVIS_FLEXEDGE, // flex edges + mjVIS_FLEXFACE, // flex element faces + mjVIS_FLEXSKIN, // flex smooth skin (disables the rest) + mjVIS_BODYBVH, // body bounding volume hierarchy + mjVIS_FLEXBVH, // flex bounding volume hierarchy + mjVIS_MESHBVH, // mesh bounding volume hierarchy + mjVIS_SDFITER, // iterations of SDF gradient descent + + mjNVISFLAG // number of visualization flags +} mjtVisFlag; + + +typedef enum mjtRndFlag_ { // flags enabling rendering effects + mjRND_SHADOW = 0, // shadows + mjRND_WIREFRAME, // wireframe + mjRND_REFLECTION, // reflections + mjRND_ADDITIVE, // additive transparency + mjRND_SKYBOX, // skybox + mjRND_FOG, // fog + mjRND_HAZE, // haze + mjRND_SEGMENT, // segmentation with random color + mjRND_IDCOLOR, // segmentation with segid+1 color + mjRND_CULL_FACE, // cull backward faces + + mjNRNDFLAG // number of rendering flags +} mjtRndFlag; + + +typedef enum mjtStereo_ { // type of stereo rendering + mjSTEREO_NONE = 0, // no stereo; use left eye only + mjSTEREO_QUADBUFFERED, // quad buffered; revert to side-by-side if no hardware support + mjSTEREO_SIDEBYSIDE // side-by-side +} mjtStereo; + + +//---------------------------------- mjvPerturb ---------------------------------------------------- + +struct mjvPerturb_ { // object selection and perturbation + int select; // selected body id; non-positive: none + int flexselect; // selected flex id; negative: none + int skinselect; // selected skin id; negative: none + int active; // perturbation bitmask (mjtPertBit) + int active2; // secondary perturbation bitmask (mjtPertBit) + mjtNum refpos[3]; // reference position for selected object + mjtNum refquat[4]; // reference orientation for selected object + mjtNum refselpos[3]; // reference position for selection point + mjtNum localpos[3]; // selection point in object coordinates + mjtNum localmass; // spatial inertia at selection point + mjtNum scale; // relative mouse motion-to-space scaling (set by initPerturb) +}; +typedef struct mjvPerturb_ mjvPerturb; + + +//---------------------------------- mjvCamera ----------------------------------------------------- + +struct mjvCamera_ { // abstract camera + // type and ids + int type; // camera type (mjtCamera) + int fixedcamid; // fixed camera id + int trackbodyid; // body id to track + + // abstract camera pose specification + mjtNum lookat[3]; // lookat point + mjtNum distance; // distance to lookat point or tracked body + mjtNum azimuth; // camera azimuth (deg) + mjtNum elevation; // camera elevation (deg) + + // orthographic / perspective + int orthographic; // 0: perspective; 1: orthographic +}; +typedef struct mjvCamera_ mjvCamera; + + +//---------------------------------- mjvGLCamera --------------------------------------------------- + +struct mjvGLCamera_ { // OpenGL camera + // camera frame + float pos[3]; // position + float forward[3]; // forward direction + float up[3]; // up direction + + // camera projection + float frustum_center; // hor. center (left,right set to match aspect) + float frustum_width; // width (not used for rendering) + float frustum_bottom; // bottom + float frustum_top; // top + float frustum_near; // near + float frustum_far; // far + + // orthographic / perspective + int orthographic; // 0: perspective; 1: orthographic +}; +typedef struct mjvGLCamera_ mjvGLCamera; + + +//---------------------------------- mjvGeom ------------------------------------------------------- + +struct mjvGeom_ { // abstract geom + // type info + int type; // geom type (mjtGeom) + int dataid; // mesh, hfield or plane id; -1: none + int objtype; // mujoco object type; mjOBJ_UNKNOWN for decor + int objid; // mujoco object id; -1 for decor + int category; // visual category + int matid; // material id; -1: no textured material + int texcoord; // mesh or flex geom has texture coordinates + int segid; // segmentation id; -1: not shown + + // 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) + float camdist; // distance to camera (used by sorter) + float modelrbound; // geom rbound from model, 0 if not model geom + mjtByte transparent; // treat geom as transparent +}; +typedef struct mjvGeom_ mjvGeom; + + +//---------------------------------- mjvLight ------------------------------------------------------ + +struct mjvLight_ { // OpenGL light + float pos[3]; // position rel. to body frame + float dir[3]; // direction rel. to body frame + float attenuation[3]; // OpenGL attenuation (quadratic model) + float cutoff; // OpenGL cutoff + float exponent; // OpenGL exponent + float ambient[3]; // ambient rgb (alpha=1) + float diffuse[3]; // diffuse rgb (alpha=1) + float specular[3]; // specular rgb (alpha=1) + mjtByte headlight; // headlight + mjtByte directional; // directional light + mjtByte castshadow; // does light cast shadows + float bulbradius; // bulb radius for soft shadows +}; +typedef struct mjvLight_ mjvLight; + + +//---------------------------------- mjvOption ----------------------------------------------------- + +struct mjvOption_ { // abstract visualization options + int label; // what objects to label (mjtLabel) + int frame; // which frame to show (mjtFrame) + mjtByte geomgroup[mjNGROUP]; // geom visualization by group + mjtByte sitegroup[mjNGROUP]; // site visualization by group + mjtByte jointgroup[mjNGROUP]; // joint visualization by group + mjtByte tendongroup[mjNGROUP]; // tendon visualization by group + mjtByte actuatorgroup[mjNGROUP]; // actuator visualization by group + mjtByte flexgroup[mjNGROUP]; // flex visualization by group + mjtByte skingroup[mjNGROUP]; // skin visualization by group + mjtByte flags[mjNVISFLAG]; // visualization flags (indexed by mjtVisFlag) + int bvh_depth; // depth of the bounding volume hierarchy to be visualized + int flex_layer; // element layer to be visualized for 3D flex +}; +typedef struct mjvOption_ mjvOption; + + +//---------------------------------- mjvScene ------------------------------------------------------ + +struct mjvScene_ { // abstract scene passed to OpenGL renderer + // abstract geoms + int maxgeom; // size of allocated geom buffer + int ngeom; // number of geoms currently in buffer + mjvGeom* geoms; // buffer for geoms (ngeom) + int* geomorder; // buffer for ordering geoms by distance to camera (ngeom) + + // flex data + int nflex; // number of flexes + int* flexedgeadr; // address of flex edges (nflex) + int* flexedgenum; // number of edges in flex (nflex) + int* flexvertadr; // address of flex vertices (nflex) + int* flexvertnum; // number of vertices in flex (nflex) + int* flexfaceadr; // address of flex faces (nflex) + int* flexfacenum; // number of flex faces allocated (nflex) + int* flexfaceused; // number of flex faces currently in use (nflex) + int* flexedge; // flex edge data (2*nflexedge) + float* flexvert; // flex vertices (3*nflexvert) + float* flexface; // flex faces vertices (9*sum(flexfacenum)) + float* flexnormal; // flex face normals (9*sum(flexfacenum)) + float* flextexcoord; // flex face texture coordinates (6*sum(flexfacenum)) + mjtByte flexvertopt; // copy of mjVIS_FLEXVERT mjvOption flag + mjtByte flexedgeopt; // copy of mjVIS_FLEXEDGE mjvOption flag + mjtByte flexfaceopt; // copy of mjVIS_FLEXFACE mjvOption flag + mjtByte flexskinopt; // copy of mjVIS_FLEXSKIN mjvOption flag + + // skin data + int nskin; // number of skins + int* skinfacenum; // number of faces in skin (nskin) + int* skinvertadr; // address of skin vertices (nskin) + int* skinvertnum; // number of vertices in skin (nskin) + float* skinvert; // skin vertex data (3*nskinvert) + float* skinnormal; // skin normal data (3*nskinvert) + + // OpenGL lights + int nlight; // number of lights currently in buffer + mjvLight lights[mjMAXLIGHT]; // buffer for lights (nlight) + + // OpenGL cameras + mjvGLCamera camera[2]; // left and right camera + + // OpenGL model transformation + mjtByte enabletransform; // enable model transformation + float translate[3]; // model translation + float rotate[4]; // model quaternion rotation + float scale; // model scaling + + // OpenGL rendering effects + int stereo; // stereoscopic rendering (mjtStereo) + mjtByte flags[mjNRNDFLAG]; // rendering flags (indexed by mjtRndFlag) + + // framing + int framewidth; // frame pixel width; 0: disable framing + float framergb[3]; // frame color +}; +typedef struct mjvScene_ mjvScene; + + +//---------------------------------- mjvFigure ----------------------------------------------------- + +struct mjvFigure_ { // abstract 2D figure passed to OpenGL renderer + // enable flags + int flg_legend; // show legend + int flg_ticklabel[2]; // show grid tick labels (x,y) + int flg_extend; // automatically extend axis ranges to fit data + int flg_barplot; // isolated line segments (i.e. GL_LINES) + int flg_selection; // vertical selection line + int flg_symmetric; // symmetric y-axis + + // style settings + float linewidth; // line width + float gridwidth; // grid line width + int gridsize[2]; // number of grid points in (x,y) + float gridrgb[3]; // grid line rgb + float figurergba[4]; // figure color and alpha + float panergba[4]; // pane color and alpha + float legendrgba[4]; // legend color and alpha + float textrgb[3]; // text color + float linergb[mjMAXLINE][3]; // line colors + float range[2][2]; // axis ranges; (min>=max) automatic + char xformat[20]; // x-tick label format for sprintf + char yformat[20]; // y-tick label format for sprintf + char minwidth[20]; // string used to determine min y-tick width + + // text labels + char title[1000]; // figure title; subplots separated with 2+ spaces + char xlabel[100]; // x-axis label + char linename[mjMAXLINE][100]; // line names for legend + + // dynamic settings + int legendoffset; // number of lines to offset legend + int subplot; // selected subplot (for title rendering) + int highlight[2]; // if point is in legend rect, highlight line + int highlightid; // if id>=0 and no point, highlight id + float selection; // selection line x-value + + // line data + int linepnt[mjMAXLINE]; // number of points in line; (0) disable + float linedata[mjMAXLINE][2*mjMAXLINEPNT]; // line data (x,y) + + // output from renderer + int xaxispixel[2]; // range of x-axis in pixels + int yaxispixel[2]; // range of y-axis in pixels + float xaxisdata[2]; // range of x-axis in data units + float yaxisdata[2]; // range of y-axis in data units +}; +typedef struct mjvFigure_ mjvFigure; + + +//---------------------------------- mjvSceneState ------------------------------------------------- + +struct mjvSceneState_ { + int nbuffer; // size of the buffer in bytes + void* buffer; // heap-allocated memory for all arrays in this struct + int maxgeom; // maximum number of mjvGeom supported by this state object + mjvScene scratch; // scratch space for vis geoms inserted by the user and plugins + + // fields in mjModel that are necessary to re-render a scene + struct { + int nv; + int nu; + int na; + int nbody; + int nbvh; + int nbvhstatic; + int njnt; + int ngeom; + int nsite; + int ncam; + int nlight; + int nmesh; + int nskin; + int nflex; + int nflexvert; + int nflextexcoord; + int nskinvert; + int nskinface; + int nskinbone; + int nskinbonevert; + int nmat; + int neq; + int ntendon; + int ntree; + int nwrap; + int nsensor; + int nnames; + int npaths; + int nsensordata; + int narena; + + mjOption opt; + mjVisual vis; + mjStatistic stat; + + int* body_parentid; + int* body_rootid; + int* body_weldid; + int* body_mocapid; + int* body_jntnum; + int* body_jntadr; + int* body_dofnum; + int* body_dofadr; + int* body_geomnum; + int* body_geomadr; + mjtNum* body_iquat; + mjtNum* body_mass; + mjtNum* body_inertia; + int* body_bvhadr; + int* body_bvhnum; + + int* bvh_depth; + int* bvh_child; + int* bvh_nodeid; + mjtNum* bvh_aabb; + + int* jnt_type; + int* jnt_bodyid; + int* jnt_group; + + int* geom_type; + int* geom_bodyid; + int* geom_contype; + int* geom_conaffinity; + int* geom_dataid; + int* geom_matid; + int* geom_group; + mjtNum* geom_size; + mjtNum* geom_aabb; + mjtNum* geom_rbound; + float* geom_rgba; + + int* site_type; + int* site_bodyid; + int* site_matid; + int* site_group; + mjtNum* site_size; + float* site_rgba; + + int* cam_orthographic; + mjtNum* cam_fovy; + mjtNum* cam_ipd; + int* cam_resolution; + float* cam_sensorsize; + float* cam_intrinsic; + + mjtByte* light_directional; + mjtByte* light_castshadow; + float* light_bulbradius; + mjtByte* light_active; + float* light_attenuation; + float* light_cutoff; + float* light_exponent; + float* light_ambient; + float* light_diffuse; + float* light_specular; + + mjtByte* flex_flatskin; + int* flex_dim; + int* flex_matid; + int* flex_group; + int* flex_interp; + int* flex_nodeadr; + int* flex_nodenum; + int* flex_nodebodyid; + int* flex_vertadr; + int* flex_vertnum; + int* flex_elem; + int* flex_elemtexcoord; + int* flex_elemlayer; + int* flex_elemadr; + int* flex_elemnum; + int* flex_elemdataadr; + int* flex_shell; + int* flex_shellnum; + int* flex_shelldataadr; + int* flex_texcoordadr; + int* flex_bvhadr; + int* flex_bvhnum; + mjtByte* flex_centered; + mjtNum* flex_node; + mjtNum* flex_radius; + float* flex_rgba; + float* flex_texcoord; + + int* hfield_pathadr; + + int* mesh_bvhadr; + int* mesh_bvhnum; + int* mesh_texcoordadr; + int* mesh_graphadr; + int* mesh_pathadr; + + int* skin_matid; + int* skin_group; + float* skin_rgba; + float* skin_inflate; + int* skin_vertadr; + int* skin_vertnum; + int* skin_texcoordadr; + int* skin_faceadr; + int* skin_facenum; + int* skin_boneadr; + int* skin_bonenum; + float* skin_vert; + int* skin_face; + int* skin_bonevertadr; + int* skin_bonevertnum; + float* skin_bonebindpos; + float* skin_bonebindquat; + int* skin_bonebodyid; + int* skin_bonevertid; + float* skin_bonevertweight; + int* skin_pathadr; + + int* tex_pathadr; + + int* mat_texid; + mjtByte* mat_texuniform; + float* mat_texrepeat; + float* mat_emission; + float* mat_specular; + float* mat_shininess; + float* mat_reflectance; + float* mat_metallic; + float* mat_roughness; + float* mat_rgba; + + int* eq_type; + int* eq_obj1id; + int* eq_obj2id; + int* eq_objtype; + mjtNum* eq_data; + + int* tendon_num; + int* tendon_matid; + int* tendon_group; + mjtByte* tendon_limited; + mjtByte* tendon_actfrclimited; + mjtNum* tendon_width; + mjtNum* tendon_range; + mjtNum* tendon_actfrcrange; + mjtNum* tendon_stiffness; + mjtNum* tendon_damping; + mjtNum* tendon_frictionloss; + mjtNum* tendon_lengthspring; + float* tendon_rgba; + + int* actuator_trntype; + int* actuator_dyntype; + int* actuator_trnid; + int* actuator_actadr; + int* actuator_actnum; + int* actuator_group; + mjtByte* actuator_ctrllimited; + mjtByte* actuator_actlimited; + mjtNum* actuator_ctrlrange; + mjtNum* actuator_actrange; + mjtNum* actuator_cranklength; + + int* sensor_type; + int* sensor_objid; + int* sensor_adr; + + int* name_bodyadr; + int* name_jntadr; + int* name_geomadr; + int* name_siteadr; + int* name_camadr; + int* name_lightadr; + int* name_eqadr; + int* name_tendonadr; + int* name_actuatoradr; + char* names; + char* paths; + } model; + + // fields in mjData that are necessary to re-render a scene + struct { + mjWarningStat warning[mjNWARNING]; + + int nefc; + int ncon; + int nisland; + + mjtNum time; + + mjtNum* act; + + mjtNum* ctrl; + mjtNum* xfrc_applied; + mjtByte* eq_active; + + mjtNum* sensordata; + + mjtNum* xpos; + mjtNum* xquat; + mjtNum* xmat; + mjtNum* xipos; + mjtNum* ximat; + mjtNum* xanchor; + mjtNum* xaxis; + mjtNum* geom_xpos; + mjtNum* geom_xmat; + mjtNum* site_xpos; + mjtNum* site_xmat; + mjtNum* cam_xpos; + mjtNum* cam_xmat; + mjtNum* light_xpos; + mjtNum* light_xdir; + + mjtNum* subtree_com; + + int* ten_wrapadr; + int* ten_wrapnum; + int* wrap_obj; + mjtNum* ten_length; + mjtNum* wrap_xpos; + + mjtNum* bvh_aabb_dyn; + mjtByte* bvh_active; + int* island_dofadr; + int* island_dofind; + int* dof_island; + int* efc_island; + int* tendon_efcadr; + + mjtNum* flexvert_xpos; + + mjContact* contact; + mjtNum* efc_force; + void* arena; + } data; +}; +typedef struct mjvSceneState_ mjvSceneState; + +#endif // MUJOCO_MJVISUALIZE_H_ diff --git a/vendor/include/mujoco/mjxmacro.h b/vendor/include/mujoco/mjxmacro.h new file mode 100644 index 0000000..7c73c59 --- /dev/null +++ b/vendor/include/mujoco/mjxmacro.h @@ -0,0 +1,814 @@ +// 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_MJXMACRO_H_ +#define MUJOCO_MJXMACRO_H_ + + +//-------------------------------- mjOption -------------------------------------------------------- + +// scalar fields of mjOption +#define MJOPTION_FLOATS \ + X( mjtNum, timestep ) \ + X( mjtNum, apirate ) \ + X( mjtNum, impratio ) \ + X( mjtNum, tolerance ) \ + X( mjtNum, ls_tolerance ) \ + X( mjtNum, noslip_tolerance ) \ + X( mjtNum, ccd_tolerance ) \ + X( mjtNum, density ) \ + X( mjtNum, viscosity ) \ + X( mjtNum, o_margin ) \ + + +#define MJOPTION_INTS \ + X( int, integrator ) \ + X( int, cone ) \ + X( int, jacobian ) \ + X( int, solver ) \ + X( int, iterations ) \ + X( int, ls_iterations ) \ + X( int, noslip_iterations ) \ + X( int, ccd_iterations ) \ + X( int, disableflags ) \ + X( int, enableflags ) \ + X( int, disableactuator ) \ + X( int, sdf_initpoints ) \ + X( int, sdf_iterations ) + + +#define MJOPTION_SCALARS \ + MJOPTION_FLOATS \ + MJOPTION_INTS + + +// vector fields of mjOption +#define MJOPTION_VECTORS \ + X( gravity, 3 ) \ + X( wind, 3 ) \ + X( magnetic, 3 ) \ + X( o_solref, mjNREF ) \ + X( o_solimp, mjNIMP ) \ + X( o_friction, 5 ) + + +//-------------------------------- mjModel --------------------------------------------------------- + +// int fields of mjModel +#define MJMODEL_INTS \ + X ( nq ) \ + XMJV( nv ) \ + XMJV( nu ) \ + XMJV( na ) \ + XMJV( nbody ) \ + XMJV( nbvh ) \ + XMJV( nbvhstatic ) \ + X ( nbvhdynamic ) \ + XMJV( njnt ) \ + XMJV( ngeom ) \ + XMJV( nsite ) \ + XMJV( ncam ) \ + XMJV( nlight ) \ + XMJV( nflex ) \ + X ( nflexnode ) \ + XMJV( nflexvert ) \ + X ( nflexedge ) \ + X ( nflexelem ) \ + X ( nflexelemdata ) \ + X ( nflexelemedge ) \ + X ( nflexshelldata ) \ + X ( nflexevpair ) \ + XMJV( nflextexcoord ) \ + XMJV( nmesh ) \ + X ( nmeshvert ) \ + X ( nmeshnormal ) \ + X ( nmeshtexcoord ) \ + X ( nmeshface ) \ + X ( nmeshgraph ) \ + X ( nmeshpoly ) \ + X ( nmeshpolyvert ) \ + X ( nmeshpolymap ) \ + XMJV( nskin ) \ + XMJV( nskinvert ) \ + X ( nskintexvert ) \ + XMJV( nskinface ) \ + XMJV( nskinbone ) \ + XMJV( nskinbonevert ) \ + X ( nhfield ) \ + X ( nhfielddata ) \ + X ( ntex ) \ + X ( ntexdata ) \ + XMJV( nmat ) \ + X ( npair ) \ + X ( nexclude ) \ + XMJV( neq ) \ + XMJV( ntendon ) \ + XMJV( nwrap ) \ + XMJV( nsensor ) \ + X ( nnumeric ) \ + X ( nnumericdata ) \ + X ( ntext ) \ + X ( ntextdata ) \ + X ( ntuple ) \ + X ( ntupledata ) \ + X ( nkey ) \ + X ( nmocap ) \ + X ( nplugin ) \ + X ( npluginattr ) \ + X ( nuser_body ) \ + X ( nuser_jnt ) \ + X ( nuser_geom ) \ + X ( nuser_site ) \ + X ( nuser_cam ) \ + X ( nuser_tendon ) \ + X ( nuser_actuator ) \ + X ( nuser_sensor ) \ + XMJV( nnames ) \ + XMJV( npaths ) \ + X ( nnames_map ) \ + X ( nM ) \ + X ( nB ) \ + X ( nC ) \ + X ( nD ) \ + X ( nJmom ) \ + XMJV( ntree ) \ + X ( ngravcomp ) \ + X ( nemax ) \ + X ( njmax ) \ + X ( nconmax ) \ + X ( nuserdata ) \ + XMJV( nsensordata ) \ + X ( npluginstate ) \ + X ( narena ) \ + X ( nbuffer ) + + /* nbuffer needs to be the final field */ + + +// define symbols needed in MJMODEL_POINTERS (corresponding to number of columns) +#define MJMODEL_POINTERS_PREAMBLE( m ) \ + int nuser_body = m->nuser_body; \ + int nuser_jnt = m->nuser_jnt; \ + int nuser_geom = m->nuser_geom; \ + int nuser_site = m->nuser_site; \ + int nuser_cam = m->nuser_cam; \ + int nuser_tendon = m->nuser_tendon; \ + int nuser_actuator = m->nuser_actuator; \ + int nuser_sensor = m->nuser_sensor; \ + int nq = m->nq; \ + int nv = m->nv; \ + int na = m->na; \ + int nu = m->nu; \ + int nmocap = m->nmocap; + +// macro for annotating that an array size in an X macro is a member of mjModel +// by default this macro does nothing, but users can redefine it as necessary +#define MJ_M(n) n + + +// pointer fields of mjModel +// XMJV means that the field is required to construct mjvScene +// (by default we define XMJV to be the same as X) +#define MJMODEL_POINTERS \ + X ( mjtNum, qpos0, nq, 1 ) \ + X ( mjtNum, qpos_spring, nq, 1 ) \ + XMJV( int, body_parentid, nbody, 1 ) \ + XMJV( int, body_rootid, nbody, 1 ) \ + XMJV( int, body_weldid, nbody, 1 ) \ + XMJV( int, body_mocapid, nbody, 1 ) \ + XMJV( int, body_jntnum, nbody, 1 ) \ + XMJV( int, body_jntadr, nbody, 1 ) \ + XMJV( int, body_dofnum, nbody, 1 ) \ + XMJV( int, body_dofadr, nbody, 1 ) \ + X ( int, body_treeid, nbody, 1 ) \ + XMJV( int, body_geomnum, nbody, 1 ) \ + XMJV( int, body_geomadr, nbody, 1 ) \ + X ( mjtByte, body_simple, nbody, 1 ) \ + X ( mjtByte, body_sameframe, nbody, 1 ) \ + X ( mjtNum, body_pos, nbody, 3 ) \ + X ( mjtNum, body_quat, nbody, 4 ) \ + X ( mjtNum, body_ipos, nbody, 3 ) \ + XMJV( mjtNum, body_iquat, nbody, 4 ) \ + XMJV( mjtNum, body_mass, nbody, 1 ) \ + X ( mjtNum, body_subtreemass, nbody, 1 ) \ + XMJV( mjtNum, body_inertia, nbody, 3 ) \ + X ( mjtNum, body_invweight0, nbody, 2 ) \ + X ( mjtNum, body_gravcomp, nbody, 1 ) \ + X ( mjtNum, body_margin, nbody, 1 ) \ + X ( mjtNum, body_user, nbody, MJ_M(nuser_body) ) \ + X ( int, body_plugin, nbody, 1 ) \ + X ( int, body_contype, nbody, 1 ) \ + X ( int, body_conaffinity, nbody, 1 ) \ + XMJV( int, body_bvhadr, nbody, 1 ) \ + XMJV( int, body_bvhnum, nbody, 1 ) \ + XMJV( int, bvh_depth, nbvh, 1 ) \ + XMJV( int, bvh_child, nbvh, 2 ) \ + XMJV( int, bvh_nodeid, nbvh, 1 ) \ + XMJV( mjtNum, bvh_aabb, nbvhstatic, 6 ) \ + XMJV( int, jnt_type, njnt, 1 ) \ + X ( int, jnt_qposadr, njnt, 1 ) \ + X ( int, jnt_dofadr, njnt, 1 ) \ + XMJV( int, jnt_bodyid, njnt, 1 ) \ + XMJV( int, jnt_group, njnt, 1 ) \ + X ( mjtByte, jnt_limited, njnt, 1 ) \ + X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \ + X ( mjtByte, jnt_actgravcomp, njnt, 1 ) \ + X ( mjtNum, jnt_solref, njnt, mjNREF ) \ + X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \ + X ( mjtNum, jnt_pos, njnt, 3 ) \ + X ( mjtNum, jnt_axis, njnt, 3 ) \ + X ( mjtNum, jnt_stiffness, njnt, 1 ) \ + X ( mjtNum, jnt_range, njnt, 2 ) \ + X ( mjtNum, jnt_actfrcrange, njnt, 2 ) \ + X ( mjtNum, jnt_margin, njnt, 1 ) \ + X ( mjtNum, jnt_user, njnt, MJ_M(nuser_jnt) ) \ + X ( int, dof_bodyid, nv, 1 ) \ + X ( int, dof_jntid, nv, 1 ) \ + X ( int, dof_parentid, nv, 1 ) \ + X ( int, dof_treeid, nv, 1 ) \ + X ( int, dof_Madr, nv, 1 ) \ + X ( int, dof_simplenum, nv, 1 ) \ + X ( mjtNum, dof_solref, nv, mjNREF ) \ + X ( mjtNum, dof_solimp, nv, mjNIMP ) \ + X ( mjtNum, dof_frictionloss, nv, 1 ) \ + X ( mjtNum, dof_armature, nv, 1 ) \ + X ( mjtNum, dof_damping, nv, 1 ) \ + X ( mjtNum, dof_invweight0, nv, 1 ) \ + X ( mjtNum, dof_M0, nv, 1 ) \ + XMJV( int, geom_type, ngeom, 1 ) \ + XMJV( int, geom_contype, ngeom, 1 ) \ + XMJV( int, geom_conaffinity, ngeom, 1 ) \ + X ( int, geom_condim, ngeom, 1 ) \ + XMJV( int, geom_bodyid, ngeom, 1 ) \ + XMJV( int, geom_dataid, ngeom, 1 ) \ + XMJV( int, geom_matid, ngeom, 1 ) \ + XMJV( int, geom_group, ngeom, 1 ) \ + X ( int, geom_priority, ngeom, 1 ) \ + X ( int, geom_plugin, ngeom, 1 ) \ + X ( mjtByte, geom_sameframe, ngeom, 1 ) \ + X ( mjtNum, geom_solmix, ngeom, 1 ) \ + X ( mjtNum, geom_solref, ngeom, mjNREF ) \ + X ( mjtNum, geom_solimp, ngeom, mjNIMP ) \ + XMJV( mjtNum, geom_size, ngeom, 3 ) \ + XMJV( mjtNum, geom_aabb, ngeom, 6 ) \ + XMJV( mjtNum, geom_rbound, ngeom, 1 ) \ + X ( mjtNum, geom_pos, ngeom, 3 ) \ + X ( mjtNum, geom_quat, ngeom, 4 ) \ + X ( mjtNum, geom_friction, ngeom, 3 ) \ + X ( mjtNum, geom_margin, ngeom, 1 ) \ + X ( mjtNum, geom_gap, ngeom, 1 ) \ + X ( mjtNum, geom_fluid, ngeom, mjNFLUID ) \ + X ( mjtNum, geom_user, ngeom, MJ_M(nuser_geom) ) \ + XMJV( float, geom_rgba, ngeom, 4 ) \ + XMJV( int, site_type, nsite, 1 ) \ + XMJV( int, site_bodyid, nsite, 1 ) \ + XMJV( int, site_matid, nsite, 1 ) \ + XMJV( int, site_group, nsite, 1 ) \ + X ( mjtByte, site_sameframe, nsite, 1 ) \ + XMJV( mjtNum, site_size, nsite, 3 ) \ + X ( mjtNum, site_pos, nsite, 3 ) \ + X ( mjtNum, site_quat, nsite, 4 ) \ + X ( mjtNum, site_user, nsite, MJ_M(nuser_site) ) \ + XMJV( float, site_rgba, nsite, 4 ) \ + X ( int, cam_mode, ncam, 1 ) \ + X ( int, cam_bodyid, ncam, 1 ) \ + X ( int, cam_targetbodyid, ncam, 1 ) \ + X ( mjtNum, cam_pos, ncam, 3 ) \ + X ( mjtNum, cam_quat, ncam, 4 ) \ + X ( mjtNum, cam_poscom0, ncam, 3 ) \ + X ( mjtNum, cam_pos0, ncam, 3 ) \ + X ( mjtNum, cam_mat0, ncam, 9 ) \ + XMJV( int, cam_orthographic, ncam, 1 ) \ + XMJV( mjtNum, cam_fovy, ncam, 1 ) \ + XMJV( mjtNum, cam_ipd, ncam, 1 ) \ + XMJV( int, cam_resolution, ncam, 2 ) \ + XMJV( float, cam_sensorsize, ncam, 2 ) \ + XMJV( float, cam_intrinsic, ncam, 4 ) \ + X ( mjtNum, cam_user, ncam, MJ_M(nuser_cam) ) \ + X ( int, light_mode, nlight, 1 ) \ + X ( int, light_bodyid, nlight, 1 ) \ + X ( int, light_targetbodyid, nlight, 1 ) \ + XMJV( mjtByte, light_directional, nlight, 1 ) \ + XMJV( mjtByte, light_castshadow, nlight, 1 ) \ + XMJV( float, light_bulbradius, nlight, 1 ) \ + XMJV( mjtByte, light_active, nlight, 1 ) \ + X ( mjtNum, light_pos, nlight, 3 ) \ + X ( mjtNum, light_dir, nlight, 3 ) \ + X ( mjtNum, light_poscom0, nlight, 3 ) \ + X ( mjtNum, light_pos0, nlight, 3 ) \ + X ( mjtNum, light_dir0, nlight, 3 ) \ + XMJV( float, light_attenuation, nlight, 3 ) \ + XMJV( float, light_cutoff, nlight, 1 ) \ + XMJV( float, light_exponent, nlight, 1 ) \ + XMJV( float, light_ambient, nlight, 3 ) \ + XMJV( float, light_diffuse, nlight, 3 ) \ + XMJV( float, light_specular, nlight, 3 ) \ + X ( int, flex_contype, nflex, 1 ) \ + X ( int, flex_conaffinity, nflex, 1 ) \ + X ( int, flex_condim, nflex, 1 ) \ + X ( int, flex_priority, nflex, 1 ) \ + X ( mjtNum, flex_solmix, nflex, 1 ) \ + X ( mjtNum, flex_solref, nflex, mjNREF ) \ + X ( mjtNum, flex_solimp, nflex, mjNIMP ) \ + X ( mjtNum, flex_friction, nflex, 3 ) \ + X ( mjtNum, flex_margin, nflex, 1 ) \ + X ( mjtNum, flex_gap, nflex, 1 ) \ + X ( mjtByte, flex_internal, nflex, 1 ) \ + X ( int, flex_selfcollide, nflex, 1 ) \ + X ( int, flex_activelayers, nflex, 1 ) \ + XMJV( int, flex_dim, nflex, 1 ) \ + XMJV( int, flex_matid, nflex, 1 ) \ + XMJV( int, flex_group, nflex, 1 ) \ + XMJV( int, flex_interp, nflex, 1 ) \ + XMJV( int, flex_nodeadr, nflex, 1 ) \ + XMJV( int, flex_nodenum, nflex, 1 ) \ + XMJV( int, flex_vertadr, nflex, 1 ) \ + XMJV( int, flex_vertnum, nflex, 1 ) \ + X ( int, flex_edgeadr, nflex, 1 ) \ + X ( int, flex_edgenum, nflex, 1 ) \ + XMJV( int, flex_elemadr, nflex, 1 ) \ + XMJV( int, flex_elemnum, nflex, 1 ) \ + XMJV( int, flex_elemdataadr, nflex, 1 ) \ + X ( int, flex_elemedgeadr, nflex, 1 ) \ + XMJV( int, flex_shellnum, nflex, 1 ) \ + XMJV( int, flex_shelldataadr, nflex, 1 ) \ + X ( int, flex_evpairadr, nflex, 1 ) \ + X ( int, flex_evpairnum, nflex, 1 ) \ + XMJV( int, flex_texcoordadr, nflex, 1 ) \ + XMJV( int, flex_nodebodyid, nflexnode, 1 ) \ + X ( int, flex_vertbodyid, nflexvert, 1 ) \ + X ( int, flex_edge, nflexedge, 2 ) \ + XMJV( int, flex_elem, nflexelemdata, 1 ) \ + XMJV( int, flex_elemtexcoord, nflexelemdata, 1 ) \ + X ( int, flex_elemedge, nflexelemedge, 1 ) \ + XMJV( int, flex_elemlayer, nflexelem, 1 ) \ + XMJV( int, flex_shell, nflexshelldata,1 ) \ + X ( int, flex_evpair, nflexevpair, 2 ) \ + X ( mjtNum, flex_vert, nflexvert, 3 ) \ + X ( mjtNum, flex_vert0, nflexvert, 3 ) \ + XMJV( mjtNum, flex_node, nflexnode, 3 ) \ + X ( mjtNum, flex_node0, nflexnode, 3 ) \ + X ( mjtNum, flexedge_length0, nflexedge, 1 ) \ + X ( mjtNum, flexedge_invweight0, nflexedge, 1 ) \ + XMJV( mjtNum, flex_radius, nflex, 1 ) \ + X ( mjtNum, flex_stiffness, nflexelem, 21 ) \ + X ( mjtNum, flex_damping, nflex, 1 ) \ + X ( mjtNum, flex_edgestiffness, nflex, 1 ) \ + X ( mjtNum, flex_edgedamping, nflex, 1 ) \ + X ( mjtByte, flex_edgeequality, nflex, 1 ) \ + X ( mjtByte, flex_rigid, nflex, 1 ) \ + X ( mjtByte, flexedge_rigid, nflexedge, 1 ) \ + XMJV( mjtByte, flex_centered, nflex, 1 ) \ + XMJV( mjtByte, flex_flatskin, nflex, 1 ) \ + XMJV( int, flex_bvhadr, nflex, 1 ) \ + XMJV( int, flex_bvhnum, nflex, 1 ) \ + XMJV( float, flex_rgba, nflex, 4 ) \ + XMJV( float, flex_texcoord, nflextexcoord, 2 ) \ + X ( int, mesh_vertadr, nmesh, 1 ) \ + X ( int, mesh_vertnum, nmesh, 1 ) \ + X ( int, mesh_normaladr, nmesh, 1 ) \ + X ( int, mesh_normalnum, nmesh, 1 ) \ + XMJV( int, mesh_texcoordadr, nmesh, 1 ) \ + X ( int, mesh_texcoordnum, nmesh, 1 ) \ + X ( int, mesh_faceadr, nmesh, 1 ) \ + X ( int, mesh_facenum, nmesh, 1 ) \ + XMJV( int, mesh_bvhadr, nmesh, 1 ) \ + XMJV( int, mesh_bvhnum, nmesh, 1 ) \ + XMJV( int, mesh_graphadr, nmesh, 1 ) \ + X ( mjtNum, mesh_scale, nmesh, 3 ) \ + X ( mjtNum, mesh_pos, nmesh, 3 ) \ + X ( mjtNum, mesh_quat, nmesh, 4 ) \ + XNV ( float, mesh_vert, nmeshvert, 3 ) \ + XNV ( float, mesh_normal, nmeshnormal, 3 ) \ + XNV ( float, mesh_texcoord, nmeshtexcoord, 2 ) \ + XNV ( int, mesh_face, nmeshface, 3 ) \ + XNV ( int, mesh_facenormal, nmeshface, 3 ) \ + XNV ( int, mesh_facetexcoord, nmeshface, 3 ) \ + XNV ( int, mesh_graph, nmeshgraph, 1 ) \ + XMJV( int, mesh_pathadr, nmesh, 1 ) \ + X ( int, mesh_polynum, nmesh, 1 ) \ + X ( int, mesh_polyadr, nmesh, 1 ) \ + X ( mjtNum, mesh_polynormal, nmeshpoly, 3 ) \ + X ( int, mesh_polyvertadr, nmeshpoly, 1 ) \ + X ( int, mesh_polyvertnum, nmeshpoly, 1 ) \ + X ( int, mesh_polyvert, nmeshpolyvert, 1 ) \ + X ( int, mesh_polymapadr, nmeshvert, 1 ) \ + X ( int, mesh_polymapnum, nmeshvert, 1 ) \ + X ( int, mesh_polymap, nmeshpolymap, 1 ) \ + XMJV( int, skin_matid, nskin, 1 ) \ + XMJV( int, skin_group, nskin, 1 ) \ + XMJV( float, skin_rgba, nskin, 4 ) \ + XMJV( float, skin_inflate, nskin, 1 ) \ + XMJV( int, skin_vertadr, nskin, 1 ) \ + XMJV( int, skin_vertnum, nskin, 1 ) \ + XMJV( int, skin_texcoordadr, nskin, 1 ) \ + XMJV( int, skin_faceadr, nskin, 1 ) \ + XMJV( int, skin_facenum, nskin, 1 ) \ + XMJV( int, skin_boneadr, nskin, 1 ) \ + XMJV( int, skin_bonenum, nskin, 1 ) \ + XMJV( float, skin_vert, nskinvert, 3 ) \ + X ( float, skin_texcoord, nskintexvert, 2 ) \ + XMJV( int, skin_face, nskinface, 3 ) \ + XMJV( int, skin_bonevertadr, nskinbone, 1 ) \ + XMJV( int, skin_bonevertnum, nskinbone, 1 ) \ + XMJV( float, skin_bonebindpos, nskinbone, 3 ) \ + XMJV( float, skin_bonebindquat, nskinbone, 4 ) \ + XMJV( int, skin_bonebodyid, nskinbone, 1 ) \ + XMJV( int, skin_bonevertid, nskinbonevert, 1 ) \ + XMJV( float, skin_bonevertweight, nskinbonevert, 1 ) \ + XMJV( int, skin_pathadr, nskin, 1 ) \ + X ( mjtNum, hfield_size, nhfield, 4 ) \ + X ( int, hfield_nrow, nhfield, 1 ) \ + X ( int, hfield_ncol, nhfield, 1 ) \ + X ( int, hfield_adr, nhfield, 1 ) \ + XNV ( float, hfield_data, nhfielddata, 1 ) \ + XMJV( int, hfield_pathadr, nhfield, 1 ) \ + X ( int, tex_type, ntex, 1 ) \ + X ( int, tex_height, ntex, 1 ) \ + X ( int, tex_width, ntex, 1 ) \ + X ( int, tex_nchannel, ntex, 1 ) \ + X ( int, tex_adr, ntex, 1 ) \ + XNV ( mjtByte, tex_data, ntexdata, 1 ) \ + XMJV( int, tex_pathadr, ntex, 1 ) \ + XMJV( int, mat_texid, nmat, mjNTEXROLE ) \ + XMJV( mjtByte, mat_texuniform, nmat, 1 ) \ + XMJV( float, mat_texrepeat, nmat, 2 ) \ + XMJV( float, mat_emission, nmat, 1 ) \ + XMJV( float, mat_specular, nmat, 1 ) \ + XMJV( float, mat_shininess, nmat, 1 ) \ + XMJV( float, mat_reflectance, nmat, 1 ) \ + XMJV( float, mat_metallic, nmat, 1 ) \ + XMJV( float, mat_roughness, nmat, 1 ) \ + XMJV( float, mat_rgba, nmat, 4 ) \ + X ( int, pair_dim, npair, 1 ) \ + X ( int, pair_geom1, npair, 1 ) \ + X ( int, pair_geom2, npair, 1 ) \ + X ( int, pair_signature, npair, 1 ) \ + X ( mjtNum, pair_solref, npair, mjNREF ) \ + X ( mjtNum, pair_solreffriction, npair, mjNREF ) \ + X ( mjtNum, pair_solimp, npair, mjNIMP ) \ + X ( mjtNum, pair_margin, npair, 1 ) \ + X ( mjtNum, pair_gap, npair, 1 ) \ + X ( mjtNum, pair_friction, npair, 5 ) \ + X ( int, exclude_signature, nexclude, 1 ) \ + XMJV( int, eq_type, neq, 1 ) \ + XMJV( int, eq_obj1id, neq, 1 ) \ + XMJV( int, eq_obj2id, neq, 1 ) \ + XMJV( int, eq_objtype, neq, 1 ) \ + X ( mjtByte, eq_active0, neq, 1 ) \ + X ( mjtNum, eq_solref, neq, mjNREF ) \ + X ( mjtNum, eq_solimp, neq, mjNIMP ) \ + XMJV( mjtNum, eq_data, neq, mjNEQDATA ) \ + X ( int, tendon_adr, ntendon, 1 ) \ + XMJV( int, tendon_num, ntendon, 1 ) \ + XMJV( int, tendon_matid, ntendon, 1 ) \ + XMJV( int, tendon_group, ntendon, 1 ) \ + XMJV( mjtByte, tendon_limited, ntendon, 1 ) \ + XMJV( mjtByte, tendon_actfrclimited, ntendon, 1 ) \ + XMJV( mjtNum, tendon_width, ntendon, 1 ) \ + X ( mjtNum, tendon_solref_lim, ntendon, mjNREF ) \ + X ( mjtNum, tendon_solimp_lim, ntendon, mjNIMP ) \ + X ( mjtNum, tendon_solref_fri, ntendon, mjNREF ) \ + X ( mjtNum, tendon_solimp_fri, ntendon, mjNIMP ) \ + XMJV( mjtNum, tendon_range, ntendon, 2 ) \ + XMJV( mjtNum, tendon_actfrcrange, ntendon, 2 ) \ + X ( mjtNum, tendon_margin, ntendon, 1 ) \ + XMJV( mjtNum, tendon_stiffness, ntendon, 1 ) \ + XMJV( mjtNum, tendon_damping, ntendon, 1 ) \ + X ( mjtNum, tendon_armature, ntendon, 1 ) \ + XMJV( mjtNum, tendon_frictionloss, ntendon, 1 ) \ + XMJV( mjtNum, tendon_lengthspring, ntendon, 2 ) \ + X ( mjtNum, tendon_length0, ntendon, 1 ) \ + X ( mjtNum, tendon_invweight0, ntendon, 1 ) \ + X ( mjtNum, tendon_user, ntendon, MJ_M(nuser_tendon) ) \ + XMJV( float, tendon_rgba, ntendon, 4 ) \ + X ( int, wrap_type, nwrap, 1 ) \ + X ( int, wrap_objid, nwrap, 1 ) \ + X ( mjtNum, wrap_prm, nwrap, 1 ) \ + XMJV( int, actuator_trntype, nu, 1 ) \ + XMJV( int, actuator_dyntype, nu, 1 ) \ + X ( int, actuator_gaintype, nu, 1 ) \ + X ( int, actuator_biastype, nu, 1 ) \ + XMJV( int, actuator_trnid, nu, 2 ) \ + XMJV( int, actuator_actadr, nu, 1 ) \ + XMJV( int, actuator_actnum, nu, 1 ) \ + XMJV( int, actuator_group, nu, 1 ) \ + XMJV( mjtByte, actuator_ctrllimited, nu, 1 ) \ + X ( mjtByte, actuator_forcelimited, nu, 1 ) \ + XMJV( mjtByte, actuator_actlimited, nu, 1 ) \ + X ( mjtNum, actuator_dynprm, nu, mjNDYN ) \ + X ( mjtNum, actuator_gainprm, nu, mjNGAIN ) \ + X ( mjtNum, actuator_biasprm, nu, mjNBIAS ) \ + X ( mjtByte, actuator_actearly, nu, 1 ) \ + XMJV( mjtNum, actuator_ctrlrange, nu, 2 ) \ + X ( mjtNum, actuator_forcerange, nu, 2 ) \ + XMJV( mjtNum, actuator_actrange, nu, 2 ) \ + X ( mjtNum, actuator_gear, nu, 6 ) \ + XMJV( mjtNum, actuator_cranklength, nu, 1 ) \ + X ( mjtNum, actuator_acc0, nu, 1 ) \ + X ( mjtNum, actuator_length0, nu, 1 ) \ + X ( mjtNum, actuator_lengthrange, nu, 2 ) \ + X ( mjtNum, actuator_user, nu, MJ_M(nuser_actuator) ) \ + X ( int, actuator_plugin, nu, 1 ) \ + XMJV( int, sensor_type, nsensor, 1 ) \ + X ( int, sensor_datatype, nsensor, 1 ) \ + X ( int, sensor_needstage, nsensor, 1 ) \ + X ( int, sensor_objtype, nsensor, 1 ) \ + XMJV( int, sensor_objid, nsensor, 1 ) \ + X ( int, sensor_reftype, nsensor, 1 ) \ + X ( int, sensor_refid, nsensor, 1 ) \ + X ( int, sensor_dim, nsensor, 1 ) \ + XMJV( int, sensor_adr, nsensor, 1 ) \ + X ( mjtNum, sensor_cutoff, nsensor, 1 ) \ + X ( mjtNum, sensor_noise, nsensor, 1 ) \ + X ( mjtNum, sensor_user, nsensor, MJ_M(nuser_sensor) ) \ + X ( int, sensor_plugin, nsensor, 1 ) \ + X ( int, plugin, nplugin, 1 ) \ + X ( int, plugin_stateadr, nplugin, 1 ) \ + X ( int, plugin_statenum, nplugin, 1 ) \ + X ( char, plugin_attr, npluginattr, 1 ) \ + X ( int, plugin_attradr, nplugin, 1 ) \ + X ( int, numeric_adr, nnumeric, 1 ) \ + X ( int, numeric_size, nnumeric, 1 ) \ + X ( mjtNum, numeric_data, nnumericdata, 1 ) \ + X ( int, text_adr, ntext, 1 ) \ + X ( int, text_size, ntext, 1 ) \ + X ( char, text_data, ntextdata, 1 ) \ + X ( int, tuple_adr, ntuple, 1 ) \ + X ( int, tuple_size, ntuple, 1 ) \ + X ( int, tuple_objtype, ntupledata, 1 ) \ + X ( int, tuple_objid, ntupledata, 1 ) \ + X ( mjtNum, tuple_objprm, ntupledata, 1 ) \ + X ( mjtNum, key_time, nkey, 1 ) \ + X ( mjtNum, key_qpos, nkey, MJ_M(nq) ) \ + X ( mjtNum, key_qvel, nkey, MJ_M(nv) ) \ + X ( mjtNum, key_act, nkey, MJ_M(na) ) \ + X ( mjtNum, key_mpos, nkey, MJ_M(nmocap)*3 ) \ + X ( mjtNum, key_mquat, nkey, MJ_M(nmocap)*4 ) \ + X ( mjtNum, key_ctrl, nkey, MJ_M(nu) ) \ + XMJV( int, name_bodyadr, nbody, 1 ) \ + XMJV( int, name_jntadr, njnt, 1 ) \ + XMJV( int, name_geomadr, ngeom, 1 ) \ + XMJV( int, name_siteadr, nsite, 1 ) \ + XMJV( int, name_camadr, ncam, 1 ) \ + XMJV( int, name_lightadr, nlight, 1 ) \ + X ( int, name_flexadr, nflex, 1 ) \ + X ( int, name_meshadr, nmesh, 1 ) \ + X ( int, name_skinadr, nskin, 1 ) \ + X ( int, name_hfieldadr, nhfield, 1 ) \ + X ( int, name_texadr, ntex, 1 ) \ + X ( int, name_matadr, nmat, 1 ) \ + X ( int, name_pairadr, npair, 1 ) \ + X ( int, name_excludeadr, nexclude, 1 ) \ + XMJV( int, name_eqadr, neq, 1 ) \ + XMJV( int, name_tendonadr, ntendon, 1 ) \ + XMJV( int, name_actuatoradr, nu, 1 ) \ + X ( int, name_sensoradr, nsensor, 1 ) \ + X ( int, name_numericadr, nnumeric, 1 ) \ + X ( int, name_textadr, ntext, 1 ) \ + X ( int, name_tupleadr, ntuple, 1 ) \ + X ( int, name_keyadr, nkey, 1 ) \ + X ( int, name_pluginadr, nplugin, 1 ) \ + XMJV( char, names, nnames, 1 ) \ + X ( int, names_map, nnames_map, 1 ) \ + XMJV( char, paths, npaths, 1 ) \ + +//-------------------------------- mjData ---------------------------------------------------------- + +// define symbols needed in MJDATA_POINTERS (corresponding to number of columns) +#define MJDATA_POINTERS_PREAMBLE( m ) \ + int nv = m->nv; + + +// pointer fields of mjData +// XMJV means that the field is required to construct mjvScene +// (by default we define XMJV to be the same as X) +#define MJDATA_POINTERS \ + X ( mjtNum, qpos, nq, 1 ) \ + X ( mjtNum, qvel, nv, 1 ) \ + XMJV( mjtNum, act, na, 1 ) \ + X ( mjtNum, qacc_warmstart, nv, 1 ) \ + X ( mjtNum, plugin_state, npluginstate, 1 ) \ + XMJV( mjtNum, ctrl, nu, 1 ) \ + X ( mjtNum, qfrc_applied, nv, 1 ) \ + XMJV( mjtNum, xfrc_applied, nbody, 6 ) \ + XMJV( mjtByte, eq_active, neq, 1 ) \ + X ( mjtNum, mocap_pos, nmocap, 3 ) \ + X ( mjtNum, mocap_quat, nmocap, 4 ) \ + X ( mjtNum, qacc, nv, 1 ) \ + X ( mjtNum, act_dot, na, 1 ) \ + X ( mjtNum, userdata, nuserdata, 1 ) \ + XMJV( mjtNum, sensordata, nsensordata, 1 ) \ + X ( int, plugin, nplugin, 1 ) \ + X ( uintptr_t, plugin_data, nplugin, 1 ) \ + XMJV( mjtNum, xpos, nbody, 3 ) \ + XMJV( mjtNum, xquat, nbody, 4 ) \ + XMJV( mjtNum, xmat, nbody, 9 ) \ + XMJV( mjtNum, xipos, nbody, 3 ) \ + XMJV( mjtNum, ximat, nbody, 9 ) \ + XMJV( mjtNum, xanchor, njnt, 3 ) \ + XMJV( mjtNum, xaxis, njnt, 3 ) \ + XMJV( mjtNum, geom_xpos, ngeom, 3 ) \ + XMJV( mjtNum, geom_xmat, ngeom, 9 ) \ + XMJV( mjtNum, site_xpos, nsite, 3 ) \ + XMJV( mjtNum, site_xmat, nsite, 9 ) \ + XMJV( mjtNum, cam_xpos, ncam, 3 ) \ + XMJV( mjtNum, cam_xmat, ncam, 9 ) \ + XMJV( mjtNum, light_xpos, nlight, 3 ) \ + XMJV( mjtNum, light_xdir, nlight, 3 ) \ + XMJV( mjtNum, subtree_com, nbody, 3 ) \ + X ( mjtNum, cdof, nv, 6 ) \ + X ( mjtNum, cinert, nbody, 10 ) \ + XMJV( mjtNum, flexvert_xpos, nflexvert, 3 ) \ + X ( mjtNum, flexelem_aabb, nflexelem, 6 ) \ + X ( int, flexedge_J_rownnz, nflexedge, 1 ) \ + X ( int, flexedge_J_rowadr, nflexedge, 1 ) \ + X ( int, flexedge_J_colind, nflexedge, MJ_M(nv) ) \ + X ( mjtNum, flexedge_J, nflexedge, MJ_M(nv) ) \ + X ( mjtNum, flexedge_length, nflexedge, 1 ) \ + XMJV( int, ten_wrapadr, ntendon, 1 ) \ + XMJV( int, ten_wrapnum, ntendon, 1 ) \ + X ( int, ten_J_rownnz, ntendon, 1 ) \ + X ( int, ten_J_rowadr, ntendon, 1 ) \ + X ( int, ten_J_colind, ntendon, MJ_M(nv) ) \ + XMJV( mjtNum, ten_length, ntendon, 1 ) \ + X ( mjtNum, ten_J, ntendon, MJ_M(nv) ) \ + XMJV( int, wrap_obj, nwrap, 2 ) \ + XMJV( mjtNum, wrap_xpos, nwrap, 6 ) \ + X ( mjtNum, actuator_length, nu, 1 ) \ + X ( int, moment_rownnz, nu, 1 ) \ + X ( int, moment_rowadr, nu, 1 ) \ + X ( int, moment_colind, nJmom, 1 ) \ + X ( mjtNum, actuator_moment, nJmom, 1 ) \ + X ( mjtNum, crb, nbody, 10 ) \ + X ( mjtNum, qM, nM, 1 ) \ + X ( mjtNum, qLD, nM, 1 ) \ + X ( mjtNum, qLDiagInv, nv, 1 ) \ + XMJV( mjtNum, bvh_aabb_dyn, nbvhdynamic, 6 ) \ + XMJV( mjtByte, bvh_active, nbvh, 1 ) \ + X ( mjtNum, flexedge_velocity, nflexedge, 1 ) \ + X ( mjtNum, ten_velocity, ntendon, 1 ) \ + X ( mjtNum, actuator_velocity, nu, 1 ) \ + X ( mjtNum, cvel, nbody, 6 ) \ + X ( mjtNum, cdof_dot, nv, 6 ) \ + X ( mjtNum, qfrc_bias, nv, 1 ) \ + X ( mjtNum, qfrc_spring, nv, 1 ) \ + X ( mjtNum, qfrc_damper, nv, 1 ) \ + X ( mjtNum, qfrc_gravcomp, nv, 1 ) \ + X ( mjtNum, qfrc_fluid, nv, 1 ) \ + X ( mjtNum, qfrc_passive, nv, 1 ) \ + X ( mjtNum, subtree_linvel, nbody, 3 ) \ + X ( mjtNum, subtree_angmom, nbody, 3 ) \ + X ( mjtNum, qH, nM, 1 ) \ + X ( mjtNum, qHDiagInv, nv, 1 ) \ + X ( int, B_rownnz, nbody, 1 ) \ + X ( int, B_rowadr, nbody, 1 ) \ + X ( int, B_colind, nB, 1 ) \ + X ( int, M_rownnz, nv, 1 ) \ + X ( int, M_rowadr, nv, 1 ) \ + X ( int, M_colind, nM, 1 ) \ + X ( int, mapM2M, nM, 1 ) \ + X ( int, C_rownnz, nv, 1 ) \ + X ( int, C_rowadr, nv, 1 ) \ + X ( int, C_colind, nC, 1 ) \ + X ( int, mapM2C, nC, 1 ) \ + X ( int, D_rownnz, nv, 1 ) \ + X ( int, D_rowadr, nv, 1 ) \ + X ( int, D_diag, nv, 1 ) \ + X ( int, D_colind, nD, 1 ) \ + X ( int, mapM2D, nD, 1 ) \ + X ( int, mapD2M, nM, 1 ) \ + X ( mjtNum, qDeriv, nD, 1 ) \ + X ( mjtNum, qLU, nD, 1 ) \ + X ( mjtNum, actuator_force, nu, 1 ) \ + X ( mjtNum, qfrc_actuator, nv, 1 ) \ + X ( mjtNum, qfrc_smooth, nv, 1 ) \ + X ( mjtNum, qacc_smooth, nv, 1 ) \ + X ( mjtNum, qfrc_constraint, nv, 1 ) \ + X ( mjtNum, qfrc_inverse, nv, 1 ) \ + X ( mjtNum, cacc, nbody, 6 ) \ + X ( mjtNum, cfrc_int, nbody, 6 ) \ + X ( mjtNum, cfrc_ext, nbody, 6 ) + + +// macro for annotating that an array size in an X macro is a member of mjData +// by default this macro does nothing, but users can redefine it as necessary +#define MJ_D(n) n + +// array of contacts +#define MJDATA_ARENA_POINTERS_CONTACT \ + X( mjContact, contact, MJ_D(ncon), 1 ) + +// array fields of mjData that are used in the primal problem +#define MJDATA_ARENA_POINTERS_SOLVER \ + X( int, efc_type, MJ_D(nefc), 1 ) \ + X( int, efc_id, MJ_D(nefc), 1 ) \ + X( int, efc_J_rownnz, MJ_D(nefc), 1 ) \ + X( int, efc_J_rowadr, MJ_D(nefc), 1 ) \ + X( int, efc_J_rowsuper, MJ_D(nefc), 1 ) \ + X( int, efc_J_colind, MJ_D(nJ), 1 ) \ + X( int, efc_JT_rownnz, MJ_M(nv), 1 ) \ + X( int, efc_JT_rowadr, MJ_M(nv), 1 ) \ + X( int, efc_JT_rowsuper, MJ_M(nv), 1 ) \ + X( int, efc_JT_colind, MJ_D(nJ), 1 ) \ + X( mjtNum, efc_J, MJ_D(nJ), 1 ) \ + X( mjtNum, efc_JT, MJ_D(nJ), 1 ) \ + X( mjtNum, efc_pos, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_margin, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_frictionloss, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_diagApprox, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_KBIP, MJ_D(nefc), 4 ) \ + X( mjtNum, efc_D, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_R, MJ_D(nefc), 1 ) \ + X( int, tendon_efcadr, MJ_M(ntendon), 1 ) \ + X( mjtNum, efc_vel, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_aref, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_b, MJ_D(nefc), 1 ) \ + X( mjtNum, efc_force, MJ_D(nefc), 1 ) \ + X( int, efc_state, MJ_D(nefc), 1 ) + +// array fields of mjData that are used in the dual problem +#define MJDATA_ARENA_POINTERS_DUAL \ + X( int, efc_AR_rownnz, MJ_D(nefc), 1 ) \ + X( int, efc_AR_rowadr, MJ_D(nefc), 1 ) \ + X( int, efc_AR_colind, MJ_D(nA), 1 ) \ + X( mjtNum, efc_AR, MJ_D(nA), 1 ) + +// array fields of mjData that are used for constraint islands +#define MJDATA_ARENA_POINTERS_ISLAND \ + X( int, dof_island, MJ_M(nv), 1 ) \ + X( int, island_dofnum, MJ_D(nisland), 1 ) \ + X( int, island_dofadr, MJ_D(nisland), 1 ) \ + X( int, island_dofind, MJ_M(nv), 1 ) \ + X( int, dof_islandind, MJ_M(nv), 1 ) \ + X( int, efc_island, MJ_D(nefc), 1 ) \ + X( int, island_efcnum, MJ_D(nisland), 1 ) \ + X( int, island_efcadr, MJ_D(nisland), 1 ) \ + X( int, island_efcind, MJ_D(nefc), 1 ) + +// array fields of mjData that live in d->arena +#define MJDATA_ARENA_POINTERS \ + MJDATA_ARENA_POINTERS_CONTACT \ + MJDATA_ARENA_POINTERS_SOLVER \ + MJDATA_ARENA_POINTERS_DUAL \ + MJDATA_ARENA_POINTERS_ISLAND + + +// scalar fields of mjData +#define MJDATA_SCALAR \ + X( size_t, narena ) \ + X( size_t, nbuffer ) \ + X( int, nplugin ) \ + X( size_t, pstack ) \ + X( size_t, pbase ) \ + X( size_t, parena ) \ + X( size_t, maxuse_stack ) \ + X( size_t, maxuse_arena ) \ + X( int, maxuse_con ) \ + X( int, maxuse_efc ) \ + X( int, ncon ) \ + X( int, ne ) \ + X( int, nf ) \ + X( int, nl ) \ + X( int, nefc ) \ + X( int, nJ ) \ + X( int, nA ) \ + X( int, nisland ) \ + X( mjtNum, time ) \ + X( uintptr_t, threadpool ) + + +// vector fields of mjData +#define MJDATA_VECTOR \ + X( size_t, maxuse_threadstack, mjMAXTHREAD, 1 ) \ + X( mjWarningStat, warning, mjNWARNING, 1 ) \ + X( mjTimerStat, timer, mjNTIMER, 1 ) \ + X( mjSolverStat, solver, mjNISLAND, mjNSOLVER ) \ + X( int, solver_niter, mjNISLAND, 1 ) \ + X( int, solver_nnz, mjNISLAND, 1 ) \ + X( mjtNum, solver_fwdinv, 2, 1 ) \ + X( mjtNum, energy, 2, 1 ) + + +// alias XMJV to be the same as X +// to obtain only X macros for fields that are relevant for mjvScene creation, +// redefine X to expand to nothing, and XMJV to do what's required +#define XMJV X + +// alias XNV to be the same as X +// to obtain only X macros for fields that are relevant for mjvScene creation, +// redefine XNV to expand to nothing +#define XNV X + +#endif // MUJOCO_MJXMACRO_H_ diff --git a/vendor/include/mujoco/mujoco.h b/vendor/include/mujoco/mujoco.h new file mode 100644 index 0000000..b23f5d9 --- /dev/null +++ b/vendor/include/mujoco/mujoco.h @@ -0,0 +1,1806 @@ +// 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_MUJOCO_H_ +#define MUJOCO_MUJOCO_H_ + +// header version; should match the library version as returned by mj_version() +#define mjVERSION_HEADER 332 + +// needed to define size_t, fabs and log10 +#include +#include + +// type definitions +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// this is a C-API +#ifdef __cplusplus +extern "C" { +#endif + +// user error and memory handlers +MJAPI extern void (*mju_user_error)(const char*); +MJAPI extern void (*mju_user_warning)(const char*); +MJAPI extern void* (*mju_user_malloc)(size_t); +MJAPI extern void (*mju_user_free)(void*); + + +// callbacks extending computation pipeline +MJAPI extern mjfGeneric mjcb_passive; +MJAPI extern mjfGeneric mjcb_control; +MJAPI extern mjfConFilt mjcb_contactfilter; +MJAPI extern mjfSensor mjcb_sensor; +MJAPI extern mjfTime mjcb_time; +MJAPI extern mjfAct mjcb_act_dyn; +MJAPI extern mjfAct mjcb_act_gain; +MJAPI extern mjfAct mjcb_act_bias; + + +// collision function table +MJAPI extern mjfCollision mjCOLLISIONFUNC[mjNGEOMTYPES][mjNGEOMTYPES]; + + +// string names +MJAPI extern const char* mjDISABLESTRING[mjNDISABLE]; +MJAPI extern const char* mjENABLESTRING[mjNENABLE]; +MJAPI extern const char* mjTIMERSTRING[mjNTIMER]; +MJAPI extern const char* mjLABELSTRING[mjNLABEL]; +MJAPI extern const char* mjFRAMESTRING[mjNFRAME]; +MJAPI extern const char* mjVISSTRING[mjNVISFLAG][3]; +MJAPI extern const char* mjRNDSTRING[mjNRNDFLAG][3]; + + +//---------------------------------- Virtual file system ------------------------------------------- + +// 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, 2: repeated name, -1: failed to load. +MJAPI int mj_addFileVFS(mjVFS* vfs, const char* directory, const char* filename); + +// Add file to VFS from buffer, return 0: success, 2: repeated name, -1: failed to load. +MJAPI int mj_addBufferVFS(mjVFS* vfs, const char* name, const void* buffer, int nbuffer); + +// 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 and deallocates VFS internal memory. +MJAPI void mj_deleteVFS(mjVFS* vfs); + + +//---------------------------------- Parse and compile --------------------------------------------- + +// Parse XML file in MJCF or URDF format, compile it, return low-level model. +// If vfs is not NULL, look up files in vfs before reading from disk. +// If error is not NULL, it must have size error_sz. +MJAPI mjModel* mj_loadXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); + +// Parse spec from XML file. +MJAPI mjSpec* mj_parseXML(const char* filename, const mjVFS* vfs, char* error, int error_sz); + +// Parse spec from XML string. +MJAPI mjSpec* mj_parseXMLString(const char* xml, const mjVFS* vfs, char* error, int error_sz); + +// Compile spec to model. +MJAPI mjModel* mj_compile(mjSpec* s, const mjVFS* vfs); + +// Recompile spec to model, preserving the state, return 0 on success. +MJAPI int mj_recompile(mjSpec* s, const mjVFS* vfs, mjModel* m, mjData* d); + +// Update XML data structures with info from low-level model created with mj_loadXML, save as MJCF. +// If error is not NULL, it must have size error_sz. +MJAPI int mj_saveLastXML(const char* filename, const mjModel* m, char* error, int error_sz); + +// Free last XML model if loaded. Called internally at each load. +MJAPI void mj_freeLastXML(void); + +// Save spec to XML string, return 0 on success, -1 on failure. +// If length of the output buffer is too small, returns the required size. +MJAPI int mj_saveXMLString(const mjSpec* s, char* xml, int xml_sz, char* error, int error_sz); + +// Save spec to XML file, return 0 on success, -1 otherwise. +MJAPI int mj_saveXML(const mjSpec* s, const char* filename, char* error, int error_sz); + + +//---------------------------------- Main simulation ----------------------------------------------- + +// Advance simulation, use control callback to obtain external force and control. +MJAPI void mj_step(const mjModel* m, mjData* d); + +// Advance simulation in two steps: before external force and control is set by user. +MJAPI void mj_step1(const mjModel* m, mjData* d); + +// Advance simulation in two steps: after external force and control is set by user. +MJAPI void mj_step2(const mjModel* m, mjData* d); + +// Forward dynamics: same as mj_step but do not integrate in time. +MJAPI void mj_forward(const mjModel* m, mjData* d); + +// Inverse dynamics: qacc must be set before calling. +MJAPI void mj_inverse(const mjModel* m, mjData* d); + +// Forward dynamics with skip; skipstage is mjtStage. +MJAPI void mj_forwardSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor); + +// Inverse dynamics with skip; skipstage is mjtStage. +MJAPI void mj_inverseSkip(const mjModel* m, mjData* d, int skipstage, int skipsensor); + + +//---------------------------------- Initialization ------------------------------------------------ + +// Set default options for length range computation. +MJAPI void mj_defaultLROpt(mjLROpt* opt); + +// Set solver parameters to default values. +MJAPI void mj_defaultSolRefImp(mjtNum* solref, mjtNum* solimp); + +// Set physics options to default values. +MJAPI void mj_defaultOption(mjOption* opt); + +// Set visual options to default values. +MJAPI void mj_defaultVisual(mjVisual* vis); + +// Copy mjModel, allocate new if dest is NULL. +MJAPI mjModel* mj_copyModel(mjModel* dest, const mjModel* src); + +// Save model to binary MJB file or memory buffer; buffer has precedence when given. +MJAPI void mj_saveModel(const mjModel* m, const char* filename, void* buffer, int buffer_sz); + +// Load model from binary MJB file. +// If vfs is not NULL, look up file in vfs before reading from disk. +MJAPI mjModel* mj_loadModel(const char* filename, const mjVFS* vfs); + +// Free memory allocation in model. +MJAPI void mj_deleteModel(mjModel* m); + +// Return size of buffer needed to hold model. +MJAPI int mj_sizeModel(const mjModel* m); + +// Allocate mjData corresponding to given model. +// If the model buffer is unallocated the initial configuration will not be set. +MJAPI mjData* mj_makeData(const mjModel* m); + +// Copy mjData. +// m is only required to contain the size fields from MJMODEL_INTS. +MJAPI mjData* mj_copyData(mjData* dest, const mjModel* m, const mjData* src); + +// Reset data to defaults. +MJAPI void mj_resetData(const mjModel* m, mjData* d); + +// Reset data to defaults, fill everything else with debug_value. +MJAPI void mj_resetDataDebug(const mjModel* m, mjData* d, unsigned char debug_value); + +// Reset data. If 0 <= key < nkey, set fields from specified keyframe. +MJAPI void mj_resetDataKeyframe(const mjModel* m, mjData* d, int key); + +#ifndef ADDRESS_SANITIZER // Stack management functions declared in mjsan.h if ASAN is active. + +// Mark a new frame on the mjData stack. +MJAPI void mj_markStack(mjData* d); + +// Free the current mjData stack frame. All pointers returned by mj_stackAlloc since the last call +// to mj_markStack must no longer be used afterwards. +MJAPI void mj_freeStack(mjData* d); + +#endif // ADDRESS_SANITIZER + +// Allocate a number of bytes on mjData stack at a specific alignment. +// Call mju_error on stack overflow. +MJAPI void* mj_stackAllocByte(mjData* d, size_t bytes, size_t alignment); + +// Allocate array of mjtNums on mjData stack. Call mju_error on stack overflow. +MJAPI mjtNum* mj_stackAllocNum(mjData* d, size_t size); + +// Allocate array of ints on mjData stack. Call mju_error on stack overflow. +MJAPI int* mj_stackAllocInt(mjData* d, size_t size); + +// Free memory allocation in mjData. +MJAPI void mj_deleteData(mjData* d); + +// Reset all callbacks to NULL pointers (NULL is the default). +MJAPI void mj_resetCallbacks(void); + +// Set constant fields of mjModel, corresponding to qpos0 configuration. +MJAPI void mj_setConst(mjModel* m, mjData* d); + +// Set actuator_lengthrange for specified actuator; return 1 if ok, 0 if error. +MJAPI int mj_setLengthRange(mjModel* m, mjData* d, int index, + const mjLROpt* opt, char* error, int error_sz); + +// Create empty spec. +MJAPI mjSpec* mj_makeSpec(void); + +// Copy spec. +MJAPI mjSpec* mj_copySpec(const mjSpec* s); + +// Free memory allocation in mjSpec. +MJAPI void mj_deleteSpec(mjSpec* s); + +// Activate plugin. Returns 0 on success. +MJAPI int mjs_activatePlugin(mjSpec* s, const char* name); + +// Turn deep copy on or off attach. Returns 0 on success. +MJAPI int mjs_setDeepCopy(mjSpec* s, int deepcopy); + + +//---------------------------------- Printing ------------------------------------------------------ + +// Print mjModel to text file, specifying format. +// float_format must be a valid printf-style format string for a single float value. +MJAPI void mj_printFormattedModel(const mjModel* m, const char* filename, const char* float_format); + +// Print model to text file. +MJAPI void mj_printModel(const mjModel* m, const char* filename); + +// Print mjData to text file, specifying format. +// float_format must be a valid printf-style format string for a single float value +MJAPI void mj_printFormattedData(const mjModel* m, const mjData* d, const char* filename, + const char* float_format); + +// Print data to text file. +MJAPI void mj_printData(const mjModel* m, const mjData* d, const char* filename); + +// Print matrix to screen. +MJAPI void mju_printMat(const mjtNum* mat, int nr, int nc); + +// Print sparse matrix to screen. +MJAPI void mju_printMatSparse(const mjtNum* mat, int nr, + const int* rownnz, const int* rowadr, const int* colind); + +// Print internal XML schema as plain text or HTML, with style-padding or  . +MJAPI int mj_printSchema(const char* filename, char* buffer, int buffer_sz, + int flg_html, int flg_pad); + + +//---------------------------------- Components ---------------------------------------------------- + +// Run position-dependent computations. +MJAPI void mj_fwdPosition(const mjModel* m, mjData* d); + +// Run velocity-dependent computations. +MJAPI void mj_fwdVelocity(const mjModel* m, mjData* d); + +// Compute actuator force qfrc_actuator. +MJAPI void mj_fwdActuation(const mjModel* m, mjData* d); + +// Add up all non-constraint forces, compute qacc_smooth. +MJAPI void mj_fwdAcceleration(const mjModel* m, mjData* d); + +// Run selected constraint solver. +MJAPI void mj_fwdConstraint(const mjModel* m, mjData* d); + +// Euler integrator, semi-implicit in velocity. +MJAPI void mj_Euler(const mjModel* m, mjData* d); + +// Runge-Kutta explicit order-N integrator. +MJAPI void mj_RungeKutta(const mjModel* m, mjData* d, int N); + +// Implicit-in-velocity integrators. +MJAPI void mj_implicit(const mjModel* m, mjData* d); + +// Run position-dependent computations in inverse dynamics. +MJAPI void mj_invPosition(const mjModel* m, mjData* d); + +// Run velocity-dependent computations in inverse dynamics. +MJAPI void mj_invVelocity(const mjModel* m, mjData* d); + +// Apply the analytical formula for inverse constraint dynamics. +MJAPI void mj_invConstraint(const mjModel* m, mjData* d); + +// Compare forward and inverse dynamics, save results in fwdinv. +MJAPI void mj_compareFwdInv(const mjModel* m, mjData* d); + + +//---------------------------------- Sub components ------------------------------------------------ + +// Evaluate position-dependent sensors. +MJAPI void mj_sensorPos(const mjModel* m, mjData* d); + +// Evaluate velocity-dependent sensors. +MJAPI void mj_sensorVel(const mjModel* m, mjData* d); + +// Evaluate acceleration and force-dependent sensors. +MJAPI void mj_sensorAcc(const mjModel* m, mjData* d); + +// Evaluate position-dependent energy (potential). +MJAPI void mj_energyPos(const mjModel* m, mjData* d); + +// Evaluate velocity-dependent energy (kinetic). +MJAPI void mj_energyVel(const mjModel* m, mjData* d); + +// Check qpos, reset if any element is too big or nan. +MJAPI void mj_checkPos(const mjModel* m, mjData* d); + +// Check qvel, reset if any element is too big or nan. +MJAPI void mj_checkVel(const mjModel* m, mjData* d); + +// Check qacc, reset if any element is too big or nan. +MJAPI void mj_checkAcc(const mjModel* m, mjData* d); + +// Run forward kinematics. +MJAPI void mj_kinematics(const mjModel* m, mjData* d); + +// Map inertias and motion dofs to global frame centered at CoM. +MJAPI void mj_comPos(const mjModel* m, mjData* d); + +// Compute camera and light positions and orientations. +MJAPI void mj_camlight(const mjModel* m, mjData* d); + +// Compute flex-related quantities. +MJAPI void mj_flex(const mjModel* m, mjData* d); + +// Compute tendon lengths, velocities and moment arms. +MJAPI void mj_tendon(const mjModel* m, mjData* d); + +// Compute actuator transmission lengths and moments. +MJAPI void mj_transmission(const mjModel* m, mjData* d); + +// Run composite rigid body inertia algorithm (CRB). +MJAPI void mj_crb(const mjModel* m, mjData* d); + +// Compute sparse L'*D*L factorizaton of inertia matrix. +MJAPI void mj_factorM(const mjModel* m, mjData* d); + +// Solve linear system M * x = y using factorization: x = inv(L'*D*L)*y +MJAPI void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); + +// Half of linear solve: x = sqrt(inv(D))*inv(L')*y +MJAPI void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, + const mjtNum* sqrtInvD, int n); + +// Compute cvel, cdof_dot. +MJAPI void mj_comVel(const mjModel* m, mjData* d); + +// Compute qfrc_passive from spring-dampers, gravity compensation and fluid forces. +MJAPI void mj_passive(const mjModel* m, mjData* d); + +// Sub-tree linear velocity and angular momentum: compute subtree_linvel, subtree_angmom. +MJAPI void mj_subtreeVel(const mjModel* m, mjData* d); + +// RNE: compute M(qpos)*qacc + C(qpos,qvel); flg_acc=0 removes inertial term. +MJAPI void mj_rne(const mjModel* m, mjData* d, int flg_acc, mjtNum* result); + +// RNE with complete data: compute cacc, cfrc_ext, cfrc_int. +MJAPI void mj_rnePostConstraint(const mjModel* m, mjData* d); + +// Run collision detection. +MJAPI void mj_collision(const mjModel* m, mjData* d); + +// Construct constraints. +MJAPI void mj_makeConstraint(const mjModel* m, mjData* d); + +// Find constraint islands. +MJAPI void mj_island(const mjModel* m, mjData* d); + +// Compute inverse constraint inertia efc_AR. +MJAPI void mj_projectConstraint(const mjModel* m, mjData* d); + +// Compute efc_vel, efc_aref. +MJAPI void mj_referenceConstraint(const mjModel* m, mjData* d); + +// Compute efc_state, efc_force, qfrc_constraint, and (optionally) cone Hessians. +// If cost is not NULL, set *cost = s(jar) where jar = Jac*qacc-aref. +MJAPI void mj_constraintUpdate(const mjModel* m, mjData* d, const mjtNum* jar, + mjtNum cost[1], int flg_coneHessian); + + +//---------------------------------- Support ------------------------------------------------------- + +// Return size of state specification. +MJAPI int mj_stateSize(const mjModel* m, unsigned int spec); + +// Get state. +MJAPI void mj_getState(const mjModel* m, const mjData* d, mjtNum* state, unsigned int spec); + +// Set state. +MJAPI void mj_setState(const mjModel* m, mjData* d, const mjtNum* state, unsigned int spec); + +// Copy current state to the k-th model keyframe. +MJAPI void mj_setKeyframe(mjModel* m, const mjData* d, int k); + +// Add contact to d->contact list; return 0 if success; 1 if buffer full. +MJAPI int mj_addContact(const mjModel* m, mjData* d, const mjContact* con); + +// Determine type of friction cone. +MJAPI int mj_isPyramidal(const mjModel* m); + +// Determine type of constraint Jacobian. +MJAPI int mj_isSparse(const mjModel* m); + +// Determine type of solver (PGS is dual, CG and Newton are primal). +MJAPI int mj_isDual(const mjModel* m); + +// Multiply dense or sparse constraint Jacobian by vector. +MJAPI void mj_mulJacVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); + +// Multiply dense or sparse constraint Jacobian transpose by vector. +MJAPI void mj_mulJacTVec(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); + +// Compute 3/6-by-nv end-effector Jacobian of global point attached to given body. +MJAPI void mj_jac(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, + const mjtNum point[3], int body); + +// Compute body frame end-effector Jacobian. +MJAPI void mj_jacBody(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); + +// Compute body center-of-mass end-effector Jacobian. +MJAPI void mj_jacBodyCom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int body); + +// Compute subtree center-of-mass end-effector Jacobian. +MJAPI void mj_jacSubtreeCom(const mjModel* m, mjData* d, mjtNum* jacp, int body); + +// Compute geom end-effector Jacobian. +MJAPI void mj_jacGeom(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int geom); + +// Compute site end-effector Jacobian. +MJAPI void mj_jacSite(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int site); + +// Compute translation end-effector Jacobian of point, and rotation Jacobian of axis. +MJAPI void mj_jacPointAxis(const mjModel* m, mjData* d, mjtNum* jacPoint, mjtNum* jacAxis, + const mjtNum point[3], const mjtNum axis[3], int body); + +// Compute 3/6-by-nv Jacobian time derivative of global point attached to given body. +MJAPI void mj_jacDot(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, + const mjtNum point[3], int body); + +// Compute subtree angular momentum matrix. +MJAPI void mj_angmomMat(const mjModel* m, mjData* d, mjtNum* mat, int body); + +// 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); + +// Convert sparse inertia matrix M into full (i.e. dense) matrix. +MJAPI void mj_fullM(const mjModel* m, mjtNum* dst, const mjtNum* M); + +// Multiply vector by inertia matrix. +MJAPI void mj_mulM(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); + +// Multiply vector by (inertia matrix)^(1/2). +MJAPI void mj_mulM2(const mjModel* m, const mjData* d, mjtNum* res, const mjtNum* vec); + +// Add inertia matrix to destination matrix. +// Destination can be sparse uncompressed, or dense when all int* are NULL +MJAPI void mj_addM(const mjModel* m, mjData* d, mjtNum* dst, int* rownnz, int* rowadr, int* colind); + +// Apply Cartesian force and torque (outside xfrc_applied mechanism). +MJAPI void mj_applyFT(const mjModel* m, mjData* d, const mjtNum force[3], const mjtNum torque[3], + const mjtNum point[3], int body, mjtNum* qfrc_target); + +// Compute object 6D velocity (rot:lin) in object-centered frame, world/local orientation. +MJAPI void mj_objectVelocity(const mjModel* m, const mjData* d, + int objtype, int objid, mjtNum res[6], int flg_local); + +// Compute object 6D acceleration (rot:lin) in object-centered frame, world/local orientation. +MJAPI void mj_objectAcceleration(const mjModel* m, const mjData* d, + int objtype, int objid, mjtNum res[6], int flg_local); + +// Returns smallest signed distance between two geoms and optionally segment from geom1 to geom2. +MJAPI mjtNum mj_geomDistance(const mjModel* m, const mjData* d, int geom1, int geom2, + mjtNum distmax, mjtNum fromto[6]); + +// Extract 6D force:torque given contact id, in the contact frame. +MJAPI void mj_contactForce(const mjModel* m, const mjData* d, int id, mjtNum result[6]); + +// Compute velocity by finite-differencing two positions. +MJAPI void mj_differentiatePos(const mjModel* m, mjtNum* qvel, mjtNum dt, + const mjtNum* qpos1, const mjtNum* qpos2); + +// Integrate position with given velocity. +MJAPI void mj_integratePos(const mjModel* m, mjtNum* qpos, const mjtNum* qvel, mjtNum dt); + +// Normalize all quaternions in qpos-type vector. +MJAPI void mj_normalizeQuat(const mjModel* m, mjtNum* qpos); + +// Map from body local to global Cartesian coordinates, sameframe takes values from mjtSameFrame. +MJAPI void mj_local2Global(mjData* d, mjtNum xpos[3], mjtNum xmat[9], const mjtNum pos[3], + const mjtNum quat[4], int body, mjtByte sameframe); + +// Sum all body masses. +MJAPI mjtNum mj_getTotalmass(const mjModel* m); + +// Scale body masses and inertias to achieve specified total mass. +MJAPI void mj_setTotalmass(mjModel* m, mjtNum newmass); + +// Return a config attribute value of a plugin instance; +// NULL: invalid plugin instance ID or attribute name +MJAPI const char* mj_getPluginConfig(const mjModel* m, int plugin_id, const char* attrib); + +// Load a dynamic library. The dynamic library is assumed to register one or more plugins. +MJAPI void mj_loadPluginLibrary(const char* path); + +// Scan a directory and load all dynamic libraries. Dynamic libraries in the specified directory +// are assumed to register one or more plugins. Optionally, if a callback is specified, it is called +// for each dynamic library encountered that registers plugins. +MJAPI void mj_loadAllPluginLibraries(const char* directory, mjfPluginLibraryLoadCallback callback); + +// Return version number: 1.0.2 is encoded as 102. +MJAPI int mj_version(void); + +// Return the current version of MuJoCo as a null-terminated string. +MJAPI const char* mj_versionString(void); + + +//---------------------------------- Ray casting --------------------------------------------------- + +// Intersect multiple rays emanating from a single point. +// Similar semantics to mj_ray, but vec is an array of (nray x 3) directions. +MJAPI void mj_multiRay(const mjModel* m, mjData* d, const mjtNum pnt[3], const mjtNum* vec, + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int* geomid, mjtNum* dist, int nray, mjtNum cutoff); + +// Intersect ray (pnt+x*vec, x>=0) with visible geoms, except geoms in bodyexclude. +// Return distance (x) to nearest surface, or -1 if no intersection and output geomid. +// geomgroup, flg_static are as in mjvOption; geomgroup==NULL skips group exclusion. +MJAPI mjtNum mj_ray(const mjModel* m, const mjData* d, const mjtNum pnt[3], const mjtNum vec[3], + const mjtByte* geomgroup, mjtByte flg_static, int bodyexclude, + int geomid[1]); + +// Intersect ray with hfield, return nearest distance or -1 if no intersection. +MJAPI mjtNum mj_rayHfield(const mjModel* m, const mjData* d, int geomid, + const mjtNum pnt[3], const mjtNum vec[3]); + +// Intersect ray with mesh, return nearest distance or -1 if no intersection. +MJAPI mjtNum mj_rayMesh(const mjModel* m, const mjData* d, int geomid, + const mjtNum pnt[3], const mjtNum vec[3]); + +// Intersect ray with pure geom, return nearest distance or -1 if no intersection. +MJAPI mjtNum mju_rayGeom(const mjtNum pos[3], const mjtNum mat[9], const mjtNum size[3], + const mjtNum pnt[3], const mjtNum vec[3], int geomtype); + +// Intersect ray with flex, return nearest distance or -1 if no intersection, +// and also output nearest vertex id. +MJAPI mjtNum mju_rayFlex(const mjModel* m, const mjData* d, int flex_layer, mjtByte flg_vert, + mjtByte flg_edge, mjtByte flg_face, mjtByte flg_skin, int flexid, + const mjtNum* pnt, const mjtNum* vec, int vertid[1]); + +// Intersect ray with skin, return nearest distance or -1 if no intersection, +// and also output nearest vertex id. +MJAPI mjtNum mju_raySkin(int nface, int nvert, const int* face, const float* vert, + const mjtNum pnt[3], const mjtNum vec[3], int vertid[1]); + + +//---------------------------------- Interaction --------------------------------------------------- + +// Set default camera. +MJAPI void mjv_defaultCamera(mjvCamera* cam); + +// Set default free camera. +MJAPI void mjv_defaultFreeCamera(const mjModel* m, mjvCamera* cam); + +// Set default perturbation. +MJAPI void mjv_defaultPerturb(mjvPerturb* pert); + +// Transform pose from room to model space. +MJAPI void mjv_room2model(mjtNum modelpos[3], mjtNum modelquat[4], const mjtNum roompos[3], + const mjtNum roomquat[4], const mjvScene* scn); + +// Transform pose from model to room space. +MJAPI void mjv_model2room(mjtNum roompos[3], mjtNum roomquat[4], const mjtNum modelpos[3], + const mjtNum modelquat[4], const mjvScene* scn); + +// Get camera info in model space; average left and right OpenGL cameras. +MJAPI void mjv_cameraInModel(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], + const mjvScene* scn); + +// Get camera info in room space; average left and right OpenGL cameras. +MJAPI void mjv_cameraInRoom(mjtNum headpos[3], mjtNum forward[3], mjtNum up[3], + const mjvScene* scn); + +// Get frustum height at unit distance from camera; average left and right OpenGL cameras. +MJAPI mjtNum mjv_frustumHeight(const mjvScene* scn); + +// Rotate 3D vec in horizontal plane by angle between (0,1) and (forward_x,forward_y). +MJAPI void mjv_alignToCamera(mjtNum res[3], const mjtNum vec[3], const mjtNum forward[3]); + +// Move camera with mouse; action is mjtMouse. +MJAPI void mjv_moveCamera(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, + const mjvScene* scn, mjvCamera* cam); + +// Move camera with mouse given a scene state; action is mjtMouse. +MJAPI void mjv_moveCameraFromState(const mjvSceneState* scnstate, int action, + mjtNum reldx, mjtNum reldy, + const mjvScene* scn, mjvCamera* cam); + +// Move perturb object with mouse; action is mjtMouse. +MJAPI void mjv_movePerturb(const mjModel* m, const mjData* d, int action, mjtNum reldx, + mjtNum reldy, const mjvScene* scn, mjvPerturb* pert); + +// Move perturb object with mouse given a scene state; action is mjtMouse. +MJAPI void mjv_movePerturbFromState(const mjvSceneState* scnstate, int action, + mjtNum reldx, mjtNum reldy, + const mjvScene* scn, mjvPerturb* pert); + +// Move model with mouse; action is mjtMouse. +MJAPI void mjv_moveModel(const mjModel* m, int action, mjtNum reldx, mjtNum reldy, + const mjtNum roomup[3], mjvScene* scn); + +// Copy perturb pos,quat from selected body; set scale for perturbation. +MJAPI void mjv_initPerturb(const mjModel* m, mjData* d, const mjvScene* scn, mjvPerturb* pert); + +// Set perturb pos,quat in d->mocap when selected body is mocap, and in d->qpos otherwise. +// Write d->qpos only if flg_paused and subtree root for selected body has free joint. +MJAPI void mjv_applyPerturbPose(const mjModel* m, mjData* d, const mjvPerturb* pert, + int flg_paused); + +// Set perturb force,torque in d->xfrc_applied, if selected body is dynamic. +MJAPI void mjv_applyPerturbForce(const mjModel* m, mjData* d, const mjvPerturb* pert); + +// Return the average of two OpenGL cameras. +MJAPI mjvGLCamera mjv_averageCamera(const mjvGLCamera* cam1, const mjvGLCamera* cam2); + +// Select geom, flex or skin with mouse, return bodyid; -1: none selected. +MJAPI int mjv_select(const mjModel* m, const mjData* d, const mjvOption* vopt, + mjtNum aspectratio, mjtNum relx, mjtNum rely, + const mjvScene* scn, mjtNum selpnt[3], + int geomid[1], int flexid[1], int skinid[1]); + + +//---------------------------------- Visualization ------------------------------------------------- + +// Set default visualization options. +MJAPI void mjv_defaultOption(mjvOption* opt); + +// Set default figure. +MJAPI void mjv_defaultFigure(mjvFigure* fig); + +// Initialize given geom fields when not NULL, set the rest to their default values. +MJAPI void mjv_initGeom(mjvGeom* geom, int type, const mjtNum size[3], + const mjtNum pos[3], const mjtNum mat[9], const float rgba[4]); + +// Set (type, size, pos, mat) for connector-type geom between given points. +// Assume that mjv_initGeom was already called to set all other properties. +// Width of mjGEOM_LINE is denominated in pixels. +MJAPI void mjv_connector(mjvGeom* geom, int type, mjtNum width, + const mjtNum from[3], const mjtNum to[3]); + +// Set default abstract scene. +MJAPI void mjv_defaultScene(mjvScene* scn); + +// Allocate resources in abstract scene. +MJAPI void mjv_makeScene(const mjModel* m, mjvScene* scn, int maxgeom); + +// Free abstract scene. +MJAPI void mjv_freeScene(mjvScene* scn); + +// Update entire scene given model state. +MJAPI void mjv_updateScene(const mjModel* m, mjData* d, const mjvOption* opt, + const mjvPerturb* pert, mjvCamera* cam, int catmask, mjvScene* scn); + +// Update entire scene from a scene state, return the number of new mjWARN_VGEOMFULL warnings. +MJAPI int mjv_updateSceneFromState(const mjvSceneState* scnstate, const mjvOption* opt, + const mjvPerturb* pert, mjvCamera* cam, int catmask, + mjvScene* scn); + +// Copy mjModel, skip large arrays not required for abstract visualization. +MJAPI void mjv_copyModel(mjModel* dest, const mjModel* src); + +// Set default scene state. +MJAPI void mjv_defaultSceneState(mjvSceneState* scnstate); + +// Allocate resources and initialize a scene state object. +MJAPI void mjv_makeSceneState(const mjModel* m, const mjData* d, + mjvSceneState* scnstate, int maxgeom); + +// Free scene state. +MJAPI void mjv_freeSceneState(mjvSceneState* scnstate); + +// Update a scene state from model and data. +MJAPI void mjv_updateSceneState(const mjModel* m, mjData* d, const mjvOption* opt, + mjvSceneState* scnstate); + +// Add geoms from selected categories. +MJAPI void mjv_addGeoms(const mjModel* m, mjData* d, const mjvOption* opt, + const mjvPerturb* pert, int catmask, mjvScene* scn); + +// Make list of lights. +MJAPI void mjv_makeLights(const mjModel* m, const mjData* d, mjvScene* scn); + +// Update camera. +MJAPI void mjv_updateCamera(const mjModel* m, const mjData* d, mjvCamera* cam, mjvScene* scn); + +// Update skins. +MJAPI void mjv_updateSkin(const mjModel* m, const mjData* d, mjvScene* scn); + + +//---------------------------------- OpenGL rendering ---------------------------------------------- + +// Set default mjrContext. +MJAPI void mjr_defaultContext(mjrContext* con); + +// Allocate resources in custom OpenGL context; fontscale is mjtFontScale. +MJAPI void mjr_makeContext(const mjModel* m, mjrContext* con, int fontscale); + +// Change font of existing context. +MJAPI void mjr_changeFont(int fontscale, mjrContext* con); + +// Add Aux buffer with given index to context; free previous Aux buffer. +MJAPI void mjr_addAux(int index, int width, int height, int samples, mjrContext* con); + +// Free resources in custom OpenGL context, set to default. +MJAPI void mjr_freeContext(mjrContext* con); + +// Resize offscreen buffers. +MJAPI void mjr_resizeOffscreen(int width, int height, mjrContext* con); + +// Upload texture to GPU, overwriting previous upload if any. +MJAPI void mjr_uploadTexture(const mjModel* m, const mjrContext* con, int texid); + +// Upload mesh to GPU, overwriting previous upload if any. +MJAPI void mjr_uploadMesh(const mjModel* m, const mjrContext* con, int meshid); + +// Upload height field to GPU, overwriting previous upload if any. +MJAPI void mjr_uploadHField(const mjModel* m, const mjrContext* con, int hfieldid); + +// Make con->currentBuffer current again. +MJAPI void mjr_restoreBuffer(const mjrContext* con); + +// Set OpenGL framebuffer for rendering: mjFB_WINDOW or mjFB_OFFSCREEN. +// If only one buffer is available, set that buffer and ignore framebuffer argument. +MJAPI void mjr_setBuffer(int framebuffer, mjrContext* con); + +// Read pixels from current OpenGL framebuffer to client buffer. +// Viewport is in OpenGL framebuffer; client buffer starts at (0,0). +MJAPI void mjr_readPixels(unsigned char* rgb, float* depth, + mjrRect viewport, const mjrContext* con); + +// Draw pixels from client buffer to current OpenGL framebuffer. +// Viewport is in OpenGL framebuffer; client buffer starts at (0,0). +MJAPI void mjr_drawPixels(const unsigned char* rgb, const float* depth, + mjrRect viewport, const mjrContext* con); + +// Blit from src viewpoint in current framebuffer to dst viewport in other framebuffer. +// If src, dst have different size and flg_depth==0, color is interpolated with GL_LINEAR. +MJAPI void mjr_blitBuffer(mjrRect src, mjrRect dst, + int flg_color, int flg_depth, const mjrContext* con); + +// Set Aux buffer for custom OpenGL rendering (call restoreBuffer when done). +MJAPI void mjr_setAux(int index, const mjrContext* con); + +// Blit from Aux buffer to con->currentBuffer. +MJAPI void mjr_blitAux(int index, mjrRect src, int left, int bottom, const mjrContext* con); + +// Draw text at (x,y) in relative coordinates; font is mjtFont. +MJAPI void mjr_text(int font, const char* txt, const mjrContext* con, + float x, float y, float r, float g, float b); + +// Draw text overlay; font is mjtFont; gridpos is mjtGridPos. +MJAPI void mjr_overlay(int font, int gridpos, mjrRect viewport, + const char* overlay, const char* overlay2, const mjrContext* con); + +// Get maximum viewport for active buffer. +MJAPI mjrRect mjr_maxViewport(const mjrContext* con); + +// Draw rectangle. +MJAPI void mjr_rectangle(mjrRect viewport, float r, float g, float b, float a); + +// Draw rectangle with centered text. +MJAPI void mjr_label(mjrRect viewport, int font, const char* txt, + float r, float g, float b, float a, float rt, float gt, float bt, + const mjrContext* con); + +// Draw 2D figure. +MJAPI void mjr_figure(mjrRect viewport, mjvFigure* fig, const mjrContext* con); + +// Render 3D scene. +MJAPI void mjr_render(mjrRect viewport, mjvScene* scn, const mjrContext* con); + +// Call glFinish. +MJAPI void mjr_finish(void); + +// Call glGetError and return result. +MJAPI int mjr_getError(void); + +// Find first rectangle containing mouse, -1: not found. +MJAPI int mjr_findRect(int x, int y, int nrect, const mjrRect* rect); + + +//---------------------------------- UI framework -------------------------------------------------- + +// Get builtin UI theme spacing (ind: 0-1). +MJAPI mjuiThemeSpacing mjui_themeSpacing(int ind); + +// Get builtin UI theme color (ind: 0-3). +MJAPI mjuiThemeColor mjui_themeColor(int ind); + +// Add definitions to UI. +MJAPI void mjui_add(mjUI* ui, const mjuiDef* def); + +// Add definitions to UI section. +MJAPI void mjui_addToSection(mjUI* ui, int sect, const mjuiDef* def); + +// Compute UI sizes. +MJAPI void mjui_resize(mjUI* ui, const mjrContext* con); + +// Update specific section/item; -1: update all. +MJAPI void mjui_update(int section, int item, const mjUI* ui, + const mjuiState* state, const mjrContext* con); + +// Handle UI event, return pointer to changed item, NULL if no change. +MJAPI mjuiItem* mjui_event(mjUI* ui, mjuiState* state, const mjrContext* con); + +// Copy UI image to current buffer. +MJAPI void mjui_render(mjUI* ui, const mjuiState* state, const mjrContext* con); + + +//---------------------------------- Error and memory ---------------------------------------------- + +// Main error function; does not return to caller. +MJAPI void mju_error(const char* msg, ...) mjPRINTFLIKE(1, 2); + +// Deprecated: use mju_error. +MJAPI void mju_error_i(const char* msg, int i); + +// Deprecated: use mju_error. +MJAPI void mju_error_s(const char* msg, const char* text); + +// Main warning function; returns to caller. +MJAPI void mju_warning(const char* msg, ...) mjPRINTFLIKE(1, 2); + +// Deprecated: use mju_warning. +MJAPI void mju_warning_i(const char* msg, int i); + +// Deprecated: use mju_warning. +MJAPI void mju_warning_s(const char* msg, const char* text); + +// Clear user error and memory handlers. +MJAPI void mju_clearHandlers(void); + +// Allocate memory; byte-align on 64; pad size to multiple of 64. +MJAPI void* mju_malloc(size_t size); + +// Free memory, using free() by default. +MJAPI void mju_free(void* ptr); + +// High-level warning function: count warnings in mjData, print only the first. +MJAPI void mj_warning(mjData* d, int warning, int info); + +// Write [datetime, type: message] to MUJOCO_LOG.TXT. +MJAPI void mju_writeLog(const char* type, const char* msg); + +// Get compiler error message from spec. +MJAPI const char* mjs_getError(mjSpec* s); + +// Return 1 if compiler error is a warning. +MJAPI int mjs_isWarning(mjSpec* s); + + +//---------------------------------- Standard math ------------------------------------------------- + +#if !defined(mjUSESINGLE) + #define mju_sqrt sqrt + #define mju_exp exp + #define mju_sin sin + #define mju_cos cos + #define mju_tan tan + #define mju_asin asin + #define mju_acos acos + #define mju_atan2 atan2 + #define mju_tanh tanh + #define mju_pow pow + #define mju_abs fabs + #define mju_log log + #define mju_log10 log10 + #define mju_floor floor + #define mju_ceil ceil + +#else + #define mju_sqrt sqrtf + #define mju_exp expf + #define mju_sin sinf + #define mju_cos cosf + #define mju_tan tanf + #define mju_asin asinf + #define mju_acos acosf + #define mju_atan2 atan2f + #define mju_tanh tanhf + #define mju_pow powf + #define mju_abs fabsf + #define mju_log logf + #define mju_log10 log10f + #define mju_floor floorf + #define mju_ceil ceilf +#endif + + +//---------------------------------- Vector math --------------------------------------------------- + +// Set res = 0. +MJAPI void mju_zero3(mjtNum res[3]); + +// Set res = vec. +MJAPI void mju_copy3(mjtNum res[3], const mjtNum data[3]); + +// Set res = vec*scl. +MJAPI void mju_scl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); + +// Set res = vec1 + vec2. +MJAPI void mju_add3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]); + +// Set res = vec1 - vec2. +MJAPI void mju_sub3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3]); + +// Set res = res + vec. +MJAPI void mju_addTo3(mjtNum res[3], const mjtNum vec[3]); + +// Set res = res - vec. +MJAPI void mju_subFrom3(mjtNum res[3], const mjtNum vec[3]); + +// Set res = res + vec*scl. +MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl); + +// Set res = vec1 + vec2*scl. +MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl); + +// Normalize vector, return length before normalization. +MJAPI mjtNum mju_normalize3(mjtNum vec[3]); + +// Return vector length (without normalizing the vector). +MJAPI mjtNum mju_norm3(const mjtNum vec[3]); + +// Return dot-product of vec1 and vec2. +MJAPI mjtNum mju_dot3(const mjtNum vec1[3], const mjtNum vec2[3]); + +// Return Cartesian distance between 3D vectors pos1 and pos2. +MJAPI mjtNum mju_dist3(const mjtNum pos1[3], const mjtNum pos2[3]); + +// Multiply 3-by-3 matrix by vector: res = mat * vec. +MJAPI void mju_mulMatVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); + +// Multiply transposed 3-by-3 matrix by vector: res = mat' * vec. +MJAPI void mju_mulMatTVec3(mjtNum res[3], const mjtNum mat[9], const mjtNum vec[3]); + +// Compute cross-product: res = cross(a, b). +MJAPI void mju_cross(mjtNum res[3], const mjtNum a[3], const mjtNum b[3]); + +// Set res = 0. +MJAPI void mju_zero4(mjtNum res[4]); + +// Set res = (1,0,0,0). +MJAPI void mju_unit4(mjtNum res[4]); + +// Set res = vec. +MJAPI void mju_copy4(mjtNum res[4], const mjtNum data[4]); + +// Normalize vector, return length before normalization. +MJAPI mjtNum mju_normalize4(mjtNum vec[4]); + +// Set res = 0. +MJAPI void mju_zero(mjtNum* res, int n); + +// Set res = val. +MJAPI void mju_fill(mjtNum* res, mjtNum val, int n); + +// Set res = vec. +MJAPI void mju_copy(mjtNum* res, const mjtNum* vec, int n); + +// Return sum(vec). +MJAPI mjtNum mju_sum(const mjtNum* vec, int n); + +// Return L1 norm: sum(abs(vec)). +MJAPI mjtNum mju_L1(const mjtNum* vec, int n); + +// Set res = vec*scl. +MJAPI void mju_scl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); + +// Set res = vec1 + vec2. +MJAPI void mju_add(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n); + +// Set res = vec1 - vec2. +MJAPI void mju_sub(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, int n); + +// Set res = res + vec. +MJAPI void mju_addTo(mjtNum* res, const mjtNum* vec, int n); + +// Set res = res - vec. +MJAPI void mju_subFrom(mjtNum* res, const mjtNum* vec, int n); + +// Set res = res + vec*scl. +MJAPI void mju_addToScl(mjtNum* res, const mjtNum* vec, mjtNum scl, int n); + +// Set res = vec1 + vec2*scl. +MJAPI void mju_addScl(mjtNum* res, const mjtNum* vec1, const mjtNum* vec2, mjtNum scl, int n); + +// Normalize vector, return length before normalization. +MJAPI mjtNum mju_normalize(mjtNum* res, int n); + +// Return vector length (without normalizing vector). +MJAPI mjtNum mju_norm(const mjtNum* res, int n); + +// Return dot-product of vec1 and vec2. +MJAPI mjtNum mju_dot(const mjtNum* vec1, const mjtNum* vec2, int n); + +// Multiply matrix and vector: res = mat * vec. +MJAPI void mju_mulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc); + +// Multiply transposed matrix and vector: res = mat' * vec. +MJAPI void mju_mulMatTVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int nr, int nc); + +// Multiply square matrix with vectors on both sides: returns vec1' * mat * vec2. +MJAPI mjtNum mju_mulVecMatVec(const mjtNum* vec1, const mjtNum* mat, const mjtNum* vec2, int n); + +// Transpose matrix: res = mat'. +MJAPI void mju_transpose(mjtNum* res, const mjtNum* mat, int nr, int nc); + +// Symmetrize square matrix res = (mat + mat')/2. +MJAPI void mju_symmetrize(mjtNum* res, const mjtNum* mat, int n); + +// Set mat to the identity matrix. +MJAPI void mju_eye(mjtNum* mat, int n); + +// Multiply matrices: res = mat1 * mat2. +MJAPI void mju_mulMatMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, + int r1, int c1, int c2); + +// Multiply matrices, second argument transposed: res = mat1 * mat2'. +MJAPI void mju_mulMatMatT(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, + int r1, int c1, int r2); + +// Multiply matrices, first argument transposed: res = mat1' * mat2. +MJAPI void mju_mulMatTMat(mjtNum* res, const mjtNum* mat1, const mjtNum* mat2, + int r1, int c1, int c2); + +// Set res = mat' * diag * mat if diag is not NULL, and res = mat' * mat otherwise. +MJAPI void mju_sqrMatTD(mjtNum* res, const mjtNum* mat, const mjtNum* diag, int nr, int nc); + +// Coordinate transform of 6D motion or force vector in rotation:translation format. +// rotnew2old is 3-by-3, NULL means no rotation; flg_force specifies force or motion type. +MJAPI void mju_transformSpatial(mjtNum res[6], const mjtNum vec[6], int flg_force, + const mjtNum newpos[3], const mjtNum oldpos[3], + const mjtNum rotnew2old[9]); + + +//---------------------------------- Sparse math --------------------------------------------------- + +// Convert matrix from dense to sparse. +// nnz is size of res and colind, return 1 if too small, 0 otherwise. +MJAPI int mju_dense2sparse(mjtNum* res, const mjtNum* mat, int nr, int nc, + int* rownnz, int* rowadr, int* colind, int nnz); + +// Convert matrix from sparse to dense. +MJAPI void mju_sparse2dense(mjtNum* res, const mjtNum* mat, int nr, int nc, + const int* rownnz, const int* rowadr, const int* colind); + + +//---------------------------------- Quaternions --------------------------------------------------- + +// Rotate vector by quaternion. +MJAPI void mju_rotVecQuat(mjtNum res[3], const mjtNum vec[3], const mjtNum quat[4]); + +// Conjugate quaternion, corresponding to opposite rotation. +MJAPI void mju_negQuat(mjtNum res[4], const mjtNum quat[4]); + +// Multiply quaternions. +MJAPI void mju_mulQuat(mjtNum res[4], const mjtNum quat1[4], const mjtNum quat2[4]); + +// Multiply quaternion and axis. +MJAPI void mju_mulQuatAxis(mjtNum res[4], const mjtNum quat[4], const mjtNum axis[3]); + +// Convert axisAngle to quaternion. +MJAPI void mju_axisAngle2Quat(mjtNum res[4], const mjtNum axis[3], mjtNum angle); + +// Convert quaternion (corresponding to orientation difference) to 3D velocity. +MJAPI void mju_quat2Vel(mjtNum res[3], const mjtNum quat[4], mjtNum dt); + +// Subtract quaternions, express as 3D velocity: qb*quat(res) = qa. +MJAPI void mju_subQuat(mjtNum res[3], const mjtNum qa[4], const mjtNum qb[4]); + +// Convert quaternion to 3D rotation matrix. +MJAPI void mju_quat2Mat(mjtNum res[9], const mjtNum quat[4]); + +// Convert 3D rotation matrix to quaternion. +MJAPI void mju_mat2Quat(mjtNum quat[4], const mjtNum mat[9]); + +// Compute time-derivative of quaternion, given 3D rotational velocity. +MJAPI void mju_derivQuat(mjtNum res[4], const mjtNum quat[4], const mjtNum vel[3]); + +// Integrate quaternion given 3D angular velocity. +MJAPI void mju_quatIntegrate(mjtNum quat[4], const mjtNum vel[3], mjtNum scale); + +// Construct quaternion performing rotation from z-axis to given vector. +MJAPI void mju_quatZ2Vec(mjtNum quat[4], const mjtNum vec[3]); + +// Extract 3D rotation from an arbitrary 3x3 matrix by refining the input quaternion. +// Returns the number of iterations required to converge +MJAPI int mju_mat2Rot(mjtNum quat[4], const mjtNum mat[9]); + +// Convert sequence of Euler angles (radians) to quaternion. +// seq[0,1,2] must be in 'xyzXYZ', lower/upper-case mean intrinsic/extrinsic rotations. +MJAPI void mju_euler2Quat(mjtNum quat[4], const mjtNum euler[3], const char* seq); + + +//---------------------------------- Poses --------------------------------------------------------- + +// Multiply two poses. +MJAPI void mju_mulPose(mjtNum posres[3], mjtNum quatres[4], + const mjtNum pos1[3], const mjtNum quat1[4], + const mjtNum pos2[3], const mjtNum quat2[4]); + +// Conjugate pose, corresponding to the opposite spatial transformation. +MJAPI void mju_negPose(mjtNum posres[3], mjtNum quatres[4], + const mjtNum pos[3], const mjtNum quat[4]); + +// Transform vector by pose. +MJAPI void mju_trnVecPose(mjtNum res[3], const mjtNum pos[3], const mjtNum quat[4], + const mjtNum vec[3]); + + +//--------------------------------- Decompositions / Solvers --------------------------------------- + +// Cholesky decomposition: mat = L*L'; return rank, decomposition performed in-place into mat. +MJAPI int mju_cholFactor(mjtNum* mat, int n, mjtNum mindiag); + +// Solve (mat*mat') * res = vec, where mat is a Cholesky factor. +MJAPI void mju_cholSolve(mjtNum* res, const mjtNum* mat, const mjtNum* vec, int n); + +// Cholesky rank-one update: L*L' +/- x*x'; return rank. +MJAPI int mju_cholUpdate(mjtNum* mat, mjtNum* x, int n, int flg_plus); + +// Band-dense Cholesky decomposition. +// Returns minimum value in the factorized diagonal, or 0 if rank-deficient. +// mat has (ntotal-ndense) x nband + ndense x ntotal elements. +// The first (ntotal-ndense) x nband store the band part, left of diagonal, inclusive. +// The second ndense x ntotal store the band part as entire dense rows. +// Add diagadd+diagmul*mat_ii to diagonal before factorization. +MJAPI mjtNum mju_cholFactorBand(mjtNum* mat, int ntotal, int nband, int ndense, + mjtNum diagadd, mjtNum diagmul); + +// Solve (mat*mat')*res = vec where mat is a band-dense Cholesky factor. +MJAPI void mju_cholSolveBand(mjtNum* res, const mjtNum* mat, const mjtNum* vec, + int ntotal, int nband, int ndense); + +// Convert banded matrix to dense matrix, fill upper triangle if flg_sym>0. +MJAPI void mju_band2Dense(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense, + mjtByte flg_sym); + +// Convert dense matrix to banded matrix. +MJAPI void mju_dense2Band(mjtNum* res, const mjtNum* mat, int ntotal, int nband, int ndense); + +// Multiply band-diagonal matrix with nvec vectors, include upper triangle if flg_sym>0. +MJAPI void mju_bandMulMatVec(mjtNum* res, const mjtNum* mat, const mjtNum* vec, + int ntotal, int nband, int ndense, int nvec, mjtByte flg_sym); + +// Address of diagonal element i in band-dense matrix representation. +MJAPI int mju_bandDiag(int i, int ntotal, int nband, int ndense); + +// Eigenvalue decomposition of symmetric 3x3 matrix, mat = eigvec * diag(eigval) * eigvec'. +MJAPI int mju_eig3(mjtNum eigval[3], mjtNum eigvec[9], mjtNum quat[4], const mjtNum mat[9]); + +// minimize 0.5*x'*H*x + x'*g s.t. lower <= x <= upper, return rank or -1 if failed +// inputs: +// n - problem dimension +// H - SPD matrix n*n +// g - bias vector n +// lower - lower bounds n +// upper - upper bounds n +// res - solution warmstart n +// return value: +// nfree <= n - rank of unconstrained subspace, -1 if failure +// outputs (required): +// res - solution n +// R - subspace Cholesky factor nfree*nfree allocated: n*(n+7) +// outputs (optional): +// index - set of free dimensions nfree allocated: n +// notes: +// the initial value of res is used to warmstart the solver +// R must have allocatd size n*(n+7), but only nfree*nfree values are used in output +// index (if given) must have allocated size n, but only nfree values are used in output +// only the lower triangles of H and R and are read from and written to, respectively +// the convenience function mju_boxQPmalloc allocates the required data structures +MJAPI int mju_boxQP(mjtNum* res, mjtNum* R, int* index, const mjtNum* H, const mjtNum* g, int n, + const mjtNum* lower, const mjtNum* upper); + +// allocate heap memory for box-constrained Quadratic Program +// as in mju_boxQP, index, lower, and upper are optional +// free all pointers with mju_free() +MJAPI void mju_boxQPmalloc(mjtNum** res, mjtNum** R, int** index, mjtNum** H, mjtNum** g, int n, + mjtNum** lower, mjtNum** upper); + + +//---------------------------------- Miscellaneous ------------------------------------------------- + +// Muscle active force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax). +MJAPI mjtNum mju_muscleGain(mjtNum len, mjtNum vel, const mjtNum lengthrange[2], + mjtNum acc0, const mjtNum prm[9]); + +// Muscle passive force, prm = (range[2], force, scale, lmin, lmax, vmax, fpmax, fvmax). +MJAPI mjtNum mju_muscleBias(mjtNum len, const mjtNum lengthrange[2], + mjtNum acc0, const mjtNum prm[9]); + +// Muscle activation dynamics, prm = (tau_act, tau_deact, smoothing_width). +MJAPI mjtNum mju_muscleDynamics(mjtNum ctrl, mjtNum act, const mjtNum prm[3]); + +// Convert contact force to pyramid representation. +MJAPI void mju_encodePyramid(mjtNum* pyramid, const mjtNum* force, const mjtNum* mu, int dim); + +// Convert pyramid representation to contact force. +MJAPI void mju_decodePyramid(mjtNum* force, const mjtNum* pyramid, const mjtNum* mu, int dim); + +// Integrate spring-damper analytically, return pos(dt). +MJAPI mjtNum mju_springDamper(mjtNum pos0, mjtNum vel0, mjtNum Kp, mjtNum Kv, mjtNum dt); + +// Return min(a,b) with single evaluation of a and b. +MJAPI mjtNum mju_min(mjtNum a, mjtNum b); + +// Return max(a,b) with single evaluation of a and b. +MJAPI mjtNum mju_max(mjtNum a, mjtNum b); + +// Clip x to the range [min, max]. +MJAPI mjtNum mju_clip(mjtNum x, mjtNum min, mjtNum max); + +// Return sign of x: +1, -1 or 0. +MJAPI mjtNum mju_sign(mjtNum x); + +// Round x to nearest integer. +MJAPI int mju_round(mjtNum x); + +// Convert type id (mjtObj) to type name. +MJAPI const char* mju_type2Str(int type); + +// Convert type name to type id (mjtObj). +MJAPI int mju_str2Type(const char* str); + +// Return human readable number of bytes using standard letter suffix. +MJAPI const char* mju_writeNumBytes(size_t nbytes); + +// Construct a warning message given the warning type and info. +MJAPI const char* mju_warningText(int warning, size_t info); + +// Return 1 if nan or abs(x)>mjMAXVAL, 0 otherwise. Used by check functions. +MJAPI int mju_isBad(mjtNum x); + +// Return 1 if all elements are 0. +MJAPI int mju_isZero(mjtNum* vec, int n); + +// Standard normal random number generator (optional second number). +MJAPI mjtNum mju_standardNormal(mjtNum* num2); + +// Convert from float to mjtNum. +MJAPI void mju_f2n(mjtNum* res, const float* vec, int n); + +// Convert from mjtNum to float. +MJAPI void mju_n2f(float* res, const mjtNum* vec, int n); + +// Convert from double to mjtNum. +MJAPI void mju_d2n(mjtNum* res, const double* vec, int n); + +// Convert from mjtNum to double. +MJAPI void mju_n2d(double* res, const mjtNum* vec, int n); + +// Insertion sort, resulting list is in increasing order. +MJAPI void mju_insertionSort(mjtNum* list, int n); + +// Integer insertion sort, resulting list is in increasing order. +MJAPI void mju_insertionSortInt(int* list, int n); + +// Generate Halton sequence. +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); + +// Sigmoid function over 0<=x<=1 using quintic polynomial. +MJAPI mjtNum mju_sigmoid(mjtNum x); + + +//---------------------------------- Derivatives --------------------------------------------------- + +// Finite differenced transition matrices (control theory notation) +// d(x_next) = A*dx + B*du +// d(sensor) = C*dx + D*du +// required output matrix dimensions: +// A: (2*nv+na x 2*nv+na) +// B: (2*nv+na x nu) +// D: (nsensordata x 2*nv+na) +// C: (nsensordata x nu) +MJAPI void mjd_transitionFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_centered, + mjtNum* A, mjtNum* B, mjtNum* C, mjtNum* D); + +// Finite differenced Jacobians of (force, sensors) = mj_inverse(state, acceleration) +// All outputs are optional. Output dimensions (transposed w.r.t Control Theory convention): +// DfDq: (nv x nv) +// DfDv: (nv x nv) +// DfDa: (nv x nv) +// DsDq: (nv x nsensordata) +// DsDv: (nv x nsensordata) +// DsDa: (nv x nsensordata) +// DmDq: (nv x nM) +// single-letter shortcuts: +// inputs: q=qpos, v=qvel, a=qacc +// outputs: f=qfrc_inverse, s=sensordata, m=qM +// notes: +// optionally computes mass matrix Jacobian DmDq +// flg_actuation specifies whether to subtract qfrc_actuator from qfrc_inverse +MJAPI void mjd_inverseFD(const mjModel* m, mjData* d, mjtNum eps, mjtByte flg_actuation, + mjtNum *DfDq, mjtNum *DfDv, mjtNum *DfDa, + mjtNum *DsDq, mjtNum *DsDv, mjtNum *DsDa, + mjtNum *DmDq); + +// Derivatives of mju_subQuat. +MJAPI void mjd_subQuat(const mjtNum qa[4], const mjtNum qb[4], mjtNum Da[9], mjtNum Db[9]); + +// Derivatives of mju_quatIntegrate. +MJAPI void mjd_quatIntegrate(const mjtNum vel[3], mjtNum scale, + mjtNum Dquat[9], mjtNum Dvel[9], mjtNum Dscale[3]); + + +//---------------------------------- Plugins ------------------------------------------------------- + +// Set default plugin definition. +MJAPI void mjp_defaultPlugin(mjpPlugin* plugin); + +// Globally register a plugin. This function is thread-safe. +// If an identical mjpPlugin is already registered, this function does nothing. +// If a non-identical mjpPlugin with the same name is already registered, an mju_error is raised. +// Two mjpPlugins are considered identical if all member function pointers and numbers are equal, +// and the name and attribute strings are all identical, however the char pointers to the strings +// need not be the same. +MJAPI int mjp_registerPlugin(const mjpPlugin* plugin); + +// Return the number of globally registered plugins. +MJAPI int mjp_pluginCount(void); + +// Look up a plugin by name. If slot is not NULL, also write its registered slot number into it. +MJAPI const mjpPlugin* mjp_getPlugin(const char* name, int* slot); + +// Look up a plugin by the registered slot number that was returned by mjp_registerPlugin. +MJAPI const mjpPlugin* mjp_getPluginAtSlot(int slot); + +// Set default resource provider definition. +MJAPI void mjp_defaultResourceProvider(mjpResourceProvider* provider); + +// Globally register a resource provider in a thread-safe manner. The provider must have a prefix +// that is not a sub-prefix or super-prefix of any current registered providers. This function +// returns a slot number > 0 on success. +MJAPI int mjp_registerResourceProvider(const mjpResourceProvider* provider); + +// Return the number of globally registered resource providers. +MJAPI int mjp_resourceProviderCount(void); + +// Return the resource provider with the prefix that matches against the resource name. +// If no match, return NULL. +MJAPI const mjpResourceProvider* mjp_getResourceProvider(const char* resource_name); + +// Look up a resource provider by slot number returned by mjp_registerResourceProvider. +// If invalid slot number, return NULL. +MJAPI const mjpResourceProvider* mjp_getResourceProviderAtSlot(int slot); + + +//---------------------------------- Threads ------------------------------------------------------- + +// Create a thread pool with the specified number of threads running. +MJAPI mjThreadPool* mju_threadPoolCreate(size_t number_of_threads); + +// Adds a thread pool to mjData and configures it for multi-threaded use. +MJAPI void mju_bindThreadPool(mjData* d, void* thread_pool); + +// Enqueue a task in a thread pool. +MJAPI void mju_threadPoolEnqueue(mjThreadPool* thread_pool, mjTask* task); + +// Destroy a thread pool. +MJAPI void mju_threadPoolDestroy(mjThreadPool* thread_pool); + +// Initialize an mjTask. +MJAPI void mju_defaultTask(mjTask* task); + +// Wait for a task to complete. +MJAPI void mju_taskJoin(mjTask* task); + + +//---------------------------------- Attachment ---------------------------------------------------- + +// Attach child to a parent, return the attached element if success or NULL otherwise. +MJAPI mjsElement* mjs_attach(mjsElement* parent, const mjsElement* child, + const char* prefix, const char* suffix); + +// Delete body and descendants from mjSpec, remove all references, return 0 on success. +MJAPI int mjs_detachBody(mjSpec* s, mjsBody* b); + +// Delete default class and descendants from mjSpec, remove all references, return 0 on success. +MJAPI int mjs_detachDefault(mjSpec* s, mjsDefault* d); + +//---------------------------------- Tree elements ------------------------------------------------- + +// Add child body to body, return child. +MJAPI mjsBody* mjs_addBody(mjsBody* body, const mjsDefault* def); + +// Add site to body, return site spec. +MJAPI mjsSite* mjs_addSite(mjsBody* body, const mjsDefault* def); + +// Add joint to body. +MJAPI mjsJoint* mjs_addJoint(mjsBody* body, const mjsDefault* def); + +// Add freejoint to body. +MJAPI mjsJoint* mjs_addFreeJoint(mjsBody* body); + +// Add geom to body. +MJAPI mjsGeom* mjs_addGeom(mjsBody* body, const mjsDefault* def); + +// Add camera to body. +MJAPI mjsCamera* mjs_addCamera(mjsBody* body, const mjsDefault* def); + +// Add light to body. +MJAPI mjsLight* mjs_addLight(mjsBody* body, const mjsDefault* def); + +// Add frame to body. +MJAPI mjsFrame* mjs_addFrame(mjsBody* body, mjsFrame* parentframe); + +// Delete object corresponding to the given element, return 0 on success. +MJAPI int mjs_delete(mjsElement* element); + + +//---------------------------------- Non-tree elements --------------------------------------------- + +// Add actuator. +MJAPI mjsActuator* mjs_addActuator(mjSpec* s, const mjsDefault* def); + +// Add sensor. +MJAPI mjsSensor* mjs_addSensor(mjSpec* s); + +// Add flex. +MJAPI mjsFlex* mjs_addFlex(mjSpec* s); + +// Add contact pair. +MJAPI mjsPair* mjs_addPair(mjSpec* s, const mjsDefault* def); + +// Add excluded body pair. +MJAPI mjsExclude* mjs_addExclude(mjSpec* s); + +// Add equality. +MJAPI mjsEquality* mjs_addEquality(mjSpec* s, const mjsDefault* def); + +// Add tendon. +MJAPI mjsTendon* mjs_addTendon(mjSpec* s, const mjsDefault* def); + +// Wrap site using tendon. +MJAPI mjsWrap* mjs_wrapSite(mjsTendon* tendon, const char* name); + +// Wrap geom using tendon. +MJAPI mjsWrap* mjs_wrapGeom(mjsTendon* tendon, const char* name, const char* sidesite); + +// Wrap joint using tendon. +MJAPI mjsWrap* mjs_wrapJoint(mjsTendon* tendon, const char* name, double coef); + +// Wrap pulley using tendon. +MJAPI mjsWrap* mjs_wrapPulley(mjsTendon* tendon, double divisor); + +// Add numeric. +MJAPI mjsNumeric* mjs_addNumeric(mjSpec* s); + +// Add text. +MJAPI mjsText* mjs_addText(mjSpec* s); + +// Add tuple. +MJAPI mjsTuple* mjs_addTuple(mjSpec* s); + +// Add keyframe. +MJAPI mjsKey* mjs_addKey(mjSpec* s); + +// Add plugin. +MJAPI mjsPlugin* mjs_addPlugin(mjSpec* s); + +// Add default. +MJAPI mjsDefault* mjs_addDefault(mjSpec* s, const char* classname, const mjsDefault* parent); + + +//---------------------------------- Assets -------------------------------------------------------- + +// Add mesh. +MJAPI mjsMesh* mjs_addMesh(mjSpec* s, const mjsDefault* def); + +// Add height field. +MJAPI mjsHField* mjs_addHField(mjSpec* s); + +// Add skin. +MJAPI mjsSkin* mjs_addSkin(mjSpec* s); + +// Add texture. +MJAPI mjsTexture* mjs_addTexture(mjSpec* s); + +// Add material. +MJAPI mjsMaterial* mjs_addMaterial(mjSpec* s, const mjsDefault* def); + + +//---------------------------------- Find and get utilities ---------------------------------------- + +// Get spec from body. +MJAPI mjSpec* mjs_getSpec(mjsElement* element); + +// Find spec (model asset) by name. +MJAPI mjSpec* mjs_findSpec(mjSpec* spec, const char* name); + +// Find body in spec by name. +MJAPI mjsBody* mjs_findBody(mjSpec* s, const char* name); + +// Find element in spec by name. +MJAPI mjsElement* mjs_findElement(mjSpec* s, mjtObj type, const char* name); + +// Find child body by name. +MJAPI mjsBody* mjs_findChild(mjsBody* body, const char* name); + +// Get parent body. +MJAPI mjsBody* mjs_getParent(mjsElement* element); + +// Get parent frame. +MJAPI mjsFrame* mjs_getFrame(mjsElement* element); + +// Find frame by name. +MJAPI mjsFrame* mjs_findFrame(mjSpec* s, const char* name); + +// Get default corresponding to an element. +MJAPI mjsDefault* mjs_getDefault(mjsElement* element); + +// Find default in model by class name. +MJAPI mjsDefault* mjs_findDefault(mjSpec* s, const char* classname); + +// Get global default from model. +MJAPI mjsDefault* mjs_getSpecDefault(mjSpec* s); + +// Get element id. +MJAPI int mjs_getId(mjsElement* element); + +// Return body's first child of given type. If recurse is nonzero, also search the body's subtree. +MJAPI mjsElement* mjs_firstChild(mjsBody* body, mjtObj type, int recurse); + +// Return body's next child of the same type; return NULL if child is last. +// If recurse is nonzero, also search the body's subtree. +MJAPI mjsElement* mjs_nextChild(mjsBody* body, mjsElement* child, int recurse); + +// 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); + + +//---------------------------------- Attribute setters --------------------------------------------- + +// Copy buffer. +MJAPI void mjs_setBuffer(mjByteVec* dest, const void* array, int size); + +// Copy text to string. +MJAPI void mjs_setString(mjString* dest, const char* text); + +// Split text to entries and copy to string vector. +MJAPI void mjs_setStringVec(mjStringVec* dest, const char* text); + +// Set entry in string vector. +MJAPI mjtByte mjs_setInStringVec(mjStringVec* dest, int i, const char* text); + +// Append text entry to string vector. +MJAPI void mjs_appendString(mjStringVec* dest, const char* text); + +// Copy int array to vector. +MJAPI void mjs_setInt(mjIntVec* dest, const int* array, int size); + +// Append int array to vector of arrays. +MJAPI void mjs_appendIntVec(mjIntVecVec* dest, const int* array, int size); + +// Copy float array to vector. +MJAPI void mjs_setFloat(mjFloatVec* dest, const float* array, int size); + +// Append float array to vector of arrays. +MJAPI void mjs_appendFloatVec(mjFloatVecVec* dest, const float* array, int size); + +// Copy double array to vector. +MJAPI void mjs_setDouble(mjDoubleVec* dest, const double* array, int size); + +// Set plugin attributes. +MJAPI void mjs_setPluginAttributes(mjsPlugin* plugin, void* attributes); + + +//---------------------------------- Attribute getters --------------------------------------------- + +// Get string contents. +MJAPI const char* mjs_getString(const mjString* source); + +// Get double array contents and optionally its size. +MJAPI const double* mjs_getDouble(const mjDoubleVec* source, int* size); + + +//---------------------------------- Spec utilities ------------------------------------------------ + +// Set element's default. +MJAPI void mjs_setDefault(mjsElement* element, const mjsDefault* def); + +// Set element's enclosing frame, return 0 on success. +MJAPI int mjs_setFrame(mjsElement* dest, mjsFrame* frame); + +// Resolve alternative orientations to quat, return error if any. +MJAPI const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* sequence, + const mjsOrientation* orientation); + +// Transform body into a frame. +MJAPI mjsFrame* mjs_bodyToFrame(mjsBody** body); + +// Set user payload, overriding the existing value for the specified key if present. +MJAPI void mjs_setUserValue(mjsElement* element, const char* key, const void* data); + +// Return user payload or NULL if none found. +MJAPI const void* mjs_getUserValue(mjsElement* element, const char* key); + +// Delete user payload. +MJAPI void mjs_deleteUserValue(mjsElement* element, const char* key); + +//---------------------------------- Element initialization --------------------------------------- + +// Default spec attributes. +MJAPI void mjs_defaultSpec(mjSpec* spec); + +// Default orientation attributes. +MJAPI void mjs_defaultOrientation(mjsOrientation* orient); + +// Default body attributes. +MJAPI void mjs_defaultBody(mjsBody* body); + +// Default frame attributes. +MJAPI void mjs_defaultFrame(mjsFrame* frame); + +// Default joint attributes. +MJAPI void mjs_defaultJoint(mjsJoint* joint); + +// Default geom attributes. +MJAPI void mjs_defaultGeom(mjsGeom* geom); + +// Default site attributes. +MJAPI void mjs_defaultSite(mjsSite* site); + +// Default camera attributes. +MJAPI void mjs_defaultCamera(mjsCamera* camera); + +// Default light attributes. +MJAPI void mjs_defaultLight(mjsLight* light); + +// Default flex attributes. +MJAPI void mjs_defaultFlex(mjsFlex* flex); + +// Default mesh attributes. +MJAPI void mjs_defaultMesh(mjsMesh* mesh); + +// Default height field attributes. +MJAPI void mjs_defaultHField(mjsHField* hfield); + +// Default skin attributes. +MJAPI void mjs_defaultSkin(mjsSkin* skin); + +// Default texture attributes. +MJAPI void mjs_defaultTexture(mjsTexture* texture); + +// Default material attributes. +MJAPI void mjs_defaultMaterial(mjsMaterial* material); + +// Default pair attributes. +MJAPI void mjs_defaultPair(mjsPair* pair); + +// Default equality attributes. +MJAPI void mjs_defaultEquality(mjsEquality* equality); + +// Default tendon attributes. +MJAPI void mjs_defaultTendon(mjsTendon* tendon); + +// Default actuator attributes. +MJAPI void mjs_defaultActuator(mjsActuator* actuator); + +// Default sensor attributes. +MJAPI void mjs_defaultSensor(mjsSensor* sensor); + +// Default numeric attributes. +MJAPI void mjs_defaultNumeric(mjsNumeric* numeric); + +// Default text attributes. +MJAPI void mjs_defaultText(mjsText* text); + +// Default tuple attributes. +MJAPI void mjs_defaultTuple(mjsTuple* tuple); + +// Default keyframe attributes. +MJAPI void mjs_defaultKey(mjsKey* key); + +// Default plugin attributes. +MJAPI void mjs_defaultPlugin(mjsPlugin* plugin); + + +//---------------------------------- Element casting ----------------------------------------------- + +// 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); + +// Safely cast an element as mjsPlugin, or return NULL if the element is not an mjsPlugin. +MJAPI mjsPlugin* mjs_asPlugin(mjsElement* element); + +#ifdef __cplusplus +} +#endif + +#endif // MUJOCO_MUJOCO_H_ From 3f698bf0d8a95b16badd94d89abd258c4080c2d7 Mon Sep 17 00:00:00 2001 From: kanarus Date: Sat, 8 Nov 2025 11:05:32 +0900 Subject: [PATCH 2/3] specify `=0.72.1` for `bindgen` dependency & regenerate bindgen.rs --- Cargo.toml | 2 +- src/bindgen.rs | 10 ++++++++-- 2 files changed, 9 insertions(+), 3 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index 758f3d7..9576b2c 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -13,7 +13,7 @@ keywords = ["mujoco", "rl", "ml", "physics", "robotics"] categories = ["api-bindings", "science::robotics", "simulation"] [build-dependencies] -bindgen = { optional = true, version = "0.72" } +bindgen = { optional = true, version = "=0.72.1" } [dev-dependencies] glfw = "0.60" diff --git a/src/bindgen.rs b/src/bindgen.rs index a7eec03..7443835 100644 --- a/src/bindgen.rs +++ b/src/bindgen.rs @@ -1,4 +1,4 @@ -/* automatically generated by rust-bindgen 0.72.0 */ +/* automatically generated by rust-bindgen 0.72.1 */ #![allow(unused, non_camel_case_types, non_snake_case, non_upper_case_globals)] @@ -2141,7 +2141,13 @@ pub type mjfResourceModified = ::core::option::Option< #[repr(C)] #[derive(Debug, Copy, Clone)] pub struct mjpResourceProvider { - pub _address: u8, + pub(crate) prefix: *const ::core::ffi::c_char, + pub(crate) open: mjfOpenResource, + pub(crate) read: mjfReadResource, + pub(crate) close: mjfCloseResource, + pub(crate) getdir: mjfGetResourceDir, + pub(crate) modified: mjfResourceModified, + pub(crate) data: *mut ::core::ffi::c_void, } impl mjtPluginCapabilityBit { pub const ACTUATOR: mjtPluginCapabilityBit = mjtPluginCapabilityBit(1); From 7b7d06f8a8b3de6c4af3832591fa0a9e6ae441dd Mon Sep 17 00:00:00 2001 From: kanarus Date: Sat, 8 Nov 2025 11:14:12 +0900 Subject: [PATCH 3/3] bindgen: simplicy by removing src/bindgen.h & inlining it in `.header_contents()` --- build.rs | 5 +---- src/bindgen.h | 1 - 2 files changed, 1 insertion(+), 5 deletions(-) delete mode 100644 src/bindgen.h diff --git a/build.rs b/build.rs index 1d110d3..40929f7 100644 --- a/build.rs +++ b/build.rs @@ -66,14 +66,11 @@ fn bindgen() { let vendor_include_mujoco = vendor_dir.join("include").join("mujoco").to_str().unwrap().to_owned(); let src_dir = std::path::Path::new(env!("CARGO_MANIFEST_DIR")).join("src"); - let bindgen_h = src_dir.join("bindgen.h").to_str().unwrap().to_owned(); let bindgen_rs = src_dir.join("bindgen.rs").to_str().unwrap().to_owned(); - println!("cargo:rerun-if-changed={bindgen_h}"); - let mut bindings = Vec::new(); bindgen::builder() - .header(bindgen_h) + .header_contents("bindgen.h", "#include \"mujoco.h\"") .clang_args([format!("-I{vendor_include}"), format!("-I{vendor_include_mujoco}")]) .use_core() .raw_line("#![allow(unused, non_camel_case_types, non_snake_case, non_upper_case_globals)]") diff --git a/src/bindgen.h b/src/bindgen.h deleted file mode 100644 index f62236c..0000000 --- a/src/bindgen.h +++ /dev/null @@ -1 +0,0 @@ -#include "mujoco.h" \ No newline at end of file