diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 5b985861..e413f772 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -748,8 +748,7 @@ Compare forward and inverse dynamics, save results in fwdinv. Sub components ^^^^^^^^^^^^^^ -These are sub-components of the simulation pipeline, called internally from the components above. It is very unlikely -that the user will need to call them. +These are sub-components of the simulation pipeline, called internally from the components above. .. _mj_sensorPos: @@ -886,6 +885,18 @@ Compute actuator transmission lengths and moments. Run composite rigid body inertia algorithm (CRB). +.. _mj_makeM: + +`mj_makeM <#mj_makeM>`__ +~~~~~~~~~~~~~~~~~~~~~~~~ + +.. mujoco-include:: mj_makeM + +Compute the composite rigid body inertia with :ref:`mj_crb`, add terms due +to :ref:`tendon armature`. The joint-space inertia matrix is stored in both ``mjData.qM`` and +``mjData.M``. These arrays represent the same quantity using different layouts (parent-based and compressed sparse row, +respectively). + .. _mj_factorM: `mj_factorM <#mj_factorM>`__ diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index ec72a183..5d00db0a 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -113,8 +113,14 @@ Integrates the simulation state using an implicit-in-velocity integrator (either .. _Subcomponents: -These are sub-components of the simulation pipeline, called internally from the components above. It is very unlikely -that the user will need to call them. +These are sub-components of the simulation pipeline, called internally from the components above. + +.. _mj_makeM: + +Compute the composite rigid body inertia with :ref:`mj_crb`, add terms due +to :ref:`tendon armature`. The joint-space inertia matrix is stored in both ``mjData.qM`` and +``mjData.M``. These arrays represent the same quantity using different layouts (parent-based and compressed sparse row, +respectively). .. _mj_factorM: diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 727950af..e276c831 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2918,8 +2918,8 @@ Attributes may be applied or ignored depending on the lighting model being used. .. _body-light-directional: :at:`directional`: :at-val:`[false, true], "false"` - This is a deprecated legacy attribute. Please use :ref:`light ` type instead. If set to "true", and no - type is specified, this will change the light type to be directional. + This is a deprecated legacy attribute. Please use :ref:`light ` type instead. If set to "true", and + no type is specified, this will change the light type to be directional. .. _body-light-castshadow: diff --git a/doc/changelog.rst b/doc/changelog.rst index 3ddc1b51..8f5135b6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -14,6 +14,9 @@ General - Replaced the :ref:`directional` (boolean) field for lights with a :ref:`type` field (of type :ref:`mjtLightType`) to allow for additional lighting types. +- Added new sub-component :ref:`mj_makeM` which combines the :ref:`mj_crb` call with additional logic to support the + introduction in 3.3.1 of :ref:`tendon armature`. In addition to the traditional + ``mjData.qM``, :ref:`mj_makeM` also computes ``mjData.M``, a CSR representation of the same matrix. Simulate ^^^^^^^^ diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 1021067a..ff87af05 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1692,7 +1692,7 @@ The stages below compute quantities that depend on the generalized positions ``m 4. Compute quantities related to :ref:`flex` objects: :ref:`mj_flex` 5. Compute the tendon lengths and moment arms. This includes the computation of minimal-length paths for spatial tendons: :ref:`mj_tendon` -6. Compute the composite rigid body inertias and joint-space inertia matrix: :ref:`mj_crb` +6. Compute the composite rigid body inertias and joint-space inertia matrix: :ref:`mj_makeM` 7. Compute the sparse factorization of the joint-space inertia matrix: :ref:`mj_factorM` 8. Construct the list of active contacts. This includes both broad-phase and near-phase collision detection: :ref:`mj_collision` diff --git a/doc/includes/references.h b/doc/includes/references.h index f61acd8e..2acf7ea6 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -267,7 +267,7 @@ struct mjData_ { 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 + // computed by mj_fwdPosition/mj_makeM mjtNum* crb; // com-based composite inertia and mass (nbody x 10) mjtNum* qM; // inertia (sparse) (nM x 1) mjtNum* M; // reduced inertia (compressed sparse row) (nC x 1) @@ -3068,6 +3068,7 @@ void mj_flex(const mjModel* m, mjData* d); void mj_tendon(const mjModel* m, mjData* d); void mj_transmission(const mjModel* m, mjData* d); void mj_crb(const mjModel* m, mjData* d); +void mj_makeM(const mjModel* m, mjData* d); void mj_factorM(const mjModel* m, mjData* d); void mj_solveM(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, int n); void mj_solveM2(const mjModel* m, mjData* d, mjtNum* x, const mjtNum* y, diff --git a/include/mujoco/mjdata.h b/include/mujoco/mjdata.h index c05db9ab..1d894fd4 100644 --- a/include/mujoco/mjdata.h +++ b/include/mujoco/mjdata.h @@ -295,7 +295,7 @@ struct mjData_ { 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 + // computed by mj_fwdPosition/mj_makeM mjtNum* crb; // com-based composite inertia and mass (nbody x 10) mjtNum* qM; // inertia (sparse) (nM x 1) mjtNum* M; // reduced inertia (compressed sparse row) (nC x 1) diff --git a/include/mujoco/mujoco.h b/include/mujoco/mujoco.h index 6c715427..467d888a 100644 --- a/include/mujoco/mujoco.h +++ b/include/mujoco/mujoco.h @@ -364,6 +364,9 @@ 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); +// Make inertia matrix. +MJAPI void mj_makeM(const mjModel* m, mjData* d); + // Compute sparse L'*D*L factorizaton of inertia matrix. MJAPI void mj_factorM(const mjModel* m, mjData* d); diff --git a/python/mujoco/functions.cc b/python/mujoco/functions.cc index d4708254..b23c6d1e 100644 --- a/python/mujoco/functions.cc +++ b/python/mujoco/functions.cc @@ -211,6 +211,7 @@ PYBIND11_MODULE(_functions, pymodule) { Def(pymodule); Def(pymodule); Def(pymodule); + Def(pymodule); Def(pymodule); DEF_WITH_OMITTED_PY_ARGS(traits::mj_solveM, "n")( pymodule, diff --git a/python/mujoco/introspect/functions.py b/python/mujoco/introspect/functions.py index cf85a819..44901fe4 100644 --- a/python/mujoco/introspect/functions.py +++ b/python/mujoco/introspect/functions.py @@ -1829,6 +1829,26 @@ FUNCTIONS: Mapping[str, FunctionDecl] = dict([ ), doc='Run composite rigid body inertia algorithm (CRB).', )), + ('mj_makeM', + FunctionDecl( + name='mj_makeM', + return_type=ValueType(name='void'), + parameters=( + FunctionParameterDecl( + name='m', + type=PointerType( + inner_type=ValueType(name='mjModel', is_const=True), + ), + ), + FunctionParameterDecl( + name='d', + type=PointerType( + inner_type=ValueType(name='mjData'), + ), + ), + ), + doc='Make inertia matrix.', + )), ('mj_factorM', FunctionDecl( name='mj_factorM', diff --git a/src/engine/engine_core_smooth.h b/src/engine/engine_core_smooth.h index a8cd1a2b..6a672b3a 100644 --- a/src/engine/engine_core_smooth.h +++ b/src/engine/engine_core_smooth.h @@ -55,7 +55,7 @@ MJAPI void mj_crb(const mjModel* m, mjData* d); MJAPI void mj_tendonArmature(const mjModel* m, mjData* d); // make inertia matrix -void mj_makeM(const mjModel* m, mjData* d); +MJAPI void mj_makeM(const mjModel* m, mjData* d); // sparse L'*D*L factorizaton of inertia-like matrix M, assumed spd (legacy implementation) MJAPI void mj_factorI_legacy(const mjModel* m, mjData* d, const mjtNum* M, diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index ae6a9ac5..d15803a7 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -6511,6 +6511,9 @@ public static unsafe extern void mj_transmission(mjModel_* m, mjData_* d); [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_crb(mjModel_* m, mjData_* d); +[DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] +public static unsafe extern void mj_makeM(mjModel_* m, mjData_* d); + [DllImport("mujoco", CallingConvention = CallingConvention.Cdecl)] public static unsafe extern void mj_factorM(mjModel_* m, mjData_* d);