From 1141b4abbc26a76374699d659e3337e8e5df3df6 Mon Sep 17 00:00:00 2001 From: Saran Tunyasuvunakool Date: Tue, 13 Aug 2024 14:33:44 -0700 Subject: [PATCH 01/24] Force dtype of `mjx.Contact.{geom1,geom2,geom}` to int32. This improves compatibility with MuJoCo. PiperOrigin-RevId: 662655282 Change-Id: I2a336d9453f40cf0bfa85a7203b55709e23a07fe --- mjx/mujoco/mjx/_src/io.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 202cfd9d..51001559 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -153,9 +153,9 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: solreffriction=jp.zeros((ncon, mujoco.mjNREF), dtype=float), solimp=jp.zeros((ncon, mujoco.mjNIMP), dtype=float), dim=dim, - geom1=jp.full((ncon,), -1, dtype=int), - geom2=jp.full((ncon,), -1, dtype=int), - geom=jp.full((ncon, 2), -1, dtype=int), + geom1=jp.full((ncon,), -1, dtype=jp.int32), + geom2=jp.full((ncon,), -1, dtype=jp.int32), + geom=jp.full((ncon, 2), -1, dtype=jp.int32), efc_address=efc_address, ) From 7efd690471bbc8fbdac10650b96b326d7f8d65dc Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 14 Aug 2024 02:23:23 -0700 Subject: [PATCH 02/24] Document where mesh transforms are saved, fixes #1894 PiperOrigin-RevId: 662836678 Change-Id: I060e3b1f29e82c6c3c188e730657cda7b99f63e9 --- doc/XMLreference.rst | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index b6f240ec..338f7b7c 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -1106,7 +1106,8 @@ to convert to one of the other supported formats. .. _legacy-msh-docs: -MSH file format +.. collapse:: Legacy MSH file format + The binary MSH file starts with 4 integers specifying the number of vertex positions (nvertex), vertex normals (nnormal), vertex texture coordinates (ntexcoord), and vertex indices making up the faces (nface), followed by the numeric data. nvertex must be at least 4. nnormal and ntexcoord can be zero (in which case the corresponding data is @@ -1190,7 +1191,8 @@ The full list of processing steps applied by the compiler to each mesh is as fol normals. If sharp edges are encountered, the renderer uses the face normals to preserve the visual information about the edge, unless :ref:`smoothnormal` is true. Note that normals cannot be provided with STL meshes; -#. Scale, translate and rotate the vertices and normals, re-normalize the normals in case of scaling; +#. Scale, translate and rotate the vertices and normals, re-normalize the normals in case of scaling. Save these + transformations in ``mjModel.mesh_{pos, quat, scale}``. #. Construct the convex hull if specified; #. Find the centroid of all triangle faces, and construct the union-of-pyramids representation. Triangles whose area is too small (below the :ref:`mjMINVAL ` value of 1E-14) result in compile error; From 2cb601c98d88acb2ccf4386991056c72a700e100 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 14 Aug 2024 03:10:28 -0700 Subject: [PATCH 03/24] Improve documentation w.r.t smoothness, differentiability and finite-differencing. PiperOrigin-RevId: 662848709 Change-Id: Icb7dc03a2c53d43d0d3a9e4ac95d963e5bc6853c --- doc/APIreference/functions.rst | 26 ++++++++++++++++++------ doc/APIreference/functions_override.rst | 27 +++++++++++++++++++------ doc/computation/index.rst | 15 ++++++++------ doc/modeling.rst | 22 ++++++++++++++------ 4 files changed, 66 insertions(+), 24 deletions(-) diff --git a/doc/APIreference/functions.rst b/doc/APIreference/functions.rst index 23561a19..f93a31b7 100644 --- a/doc/APIreference/functions.rst +++ b/doc/APIreference/functions.rst @@ -2589,7 +2589,7 @@ mjd_transitionFD .. mujoco-include:: mjd_transitionFD -Finite-differenced discrete-time transition matrices. +Compute finite-differenced discrete-time transition matrices. Letting :math:`x, u` denote the current :ref:`state` and :ref:`control` vector in an mjData instance, and letting :math:`y, s` denote the next state and sensor @@ -2610,12 +2610,26 @@ These matrices and their dimensions are: - All outputs are optional (can be NULL). - ``eps`` is the finite-differencing epsilon. - ``flg_centered`` denotes whether to use forward (0) or centered (1) differences. -- Accuracy can be somewhat improved if solver :ref:`iterations` are set to a - fixed (small) value and solver :ref:`tolerance` is set to 0. This insures that - all calls to the solver will perform exactly the same number of iterations. +- The Runge-Kutta integrator (:ref:`mjINT_RK4`) is not supported. -.. attention:: - - The Runge-Kutta 4th-order integrator (``mjINT_RK4``) is not supported. +.. admonition:: Improving speed and accuracy + :class: tip + + warmstart + If warm-starts are not :ref:`disabled`, the warm-start accelerations + ``mjData.qacc_warmstart`` which are present at call-time are loaded at the start of every relevant pipeline call, + to preserve determinism. If solver computations are an expensive part of the simulation, the following trick can + lead to significant speed-ups: First call :ref:`mj_forward` to let the solver converge, then reduce :ref:`solver + iterations` significantly, then call :ref:`mjd_transitionFD`, finally, restore the original + value of :ref:`iterations`. Because we are already near the solution, few iteration are required + to find the new minimum. This is especially true for the :ref:`Newton` solver, where the required + number of iteration for convergence near the minimum can be as low as 1. + + tolerance + Accuracy can be improved if solver :ref:`tolerance` is set to 0. This means that all calls to + the solver will perform exactly the same number of iterations, preventing numerical errors due to early + termination. Of course, this means that :ref:`solver iterations` should be small, to not tread + water at the minimum. This method and the one described above can and should be combined. .. _mjd_inverseFD: diff --git a/doc/APIreference/functions_override.rst b/doc/APIreference/functions_override.rst index 35e651e7..4e7b6b50 100644 --- a/doc/APIreference/functions_override.rst +++ b/doc/APIreference/functions_override.rst @@ -567,7 +567,7 @@ outputs of derivative functions are the trailing rather than leading arguments. .. _mjd_transitionFD: -Finite-differenced discrete-time transition matrices. +Compute finite-differenced discrete-time transition matrices. Letting :math:`x, u` denote the current :ref:`state` and :ref:`control` vector in an mjData instance, and letting :math:`y, s` denote the next state and sensor @@ -588,12 +588,27 @@ These matrices and their dimensions are: - All outputs are optional (can be NULL). - ``eps`` is the finite-differencing epsilon. - ``flg_centered`` denotes whether to use forward (0) or centered (1) differences. -- Accuracy can be somewhat improved if solver :ref:`iterations` are set to a - fixed (small) value and solver :ref:`tolerance` is set to 0. This insures that - all calls to the solver will perform exactly the same number of iterations. +- The Runge-Kutta integrator (:ref:`mjINT_RK4`) is not supported. + +.. admonition:: Improving speed and accuracy + :class: tip + + warmstart + If warm-starts are not :ref:`disabled`, the warm-start accelerations + ``mjData.qacc_warmstart`` which are present at call-time are loaded at the start of every relevant pipeline call, + to preserve determinism. If solver computations are an expensive part of the simulation, the following trick can + lead to significant speed-ups: First call :ref:`mj_forward` to let the solver converge, then reduce :ref:`solver + iterations` significantly, then call :ref:`mjd_transitionFD`, finally, restore the original + value of :ref:`iterations`. Because we are already near the solution, few iteration are required + to find the new minimum. This is especially true for the :ref:`Newton` solver, where the required + number of iteration for convergence near the minimum can be as low as 1. + + tolerance + Accuracy can be improved if solver :ref:`tolerance` is set to 0. This means that all calls to + the solver will perform exactly the same number of iterations, preventing numerical errors due to early + termination. Of course, this means that :ref:`solver iterations` should be small, to not tread + water at the minimum. This method and the one described above can and should be combined. -.. attention:: - - The Runge-Kutta 4th-order integrator (``mjINT_RK4``) is not supported. .. _mjd_inverseFD: diff --git a/doc/computation/index.rst b/doc/computation/index.rst index 06e0da68..8d00a459 100644 --- a/doc/computation/index.rst +++ b/doc/computation/index.rst @@ -1722,20 +1722,23 @@ The top-level function :ref:`mj_inverse` invokes the following sequence of compu Derivatives ----------- -MuJoCo's entire computational pipline including its constraint solver are analytically differentiable. Writing -efficient implementations of these derivatives is a long term goal of the development team. Analytic derivatives of the -smooth dynamics (excluding constraints) with respect to velocity are already computed and enable the two +MuJoCo's entire computational pipline including its constraint solver are analytically differentiable in principle. +Writing efficient implementations of these derivatives is a long term goal of the development team. Analytic derivatives +of the smooth dynamics (excluding constraints) with respect to velocity are already computed and enable the two :ref:`implicit integrators`. +Note that the default value of the :ref:`solver impedance` is such that contacts are *not* +differentiable by default, and needs to be :ref:`set to 0` in order for contact-force onset to be smooth. + Two functions are currently available which use efficient finite-differencing in order to compute dynamics Jacobians: :ref:`mjd_transitionFD`: Computes state-transition and control-transition Jacobians for the discrete-time forward dynamics (:ref:`mj_step`). - See :ref:`API documentation`. + See :ref:`documentation`. :ref:`mjd_inverseFD`: - Computes Jacobians for the continuous-time inverse dynamics (:ref:`mj_inverse`). - See :ref:`API documentation`. + Computes Jacobians for the continuous or discrete-time inverse dynamics (:ref:`mj_inverse`). + See :ref:`documentation`. These derivatives are made efficient by exploiting MuJoCo's configurable computation pipeline so that quantities are not recomputed when not required. For example when differencing with respect to controls, quantities which depend only on diff --git a/doc/modeling.rst b/doc/modeling.rst index 325317f1..41b09598 100644 --- a/doc/modeling.rst +++ b/doc/modeling.rst @@ -347,8 +347,15 @@ of the function :math:`d(r)` is determined by the element-specific parameter vec For friction loss or friction dimensions of elliptic cones, the violation :math:`r` is identically zero, so only :math:`d(0)` affects these constraints, all other :at:`solimp` values are ignored. - .. tip:: - For completely smooth dynamics, limits and contacts should have :math:`d_0=0`. + .. _solimp0: + + .. admonition:: Smoothness and differentiability + :class: tip + + For completely smooth (differentiable) dynamics, limits and contacts should have :math:`d_0=0` (``solimp[0]=0``). + Specifically for contacts, the :ref:`mixing rules` of geom-associated solver parameters should be kept + in mind. See also discussion of derivatives in the :ref:`Computation chapter` and in the + :ref:`mjd_transitionFD` documentation. .. _CSolverReference: @@ -485,11 +492,14 @@ are as follows: **margin**, **gap** The maximum of the two geom margins (or gaps respectively) is used. The geom priority is ignored here, because the margin and gap are distance properties and a one-sided specification makes little sense. + +.. _solmixing: + **solref**, **solimp** - If one of the two geoms has higher priority, its solref and solimp parameters are used. If both geoms have the same - priority, the weighted average is used. The weights are proportional to the solmix attributes, i.e., weight1 = - solmix1 / (solmix1 + solmix2) and similarly for weight2. There is one important exception to this weighted averaging - rule. If solref for either geom is non-positive, i.e., it relies on the direct format, + If one of the two geoms has higher :ref:`priority`, its solref and solimp parameters are used. If + both geoms have the same priority, the weighted average is used. The weights are proportional to the solmix + attributes, i.e., weight1 = solmix1 / (solmix1 + solmix2) and similarly for weight2. There is one important exception + to this weighted averaging rule. If solref for either geom is non-positive, i.e., it relies on the direct format, then the element-wise minimum is used regardless of solmix. This is because averaging solref parameters in different formats would be meaningless. From afd7c73f44e78c262aada0eadb6695c62c32632c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 14 Aug 2024 04:41:51 -0700 Subject: [PATCH 04/24] Attach multiple keyframes. Still missing: support for ctrl, mocap, and time. PiperOrigin-RevId: 662871569 Change-Id: I8a418c4007663e3063d7146bd72db4eba78ddc5d --- doc/changelog.rst | 2 ++ src/user/user_model.cc | 31 +++++++++++++++++----------- src/user/user_model.h | 2 ++ src/user/user_objects.cc | 41 +++++++++++++++++++++++++++++++------- src/user/user_objects.h | 20 ++++++++++++------- test/user/user_api_test.cc | 3 +++ 6 files changed, 73 insertions(+), 26 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index f601660f..e48c492b 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -9,6 +9,8 @@ General ^^^^^^^ 1. Add :ref:`mjSpec` option for creating a texture from a buffer. 2. :ref:`shellinertia ` is now supported by all geom types. +3. Add support for :ref:`attaching` keyframes. Note: this only supports keyframe containing qpos, qvel, + and act. Version 3.2.2 (Aug 8, 2024) --------------------------- diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 0d25cc59..5ea2124c 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -109,6 +109,9 @@ mjCModel::mjCModel() { // this class allocated the plugins plugin_owner = true; + + // default state name + state_name_ = "state"; } @@ -2862,14 +2865,17 @@ void mjCModel::CopyObjects(mjModel* m) { template void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act) { for (auto joint : joints_) { - if (qpos) mjuu_copyvec(joint->qpos, qpos + joint->qposadr_, joint->nq()); - if (qvel) mjuu_copyvec(joint->qvel, qvel + joint->dofadr_, joint->nv()); + if (joint->qposadr_ == -1 || joint->dofadr_ == -1) { + throw mjCError(NULL, "SaveState: joint %s has no address", joint->name.c_str()); + } + if (qpos) mjuu_copyvec(joint->qpos(), qpos + joint->qposadr_, joint->nq()); + if (qvel) mjuu_copyvec(joint->qvel(), qvel + joint->dofadr_, joint->nv()); } for (auto actuator : actuators_) { - if (actuator->actadr_ != -1 && act) { - actuator->act.assign(actuator->actdim_, 0); - mjuu_copyvec(actuator->act.data(), act + actuator->actadr_, actuator->actdim_); + if (actuator->actadr_ != -1 && actuator->actdim_ != -1 && act) { + actuator->act().assign(actuator->actdim_, 0); + mjuu_copyvec(actuator->act().data(), act + actuator->actadr_, actuator->actdim_); } } } @@ -2893,21 +2899,21 @@ template void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act) { for (auto joint : joints_) { if (qpos) { - if (mjuu_defined(joint->qpos[0])) { - mjuu_copyvec(qpos + joint->qposadr_, joint->qpos, joint->nq()); + if (mjuu_defined(joint->qpos()[0])) { + mjuu_copyvec(qpos + joint->qposadr_, joint->qpos(), joint->nq()); } else { mjuu_copyvec(qpos + joint->qposadr_, pos0 + joint->qposadr_, joint->nq()); } } - if (mjuu_defined(joint->qvel[0]) && qvel) { - mjuu_copyvec(qvel + joint->dofadr_, joint->qvel, joint->nv()); + if (mjuu_defined(joint->qvel()[0]) && qvel) { + mjuu_copyvec(qvel + joint->dofadr_, joint->qvel(), joint->nv()); } } // restore act for (auto actuator : actuators_) { - if (mjuu_defined(actuator->act[0]) && act) { - mjuu_copyvec(act + actuator->actadr_, actuator->act.data(), actuator->actdim_); + if (!actuator->act().empty() && mjuu_defined(actuator->act()[0]) && act) { + mjuu_copyvec(act + actuator->actadr_, actuator->act().data(), actuator->actdim_); } } } @@ -2941,8 +2947,8 @@ void mjCModel::StoreKeyframes() { info.qvel = !key->spec_qvel_.empty(); info.act = !key->spec_act_.empty(); key_pending_.push_back(info); + state_name_ = info.name; SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); - break; // (b/350784262) save only the first keyframe for now } if (resetlists) { @@ -3512,6 +3518,7 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { if (info.qpos) key->spec_qpos_.assign(nq, 0); if (info.qvel) key->spec_qvel_.assign(nv, 0); if (info.act) key->spec_act_.assign(na, 0); + state_name_ = info.name; RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); } diff --git a/src/user/user_model.h b/src/user/user_model.h index 22e37135..674ab72a 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -381,5 +381,7 @@ class mjCModel : public mjCModel_, private mjSpec { mjCError errInfo; // last error info bool plugin_owner; // this class allocated the plugins std::vector key_pending_; // attached keyframes + + std::string state_name_; }; #endif // MUJOCO_SRC_USER_USER_MODEL_H_ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index fb0a18fd..0afb39e7 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1408,8 +1408,8 @@ void mjCBody::ComputeBVH() { // reset keyframe references for allowing self-attach void mjCBody::ForgetKeyframes() const { for (auto joint : joints) { - joint->qpos[0] = mjNAN; - joint->qvel[0] = mjNAN; + joint->qpos_.clear(); + joint->qvel_.clear(); } for (auto body : bodies) { body->ForgetKeyframes(); @@ -1769,8 +1769,8 @@ mjCJoint::mjCJoint(mjCModel* _model, mjCDef* _def) { CopyFromSpec(); // no previous state when a joint is created - qpos[0] = mjNAN; - qvel[0] = mjNAN; + qposadr_ = -1; + dofadr_ = -1; } @@ -1828,6 +1828,24 @@ int mjCJoint::nv(mjtJoint joint_type) { +mjtNum* mjCJoint::qpos() { + if (qpos_.find(model->state_name_) == qpos_.end()) { + qpos_[model->state_name_] = {mjNAN, 0, 0, 0, 0, 0, 0}; + } + return qpos_.at(model->state_name_).data(); +} + + + +mjtNum* mjCJoint::qvel() { + if (qvel_.find(model->state_name_) == qvel_.end()) { + qvel_[model->state_name_] = {mjNAN, 0, 0, 0, 0, 0}; + } + return qvel_.at(model->state_name_).data(); +} + + + void mjCJoint::PointToLocal() { spec.element = static_cast(this); spec.name = &name; @@ -5349,7 +5367,8 @@ mjCActuator::mjCActuator(mjCModel* _model, mjCDef* _def) { PointToLocal(); // no previous state when an actuator is created - act.push_back(mjNAN); + actadr_ = -1; + actdim_ = -1; } @@ -5374,8 +5393,7 @@ mjCActuator& mjCActuator::operator=(const mjCActuator& other) { void mjCActuator::ForgetKeyframes() { - act.clear(); - act.push_back(mjNAN); + act_.clear(); } @@ -5386,6 +5404,15 @@ bool mjCActuator::is_actlimited() const { return islimited(actlimited, actrange) +std::vector& mjCActuator::act() { + if (act_.find(model->state_name_) == act_.end()) { + act_[model->state_name_] = std::vector(model->nu, mjNAN); + } + return act_.at(model->state_name_); +} + + + void mjCActuator::PointToLocal() { spec.element = static_cast(this); spec.name = &name; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 8eed97d9..bd39c7ef 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -16,6 +16,7 @@ #define MUJOCO_SRC_USER_USER_OBJECTS_H_ #include +#include #include #include #include @@ -401,10 +402,10 @@ class mjCJoint_ : public mjCBase { mjCBody* body; // joint's body // variable used for temporarily storing the state of the joint - int qposadr_; // address of dof in data->qpos - int dofadr_; // address of dof in data->qvel - mjtNum qpos[7]; // qpos at the previous step - mjtNum qvel[6]; // qvel at the previous step + int qposadr_; // address of dof in data->qpos + int dofadr_; // address of dof in data->qvel + std::map> qpos_; // qpos at the previous step + std::map> qvel_; // qvel at the previous step // variable-size data std::vector userdata_; @@ -443,6 +444,9 @@ class mjCJoint : public mjCJoint_, private mjsJoint { int nq() const { return nq(spec.type); } int nv() const { return nv(spec.type); } + mjtNum* qpos(); + mjtNum* qvel(); + private: int Compile(void); // compiler; return dofnum void PointToLocal(void); @@ -1390,9 +1394,9 @@ class mjCActuator_ : public mjCBase { int trnid[2]; // id of transmission target // variable used for temporarily storing the state of the actuator - int actadr_; // address of dof in data->act - int actdim_; // number of dofs in data->act - std::vector act; // act at the previous step + int actadr_; // address of dof in data->act + int actdim_; // number of dofs in data->act + std::map> act_; // act at the previous step // variable-size data std::string plugin_name; @@ -1431,6 +1435,8 @@ class mjCActuator : public mjCActuator_, private mjsActuator { bool is_forcelimited() const; bool is_actlimited() const; + std::vector& act(); + private: void Compile(void); // compiler void CopyFromSpec(); diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e5c581d5..75280243 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -504,6 +504,7 @@ TEST_F(MujocoTest, AttachSame) { + )"; @@ -621,6 +622,7 @@ TEST_F(MujocoTest, AttachDifferent) { + )"; @@ -742,6 +744,7 @@ TEST_F(MujocoTest, AttachFrame) { + )"; From 7be4df7e03217b08997fa15107e7015a3fb12473 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 14 Aug 2024 05:13:51 -0700 Subject: [PATCH 05/24] Add ctrl to attached keyframes. PiperOrigin-RevId: 662878508 Change-Id: Ie273fae0220c8b257766cbbd90a880d44b901725 --- src/user/user_api.cc | 4 ++-- src/user/user_model.cc | 32 ++++++++++++++++++++++++-------- src/user/user_model.h | 5 +++-- src/user/user_objects.cc | 10 ++++++++++ src/user/user_objects.h | 2 ++ test/user/user_api_test.cc | 20 ++++++++++---------- 6 files changed, 51 insertions(+), 22 deletions(-) diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 1a03b274..7ff36d73 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -96,12 +96,12 @@ void mj_recompile(mjSpec* s, const mjVFS* vfs, mjModel* m, mjData* d) { mjtNum time = 0; if (d) { time = d->time; - modelC->SaveState(d->qpos, d->qvel, d->act); + modelC->SaveState(d->qpos, d->qvel, d->act, d->ctrl); } modelC->Compile(vfs, &m); if (d) { modelC->MakeData(m, &d); - modelC->RestoreState(m->qpos0, d->qpos, d->qvel, d->act); + modelC->RestoreState(m->qpos0, d->qpos, d->qvel, d->act, d->ctrl); d->time = time; } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 5ea2124c..b96189be 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2863,7 +2863,7 @@ void mjCModel::CopyObjects(mjModel* m) { // save the current state template -void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act) { +void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act, const T* ctrl) { for (auto joint : joints_) { if (joint->qposadr_ == -1 || joint->dofadr_ == -1) { throw mjCError(NULL, "SaveState: joint %s has no address", joint->name.c_str()); @@ -2872,11 +2872,15 @@ void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act) { if (qvel) mjuu_copyvec(joint->qvel(), qvel + joint->dofadr_, joint->nv()); } - for (auto actuator : actuators_) { + for (unsigned int i=0; iactadr_ != -1 && actuator->actdim_ != -1 && act) { actuator->act().assign(actuator->actdim_, 0); mjuu_copyvec(actuator->act().data(), act + actuator->actadr_, actuator->actdim_); } + if (ctrl) { + actuator->ctrl() = ctrl[i]; + } } } @@ -2896,7 +2900,7 @@ void mjCModel::MakeData(const mjModel* m, mjData** dest) { // restore the previous state template -void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act) { +void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act, T* ctrl) { for (auto joint : joints_) { if (qpos) { if (mjuu_defined(joint->qpos()[0])) { @@ -2911,10 +2915,14 @@ void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act) { } // restore act - for (auto actuator : actuators_) { + for (unsigned int i=0; iact().empty() && mjuu_defined(actuator->act()[0]) && act) { mjuu_copyvec(act + actuator->actadr_, actuator->act().data(), actuator->actdim_); } + if (ctrl) { + ctrl[i] = mjuu_defined(actuator->ctrl()) ? actuator->ctrl() : 0; + } } } @@ -2923,10 +2931,11 @@ void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act) { // force explicit instantiations template void mjCModel::SaveState(const mjtNum* qpos, const mjtNum* qvel, - const mjtNum* act); + const mjtNum* act, + const mjtNum* ctrl); template void mjCModel::RestoreState(const mjtNum* qpos0, mjtNum* qpos, - mjtNum* qvel, mjtNum* act); + mjtNum* qvel, mjtNum* act, mjtNum* ctrl); @@ -2946,9 +2955,11 @@ void mjCModel::StoreKeyframes() { info.qpos = !key->spec_qpos_.empty(); info.qvel = !key->spec_qvel_.empty(); info.act = !key->spec_act_.empty(); + info.ctrl = !key->spec_ctrl_.empty(); key_pending_.push_back(info); state_name_ = info.name; - SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); + SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(), + key->spec_act_.data(), key->spec_ctrl_.data()); } if (resetlists) { @@ -3506,6 +3517,9 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { if (!key->spec_act_.empty()) { key->spec_act_.resize(na); } + if (!key->spec_ctrl_.empty()) { + key->spec_ctrl_.resize(nu); + } } // store dof offsets in joints and actuators @@ -3518,8 +3532,10 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { if (info.qpos) key->spec_qpos_.assign(nq, 0); if (info.qvel) key->spec_qvel_.assign(nv, 0); if (info.act) key->spec_act_.assign(na, 0); + if (info.ctrl) key->spec_ctrl_.assign(nu, 0); state_name_ = info.name; - RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); + RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(), + key->spec_act_.data(), key->spec_ctrl_.data()); } // the attached keyframes have been copied into the model diff --git a/src/user/user_model.h b/src/user/user_model.h index 674ab72a..8b004b79 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -39,6 +39,7 @@ typedef struct mjKeyInfo_ { bool qpos; bool qvel; bool act; + bool ctrl; } mjKeyInfo; class mjCModel_ : public mjsElement { @@ -283,8 +284,8 @@ class mjCModel : public mjCModel_, private mjSpec { std::string_view name = ""); // save/restore the current state - template void SaveState(const T* qpos, const T* qvel, const T* act); - template void RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act); + template void SaveState(const T* qpos, const T* qvel, const T* act, const T* ctrl); + template void RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act, T* ctrl); // clear existing data void MakeData(const mjModel* m, mjData** dest); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 0afb39e7..12f55c1f 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -5394,6 +5394,7 @@ mjCActuator& mjCActuator::operator=(const mjCActuator& other) { void mjCActuator::ForgetKeyframes() { act_.clear(); + ctrl_.clear(); } @@ -5413,6 +5414,15 @@ std::vector& mjCActuator::act() { +mjtNum& mjCActuator::ctrl() { + if (ctrl_.find(model->state_name_) == ctrl_.end()) { + ctrl_[model->state_name_] = mjNAN; + } + return ctrl_.at(model->state_name_); +} + + + void mjCActuator::PointToLocal() { spec.element = static_cast(this); spec.name = &name; diff --git a/src/user/user_objects.h b/src/user/user_objects.h index bd39c7ef..62401489 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -1397,6 +1397,7 @@ class mjCActuator_ : public mjCBase { int actadr_; // address of dof in data->act int actdim_; // number of dofs in data->act std::map> act_; // act at the previous step + std::map ctrl_; // ctrl at the previous step // variable-size data std::string plugin_name; @@ -1436,6 +1437,7 @@ class mjCActuator : public mjCActuator_, private mjsActuator { bool is_actlimited() const; std::vector& act(); + mjtNum& ctrl(); private: void Compile(void); // compiler diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 75280243..9024be04 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -430,8 +430,8 @@ static constexpr char xml_child[] = R"( - - + + )"; @@ -501,10 +501,10 @@ TEST_F(MujocoTest, AttachSame) { - - - - + + + + )"; @@ -621,8 +621,8 @@ TEST_F(MujocoTest, AttachDifferent) { - - + + )"; @@ -743,8 +743,8 @@ TEST_F(MujocoTest, AttachFrame) { - - + + )"; From 5927b3d4d903918fa668bbc1b013dfe8986dbe1e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 14 Aug 2024 05:46:36 -0700 Subject: [PATCH 06/24] Add time to attached keyframes. PiperOrigin-RevId: 662886533 Change-Id: If9118a226250d2e806d184c9715c7e6a1390fd95 --- src/user/user_model.cc | 2 ++ src/user/user_model.h | 1 + test/user/user_api_test.cc | 20 ++++++++++---------- 3 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index b96189be..178dbdd7 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2952,6 +2952,7 @@ void mjCModel::StoreKeyframes() { for (auto key : keys_) { mjKeyInfo info; info.name = prefix + key->name + suffix; + info.time = key->spec.time; info.qpos = !key->spec_qpos_.empty(); info.qvel = !key->spec_qvel_.empty(); info.act = !key->spec_act_.empty(); @@ -3529,6 +3530,7 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { for (const auto& info : key_pending_) { mjCKey* key = (mjCKey*)FindObject(mjOBJ_KEY, info.name); key->name = info.name; + key->spec.time = info.time; if (info.qpos) key->spec_qpos_.assign(nq, 0); if (info.qvel) key->spec_qvel_.assign(nv, 0); if (info.act) key->spec_act_.assign(na, 0); diff --git a/src/user/user_model.h b/src/user/user_model.h index 8b004b79..ed7ef8cc 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -36,6 +36,7 @@ typedef std::array mjListKeyMap; typedef struct mjKeyInfo_ { std::string name; + double time; bool qpos; bool qvel; bool act; diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 9024be04..795503da 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -430,8 +430,8 @@ static constexpr char xml_child[] = R"( - - + + )"; @@ -501,10 +501,10 @@ TEST_F(MujocoTest, AttachSame) { - - - - + + + + )"; @@ -621,8 +621,8 @@ TEST_F(MujocoTest, AttachDifferent) { - - + + )"; @@ -743,8 +743,8 @@ TEST_F(MujocoTest, AttachFrame) { - - + + )"; From 4c64e6f036be93591d26b47f56cab4c05208ffd7 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 14 Aug 2024 10:41:40 -0700 Subject: [PATCH 07/24] Add mocap state to SaveState/RestoreState and attached keyframes. PiperOrigin-RevId: 662975257 Change-Id: I3edb44f34426525813033902caf31db16ad7b2ab --- doc/XMLreference.rst | 1 - doc/changelog.rst | 5 +- src/user/user_api.cc | 6 +- src/user/user_model.cc | 145 ++++++++++++++++++++++++++++--------- src/user/user_model.h | 16 ++-- src/user/user_objects.cc | 52 +++++++++---- src/user/user_objects.h | 16 +++- test/user/user_api_test.cc | 90 +++++++++++++++++++++++ 8 files changed, 264 insertions(+), 67 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 338f7b7c..645ea9e2 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -3808,7 +3808,6 @@ all attachments will appear in the saved XML file. - An entire model cannot be attached (i.e. including all elements, referenced or not). - All assets from the child model will be copied in, whether they are referenced or not. - Self-attach or circular references are not checked for and will lead to infinite loops. - - :ref:`Keyframes` are not yet supported. When attaching, all keyframes will be deleted. .. _body-attach-model: diff --git a/doc/changelog.rst b/doc/changelog.rst index e48c492b..80294d25 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,10 +7,9 @@ Upcoming version (not yet released) General ^^^^^^^ -1. Add :ref:`mjSpec` option for creating a texture from a buffer. +1. Added :ref:`mjSpec` option for creating a texture from a buffer. 2. :ref:`shellinertia ` is now supported by all geom types. -3. Add support for :ref:`attaching` keyframes. Note: this only supports keyframe containing qpos, qvel, - and act. +3. Added support for :ref:`attaching` keyframes. Version 3.2.2 (Aug 8, 2024) --------------------------- diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 7ff36d73..bf3a98c6 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -93,15 +93,17 @@ mjModel* mj_compile(mjSpec* s, const mjVFS* vfs) { // recompile spec into existing model and data while preserving the state void mj_recompile(mjSpec* s, const mjVFS* vfs, mjModel* m, mjData* d) { mjCModel* modelC = static_cast(s->element); + std::string state_name = "state"; mjtNum time = 0; if (d) { time = d->time; - modelC->SaveState(d->qpos, d->qvel, d->act, d->ctrl); + modelC->SaveState(state_name, d->qpos, d->qvel, d->act, d->ctrl, d->mocap_pos, d->mocap_quat); } modelC->Compile(vfs, &m); if (d) { modelC->MakeData(m, &d); - modelC->RestoreState(m->qpos0, d->qpos, d->qvel, d->act, d->ctrl); + modelC->RestoreState(state_name, m->qpos0, m->body_pos, m->body_quat, d->qpos, d->qvel, + d->act, d->ctrl, d->mocap_pos, d->mocap_quat); d->time = time; } } diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 178dbdd7..bfcce1fd 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -109,9 +109,6 @@ mjCModel::mjCModel() { // this class allocated the plugins plugin_owner = true; - - // default state name - state_name_ = "state"; } @@ -216,6 +213,7 @@ void mjCModel::SaveDofOffsets() { int qposadr = 0; int dofadr = 0; int actadr = 0; + int nmocap = 0; for (auto joint : joints_) { joint->qposadr_ = qposadr; @@ -233,6 +231,15 @@ void mjCModel::SaveDofOffsets() { actuator->actadr_ = actuator->actdim_ ? actadr : -1; actadr += actuator->actdim_; } + + for (mjCBody* body : bodies_) { + if (body->spec.mocap) { + body->mocapid = nmocap; + nmocap++; + } else { + body->mocapid = -1; + } + } } @@ -2863,23 +2870,36 @@ void mjCModel::CopyObjects(mjModel* m) { // save the current state template -void mjCModel::SaveState(const T* qpos, const T* qvel, const T* act, const T* ctrl) { +void mjCModel::SaveState(const std::string& state_name, const T* qpos, const T* qvel, const T* act, + const T* ctrl, const T* mpos, const T* mquat) { for (auto joint : joints_) { if (joint->qposadr_ == -1 || joint->dofadr_ == -1) { throw mjCError(NULL, "SaveState: joint %s has no address", joint->name.c_str()); } - if (qpos) mjuu_copyvec(joint->qpos(), qpos + joint->qposadr_, joint->nq()); - if (qvel) mjuu_copyvec(joint->qvel(), qvel + joint->dofadr_, joint->nv()); + if (qpos) mjuu_copyvec(joint->qpos(state_name), qpos + joint->qposadr_, joint->nq()); + if (qvel) mjuu_copyvec(joint->qvel(state_name), qvel + joint->dofadr_, joint->nv()); } for (unsigned int i=0; iactadr_ != -1 && actuator->actdim_ != -1 && act) { - actuator->act().assign(actuator->actdim_, 0); - mjuu_copyvec(actuator->act().data(), act + actuator->actadr_, actuator->actdim_); + actuator->act(state_name).assign(actuator->actdim_, 0); + mjuu_copyvec(actuator->act(state_name).data(), act + actuator->actadr_, actuator->actdim_); } if (ctrl) { - actuator->ctrl() = ctrl[i]; + actuator->ctrl(state_name) = ctrl[i]; + } + } + + for (auto body : bodies_) { + if (!body->spec.mocap) { + continue; + } + if (mpos) { + mjuu_copyvec(body->mpos(state_name), mpos + 3*body->mocapid, 3); + } + if (mquat) { + mjuu_copyvec(body->mquat(state_name), mquat + 4*body->mocapid, 4); } } } @@ -2900,42 +2920,63 @@ void mjCModel::MakeData(const mjModel* m, mjData** dest) { // restore the previous state template -void mjCModel::RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act, T* ctrl) { +void mjCModel::RestoreState(const std::string& state_name, const mjtNum* pos0, + const mjtNum* mpos0, const mjtNum* mquat0, T* qpos, + T* qvel, T* act, T* ctrl, T* mpos, T* mquat) { for (auto joint : joints_) { if (qpos) { - if (mjuu_defined(joint->qpos()[0])) { - mjuu_copyvec(qpos + joint->qposadr_, joint->qpos(), joint->nq()); + if (mjuu_defined(joint->qpos(state_name)[0])) { + mjuu_copyvec(qpos + joint->qposadr_, joint->qpos(state_name), joint->nq()); } else { mjuu_copyvec(qpos + joint->qposadr_, pos0 + joint->qposadr_, joint->nq()); } } - if (mjuu_defined(joint->qvel()[0]) && qvel) { - mjuu_copyvec(qvel + joint->dofadr_, joint->qvel(), joint->nv()); + if (mjuu_defined(joint->qvel(state_name)[0]) && qvel) { + mjuu_copyvec(qvel + joint->dofadr_, joint->qvel(state_name), joint->nv()); } } // restore act for (unsigned int i=0; iact().empty() && mjuu_defined(actuator->act()[0]) && act) { - mjuu_copyvec(act + actuator->actadr_, actuator->act().data(), actuator->actdim_); + if (!actuator->act(state_name).empty() && mjuu_defined(actuator->act(state_name)[0]) && act) { + mjuu_copyvec(act + actuator->actadr_, actuator->act(state_name).data(), actuator->actdim_); } if (ctrl) { - ctrl[i] = mjuu_defined(actuator->ctrl()) ? actuator->ctrl() : 0; + ctrl[i] = mjuu_defined(actuator->ctrl(state_name)) ? actuator->ctrl(state_name) : 0; + } + } + + for (unsigned int i=0; imocap) { + continue; + } + if (mpos) { + if (mjuu_defined(body->mpos(state_name)[0])) { + mjuu_copyvec(mpos + 3*body->mocapid, body->mpos(state_name), 3); + } else { + mjuu_copyvec(mpos + 3*body->mocapid, mpos0 + 3*i, 3); + } + } + if (mquat) { + if (mjuu_defined(body->mquat(state_name)[0])) { + mjuu_copyvec(mquat + 4*body->mocapid, body->mquat(state_name), 4); + } else { + mjuu_copyvec(mquat + 4*body->mocapid, mquat0 + 4*i, 4); + } } } } - - // force explicit instantiations -template void mjCModel::SaveState(const mjtNum* qpos, - const mjtNum* qvel, - const mjtNum* act, - const mjtNum* ctrl); +template void mjCModel::SaveState( + const std::string& name, const mjtNum* qpos, const mjtNum* qvel, const mjtNum* act, + const mjtNum* ctrl, const mjtNum* mpos, const mjtNum* mquat); -template void mjCModel::RestoreState(const mjtNum* qpos0, mjtNum* qpos, - mjtNum* qvel, mjtNum* act, mjtNum* ctrl); +template void mjCModel::RestoreState( + const std::string& name, const mjtNum* qpos0, const mjtNum* mpos0, const mjtNum* mquat0, + mjtNum* qpos, mjtNum* qvel, mjtNum* act, mjtNum* ctrl, mjtNum* mpos, mjtNum* mquat); @@ -2957,10 +2998,12 @@ void mjCModel::StoreKeyframes() { info.qvel = !key->spec_qvel_.empty(); info.act = !key->spec_act_.empty(); info.ctrl = !key->spec_ctrl_.empty(); + info.mpos = !key->spec_mpos_.empty(); + info.mquat = !key->spec_mquat_.empty(); key_pending_.push_back(info); - state_name_ = info.name; - SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(), - key->spec_act_.data(), key->spec_ctrl_.data()); + SaveState(info.name, key->spec_qpos_.data(), key->spec_qvel_.data(), + key->spec_act_.data(), key->spec_ctrl_.data(), + key->spec_mpos_.data(), key->spec_mquat_.data()); } if (resetlists) { @@ -3502,7 +3545,10 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { return; } - // resize non-pending keyframes to the new number of dofs + // store dof offsets in joints and actuators + SaveDofOffsets(); + + // resize existing keyframes to the new state, fill in missing default values for (unsigned int i = 0; i < nkey - key_pending_.size(); i++) { mjCKey* key = keys_[i]; if (!key->spec_qpos_.empty()) { @@ -3521,12 +3567,36 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { if (!key->spec_ctrl_.empty()) { key->spec_ctrl_.resize(nu); } + if (!key->spec_mpos_.empty()) { + int nmocap0 = key->spec_mpos_.size() / 3; + key->spec_mpos_.resize(3*nmocap); + for (unsigned int j = 0; j < bodies_.size(); j++) { + if (bodies_[j]->mocapid < nmocap0) { + continue; + } + int i = bodies_[j]->mocapid; + key->spec_mpos_[3*i+0] = (double)m->body_pos[3*j+0]; + key->spec_mpos_[3*i+1] = (double)m->body_pos[3*j+1]; + key->spec_mpos_[3*i+2] = (double)m->body_pos[3*j+2]; + } + } + if (!key->spec_mquat_.empty()) { + int nmocap0 = key->spec_mquat_.size() / 4; + key->spec_mquat_.resize(4*nmocap); + for (unsigned int j = 0; j < bodies_.size(); j++) { + if (bodies_[j]->mocapid < nmocap0) { + continue; + } + int i = bodies_[j]->mocapid; + key->spec_mquat_[4*i+0] = (double)m->body_quat[4*j+0]; + key->spec_mquat_[4*i+1] = (double)m->body_quat[4*j+1]; + key->spec_mquat_[4*i+2] = (double)m->body_quat[4*j+2]; + key->spec_mquat_[4*i+3] = (double)m->body_quat[4*j+3]; + } + } } - // store dof offsets in joints and actuators - SaveDofOffsets(); - - // copy state stored in joints and actuators to keyframes + // create new keyframes, fill in missing default values for (const auto& info : key_pending_) { mjCKey* key = (mjCKey*)FindObject(mjOBJ_KEY, info.name); key->name = info.name; @@ -3535,9 +3605,12 @@ void mjCModel::ResolveKeyframes(const mjModel* m) { if (info.qvel) key->spec_qvel_.assign(nv, 0); if (info.act) key->spec_act_.assign(na, 0); if (info.ctrl) key->spec_ctrl_.assign(nu, 0); - state_name_ = info.name; - RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(), - key->spec_act_.data(), key->spec_ctrl_.data()); + if (info.mpos) key->spec_mpos_.assign(3*nmocap, 0); + if (info.mquat) key->spec_mquat_.assign(4*nmocap, 0); + RestoreState(info.name, m->qpos0, m->body_pos, m->body_quat, + key->spec_qpos_.data(), key->spec_qvel_.data(), + key->spec_act_.data(), key->spec_ctrl_.data(), + key->spec_mpos_.data(), key->spec_mquat_.data()); } // the attached keyframes have been copied into the model diff --git a/src/user/user_model.h b/src/user/user_model.h index ed7ef8cc..f4ccf839 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -41,6 +41,8 @@ typedef struct mjKeyInfo_ { bool qvel; bool act; bool ctrl; + bool mpos; + bool mquat; } mjKeyInfo; class mjCModel_ : public mjsElement { @@ -284,9 +286,15 @@ class mjCModel : public mjCModel_, private mjSpec { template void DeleteMaterial(std::vector& list, std::string_view name = ""); - // save/restore the current state - template void SaveState(const T* qpos, const T* qvel, const T* act, const T* ctrl); - template void RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act, T* ctrl); + // save the current state + template + void SaveState(const std::string& state_name, const T* qpos, const T* qvel, const T* act, + const T* ctrl, const T* mpos, const T* mquat); + + // restore the previously saved state + template + void RestoreState(const std::string& state_name, const mjtNum* pos0, const mjtNum* mpos0, + const mjtNum* mquat0, T* qpos, T* qvel, T* act, T* ctrl, T* mpos, T* mquat); // clear existing data void MakeData(const mjModel* m, mjData** dest); @@ -383,7 +391,5 @@ class mjCModel : public mjCModel_, private mjSpec { mjCError errInfo; // last error info bool plugin_owner; // this class allocated the plugins std::vector key_pending_; // attached keyframes - - std::string state_name_; }; #endif // MUJOCO_SRC_USER_USER_MODEL_H_ diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 12f55c1f..3a509b15 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -1411,6 +1411,8 @@ void mjCBody::ForgetKeyframes() const { joint->qpos_.clear(); joint->qvel_.clear(); } + model->FindBody((mjCBody*)this, name)->mpos_.clear(); // this is a hack to avoid const + model->FindBody((mjCBody*)this, name)->mquat_.clear(); // this is a hack to avoid const for (auto body : bodies) { body->ForgetKeyframes(); } @@ -1418,6 +1420,24 @@ void mjCBody::ForgetKeyframes() const { +mjtNum* mjCBody::mpos(const std::string& state_name) { + if (mpos_.find(state_name) == mpos_.end()) { + mpos_[state_name] = {mjNAN, 0, 0}; + } + return mpos_.at(state_name).data(); +} + + + +mjtNum* mjCBody::mquat(const std::string& state_name) { + if (mquat_.find(state_name) == mquat_.end()) { + mquat_[state_name] = {mjNAN, 0, 0, 0}; + } + return mquat_.at(state_name).data(); +} + + + // compiler void mjCBody::Compile(void) { CopyFromSpec(); @@ -1828,20 +1848,20 @@ int mjCJoint::nv(mjtJoint joint_type) { -mjtNum* mjCJoint::qpos() { - if (qpos_.find(model->state_name_) == qpos_.end()) { - qpos_[model->state_name_] = {mjNAN, 0, 0, 0, 0, 0, 0}; +mjtNum* mjCJoint::qpos(const std::string& state_name) { + if (qpos_.find(state_name) == qpos_.end()) { + qpos_[state_name] = {mjNAN, 0, 0, 0, 0, 0, 0}; } - return qpos_.at(model->state_name_).data(); + return qpos_.at(state_name).data(); } -mjtNum* mjCJoint::qvel() { - if (qvel_.find(model->state_name_) == qvel_.end()) { - qvel_[model->state_name_] = {mjNAN, 0, 0, 0, 0, 0}; +mjtNum* mjCJoint::qvel(const std::string& state_name) { + if (qvel_.find(state_name) == qvel_.end()) { + qvel_[state_name] = {mjNAN, 0, 0, 0, 0, 0}; } - return qvel_.at(model->state_name_).data(); + return qvel_.at(state_name).data(); } @@ -5405,20 +5425,20 @@ bool mjCActuator::is_actlimited() const { return islimited(actlimited, actrange) -std::vector& mjCActuator::act() { - if (act_.find(model->state_name_) == act_.end()) { - act_[model->state_name_] = std::vector(model->nu, mjNAN); +std::vector& mjCActuator::act(const std::string& state_name) { + if (act_.find(state_name) == act_.end()) { + act_[state_name] = std::vector(model->nu, mjNAN); } - return act_.at(model->state_name_); + return act_.at(state_name); } -mjtNum& mjCActuator::ctrl() { - if (ctrl_.find(model->state_name_) == ctrl_.end()) { - ctrl_[model->state_name_] = mjNAN; +mjtNum& mjCActuator::ctrl(const std::string& state_name) { + if (ctrl_.find(state_name) == ctrl_.end()) { + ctrl_[state_name] = mjNAN; } - return ctrl_.at(model->state_name_); + return ctrl_.at(state_name); } diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 62401489..c55cdcd0 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -255,6 +255,10 @@ class mjCBody_ : public mjCBase { std::string plugin_instance_name; std::vector userdata_; std::vector spec_userdata_; + + // variables used for temporarily storing the state of the mocap bodies + std::map> mpos_; // saved mocap_pos + std::map> mquat_; // saved mocap_quat }; class mjCBody : public mjCBody_, private mjsBody { @@ -323,6 +327,10 @@ class mjCBody : public mjCBody_, private mjsBody { // reset keyframe references for allowing self-attach void ForgetKeyframes() const; + // get mocap position and quaternion + mjtNum* mpos(const std::string& state_name); + mjtNum* mquat(const std::string& state_name); + private: mjCBody(const mjCBody& other, mjCModel* _model); // copy constructor mjCBody& operator=(const mjCBody& other); // copy assignment @@ -444,8 +452,8 @@ class mjCJoint : public mjCJoint_, private mjsJoint { int nq() const { return nq(spec.type); } int nv() const { return nv(spec.type); } - mjtNum* qpos(); - mjtNum* qvel(); + mjtNum* qpos(const std::string& state_name); + mjtNum* qvel(const std::string& state_name); private: int Compile(void); // compiler; return dofnum @@ -1436,8 +1444,8 @@ class mjCActuator : public mjCActuator_, private mjsActuator { bool is_forcelimited() const; bool is_actlimited() const; - std::vector& act(); - mjtNum& ctrl(); + std::vector& act(const std::string& state_name); + mjtNum& ctrl(const std::string& state_name); private: void Compile(void); // compiler diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 795503da..8f539ee8 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -870,6 +870,8 @@ TEST_F(MujocoTest, PreserveState) { + + @@ -888,6 +890,7 @@ TEST_F(MujocoTest, PreserveState) { + @@ -915,6 +918,14 @@ TEST_F(MujocoTest, PreserveState) { data->ctrl[1] = 2; d_expected->ctrl[0] = 2; + // set mocap + data->mocap_pos[3] = 1; + data->mocap_quat[4] = 0; + data->mocap_quat[5] = 1; + d_expected->mocap_pos[0] = 1; + d_expected->mocap_quat[0] = 0; + d_expected->mocap_quat[1] = 1; + // step models mj_step(model, data); mj_step(m_expected, d_expected); @@ -925,6 +936,11 @@ TEST_F(MujocoTest, PreserveState) { EXPECT_THAT(body, NotNull()); EXPECT_THAT(mjs_detachBody(spec, body), 0); + // detach mocap + mjsBody* mocap_body = mjs_findBody(spec, "mocap_detach"); + EXPECT_THAT(mocap_body, NotNull()); + EXPECT_THAT(mjs_detachBody(spec, mocap_body), 0); + // add body mjsBody* newbody = mjs_addBody(mjs_findBody(spec, "world"), 0); EXPECT_THAT(newbody, NotNull()); @@ -968,6 +984,17 @@ TEST_F(MujocoTest, PreserveState) { EXPECT_EQ(data->act[i], d_expected->act[i]) << i; } + // compare mocap + EXPECT_EQ(model->nmocap, m_expected->nmocap); + for (int i = 0; i < model->nmocap; ++i) { + for (int j = 0; j < 3; ++j) { + EXPECT_EQ(data->mocap_pos[3*i+j], d_expected->mocap_pos[3*i+j]) << i; + } + for (int j = 0; j < 4; ++j) { + EXPECT_EQ(data->mocap_quat[4*i+j], d_expected->mocap_quat[4*i+j]) << i; + } + } + // check that the function is callable with no data mj_deleteData(data); mj_recompile(spec, 0, model, nullptr); @@ -979,5 +1006,68 @@ TEST_F(MujocoTest, PreserveState) { mj_deleteModel(m_expected); } +TEST_F(MujocoTest, AttachMocap) { + std::array er; + mjtNum tol = 0; + std::string field = ""; + + static constexpr char xml[] = R"( + + + + + + + + )"; + + static constexpr char xml_expected[] = R"( + + + + + + + + + + )"; + + mjSpec* spec = mj_parseXMLString(xml, 0, er.data(), er.size()); + EXPECT_THAT(spec, NotNull()) << er.data(); + + mjsBody* body = mjs_findBody(spec, "mocap"); + EXPECT_THAT(body, NotNull()); + + mjsBody* world = mjs_findBody(spec, "world"); + EXPECT_THAT(world, NotNull()); + + mjsFrame* frame = mjs_addFrame(world, NULL); + mjs_attachBody(frame, body, "attached-", "-1"); + + mjsBody* attached_body = mjs_findBody(spec, "attached-mocap-1"); + EXPECT_THAT(attached_body, NotNull()); + attached_body->pos[0] = 3; + attached_body->pos[1] = 3; + attached_body->pos[2] = 3; + attached_body->quat[0] = 0; + attached_body->quat[1] = 0; + attached_body->quat[2] = 1; + attached_body->quat[3] = 0; + + mjModel* model = mj_compile(spec, 0); + EXPECT_THAT(model, NotNull()); + + mjModel* m_expected = LoadModelFromString(xml_expected, er.data(), er.size()); + EXPECT_THAT(m_expected, NotNull()) << er.data(); + EXPECT_LE(CompareModel(model, m_expected, field), tol) + << "Expected and attached models are different!\n" + << "Different field: " << field << '\n'; + + mj_deleteSpec(spec); + mj_deleteModel(model); + mj_deleteModel(m_expected); +} + } // namespace } // namespace mujoco From d932faa1d45a0736cc3edcea0f0fa887746d75f9 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Wed, 14 Aug 2024 13:48:20 -0700 Subject: [PATCH 08/24] Flip order of cases in user_objects.cc (readability) PiperOrigin-RevId: 663040865 Change-Id: I27d06a6a769d5bfc41189d270021b4fc1d4b5932 --- src/user/user_objects.cc | 94 ++++++++++++++++++++-------------------- 1 file changed, 47 insertions(+), 47 deletions(-) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 3a509b15..1c177d5a 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -2128,15 +2128,15 @@ double mjCGeom::GetVolume() const { return mesh->GetVolumeRef(typeinertia); } - // compute from geom shape (type) and inertia (typeinertia) + // compute from geom shape (type) and inertia type (typeinertia) switch (type) { case mjGEOM_SPHERE: { double radius = size[0]; switch (typeinertia) { - case mjINERTIA_SHELL: - return 4 * mjPI * radius * radius; case mjINERTIA_VOLUME: return 4 * mjPI * radius * radius * radius / 3; + case mjINERTIA_SHELL: + return 4 * mjPI * radius * radius; } break; } @@ -2144,10 +2144,10 @@ double mjCGeom::GetVolume() const { double height = 2 * size[1]; double radius = size[0]; switch (typeinertia) { - case mjINERTIA_SHELL: - return 4 * mjPI * radius * radius + 2 * mjPI * radius * height; case mjINERTIA_VOLUME: return mjPI * (radius * radius * height + 4 * radius * radius * radius / 3); + case mjINERTIA_SHELL: + return 4 * mjPI * radius * radius + 2 * mjPI * radius * height; } break; } @@ -2155,15 +2155,17 @@ double mjCGeom::GetVolume() const { double height = 2 * size[1]; double radius = size[0]; switch (typeinertia) { - case mjINERTIA_SHELL: - return 2 * mjPI * radius * radius + 2 * mjPI * radius * height; case mjINERTIA_VOLUME: return mjPI * radius * radius * height; + case mjINERTIA_SHELL: + return 2 * mjPI * radius * radius + 2 * mjPI * radius * height; } break; } case mjGEOM_ELLIPSOID: { switch (typeinertia) { + case mjINERTIA_VOLUME: + return 4 * mjPI * size[0] * size[1] * size[2] / 3; case mjINERTIA_SHELL: { // Thomsen approximation // https://www.numericana.com/answer/ellipsoid.htm#thomsen @@ -2173,18 +2175,16 @@ double mjCGeom::GetVolume() const { std::pow(size[2] * size[0], p); return 4 * mjPI * std::pow(tmp / 3, 1 / p); } - case mjINERTIA_VOLUME: - return 4 * mjPI * size[0] * size[1] * size[2] / 3; } break; } case mjGEOM_HFIELD: case mjGEOM_BOX: { switch (typeinertia) { - case mjINERTIA_SHELL: - return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]); case mjINERTIA_VOLUME: return size[0] * size[1] * size[2] * 8; + case mjINERTIA_SHELL: + return 8 * (size[0] * size[1] + size[1] * size[2] + size[2] * size[0]); } break; } @@ -2223,16 +2223,16 @@ void mjCGeom::SetInertia(void) { return; } - // compute from geom shape (type) and inertia (typeinertia) + // compute from geom shape (type) and inertia type (typeinertia) switch (type) { case mjGEOM_SPHERE: { switch (typeinertia) { - case mjINERTIA_SHELL: - inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3; - return; case mjINERTIA_VOLUME: inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 5; return; + case mjINERTIA_SHELL: + inertia[0] = inertia[1] = inertia[2] = 2 * mass_ * size[0] * size[0] / 3; + return; } break; } @@ -2241,6 +2241,22 @@ void mjCGeom::SetInertia(void) { double height = 2 * size[1]; double radius = size[0]; switch (typeinertia) { + case mjINERTIA_VOLUME: { + double sphere_mass = + mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol) + double cylinder_mass = mass_ - sphere_mass; + + // cylinder part + inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12; + inertia[2] = cylinder_mass * radius * radius / 2; + + // add two hemispheres, displace along third axis + double sphere_inertia = 2 * sphere_mass * radius * radius / 5; + inertia[0] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8; + inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8; + inertia[2] += sphere_inertia; + return; + } case mjINERTIA_SHELL: { // surface area double Asphere = 4 * mjPI * radius * radius; @@ -2264,22 +2280,6 @@ void mjCGeom::SetInertia(void) { inertia[2] += sphere_inertia; return; } - case mjINERTIA_VOLUME: { - double sphere_mass = - mass_ * 4 * radius / (4 * radius + 3 * height); // mass*(sphere_vol/total_vol) - double cylinder_mass = mass_ - sphere_mass; - - // cylinder part - inertia[0] = inertia[1] = cylinder_mass * (3 * radius * radius + height * height) / 12; - inertia[2] = cylinder_mass * radius * radius / 2; - - // add two hemispheres, displace along third axis - double sphere_inertia = 2 * sphere_mass * radius * radius / 5; - inertia[0] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8; - inertia[1] += sphere_inertia + sphere_mass * height * (3 * radius + 2 * height) / 8; - inertia[2] += sphere_inertia; - return; - } break; } break; @@ -2289,6 +2289,10 @@ void mjCGeom::SetInertia(void) { double height = 2 * halfheight; double radius = size[0]; switch (typeinertia) { + case mjINERTIA_VOLUME: + inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12; + inertia[2] = mass_ * radius * radius / 2; + return; case mjINERTIA_SHELL: { // surface area double Adisk = mjPI * radius * radius; @@ -2314,10 +2318,6 @@ void mjCGeom::SetInertia(void) { inertia[2] += 2 * inertia_disk_z; return; } - case mjINERTIA_VOLUME: - inertia[0] = inertia[1] = mass_ * (3 * radius * radius + height * height) / 12; - inertia[2] = mass_ * radius * radius / 2; - return; } break; } @@ -2326,6 +2326,12 @@ void mjCGeom::SetInertia(void) { double s11 = size[1] * size[1]; double s22 = size[2] * size[2]; switch (typeinertia) { + case mjINERTIA_VOLUME: { + inertia[0] = mass_ * (s11 + s22) / 5; + inertia[1] = mass_ * (s00 + s22) / 5; + inertia[2] = mass_ * (s00 + s11) / 5; + return; + } case mjINERTIA_SHELL: { // approximate shell inertia by subtracting ellipsoid from expanded ellipsoid double eps = 1e-6; @@ -2361,12 +2367,6 @@ void mjCGeom::SetInertia(void) { inertia[2] = inertia_b[2] - inertia_a[2]; return; } - case mjINERTIA_VOLUME: { - inertia[0] = mass_ * (s11 + s22) / 5; - inertia[1] = mass_ * (s00 + s22) / 5; - inertia[2] = mass_ * (s00 + s11) / 5; - return; - } } break; } @@ -2376,6 +2376,12 @@ void mjCGeom::SetInertia(void) { double s11 = size[1] * size[1]; double s22 = size[2] * size[2]; switch (typeinertia) { + case mjINERTIA_VOLUME: { + inertia[0] = mass_ * (s11 + s22) / 3; + inertia[1] = mass_ * (s00 + s22) / 3; + inertia[2] = mass_ * (s00 + s11) / 3; + return; + } case mjINERTIA_SHELL: { // length double lx = 2 * size[0]; // side 0 @@ -2412,12 +2418,6 @@ void mjCGeom::SetInertia(void) { inertia[2] = 2 * (mass1 * s00 + mass2 * s11 + Iz0 + Iz1 + Iz2); return; } - case mjINERTIA_VOLUME: { - inertia[0] = mass_ * (s11 + s22) / 3; - inertia[1] = mass_ * (s00 + s22) / 3; - inertia[2] = mass_ * (s00 + s11) / 3; - return; - } break; } break; From 1f5f9a8a676330bdbc0ebe340867d0268b321cd1 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 15 Aug 2024 02:40:23 -0700 Subject: [PATCH 09/24] Improve variable naming in xml_native_reader.cc PiperOrigin-RevId: 663236609 Change-Id: I0a3af8432054172a3fe687867706eeef14a2529d --- src/xml/xml_native_reader.cc | 1211 +++++++++++++++++----------------- 1 file changed, 605 insertions(+), 606 deletions(-) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 83484035..433e33a7 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -1319,115 +1319,115 @@ void mjXReader::Statistic(XMLElement* section) { //---------------------------------- one-element parsers ------------------------------------------- // flex element parser -void mjXReader::OneFlex(XMLElement* elem, mjsFlex* pflex) { +void mjXReader::OneFlex(XMLElement* elem, mjsFlex* flex) { string text, name, material; int n; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pflex->name, name.c_str()); + mjs_setString(flex->name, name.c_str()); } if (ReadAttrTxt(elem, "material", material)) { - mjs_setString(pflex->material, material.c_str()); + mjs_setString(flex->material, material.c_str()); } - ReadAttr(elem, "radius", 1, &pflex->radius, text); - ReadAttr(elem, "rgba", 4, pflex->rgba, text); + ReadAttr(elem, "radius", 1, &flex->radius, text); + ReadAttr(elem, "rgba", 4, flex->rgba, text); if (MapValue(elem, "flatskin", &n, bool_map, 2)) { - pflex->flatskin = (n==1); + flex->flatskin = (n==1); } - ReadAttrInt(elem, "dim", &pflex->dim); - ReadAttrInt(elem, "group", &pflex->group); + ReadAttrInt(elem, "dim", &flex->dim); + ReadAttrInt(elem, "group", &flex->group); // read data vectors if (ReadAttrTxt(elem, "body", text, true)) { - mjs_setStringVec(pflex->vertbody, text.c_str()); + mjs_setStringVec(flex->vertbody, text.c_str()); } auto vert = ReadAttrVec(elem, "vertex"); if (vert.has_value()) { - mjs_setDouble(pflex->vert, vert->data(), vert->size()); + mjs_setDouble(flex->vert, vert->data(), vert->size()); } auto element = ReadAttrVec(elem, "element", true); if (element.has_value()) { - mjs_setInt(pflex->elem, element->data(), element->size()); + mjs_setInt(flex->elem, element->data(), element->size()); } auto texcoord = ReadAttrVec(elem, "texcoord"); if (texcoord.has_value()) { - mjs_setFloat(pflex->texcoord, texcoord->data(), texcoord->size()); + mjs_setFloat(flex->texcoord, texcoord->data(), texcoord->size()); } // contact subelement XMLElement* cont = FirstChildElement(elem, "contact"); if (cont) { - ReadAttrInt(cont, "contype", &pflex->contype); - ReadAttrInt(cont, "conaffinity", &pflex->conaffinity); - ReadAttrInt(cont, "condim", &pflex->condim); - ReadAttrInt(cont, "priority", &pflex->priority); - ReadAttr(cont, "friction", 3, pflex->friction, text, false, false); - ReadAttr(cont, "solmix", 1, &pflex->solmix, text); - ReadAttr(cont, "solref", mjNREF, pflex->solref, text, false, false); - ReadAttr(cont, "solimp", mjNIMP, pflex->solimp, text, false, false); - ReadAttr(cont, "margin", 1, &pflex->margin, text); - ReadAttr(cont, "gap", 1, &pflex->gap, text); + ReadAttrInt(cont, "contype", &flex->contype); + ReadAttrInt(cont, "conaffinity", &flex->conaffinity); + ReadAttrInt(cont, "condim", &flex->condim); + ReadAttrInt(cont, "priority", &flex->priority); + ReadAttr(cont, "friction", 3, flex->friction, text, false, false); + ReadAttr(cont, "solmix", 1, &flex->solmix, text); + ReadAttr(cont, "solref", mjNREF, flex->solref, text, false, false); + ReadAttr(cont, "solimp", mjNIMP, flex->solimp, text, false, false); + ReadAttr(cont, "margin", 1, &flex->margin, text); + ReadAttr(cont, "gap", 1, &flex->gap, text); if (MapValue(cont, "internal", &n, bool_map, 2)) { - pflex->internal = (n==1); + flex->internal = (n==1); } - MapValue(cont, "selfcollide", &pflex->selfcollide, flexself_map, 5); - ReadAttrInt(cont, "activelayers", &pflex->activelayers); + MapValue(cont, "selfcollide", &flex->selfcollide, flexself_map, 5); + ReadAttrInt(cont, "activelayers", &flex->activelayers); } // edge subelement XMLElement* edge = FirstChildElement(elem, "edge"); if (edge) { - ReadAttr(edge, "stiffness", 1, &pflex->edgestiffness, text); - ReadAttr(edge, "damping", 1, &pflex->edgedamping, text); + ReadAttr(edge, "stiffness", 1, &flex->edgestiffness, text); + ReadAttr(edge, "damping", 1, &flex->edgedamping, text); } // write error info - mjs_setString(pflex->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(flex->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // mesh element parser -void mjXReader::OneMesh(XMLElement* elem, mjsMesh* pmesh, const mjVFS* vfs) { +void mjXReader::OneMesh(XMLElement* elem, mjsMesh* mesh, const mjVFS* vfs) { int n; string text, name, content_type; // read attributes if (ReadAttrTxt(elem, "name", name)) { - *pmesh->name = name; + *mesh->name = name; } if (ReadAttrTxt(elem, "content_type", content_type)) { - *pmesh->content_type = content_type; + *mesh->content_type = content_type; } auto file = ReadAttrFile(elem, "file", vfs, MeshDir()); if (file) { - mjs_setString(pmesh->file, file->c_str()); + mjs_setString(mesh->file, file->c_str()); } - ReadAttr(elem, "refpos", 3, pmesh->refpos, text); - ReadAttr(elem, "refquat", 4, pmesh->refquat, text); - ReadAttr(elem, "scale", 3, pmesh->scale, text); + ReadAttr(elem, "refpos", 3, mesh->refpos, text); + ReadAttr(elem, "refquat", 4, mesh->refquat, text); + ReadAttr(elem, "scale", 3, mesh->scale, text); XMLElement* eplugin = FirstChildElement(elem, "plugin"); if (eplugin) { - OnePlugin(eplugin, &pmesh->plugin); + OnePlugin(eplugin, &mesh->plugin); } if (MapValue(elem, "smoothnormal", &n, bool_map, 2)) { - pmesh->smoothnormal = (n==1); + mesh->smoothnormal = (n==1); } if (ReadAttrInt(elem, "maxhullvert", &n)) { if (n != 0 && n < 4) throw mjXError(elem, "maxhullvert must be larger than 3"); - pmesh->maxhullvert = n; + mesh->maxhullvert = n; } // read user vertex data if (ReadAttrTxt(elem, "vertex", text)) { auto uservert = ReadAttrVec(elem, "vertex"); if (uservert.has_value()) { - mjs_setFloat(pmesh->uservert, uservert->data(), uservert->size()); + mjs_setFloat(mesh->uservert, uservert->data(), uservert->size()); } } @@ -1435,7 +1435,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* pmesh, const mjVFS* vfs) { if (ReadAttrTxt(elem, "normal", text)) { auto usernormal = ReadAttrVec(elem, "normal"); if (usernormal.has_value()) { - mjs_setFloat(pmesh->usernormal, usernormal->data(), usernormal->size()); + mjs_setFloat(mesh->usernormal, usernormal->data(), usernormal->size()); } } @@ -1443,7 +1443,7 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* pmesh, const mjVFS* vfs) { if (ReadAttrTxt(elem, "texcoord", text)) { auto usertexcoord = ReadAttrVec(elem, "texcoord"); if (usertexcoord.has_value()) { - mjs_setFloat(pmesh->usertexcoord, usertexcoord->data(), usertexcoord->size()); + mjs_setFloat(mesh->usertexcoord, usertexcoord->data(), usertexcoord->size()); } } @@ -1451,55 +1451,55 @@ void mjXReader::OneMesh(XMLElement* elem, mjsMesh* pmesh, const mjVFS* vfs) { if (ReadAttrTxt(elem, "face", text)) { auto userface = ReadAttrVec(elem, "face"); if (userface.has_value()) { - mjs_setInt(pmesh->userface, userface->data(), userface->size()); + mjs_setInt(mesh->userface, userface->data(), userface->size()); } } // write error info - mjs_setString(pmesh->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(mesh->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // skin element parser -void mjXReader::OneSkin(XMLElement* elem, mjsSkin* pskin, const mjVFS* vfs) { +void mjXReader::OneSkin(XMLElement* elem, mjsSkin* skin, const mjVFS* vfs) { string text, name, material; float data[4]; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pskin->name, name.c_str()); + mjs_setString(skin->name, name.c_str()); } auto file = ReadAttrFile(elem, "file", vfs, AssetDir()); if (file.has_value()) { - mjs_setString(pskin->file, file->c_str()); + mjs_setString(skin->file, file->c_str()); } if (ReadAttrTxt(elem, "material", material)) { - mjs_setString(pskin->material, material.c_str()); + mjs_setString(skin->material, material.c_str()); } - ReadAttrInt(elem, "group", &pskin->group); - if (pskin->group<0 || pskin->group>=mjNGROUP) { + ReadAttrInt(elem, "group", &skin->group); + if (skin->group<0 || skin->group>=mjNGROUP) { throw mjXError(elem, "skin group must be between 0 and 5"); } - ReadAttr(elem, "rgba", 4, pskin->rgba, text); - ReadAttr(elem, "inflate", 1, &pskin->inflate, text); + ReadAttr(elem, "rgba", 4, skin->rgba, text); + ReadAttr(elem, "inflate", 1, &skin->inflate, text); // read vertex data auto vertex = ReadAttrVec(elem, "vertex"); if (vertex.has_value()) { - mjs_setFloat(pskin->vert, vertex->data(), vertex->size()); + mjs_setFloat(skin->vert, vertex->data(), vertex->size()); } // read texcoord data auto texcoord = ReadAttrVec(elem, "texcoord"); if (texcoord.has_value()) { - mjs_setFloat(pskin->texcoord, texcoord->data(), texcoord->size()); + mjs_setFloat(skin->texcoord, texcoord->data(), texcoord->size()); } // read user face data auto face = ReadAttrVec(elem, "face"); if (face.has_value()) { - mjs_setInt(pskin->face, face->data(), face->size()); + mjs_setInt(skin->face, face->data(), face->size()); } // read bones @@ -1510,7 +1510,7 @@ void mjXReader::OneSkin(XMLElement* elem, mjsSkin* pskin, const mjVFS* vfs) { while (bone) { // read body ReadAttrTxt(bone, "body", text, true); - mjs_appendString(pskin->bodyname, text.c_str()); + mjs_appendString(skin->bodyname, text.c_str()); // read bindpos ReadAttr(bone, "bindpos", 3, data, text, true); @@ -1528,13 +1528,13 @@ void mjXReader::OneSkin(XMLElement* elem, mjsSkin* pskin, const mjVFS* vfs) { // read vertid auto tempid = ReadAttrVec(bone, "vertid", true); if (tempid.has_value()) { - mjs_appendIntVec(pskin->vertid, tempid->data(), tempid->size()); + mjs_appendIntVec(skin->vertid, tempid->data(), tempid->size()); } // read vertweight auto tempweight = ReadAttrVec(bone, "vertweight", true); if (tempweight.has_value()) { - mjs_appendFloatVec(pskin->vertweight, tempweight->data(), tempweight->size()); + mjs_appendFloatVec(skin->vertweight, tempweight->data(), tempweight->size()); } // advance to next bone @@ -1542,28 +1542,28 @@ void mjXReader::OneSkin(XMLElement* elem, mjsSkin* pskin, const mjVFS* vfs) { } // set bind vectors - mjs_setFloat(pskin->bindpos, bindpos.data(), bindpos.size()); - mjs_setFloat(pskin->bindquat, bindquat.data(), bindquat.size()); + mjs_setFloat(skin->bindpos, bindpos.data(), bindpos.size()); + mjs_setFloat(skin->bindquat, bindquat.data(), bindquat.size()); // write error info - mjs_setString(pskin->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(skin->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // material element parser -void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* pmat) { +void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* material) { string text, name, texture; int n; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pmat->name, name.c_str()); + mjs_setString(material->name, name.c_str()); } bool tex_attributes_found = false; if (ReadAttrTxt(elem, "texture", texture)) { - mjs_setInStringVec(pmat->textures, mjTEXROLE_RGB, texture.c_str()); + mjs_setInStringVec(material->textures, mjTEXROLE_RGB, texture.c_str()); tex_attributes_found = true; } @@ -1576,77 +1576,77 @@ void mjXReader::OneMaterial(XMLElement* elem, mjsMaterial* pmat) { int role = FindKey(texrole_map, texrole_sz, tex_elem->Name()); string texmat; ReadAttrTxt(tex_elem, "texture", texmat, true); - mjs_setInStringVec(pmat->textures, role, texmat.c_str()); + mjs_setInStringVec(material->textures, role, texmat.c_str()); tex_elem = NextSiblingElement(tex_elem); } if (MapValue(elem, "texuniform", &n, bool_map, 2)) { - pmat->texuniform = (n==1); + material->texuniform = (n==1); } - ReadAttr(elem, "texrepeat", 2, pmat->texrepeat, text); - ReadAttr(elem, "emission", 1, &pmat->emission, text); - ReadAttr(elem, "specular", 1, &pmat->specular, text); - ReadAttr(elem, "shininess", 1, &pmat->shininess, text); - ReadAttr(elem, "reflectance", 1, &pmat->reflectance, text); - ReadAttr(elem, "metallic", 1, &pmat->metallic, text); - ReadAttr(elem, "roughness", 1, &pmat->roughness, text); - ReadAttr(elem, "rgba", 4, pmat->rgba, text); + ReadAttr(elem, "texrepeat", 2, material->texrepeat, text); + ReadAttr(elem, "emission", 1, &material->emission, text); + ReadAttr(elem, "specular", 1, &material->specular, text); + ReadAttr(elem, "shininess", 1, &material->shininess, text); + ReadAttr(elem, "reflectance", 1, &material->reflectance, text); + ReadAttr(elem, "metallic", 1, &material->metallic, text); + ReadAttr(elem, "roughness", 1, &material->roughness, text); + ReadAttr(elem, "rgba", 4, material->rgba, text); // write error info - mjs_setString(pmat->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(material->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // joint element parser -void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) { +void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) { string text, name; std::vector userdata; int n; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pjoint->name, name.c_str()); + mjs_setString(joint->name, name.c_str()); } if (MapValue(elem, "type", &n, joint_map, joint_sz)) { - pjoint->type = (mjtJoint)n; + joint->type = (mjtJoint)n; } - MapValue(elem, "limited", &pjoint->limited, TFAuto_map, 3); - MapValue(elem, "actuatorfrclimited", &pjoint->actfrclimited, TFAuto_map, 3); - ReadAttrInt(elem, "group", &pjoint->group); - ReadAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, text, false, false); - ReadAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, text, false, false); - ReadAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, text, false, false); - ReadAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, text, false, false); - ReadAttr(elem, "pos", 3, pjoint->pos, text); - ReadAttr(elem, "axis", 3, pjoint->axis, text); - ReadAttr(elem, "springdamper", 2, pjoint->springdamper, text); - ReadAttr(elem, "stiffness", 1, &pjoint->stiffness, text); - ReadAttr(elem, "range", 2, pjoint->range, text); - ReadAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, text); - ReadAttr(elem, "margin", 1, &pjoint->margin, text); - ReadAttr(elem, "ref", 1, &pjoint->ref, text); - ReadAttr(elem, "springref", 1, &pjoint->springref, text); - ReadAttr(elem, "armature", 1, &pjoint->armature, text); - ReadAttr(elem, "damping", 1, &pjoint->damping, text); - ReadAttr(elem, "frictionloss", 1, &pjoint->frictionloss, text); + MapValue(elem, "limited", &joint->limited, TFAuto_map, 3); + MapValue(elem, "actuatorfrclimited", &joint->actfrclimited, TFAuto_map, 3); + ReadAttrInt(elem, "group", &joint->group); + ReadAttr(elem, "solreflimit", mjNREF, joint->solref_limit, text, false, false); + ReadAttr(elem, "solimplimit", mjNIMP, joint->solimp_limit, text, false, false); + ReadAttr(elem, "solreffriction", mjNREF, joint->solref_friction, text, false, false); + ReadAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, text, false, false); + ReadAttr(elem, "pos", 3, joint->pos, text); + ReadAttr(elem, "axis", 3, joint->axis, text); + ReadAttr(elem, "springdamper", 2, joint->springdamper, text); + ReadAttr(elem, "stiffness", 1, &joint->stiffness, text); + ReadAttr(elem, "range", 2, joint->range, text); + ReadAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, text); + ReadAttr(elem, "margin", 1, &joint->margin, text); + ReadAttr(elem, "ref", 1, &joint->ref, text); + ReadAttr(elem, "springref", 1, &joint->springref, text); + ReadAttr(elem, "armature", 1, &joint->armature, text); + ReadAttr(elem, "damping", 1, &joint->damping, text); + ReadAttr(elem, "frictionloss", 1, &joint->frictionloss, text); if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) { - pjoint->actgravcomp = (n==1); + joint->actgravcomp = (n==1); } // read userdata if (ReadVector(elem, "user", userdata, text)) { - mjs_setDouble(pjoint->userdata, userdata.data(), userdata.size()); + mjs_setDouble(joint->userdata, userdata.data(), userdata.size()); } // write error info - mjs_setString(pjoint->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(joint->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // geom element parser -void mjXReader::OneGeom(XMLElement* elem, mjsGeom* pgeom) { +void mjXReader::OneGeom(XMLElement* elem, mjsGeom* geom) { string text, name; std::vector userdata; std::string hfieldname, meshname, material; @@ -1654,65 +1654,65 @@ void mjXReader::OneGeom(XMLElement* elem, mjsGeom* pgeom) { // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pgeom->name, name.c_str()); + mjs_setString(geom->name, name.c_str()); } if (MapValue(elem, "type", &n, geom_map, mjNGEOMTYPES)) { - pgeom->type = (mjtGeom)n; + geom->type = (mjtGeom)n; } - ReadAttr(elem, "size", 3, pgeom->size, text, false, false); - ReadAttrInt(elem, "contype", &pgeom->contype); - ReadAttrInt(elem, "conaffinity", &pgeom->conaffinity); - ReadAttrInt(elem, "condim", &pgeom->condim); - ReadAttrInt(elem, "group", &pgeom->group); - ReadAttrInt(elem, "priority", &pgeom->priority); - ReadAttr(elem, "friction", 3, pgeom->friction, text, false, false); - ReadAttr(elem, "solmix", 1, &pgeom->solmix, text); - ReadAttr(elem, "solref", mjNREF, pgeom->solref, text, false, false); - ReadAttr(elem, "solimp", mjNIMP, pgeom->solimp, text, false, false); - ReadAttr(elem, "margin", 1, &pgeom->margin, text); - ReadAttr(elem, "gap", 1, &pgeom->gap, text); + ReadAttr(elem, "size", 3, geom->size, text, false, false); + ReadAttrInt(elem, "contype", &geom->contype); + ReadAttrInt(elem, "conaffinity", &geom->conaffinity); + ReadAttrInt(elem, "condim", &geom->condim); + ReadAttrInt(elem, "group", &geom->group); + ReadAttrInt(elem, "priority", &geom->priority); + ReadAttr(elem, "friction", 3, geom->friction, text, false, false); + ReadAttr(elem, "solmix", 1, &geom->solmix, text); + ReadAttr(elem, "solref", mjNREF, geom->solref, text, false, false); + ReadAttr(elem, "solimp", mjNIMP, geom->solimp, text, false, false); + ReadAttr(elem, "margin", 1, &geom->margin, text); + ReadAttr(elem, "gap", 1, &geom->gap, text); if (ReadAttrTxt(elem, "hfield", hfieldname)) { - mjs_setString(pgeom->hfieldname, hfieldname.c_str()); + mjs_setString(geom->hfieldname, hfieldname.c_str()); } if (ReadAttrTxt(elem, "mesh", meshname)) { - mjs_setString(pgeom->meshname, meshname.c_str()); + mjs_setString(geom->meshname, meshname.c_str()); } - ReadAttr(elem, "fitscale", 1, &pgeom->fitscale, text); + ReadAttr(elem, "fitscale", 1, &geom->fitscale, text); if (ReadAttrTxt(elem, "material", material)) { - mjs_setString(pgeom->material, material.c_str()); + mjs_setString(geom->material, material.c_str()); } - ReadAttr(elem, "rgba", 4, pgeom->rgba, text); + ReadAttr(elem, "rgba", 4, geom->rgba, text); if (MapValue(elem, "fluidshape", &n, fluid_map, 2)) { - pgeom->fluid_ellipsoid = (n == 1); + geom->fluid_ellipsoid = (n == 1); } - ReadAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, text, false, false); + ReadAttr(elem, "fluidcoef", 5, geom->fluid_coefs, text, false, false); // read userdata if (ReadVector(elem, "user", userdata, text)) { - mjs_setDouble(pgeom->userdata, userdata.data(), userdata.size()); + mjs_setDouble(geom->userdata, userdata.data(), userdata.size()); } // plugin sub-element XMLElement* eplugin = FirstChildElement(elem, "plugin"); if (eplugin) { - OnePlugin(eplugin, &pgeom->plugin); + OnePlugin(eplugin, &geom->plugin); } // remaining attributes - ReadAttr(elem, "mass", 1, &pgeom->mass, text); - ReadAttr(elem, "density", 1, &pgeom->density, text); - ReadAttr(elem, "fromto", 6, pgeom->fromto, text); - ReadAttr(elem, "pos", 3, pgeom->pos, text); - ReadQuat(elem, "quat", pgeom->quat, text); - ReadAlternative(elem, pgeom->alt); + ReadAttr(elem, "mass", 1, &geom->mass, text); + ReadAttr(elem, "density", 1, &geom->density, text); + ReadAttr(elem, "fromto", 6, geom->fromto, text); + ReadAttr(elem, "pos", 3, geom->pos, text); + ReadQuat(elem, "quat", geom->quat, text); + ReadAlternative(elem, geom->alt); // compute inertia using either solid or shell geometry if (MapValue(elem, "shellinertia", &n, meshtype_map, 2)) { - pgeom->typeinertia = (mjtGeomInertia)n; + geom->typeinertia = (mjtGeomInertia)n; } // write error info - mjs_setString(pgeom->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(geom->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } @@ -1752,41 +1752,41 @@ void mjXReader::OneSite(XMLElement* elem, mjsSite* site) { // camera element parser -void mjXReader::OneCamera(XMLElement* elem, mjsCamera* pcam) { +void mjXReader::OneCamera(XMLElement* elem, mjsCamera* camera) { int n; string text, name, targetbody; std::vector userdata; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pcam->name, name.c_str()); + mjs_setString(camera->name, name.c_str()); } if (ReadAttrTxt(elem, "target", targetbody)) { - mjs_setString(pcam->targetbody, targetbody.c_str()); + mjs_setString(camera->targetbody, targetbody.c_str()); } if (MapValue(elem, "mode", &n, camlight_map, camlight_sz)) { - pcam->mode = (mjtCamLight)n; + camera->mode = (mjtCamLight)n; } - ReadAttr(elem, "pos", 3, pcam->pos, text); - ReadQuat(elem, "quat", pcam->quat, text); - ReadAlternative(elem, pcam->alt); - ReadAttr(elem, "ipd", 1, &pcam->ipd, text); + ReadAttr(elem, "pos", 3, camera->pos, text); + ReadQuat(elem, "quat", camera->quat, text); + ReadAlternative(elem, camera->alt); + ReadAttr(elem, "ipd", 1, &camera->ipd, text); if (MapValue(elem, "orthographic", &n, bool_map, 2)) { - pcam->orthographic = (n==1); + camera->orthographic = (n==1); } - bool has_principal = ReadAttr(elem, "principalpixel", 2, pcam->principal_pixel, text) || - ReadAttr(elem, "principal", 2, pcam->principal_length, text); - bool has_focal = ReadAttr(elem, "focalpixel", 2, pcam->focal_pixel, text) || - ReadAttr(elem, "focal", 2, pcam->focal_length, text); + bool has_principal = ReadAttr(elem, "principalpixel", 2, camera->principal_pixel, text) || + ReadAttr(elem, "principal", 2, camera->principal_length, text); + bool has_focal = ReadAttr(elem, "focalpixel", 2, camera->focal_pixel, text) || + ReadAttr(elem, "focal", 2, camera->focal_length, text); bool needs_sensorsize = has_principal || has_focal; - bool has_sensorsize = ReadAttr(elem, "sensorsize", 2, pcam->sensor_size, text, needs_sensorsize); - bool has_fovy = ReadAttr(elem, "fovy", 1, &pcam->fovy, text); + bool has_sensorsize = ReadAttr(elem, "sensorsize", 2, camera->sensor_size, text, needs_sensorsize); + bool has_fovy = ReadAttr(elem, "fovy", 1, &camera->fovy, text); bool needs_resolution = has_focal || has_sensorsize; - ReadAttr(elem, "resolution", 2, pcam->resolution, text, needs_resolution); + ReadAttr(elem, "resolution", 2, camera->resolution, text, needs_resolution); - if (pcam->resolution[0] < 0 || pcam->resolution[1] < 0) { + if (camera->resolution[0] < 0 || camera->resolution[1] < 0) { throw mjXError(elem, "camera resolution cannot be negative"); } @@ -1798,128 +1798,128 @@ void mjXReader::OneCamera(XMLElement* elem, mjsCamera* pcam) { // read userdata ReadVector(elem, "user", userdata, text); - mjs_setDouble(pcam->userdata, userdata.data(), userdata.size()); + mjs_setDouble(camera->userdata, userdata.data(), userdata.size()); // write error info - mjs_setString(pcam->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(camera->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // light element parser -void mjXReader::OneLight(XMLElement* elem, mjsLight* plight) { +void mjXReader::OneLight(XMLElement* elem, mjsLight* light) { int n; string text, name, targetbody; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(plight->name, name.c_str()); + mjs_setString(light->name, name.c_str()); } if (ReadAttrTxt(elem, "target", targetbody)) { - mjs_setString(plight->targetbody, targetbody.c_str()); + mjs_setString(light->targetbody, targetbody.c_str()); } if (MapValue(elem, "mode", &n, camlight_map, camlight_sz)) { - plight->mode = (mjtCamLight)n; + light->mode = (mjtCamLight)n; } if (MapValue(elem, "directional", &n, bool_map, 2)) { - plight->directional = (n==1); + light->directional = (n==1); } if (MapValue(elem, "castshadow", &n, bool_map, 2)) { - plight->castshadow = (n==1); + light->castshadow = (n==1); } if (MapValue(elem, "active", &n, bool_map, 2)) { - plight->active = (n==1); + light->active = (n==1); } - ReadAttr(elem, "pos", 3, plight->pos, text); - ReadAttr(elem, "dir", 3, plight->dir, text); - ReadAttr(elem, "bulbradius", 1, &plight->bulbradius, text); - ReadAttr(elem, "attenuation", 3, plight->attenuation, text); - ReadAttr(elem, "cutoff", 1, &plight->cutoff, text); - ReadAttr(elem, "exponent", 1, &plight->exponent, text); - ReadAttr(elem, "ambient", 3, plight->ambient, text); - ReadAttr(elem, "diffuse", 3, plight->diffuse, text); - ReadAttr(elem, "specular", 3, plight->specular, text); + ReadAttr(elem, "pos", 3, light->pos, text); + ReadAttr(elem, "dir", 3, light->dir, text); + ReadAttr(elem, "bulbradius", 1, &light->bulbradius, text); + ReadAttr(elem, "attenuation", 3, light->attenuation, text); + ReadAttr(elem, "cutoff", 1, &light->cutoff, text); + ReadAttr(elem, "exponent", 1, &light->exponent, text); + ReadAttr(elem, "ambient", 3, light->ambient, text); + ReadAttr(elem, "diffuse", 3, light->diffuse, text); + ReadAttr(elem, "specular", 3, light->specular, text); // write error info - mjs_setString(plight->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(light->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // pair element parser -void mjXReader::OnePair(XMLElement* elem, mjsPair* ppair) { +void mjXReader::OnePair(XMLElement* elem, mjsPair* pair) { string text, name, geomname1, geomname2; // regular only if (!readingdefaults) { if (ReadAttrTxt(elem, "geom1", geomname1)) { - mjs_setString(ppair->geomname1, geomname1.c_str()); + mjs_setString(pair->geomname1, geomname1.c_str()); } if (ReadAttrTxt(elem, "geom2", geomname2)) { - mjs_setString(ppair->geomname2, geomname2.c_str()); + mjs_setString(pair->geomname2, geomname2.c_str()); } } // read other parameters if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(ppair->name, name.c_str()); + mjs_setString(pair->name, name.c_str()); } - ReadAttrInt(elem, "condim", &ppair->condim); - ReadAttr(elem, "solref", mjNREF, ppair->solref, text, false, false); - ReadAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, text, false, false); - ReadAttr(elem, "solimp", mjNIMP, ppair->solimp, text, false, false); - ReadAttr(elem, "margin", 1, &ppair->margin, text); - ReadAttr(elem, "gap", 1, &ppair->gap, text); - ReadAttr(elem, "friction", 5, ppair->friction, text, false, false); + ReadAttrInt(elem, "condim", &pair->condim); + ReadAttr(elem, "solref", mjNREF, pair->solref, text, false, false); + ReadAttr(elem, "solreffriction", mjNREF, pair->solreffriction, text, false, false); + ReadAttr(elem, "solimp", mjNIMP, pair->solimp, text, false, false); + ReadAttr(elem, "margin", 1, &pair->margin, text); + ReadAttr(elem, "gap", 1, &pair->gap, text); + ReadAttr(elem, "friction", 5, pair->friction, text, false, false); // write error info - mjs_setString(ppair->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(pair->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // equality element parser -void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) { +void mjXReader::OneEquality(XMLElement* elem, mjsEquality* equality) { int n; string text, name1, name2, name; // read type (bad keywords already detected by schema) text = elem->Value(); - pequality->type = (mjtEq)FindKey(equality_map, equality_sz, text); + equality->type = (mjtEq)FindKey(equality_map, equality_sz, text); // regular only if (!readingdefaults) { if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pequality->name, name.c_str()); + mjs_setString(equality->name, name.c_str()); } - switch (pequality->type) { + switch (equality->type) { case mjEQ_CONNECT: ReadAttrTxt(elem, "body1", name1, true); ReadAttrTxt(elem, "body2", name2); - ReadAttr(elem, "anchor", 3, pequality->data, text, true); + ReadAttr(elem, "anchor", 3, equality->data, text, true); break; case mjEQ_WELD: ReadAttrTxt(elem, "body1", name1, true); ReadAttrTxt(elem, "body2", name2); - ReadAttr(elem, "relpose", 7, pequality->data+3, text); - ReadAttr(elem, "torquescale", 1, pequality->data+10, text); - if (!ReadAttr(elem, "anchor", 3, pequality->data, text)) { - mjuu_zerovec(pequality->data, 3); + ReadAttr(elem, "relpose", 7, equality->data+3, text); + ReadAttr(elem, "torquescale", 1, equality->data+10, text); + if (!ReadAttr(elem, "anchor", 3, equality->data, text)) { + mjuu_zerovec(equality->data, 3); } break; case mjEQ_JOINT: ReadAttrTxt(elem, "joint1", name1, true); ReadAttrTxt(elem, "joint2", name2); - ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false); + ReadAttr(elem, "polycoef", 5, equality->data, text, false, false); break; case mjEQ_TENDON: ReadAttrTxt(elem, "tendon1", name1, true); ReadAttrTxt(elem, "tendon2", name2); - ReadAttr(elem, "polycoef", 5, pequality->data, text, false, false); + ReadAttr(elem, "polycoef", 5, equality->data, text, false, false); break; case mjEQ_FLEX: @@ -1934,114 +1934,114 @@ void mjXReader::OneEquality(XMLElement* elem, mjsEquality* pequality) { throw mjXError(elem, "unrecognized equality constraint type"); } - mjs_setString(pequality->name1, name1.c_str()); + mjs_setString(equality->name1, name1.c_str()); if (!name2.empty()) { - mjs_setString(pequality->name2, name2.c_str()); + mjs_setString(equality->name2, name2.c_str()); } } // read attributes if (MapValue(elem, "active", &n, bool_map, 2)) { - pequality->active = (n==1); + equality->active = (n==1); } - ReadAttr(elem, "solref", mjNREF, pequality->solref, text, false, false); - ReadAttr(elem, "solimp", mjNIMP, pequality->solimp, text, false, false); + ReadAttr(elem, "solref", mjNREF, equality->solref, text, false, false); + ReadAttr(elem, "solimp", mjNIMP, equality->solimp, text, false, false); // write error info - mjs_setString(pequality->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(equality->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // tendon element parser -void mjXReader::OneTendon(XMLElement* elem, mjsTendon* pten) { +void mjXReader::OneTendon(XMLElement* elem, mjsTendon* tendon) { string text, name, material; std::vector userdata; // read attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pten->name, name.c_str()); + mjs_setString(tendon->name, name.c_str()); } - ReadAttrInt(elem, "group", &pten->group); + ReadAttrInt(elem, "group", &tendon->group); if (ReadAttrTxt(elem, "material", material)) { - mjs_setString(pten->material, material.c_str()); + mjs_setString(tendon->material, material.c_str()); } - MapValue(elem, "limited", &pten->limited, TFAuto_map, 3); - ReadAttr(elem, "width", 1, &pten->width, text); - ReadAttr(elem, "solreflimit", mjNREF, pten->solref_limit, text, false, false); - ReadAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, text, false, false); - ReadAttr(elem, "solreffriction", mjNREF, pten->solref_friction, text, false, false); - ReadAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, text, false, false); - ReadAttr(elem, "range", 2, pten->range, text); - ReadAttr(elem, "margin", 1, &pten->margin, text); - ReadAttr(elem, "stiffness", 1, &pten->stiffness, text); - ReadAttr(elem, "damping", 1, &pten->damping, text); - ReadAttr(elem, "frictionloss", 1, &pten->frictionloss, text); + MapValue(elem, "limited", &tendon->limited, TFAuto_map, 3); + ReadAttr(elem, "width", 1, &tendon->width, text); + ReadAttr(elem, "solreflimit", mjNREF, tendon->solref_limit, text, false, false); + ReadAttr(elem, "solimplimit", mjNIMP, tendon->solimp_limit, text, false, false); + ReadAttr(elem, "solreffriction", mjNREF, tendon->solref_friction, text, false, false); + ReadAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, text, false, false); + ReadAttr(elem, "range", 2, tendon->range, text); + ReadAttr(elem, "margin", 1, &tendon->margin, text); + ReadAttr(elem, "stiffness", 1, &tendon->stiffness, text); + ReadAttr(elem, "damping", 1, &tendon->damping, text); + ReadAttr(elem, "frictionloss", 1, &tendon->frictionloss, text); // read springlength, either one or two values; if one, copy to second value - if (ReadAttr(elem, "springlength", 2, pten->springlength, text, false, false) == 1) { - pten->springlength[1] = pten->springlength[0]; + if (ReadAttr(elem, "springlength", 2, tendon->springlength, text, false, false) == 1) { + tendon->springlength[1] = tendon->springlength[0]; } - ReadAttr(elem, "rgba", 4, pten->rgba, text); + ReadAttr(elem, "rgba", 4, tendon->rgba, text); // read userdata if (ReadVector(elem, "user", userdata, text)) { - mjs_setDouble(pten->userdata, userdata.data(), userdata.size()); + mjs_setDouble(tendon->userdata, userdata.data(), userdata.size()); } // write error info - mjs_setString(pten->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(tendon->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } // actuator element parser -void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { +void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { string text, type, name, target, slidersite, refsite; // common attributes if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pact->name, name.c_str()); + mjs_setString(actuator->name, name.c_str()); } - ReadAttrInt(elem, "group", &pact->group); - MapValue(elem, "ctrllimited", &pact->ctrllimited, TFAuto_map, 3); - MapValue(elem, "forcelimited", &pact->forcelimited, TFAuto_map, 3); - MapValue(elem, "actlimited", &pact->actlimited, TFAuto_map, 3); - ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text); - ReadAttr(elem, "forcerange", 2, pact->forcerange, text); - ReadAttr(elem, "actrange", 2, pact->actrange, text); - ReadAttr(elem, "lengthrange", 2, pact->lengthrange, text); - ReadAttr(elem, "gear", 6, pact->gear, text, false, false); + ReadAttrInt(elem, "group", &actuator->group); + MapValue(elem, "ctrllimited", &actuator->ctrllimited, TFAuto_map, 3); + MapValue(elem, "forcelimited", &actuator->forcelimited, TFAuto_map, 3); + MapValue(elem, "actlimited", &actuator->actlimited, TFAuto_map, 3); + ReadAttr(elem, "ctrlrange", 2, actuator->ctrlrange, text); + ReadAttr(elem, "forcerange", 2, actuator->forcerange, text); + ReadAttr(elem, "actrange", 2, actuator->actrange, text); + ReadAttr(elem, "lengthrange", 2, actuator->lengthrange, text); + ReadAttr(elem, "gear", 6, actuator->gear, text, false, false); // transmission target and type int cnt = 0; if (ReadAttrTxt(elem, "joint", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_JOINT; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_JOINT; cnt++; } if (ReadAttrTxt(elem, "jointinparent", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_JOINTINPARENT; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_JOINTINPARENT; cnt++; } if (ReadAttrTxt(elem, "tendon", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_TENDON; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_TENDON; cnt++; } if (ReadAttrTxt(elem, "cranksite", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_SLIDERCRANK; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_SLIDERCRANK; cnt++; } if (ReadAttrTxt(elem, "site", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_SITE; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_SITE; cnt++; } if (ReadAttrTxt(elem, "body", target)) { - mjs_setString(pact->target, target.c_str()); - pact->trntype = mjTRN_BODY; + mjs_setString(actuator->target, target.c_str()); + actuator->trntype = mjTRN_BODY; cnt++; } // check for repeated transmission @@ -2050,21 +2050,21 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { } // slidercrank-specific parameters - int r1 = ReadAttr(elem, "cranklength", 1, &pact->cranklength, text); + int r1 = ReadAttr(elem, "cranklength", 1, &actuator->cranklength, text); int r2 = ReadAttrTxt(elem, "slidersite", slidersite); if (r2) { - mjs_setString(pact->slidersite, slidersite.c_str()); + mjs_setString(actuator->slidersite, slidersite.c_str()); } - if ((r1 || r2) && pact->trntype!=mjTRN_SLIDERCRANK && pact->trntype!=mjTRN_UNDEFINED) { + if ((r1 || r2) && actuator->trntype!=mjTRN_SLIDERCRANK && actuator->trntype!=mjTRN_UNDEFINED) { throw mjXError(elem, "cranklength and slidersite can only be used in slidercrank transmission"); } // site-specific parameters (refsite) int r3 = ReadAttrTxt(elem, "refsite", refsite); if (r3) { - mjs_setString(pact->refsite, refsite.c_str()); + mjs_setString(actuator->refsite, refsite.c_str()); } - if (r3 && pact->trntype!=mjTRN_SITE && pact->trntype!=mjTRN_UNDEFINED) { + if (r3 && actuator->trntype!=mjTRN_SITE && actuator->trntype!=mjTRN_UNDEFINED) { throw mjXError(elem, "refsite can only be used with site transmission"); } @@ -2076,39 +2076,39 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { // explicit attributes int n; if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) { - pact->dyntype = (mjtDyn)n; + actuator->dyntype = (mjtDyn)n; } if (MapValue(elem, "gaintype", &n, gain_map, gain_sz)) { - pact->gaintype = (mjtGain)n; + actuator->gaintype = (mjtGain)n; } if (MapValue(elem, "biastype", &n, bias_map, bias_sz)) { - pact->biastype = (mjtBias)n; + actuator->biastype = (mjtBias)n; } if (MapValue(elem, "actearly", &n, bool_map, 2)) { - pact->actearly = (n==1); + actuator->actearly = (n==1); } - ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false); - ReadAttr(elem, "gainprm", mjNGAIN, pact->gainprm, text, false, false); - ReadAttr(elem, "biasprm", mjNBIAS, pact->biasprm, text, false, false); - ReadAttrInt(elem, "actdim", &pact->actdim); + ReadAttr(elem, "dynprm", mjNDYN, actuator->dynprm, text, false, false); + ReadAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, text, false, false); + ReadAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, text, false, false); + ReadAttrInt(elem, "actdim", &actuator->actdim); } // direct drive motor else if (type=="motor") { // unit gain - pact->gainprm[0] = 1; + actuator->gainprm[0] = 1; // implied parameters - pact->dyntype = mjDYN_NONE; - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_NONE; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; } // position or integrated velocity servo else if (type=="position" || type=="intvelocity") { // explicit attributes - ReadAttr(elem, "kp", 1, pact->gainprm, text); - pact->biasprm[1] = -pact->gainprm[0]; + ReadAttr(elem, "kp", 1, actuator->gainprm, text); + actuator->biasprm[1] = -actuator->gainprm[0]; // read kv double kv = -1; // -1: undefined @@ -2126,165 +2126,165 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { if (dampratio > 0 && kv > 0) { throw mjXError(elem, "kv and dampratio cannot both be defined"); } - if (kv > 0) pact->biasprm[2] = -kv; - if (dampratio > 0) pact->biasprm[2] = dampratio; + if (kv > 0) actuator->biasprm[2] = -kv; + if (dampratio > 0) actuator->biasprm[2] = dampratio; // read timeconst, set dyntype - if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) { - if (pact->dynprm[0] < 0) + if (ReadAttr(elem, "timeconst", 1, actuator->dynprm, text)) { + if (actuator->dynprm[0] < 0) throw mjXError(elem, "timeconst cannot be negative"); - pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE; + actuator->dyntype = actuator->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE; } // handle inheritrange - ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text); - if (pact->inheritrange > 0) { + ReadAttr(elem, "inheritrange", 1, &actuator->inheritrange, text); + if (actuator->inheritrange > 0) { if (type == "position") { - if (pact->ctrlrange[0] || pact->ctrlrange[1]) { + if (actuator->ctrlrange[0] || actuator->ctrlrange[1]) { throw mjXError(elem, "ctrlrange and inheritrange cannot both be defined"); } } else { - if (pact->actrange[0] || pact->actrange[1]) { + if (actuator->actrange[0] || actuator->actrange[1]) { throw mjXError(elem, "actrange and inheritrange cannot both be defined"); } } } // implied parameters - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_AFFINE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; if (type=="intvelocity") { - pact->dyntype = mjDYN_INTEGRATOR; - pact->actlimited = 1; + actuator->dyntype = mjDYN_INTEGRATOR; + actuator->actlimited = 1; } } // velocity servo else if (type=="velocity") { // clear bias - mjuu_zerovec(pact->biasprm, mjNBIAS); + mjuu_zerovec(actuator->biasprm, mjNBIAS); // explicit attributes - ReadAttr(elem, "kv", 1, pact->gainprm, text); - pact->biasprm[2] = -pact->gainprm[0]; + ReadAttr(elem, "kv", 1, actuator->gainprm, text); + actuator->biasprm[2] = -actuator->gainprm[0]; // implied parameters - pact->dyntype = mjDYN_NONE; - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_AFFINE; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; } // damper else if (type=="damper") { // clear gain - mjuu_zerovec(pact->gainprm, mjNGAIN); + mjuu_zerovec(actuator->gainprm, mjNGAIN); // explicit attributes - ReadAttr(elem, "kv", 1, pact->gainprm+2, text); - if (pact->gainprm[2]<0) + ReadAttr(elem, "kv", 1, actuator->gainprm+2, text); + if (actuator->gainprm[2]<0) throw mjXError(elem, "damping coefficient cannot be negative"); - pact->gainprm[2] = -pact->gainprm[2]; + actuator->gainprm[2] = -actuator->gainprm[2]; // require nonnegative range - if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { + if (actuator->ctrlrange[0]<0 || actuator->ctrlrange[1]<0) { throw mjXError(elem, "damper control range cannot be negative"); } // implied parameters - pact->ctrllimited = 1; - pact->dyntype = mjDYN_NONE; - pact->gaintype = mjGAIN_AFFINE; - pact->biastype = mjBIAS_NONE; + actuator->ctrllimited = 1; + actuator->dyntype = mjDYN_NONE; + actuator->gaintype = mjGAIN_AFFINE; + actuator->biastype = mjBIAS_NONE; } // cylinder else if (type=="cylinder") { // explicit attributes - ReadAttr(elem, "timeconst", 1, pact->dynprm, text); - ReadAttr(elem, "bias", 3, pact->biasprm, text); - ReadAttr(elem, "area", 1, pact->gainprm, text); + ReadAttr(elem, "timeconst", 1, actuator->dynprm, text); + ReadAttr(elem, "bias", 3, actuator->biasprm, text); + ReadAttr(elem, "area", 1, actuator->gainprm, text); double diameter; if (ReadAttr(elem, "diameter", 1, &diameter, text)) { - pact->gainprm[0] = mjPI / 4 * diameter*diameter; + actuator->gainprm[0] = mjPI / 4 * diameter*diameter; } // implied parameters - pact->dyntype = mjDYN_FILTER; - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_AFFINE; + actuator->dyntype = mjDYN_FILTER; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_AFFINE; } // muscle else if (type=="muscle") { // set muscle defaults if same as global defaults - if (pact->dynprm[0]==1) pact->dynprm[0] = 0.01; // tau act - if (pact->dynprm[1]==0) pact->dynprm[1] = 0.04; // tau deact - if (pact->gainprm[0]==1) pact->gainprm[0] = 0.75; // range[0] - if (pact->gainprm[1]==0) pact->gainprm[1] = 1.05; // range[1] - if (pact->gainprm[2]==0) pact->gainprm[2] = -1; // force - if (pact->gainprm[3]==0) pact->gainprm[3] = 200; // scale - if (pact->gainprm[4]==0) pact->gainprm[4] = 0.5; // lmin - if (pact->gainprm[5]==0) pact->gainprm[5] = 1.6; // lmax - if (pact->gainprm[6]==0) pact->gainprm[6] = 1.5; // vmax - if (pact->gainprm[7]==0) pact->gainprm[7] = 1.3; // fpmax - if (pact->gainprm[8]==0) pact->gainprm[8] = 1.2; // fvmax + if (actuator->dynprm[0]==1) actuator->dynprm[0] = 0.01; // tau act + if (actuator->dynprm[1]==0) actuator->dynprm[1] = 0.04; // tau deact + if (actuator->gainprm[0]==1) actuator->gainprm[0] = 0.75; // range[0] + if (actuator->gainprm[1]==0) actuator->gainprm[1] = 1.05; // range[1] + if (actuator->gainprm[2]==0) actuator->gainprm[2] = -1; // force + if (actuator->gainprm[3]==0) actuator->gainprm[3] = 200; // scale + if (actuator->gainprm[4]==0) actuator->gainprm[4] = 0.5; // lmin + if (actuator->gainprm[5]==0) actuator->gainprm[5] = 1.6; // lmax + if (actuator->gainprm[6]==0) actuator->gainprm[6] = 1.5; // vmax + if (actuator->gainprm[7]==0) actuator->gainprm[7] = 1.3; // fpmax + if (actuator->gainprm[8]==0) actuator->gainprm[8] = 1.2; // fvmax // explicit attributes - ReadAttr(elem, "timeconst", 2, pact->dynprm, text); - ReadAttr(elem, "tausmooth", 1, pact->dynprm+2, text); - if (pact->dynprm[2]<0) + ReadAttr(elem, "timeconst", 2, actuator->dynprm, text); + ReadAttr(elem, "tausmooth", 1, actuator->dynprm+2, text); + if (actuator->dynprm[2]<0) throw mjXError(elem, "muscle tausmooth cannot be negative"); - ReadAttr(elem, "range", 2, pact->gainprm, text); - ReadAttr(elem, "force", 1, pact->gainprm+2, text); - ReadAttr(elem, "scale", 1, pact->gainprm+3, text); - ReadAttr(elem, "lmin", 1, pact->gainprm+4, text); - ReadAttr(elem, "lmax", 1, pact->gainprm+5, text); - ReadAttr(elem, "vmax", 1, pact->gainprm+6, text); - ReadAttr(elem, "fpmax", 1, pact->gainprm+7, text); - ReadAttr(elem, "fvmax", 1, pact->gainprm+8, text); + ReadAttr(elem, "range", 2, actuator->gainprm, text); + ReadAttr(elem, "force", 1, actuator->gainprm+2, text); + ReadAttr(elem, "scale", 1, actuator->gainprm+3, text); + ReadAttr(elem, "lmin", 1, actuator->gainprm+4, text); + ReadAttr(elem, "lmax", 1, actuator->gainprm+5, text); + ReadAttr(elem, "vmax", 1, actuator->gainprm+6, text); + ReadAttr(elem, "fpmax", 1, actuator->gainprm+7, text); + ReadAttr(elem, "fvmax", 1, actuator->gainprm+8, text); // biasprm = gainprm for (int n=0; n<9; n++) { - pact->biasprm[n] = pact->gainprm[n]; + actuator->biasprm[n] = actuator->gainprm[n]; } // implied parameters - pact->dyntype = mjDYN_MUSCLE; - pact->gaintype = mjGAIN_MUSCLE; - pact->biastype = mjBIAS_MUSCLE; + actuator->dyntype = mjDYN_MUSCLE; + actuator->gaintype = mjGAIN_MUSCLE; + actuator->biastype = mjBIAS_MUSCLE; } // adhesion else if (type=="adhesion") { // explicit attributes - ReadAttr(elem, "gain", 1, pact->gainprm, text); - if (pact->gainprm[0]<0) + ReadAttr(elem, "gain", 1, actuator->gainprm, text); + if (actuator->gainprm[0]<0) throw mjXError(elem, "adhesion gain cannot be negative"); // require nonnegative range - ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text); - if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { + ReadAttr(elem, "ctrlrange", 2, actuator->ctrlrange, text); + if (actuator->ctrlrange[0]<0 || actuator->ctrlrange[1]<0) { throw mjXError(elem, "adhesion control range cannot be negative"); } // implied parameters - pact->ctrllimited = 1; - pact->gaintype = mjGAIN_FIXED; - pact->biastype = mjBIAS_NONE; + actuator->ctrllimited = 1; + actuator->gaintype = mjGAIN_FIXED; + actuator->biastype = mjBIAS_NONE; } else if (type == "plugin") { - OnePlugin(elem, &pact->plugin); + OnePlugin(elem, &actuator->plugin); int n; if (MapValue(elem, "dyntype", &n, dyn_map, dyn_sz)) { - pact->dyntype = (mjtDyn)n; + actuator->dyntype = (mjtDyn)n; } if (MapValue(elem, "actearly", &n, bool_map, 2)) { - pact->actearly = (n==1); + actuator->actearly = (n==1); } - ReadAttr(elem, "dynprm", mjNDYN, pact->dynprm, text, false, false); - ReadAttrInt(elem, "actdim", &pact->actdim); + ReadAttr(elem, "dynprm", mjNDYN, actuator->dynprm, text, false, false); + ReadAttrInt(elem, "actdim", &actuator->actdim); } else { // SHOULD NOT OCCUR @@ -2294,11 +2294,11 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) { // read userdata std::vector userdata; if (ReadVector(elem, "user", userdata, text)) { - mjs_setDouble(pact->userdata, userdata.data(), userdata.size()); + mjs_setDouble(actuator->userdata, userdata.data(), userdata.size()); } // write info - mjs_setString(pact->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(actuator->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); } @@ -2537,7 +2537,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* pbody, mjsDefault* def) // make flexcomp -void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* pbody, const mjVFS* vfs) { +void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { string text, material; int n; @@ -2659,7 +2659,7 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* pbody, const mjVFS* vfs) // make flexcomp char error[200]; - bool res = fcomp.Make(spec, pbody, error, 200); + bool res = fcomp.Make(spec, body, error, 200); // throw error if (!res) { @@ -2848,7 +2848,7 @@ void mjXReader::Extension(XMLElement* section) { // custom section parser void mjXReader::Custom(XMLElement* section) { - string text, name; + string str, name; XMLElement* elem; double data[500]; @@ -2862,65 +2862,65 @@ void mjXReader::Custom(XMLElement* section) { // numeric if (name=="numeric") { // create custom - mjsNumeric* pnum = mjs_addNumeric(spec); + mjsNumeric* numeric = mjs_addNumeric(spec); // write error info - mjs_setString(pnum->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(numeric->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read attributes ReadAttrTxt(elem, "name", elname, true); - mjs_setString(pnum->name, elname.c_str()); - if (ReadAttrInt(elem, "size", &pnum->size)) { - int sz = pnum->size < 500 ? pnum->size : 500; + mjs_setString(numeric->name, elname.c_str()); + if (ReadAttrInt(elem, "size", &numeric->size)) { + int sz = numeric->size < 500 ? numeric->size : 500; for (int i=0; isize = 501; + numeric->size = 501; } - int len = ReadAttr(elem, "data", pnum->size, data, text, false, false); - if (pnum->size==501) { - pnum->size = len; + int len = ReadAttr(elem, "data", numeric->size, data, str, false, false); + if (numeric->size==501) { + numeric->size = len; } - if (pnum->size<1 || pnum->size>500) { + if (numeric->size<1 || numeric->size>500) { throw mjXError(elem, "custom field size must be between 1 and 500"); } // copy data - mjs_setDouble(pnum->data, data, pnum->size); + mjs_setDouble(numeric->data, data, numeric->size); } // text else if (name=="text") { // create custom - mjsText* pte = mjs_addText(spec); + mjsText* text = mjs_addText(spec); // write error info - mjs_setString(pte->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(text->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read attributes ReadAttrTxt(elem, "name", elname, true); - mjs_setString(pte->name, elname.c_str()); - ReadAttrTxt(elem, "data", text, true); - if (text.empty()) { + mjs_setString(text->name, elname.c_str()); + ReadAttrTxt(elem, "data", str, true); + if (str.empty()) { throw mjXError(elem, "text field cannot be empty"); } // copy data - mjs_setString(pte->data, text.c_str()); + mjs_setString(text->data, str.c_str()); } // tuple else if (name=="tuple") { // create custom - mjsTuple* ptu = mjs_addTuple(spec); + mjsTuple* tuple = mjs_addTuple(spec); // write error info - mjs_setString(ptu->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(tuple->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read attributes ReadAttrTxt(elem, "name", elname, true); - mjs_setString(ptu->name, elname.c_str()); + mjs_setString(tuple->name, elname.c_str()); // read objects and add XMLElement* obj = FirstChildElement(elem); @@ -2935,20 +2935,20 @@ void mjXReader::Custom(XMLElement* section) { // new object if (name=="element") { // read type, check and assign - ReadAttrTxt(obj, "objtype", text, true); - mjtObj otype = (mjtObj)mju_str2Type(text.c_str()); + ReadAttrTxt(obj, "objtype", str, true); + mjtObj otype = (mjtObj)mju_str2Type(str.c_str()); if (otype==mjOBJ_UNKNOWN) { throw mjXError(obj, "unknown object type"); } objtype.push_back(otype); // read name and assign - ReadAttrTxt(obj, "objname", text, true); - objname += " " + text; + ReadAttrTxt(obj, "objname", str, true); + objname += " " + str; // read parameter and assign double oprm = 0; - ReadAttr(obj, "prm", 1, &oprm, text); + ReadAttr(obj, "prm", 1, &oprm, str); objprm.push_back(oprm); } @@ -2956,9 +2956,9 @@ void mjXReader::Custom(XMLElement* section) { obj = NextSiblingElement(obj); } - mjs_setInt(ptu->objtype, objtype.data(), objtype.size()); - mjs_setStringVec(ptu->objname, objname.c_str()); - mjs_setDouble(ptu->objprm, objprm.data(), objprm.size()); + mjs_setInt(tuple->objtype, objtype.data(), objtype.size()); + mjs_setStringVec(tuple->objname, objname.c_str()); + mjs_setDouble(tuple->objprm, objprm.data(), objprm.size()); } // advance to next element @@ -3122,59 +3122,59 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { // texture sub-element if (name=="texture") { // create texture - mjsTexture* ptex = mjs_addTexture(spec); + mjsTexture* texture = mjs_addTexture(spec); // write error info - mjs_setString(ptex->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(texture->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read attributes if (MapValue(elem, "type", &n, texture_map, texture_sz)) { - ptex->type = (mjtTexture)n; + texture->type = (mjtTexture)n; } if (ReadAttrTxt(elem, "name", texname)) { - mjs_setString(ptex->name, texname.c_str()); + mjs_setString(texture->name, texname.c_str()); } if (ReadAttrTxt(elem, "content_type", content_type)) { - mjs_setString(ptex->content_type, content_type.c_str()); + mjs_setString(texture->content_type, content_type.c_str()); } auto file = ReadAttrFile(elem, "file", vfs, TextureDir()); if (file.has_value()) { - mjs_setString(ptex->file, file->c_str()); + mjs_setString(texture->file, file->c_str()); } - ReadAttrInt(elem, "width", &ptex->width); - ReadAttrInt(elem, "height", &ptex->height); - if (!ReadAttrInt(elem, "nchannel", &ptex->nchannel)) { - ptex->nchannel = 3; + ReadAttrInt(elem, "width", &texture->width); + ReadAttrInt(elem, "height", &texture->height); + if (!ReadAttrInt(elem, "nchannel", &texture->nchannel)) { + texture->nchannel = 3; } - ReadAttr(elem, "rgb1", 3, ptex->rgb1, text); - ReadAttr(elem, "rgb2", 3, ptex->rgb2, text); - ReadAttr(elem, "markrgb", 3, ptex->markrgb, text); - ReadAttr(elem, "random", 1, &ptex->random, text); + ReadAttr(elem, "rgb1", 3, texture->rgb1, text); + ReadAttr(elem, "rgb2", 3, texture->rgb2, text); + ReadAttr(elem, "markrgb", 3, texture->markrgb, text); + ReadAttr(elem, "random", 1, &texture->random, text); if (MapValue(elem, "builtin", &n, builtin_map, builtin_sz)) { - ptex->builtin = (mjtBuiltin)n; + texture->builtin = (mjtBuiltin)n; } if (MapValue(elem, "mark", &n, mark_map, mark_sz)) { - ptex->mark = (mjtMark)n; + texture->mark = (mjtMark)n; } if (MapValue(elem, "hflip", &n, bool_map, 2)) { - ptex->hflip = (n!=0); + texture->hflip = (n!=0); } if (MapValue(elem, "vflip", &n, bool_map, 2)) { - ptex->vflip = (n!=0); + texture->vflip = (n!=0); } // grid - ReadAttr(elem, "gridsize", 2, ptex->gridsize, text); + ReadAttr(elem, "gridsize", 2, texture->gridsize, text); if (ReadAttrTxt(elem, "gridlayout", text)) { // check length if (text.length()>12) { throw mjXError(elem, "gridlayout length cannot exceed 12 characters"); } - if (text.length()!=ptex->gridsize[0]*ptex->gridsize[1]) { + if (text.length()!=texture->gridsize[0]*texture->gridsize[1]) { throw mjXError(elem, "gridlayout length must match gridsize"); } - memcpy(ptex->gridlayout, text.data(), text.length()); + memcpy(texture->gridlayout, text.data(), text.length()); } // separate files @@ -3190,59 +3190,59 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { } else { cubefiles[i] = ""; } - mjs_setInStringVec(ptex->cubefiles, i, cubefiles[i].c_str()); + mjs_setInStringVec(texture->cubefiles, i, cubefiles[i].c_str()); } } // material sub-element else if (name=="material") { // create material and parse - mjsMaterial* pmat = mjs_addMaterial(spec, def); - OneMaterial(elem, pmat); + mjsMaterial* material = mjs_addMaterial(spec, def); + OneMaterial(elem, material); } // mesh sub-element else if (name=="mesh") { // create mesh and parse - mjsMesh* pmesh = mjs_addMesh(spec, def); - OneMesh(elem, pmesh, vfs); + mjsMesh* mesh = mjs_addMesh(spec, def); + OneMesh(elem, mesh, vfs); } // skin sub-element... deprecate ??? else if (name=="skin") { // create skin and parse - mjsSkin* pskin = mjs_addSkin(spec); - OneSkin(elem, pskin, vfs); + mjsSkin* skin = mjs_addSkin(spec); + OneSkin(elem, skin, vfs); } // hfield sub-element else if (name=="hfield") { // create hfield - mjsHField* phf = mjs_addHField(spec); + mjsHField* hfield = mjs_addHField(spec); // write error info - mjs_setString(phf->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(hfield->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read attributes string name, content_type; if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(phf->name, name.c_str()); + mjs_setString(hfield->name, name.c_str()); } if (ReadAttrTxt(elem, "content_type", content_type)) { - mjs_setString(phf->content_type, content_type.c_str()); + mjs_setString(hfield->content_type, content_type.c_str()); } auto file = ReadAttrFile(elem, "file", vfs, AssetDir()); if (file.has_value()) { - mjs_setString(phf->file, file->c_str()); + mjs_setString(hfield->file, file->c_str()); } - ReadAttrInt(elem, "nrow", &phf->nrow); - ReadAttrInt(elem, "ncol", &phf->ncol); - ReadAttr(elem, "size", 4, phf->size, text, true); + ReadAttrInt(elem, "nrow", &hfield->nrow); + ReadAttrInt(elem, "ncol", &hfield->ncol); + ReadAttr(elem, "size", 4, hfield->size, text, true); // allocate buffer for dynamic hfield, copy user data if given - if (!file.has_value() && phf->nrow>0 && phf->ncol>0) { - int nrow = phf->nrow; - int ncol = phf->ncol; + if (!file.has_value() && hfield->nrow>0 && hfield->ncol>0) { + int nrow = hfield->nrow; + int ncol = hfield->ncol; // read user data auto userdata = ReadAttrVec(elem, "elevation"); @@ -3262,13 +3262,13 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { } } - mjs_setFloat(phf->userdata, flipped.data(), flipped.size()); + mjs_setFloat(hfield->userdata, flipped.data(), flipped.size()); } // user data not given, set to 0 else { std::vector zero(nrow*ncol); - mjs_setFloat(phf->userdata, zero.data(), zero.size()); + mjs_setFloat(hfield->userdata, zero.data(), zero.size()); } } } @@ -3302,19 +3302,19 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { // body/world section parser; recursive -void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, +void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, const mjVFS* vfs) { string text, name; XMLElement* elem; int n; // sanity check - if (!pbody) { + if (!body) { throw mjXError(section, "null body pointer"); } // no attributes allowed in world body - if (mjs_getId(pbody->element)==0 && section->FirstAttribute() && !frame) { + if (mjs_getId(body->element)==0 && section->FirstAttribute() && !frame) { throw mjXError(section, "World body cannot have attributes"); } @@ -3327,22 +3327,22 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, // get class if specified, otherwise use body mjsDefault* def = GetClass(elem); if (!def) { - def = mjs_getDefault(frame ? frame->element : pbody->element); + def = mjs_getDefault(frame ? frame->element : body->element); } // inertial sub-element if (name=="inertial") { // no inertia allowed in world body - if (mjs_getId(pbody->element)==0) { + if (mjs_getId(body->element)==0) { throw mjXError(elem, "World body cannot have inertia"); } - pbody->explicitinertial = true; - ReadAttr(elem, "pos", 3, pbody->ipos, text, true); - ReadQuat(elem, "quat", pbody->iquat, text); - ReadAttr(elem, "mass", 1, &pbody->mass, text, true); - ReadAttr(elem, "diaginertia", 3, pbody->inertia, text); - bool alt = ReadAlternative(elem, pbody->ialt); - bool full = ReadAttr(elem, "fullinertia", 6, pbody->fullinertia, text); + body->explicitinertial = true; + ReadAttr(elem, "pos", 3, body->ipos, text, true); + ReadQuat(elem, "quat", body->iquat, text); + ReadAttr(elem, "mass", 1, &body->mass, text, true); + ReadAttr(elem, "diaginertia", 3, body->inertia, text); + bool alt = ReadAlternative(elem, body->ialt); + bool full = ReadAttr(elem, "fullinertia", 6, body->fullinertia, text); if (alt && full) { throw mjXError(elem, "fullinertia and orientation specifiers cannot be used together"); } @@ -3351,50 +3351,50 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, // joint sub-element else if (name=="joint") { // no joints allowed in world body - if (mjs_getId(pbody->element)==0) { + if (mjs_getId(body->element)==0) { throw mjXError(elem, "World body cannot have joints"); } // create joint and parse - mjsJoint* pjoint = mjs_addJoint(pbody, def); - OneJoint(elem, pjoint); - mjs_setFrame(pjoint->element, frame); + mjsJoint* joint = mjs_addJoint(body, def); + OneJoint(elem, joint); + mjs_setFrame(joint->element, frame); } // freejoint sub-element else if (name=="freejoint") { // no joints allowed in world body - if (mjs_getId(pbody->element)==0) { + if (mjs_getId(body->element)==0) { throw mjXError(elem, "World body cannot have joints"); } // create free joint without defaults - mjsJoint* pjoint = mjs_addFreeJoint(pbody); - mjs_setFrame(pjoint->element, frame); + mjsJoint* joint = mjs_addFreeJoint(body); + mjs_setFrame(joint->element, frame); // save defaults after creation, to make sure writing is ok - mjs_setDefault(pjoint->element, def); + mjs_setDefault(joint->element, def); // read attributes std::string name; if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pjoint->name, name.c_str()); + mjs_setString(joint->name, name.c_str()); } - ReadAttrInt(elem, "group", &pjoint->group); + ReadAttrInt(elem, "group", &joint->group); } // geom sub-element else if (name=="geom") { // create geom and parse - mjsGeom* pgeom = mjs_addGeom(pbody, def); - OneGeom(elem, pgeom); - mjs_setFrame(pgeom->element, frame); + mjsGeom* geom = mjs_addGeom(body, def); + OneGeom(elem, geom); + mjs_setFrame(geom->element, frame); } // site sub-element else if (name=="site") { // create site and parse - mjsSite* site = mjs_addSite(pbody, def); + mjsSite* site = mjs_addSite(body, def); OneSite(elem, site); mjs_setFrame(site->element, frame); } @@ -3402,34 +3402,34 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, // camera sub-element else if (name=="camera") { // create camera and parse - mjsCamera* pcam = mjs_addCamera(pbody, def); - OneCamera(elem, pcam); - mjs_setFrame(pcam->element, frame); + mjsCamera* camera = mjs_addCamera(body, def); + OneCamera(elem, camera); + mjs_setFrame(camera->element, frame); } // light sub-element else if (name=="light") { // create light and parse - mjsLight* plight = mjs_addLight(pbody, def); - OneLight(elem, plight); - mjs_setFrame(plight->element, frame); + mjsLight* light = mjs_addLight(body, def); + OneLight(elem, light); + mjs_setFrame(light->element, frame); } // plugin sub-element else if (name == "plugin") { - OnePlugin(elem, &(pbody->plugin)); + OnePlugin(elem, &(body->plugin)); } // composite sub-element else if (name=="composite") { // parse composite - OneComposite(elem, pbody, def); + OneComposite(elem, body, def); } // flexcomp sub-element else if (name=="flexcomp") { // parse flexcomp - OneFlexcomp(elem, pbody, vfs); + OneFlexcomp(elem, body, vfs); } // frame sub-element @@ -3445,7 +3445,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, } // create frame - mjsFrame* pframe = mjs_addFrame(pbody, frame); + mjsFrame* pframe = mjs_addFrame(body, frame); mjs_setString(pframe->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); mjs_setDefault(pframe->element, childdef ? childdef : def); @@ -3461,7 +3461,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, ReadQuat(elem, "quat", pframe->quat, text); ReadAlternative(elem, pframe->alt); - Body(elem, pbody, pframe, vfs); + Body(elem, body, pframe, vfs); } // replicate sub-element @@ -3494,7 +3494,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, } // create subtree - mjsBody* subtree = mjs_addBody(pbody, childdef); + mjsBody* subtree = mjs_addBody(body, childdef); double pos[3] = {0, 0, 0}; double quat[4] = {1, 0, 0, 0}; @@ -3524,7 +3524,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, UpdateString(suffix, count, i); // attach to parent - if (mjs_attachFrame(pbody, pframe, /*prefix=*/"", suffix.c_str()) != 0) { + if (mjs_attachFrame(body, pframe, /*prefix=*/"", suffix.c_str()) != 0) { throw mjXError(elem, mjs_getError(spec)); } } @@ -3546,41 +3546,40 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, } // create child body - mjsBody* pchild = mjs_addBody(pbody, childdef); - mjs_setString(pchild->info, - std::string("line " + std::to_string(elem->GetLineNum())).c_str()); + mjsBody* child = mjs_addBody(body, childdef); + mjs_setString(child->info, std::string("line " + std::to_string(elem->GetLineNum())).c_str()); // set default from class or childclass - mjs_setDefault(pchild->element, childdef ? childdef : def); + mjs_setDefault(child->element, childdef ? childdef : def); // read attributes std::string name, childclass; if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(pchild->name, name.c_str()); + mjs_setString(child->name, name.c_str()); } if (ReadAttrTxt(elem, "childclass", childclass)) { - mjs_setString(pchild->childclass, childclass.c_str()); + mjs_setString(child->childclass, childclass.c_str()); } - ReadAttr(elem, "pos", 3, pchild->pos, text); - ReadQuat(elem, "quat", pchild->quat, text); + ReadAttr(elem, "pos", 3, child->pos, text); + ReadQuat(elem, "quat", child->quat, text); if (MapValue(elem, "mocap", &n, bool_map, 2)) { - pchild->mocap = (n==1); + child->mocap = (n==1); } - ReadAlternative(elem, pchild->alt); + ReadAlternative(elem, child->alt); // read gravcomp - ReadAttr(elem, "gravcomp", 1, &pchild->gravcomp, text); + ReadAttr(elem, "gravcomp", 1, &child->gravcomp, text); // read userdata std::vector userdata; ReadVector(elem, "user", userdata, text); - mjs_setDouble(pchild->userdata, userdata.data(), userdata.size()); + mjs_setDouble(child->userdata, userdata.data(), userdata.size()); // add frame - mjs_setFrame(pchild->element, frame); + mjs_setFrame(child->element, frame); // make recursive call - Body(elem, pchild, nullptr, vfs); + Body(elem, child, nullptr, vfs); } // attachment @@ -3591,7 +3590,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, mjsFrame* frame, ReadAttrTxt(elem, "prefix", prefix); mjsBody* child = mjs_findBody(spec, (prefix+body_name).c_str()); - mjsFrame* pframe = frame ? frame : mjs_addFrame(pbody, nullptr); + mjsFrame* pframe = frame ? frame : mjs_addFrame(body, nullptr); if (!child) { mjSpec* asset = mjs_findSpec(spec, model_name.c_str()); @@ -3644,26 +3643,26 @@ void mjXReader::Contact(XMLElement* section) { // geom pair to include if (name=="pair") { // create pair and parse - mjsPair* ppair = mjs_addPair(spec, def); - OnePair(elem, ppair); + mjsPair* pair = mjs_addPair(spec, def); + OnePair(elem, pair); } // body pair to exclude else if (name=="exclude") { - mjsExclude* pexclude = mjs_addExclude(spec); + mjsExclude* exclude = mjs_addExclude(spec); string exname, exbody1, exbody2; // write error info - mjs_setString(pexclude->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(exclude->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // read name and body names if (ReadAttrTxt(elem, "name", exname)) { - mjs_setString(pexclude->name, exname.c_str()); + mjs_setString(exclude->name, exname.c_str()); } ReadAttrTxt(elem, "body1", exbody1, true); - mjs_setString(pexclude->bodyname1, exbody1.c_str()); + mjs_setString(exclude->bodyname1, exbody1.c_str()); ReadAttrTxt(elem, "body2", exbody2, true); - mjs_setString(pexclude->bodyname2, exbody2.c_str()); + mjs_setString(exclude->bodyname2, exbody2.c_str()); } // advance to next element @@ -3687,8 +3686,8 @@ void mjXReader::Equality(XMLElement* section) { } // create equality constraint and parse - mjsEquality* pequality = mjs_addEquality(spec, def); - OneEquality(elem, pequality); + mjsEquality* equality = mjs_addEquality(spec, def); + OneEquality(elem, equality); // advance to next element elem = NextSiblingElement(elem); @@ -3717,15 +3716,15 @@ void mjXReader::Deformable(XMLElement* section, const mjVFS* vfs) { // flex sub-element if (name=="flex") { // create flex and parse - mjsFlex* pflex = mjs_addFlex(spec); - OneFlex(elem, pflex); + mjsFlex* flex = mjs_addFlex(spec); + OneFlex(elem, flex); } // skin sub-element else if (name=="skin") { // create skin and parse - mjsSkin* pskin = mjs_addSkin(spec); - OneSkin(elem, pskin, vfs); + mjsSkin* skin = mjs_addSkin(spec); + OneSkin(elem, skin, vfs); } // advance to next element @@ -3751,46 +3750,46 @@ void mjXReader::Tendon(XMLElement* section) { } // create equality constraint and parse - mjsTendon* pten = mjs_addTendon(spec, def); - OneTendon(elem, pten); + mjsTendon* tendon = mjs_addTendon(spec, def); + OneTendon(elem, tendon); // process wrap sub-elements XMLElement* sub = FirstChildElement(elem); while (sub) { // get wrap type - string wrap = sub->Value(); - mjsWrap* pwrap;; + string type = sub->Value(); + mjsWrap* wrap;; // read attributes depending on type - if (wrap=="site") { + if (type=="site") { ReadAttrTxt(sub, "site", text, true); - pwrap = mjs_wrapSite(pten, text.c_str()); + wrap = mjs_wrapSite(tendon, text.c_str()); } - else if (wrap=="geom") { + else if (type=="geom") { ReadAttrTxt(sub, "geom", text, true); if (!ReadAttrTxt(sub, "sidesite", text1)) { text1.clear(); } - pwrap = mjs_wrapGeom(pten, text.c_str(), text1.c_str()); + wrap = mjs_wrapGeom(tendon, text.c_str(), text1.c_str()); } - else if (wrap=="pulley") { + else if (type=="pulley") { ReadAttr(sub, "divisor", 1, &data, text, true); - pwrap = mjs_wrapPulley(pten, data); + wrap = mjs_wrapPulley(tendon, data); } - else if (wrap=="joint") { + else if (type=="joint") { ReadAttrTxt(sub, "joint", text, true); ReadAttr(sub, "coef", 1, &data, text1, true); - pwrap = mjs_wrapJoint(pten, text.c_str(), data); + wrap = mjs_wrapJoint(tendon, text.c_str(), data); } else { throw mjXError(sub, "unknown wrap type"); // SHOULD NOT OCCUR } - mjs_setString(pwrap->info, ("line " + std::to_string(sub->GetLineNum())).c_str()); + mjs_setString(wrap->info, ("line " + std::to_string(sub->GetLineNum())).c_str()); // advance to next sub-element sub = NextSiblingElement(sub); @@ -3817,8 +3816,8 @@ void mjXReader::Actuator(XMLElement* section) { } // create actuator and parse - mjsActuator* pact = mjs_addActuator(spec, def); - OneActuator(elem, pact); + mjsActuator* actuator = mjs_addActuator(spec, def); + OneActuator(elem, actuator); // advance to next element elem = NextSiblingElement(elem); @@ -3833,237 +3832,237 @@ void mjXReader::Sensor(XMLElement* section) { XMLElement* elem = FirstChildElement(section); while (elem) { // create sensor, get string type - mjsSensor* psen = mjs_addSensor(spec); + mjsSensor* sensor = mjs_addSensor(spec); string type = elem->Value(); string text, name, objname, refname; std::vector userdata; // read name, noise, userdata if (ReadAttrTxt(elem, "name", name)) { - mjs_setString(psen->name, name.c_str()); + mjs_setString(sensor->name, name.c_str()); } - ReadAttr(elem, "cutoff", 1, &psen->cutoff, text); - ReadAttr(elem, "noise", 1, &psen->noise, text); + ReadAttr(elem, "cutoff", 1, &sensor->cutoff, text); + ReadAttr(elem, "noise", 1, &sensor->noise, text); if (ReadVector(elem, "user", userdata, text)) { - mjs_setDouble(psen->userdata, userdata.data(), userdata.size()); + mjs_setDouble(sensor->userdata, userdata.data(), userdata.size()); } // common robotic sensors, attached to a site if (type=="touch") { - psen->type = mjSENS_TOUCH; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_TOUCH; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="accelerometer") { - psen->type = mjSENS_ACCELEROMETER; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_ACCELEROMETER; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="velocimeter") { - psen->type = mjSENS_VELOCIMETER; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_VELOCIMETER; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="gyro") { - psen->type = mjSENS_GYRO; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_GYRO; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="force") { - psen->type = mjSENS_FORCE; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_FORCE; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="torque") { - psen->type = mjSENS_TORQUE; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_TORQUE; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="magnetometer") { - psen->type = mjSENS_MAGNETOMETER; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_MAGNETOMETER; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } else if (type=="camprojection") { - psen->type = mjSENS_CAMPROJECTION; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_CAMPROJECTION; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); ReadAttrTxt(elem, "camera", refname, true); - psen->reftype = mjOBJ_CAMERA; + sensor->reftype = mjOBJ_CAMERA; } else if (type=="rangefinder") { - psen->type = mjSENS_RANGEFINDER; - psen->objtype = mjOBJ_SITE; + sensor->type = mjSENS_RANGEFINDER; + sensor->objtype = mjOBJ_SITE; ReadAttrTxt(elem, "site", objname, true); } // sensors related to scalar joints, tendons, actuators else if (type=="jointpos") { - psen->type = mjSENS_JOINTPOS; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTPOS; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointvel") { - psen->type = mjSENS_JOINTVEL; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTVEL; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="tendonpos") { - psen->type = mjSENS_TENDONPOS; - psen->objtype = mjOBJ_TENDON; + sensor->type = mjSENS_TENDONPOS; + sensor->objtype = mjOBJ_TENDON; ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonvel") { - psen->type = mjSENS_TENDONVEL; - psen->objtype = mjOBJ_TENDON; + sensor->type = mjSENS_TENDONVEL; + sensor->objtype = mjOBJ_TENDON; ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="actuatorpos") { - psen->type = mjSENS_ACTUATORPOS; - psen->objtype = mjOBJ_ACTUATOR; + sensor->type = mjSENS_ACTUATORPOS; + sensor->objtype = mjOBJ_ACTUATOR; ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="actuatorvel") { - psen->type = mjSENS_ACTUATORVEL; - psen->objtype = mjOBJ_ACTUATOR; + sensor->type = mjSENS_ACTUATORVEL; + sensor->objtype = mjOBJ_ACTUATOR; ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="actuatorfrc") { - psen->type = mjSENS_ACTUATORFRC; - psen->objtype = mjOBJ_ACTUATOR; + sensor->type = mjSENS_ACTUATORFRC; + sensor->objtype = mjOBJ_ACTUATOR; ReadAttrTxt(elem, "actuator", objname, true); } else if (type=="jointactuatorfrc") { - psen->type = mjSENS_JOINTACTFRC; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTACTFRC; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } // sensors related to ball joints else if (type=="ballquat") { - psen->type = mjSENS_BALLQUAT; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_BALLQUAT; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="ballangvel") { - psen->type = mjSENS_BALLANGVEL; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_BALLANGVEL; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } // joint and tendon limit sensors else if (type=="jointlimitpos") { - psen->type = mjSENS_JOINTLIMITPOS; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTLIMITPOS; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointlimitvel") { - psen->type = mjSENS_JOINTLIMITVEL; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTLIMITVEL; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="jointlimitfrc") { - psen->type = mjSENS_JOINTLIMITFRC; - psen->objtype = mjOBJ_JOINT; + sensor->type = mjSENS_JOINTLIMITFRC; + sensor->objtype = mjOBJ_JOINT; ReadAttrTxt(elem, "joint", objname, true); } else if (type=="tendonlimitpos") { - psen->type = mjSENS_TENDONLIMITPOS; - psen->objtype = mjOBJ_TENDON; + sensor->type = mjSENS_TENDONLIMITPOS; + sensor->objtype = mjOBJ_TENDON; ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonlimitvel") { - psen->type = mjSENS_TENDONLIMITVEL; - psen->objtype = mjOBJ_TENDON; + sensor->type = mjSENS_TENDONLIMITVEL; + sensor->objtype = mjOBJ_TENDON; ReadAttrTxt(elem, "tendon", objname, true); } else if (type=="tendonlimitfrc") { - psen->type = mjSENS_TENDONLIMITFRC; - psen->objtype = mjOBJ_TENDON; + sensor->type = mjSENS_TENDONLIMITFRC; + sensor->objtype = mjOBJ_TENDON; ReadAttrTxt(elem, "tendon", objname, true); } // sensors attached to an object with spatial frame: (x)body, geom, site, camera else if (type=="framepos") { - psen->type = mjSENS_FRAMEPOS; + sensor->type = mjSENS_FRAMEPOS; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="framequat") { - psen->type = mjSENS_FRAMEQUAT; + sensor->type = mjSENS_FRAMEQUAT; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="framexaxis") { - psen->type = mjSENS_FRAMEXAXIS; + sensor->type = mjSENS_FRAMEXAXIS; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="frameyaxis") { - psen->type = mjSENS_FRAMEYAXIS; + sensor->type = mjSENS_FRAMEYAXIS; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="framezaxis") { - psen->type = mjSENS_FRAMEZAXIS; + sensor->type = mjSENS_FRAMEZAXIS; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="framelinvel") { - psen->type = mjSENS_FRAMELINVEL; + sensor->type = mjSENS_FRAMELINVEL; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="frameangvel") { - psen->type = mjSENS_FRAMEANGVEL; + sensor->type = mjSENS_FRAMEANGVEL; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "refname", refname, true); } else if (ReadAttrTxt(elem, "refname", text)) { throw mjXError(elem, "refname '%s' given but reftype is missing", text.c_str()); } } else if (type=="framelinacc") { - psen->type = mjSENS_FRAMELINACC; + sensor->type = mjSENS_FRAMELINACC; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); } else if (type=="frameangacc") { - psen->type = mjSENS_FRAMEANGACC; + sensor->type = mjSENS_FRAMEANGACC; ReadAttrTxt(elem, "objtype", text, true); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname, true); } // sensors related to kinematic subtrees; attached to a body (which is the subtree root) else if (type=="subtreecom") { - psen->type = mjSENS_SUBTREECOM; - psen->objtype = mjOBJ_BODY; + sensor->type = mjSENS_SUBTREECOM; + sensor->objtype = mjOBJ_BODY; ReadAttrTxt(elem, "body", objname, true); } else if (type=="subtreelinvel") { - psen->type = mjSENS_SUBTREELINVEL; - psen->objtype = mjOBJ_BODY; + sensor->type = mjSENS_SUBTREELINVEL; + sensor->objtype = mjOBJ_BODY; ReadAttrTxt(elem, "body", objname, true); } else if (type=="subtreeangmom") { - psen->type = mjSENS_SUBTREEANGMOM; - psen->objtype = mjOBJ_BODY; + sensor->type = mjSENS_SUBTREEANGMOM; + sensor->objtype = mjOBJ_BODY; ReadAttrTxt(elem, "body", objname, true); } @@ -4074,85 +4073,85 @@ void mjXReader::Sensor(XMLElement* section) { if (has_body1 == has_geom1) { throw mjXError(elem, "exactly one of (geom1, body1) must be specified"); } - psen->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM; + sensor->objtype = has_body1 ? mjOBJ_BODY : mjOBJ_GEOM; bool has_body2 = ReadAttrTxt(elem, "body2", refname); bool has_geom2 = ReadAttrTxt(elem, "geom2", refname); if (has_body2 == has_geom2) { throw mjXError(elem, "exactly one of (geom2, body2) must be specified"); } - psen->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM; + sensor->reftype = has_body2 ? mjOBJ_BODY : mjOBJ_GEOM; if (type=="distance") { - psen->type = mjSENS_GEOMDIST; + sensor->type = mjSENS_GEOMDIST; } else if (type=="normal") { - psen->type = mjSENS_GEOMNORMAL; + sensor->type = mjSENS_GEOMNORMAL; } else { - psen->type = mjSENS_GEOMFROMTO; + sensor->type = mjSENS_GEOMFROMTO; } } // global sensors else if (type=="clock") { - psen->type = mjSENS_CLOCK; - psen->objtype = mjOBJ_UNKNOWN; + sensor->type = mjSENS_CLOCK; + sensor->objtype = mjOBJ_UNKNOWN; } // user-defined sensor else if (type=="user") { - psen->type = mjSENS_USER; + sensor->type = mjSENS_USER; bool objname_given = ReadAttrTxt(elem, "objname", objname); if (ReadAttrTxt(elem, "objtype", text)) { if (!objname_given) { throw mjXError(elem, "objtype '%s' given but objname is missing", text.c_str()); } - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); } else if (objname_given) { throw mjXError(elem, "objname '%s' given but objtype is missing", objname.c_str()); } - ReadAttrInt(elem, "dim", &psen->dim, true); + ReadAttrInt(elem, "dim", &sensor->dim, true); // keywords if (MapValue(elem, "needstage", &n, stage_map, stage_sz)) { - psen->needstage = (mjtStage)n; + sensor->needstage = (mjtStage)n; } if (MapValue(elem, "datatype", &n, datatype_map, datatype_sz)) { - psen->datatype = (mjtDataType)n; + sensor->datatype = (mjtDataType)n; } } else if (type=="plugin") { - psen->type = mjSENS_PLUGIN; - OnePlugin(elem, &psen->plugin); + sensor->type = mjSENS_PLUGIN; + OnePlugin(elem, &sensor->plugin); ReadAttrTxt(elem, "objtype", text); - psen->objtype = (mjtObj)mju_str2Type(text.c_str()); + sensor->objtype = (mjtObj)mju_str2Type(text.c_str()); ReadAttrTxt(elem, "objname", objname); - if (psen->objtype != mjOBJ_UNKNOWN && objname.empty()) { + if (sensor->objtype != mjOBJ_UNKNOWN && objname.empty()) { throw mjXError(elem, "objtype is specified but objname is not"); } - if (psen->objtype == mjOBJ_UNKNOWN && !objname.empty()) { + if (sensor->objtype == mjOBJ_UNKNOWN && !objname.empty()) { throw mjXError(elem, "objname is specified but objtype is not"); } if (ReadAttrTxt(elem, "reftype", text)) { - psen->reftype = (mjtObj)mju_str2Type(text.c_str()); + sensor->reftype = (mjtObj)mju_str2Type(text.c_str()); } ReadAttrTxt(elem, "refname", refname); - if (psen->reftype != mjOBJ_UNKNOWN && refname.empty()) { + if (sensor->reftype != mjOBJ_UNKNOWN && refname.empty()) { throw mjXError(elem, "reftype is specified but refname is not"); } - if (psen->reftype == mjOBJ_UNKNOWN && !refname.empty()) { + if (sensor->reftype == mjOBJ_UNKNOWN && !refname.empty()) { throw mjXError(elem, "refname is specified but reftype is not"); } } if (!objname.empty()) { - mjs_setString(psen->objname, objname.c_str()); + mjs_setString(sensor->objname, objname.c_str()); } if (!refname.empty()) { - mjs_setString(psen->refname, refname.c_str()); + mjs_setString(sensor->refname, refname.c_str()); } // write info - mjs_setString(psen->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(sensor->info, ("line " + std::to_string(elem->GetLineNum())).c_str()); // advance to next element elem = NextSiblingElement(elem); @@ -4173,47 +4172,47 @@ void mjXReader::Keyframe(XMLElement* section) { string text, name = ""; // add keyframe - mjsKey* pk = mjs_addKey(spec); + mjsKey* key = mjs_addKey(spec); // read name, time ReadAttrTxt(elem, "name", name); - mjs_setString(pk->name, name.c_str()); - ReadAttr(elem, "time", 1, &pk->time, text); + mjs_setString(key->name, name.c_str()); + ReadAttr(elem, "time", 1, &key->time, text); // read qpos n = ReadAttr(elem, "qpos", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->qpos, data, n); + mjs_setDouble(key->qpos, data, n); } // read qvel n = ReadAttr(elem, "qvel", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->qvel, data, n); + mjs_setDouble(key->qvel, data, n); } // read act n = ReadAttr(elem, "act", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->act, data, n); + mjs_setDouble(key->act, data, n); } // read mpos n = ReadAttr(elem, "mpos", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->mpos, data, n); + mjs_setDouble(key->mpos, data, n); } // read mquat n = ReadAttr(elem, "mquat", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->mquat, data, n); + mjs_setDouble(key->mquat, data, n); } // read ctrl n = ReadAttr(elem, "ctrl", 1000, data, text, false, false); if (n) { - mjs_setDouble(pk->ctrl, data, n); + mjs_setDouble(key->ctrl, data, n); } // advance to next element From 7f09a7d8c62ec603c23968029791b3931d2bb6c6 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 15 Aug 2024 04:34:30 -0700 Subject: [PATCH 10/24] Fix bug in S2D and improve readability of GJK + EPA code. PiperOrigin-RevId: 663261102 Change-Id: Ibb1d801078cc02bc7ccec246be6dc50fa4f2be18 --- src/engine/engine_collision_gjk.c | 111 +++++++++++++---------- test/engine/engine_collision_gjk_test.cc | 2 +- 2 files changed, 64 insertions(+), 49 deletions(-) diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 18c4ffe7..55f202e9 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -246,6 +246,33 @@ static mjtNum det3(const mjtNum v1[3], const mjtNum v2[3], const mjtNum v3[3]) { } +// res = origin projected onto plane defined by v1, v2, v3 +static inline void projectOriginPlane(mjtNum res[3], mjtNum normal[3], const mjtNum v1[3], + const mjtNum v2[3], const mjtNum v3[3]) { + mjtNum diff1[3], diff2[3], tmp[3]; + mju_sub3(diff1, v2, v1); + mju_sub3(diff2, v3, v1); + mju_cross(tmp, diff1, diff2); // vector normal to the plane + + // res = tmp * / ||tmp||^2 + mjtNum tmp_sqr = mju_dot3(tmp, tmp); + mju_scl3(res, tmp, mju_dot3(tmp, v1) / tmp_sqr); + if (normal) mju_scl3(normal, tmp, 1/mju_sqrt(tmp_sqr)); +} + + + +// res = origin projected onto line defined by v1, v2 +static inline void projectOriginLine(mjtNum res[3], const mjtNum v1[3], const mjtNum v2[3]) { + // res = v2 - / * (v2 - v1) + mjtNum diff[3]; + mju_sub3(diff, v2, v1); + mjtNum temp1 = mju_dot3(v2, diff); + mjtNum temp2 = mju_dot3(diff, diff); + mju_addScl3(res, v2, diff, - temp1 / temp2); +} + + // returns true only when a and b are both strictly positive or both strictly negative static int compareSigns(mjtNum a, mjtNum b) { @@ -391,32 +418,25 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { const mjtNum* s2 = simplex + 3; const mjtNum* s3 = simplex + 6; - // compute normal - mjtNum diff1[3], diff2[3], n[3]; - mju_sub3(diff1, s2, s1); - mju_sub3(diff2, s3, s1); - mju_cross(n, diff1, diff2); - - // project origin + // project origin onto affine hull of the simplex mjtNum p_o[3]; - mju_scl3(p_o, n, mju_dot3(n, s1) / mju_dot3(n, n)); + projectOriginPlane(p_o, NULL, s1, s2, s3); - mjtNum mu_max = 0; - - // Below are the minors M_i4 of the matrix M given by - // [[ s1_x, s2_x, s3_x, s4_x ], - // [ s1_y, s2_y, s3_y, s4_y ], - // [ s1_z, s2_z, s3_z, s4_z ], - // [ 1, 1, 1, 1 ]] + // Below are the minors M_i4 of the matrix M given by + // [[ s1_x, s2_x, s3_x, s4_x ], + // [ s1_y, s2_y, s3_y, s4_y ], + // [ s1_z, s2_z, s3_z, s4_z ], + // [ 1, 1, 1, 1 ]] mjtNum M_14 = s2[1]*s3[2] - s2[2]*s3[1] - s1[1]*s3[2] + s1[2]*s3[1] + s1[1]*s2[2] - s1[2]*s2[1]; mjtNum M_24 = s2[0]*s3[2] - s2[2]*s3[0] - s1[0]*s3[2] + s1[2]*s3[0] + s1[0]*s2[2] - s1[2]*s2[0]; mjtNum M_34 = s2[0]*s3[1] - s2[1]*s3[0] - s1[0]*s3[1] + s1[1]*s3[0] + s1[0]*s2[1] - s1[1]*s2[0]; // exclude one of the axes with the largest projection of the simplex using the computed minors + mjtNum M_max = 0; mjtNum s1_2D[2], s2_2D[2], s3_2D[2], p_o_2D[2]; mjtNum mu1 = mju_abs(M_14), mu2 = mju_abs(M_24), mu3 = mju_abs(M_34); if (mu1 >= mu2 && mu1 >= mu3) { - mu_max = mu1; + M_max = M_14; s1_2D[0] = s1[1]; s1_2D[1] = s1[2]; @@ -429,7 +449,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[0] = p_o[1]; p_o_2D[1] = p_o[2]; } else if (mu2 >= mu3) { - mu_max = mu2; + M_max = M_24; s1_2D[0] = s1[0]; s1_2D[1] = s1[2]; @@ -442,7 +462,7 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[0] = p_o[0]; p_o_2D[1] = p_o[2]; } else { - mu_max = mu3; + M_max = M_34; s1_2D[0] = s1[0]; s1_2D[1] = s1[1]; @@ -456,25 +476,32 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { p_o_2D[1] = p_o[1]; } - // substitute p_o as a vertex in simplex - mjtNum C1 = p_o_2D[0]*s2_2D[1] + p_o_2D[1]*s3_2D[0] + s2_2D[0]*s3_2D[1] - - p_o_2D[0]*s3_2D[1] - p_o_2D[1]*s2_2D[0] - s3_2D[0]*s2_2D[1]; + // compute the cofactors C3i of the following matrix: + // [[ s1_2D[0] - p_o_2D[0], s2_2D[0] - p_o_2D[0], s3_2D[0] - p_o_2D[0] ], + // [ s1_2D[1] - p_o_2D[1], s2_2D[1] - p_o_2D[1], s3_2D[1] - p_o_2D[1] ], + // [ 1, 1, 1 ]] - mjtNum C2 = p_o_2D[0]*s3_2D[1] + p_o_2D[1]*s1_2D[0] + s3_2D[0]*s1_2D[1] - - p_o_2D[0]*s1_2D[1] - p_o_2D[1]*s3_2D[0] - s1_2D[0]*s3_2D[1]; + // C31 corresponds to the signed area of 2-simplex: (p_o_2D, s2_2D, s3_2D) + mjtNum C31 = p_o_2D[0]*s2_2D[1] + p_o_2D[1]*s3_2D[0] + s2_2D[0]*s3_2D[1] + - p_o_2D[0]*s3_2D[1] - p_o_2D[1]*s2_2D[0] - s3_2D[0]*s2_2D[1]; - mjtNum C3 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] - - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; + // C32 corresponds to the signed area of 2-simplex: (_po_2D, s1_2D, s3_2D) + mjtNum C32 = p_o_2D[0]*s3_2D[1] + p_o_2D[1]*s1_2D[0] + s3_2D[0]*s1_2D[1] + - p_o_2D[0]*s1_2D[1] - p_o_2D[1]*s3_2D[0] - s1_2D[0]*s3_2D[1]; - int comp1 = compareSigns(mu_max, C1), - comp2 = compareSigns(mu_max, C2), - comp3 = compareSigns(mu_max, C3); + // C33 corresponds to the signed area of 2-simplex: (p_o_2D, s1_2D, s2_2D) + mjtNum C33 = p_o_2D[0]*s1_2D[1] + p_o_2D[1]*s2_2D[0] + s1_2D[0]*s2_2D[1] + - p_o_2D[0]*s2_2D[1] - p_o_2D[1]*s1_2D[0] - s2_2D[0]*s1_2D[1]; - // inside the simplex + int comp1 = compareSigns(M_max, C31), + comp2 = compareSigns(M_max, C32), + comp3 = compareSigns(M_max, C33); + + // all the same sign, p_o is inside the 2-simplex if (comp1 && comp2 && comp3) { - lambda[0] = C1 / mu_max; - lambda[1] = C2 / mu_max; - lambda[2] = C3 / mu_max; + lambda[0] = C31 / M_max; + lambda[1] = C32 / M_max; + lambda[2] = C33 / M_max; return; } @@ -517,9 +544,9 @@ static void S2D(mjtNum lambda[3], const mjtNum simplex[9]) { lincomb(x, lambda_1d, verts, 2); mjtNum d = mju_norm3(x); if (d < dist) { + lambda[0] = 0; lambda[1] = lambda_1d[0]; lambda[2] = lambda_1d[1]; - lambda[0] = 0; dist = d; } } @@ -533,13 +560,8 @@ static void S1D(mjtNum lambda[2], const mjtNum simplex[6]) { const mjtNum* s2 = simplex + 3; // find projection of origin onto the 1-simplex: - // p_o = s2 - / * (s2 - s1) mjtNum p_o[3]; - mjtNum diff[3]; - mju_sub3(diff, s2, s1); - mjtNum temp1 = mju_dot3(s2, diff); - mjtNum temp2 = mju_dot3(diff, diff); - mju_addScl3(p_o, s2, diff, - temp1 / temp2); + projectOriginLine(p_o, s1, s2); // find the axis with the largest projection "shadow" of the simplex mjtNum mu_max = 0; @@ -785,18 +807,11 @@ static void attachFace(Polytope* pt, int v1, int v2, int v3) { face->verts[1] = v2; face->verts[2] = v3; - // compute normal n + // compute witness point v mjtNum* pv1 = pt->verts[v1].v; mjtNum* pv2 = pt->verts[v2].v; mjtNum* pv3 = pt->verts[v3].v; - mjtNum diff1[3], diff2[3]; - mju_sub3(diff1, pv2, pv1); - mju_sub3(diff2, pv3, pv1); - mju_cross(face->n, diff1, diff2); - mju_normalize3(face->n); - - // compute witness point v - mju_scl3(face->v, face->n, mju_dot3(face->n, pv1)); + projectOriginPlane(face->v, face->n, pv1, pv2, pv3); face->dist = mju_norm3(face->v); // orientation check diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 86dd9f09..8ced09d0 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -218,7 +218,7 @@ TEST_F(MjGjkTest, CapsuleCapsule) { int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); - EXPECT_NEAR(dist, 0.4765, .0001); + EXPECT_NEAR(dist, 0.4711, .0001); mj_deleteData(data); mj_deleteModel(model); } From b2018ff506281fcbeefe3b98e3b0a790bd40e3eb Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 15 Aug 2024 05:10:37 -0700 Subject: [PATCH 11/24] Replace function with static array in engine_forward.c PiperOrigin-RevId: 663268322 Change-Id: Ibd161a8eda55fc04cf37e465f0d0ac0adcfc7847 --- src/engine/engine_forward.c | 19 +++---------------- 1 file changed, 3 insertions(+), 16 deletions(-) diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index cb326594..1a43c1d9 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -264,21 +264,6 @@ static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, i -// return number of dofs given joint type -static int jnt_dofnum(mjtJoint type) { - if (type == mjJNT_FREE) { - return 6; - } - - if (type == mjJNT_BALL) { - return 3; - } - - return 1; -} - - - // (qpos, qvel, ctrl, act) => (qfrc_actuator, actuator_force, act_dot) void mj_fwdActuation(const mjModel* m, mjData* d) { TM_START; @@ -493,6 +478,8 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { // actuator-level gravity compensation if (m->ngravcomp && !mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { + // number of dofs for each joint type: {mjJNT_FREE, mjJNT_BALL, mjJNT_SLIDE, mjJNT_HINGE} + static const int jnt_dofnum[4] = {6, 3, 1, 1}; int njnt = m->njnt; for (int i=0; i < njnt; i++) { // skip if gravcomp added as passive force @@ -501,7 +488,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // add gravcomp force - int dofnum = jnt_dofnum(m->jnt_type[i]); + int dofnum = jnt_dofnum[m->jnt_type[i]]; int dofadr = m->jnt_dofadr[i]; mju_addTo(d->qfrc_actuator + dofadr, d->qfrc_gravcomp + dofadr, dofnum); } From 4b88e9bebf43f92de4b0f7a51116b7f3460b273e Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 15 Aug 2024 06:09:45 -0700 Subject: [PATCH 12/24] Do not delete keyframes during mjs_detachBody. PiperOrigin-RevId: 663281082 Change-Id: I6eedf8c5a3a422bcd78086f2fe30c2099f0b9d16 --- src/user/user_model.cc | 34 +++++++++++++++++++++++++--------- src/user/user_objects.cc | 3 --- test/user/user_api_test.cc | 5 +++++ 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index bfcce1fd..cbd37927 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -325,6 +325,16 @@ void mjCModel::RemoveFromList(std::vector& list, const mjCModel& other) { +template <> +void mjCModel::DeleteAll(std::vector& elements) { + for (mjCKey* element : elements) { + delete element; + } + elements.clear(); +} + + + mjCModel& mjCModel::operator-=(const mjCBody& subtree) { mjCModel oldmodel(*this); @@ -345,6 +355,10 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { MakeLists(world); ProcessLists(/*checkrepeat=*/false); + // store keyframes in the old model + oldmodel.key_pending_.clear(); + oldmodel.StoreKeyframes(); + // check if we have to remove anything else RemoveFromList(pairs_, oldmodel); RemoveFromList(excludes_, oldmodel); @@ -353,6 +367,12 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) { RemoveFromList(actuators_, oldmodel); RemoveFromList(sensors_, oldmodel); + // move all keyframes to pending so that they will be resized + DeleteAll(keys_); + for (const auto& key : oldmodel.key_pending_) { + key_pending_.push_back(key); + } + // restore to the original state if (!compiled) { ResetTreeLists(); @@ -1163,13 +1183,6 @@ void mjCModel::DeleteAll(std::vector& elements) { elements.clear(); } -template <> -void mjCModel::DeleteAll(std::vector& elements) { - for (mjCKey* element : elements) { - delete element; - } - elements.clear(); -} // set nuser fields void mjCModel::SetNuser() { @@ -2983,14 +2996,17 @@ template void mjCModel::RestoreState( // resolve keyframe references void mjCModel::StoreKeyframes() { bool resetlists = false; - if (joints_.empty()) { + + // create tree lists if they are empty, occurs if an uncompiled model is attached + if (bodies_.size() == 1 && geoms_.empty() && sites_.empty() && joints_.empty() && + cameras_.empty() && lights_.empty() && frames_.empty()) { MakeLists(bodies_[0]); resetlists = true; } SaveDofOffsets(); - for (auto key : keys_) { + for (auto& key : keys_) { mjKeyInfo info; info.name = prefix + key->name + suffix; info.time = key->spec.time; diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 1c177d5a..cc0c7c2f 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -937,9 +937,6 @@ mjCBody& mjCBody::operator-=(const mjCBody& subtree) { *bodies[i] -= subtree; } - // (b/350784262) delete keyframes - model->DeleteAll(model->keys_); - return *this; } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 8f539ee8..5eb68356 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -816,6 +816,11 @@ void TestDetachBody(bool compile) { + + + + + )"; // model with one cylinder and a hinge From 64575f399bb6231cea48810e0189186c8aba8587 Mon Sep 17 00:00:00 2001 From: Kyle Bayes Date: Thu, 15 Aug 2024 08:48:50 -0700 Subject: [PATCH 13/24] Add heightfield collision support for GJK + EPA implementation. PiperOrigin-RevId: 663321119 Change-Id: I01a85ae15386c997bff4e9c6d71555a4380dfffe --- src/engine/engine_collision_convex.c | 126 ++++++++++++----------- src/engine/engine_collision_convex.h | 3 + src/engine/engine_collision_gjk.c | 12 +-- test/engine/engine_collision_gjk_test.cc | 37 ++++++- 4 files changed, 110 insertions(+), 68 deletions(-) diff --git a/src/engine/engine_collision_convex.c b/src/engine/engine_collision_convex.c index 5ba14c02..942e6172 100644 --- a/src/engine/engine_collision_convex.c +++ b/src/engine/engine_collision_convex.c @@ -360,8 +360,10 @@ static void mju_rotateFrame(const mjtNum origin[3], const mjtNum rot[9], int mjc_Convex(const mjModel* m, const mjData* d, mjContact* con, int g1, int g2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -586,46 +588,45 @@ int mjc_PlaneConvex(const mjModel* m, const mjData* d, //---------------------------- heightfield collisions --------------------------------------------- -// ccd prism object type -struct _mjtPrism { - mjtNum v[6][3]; -}; - -typedef struct _mjtPrism mjtPrism; - - -// ccd prism support function -static void prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { +// prism support function +static void mjc_prism_support(mjtNum res[3], mjCCDObj* obj, const mjtNum dir[3]) { int istart, ibest; mjtNum best, tmp; - const mjtPrism* p = (const mjtPrism*)obj; // find best vertex in halfspace determined by dir.z - istart = dir->v[2] < 0 ? 0 : 3; + istart = dir[2] < 0 ? 0 : 3; ibest = istart; - best = mju_dot3(p->v[istart], dir->v); + best = mju_dot3(obj->prism[istart], dir); for (int i=istart+1; i < istart+3; i++) { - if ((tmp = mju_dot3(p->v[i], dir->v)) > best) { + if ((tmp = mju_dot3(obj->prism[i], dir)) > best) { ibest = i; best = tmp; } } // copy best point - mju_copy3(vec->v, p->v[ibest]); + mju_copy3(res, obj->prism[ibest]); +} + +// ccd prism support function +static void mjccd_prism_support(const void *obj, const ccd_vec3_t *dir, ccd_vec3_t *vec) { + mjc_prism_support(vec->v, (mjCCDObj*) obj, dir->v); } -// ccd prism center function -static void prism_center(const void *obj, ccd_vec3_t *center) { - const mjtPrism* p = (const mjtPrism*)obj; - +// prism center function +static void mjc_prism_center(mjtNum res[3], const mjCCDObj* obj) { // compute mean - mju_zero3(center->v); + mju_zero3(res); for (int i=0; i < 6; i++) { - mju_addTo3(center->v, p->v[i]); + mju_addTo3(res, obj->prism[i]); } - mju_scl3(center->v, center->v, 1.0/6.0); + mju_scl3(res, res, 1.0/6.0); +} + +// ccd prism center function +static void mjccd_prism_center(const void *obj, ccd_vec3_t *center) { + mjc_prism_center(center->v, (const mjCCDObj*) obj); } @@ -636,17 +637,17 @@ static void prism_firstdir(const void* o1, const void* o2, ccd_vec3_t *vec) { // add vertex to prism, count vertices -static void addVert(int* nvert, mjtPrism* prism, mjtNum x, mjtNum y, mjtNum z) { +static void addVert(int* nvert, mjCCDObj* obj, mjtNum x, mjtNum y, mjtNum z) { // move old data - mju_copy3(prism->v[0], prism->v[1]); - mju_copy3(prism->v[1], prism->v[2]); - mju_copy3(prism->v[3], prism->v[4]); - mju_copy3(prism->v[4], prism->v[5]); + mju_copy3(obj->prism[0], obj->prism[1]); + mju_copy3(obj->prism[1], obj->prism[2]); + mju_copy3(obj->prism[3], obj->prism[4]); + mju_copy3(obj->prism[4], obj->prism[5]); // add new vertex at last position - prism->v[2][0] = prism->v[5][0] = x; - prism->v[2][1] = prism->v[5][1] = y; - prism->v[5][2] = z; + obj->prism[2][0] = obj->prism[5][0] = x; + obj->prism[2][1] = obj->prism[5][1] = y; + obj->prism[5][2] = z; // count (*nvert)++; @@ -665,12 +666,15 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, int ncol = m->hfield_ncol[hid]; int dr[2], cnt, rmin, rmax, cmin, cmax; const float* data = m->hfield_data + m->hfield_adr[hid]; - mjtPrism prism; + mjCCDObj obj1; + obj1.center = mjc_prism_center; + obj1.support = mjc_prism_support; // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; // point size1 to hfield size instead of geom1 size @@ -713,32 +717,32 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // get support point in +X ccdVec3Set(&dirccd, 1, 0, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); xmax = vecccd.v[0]; // get support point in -X ccdVec3Set(&dirccd, -1, 0, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); xmin = vecccd.v[0]; // get support point in +Y ccdVec3Set(&dirccd, 0, 1, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); ymax = vecccd.v[1]; // get support point in -Y ccdVec3Set(&dirccd, 0, -1, 0); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); ymin = vecccd.v[1]; // get support point in +Z ccdVec3Set(&dirccd, 0, 0, 1); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); zmax = vecccd.v[2]; // get support point in -Z ccdVec3Set(&dirccd, 0, 0, -1); - mjccd_support(&obj, &dirccd, &vecccd); + mjccd_support(&obj2, &dirccd, &vecccd); zmin = vecccd.v[2]; // box-box test @@ -767,13 +771,13 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, // init ccd structure mjc_initCCD(&ccd, m); ccd.first_dir = prism_firstdir; - ccd.center1 = prism_center; + ccd.center1 = mjccd_prism_center; ccd.center2 = mjccd_center; - ccd.support1 = prism_support; + ccd.support1 = mjccd_prism_support; ccd.support2 = mjccd_support; // geom margin needed for actual collision test - obj.margin = margin; + obj2.margin = margin; // compute real-valued grid step, and triangulation direction dx = (2.0*size1[0]) / (ncol-1); @@ -782,7 +786,7 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, dr[1] = 0; // set zbottom value using base size - prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -size1[3]; + obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -size1[3]; // process all prisms in sub-grid cnt = 0; @@ -791,19 +795,20 @@ int mjc_ConvexHField(const mjModel* m, const mjData* d, for (int c=cmin; c <= cmax; c++) { for (int i=0; i < 2; i++) { // send vertex to prism constructor - addVert(&nvert, &prism, dx*c-size1[0], dy*(r+dr[i])-size1[1], + addVert(&nvert, &obj1, dx*c-size1[0], dy*(r+dr[i])-size1[1], data[(r+dr[i])*ncol+c]*size1[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test - if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { + if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin + && obj1.prism[5][2] < zmin) { continue; } // run MPR, save contact - if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0 && - !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { + if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0 + && !ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; mju_mulMatVec3(con[cnt].frame, mat1, dirccd.v); @@ -1103,8 +1108,10 @@ void mjc_fixNormal(const mjModel* m, const mjData* d, mjContact* con, int g1, in int mjc_ConvexElem(const mjModel* m, const mjData* d, mjContact* con, int g1, int f1, int e1, int v1, int f2, int e2, mjtNum margin) { ccd_t ccd; - mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, f1, e1, v1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, -1, -1, f2, e2, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; // init ccd structure mjc_initCCD(&ccd, m); @@ -1128,7 +1135,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, mjtNum vec[3], dx, dy; mjtNum xmin, xmax, ymin, ymax, zmin, zmax; int dr[2], cnt, rmin, rmax, cmin, cmax; - mjtPrism prism; + mjCCDObj obj1; + obj1.center = mjc_prism_center; + obj1.support = mjc_prism_support; // get hfield info int hid = m->geom_dataid[g]; @@ -1151,7 +1160,8 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // ccd-related ccd_vec3_t dirccd, vecccd; ccd_real_t depth; - mjCCDObj obj = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj2 = {m, d, -1, -1, f, e, -1, margin, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; //------------------------------------- AABB computation, box-box test @@ -1211,9 +1221,9 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, // init ccd structure CCD_INIT(&ccd); ccd.first_dir = prism_firstdir; - ccd.center1 = prism_center; + ccd.center1 = mjccd_prism_center; ccd.center2 = mjccd_center; - ccd.support1 = prism_support; + ccd.support1 = mjccd_prism_support; ccd.support2 = mjccd_support; // set ccd parameters @@ -1227,7 +1237,7 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, dr[1] = 0; // set zbottom value using base size - prism.v[0][2] = prism.v[1][2] = prism.v[2][2] = -hsize[3]; + obj1.prism[0][2] = obj1.prism[1][2] = obj1.prism[2][2] = -hsize[3]; // process all prisms in sub-grid cnt = 0; @@ -1236,18 +1246,18 @@ int mjc_HFieldElem(const mjModel* m, const mjData* d, mjContact* con, for (int c=cmin; c <= cmax; c++) { for (int k=0; k < 2; k++) { // send vertex to prism constructor - addVert(&nvert, &prism, dx*c-hsize[0], dy*(r+dr[k])-hsize[1], + addVert(&nvert, &obj1, dx*c-hsize[0], dy*(r+dr[k])-hsize[1], hdata[(r+dr[k])*ncol+c]*hsize[2]+margin); // check for enough vertices if (nvert > 2) { // prism height test - if (prism.v[3][2] < zmin && prism.v[4][2] < zmin && prism.v[5][2] < zmin) { + if (obj1.prism[3][2] < zmin && obj1.prism[4][2] < zmin && obj1.prism[5][2] < zmin) { continue; } // run MPR, save contact - if (_mjCCDPENETRATION(&prism, &obj, &ccd, &depth, &dirccd, &vecccd) == 0) { + if (_mjCCDPENETRATION(&obj1, &obj2, &ccd, &depth, &dirccd, &vecccd) == 0) { if (!ccdVec3Eq(&dirccd, ccd_vec3_origin)) { // fill in contact data, transform to global coordinates con[cnt].dist = -depth; diff --git a/src/engine/engine_collision_convex.h b/src/engine/engine_collision_convex.h index 24f38648..9c27623f 100644 --- a/src/engine/engine_collision_convex.h +++ b/src/engine/engine_collision_convex.h @@ -52,6 +52,9 @@ struct _mjCCDObj { mjtNum margin; mjtNum rotate[4]; mjtNum x0[3]; // initial guess of the witness point + void (*center)(mjtNum res[3], const struct _mjCCDObj* obj); + void (*support)(mjtNum res[3], struct _mjCCDObj* obj, const mjtNum dir[3]); + mjtNum prism[6][3]; // for hfield }; typedef struct _mjCCDObj mjCCDObj; diff --git a/src/engine/engine_collision_gjk.c b/src/engine/engine_collision_gjk.c index 55f202e9..2118d67a 100644 --- a/src/engine/engine_collision_gjk.c +++ b/src/engine/engine_collision_gjk.c @@ -203,8 +203,8 @@ static void gjk_support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* ob mju_scl3(dir, dir_neg, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) - mjc_support(s1, obj1, dir); - mjc_support(s2, obj2, dir_neg); + obj1->support(s1, obj1, dir); + obj2->support(s2, obj2, dir_neg); } @@ -218,8 +218,8 @@ static void support(mjtNum s1[3], mjtNum s2[3], mjCCDObj* obj1, mjCCDObj* obj2, mju_scl3(dir_neg, dir, -1); // compute S_{A-B}(dir) = S_A(dir) - S_B(-dir) - mjc_support(s1, obj1, dir); - mjc_support(s2, obj2, dir_neg); + obj1->support(s1, obj1, dir); + obj2->support(s2, obj2, dir_neg); } @@ -1007,8 +1007,8 @@ int mj_gjkPenetration(const void *obj1, const void *obj2, const ccd_t *ccd, mjCCDObj* o2 = (mjCCDObj*) obj2; nearest.n[1] = 34; - mjc_center(o1->x0, o1); - mjc_center(o2->x0, o2); + o1->center(o1->x0, o1); + o2->center(o2->x0, o2); config.max_iterations = ccd->max_iterations; config.tolerance = ccd->mpr_tolerance; diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 8ced09d0..f7a30e1b 100644 --- a/test/engine/engine_collision_gjk_test.cc +++ b/test/engine/engine_collision_gjk_test.cc @@ -50,8 +50,10 @@ void mjccd_support(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *vec) { mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum x2[3]) { mjCCDConfig config = {kMaxIterations, kTolerance}; - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; mjc_center(obj1.x0, &obj1); mjc_center(obj2.x0, &obj2); mjtNum dist = mj_gjk(&config, &obj1, &obj2); @@ -63,8 +65,10 @@ mjtNum run_gjk(mjModel* m, mjData* d, int g1, int g2, mjtNum x1[3], mjtNum run_gjkPenetration(mjModel* m, mjData* d, int g1, int g2, mjtNum dir[3] = nullptr, mjtNum pos[3] = nullptr) { - mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; - mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}}; + mjCCDObj obj1 = {m, d, g1, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; + mjCCDObj obj2 = {m, d, g2, -1, -1, -1, -1, 0, {1, 0, 0, 0}, {0, 0, 0}, + mjc_center, mjc_support}; ccd_t ccd; ccd.mpr_tolerance = kTolerance; ccd.epa_tolerance = kTolerance; @@ -198,6 +202,31 @@ TEST_F(MjGjkTest, EllipsoidEllipsoid) { mj_deleteModel(model); } +TEST_F(MjGjkTest, EllipsoidEllipsoidIntersect) { + static constexpr char xml[] = R"( + + + + + + )"; + + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << "Failed to load model: " << error.data(); + + mjData* data = mj_makeData(model); + mj_forward(model, data); + + int geom1 = mj_name2id(model, mjOBJ_GEOM, "geom1"); + int geom2 = mj_name2id(model, mjOBJ_GEOM, "geom2"); + mjtNum dist = run_gjk(model, data, geom1, geom2, nullptr, nullptr); + + EXPECT_NEAR(dist, 0, kTolerance); + mj_deleteData(data); + mj_deleteModel(model); +} + TEST_F(MjGjkTest, CapsuleCapsule) { static constexpr char xml[] = R"( From e0e134ca6397c1b78795606a5c2c545466d93ea2 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 15 Aug 2024 10:18:56 -0700 Subject: [PATCH 14/24] Add plugin tests for mjSpec. Fixes #1903. PiperOrigin-RevId: 663352920 Change-Id: I3a9e17aec5694e49c20b542f4b0a4402f88a2f17 --- .../mujoco/codegen/generate_spec_bindings.py | 6 ++-- python/mujoco/specs.cc | 9 +++-- python/mujoco/specs_test.py | 30 ++++++++++++++++ src/user/user_api.cc | 3 ++ test/user/user_api_test.cc | 36 +++++++++++++++++++ 5 files changed, 79 insertions(+), 5 deletions(-) diff --git a/python/mujoco/codegen/generate_spec_bindings.py b/python/mujoco/codegen/generate_spec_bindings.py index ef4aedf4..50b40d1b 100644 --- a/python/mujoco/codegen/generate_spec_bindings.py +++ b/python/mujoco/codegen/generate_spec_bindings.py @@ -39,8 +39,10 @@ def _value_binding_code( fullvarname = 'ptr->' + varname if field.name.startswith('mjs'): # all other mjs are raw structs fulltype = field.name.replace('mjs', 'raw::Mjs') - if field.name != 'mjsPlugin' and field.name != 'mjsOrientation': - fulltype = fulltype + '*' # plugin and orientation are pointers + if field.name == 'mjsPlugin' or field.name == 'mjsOrientation': + fulltype = fulltype + '&' # plugin and orientation are not pointers + else: + fulltype = fulltype + '*' def_property_args = ( f'"{varname}"', diff --git a/python/mujoco/specs.cc b/python/mujoco/specs.cc index c280911e..0f4ab874 100644 --- a/python/mujoco/specs.cc +++ b/python/mujoco/specs.cc @@ -1017,9 +1017,12 @@ PYBIND11_MODULE(_specs, m) { mjsTuple.def("delete", [](raw::MjsTuple& self) { mjs_delete(self.element); }); // ============================= MJSPLUGIN =================================== - mjsPlugin.def_property_readonly("id", [](raw::MjsPlugin& self) -> int { - return mjs_getId(self.instance); - }); + mjsPlugin.def_property( + "id", + [](raw::MjsPlugin& self) -> int { return mjs_getId(self.instance); }, + [](raw::MjsPlugin& self, raw::MjsPlugin* other) { + self.instance = other->instance; + }); mjsPlugin.def("delete", [](raw::MjsPlugin& self) { mjs_delete(self.instance); }); diff --git a/python/mujoco/specs_test.py b/python/mujoco/specs_test.py index ad068f91..a860f443 100644 --- a/python/mujoco/specs_test.py +++ b/python/mujoco/specs_test.py @@ -322,5 +322,35 @@ class SpecsTest(absltest.TestCase): self.assertEqual(model.nsite, 10) self.assertEqual(model.nsensor, 9) + def test_plugin(self): + xml = """ + + + + + + """ + + spec = mujoco.MjSpec() + spec.from_string(xml) + self.assertIsNotNone(spec.worldbody) + + body = spec.worldbody.add_body() + body.plugin.name = 'mujoco.elasticity.cable' + body.plugin.id = spec.add_plugin() + body.plugin.active = True + self.assertEqual(body.plugin.id, 0) + + geom = body.add_geom() + geom.type = mujoco.mjtGeom.mjGEOM_BOX + geom.size[0] = 1 + geom.size[1] = 1 + geom.size[2] = 1 + + model = spec.compile() + self.assertIsNotNone(model) + self.assertEqual(model.nplugin, 1) + self.assertEqual(model.body_plugin[1], 0) + if __name__ == '__main__': absltest.main() diff --git a/src/user/user_api.cc b/src/user/user_api.cc index bf3a98c6..4743f83b 100644 --- a/src/user/user_api.cc +++ b/src/user/user_api.cc @@ -606,6 +606,9 @@ const char* mjs_resolveOrientation(double quat[4], mjtByte degree, const char* s // get id int mjs_getId(mjsElement* element) { + if (!element) { + return -1; + } return static_cast(element)->id; } diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index 5eb68356..346f179d 100644 --- a/test/user/user_api_test.cc +++ b/test/user/user_api_test.cc @@ -20,6 +20,7 @@ #include #include #include +#include #include #include @@ -104,6 +105,41 @@ TEST_F(MujocoTest, TreeTraversal) { mj_deleteSpec(spec); } +TEST_F(PluginTest, ActivatePlugin) { + std::string plugin_name = "mujoco.elasticity.cable"; + mjSpec* spec = mj_makeSpec(); + + // get slot of requested plugin + int plugin_slot = -1; + const mjpPlugin* plugin = mjp_getPlugin(plugin_name.c_str(), &plugin_slot); + EXPECT_THAT(plugin, NotNull()); + + // activated plugin in the slot + std::vector> active_plugins; + active_plugins.emplace_back(std::make_pair(plugin, plugin_slot)); + mjs_setActivePlugins(spec, &active_plugins); + + // associate plugin to body + mjsBody* body = mjs_addBody(mjs_findBody(spec, "world"), 0); + mjs_setString(body->plugin.name, plugin_name.c_str()); + body->plugin.instance = mjs_addPlugin(spec)->instance; + body->plugin.active = true; + mjsGeom* geom = mjs_addGeom(body, 0); + geom->type = mjGEOM_BOX; + geom->size[0] = 1; + geom->size[1] = 1; + geom->size[2] = 1; + + // compile and check that the plugin is present + mjModel* model = mj_compile(spec, NULL); + EXPECT_THAT(model, NotNull()); + EXPECT_THAT(model->nplugin, 1); + EXPECT_THAT(model->body_plugin[1], 0); + + mj_deleteSpec(spec); + mj_deleteModel(model); +} + // ------------------- test recompilation multiple files ----------------------- TEST_F(PluginTest, RecompileCompare) { mjtNum tol = 0; From a74c184f9d6ead05b0c2193a79746ee0fa19c019 Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Thu, 15 Aug 2024 10:32:52 -0700 Subject: [PATCH 15/24] Add efc_pos to MJX. fixes #1388 PiperOrigin-RevId: 663358494 Change-Id: I63885f9208d12edbaa4b7d123ce4c6d16f40dcbe --- doc/changelog.rst | 4 ++++ mjx/mujoco/mjx/_src/constraint.py | 8 ++++---- mjx/mujoco/mjx/_src/constraint_test.py | 2 ++ mjx/mujoco/mjx/_src/io.py | 10 +++++++++- mjx/mujoco/mjx/_src/types.py | 2 ++ 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 80294d25..8a29045c 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -11,6 +11,10 @@ General 2. :ref:`shellinertia ` is now supported by all geom types. 3. Added support for :ref:`attaching` keyframes. +MJX +^^^ +4. Added ``efc_pos`` to ``mjx.Data``. + Version 3.2.2 (Aug 8, 2024) --------------------------- diff --git a/mjx/mujoco/mjx/_src/constraint.py b/mjx/mujoco/mjx/_src/constraint.py index eb16863d..8f0bbf92 100644 --- a/mjx/mujoco/mjx/_src/constraint.py +++ b/mjx/mujoco/mjx/_src/constraint.py @@ -529,7 +529,7 @@ def make_constraint(m: Model, d: Data) -> Data: if not efcs: z = jp.empty(0) d = d.replace(efc_J=jp.empty((0, m.nv))) - d = d.replace(efc_D=z, efc_aref=z, efc_frictionloss=z) + d = d.replace(efc_D=z, efc_aref=z, efc_frictionloss=z, efc_pos=z) return d efc = jax.tree_util.tree_map(lambda *x: jp.concatenate(x), *efcs) @@ -539,10 +539,10 @@ def make_constraint(m: Model, d: Data) -> Data: k, b, imp = _kbi(m, efc.solref, efc.solimp, efc.pos_imp) r = jp.maximum(efc.invweight * (1 - imp) / imp, mujoco.mjMINVAL) aref = -b * (efc.J @ d.qvel) - k * imp * efc.pos_aref - return aref, r + return aref, r, efc.pos_aref - aref, r = fn(efc) - d = d.replace(efc_J=efc.J, efc_D=1 / r, efc_aref=aref) + aref, r, pos = fn(efc) + d = d.replace(efc_J=efc.J, efc_D=1 / r, efc_aref=aref, efc_pos=pos) d = d.replace(efc_frictionloss=jp.zeros_like(r)) return d diff --git a/mjx/mujoco/mjx/_src/constraint_test.py b/mjx/mujoco/mjx/_src/constraint_test.py index db13e699..07a57475 100644 --- a/mjx/mujoco/mjx/_src/constraint_test.py +++ b/mjx/mujoco/mjx/_src/constraint_test.py @@ -65,6 +65,8 @@ class ConstraintTest(parameterized.TestCase): _assert_eq(d.efc_aref, dx.efc_aref[order][:d.nefc], 'efc_aref') _assert_eq(0, dx.efc_aref[order][d.nefc:], 'efc_aref') _assert_eq(d.efc_D, dx.efc_D[order][:d.nefc], 'efc_D') + _assert_eq(d.efc_pos, dx.efc_pos[order][:d.nefc], 'efc_pos') + def test_disable_refsafe(self): m = test_util.load_test_file('constraints.xml') diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index 51001559..d10a4760 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -270,6 +270,7 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: contact=contact, efc_type=efc_type, efc_J=jp.zeros((nefc, m.nv), dtype=float), + efc_pos=jp.zeros((nefc,), dtype=float), efc_frictionloss=jp.zeros((nefc,), dtype=float), efc_D=jp.zeros((nefc,), dtype=float), efc_aref=jp.zeros((nefc,), dtype=float), @@ -461,7 +462,14 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: fields['efc_J'] = fields['efc_J'].reshape((-1 if m.nv else 0, m.nv)) # move efc rows to their correct offsets - for fname in ('efc_J', 'efc_frictionloss', 'efc_D', 'efc_aref', 'efc_force'): + for fname in ( + 'efc_J', + 'efc_pos', + 'efc_frictionloss', + 'efc_D', + 'efc_aref', + 'efc_force', + ): value = np.zeros((nefc, m.nv)) if fname == 'efc_J' else np.zeros(nefc) for i in range(3): value_beg = sum([ne, nf][:i]) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index d5c3bdcc..568cec28 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -1137,6 +1137,7 @@ class Data(PyTreeNode): contact: all detected contacts (ncon,) efc_type: constraint type (nefc,) efc_J: constraint Jacobian (nefc, nv) + efc_pos: constraint position (equality, contact) (nefc,) efc_frictionloss: frictionloss (friction) (nefc,) efc_D: constraint mass (nefc,) efc_aref: reference pseudo-acceleration (nefc,) @@ -1257,6 +1258,7 @@ class Data(PyTreeNode): # dynamically sized - position dependent: efc_type: jax.Array efc_J: jax.Array # pylint:disable=invalid-name + efc_pos: jax.Array efc_frictionloss: jax.Array efc_D: jax.Array # pylint:disable=invalid-name # dynamically sized - position & velocity dependent: From 9924d9ca897e9ab83251acc87486bd67143422b5 Mon Sep 17 00:00:00 2001 From: Michael Ahn Date: Thu, 15 Aug 2024 20:36:56 -0700 Subject: [PATCH 16/24] Fixed incorrect mesh face circumradius calculation - In 8e2f830, this was changed from `mju_norm3` to `mjuu_normvec`, but the latter will mutate its input to be normalized instead of just returning the norm (the former was likely mistaken as `mju_normalize3` which does mutate its input). --- src/user/user_mesh.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/user/user_mesh.cc b/src/user/user_mesh.cc index 6a637f40..22fc1ca6 100644 --- a/src/user/user_mesh.cc +++ b/src/user/user_mesh.cc @@ -1915,7 +1915,7 @@ void mjCMesh::MakeCenter(void) { // compute circumradius double norm_a_2 = mjuu_dot3(a, a); double norm_b_2 = mjuu_dot3(b, b); - double area = mjuu_normvec(nrm, 3); + double area = sqrt(mjuu_dot3(nrm, nrm)); // compute circumcenter double res[3], vec[3] = { From 6db96e07e43f441a89cdc129ac751b6d8d7567ef Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 16 Aug 2024 03:09:23 -0700 Subject: [PATCH 17/24] Add position-dependent sensors to MJX: magnetometer, ballquat, subtreecom, framepos, framexaxis, frameyaxis, framezaxis, and clock. PiperOrigin-RevId: 663668212 Change-Id: Ifc9adc6bff4544172ab22572da04a093aebfdbd4 --- doc/changelog.rst | 2 + mjx/mujoco/mjx/_src/sensor.py | 121 ++++++++++++++++++++++++---- mjx/mujoco/mjx/_src/types.py | 35 ++++++++ mjx/mujoco/mjx/test_data/sensor.xml | 56 +++++++++++-- 4 files changed, 195 insertions(+), 19 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 8a29045c..9504fd2d 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -14,6 +14,8 @@ General MJX ^^^ 4. Added ``efc_pos`` to ``mjx.Data``. +5. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, + ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``SUBTREECOM``, ``CLOCK``. Version 3.2.2 (Aug 8, 2024) --------------------------- diff --git a/mjx/mujoco/mjx/_src/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index a59e9c58..b92c32e0 100644 --- a/mjx/mujoco/mjx/_src/sensor.py +++ b/mjx/mujoco/mjx/_src/sensor.py @@ -15,11 +15,14 @@ """Sensor functions.""" import jax +from jax import numpy as jp +import mujoco # pylint: disable=g-importing-member +from mujoco.mjx._src import math from mujoco.mjx._src.types import Data from mujoco.mjx._src.types import Model +from mujoco.mjx._src.types import ObjType from mujoco.mjx._src.types import SensorType -from typing import Tuple # pylint: enable=g-importing-member import numpy as np @@ -27,19 +30,109 @@ import numpy as np def sensor_pos(m: Model, d: Data) -> Data: """Compute position-dependent sensors values.""" - sensordata = d.sensordata - if np.isin(SensorType.JOINTPOS, m.sensor_type): - # jointpos - i = m.sensor_type == SensorType.JOINTPOS - objid = m.sensor_objid[i] - adr = m.sensor_adr[i] - sensordata = sensordata.at[adr].set(d.qpos[m.jnt_qposadr[objid]]) - if np.isin(SensorType.ACTUATORPOS, m.sensor_type): - # actuatorpos - i = m.sensor_type == SensorType.ACTUATORPOS - objid = m.sensor_objid[i] - adr = m.sensor_adr[i] - sensordata = sensordata.at[adr].set(d.actuator_length[objid]) + # no position-dependent sensors + if sum(m.sensor_needstage == mujoco.mjtStage.mjSTAGE_POS) == 0: + return d + + # position and orientation by object type + objtype_data = { + ObjType.UNKNOWN: ( + np.expand_dims(np.eye(3), axis=0), + np.zeros((1, 3)), + ), # world + ObjType.BODY: (d.xipos, d.ximat), + ObjType.XBODY: (d.xpos, d.xmat), + ObjType.GEOM: (d.geom_xpos, d.geom_xmat), + ObjType.SITE: (d.site_xpos, d.site_xmat), + ObjType.CAMERA: (d.cam_xpos, d.cam_xmat), + } + + # frame axis indexing + frame_axis = { + SensorType.FRAMEXAXIS: 0, + SensorType.FRAMEYAXIS: 1, + SensorType.FRAMEZAXIS: 2, + } + + sensors, adrs = [], [] + + for sensor_type in set(m.sensor_type): + idx = m.sensor_type == sensor_type + objid = m.sensor_objid[idx] + adr = m.sensor_adr[idx] + + if sensor_type == SensorType.MAGNETOMETER: + sensor = jax.vmap(lambda xmat: xmat.T @ m.opt.magnetic)( + d.site_xmat[objid] + ).reshape(-1) + adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) + elif sensor_type == SensorType.JOINTPOS: + sensor = d.qpos[m.jnt_qposadr[objid]] + elif sensor_type == SensorType.ACTUATORPOS: + sensor = d.actuator_length[objid] + elif sensor_type == SensorType.BALLQUAT: + jnt_qposadr = m.jnt_qposadr[objid, None] + np.arange(4)[None] + quat = d.qpos[jnt_qposadr] + sensor = jax.vmap(math.normalize)(quat).reshape(-1) + adr = (adr[:, None] + np.arange(4)[None]).reshape(-1) + elif sensor_type == SensorType.FRAMEPOS: + + def _framepos(xpos, xpos_ref, xmat_ref, refid): + return jp.where(refid == -1, xpos, xmat_ref.T @ (xpos - xpos_ref)) + + objtype = m.sensor_objtype[idx] + reftype = m.sensor_reftype[idx] + refid = m.sensor_refid[idx] + + # evaluate for valid object and reference object type pairs + for ot, rt in set(zip(objtype, reftype)): + id_ = (objtype == ot) & (reftype == rt) + refid_ = refid[id_] + xpos, _ = objtype_data[ot] + xpos_ref, xmat_ref = objtype_data[rt] + xpos = xpos[objid[id_]] + xpos_ref = xpos_ref[refid_] + xmat_ref = xmat_ref[refid_] + sensor = jax.vmap(_framepos)(xpos, xpos_ref, xmat_ref, refid_) + adr_ = adr[id_, None] + np.arange(3)[None] + sensors.append(sensor.reshape(-1)) + adrs.append(adr_.reshape(-1)) + continue # avoid adding to sensors/adrs list a second time + elif sensor_type in frame_axis: + + def _frameaxis(xmat, xmat_ref, refid): + axis = xmat[:, frame_axis[sensor_type]] + return jp.where(refid == -1, axis, xmat_ref.T @ axis) + + objtype = m.sensor_objtype[idx] + reftype = m.sensor_reftype[idx] + refid = m.sensor_refid[idx] + + # evaluate for valid object and reference object type pairs + for ot, rt in set(zip(objtype, reftype)): + id_ = (objtype == ot) & (reftype == rt) + refid_ = refid[id_] + _, xmat = objtype_data[ot] + _, xmat_ref = objtype_data[rt] + xmat = xmat[objid[id_]] + xmat_ref = xmat_ref[refid_] + sensor = jax.vmap(_frameaxis)(xmat, xmat_ref, refid_) + adr_ = adr[id_, None] + np.arange(3)[None] + sensors.append(sensor.reshape(-1)) + adrs.append(adr_.reshape(-1)) + continue # avoid adding to sensors/adrs list a second time + elif sensor_type == SensorType.SUBTREECOM: + sensor = d.subtree_com[objid].reshape(-1) + adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) + elif sensor_type == SensorType.CLOCK: + sensor = jp.repeat(d.time, sum(idx)) + + sensors.append(sensor) + adrs.append(adr) + + sensordata = d.sensordata.at[np.concatenate(adrs)].set( + jp.concatenate(sensors) + ) return d.replace(sensordata=sensordata) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 568cec28..8b690c11 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -282,11 +282,46 @@ class SensorType(enum.IntEnum): """Type of sensor. Members: + MAGNETOMETER: magnetometer JOINTPOS: joint position ACTUATORPOS: actuator position + BALLQUAT: ball joint orientation + FRAMEPOS: frame position + FRAMEXAXIS: frame x-axis + FRAMEYAXIS: frame y-axis + FRAMEZAXIS: frame z-axis + SUBTREECOM: subtree centor of mass + CLOCK: simulation time """ + MAGNETOMETER = mujoco.mjtSensor.mjSENS_MAGNETOMETER JOINTPOS = mujoco.mjtSensor.mjSENS_JOINTPOS ACTUATORPOS = mujoco.mjtSensor.mjSENS_ACTUATORPOS + BALLQUAT = mujoco.mjtSensor.mjSENS_BALLQUAT + FRAMEPOS = mujoco.mjtSensor.mjSENS_FRAMEPOS + FRAMEXAXIS = mujoco.mjtSensor.mjSENS_FRAMEXAXIS + FRAMEYAXIS = mujoco.mjtSensor.mjSENS_FRAMEYAXIS + FRAMEZAXIS = mujoco.mjtSensor.mjSENS_FRAMEZAXIS + SUBTREECOM = mujoco.mjtSensor.mjSENS_SUBTREECOM + CLOCK = mujoco.mjtSensor.mjSENS_CLOCK + + +class ObjType(PyTreeNode): + """Type of object. + + Members: + UNKNOWN: unknown object type + BODY: body + XBODY: body, used to access regular frame instead of i-frame + GEOM: geom + SITE: site + CAMERA: camera + """ + UNKNOWN = mujoco.mjtObj.mjOBJ_UNKNOWN + BODY = mujoco.mjtObj.mjOBJ_BODY + XBODY = mujoco.mjtObj.mjOBJ_XBODY + GEOM = mujoco.mjtObj.mjOBJ_GEOM + SITE = mujoco.mjtObj.mjOBJ_SITE + CAMERA = mujoco.mjtObj.mjOBJ_CAMERA class Option(PyTreeNode): diff --git a/mjx/mujoco/mjx/test_data/sensor.xml b/mjx/mujoco/mjx/test_data/sensor.xml index 810abd1d..13178604 100644 --- a/mjx/mujoco/mjx/test_data/sensor.xml +++ b/mjx/mujoco/mjx/test_data/sensor.xml @@ -1,28 +1,74 @@ - + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + From 0bcaa85650d56b5d5d4564d52e05563c00c3e965 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Aug 2024 04:06:30 -0700 Subject: [PATCH 18/24] Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit integrators, wrong derivatives would be computed. PiperOrigin-RevId: 663680592 Change-Id: I5d72888cab19dd5e51f552f63fe0968db93f5dcd --- doc/changelog.rst | 7 +++ src/engine/engine_derivative.c | 12 ++-- test/engine/engine_derivative_test.cc | 79 +++++++++++++++++++++++++++ 3 files changed, 93 insertions(+), 5 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 9504fd2d..ea4a2edb 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -17,6 +17,13 @@ MJX 5. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``SUBTREECOM``, ``CLOCK``. +Bug fixes +^^^^^^^^^ +6. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, + contribution by :github:user:`michael-ahn`). +7. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit + integrators, wrong derivatives would be computed. + Version 3.2.2 (Aug 8, 2024) --------------------------- diff --git a/src/engine/engine_derivative.c b/src/engine/engine_derivative.c index dec37eb8..b321ba88 100644 --- a/src/engine/engine_derivative.c +++ b/src/engine/engine_derivative.c @@ -338,7 +338,7 @@ static void mjd_comVel_vel_dense(const mjModel* m, mjData* d, mjtNum* Dcvel, mjt mju_zero(Dcvel, nbody*6*nv); // forward pass over bodies: accumulate Dcvel, set Dcdofdot - for (int i=1; i < m->nbody; i++) { + for (int i=1; i < nbody; i++) { // Dcvel = Dcvel_parent mju_copy(Dcvel+i*6*nv, Dcvel+m->body_parentid[i]*6*nv, 6*nv); @@ -450,7 +450,7 @@ void mjd_rne_vel_dense(const mjModel* m, mjData* d) { mju_zero(Dcfrcbody, 6*nv); // backward pass over bodies: accumulate Dcfrcbody - for (int i=m->nbody-1; i > 0; i--) { + for (int i=nbody-1; i > 0; i--) { if (m->body_parentid[i]) { mju_addTo(Dcfrcbody+m->body_parentid[i]*6*nv, Dcfrcbody+i*6*nv, 6*nv); } @@ -825,7 +825,7 @@ static mjtNum mjd_muscleGain_vel(mjtNum len, mjtNum vel, const mjtNum lengthrang // add (d qfrc_actuator / d qvel) to qDeriv void mjd_actuator_vel(const mjModel* m, mjData* d) { - int nv = m->nv; + int nv = m->nv, nu = m->nu; // disabled: nothing to add if (mjDISABLED(mjDSBL_ACTUATION)) { @@ -833,7 +833,7 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { } // process actuators - for (int i=0; i < m->nu; i++) { + for (int i=0; i < nu; i++) { // skip if disabled if (mj_actuatorDisabled(m, i)) { continue; @@ -867,7 +867,9 @@ void mjd_actuator_vel(const mjModel* m, mjData* d) { if (m->actuator_dyntype[i] == mjDYN_NONE) { bias_vel += gain_vel * d->ctrl[i]; } else { - bias_vel += gain_vel * d->act[i-(m->nu - m->na)]; + int act_first = m->actuator_actadr[i]; + int act_last = act_first + m->actuator_actnum[i] - 1; + bias_vel += gain_vel * d->act[act_last]; } } diff --git a/test/engine/engine_derivative_test.cc b/test/engine/engine_derivative_test.cc index 9a7a776e..778ddadb 100644 --- a/test/engine/engine_derivative_test.cc +++ b/test/engine/engine_derivative_test.cc @@ -223,6 +223,85 @@ TEST_F(DerivativeTest, DisabledActuators) { mj_deleteModel(m1); } +// actuator order has no effect +TEST_F(DerivativeTest, ActuatorOrder) { + // model with stateful actuator first + static constexpr char xml1[] = R"( + + + )"; + + char error[1024]; + mjModel* m1 = LoadModelFromString(xml1, error, sizeof(error)); + ASSERT_THAT(m1, NotNull()) << "Failed to load model: " << error; + mjData* d1 = mj_makeData(m1); + + d1->ctrl[0] = 6; + d1->ctrl[1] = 6; + + while (d1->time < 1) + mj_step(m1, d1); + + // model with stateful actuator second + static constexpr char xml2[] = R"( + + + )"; + + mjModel* m2 = LoadModelFromString(xml2, error, sizeof(error)); + ASSERT_THAT(m2, NotNull()) << "Failed to load model: " << error; + mjData* d2 = mj_makeData(m2); + + d2->ctrl[0] = 6; + d2->ctrl[1] = 6; + + while (d2->time < 1) + mj_step(m2, d2); + + // expect same qvel in both models + EXPECT_EQ(d1->qvel[0], d2->qvel[0]); + EXPECT_EQ(d1->qvel[1], d2->qvel[1]); + + mj_deleteData(d2); + mj_deleteModel(m2); + mj_deleteData(d1); + mj_deleteModel(m1); +} + // compare analytic and fin-diff d_qfrc_passive/d_qvel TEST_F(DerivativeTest, PassiveDvel) { for (const char* local_path : {kTumblingThinObjectPath, From cfc7dc984897e7e9efe912720672ae0f67f94dcf Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Fri, 16 Aug 2024 04:21:18 -0700 Subject: [PATCH 19/24] Clarify that keyframes are not replicated in nested attachments. PiperOrigin-RevId: 663683684 Change-Id: Id180b3a6afad6809d8189a7a525c474bd3a91fc1 --- doc/APIreference/APItypes.rst | 4 ++++ doc/XMLreference.rst | 7 +++++++ doc/changelog.rst | 5 +++++ 3 files changed, 16 insertions(+) diff --git a/doc/APIreference/APItypes.rst b/doc/APIreference/APItypes.rst index 61d2d7fa..9be6d9e4 100644 --- a/doc/APIreference/APItypes.rst +++ b/doc/APIreference/APItypes.rst @@ -1393,6 +1393,8 @@ Alternative orientation specifiers. .. _ArrayHandles: +.. _mjBuffer: + .. _mjString: .. _mjStringVec: @@ -1417,6 +1419,7 @@ C handles for C++ strings and vector types. When using from C, use the provided #ifdef __cplusplus // C++: defined to be compatible with corresponding std types + using mjBuffer = std::vector; using mjString = std::string; using mjStringVec = std::vector; using mjIntVec = std::vector; @@ -1426,6 +1429,7 @@ C handles for C++ strings and vector types. When using from C, use the provided using mjDoubleVec = std::vector; #else // C: opaque types + typedef void mjBuffer; typedef void mjString; typedef void mjStringVec; typedef void mjIntVec; diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 645ea9e2..e9c3f0fb 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -175,6 +175,12 @@ replicating 200 times, suffixes will be ``000, 001, ...`` etc). All referencing and namespaced appropriately. Detailed examples of models using replicate can be found in the `model/replicate/ `__ directory. +There is a caveat concerning :ref:`keyframes` when using replicate. Since :ref:`mjs_attachFrame` is used to +self-attach multiple times the enclosed kinematic tree, if this tree contains further :ref:`attach` +elements, keyframes will not be replicated nor namespaced by :ref:`replicate`, but they will be attached and +namespaced once by the innermost call of :ref:`mjs_attachFrame` or :ref:`mjs_attachBody`. See the limitations discussed +in :ref:`attach`. + .. _replicate-count: :at:`count`: :at-val:`int, required` @@ -3808,6 +3814,7 @@ all attachments will appear in the saved XML file. - An entire model cannot be attached (i.e. including all elements, referenced or not). - All assets from the child model will be copied in, whether they are referenced or not. - Self-attach or circular references are not checked for and will lead to infinite loops. + - :ref:`Keyframes` are attached once, so they are not replicated in nested attachments. .. _body-attach-model: diff --git a/doc/changelog.rst b/doc/changelog.rst index ea4a2edb..40713d29 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -24,6 +24,11 @@ Bug fixes 7. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit integrators, wrong derivatives would be computed. +Python bindings +^^^^^^^^^^^^^^^ +8. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). + + Version 3.2.2 (Aug 8, 2024) --------------------------- From e0e08aa597cc2f4fa0f3ecd59206566a01a8b2aa Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Aug 2024 05:33:45 -0700 Subject: [PATCH 20/24] Rename xml_native_writer variables to be consistent with xml_native_reader. PiperOrigin-RevId: 663699130 Change-Id: If11ed48d3b30d3c6d09ca77e3d8048c2a3d9f054 --- src/xml/xml_native_reader.cc | 4 +- src/xml/xml_native_writer.cc | 1092 +++++++++++++++++----------------- 2 files changed, 548 insertions(+), 548 deletions(-) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 433e33a7..5881ba4f 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -2304,7 +2304,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* actuator) { // make composite -void mjXReader::OneComposite(XMLElement* elem, mjsBody* pbody, mjsDefault* def) { +void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { string text; int n; @@ -2526,7 +2526,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* pbody, mjsDefault* def) // make composite char error[200]; - bool res = comp.Make(spec, pbody, error, 200); + bool res = comp.Make(spec, body, error, 200); // throw error if (!res) { diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 23c77c63..993d1a52 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -124,54 +124,54 @@ XMLElement* mjXWriter::InsertEnd(XMLElement* parent, const char* name) { //---------------------------------- class mjXWriter: one-element writers -------------------------- // write flex -void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) { +void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* flex) { std::string text; mjCFlex defflex; // common attributes - WriteAttrTxt(elem, "name", pflex->name); - WriteAttr(elem, "radius", 1, &pflex->radius, &defflex.radius); - if (pflex->get_material() != defflex.get_material()) { - WriteAttrTxt(elem, "material", pflex->get_material()); + WriteAttrTxt(elem, "name", flex->name); + WriteAttr(elem, "radius", 1, &flex->radius, &defflex.radius); + if (flex->get_material() != defflex.get_material()) { + WriteAttrTxt(elem, "material", flex->get_material()); } - WriteAttr(elem, "rgba", 4, pflex->rgba, defflex.rgba); - WriteAttrKey(elem, "flatskin", bool_map, 2, pflex->flatskin, defflex.flatskin); - WriteAttrInt(elem, "dim", pflex->dim, defflex.dim); - WriteAttrInt(elem, "group", pflex->group, defflex.group); + WriteAttr(elem, "rgba", 4, flex->rgba, defflex.rgba); + WriteAttrKey(elem, "flatskin", bool_map, 2, flex->flatskin, defflex.flatskin); + WriteAttrInt(elem, "dim", flex->dim, defflex.dim); + WriteAttrInt(elem, "group", flex->group, defflex.group); // data vectors - if (!pflex->get_vertbody().empty()) { - text = VectorToString(pflex->get_vertbody()); + if (!flex->get_vertbody().empty()) { + text = VectorToString(flex->get_vertbody()); WriteAttrTxt(elem, "body", text); } - if (!pflex->get_vert().empty()) { - text = VectorToString(pflex->get_vert()); + if (!flex->get_vert().empty()) { + text = VectorToString(flex->get_vert()); WriteAttrTxt(elem, "vertex", text); } - if (!pflex->get_elem().empty()) { - text = VectorToString(pflex->get_elem()); + if (!flex->get_elem().empty()) { + text = VectorToString(flex->get_elem()); WriteAttrTxt(elem, "element", text); } - if (!pflex->get_texcoord().empty()) { - text = VectorToString(pflex->get_texcoord()); + if (!flex->get_texcoord().empty()) { + text = VectorToString(flex->get_texcoord()); WriteAttrTxt(elem, "texcoord", text); } // contact subelement XMLElement* cont = InsertEnd(elem, "contact"); - WriteAttrInt(cont, "contype", pflex->contype, defflex.contype); - WriteAttrInt(cont, "conaffinity", pflex->conaffinity, defflex.conaffinity); - WriteAttrInt(cont, "condim", pflex->condim, defflex.condim); - WriteAttrInt(cont, "priority", pflex->priority, defflex.priority); - WriteAttr(cont, "friction", 3, pflex->friction, defflex.friction); - WriteAttr(cont, "solmix", 1, &pflex->solmix, &defflex.solmix); - WriteAttr(cont, "solref", mjNREF, pflex->solref, defflex.solref); - WriteAttr(cont, "solimp", mjNIMP, pflex->solimp, defflex.solimp); - WriteAttr(cont, "margin", 1, &pflex->margin, &defflex.margin); - WriteAttr(cont, "gap", 1, &pflex->gap, &defflex.gap); - WriteAttrKey(cont, "internal", bool_map, 2, pflex->internal, defflex.internal); - WriteAttrKey(cont, "selfcollide", flexself_map, 5, pflex->selfcollide, defflex.selfcollide); - WriteAttrInt(cont, "activelayers", pflex->activelayers, defflex.activelayers); + WriteAttrInt(cont, "contype", flex->contype, defflex.contype); + WriteAttrInt(cont, "conaffinity", flex->conaffinity, defflex.conaffinity); + WriteAttrInt(cont, "condim", flex->condim, defflex.condim); + WriteAttrInt(cont, "priority", flex->priority, defflex.priority); + WriteAttr(cont, "friction", 3, flex->friction, defflex.friction); + WriteAttr(cont, "solmix", 1, &flex->solmix, &defflex.solmix); + WriteAttr(cont, "solref", mjNREF, flex->solref, defflex.solref); + WriteAttr(cont, "solimp", mjNIMP, flex->solimp, defflex.solimp); + WriteAttr(cont, "margin", 1, &flex->margin, &defflex.margin); + WriteAttr(cont, "gap", 1, &flex->gap, &defflex.gap); + WriteAttrKey(cont, "internal", bool_map, 2, flex->internal, defflex.internal); + WriteAttrKey(cont, "selfcollide", flexself_map, 5, flex->selfcollide, defflex.selfcollide); + WriteAttrInt(cont, "activelayers", flex->activelayers, defflex.activelayers); // remove contact is no attributes if (!cont->FirstAttribute()) { @@ -180,8 +180,8 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) { // edge subelement XMLElement* edge = InsertEnd(elem, "edge"); - WriteAttr(edge, "stiffness", 1, &pflex->edgestiffness, &defflex.edgestiffness); - WriteAttr(edge, "damping", 1, &pflex->edgedamping, &defflex.edgedamping); + WriteAttr(edge, "stiffness", 1, &flex->edgestiffness, &defflex.edgestiffness); + WriteAttr(edge, "damping", 1, &flex->edgedamping, &defflex.edgedamping); // remove edge if no attributes if (!edge->FirstAttribute()) { @@ -192,99 +192,99 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) { // write mesh -void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* pmesh, mjCDef* def) { +void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) { std::string text; // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pmesh->name); - if (pmesh->classname != "main") { - WriteAttrTxt(elem, "class", pmesh->classname); + WriteAttrTxt(elem, "name", mesh->name); + if (mesh->classname != "main") { + WriteAttrTxt(elem, "class", mesh->classname); } - WriteAttrTxt(elem, "content_type", pmesh->ContentType()); - WriteAttrTxt(elem, "file", pmesh->File()); + WriteAttrTxt(elem, "content_type", mesh->ContentType()); + WriteAttrTxt(elem, "file", mesh->File()); // write vertex data - if (!pmesh->UserVert().empty()) { - text = VectorToString(pmesh->UserVert()); + if (!mesh->UserVert().empty()) { + text = VectorToString(mesh->UserVert()); WriteAttrTxt(elem, "vertex", text); } // write normal data - if (!pmesh->UserNormal().empty()) { - text = VectorToString(pmesh->UserNormal()); + if (!mesh->UserNormal().empty()) { + text = VectorToString(mesh->UserNormal()); WriteAttrTxt(elem, "normal", text); } // write texcoord data - if (!pmesh->UserTexcoord().empty()) { - text = VectorToString(pmesh->UserTexcoord()); + if (!mesh->UserTexcoord().empty()) { + text = VectorToString(mesh->UserTexcoord()); WriteAttrTxt(elem, "texcoord", text); } // write face data - if (!pmesh->UserFace().empty()) { - text = VectorToString(pmesh->UserFace()); + if (!mesh->UserFace().empty()) { + text = VectorToString(mesh->UserFace()); WriteAttrTxt(elem, "face", text); } } // defaults and regular - WriteAttr(elem, "refpos", 3, pmesh->Refpos(), def->Mesh().Refpos()); - WriteAttr(elem, "refquat", 4, pmesh->Refquat(), def->Mesh().Refquat()); - WriteAttr(elem, "scale", 3, pmesh->Scale(), def->Mesh().Scale()); - WriteAttrKey(elem, "smoothnormal", bool_map, 2, pmesh->SmoothNormal(), + WriteAttr(elem, "refpos", 3, mesh->Refpos(), def->Mesh().Refpos()); + WriteAttr(elem, "refquat", 4, mesh->Refquat(), def->Mesh().Refquat()); + WriteAttr(elem, "scale", 3, mesh->Scale(), def->Mesh().Scale()); + WriteAttrKey(elem, "smoothnormal", bool_map, 2, mesh->SmoothNormal(), def->Mesh().SmoothNormal()); } // write skin -void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* pskin) { +void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* skin) { std::string text; mjCDef mydef; float zero = 0; // write attributes - WriteAttrTxt(elem, "name", pskin->name); - WriteAttrTxt(elem, "file", pskin->File()); - WriteAttrTxt(elem, "material", pskin->get_material()); - WriteAttrInt(elem, "group", pskin->group, 0); - WriteAttr(elem, "rgba", 4, pskin->rgba, mydef.Geom().rgba); - WriteAttr(elem, "inflate", 1, &pskin->inflate, &zero); + WriteAttrTxt(elem, "name", skin->name); + WriteAttrTxt(elem, "file", skin->File()); + WriteAttrTxt(elem, "material", skin->get_material()); + WriteAttrInt(elem, "group", skin->group, 0); + WriteAttr(elem, "rgba", 4, skin->rgba, mydef.Geom().rgba); + WriteAttr(elem, "inflate", 1, &skin->inflate, &zero); // write data if no file - if (pskin->File().empty()) { + if (skin->File().empty()) { // mesh vert - text = VectorToString(pskin->get_vert()); + text = VectorToString(skin->get_vert()); WriteAttrTxt(elem, "vertex", text); // mesh texcoord - if (!pskin->get_texcoord().empty()) { - text = VectorToString(pskin->get_texcoord()); + if (!skin->get_texcoord().empty()) { + text = VectorToString(skin->get_texcoord()); WriteAttrTxt(elem, "texcoord", text); } // mesh face - text = VectorToString(pskin->get_face()); + text = VectorToString(skin->get_face()); WriteAttrTxt(elem, "face", text); // bones - for (size_t i=0; iget_bodyname().size(); i++) { + for (size_t i=0; iget_bodyname().size(); i++) { // make bone XMLElement* bone = InsertEnd(elem, "bone"); // write attributes - WriteAttrTxt(bone, "body", pskin->get_bodyname()[i]); - WriteAttr(bone, "bindpos", 3, pskin->get_bindpos().data()+3*i); - WriteAttr(bone, "bindquat", 4, pskin->get_bindquat().data()+4*i); + WriteAttrTxt(bone, "body", skin->get_bodyname()[i]); + WriteAttr(bone, "bindpos", 3, skin->get_bindpos().data()+3*i); + WriteAttr(bone, "bindquat", 4, skin->get_bindquat().data()+4*i); // write vertid - text = VectorToString(pskin->get_vertid()[i]); + text = VectorToString(skin->get_vertid()[i]); WriteAttrTxt(bone, "vertid", text); // write vertweight - text = VectorToString(pskin->get_vertweight()[i]); + text = VectorToString(skin->get_vertweight()[i]); WriteAttrTxt(bone, "vertweight", text); } } @@ -293,128 +293,128 @@ void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* pskin) { // write material -void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* pmat, mjCDef* def) { +void mjXWriter::OneMaterial(XMLElement* elem, const mjCMaterial* material, mjCDef* def) { // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pmat->name); - if (pmat->classname != "main") { - WriteAttrTxt(elem, "class", pmat->classname); + WriteAttrTxt(elem, "name", material->name); + if (material->classname != "main") { + WriteAttrTxt(elem, "class", material->classname); } } // defaults and regular bool has_non_rgb = false; for (int i=1; itextures_[i].empty()) { + if (!material->textures_[i].empty()) { if (i != mjTEXROLE_RGB) { has_non_rgb = true; } } - if (pmat->textures_[i] != def->Material().textures_[i]) { - WriteAttrTxt(elem, "texture", pmat->get_texture(i)); + if (material->textures_[i] != def->Material().textures_[i]) { + WriteAttrTxt(elem, "texture", material->get_texture(i)); } } if (has_non_rgb) { // // TODO elem = InsertEnd(section, "role"); throw mjXError(0, "no support for non-RGB textures."); } - WriteAttrKey(elem, "texuniform", bool_map, 2, pmat->texuniform, def->Material().texuniform); - WriteAttr(elem, "texrepeat", 2, pmat->texrepeat, def->Material().texrepeat); - WriteAttr(elem, "emission", 1, &pmat->emission, &def->Material().emission); - WriteAttr(elem, "specular", 1, &pmat->specular, &def->Material().specular); - WriteAttr(elem, "shininess", 1, &pmat->shininess, &def->Material().shininess); - WriteAttr(elem, "reflectance", 1, &pmat->reflectance, &def->Material().reflectance); - WriteAttr(elem, "metallic", 1, &pmat->metallic, &def->Material().metallic); - WriteAttr(elem, "roughness", 1, &pmat->roughness, &def->Material().roughness); - WriteAttr(elem, "rgba", 4, pmat->rgba, def->Material().rgba); + WriteAttrKey(elem, "texuniform", bool_map, 2, material->texuniform, def->Material().texuniform); + WriteAttr(elem, "texrepeat", 2, material->texrepeat, def->Material().texrepeat); + WriteAttr(elem, "emission", 1, &material->emission, &def->Material().emission); + WriteAttr(elem, "specular", 1, &material->specular, &def->Material().specular); + WriteAttr(elem, "shininess", 1, &material->shininess, &def->Material().shininess); + WriteAttr(elem, "reflectance", 1, &material->reflectance, &def->Material().reflectance); + WriteAttr(elem, "metallic", 1, &material->metallic, &def->Material().metallic); + WriteAttr(elem, "roughness", 1, &material->roughness, &def->Material().roughness); + WriteAttr(elem, "rgba", 4, material->rgba, def->Material().rgba); } // write joint -void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* pjoint, mjCDef* def, +void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def, std::string_view classname) { double zero = 0; // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pjoint->name); - if (classname != pjoint->classname && pjoint->classname != "main") { - WriteAttrTxt(elem, "class", pjoint->classname); + WriteAttrTxt(elem, "name", joint->name); + if (classname != joint->classname && joint->classname != "main") { + WriteAttrTxt(elem, "class", joint->classname); } - if (pjoint->type != mjJNT_FREE) { - WriteAttr(elem, "pos", 3, pjoint->pos); + if (joint->type != mjJNT_FREE) { + WriteAttr(elem, "pos", 3, joint->pos); } - if (pjoint->type != mjJNT_FREE && pjoint->type != mjJNT_BALL) { - WriteAttr(elem, "axis", 3, pjoint->axis); + if (joint->type != mjJNT_FREE && joint->type != mjJNT_BALL) { + WriteAttr(elem, "axis", 3, joint->axis); } } // defaults and regular - if (pjoint->type != def->Joint().type) { - WriteAttrTxt(elem, "type", FindValue(joint_map, joint_sz, pjoint->type)); + if (joint->type != def->Joint().type) { + WriteAttrTxt(elem, "type", FindValue(joint_map, joint_sz, joint->type)); } - WriteAttrInt(elem, "group", pjoint->group, def->Joint().group); - WriteAttr(elem, "ref", 1, &pjoint->ref, &zero); - WriteAttr(elem, "springref", 1, &pjoint->springref, &zero); - WriteAttr(elem, "solreflimit", mjNREF, pjoint->solref_limit, def->Joint().solref_limit, true); - WriteAttr(elem, "solimplimit", mjNIMP, pjoint->solimp_limit, def->Joint().solimp_limit, true); - WriteAttr(elem, "solreffriction", mjNREF, pjoint->solref_friction, def->Joint().solref_friction, + WriteAttrInt(elem, "group", joint->group, def->Joint().group); + WriteAttr(elem, "ref", 1, &joint->ref, &zero); + WriteAttr(elem, "springref", 1, &joint->springref, &zero); + WriteAttr(elem, "solreflimit", mjNREF, joint->solref_limit, def->Joint().solref_limit, true); + WriteAttr(elem, "solimplimit", mjNIMP, joint->solimp_limit, def->Joint().solimp_limit, true); + WriteAttr(elem, "solreffriction", mjNREF, joint->solref_friction, def->Joint().solref_friction, true); - WriteAttr(elem, "solimpfriction", mjNIMP, pjoint->solimp_friction, def->Joint().solimp_friction, + WriteAttr(elem, "solimpfriction", mjNIMP, joint->solimp_friction, def->Joint().solimp_friction, true); - WriteAttr(elem, "stiffness", 1, &pjoint->stiffness, &def->Joint().stiffness); - WriteAttrKey(elem, "limited", TFAuto_map, 3, pjoint->limited, def->Joint().limited); - WriteAttr(elem, "range", 2, pjoint->range, def->Joint().range); - WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->actfrclimited, + WriteAttr(elem, "stiffness", 1, &joint->stiffness, &def->Joint().stiffness); + WriteAttrKey(elem, "limited", TFAuto_map, 3, joint->limited, def->Joint().limited); + WriteAttr(elem, "range", 2, joint->range, def->Joint().range); + WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, joint->actfrclimited, def->Joint().actfrclimited); - WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, pjoint->actgravcomp, def->Joint().actgravcomp); - WriteAttr(elem, "actuatorfrcrange", 2, pjoint->actfrcrange, def->Joint().actfrcrange); - WriteAttr(elem, "margin", 1, &pjoint->margin, &def->Joint().margin); - WriteAttr(elem, "armature", 1, &pjoint->armature, &def->Joint().armature); - WriteAttr(elem, "damping", 1, &pjoint->damping, &def->Joint().damping); - WriteAttr(elem, "frictionloss", 1, &pjoint->frictionloss, &def->Joint().frictionloss); + WriteAttrKey(elem, "actuatorgravcomp", bool_map, 2, joint->actgravcomp, def->Joint().actgravcomp); + WriteAttr(elem, "actuatorfrcrange", 2, joint->actfrcrange, def->Joint().actfrcrange); + WriteAttr(elem, "margin", 1, &joint->margin, &def->Joint().margin); + WriteAttr(elem, "armature", 1, &joint->armature, &def->Joint().armature); + WriteAttr(elem, "damping", 1, &joint->damping, &def->Joint().damping); + WriteAttr(elem, "frictionloss", 1, &joint->frictionloss, &def->Joint().frictionloss); // userdata if (writingdefaults) { - WriteVector(elem, "user", pjoint->get_userdata()); + WriteVector(elem, "user", joint->get_userdata()); } else { - WriteVector(elem, "user", pjoint->get_userdata(), def->Joint().get_userdata()); + WriteVector(elem, "user", joint->get_userdata(), def->Joint().get_userdata()); } } // write geom -void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def, +void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; double mass = 0; // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pgeom->name); - if (classname != pgeom->classname && pgeom->classname != "main") { - WriteAttrTxt(elem, "class", pgeom->classname); + WriteAttrTxt(elem, "name", geom->name); + if (classname != geom->classname && geom->classname != "main") { + WriteAttrTxt(elem, "class", geom->classname); } - if (mjGEOMINFO[pgeom->type]) { - WriteAttr(elem, "size", mjGEOMINFO[pgeom->type], pgeom->size, def->Geom().size); + if (mjGEOMINFO[geom->type]) { + WriteAttr(elem, "size", mjGEOMINFO[geom->type], geom->size, def->Geom().size); } - if (mjuu_defined(pgeom->mass)) { - mass = pgeom->GetVolume() * def->Geom().density; + if (mjuu_defined(geom->mass)) { + mass = geom->GetVolume() * def->Geom().density; } // mesh geom - if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) { - mjCMesh* pmesh = pgeom->mesh; + if (geom->type==mjGEOM_MESH || geom->type==mjGEOM_SDF) { + mjCMesh* mesh = geom->mesh; // write pos/quat if there is a difference - if (!SameVector(pgeom->pos, pmesh->GetPosPtr(pgeom->typeinertia), 3) || - !SameVector(pgeom->quat, pmesh->GetQuatPtr(pgeom->typeinertia), 4)) { + if (!SameVector(geom->pos, mesh->GetPosPtr(geom->typeinertia), 3) || + !SameVector(geom->quat, mesh->GetQuatPtr(geom->typeinertia), 4)) { // recover geom pos/quat before mesh frame transformation double p[3], q[4]; - mjuu_copyvec(p, pgeom->pos, 3); - mjuu_copyvec(q, pgeom->quat, 4); - mjuu_frameaccuminv(p, q, pmesh->GetPosPtr(pgeom->typeinertia), - pmesh->GetQuatPtr(pgeom->typeinertia)); + mjuu_copyvec(p, geom->pos, 3); + mjuu_copyvec(q, geom->quat, 4); + mjuu_frameaccuminv(p, q, mesh->GetPosPtr(geom->typeinertia), + mesh->GetQuatPtr(geom->typeinertia)); // write WriteAttr(elem, "pos", 3, p, unitq+1); @@ -424,229 +424,229 @@ void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* pgeom, mjCDef* def, // non-mesh geom else { - WriteAttr(elem, "pos", 3, pgeom->pos, unitq+1); - WriteAttr(elem, "quat", 4, pgeom->quat, unitq); + WriteAttr(elem, "pos", 3, geom->pos, unitq+1); + WriteAttr(elem, "quat", 4, geom->quat, unitq); } } else { - WriteAttr(elem, "size", 3, pgeom->size, def->Geom().size); + WriteAttr(elem, "size", 3, geom->size, def->Geom().size); } // defaults and regular - WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, pgeom->type, def->Geom().type); - WriteAttrInt(elem, "contype", pgeom->contype, def->Geom().contype); - WriteAttrInt(elem, "conaffinity", pgeom->conaffinity, def->Geom().conaffinity); - WriteAttrInt(elem, "condim", pgeom->condim, def->Geom().condim); - WriteAttrInt(elem, "group", pgeom->group, def->Geom().group); - WriteAttrInt(elem, "priority", pgeom->priority, def->Geom().priority); - WriteAttr(elem, "friction", 3, pgeom->friction, def->Geom().friction, true); - WriteAttr(elem, "solmix", 1, &pgeom->solmix, &def->Geom().solmix); - WriteAttr(elem, "solref", mjNREF, pgeom->solref, def->Geom().solref, true); - WriteAttr(elem, "solimp", mjNIMP, pgeom->solimp, def->Geom().solimp, true); - WriteAttr(elem, "margin", 1, &pgeom->margin, &def->Geom().margin); - WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap); - WriteAttr(elem, "gap", 1, &pgeom->gap, &def->Geom().gap); - WriteAttrKey(elem, "fluidshape", fluid_map, 2, pgeom->fluid_ellipsoid, def->Geom().fluid_ellipsoid); - WriteAttr(elem, "fluidcoef", 5, pgeom->fluid_coefs, def->Geom().fluid_coefs); - WriteAttrKey(elem, "shellinertia", meshtype_map, 2, pgeom->typeinertia, def->Geom().typeinertia); - if (mjuu_defined(pgeom->mass)) { - WriteAttr(elem, "mass", 1, &pgeom->mass_, &mass); + WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, geom->type, def->Geom().type); + WriteAttrInt(elem, "contype", geom->contype, def->Geom().contype); + WriteAttrInt(elem, "conaffinity", geom->conaffinity, def->Geom().conaffinity); + WriteAttrInt(elem, "condim", geom->condim, def->Geom().condim); + WriteAttrInt(elem, "group", geom->group, def->Geom().group); + WriteAttrInt(elem, "priority", geom->priority, def->Geom().priority); + WriteAttr(elem, "friction", 3, geom->friction, def->Geom().friction, true); + WriteAttr(elem, "solmix", 1, &geom->solmix, &def->Geom().solmix); + WriteAttr(elem, "solref", mjNREF, geom->solref, def->Geom().solref, true); + WriteAttr(elem, "solimp", mjNIMP, geom->solimp, def->Geom().solimp, true); + WriteAttr(elem, "margin", 1, &geom->margin, &def->Geom().margin); + WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap); + WriteAttr(elem, "gap", 1, &geom->gap, &def->Geom().gap); + WriteAttrKey(elem, "fluidshape", fluid_map, 2, geom->fluid_ellipsoid, def->Geom().fluid_ellipsoid); + WriteAttr(elem, "fluidcoef", 5, geom->fluid_coefs, def->Geom().fluid_coefs); + WriteAttrKey(elem, "shellinertia", meshtype_map, 2, geom->typeinertia, def->Geom().typeinertia); + if (mjuu_defined(geom->mass)) { + WriteAttr(elem, "mass", 1, &geom->mass_, &mass); } else { - WriteAttr(elem, "density", 1, &pgeom->density, &def->Geom().density); + WriteAttr(elem, "density", 1, &geom->density, &def->Geom().density); } - if (pgeom->get_material() != def->Geom().get_material()) { - WriteAttrTxt(elem, "material", pgeom->get_material()); + if (geom->get_material() != def->Geom().get_material()) { + WriteAttrTxt(elem, "material", geom->get_material()); } - WriteAttr(elem, "rgba", 4, pgeom->rgba, def->Geom().rgba); + WriteAttr(elem, "rgba", 4, geom->rgba, def->Geom().rgba); // hfield and mesh attributes - if (pgeom->type==mjGEOM_HFIELD) { - WriteAttrTxt(elem, "hfield", pgeom->get_hfieldname()); + if (geom->type==mjGEOM_HFIELD) { + WriteAttrTxt(elem, "hfield", geom->get_hfieldname()); } - if (pgeom->type==mjGEOM_MESH || pgeom->type==mjGEOM_SDF) { - WriteAttrTxt(elem, "mesh", pgeom->get_meshname()); + if (geom->type==mjGEOM_MESH || geom->type==mjGEOM_SDF) { + WriteAttrTxt(elem, "mesh", geom->get_meshname()); } // userdata if (writingdefaults) { - WriteVector(elem, "user", pgeom->get_userdata()); + WriteVector(elem, "user", geom->get_userdata()); } else { - WriteVector(elem, "user", pgeom->get_userdata(), def->Geom().get_userdata()); + WriteVector(elem, "user", geom->get_userdata(), def->Geom().get_userdata()); } // write plugin - if (pgeom->plugin.active) { - OnePlugin(InsertEnd(elem, "plugin"), &pgeom->plugin); + if (geom->plugin.active) { + OnePlugin(InsertEnd(elem, "plugin"), &geom->plugin); } } // write site -void mjXWriter::OneSite(XMLElement* elem, const mjCSite* psite, mjCDef* def, +void mjXWriter::OneSite(XMLElement* elem, const mjCSite* site, mjCDef* def, std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", psite->name); - if (classname != psite->classname && psite->classname != "main") { - WriteAttrTxt(elem, "class", psite->classname); + WriteAttrTxt(elem, "name", site->name); + if (classname != site->classname && site->classname != "main") { + WriteAttrTxt(elem, "class", site->classname); } - WriteAttr(elem, "pos", 3, psite->pos); - WriteAttr(elem, "quat", 4, psite->quat, unitq); - if (mjGEOMINFO[psite->type]) { - WriteAttr(elem, "size", mjGEOMINFO[psite->type], psite->size, def->Site().size); + WriteAttr(elem, "pos", 3, site->pos); + WriteAttr(elem, "quat", 4, site->quat, unitq); + if (mjGEOMINFO[site->type]) { + WriteAttr(elem, "size", mjGEOMINFO[site->type], site->size, def->Site().size); } } else { - WriteAttr(elem, "size", 3, psite->size, def->Site().size); + WriteAttr(elem, "size", 3, site->size, def->Site().size); } // defaults and regular - WriteAttrInt(elem, "group", psite->group, def->Site().group); - WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, psite->type, def->Site().type); - if (psite->get_material() != def->Site().get_material()) { - WriteAttrTxt(elem, "material", psite->get_material()); + WriteAttrInt(elem, "group", site->group, def->Site().group); + WriteAttrKey(elem, "type", geom_map, mjNGEOMTYPES, site->type, def->Site().type); + if (site->get_material() != def->Site().get_material()) { + WriteAttrTxt(elem, "material", site->get_material()); } - WriteAttr(elem, "rgba", 4, psite->rgba, def->Site().rgba); + WriteAttr(elem, "rgba", 4, site->rgba, def->Site().rgba); // userdata if (writingdefaults) { - WriteVector(elem, "user", psite->get_userdata()); + WriteVector(elem, "user", site->get_userdata()); } else { - WriteVector(elem, "user", psite->get_userdata(), def->Site().get_userdata()); + WriteVector(elem, "user", site->get_userdata(), def->Site().get_userdata()); } } // write camera -void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* pcam, mjCDef* def, +void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* camera, mjCDef* def, std::string_view classname) { double unitq[4] = {1, 0, 0, 0}; // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pcam->name); - if (classname != pcam->classname && pcam->classname != "main") { - WriteAttrTxt(elem, "class", pcam->classname); + WriteAttrTxt(elem, "name", camera->name); + if (classname != camera->classname && camera->classname != "main") { + WriteAttrTxt(elem, "class", camera->classname); } - WriteAttrTxt(elem, "target", pcam->get_targetbody()); - WriteAttr(elem, "pos", 3, pcam->pos); - WriteAttr(elem, "quat", 4, pcam->quat, unitq); + WriteAttrTxt(elem, "target", camera->get_targetbody()); + WriteAttr(elem, "pos", 3, camera->pos); + WriteAttr(elem, "quat", 4, camera->quat, unitq); } // defaults and regular - WriteAttr(elem, "ipd", 1, &pcam->ipd, &def->Camera().ipd); - WriteAttrKey(elem, "mode", camlight_map, camlight_sz, pcam->mode, def->Camera().mode); - WriteAttr(elem, "resolution", 2, pcam->resolution, def->Camera().resolution); - WriteAttrKey(elem, "orthographic", bool_map, 2, pcam->orthographic, def->Camera().orthographic); + WriteAttr(elem, "ipd", 1, &camera->ipd, &def->Camera().ipd); + WriteAttrKey(elem, "mode", camlight_map, camlight_sz, camera->mode, def->Camera().mode); + WriteAttr(elem, "resolution", 2, camera->resolution, def->Camera().resolution); + WriteAttrKey(elem, "orthographic", bool_map, 2, camera->orthographic, def->Camera().orthographic); // camera intrinsics if specified - if (pcam->sensor_size[0]>0 && pcam->sensor_size[1]>0) { - WriteAttr(elem, "sensorsize", 2, pcam->sensor_size); - WriteAttr(elem, "focal", 2, pcam->focal_length, def->Camera().focal_length); - WriteAttr(elem, "focalpixel", 2, pcam->focal_pixel, def->Camera().focal_pixel); - WriteAttr(elem, "principal", 2, pcam->principal_length, def->Camera().principal_length); - WriteAttr(elem, "principalpixel", 2, pcam->principal_pixel, def->Camera().principal_pixel); + if (camera->sensor_size[0]>0 && camera->sensor_size[1]>0) { + WriteAttr(elem, "sensorsize", 2, camera->sensor_size); + WriteAttr(elem, "focal", 2, camera->focal_length, def->Camera().focal_length); + WriteAttr(elem, "focalpixel", 2, camera->focal_pixel, def->Camera().focal_pixel); + WriteAttr(elem, "principal", 2, camera->principal_length, def->Camera().principal_length); + WriteAttr(elem, "principalpixel", 2, camera->principal_pixel, def->Camera().principal_pixel); } else { - WriteAttr(elem, "fovy", 1, &pcam->fovy, &def->Camera().fovy); + WriteAttr(elem, "fovy", 1, &camera->fovy, &def->Camera().fovy); } // userdata if (writingdefaults) { - WriteVector(elem, "user", pcam->get_userdata()); + WriteVector(elem, "user", camera->get_userdata()); } else { - WriteVector(elem, "user", pcam->get_userdata(), def->Camera().get_userdata()); + WriteVector(elem, "user", camera->get_userdata(), def->Camera().get_userdata()); } } // write light -void mjXWriter::OneLight(XMLElement* elem, const mjCLight* plight, mjCDef* def, +void mjXWriter::OneLight(XMLElement* elem, const mjCLight* light, mjCDef* def, std::string_view classname) { // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", plight->name); - if (classname != plight->classname && plight->classname != "main") { - WriteAttrTxt(elem, "class", plight->classname); + WriteAttrTxt(elem, "name", light->name); + if (classname != light->classname && light->classname != "main") { + WriteAttrTxt(elem, "class", light->classname); } - WriteAttrTxt(elem, "target", plight->get_targetbody()); - WriteAttr(elem, "pos", 3, plight->pos); - WriteAttr(elem, "dir", 3, plight->dir); + WriteAttrTxt(elem, "target", light->get_targetbody()); + WriteAttr(elem, "pos", 3, light->pos); + WriteAttr(elem, "dir", 3, light->dir); } // defaults and regular - WriteAttr(elem, "bulbradius", 1, &plight->bulbradius, &def->Light().bulbradius); - WriteAttrKey(elem, "directional", bool_map, 2, plight->directional, def->Light().directional); - WriteAttrKey(elem, "castshadow", bool_map, 2, plight->castshadow, def->Light().castshadow); - WriteAttrKey(elem, "active", bool_map, 2, plight->active, def->Light().active); - WriteAttr(elem, "attenuation", 3, plight->attenuation, def->Light().attenuation); - WriteAttr(elem, "cutoff", 1, &plight->cutoff, &def->Light().cutoff); - WriteAttr(elem, "exponent", 1, &plight->exponent, &def->Light().exponent); - WriteAttr(elem, "ambient", 3, plight->ambient, def->Light().ambient); - WriteAttr(elem, "diffuse", 3, plight->diffuse, def->Light().diffuse); - WriteAttr(elem, "specular", 3, plight->specular, def->Light().specular); - WriteAttrKey(elem, "mode", camlight_map, camlight_sz, plight->mode, def->Light().mode); + WriteAttr(elem, "bulbradius", 1, &light->bulbradius, &def->Light().bulbradius); + WriteAttrKey(elem, "directional", bool_map, 2, light->directional, def->Light().directional); + WriteAttrKey(elem, "castshadow", bool_map, 2, light->castshadow, def->Light().castshadow); + WriteAttrKey(elem, "active", bool_map, 2, light->active, def->Light().active); + WriteAttr(elem, "attenuation", 3, light->attenuation, def->Light().attenuation); + WriteAttr(elem, "cutoff", 1, &light->cutoff, &def->Light().cutoff); + WriteAttr(elem, "exponent", 1, &light->exponent, &def->Light().exponent); + WriteAttr(elem, "ambient", 3, light->ambient, def->Light().ambient); + WriteAttr(elem, "diffuse", 3, light->diffuse, def->Light().diffuse); + WriteAttr(elem, "specular", 3, light->specular, def->Light().specular); + WriteAttrKey(elem, "mode", camlight_map, camlight_sz, light->mode, def->Light().mode); } // write pair -void mjXWriter::OnePair(XMLElement* elem, const mjCPair* ppair, mjCDef* def) { +void mjXWriter::OnePair(XMLElement* elem, const mjCPair* pair, mjCDef* def) { // regular if (!writingdefaults) { - if (ppair->classname != "main") { - WriteAttrTxt(elem, "class", ppair->classname); + if (pair->classname != "main") { + WriteAttrTxt(elem, "class", pair->classname); } - WriteAttrTxt(elem, "geom1", ppair->get_geomname1()); - WriteAttrTxt(elem, "geom2", ppair->get_geomname2()); + WriteAttrTxt(elem, "geom1", pair->get_geomname1()); + WriteAttrTxt(elem, "geom2", pair->get_geomname2()); } // defaults and regular - WriteAttrTxt(elem, "name", ppair->name); - WriteAttrInt(elem, "condim", ppair->condim, def->Pair().spec.condim); - WriteAttr(elem, "margin", 1, &ppair->margin, &def->Pair().spec.margin); - WriteAttr(elem, "gap", 1, &ppair->gap, &def->Pair().spec.gap); - WriteAttr(elem, "solref", mjNREF, ppair->solref, def->Pair().spec.solref, true); - WriteAttr(elem, "solreffriction", mjNREF, ppair->solreffriction, def->Pair().spec.solreffriction, + WriteAttrTxt(elem, "name", pair->name); + WriteAttrInt(elem, "condim", pair->condim, def->Pair().spec.condim); + WriteAttr(elem, "margin", 1, &pair->margin, &def->Pair().spec.margin); + WriteAttr(elem, "gap", 1, &pair->gap, &def->Pair().spec.gap); + WriteAttr(elem, "solref", mjNREF, pair->solref, def->Pair().spec.solref, true); + WriteAttr(elem, "solreffriction", mjNREF, pair->solreffriction, def->Pair().spec.solreffriction, true); - WriteAttr(elem, "solimp", mjNIMP, ppair->solimp, def->Pair().spec.solimp, true); - WriteAttr(elem, "friction", 5, ppair->friction, def->Pair().spec.friction); // all 5 values + WriteAttr(elem, "solimp", mjNIMP, pair->solimp, def->Pair().spec.solimp, true); + WriteAttr(elem, "friction", 5, pair->friction, def->Pair().spec.friction); // all 5 values } // write equality -void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* peq, mjCDef* def) { +void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* equality, mjCDef* def) { // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", peq->name); - if (peq->classname != "main") { - WriteAttrTxt(elem, "class", peq->classname); + WriteAttrTxt(elem, "name", equality->name); + if (equality->classname != "main") { + WriteAttrTxt(elem, "class", equality->classname); } - switch (peq->type) { + switch (equality->type) { case mjEQ_CONNECT: - WriteAttrTxt(elem, "body1", mjs_getString(peq->name1)); - WriteAttrTxt(elem, "body2", mjs_getString(peq->name2)); - WriteAttr(elem, "anchor", 3, peq->data); + WriteAttrTxt(elem, "body1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "body2", mjs_getString(equality->name2)); + WriteAttr(elem, "anchor", 3, equality->data); break; case mjEQ_WELD: - WriteAttrTxt(elem, "body1", mjs_getString(peq->name1)); - WriteAttrTxt(elem, "body2", mjs_getString(peq->name2)); - WriteAttr(elem, "anchor", 3, peq->data); - WriteAttr(elem, "torquescale", 1, peq->data+10); - WriteAttr(elem, "relpose", 7, peq->data+3); + WriteAttrTxt(elem, "body1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "body2", mjs_getString(equality->name2)); + WriteAttr(elem, "anchor", 3, equality->data); + WriteAttr(elem, "torquescale", 1, equality->data+10); + WriteAttr(elem, "relpose", 7, equality->data+3); break; case mjEQ_JOINT: - WriteAttrTxt(elem, "joint1", mjs_getString(peq->name1)); - WriteAttrTxt(elem, "joint2", mjs_getString(peq->name2)); - WriteAttr(elem, "polycoef", 5, peq->data); + WriteAttrTxt(elem, "joint1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "joint2", mjs_getString(equality->name2)); + WriteAttr(elem, "polycoef", 5, equality->data); break; case mjEQ_TENDON: - WriteAttrTxt(elem, "tendon1", mjs_getString(peq->name1)); - WriteAttrTxt(elem, "tendon2", mjs_getString(peq->name2)); - WriteAttr(elem, "polycoef", 5, peq->data); + WriteAttrTxt(elem, "tendon1", mjs_getString(equality->name1)); + WriteAttrTxt(elem, "tendon2", mjs_getString(equality->name2)); + WriteAttr(elem, "polycoef", 5, equality->data); break; case mjEQ_FLEX: - WriteAttrTxt(elem, "flex", mjs_getString(peq->name1)); + WriteAttrTxt(elem, "flex", mjs_getString(equality->name1)); break; default: @@ -655,99 +655,99 @@ void mjXWriter::OneEquality(XMLElement* elem, const mjCEquality* peq, mjCDef* de } // defaults and regular - WriteAttrKey(elem, "active", bool_map, 2, peq->active, def->Equality().active); - WriteAttr(elem, "solref", mjNREF, peq->solref, def->Equality().solref, true); - WriteAttr(elem, "solimp", mjNIMP, peq->solimp, def->Equality().solimp, true); + WriteAttrKey(elem, "active", bool_map, 2, equality->active, def->Equality().active); + WriteAttr(elem, "solref", mjNREF, equality->solref, def->Equality().solref, true); + WriteAttr(elem, "solimp", mjNIMP, equality->solimp, def->Equality().solimp, true); } // write tendon -void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* pten, mjCDef* def) { - bool fixed = (pten->GetWrap(0) && pten->GetWrap(0)->type==mjWRAP_JOINT); +void mjXWriter::OneTendon(XMLElement* elem, const mjCTendon* tendon, mjCDef* def) { + bool fixed = (tendon->GetWrap(0) && tendon->GetWrap(0)->type==mjWRAP_JOINT); // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pten->name); - if (pten->classname != "main") { - WriteAttrTxt(elem, "class", pten->classname); + WriteAttrTxt(elem, "name", tendon->name); + if (tendon->classname != "main") { + WriteAttrTxt(elem, "class", tendon->classname); } } // defaults and regular - WriteAttrInt(elem, "group", pten->group, def->Tendon().group); - WriteAttr(elem, "solreflimit", mjNREF, pten->solref_limit, def->Tendon().solref_limit, true); - WriteAttr(elem, "solimplimit", mjNIMP, pten->solimp_limit, def->Tendon().solimp_limit, true); - WriteAttr(elem, "solreffriction", mjNREF, pten->solref_friction, def->Tendon().solref_friction, + WriteAttrInt(elem, "group", tendon->group, def->Tendon().group); + WriteAttr(elem, "solreflimit", mjNREF, tendon->solref_limit, def->Tendon().solref_limit, true); + WriteAttr(elem, "solimplimit", mjNIMP, tendon->solimp_limit, def->Tendon().solimp_limit, true); + WriteAttr(elem, "solreffriction", mjNREF, tendon->solref_friction, def->Tendon().solref_friction, true); - WriteAttr(elem, "solimpfriction", mjNIMP, pten->solimp_friction, def->Tendon().solimp_friction, + WriteAttr(elem, "solimpfriction", mjNIMP, tendon->solimp_friction, def->Tendon().solimp_friction, true); - WriteAttrKey(elem, "limited", TFAuto_map, 3, pten->limited, def->Tendon().limited); - WriteAttr(elem, "range", 2, pten->range, def->Tendon().range); - WriteAttr(elem, "margin", 1, &pten->margin, &def->Tendon().margin); - WriteAttr(elem, "stiffness", 1, &pten->stiffness, &def->Tendon().stiffness); - WriteAttr(elem, "damping", 1, &pten->damping, &def->Tendon().damping); - WriteAttr(elem, "frictionloss", 1, &pten->frictionloss, &def->Tendon().frictionloss); - if (pten->springlength[0] != pten->springlength[1] || + WriteAttrKey(elem, "limited", TFAuto_map, 3, tendon->limited, def->Tendon().limited); + WriteAttr(elem, "range", 2, tendon->range, def->Tendon().range); + WriteAttr(elem, "margin", 1, &tendon->margin, &def->Tendon().margin); + WriteAttr(elem, "stiffness", 1, &tendon->stiffness, &def->Tendon().stiffness); + WriteAttr(elem, "damping", 1, &tendon->damping, &def->Tendon().damping); + WriteAttr(elem, "frictionloss", 1, &tendon->frictionloss, &def->Tendon().frictionloss); + if (tendon->springlength[0] != tendon->springlength[1] || def->Tendon().springlength[0] != def->Tendon().springlength[1]) { - WriteAttr(elem, "springlength", 2, pten->springlength, def->Tendon().springlength); + WriteAttr(elem, "springlength", 2, tendon->springlength, def->Tendon().springlength); } else { - WriteAttr(elem, "springlength", 1, pten->springlength, def->Tendon().springlength); + WriteAttr(elem, "springlength", 1, tendon->springlength, def->Tendon().springlength); } // spatial only if (!fixed) { - if (pten->get_material()!=def->Tendon().get_material()) { - WriteAttrTxt(elem, "material", pten->get_material()); + if (tendon->get_material()!=def->Tendon().get_material()) { + WriteAttrTxt(elem, "material", tendon->get_material()); } - WriteAttr(elem, "width", 1, &pten->width, &def->Tendon().width); - WriteAttr(elem, "rgba", 4, pten->rgba, def->Tendon().rgba); + WriteAttr(elem, "width", 1, &tendon->width, &def->Tendon().width); + WriteAttr(elem, "rgba", 4, tendon->rgba, def->Tendon().rgba); } // userdata if (writingdefaults) { - WriteVector(elem, "user", pten->get_userdata()); + WriteVector(elem, "user", tendon->get_userdata()); } else { - WriteVector(elem, "user", pten->get_userdata(), def->Tendon().get_userdata()); + WriteVector(elem, "user", tendon->get_userdata(), def->Tendon().get_userdata()); } } // write actuator -void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* def) { +void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDef* def) { // regular if (!writingdefaults) { - WriteAttrTxt(elem, "name", pact->name); - if (pact->classname != "main") { - WriteAttrTxt(elem, "class", pact->classname); + WriteAttrTxt(elem, "name", actuator->name); + if (actuator->classname != "main") { + WriteAttrTxt(elem, "class", actuator->classname); } // transmission target - switch (pact->trntype) { + switch (actuator->trntype) { case mjTRN_JOINT: - WriteAttrTxt(elem, "joint", pact->get_target()); + WriteAttrTxt(elem, "joint", actuator->get_target()); break; case mjTRN_JOINTINPARENT: - WriteAttrTxt(elem, "jointinparent", pact->get_target()); + WriteAttrTxt(elem, "jointinparent", actuator->get_target()); break; case mjTRN_TENDON: - WriteAttrTxt(elem, "tendon", pact->get_target()); + WriteAttrTxt(elem, "tendon", actuator->get_target()); break; case mjTRN_SLIDERCRANK: - WriteAttrTxt(elem, "cranksite", pact->get_target()); - WriteAttrTxt(elem, "slidersite", pact->get_slidersite()); + WriteAttrTxt(elem, "cranksite", actuator->get_target()); + WriteAttrTxt(elem, "slidersite", actuator->get_slidersite()); break; case mjTRN_SITE: - WriteAttrTxt(elem, "site", pact->get_target()); - WriteAttrTxt(elem, "refsite", pact->get_refsite()); + WriteAttrTxt(elem, "site", actuator->get_target()); + WriteAttrTxt(elem, "refsite", actuator->get_refsite()); break; case mjTRN_BODY: - WriteAttrTxt(elem, "body", pact->get_target()); + WriteAttrTxt(elem, "body", actuator->get_target()); break; default: // SHOULD NOT OCCUR @@ -756,46 +756,46 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* d } // defaults and regular - WriteAttrInt(elem, "group", pact->group, def->Actuator().group); - WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, pact->ctrllimited, def->Actuator().ctrllimited); - WriteAttr(elem, "ctrlrange", 2, pact->ctrlrange, def->Actuator().ctrlrange); - WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, pact->forcelimited, def->Actuator().forcelimited); - WriteAttr(elem, "forcerange", 2, pact->forcerange, def->Actuator().forcerange); - WriteAttrKey(elem, "actlimited", TFAuto_map, 3, pact->actlimited, def->Actuator().actlimited); - WriteAttr(elem, "actrange", 2, pact->actrange, def->Actuator().actrange); - WriteAttr(elem, "lengthrange", 2, pact->lengthrange, def->Actuator().lengthrange); - WriteAttr(elem, "gear", 6, pact->gear, def->Actuator().gear); - WriteAttr(elem, "cranklength", 1, &pact->cranklength, &def->Actuator().cranklength); - WriteAttrKey(elem, "actearly", bool_map, 2, pact->actearly, + WriteAttrInt(elem, "group", actuator->group, def->Actuator().group); + WriteAttrKey(elem, "ctrllimited", TFAuto_map, 3, actuator->ctrllimited, def->Actuator().ctrllimited); + WriteAttr(elem, "ctrlrange", 2, actuator->ctrlrange, def->Actuator().ctrlrange); + WriteAttrKey(elem, "forcelimited", TFAuto_map, 3, actuator->forcelimited, def->Actuator().forcelimited); + WriteAttr(elem, "forcerange", 2, actuator->forcerange, def->Actuator().forcerange); + WriteAttrKey(elem, "actlimited", TFAuto_map, 3, actuator->actlimited, def->Actuator().actlimited); + WriteAttr(elem, "actrange", 2, actuator->actrange, def->Actuator().actrange); + WriteAttr(elem, "lengthrange", 2, actuator->lengthrange, def->Actuator().lengthrange); + WriteAttr(elem, "gear", 6, actuator->gear, def->Actuator().gear); + WriteAttr(elem, "cranklength", 1, &actuator->cranklength, &def->Actuator().cranklength); + WriteAttrKey(elem, "actearly", bool_map, 2, actuator->actearly, def->Actuator().actearly); // special handling of actdim which has default value of -1 if (writingdefaults) { - WriteAttrInt(elem, "actdim", pact->actdim, def->Actuator().actdim); + WriteAttrInt(elem, "actdim", actuator->actdim, def->Actuator().actdim); } else { - int default_actdim = pact->dyntype == mjDYN_NONE ? 0 : 1; - WriteAttrInt(elem, "actdim", pact->actdim, default_actdim); + int default_actdim = actuator->dyntype == mjDYN_NONE ? 0 : 1; + WriteAttrInt(elem, "actdim", actuator->actdim, default_actdim); } - WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, pact->dyntype, def->Actuator().dyntype); - WriteAttr(elem, "dynprm", mjNDYN, pact->dynprm, def->Actuator().dynprm); + WriteAttrKey(elem, "dyntype", dyn_map, dyn_sz, actuator->dyntype, def->Actuator().dyntype); + WriteAttr(elem, "dynprm", mjNDYN, actuator->dynprm, def->Actuator().dynprm); // plugins: write config attributes - if (pact->plugin.active) { - OnePlugin(elem, &pact->plugin); + if (actuator->plugin.active) { + OnePlugin(elem, &actuator->plugin); } // non-plugins: write actuator parameters else { - WriteAttrKey(elem, "gaintype", gain_map, gain_sz, pact->gaintype, def->Actuator().gaintype); - WriteAttrKey(elem, "biastype", bias_map, bias_sz, pact->biastype, def->Actuator().biastype); - WriteAttr(elem, "gainprm", mjNGAIN, pact->gainprm, def->Actuator().gainprm, true); - WriteAttr(elem, "biasprm", mjNBIAS, pact->biasprm, def->Actuator().biasprm, true); + WriteAttrKey(elem, "gaintype", gain_map, gain_sz, actuator->gaintype, def->Actuator().gaintype); + WriteAttrKey(elem, "biastype", bias_map, bias_sz, actuator->biastype, def->Actuator().biastype); + WriteAttr(elem, "gainprm", mjNGAIN, actuator->gainprm, def->Actuator().gainprm, true); + WriteAttr(elem, "biasprm", mjNBIAS, actuator->biasprm, def->Actuator().biasprm, true); } // userdata if (writingdefaults) { - WriteVector(elem, "user", pact->get_userdata()); + WriteVector(elem, "user", actuator->get_userdata()); } else { - WriteVector(elem, "user", pact->get_userdata(), def->Actuator().get_userdata()); + WriteVector(elem, "user", actuator->get_userdata(), def->Actuator().get_userdata()); } } @@ -1203,11 +1203,11 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) { XMLElement* section; // pointer to parent defaults - mjCDef* par; + mjCDef* parent; if (def->parent) { - par = def->parent; + parent = def->parent; } else { - par = new mjCDef; + parent = new mjCDef; } // create section, write class name @@ -1218,63 +1218,63 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) { // mesh elem = InsertEnd(section, "mesh"); - OneMesh(elem, &def->Mesh(), par); + OneMesh(elem, &def->Mesh(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // material elem = InsertEnd(section, "material"); - OneMaterial(elem, &def->Material(), par); + OneMaterial(elem, &def->Material(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // joint elem = InsertEnd(section, "joint"); - OneJoint(elem, &def->Joint(), par); + OneJoint(elem, &def->Joint(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // geom elem = InsertEnd(section, "geom"); - OneGeom(elem, &def->Geom(), par); + OneGeom(elem, &def->Geom(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // site elem = InsertEnd(section, "site"); - OneSite(elem, &def->Site(), par); + OneSite(elem, &def->Site(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // camera elem = InsertEnd(section, "camera"); - OneCamera(elem, &def->Camera(), par); + OneCamera(elem, &def->Camera(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // light elem = InsertEnd(section, "light"); - OneLight(elem, &def->Light(), par); + OneLight(elem, &def->Light(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // pair elem = InsertEnd(section, "pair"); - OnePair(elem, &def->Pair(), par); + OnePair(elem, &def->Pair(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // equality elem = InsertEnd(section, "equality"); - OneEquality(elem, &def->Equality(), par); + OneEquality(elem, &def->Equality(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // tendon elem = InsertEnd(section, "tendon"); - OneTendon(elem, &def->Tendon(), par); + OneTendon(elem, &def->Tendon(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // actuator elem = InsertEnd(section, "general"); - OneActuator(elem, &def->Actuator(), par); + OneActuator(elem, &def->Actuator(), parent); if (!elem->FirstAttribute()) section->DeleteChild(elem); // if top-level class has no members or children, delete it and return if (!def->parent && section->NoChildren() && def->child.empty()) { root->DeleteChild(section); - delete par; + delete parent; return; } @@ -1285,7 +1285,7 @@ void mjXWriter::Default(XMLElement* root, mjCDef* def) { // delete parent defaults if allocated here if (!def->parent) { - delete par; + delete parent; } } @@ -1373,33 +1373,33 @@ void mjXWriter::Custom(XMLElement* root) { // write all numerics for (int i=0; iGetObject(mjOBJ_NUMERIC, i); + mjCNumeric* numeric = (mjCNumeric*)model->GetObject(mjOBJ_NUMERIC, i); elem = InsertEnd(section, "numeric"); - WriteAttrTxt(elem, "name", ptr->name); - WriteAttrInt(elem, "size", ptr->size); - WriteAttr(elem, "data", ptr->size, ptr->data_.data()); + WriteAttrTxt(elem, "name", numeric->name); + WriteAttrInt(elem, "size", numeric->size); + WriteAttr(elem, "data", numeric->size, numeric->data_.data()); } // write all texts for (int i=0; iGetObject(mjOBJ_TEXT, i); + mjCText* text = (mjCText*)model->GetObject(mjOBJ_TEXT, i); elem = InsertEnd(section, "text"); - WriteAttrTxt(elem, "name", ptr->name); - WriteAttrTxt(elem, "data", ptr->data_.c_str()); + WriteAttrTxt(elem, "name", text->name); + WriteAttrTxt(elem, "data", text->data_.c_str()); } // write all tuples for (int i=0; iGetObject(mjOBJ_TUPLE, i); + mjCTuple* tuple = (mjCTuple*)model->GetObject(mjOBJ_TUPLE, i); elem = InsertEnd(section, "tuple"); - WriteAttrTxt(elem, "name", ptr->name); + WriteAttrTxt(elem, "name", tuple->name); // write objects in tuple - for (int j=0; j<(int)ptr->objtype_.size(); j++) { + for (int j=0; j<(int)tuple->objtype_.size(); j++) { XMLElement* obj = InsertEnd(elem, "element"); - WriteAttrTxt(obj, "objtype", mju_type2Str((int)ptr->objtype_[j])); - WriteAttrTxt(obj, "objname", ptr->objname_[j].c_str()); - double oprm = ptr->objprm_[j]; + WriteAttrTxt(obj, "objtype", mju_type2Str((int)tuple->objtype_[j])); + WriteAttrTxt(obj, "objname", tuple->objname_[j].c_str()); + double oprm = tuple->objprm_[j]; if (oprm!=0) { WriteAttr(obj, "prm", 1, &oprm); } @@ -1431,58 +1431,58 @@ void mjXWriter::Asset(XMLElement* root) { mjCTexture deftex(0); for (int i=0; iGetObject(mjOBJ_TEXTURE, i); + mjCTexture* texture = (mjCTexture*)model->GetObject(mjOBJ_TEXTURE, i); elem = InsertEnd(section, "texture"); // write common attributes - WriteAttrKey(elem, "type", texture_map, texture_sz, ptex->type); - WriteAttrTxt(elem, "name", ptex->name); + WriteAttrKey(elem, "type", texture_map, texture_sz, texture->type); + WriteAttrTxt(elem, "name", texture->name); // write builtin - if (ptex->builtin!=mjBUILTIN_NONE) { - WriteAttrKey(elem, "builtin", builtin_map, builtin_sz, ptex->builtin); - WriteAttrKey(elem, "mark", mark_map, mark_sz, ptex->mark, deftex.mark); - WriteAttr(elem, "rgb1", 3, ptex->rgb1, deftex.rgb1); - WriteAttr(elem, "rgb2", 3, ptex->rgb2, deftex.rgb2); - WriteAttr(elem, "markrgb", 3, ptex->markrgb, deftex.markrgb); - WriteAttr(elem, "random", 1, &ptex->random, &deftex.random); - WriteAttrInt(elem, "width", ptex->width); - WriteAttrInt(elem, "height", ptex->height); + if (texture->builtin!=mjBUILTIN_NONE) { + WriteAttrKey(elem, "builtin", builtin_map, builtin_sz, texture->builtin); + WriteAttrKey(elem, "mark", mark_map, mark_sz, texture->mark, deftex.mark); + WriteAttr(elem, "rgb1", 3, texture->rgb1, deftex.rgb1); + WriteAttr(elem, "rgb2", 3, texture->rgb2, deftex.rgb2); + WriteAttr(elem, "markrgb", 3, texture->markrgb, deftex.markrgb); + WriteAttr(elem, "random", 1, &texture->random, &deftex.random); + WriteAttrInt(elem, "width", texture->width); + WriteAttrInt(elem, "height", texture->height); } // write buffer - else if (ptex->get_cubefiles()[0].empty() && ptex->get_cubefiles()[1].empty() && - ptex->get_cubefiles()[2].empty() && ptex->get_cubefiles()[3].empty() && - ptex->get_cubefiles()[4].empty() && ptex->get_cubefiles()[5].empty() && - ptex->File().empty() && ptex->gridsize[0] == 1 && ptex->gridsize[1] == 1) { + else if (texture->get_cubefiles()[0].empty() && texture->get_cubefiles()[1].empty() && + texture->get_cubefiles()[2].empty() && texture->get_cubefiles()[3].empty() && + texture->get_cubefiles()[4].empty() && texture->get_cubefiles()[5].empty() && + texture->File().empty() && texture->gridsize[0] == 1 && texture->gridsize[1] == 1) { throw mjXError(0, "no support for buffer textures."); } // write textures loaded from files else { // write single file - WriteAttrTxt(elem, "content_type", ptex->get_content_type()); - WriteAttrTxt(elem, "file", ptex->File()); + WriteAttrTxt(elem, "content_type", texture->get_content_type()); + WriteAttrTxt(elem, "file", texture->File()); // write separate files - WriteAttrTxt(elem, "fileright", ptex->get_cubefiles()[0]); - WriteAttrTxt(elem, "fileleft", ptex->get_cubefiles()[1]); - WriteAttrTxt(elem, "fileup", ptex->get_cubefiles()[2]); - WriteAttrTxt(elem, "filedown", ptex->get_cubefiles()[3]); - WriteAttrTxt(elem, "filefront", ptex->get_cubefiles()[4]); - WriteAttrTxt(elem, "fileback", ptex->get_cubefiles()[5]); - if (ptex->hflip) { + WriteAttrTxt(elem, "fileright", texture->get_cubefiles()[0]); + WriteAttrTxt(elem, "fileleft", texture->get_cubefiles()[1]); + WriteAttrTxt(elem, "fileup", texture->get_cubefiles()[2]); + WriteAttrTxt(elem, "filedown", texture->get_cubefiles()[3]); + WriteAttrTxt(elem, "filefront", texture->get_cubefiles()[4]); + WriteAttrTxt(elem, "fileback", texture->get_cubefiles()[5]); + if (texture->hflip) { WriteAttrKey(elem, "hflip", bool_map, 2, 1); } - if (ptex->vflip) { + if (texture->vflip) { WriteAttrKey(elem, "vflip", bool_map, 2, 1); } // write grid - if (ptex->gridsize[0] != 1 || ptex->gridsize[1] != 1) { - double gsize[2] = { (double)ptex->gridsize[0], (double)ptex->gridsize[1] }; + if (texture->gridsize[0] != 1 || texture->gridsize[1] != 1) { + double gsize[2] = { (double)texture->gridsize[0], (double)texture->gridsize[1] }; WriteAttr(elem, "gridsize", 2, gsize); - WriteAttrTxt(elem, "gridlayout", ptex->gridlayout); + WriteAttrTxt(elem, "gridlayout", texture->gridlayout); } } } @@ -1490,43 +1490,43 @@ void mjXWriter::Asset(XMLElement* root) { // write materials for (int i=0; iGetObject(mjOBJ_MATERIAL, i); + mjCMaterial* material = (mjCMaterial*)model->GetObject(mjOBJ_MATERIAL, i); elem = InsertEnd(section, "material"); - OneMaterial(elem, pmat, model->def_map[pmat->classname]); + OneMaterial(elem, material, model->def_map[material->classname]); } // write meshes for (int i=0; iGetObject(mjOBJ_MESH, i); - if (pmesh->Plugin().active) { + mjCMesh* mesh = (mjCMesh*)model->GetObject(mjOBJ_MESH, i); + if (mesh->Plugin().active) { elem = InsertEnd(section, "mesh"); - WriteAttrTxt(elem, "name", pmesh->name); - OnePlugin(InsertEnd(elem, "plugin"), &pmesh->Plugin()); + WriteAttrTxt(elem, "name", mesh->name); + OnePlugin(InsertEnd(elem, "plugin"), &mesh->Plugin()); } else{ elem = InsertEnd(section, "mesh"); - OneMesh(elem, pmesh, model->def_map[pmesh->classname]); + OneMesh(elem, mesh, model->def_map[mesh->classname]); } } // write hfields for (int i=0; iGetObject(mjOBJ_HFIELD, i); + mjCHField* hfield = (mjCHField*)model->GetObject(mjOBJ_HFIELD, i); elem = InsertEnd(section, "hfield"); // write attributes - WriteAttrTxt(elem, "name", phf->name); - WriteAttr(elem, "size", 4, phf->size); - if (!phf->file_.empty()) { - WriteAttrTxt(elem, "content_type", phf->content_type_); - WriteAttrTxt(elem, "file", phf->file_); + WriteAttrTxt(elem, "name", hfield->name); + WriteAttr(elem, "size", 4, hfield->size); + if (!hfield->file_.empty()) { + WriteAttrTxt(elem, "content_type", hfield->content_type_); + WriteAttrTxt(elem, "file", hfield->file_); } else { - WriteAttrInt(elem, "nrow", phf->nrow); - WriteAttrInt(elem, "ncol", phf->ncol); - if (!phf->get_userdata().empty()) { + WriteAttrInt(elem, "nrow", hfield->nrow); + WriteAttrInt(elem, "ncol", hfield->ncol); + if (!hfield->get_userdata().empty()) { std::string text; - Vector2String(text, phf->get_userdata(), phf->ncol); + Vector2String(text, hfield->get_userdata(), hfield->ncol); WriteAttrTxt(elem, "elevation", text); } } @@ -1734,21 +1734,21 @@ void mjXWriter::Contact(XMLElement* root) { // write all geom pairs for (int i=0; iGetObject(mjOBJ_PAIR, i); + mjCPair* pair = (mjCPair*)model->GetObject(mjOBJ_PAIR, i); elem = InsertEnd(section, "pair"); - OnePair(elem, ppair, model->def_map[ppair->classname]); + OnePair(elem, pair, model->def_map[pair->classname]); } // write all exclude pairs for (int i=0; iGetObject(mjOBJ_EXCLUDE, i); + mjCBodyPair* exclude = (mjCBodyPair*)model->GetObject(mjOBJ_EXCLUDE, i); elem = InsertEnd(section, "exclude"); // write attributes - WriteAttrTxt(elem, "name", pexclude->name); - WriteAttrTxt(elem, "body1", pexclude->get_bodyname1()); - WriteAttrTxt(elem, "body2", pexclude->get_bodyname2()); + WriteAttrTxt(elem, "name", exclude->name); + WriteAttrTxt(elem, "body1", exclude->get_bodyname1()); + WriteAttrTxt(elem, "body2", exclude->get_bodyname2()); } } @@ -1767,9 +1767,9 @@ void mjXWriter::Equality(XMLElement* root) { // write all constraints for (int i=0; iGetObject(mjOBJ_EQUALITY, i); - XMLElement* elem = InsertEnd(section, FindValue(equality_map, equality_sz, peq->type).c_str()); - OneEquality(elem, peq, model->def_map[peq->classname]); + mjCEquality* equality = (mjCEquality*)model->GetObject(mjOBJ_EQUALITY, i); + XMLElement* elem = InsertEnd(section, FindValue(equality_map, equality_sz, equality->type).c_str()); + OneEquality(elem, equality, model->def_map[equality->classname]); } } @@ -1794,17 +1794,17 @@ void mjXWriter::Deformable(XMLElement* root) { // write flexes for (int i=0; iGetObject(mjOBJ_FLEX, i); + mjCFlex* flex = (mjCFlex*)model->GetObject(mjOBJ_FLEX, i); elem = InsertEnd(section, "flex"); - OneFlex(elem, pflex); + OneFlex(elem, flex); } // write skins for (int i=0; iGetObject(mjOBJ_SKIN, i); + mjCSkin* skin = (mjCSkin*)model->GetObject(mjOBJ_SKIN, i); elem = InsertEnd(section, "skin"); - OneSkin(elem, pskin); + OneSkin(elem, skin); } } @@ -1824,42 +1824,42 @@ void mjXWriter::Tendon(XMLElement* root) { // write all tendons for (int i=0; iGetObject(mjOBJ_TENDON, i); - if (!pten->NumWraps()) { // SHOULD NOT OCCUR + mjCTendon* tendon = (mjCTendon*)model->GetObject(mjOBJ_TENDON, i); + if (!tendon->NumWraps()) { // SHOULD NOT OCCUR continue; } XMLElement* elem = InsertEnd(section, - pten->GetWrap(0)->type==mjWRAP_JOINT ? "fixed" : "spatial"); - OneTendon(elem, pten, model->def_map[pten->classname]); + tendon->GetWrap(0)->type==mjWRAP_JOINT ? "fixed" : "spatial"); + OneTendon(elem, tendon, model->def_map[tendon->classname]); // write wraps - XMLElement* wrap; - for (int j=0; jNumWraps(); j++) { - const mjCWrap* pw = pten->GetWrap(j); - switch (pw->type) { + XMLElement* wrapelem; + for (int j=0; jNumWraps(); j++) { + const mjCWrap* wrap = tendon->GetWrap(j); + switch (wrap->type) { case mjWRAP_JOINT: - wrap = InsertEnd(elem, "joint"); - WriteAttrTxt(wrap, "joint", pw->obj->name); - WriteAttr(wrap, "coef", 1, &pw->prm); + wrapelem = InsertEnd(elem, "joint"); + WriteAttrTxt(wrapelem, "joint", wrap->obj->name); + WriteAttr(wrapelem, "coef", 1, &wrap->prm); break; case mjWRAP_SITE: - wrap = InsertEnd(elem, "site"); - WriteAttrTxt(wrap, "site", pw->obj->name); + wrapelem = InsertEnd(elem, "site"); + WriteAttrTxt(wrapelem, "site", wrap->obj->name); break; case mjWRAP_SPHERE: case mjWRAP_CYLINDER: - wrap = InsertEnd(elem, "geom"); - WriteAttrTxt(wrap, "geom", pw->obj->name); - if (!pw->sidesite.empty()) { - WriteAttrTxt(wrap, "sidesite", pw->sidesite); + wrapelem = InsertEnd(elem, "geom"); + WriteAttrTxt(wrapelem, "geom", wrap->obj->name); + if (!wrap->sidesite.empty()) { + WriteAttrTxt(wrapelem, "sidesite", wrap->sidesite); } break; case mjWRAP_PULLEY: - wrap = InsertEnd(elem, "pulley"); - WriteAttr(wrap, "divisor", 1, &pw->prm); + wrapelem = InsertEnd(elem, "pulley"); + WriteAttr(wrapelem, "divisor", 1, &wrap->prm); break; default: @@ -1884,14 +1884,14 @@ void mjXWriter::Actuator(XMLElement* root) { // write all actuators for (int i=0; iGetObject(mjOBJ_ACTUATOR, i); + mjCActuator* actuator = (mjCActuator*)model->GetObject(mjOBJ_ACTUATOR, i); XMLElement* elem; - if (pact->plugin.active) { + if (actuator->plugin.active) { elem = InsertEnd(section, "plugin"); } else { elem = InsertEnd(section, "general"); } - OneActuator(elem, pact, model->def_map[pact->classname]); + OneActuator(elem, actuator, model->def_map[actuator->classname]); } } @@ -1913,231 +1913,231 @@ void mjXWriter::Sensor(XMLElement* root) { // write all sensors for (int i=0; iSensors()[i]; + mjCSensor* sensor = model->Sensors()[i]; std::string instance_name = ""; std::string plugin_name = ""; // write sensor type and type-specific attributes - switch (psen->type) { + switch (sensor->type) { // common robotic sensors, attached to a site case mjSENS_TOUCH: elem = InsertEnd(section, "touch"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_ACCELEROMETER: elem = InsertEnd(section, "accelerometer"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_VELOCIMETER: elem = InsertEnd(section, "velocimeter"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_GYRO: elem = InsertEnd(section, "gyro"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_FORCE: elem = InsertEnd(section, "force"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_TORQUE: elem = InsertEnd(section, "torque"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_MAGNETOMETER: elem = InsertEnd(section, "magnetometer"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_RANGEFINDER: elem = InsertEnd(section, "rangefinder"); - WriteAttrTxt(elem, "site", psen->get_objname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); break; case mjSENS_CAMPROJECTION: elem = InsertEnd(section, "camprojection"); - WriteAttrTxt(elem, "site", psen->get_objname()); - WriteAttrTxt(elem, "camera", psen->get_refname()); + WriteAttrTxt(elem, "site", sensor->get_objname()); + WriteAttrTxt(elem, "camera", sensor->get_refname()); break; // sensors related to scalar joints, tendons, actuators case mjSENS_JOINTPOS: elem = InsertEnd(section, "jointpos"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_JOINTVEL: elem = InsertEnd(section, "jointvel"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_TENDONPOS: elem = InsertEnd(section, "tendonpos"); - WriteAttrTxt(elem, "tendon", psen->get_objname()); + WriteAttrTxt(elem, "tendon", sensor->get_objname()); break; case mjSENS_TENDONVEL: elem = InsertEnd(section, "tendonvel"); - WriteAttrTxt(elem, "tendon", psen->get_objname()); + WriteAttrTxt(elem, "tendon", sensor->get_objname()); break; case mjSENS_ACTUATORPOS: elem = InsertEnd(section, "actuatorpos"); - WriteAttrTxt(elem, "actuator", psen->get_objname()); + WriteAttrTxt(elem, "actuator", sensor->get_objname()); break; case mjSENS_ACTUATORVEL: elem = InsertEnd(section, "actuatorvel"); - WriteAttrTxt(elem, "actuator", psen->get_objname()); + WriteAttrTxt(elem, "actuator", sensor->get_objname()); break; case mjSENS_ACTUATORFRC: elem = InsertEnd(section, "actuatorfrc"); - WriteAttrTxt(elem, "actuator", psen->get_objname()); + WriteAttrTxt(elem, "actuator", sensor->get_objname()); break; case mjSENS_JOINTACTFRC: elem = InsertEnd(section, "jointactuatorfrc"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; // sensors related to ball joints case mjSENS_BALLQUAT: elem = InsertEnd(section, "ballquat"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_BALLANGVEL: elem = InsertEnd(section, "ballangvel"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; // joint and tendon limit sensors case mjSENS_JOINTLIMITPOS: elem = InsertEnd(section, "jointlimitpos"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_JOINTLIMITVEL: elem = InsertEnd(section, "jointlimitvel"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_JOINTLIMITFRC: elem = InsertEnd(section, "jointlimitfrc"); - WriteAttrTxt(elem, "joint", psen->get_objname()); + WriteAttrTxt(elem, "joint", sensor->get_objname()); break; case mjSENS_TENDONLIMITPOS: elem = InsertEnd(section, "tendonlimitpos"); - WriteAttrTxt(elem, "tendon", psen->get_objname()); + WriteAttrTxt(elem, "tendon", sensor->get_objname()); break; case mjSENS_TENDONLIMITVEL: elem = InsertEnd(section, "tendonlimitvel"); - WriteAttrTxt(elem, "tendon", psen->get_objname()); + WriteAttrTxt(elem, "tendon", sensor->get_objname()); break; case mjSENS_TENDONLIMITFRC: elem = InsertEnd(section, "tendonlimitfrc"); - WriteAttrTxt(elem, "tendon", psen->get_objname()); + WriteAttrTxt(elem, "tendon", sensor->get_objname()); break; // sensors attached to an object with spatial frame: (x)body, geom, site, camera case mjSENS_FRAMEPOS: elem = InsertEnd(section, "framepos"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEQUAT: elem = InsertEnd(section, "framequat"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEXAXIS: elem = InsertEnd(section, "framexaxis"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEYAXIS: elem = InsertEnd(section, "frameyaxis"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEZAXIS: elem = InsertEnd(section, "framezaxis"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMELINVEL: elem = InsertEnd(section, "framelinvel"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEANGVEL: elem = InsertEnd(section, "frameangvel"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMELINACC: elem = InsertEnd(section, "framelinacc"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; case mjSENS_FRAMEANGACC: elem = InsertEnd(section, "frameangacc"); - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - if (psen->reftype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "reftype", mju_type2Str(psen->reftype)); - WriteAttrTxt(elem, "refname", psen->get_refname()); + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + if (sensor->reftype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "reftype", mju_type2Str(sensor->reftype)); + WriteAttrTxt(elem, "refname", sensor->get_refname()); } break; // sensors related to kinematic subtrees; attached to a body (which is the subtree root) case mjSENS_SUBTREECOM: elem = InsertEnd(section, "subtreecom"); - WriteAttrTxt(elem, "body", psen->get_objname()); + WriteAttrTxt(elem, "body", sensor->get_objname()); break; case mjSENS_SUBTREELINVEL: elem = InsertEnd(section, "subtreelinvel"); - WriteAttrTxt(elem, "body", psen->get_objname()); + WriteAttrTxt(elem, "body", sensor->get_objname()); break; case mjSENS_SUBTREEANGMOM: elem = InsertEnd(section, "subtreeangmom"); - WriteAttrTxt(elem, "body", psen->get_objname()); + WriteAttrTxt(elem, "body", sensor->get_objname()); break; case mjSENS_GEOMDIST: elem = InsertEnd(section, "distance"); - WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); - WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); + WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname()); break; case mjSENS_GEOMNORMAL: elem = InsertEnd(section, "normal"); - WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); - WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); + WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname()); break; case mjSENS_GEOMFROMTO: elem = InsertEnd(section, "fromto"); - WriteAttrTxt(elem, psen->objtype == mjOBJ_BODY ? "body1" : "geom1", psen->get_objname()); - WriteAttrTxt(elem, psen->reftype == mjOBJ_BODY ? "body2" : "geom2", psen->get_refname()); + WriteAttrTxt(elem, sensor->objtype == mjOBJ_BODY ? "body1" : "geom1", sensor->get_objname()); + WriteAttrTxt(elem, sensor->reftype == mjOBJ_BODY ? "body2" : "geom2", sensor->get_refname()); break; // global sensors @@ -2149,21 +2149,21 @@ void mjXWriter::Sensor(XMLElement* root) { // plugin-controlled sensor case mjSENS_PLUGIN: elem = InsertEnd(section, "plugin"); - if (psen->objtype != mjOBJ_UNKNOWN) { - WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); + if (sensor->objtype != mjOBJ_UNKNOWN) { + WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); } - OnePlugin(elem, &psen->plugin); + OnePlugin(elem, &sensor->plugin); break; // user-defined sensor case mjSENS_USER: elem = InsertEnd(section, "user"); - if (mju_type2Str(psen->objtype)) WriteAttrTxt(elem, "objtype", mju_type2Str(psen->objtype)); - WriteAttrTxt(elem, "objname", psen->get_objname()); - WriteAttrInt(elem, "dim", psen->dim); - WriteAttrKey(elem, "needstage", stage_map, stage_sz, (int)psen->needstage); - WriteAttrKey(elem, "datatype", datatype_map, datatype_sz, (int)psen->datatype); + if (mju_type2Str(sensor->objtype)) WriteAttrTxt(elem, "objtype", mju_type2Str(sensor->objtype)); + WriteAttrTxt(elem, "objname", sensor->get_objname()); + WriteAttrInt(elem, "dim", sensor->dim); + WriteAttrKey(elem, "needstage", stage_map, stage_sz, (int)sensor->needstage); + WriteAttrKey(elem, "datatype", datatype_map, datatype_sz, (int)sensor->datatype); break; default: @@ -2171,12 +2171,12 @@ void mjXWriter::Sensor(XMLElement* root) { } // write name, noise, userdata - WriteAttrTxt(elem, "name", psen->name); - WriteAttr(elem, "cutoff", 1, &psen->cutoff, &zero); - if (psen->type != mjSENS_PLUGIN) { - WriteAttr(elem, "noise", 1, &psen->noise, &zero); + WriteAttrTxt(elem, "name", sensor->name); + WriteAttr(elem, "cutoff", 1, &sensor->cutoff, &zero); + if (sensor->type != mjSENS_PLUGIN) { + WriteAttr(elem, "noise", 1, &sensor->noise, &zero); } - WriteVector(elem, "user", psen->get_userdata()); + WriteVector(elem, "user", sensor->get_userdata()); } // remove section if empty @@ -2197,24 +2197,24 @@ void mjXWriter::Keyframe(XMLElement* root) { XMLElement* elem = InsertEnd(section, "key"); bool change = false; - mjCKey* pk = model->Keys()[i]; + mjCKey* key = model->Keys()[i]; // check name and write - if (!pk->name.empty()) { - WriteAttrTxt(elem, "name", pk->name); + if (!key->name.empty()) { + WriteAttrTxt(elem, "name", key->name); change = true; } // check time and write - if (pk->time!=0) { - WriteAttr(elem, "time", 1, &pk->time); + if (key->time!=0) { + WriteAttr(elem, "time", 1, &key->time); change = true; } // check qpos and write for (int j=0; jnq; j++) { - if (pk->qpos_[j]!=model->qpos0[j]) { - WriteAttr(elem, "qpos", model->nq, pk->qpos_.data()); + if (key->qpos_[j]!=model->qpos0[j]) { + WriteAttr(elem, "qpos", model->nq, key->qpos_.data()); change = true; break; } @@ -2222,8 +2222,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check qvel and write for (int j=0; jnv; j++) { - if (pk->qvel_[j]!=0) { - WriteAttr(elem, "qvel", model->nv, pk->qvel_.data()); + if (key->qvel_[j]!=0) { + WriteAttr(elem, "qvel", model->nv, key->qvel_.data()); change = true; break; } @@ -2231,8 +2231,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check act and write for (int j=0; jna; j++) { - if (pk->act_[j]!=0) { - WriteAttr(elem, "act", model->na, pk->act_.data()); + if (key->act_[j]!=0) { + WriteAttr(elem, "act", model->na, key->act_.data()); change = true; break; } @@ -2242,12 +2242,12 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->nmocap) { for (int j=0; jnbody; j++) { if (model->Bodies()[j]->mocap) { - mjCBody* pb = model->Bodies()[j]; - int id = pb->mocapid; - if (pb->pos[0] != pk->mpos_[3*id] || - pb->pos[1] != pk->mpos_[3*id+1] || - pb->pos[2] != pk->mpos_[3*id+2]) { - WriteAttr(elem, "mpos", 3*model->nmocap, pk->mpos_.data()); + mjCBody* body = model->Bodies()[j]; + int id = body->mocapid; + if (body->pos[0] != key->mpos_[3*id] || + body->pos[1] != key->mpos_[3*id+1] || + body->pos[2] != key->mpos_[3*id+2]) { + WriteAttr(elem, "mpos", 3*model->nmocap, key->mpos_.data()); change = true; break; } @@ -2259,13 +2259,13 @@ void mjXWriter::Keyframe(XMLElement* root) { if (model->nmocap) { for (int j=0; jnbody; j++) { if (model->Bodies()[j]->mocap) { - mjCBody* pb = model->Bodies()[j]; - int id = pb->mocapid; - if (pb->quat[0] != pk->mquat_[4*id] || - pb->quat[1] != pk->mquat_[4*id+1] || - pb->quat[2] != pk->mquat_[4*id+2] || - pb->quat[3] != pk->mquat_[4*id+3]) { - WriteAttr(elem, "mquat", 4*model->nmocap, pk->mquat_.data()); + mjCBody* body = model->Bodies()[j]; + int id = body->mocapid; + if (body->quat[0] != key->mquat_[4*id] || + body->quat[1] != key->mquat_[4*id+1] || + body->quat[2] != key->mquat_[4*id+2] || + body->quat[3] != key->mquat_[4*id+3]) { + WriteAttr(elem, "mquat", 4*model->nmocap, key->mquat_.data()); change = true; break; } @@ -2275,8 +2275,8 @@ void mjXWriter::Keyframe(XMLElement* root) { // check ctrl and write for (int j=0; jnu; j++) { - if (pk->ctrl_[j]!=0) { - WriteAttr(elem, "ctrl", model->nu, pk->ctrl_.data()); + if (key->ctrl_[j]!=0) { + WriteAttr(elem, "ctrl", model->nu, key->ctrl_.data()); change = true; break; } From f864108c23faadb422dbbed2669d4b6c3eb1297a Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Aug 2024 06:13:51 -0700 Subject: [PATCH 21/24] `using` for `std::{string, string_view}`, where helpful for readibility. PiperOrigin-RevId: 663707900 Change-Id: Icbea374d5f99c7d267140d83f541f0ee872de8c1 --- src/xml/xml_native_reader.cc | 77 +++++++++++++------------- src/xml/xml_native_writer.cc | 102 +++++++++++++++++------------------ 2 files changed, 89 insertions(+), 90 deletions(-) diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 5881ba4f..d9576e7d 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -48,20 +48,21 @@ namespace { using std::string; +using std::string_view; using std::vector; using mujoco::user::FilePath; using tinyxml2::XMLElement; void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjsPlugin* p) { - std::map> config_attribs; + std::map> config_attribs; XMLElement* child = FirstChildElement(elem); while (child) { - std::string_view name = child->Value(); + string_view name = child->Value(); if (name == "config") { - std::string key, value; + string key, value; mjXUtil::ReadAttrTxt(child, "key", key, /* required = */ true); if (config_attribs.find(key) != config_attribs.end()) { - std::string err = "duplicate config key: " + key; + string err = "duplicate config key: " + key; throw mjXError(child, "%s", err.c_str()); } mjXUtil::ReadAttrTxt(child, "value", value, /* required = */ true); @@ -79,10 +80,10 @@ void ReadPluginConfigs(tinyxml2::XMLElement* elem, mjsPlugin* p) { } } -static void UpdateString(std::string& psuffix, int count, int i) { +static void UpdateString(string& psuffix, int count, int i) { int ndigits = std::to_string(count).length(); - std::string i_string = std::to_string(i); - std::string prefix = ""; + string i_string = std::to_string(i); + string prefix = ""; while (ndigits-- > i_string.length()) { prefix += '0'; } @@ -996,7 +997,7 @@ void mjXReader::Compiler(XMLElement* section, mjSpec* spec) { mjs_setString(spec->texturedir, text.c_str()); } // meshdir and texturedir take precedence over assetdir - std::string meshdir, texturedir; + string meshdir, texturedir; if (ReadAttrTxt(section, "meshdir", meshdir)) { mjs_setString(spec->meshdir, meshdir.c_str()); }; @@ -1157,11 +1158,11 @@ void mjXReader::Size(XMLElement* section, mjSpec* spec) { } // trim entire string - std::string trimmed; + string trimmed; { - std::istringstream strm((std::string(pstr))); + std::istringstream strm((string(pstr))); strm >> trimmed; - std::string trailing; + string trailing; strm >> trailing; if (!trailing.empty() || !strm.eof()) { throw mjXError(section, "%s", err_msg); @@ -1649,7 +1650,7 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* joint) { void mjXReader::OneGeom(XMLElement* elem, mjsGeom* geom) { string text, name; std::vector userdata; - std::string hfieldname, meshname, material; + string hfieldname, meshname, material; int n; // read attributes @@ -1722,7 +1723,7 @@ void mjXReader::OneSite(XMLElement* elem, mjsSite* site) { int n; string text, name; std::vector userdata; - std::string material; + string material; // read attributes if (ReadAttrTxt(elem, "name", name)) { @@ -2328,7 +2329,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { } // cable - std::string curves; + string curves; ReadAttrTxt(elem, "curve", curves); ReadAttrTxt(elem, "initial", comp.initial); ReadAttr(elem, "size", 3, comp.size, text, false, false); @@ -2381,7 +2382,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { // geom XMLElement* egeom = FirstChildElement(elem, "geom"); if (egeom) { - std::string material; + string material; mjsGeom& dgeom = *comp.def[0].spec.geom; if (MapValue(egeom, "type", &n, geom_map, mjNGEOMTYPES)) { dgeom.type = (mjtGeom)n; @@ -2409,7 +2410,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { // site XMLElement* esite = FirstChildElement(elem, "site"); if (esite) { - std::string material; + string material; mjsSite& dsite = *comp.def[0].spec.site; ReadAttr(esite, "size", 3, dsite.size, text, false, false); ReadAttrInt(esite, "group", &dsite.group); @@ -2486,7 +2487,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { ReadAttr(eten, "solimpfix", mjNIMP, dequality.solimp, text, false, false); // tendon attributes - std::string material; + string material; MapValue(elem, "limited", &dtendon.limited, TFAuto_map, 3); ReadAttrInt(eten, "group", &dtendon.group); ReadAttr(eten, "solreflimit", mjNREF, dtendon.solref_limit, text, false, false); @@ -2672,8 +2673,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* body, const mjVFS* vfs) { // add plugin void mjXReader::OnePlugin(XMLElement* elem, mjsPlugin* plugin) { plugin->active = true; - std::string name = ""; - std::string instance_name = ""; + string name = ""; + string instance_name = ""; ReadAttrTxt(elem, "plugin", name); ReadAttrTxt(elem, "instance", instance_name); mjs_setString(plugin->name, name.c_str()); @@ -2793,10 +2794,10 @@ void mjXReader::Extension(XMLElement* section) { while (elem) { // get sub-element name - std::string_view name = elem->Value(); + string_view name = elem->Value(); if (name == "plugin") { - std::string plugin_name; + string plugin_name; int plugin_slot = -1; ReadAttrTxt(elem, "plugin", plugin_name, /* required = */ true); const mjpPlugin* plugin = mjp_getPlugin(plugin_name.c_str(), &plugin_slot); @@ -2817,7 +2818,7 @@ void mjXReader::Extension(XMLElement* section) { XMLElement* child = FirstChildElement(elem); while (child) { - if (std::string(child->Value())=="instance") { + if (string(child->Value())=="instance") { if (spec->hasImplicitPluginElem) { throw mjXError( child, "explicit plugin instance must appear before implicit plugin elements"); @@ -2925,7 +2926,7 @@ void mjXReader::Custom(XMLElement* section) { // read objects and add XMLElement* obj = FirstChildElement(elem); std::vector objtype; - std::string objname = ""; + string objname = ""; std::vector objprm; while (obj) { @@ -3178,8 +3179,8 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { } // separate files - std::vector cubefiles(6); - std::vector cubefile_names = {"fileright", "fileleft", + std::vector cubefiles(6); + std::vector cubefile_names = {"fileright", "fileleft", "fileup", "filedown", "filefront", "fileback"}; for (int i = 0; i < cubefiles.size(); i++) { @@ -3285,7 +3286,7 @@ void mjXReader::Asset(XMLElement* section, const mjVFS* vfs) { } // overwrite model name if given - std::string modelname = ""; + string modelname = ""; if (ReadAttrTxt(elem, "name", modelname)) { mjs_setString(child->modelname, modelname.c_str()); } @@ -3376,7 +3377,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, mjs_setDefault(joint->element, def); // read attributes - std::string name; + string name; if (ReadAttrTxt(elem, "name", name)) { mjs_setString(joint->name, name.c_str()); } @@ -3450,7 +3451,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, mjs_setDefault(pframe->element, childdef ? childdef : def); // read attributes - std::string name, childclass; + string name, childclass; if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pframe->name, name.c_str()); } @@ -3469,7 +3470,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, int count; double offset[3] = {0, 0, 0}; double euler[3] = {0, 0, 0}; - std::string separator = ""; + string separator = ""; ReadAttr(elem, "count", 1, &count, text, true); ReadAttr(elem, "offset", 3, offset, text); ReadAttr(elem, "euler", 3, euler, text); @@ -3520,7 +3521,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]); // process suffix - std::string suffix = separator; + string suffix = separator; UpdateString(suffix, count, i); // attach to parent @@ -3547,13 +3548,13 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, // create child body mjsBody* child = mjs_addBody(body, childdef); - mjs_setString(child->info, std::string("line " + std::to_string(elem->GetLineNum())).c_str()); + mjs_setString(child->info, string("line " + std::to_string(elem->GetLineNum())).c_str()); // set default from class or childclass mjs_setDefault(child->element, childdef ? childdef : def); // read attributes - std::string name, childclass; + string name, childclass; if (ReadAttrTxt(elem, "name", name)) { mjs_setString(child->name, name.c_str()); } @@ -3584,7 +3585,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame, // attachment else if (name=="attach") { - std::string model_name, body_name, prefix; + string model_name, body_name, prefix; ReadAttrTxt(elem, "model", model_name); ReadAttrTxt(elem, "body", body_name); ReadAttrTxt(elem, "prefix", prefix); @@ -4232,26 +4233,26 @@ mjsDefault* mjXReader::GetClass(XMLElement* section) { if (!def) { throw mjXError( section, - std::string("unknown default class name '" + text + "'").c_str()); + string("unknown default class name '" + text + "'").c_str()); } } return def; } -void mjXReader::SetModelFileDir(const std::string& modelfiledir) { +void mjXReader::SetModelFileDir(const string& modelfiledir) { modelfiledir_ = FilePath(modelfiledir); } -void mjXReader::SetAssetDir(const std::string& assetdir) { +void mjXReader::SetAssetDir(const string& assetdir) { assetdir_ = FilePath(assetdir); } -void mjXReader::SetMeshDir(const std::string& meshdir) { +void mjXReader::SetMeshDir(const string& meshdir) { meshdir_ = FilePath(meshdir); } -void mjXReader::SetTextureDir(const std::string& texturedir) { +void mjXReader::SetTextureDir(const string& texturedir) { texturedir_ = FilePath(texturedir); } diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 993d1a52..8d48cbce 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -39,6 +39,8 @@ namespace { +using std::string; +using std::string_view; using tinyxml2::XMLComment; using tinyxml2::XMLDocument; using tinyxml2::XMLElement; @@ -62,7 +64,7 @@ class mj_XMLPrinter : public tinyxml2::XMLPrinter { // save XML file using custom 2-space indentation -static std::string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) { +static string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) { doc.ClearError(); mj_XMLPrinter stream(nullptr, /*compact=*/false); doc.Print(&stream); @@ -70,22 +72,22 @@ static std::string WriteDoc(XMLDocument& doc, char *error, size_t error_sz) { mjCopyError(error, doc.ErrorStr(), error_sz); return ""; } - std::string str = std::string(stream.CStr()); + string str = string(stream.CStr()); // top level sections - std::array sections = { + std::array sections = { "", "name); @@ -803,8 +803,8 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* actuator, mjCDe // write plugin void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) { - const std::string instance_name = std::string(mjs_getString(plugin->instance_name)); - const std::string plugin_name = std::string(mjs_getString(plugin->name)); + const string instance_name = string(mjs_getString(plugin->instance_name)); + const string plugin_name = string(mjs_getString(plugin->name)); if (!instance_name.empty()) { WriteAttrTxt(elem, "instance", instance_name); } else { @@ -813,7 +813,7 @@ void mjXWriter::OnePlugin(XMLElement* elem, const mjsPlugin* plugin) { static_cast(plugin->instance)->spec.plugin_slot); const char* c = &(static_cast(plugin->instance)->flattened_attributes[0]); for (int i = 0; i < pplugin->nattribute; ++i) { - std::string value(c); + string value(c); if (!value.empty()) { XMLElement* config_elem = InsertEnd(elem, "config"); WriteAttrTxt(config_elem, "key", pplugin->attributes[i]); @@ -844,7 +844,7 @@ void mjXWriter::SetModel(const mjSpec* spec) { // save existing model in MJCF canonical format, must be compiled -std::string mjXWriter::Write(char *error, size_t error_sz) { +string mjXWriter::Write(char *error, size_t error_sz) { // check model if (!model || !model->IsCompiled()) { mjCopyError(error, "XML Write error: Only compiled model can be written", error_sz); @@ -860,7 +860,7 @@ std::string mjXWriter::Write(char *error, size_t error_sz) { doc.InsertFirstChild(root); // write comment if present - std::string text = mjs_getString(model->comment); + string text = mjs_getString(model->comment); if (!text.empty()) { XMLComment* comment = doc.NewComment(text.c_str()); root->LinkEndChild(comment); @@ -1332,7 +1332,7 @@ void mjXWriter::Extension(XMLElement* root) { // write plugin config attributes const char* c = &pp->flattened_attributes[0]; for (int i = 0; i < plugin->nattribute; ++i) { - std::string value(c); + string value(c); if (!value.empty()) { XMLElement* config_elem = InsertEnd(elem, "config"); WriteAttrTxt(config_elem, "key", plugin->attributes[i]); @@ -1525,7 +1525,7 @@ void mjXWriter::Asset(XMLElement* root) { WriteAttrInt(elem, "nrow", hfield->nrow); WriteAttrInt(elem, "ncol", hfield->ncol); if (!hfield->get_userdata().empty()) { - std::string text; + string text; Vector2String(text, hfield->get_userdata(), hfield->ncol); WriteAttrTxt(elem, "elevation", text); } @@ -1555,8 +1555,7 @@ XMLElement* mjXWriter::OneFrame(XMLElement* elem, mjCFrame* frame) { // recursive body and frame writer -void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, - std::string_view childclass) { +void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, string_view childclass) { double unitq[4] = {1, 0, 0, 0}; if (!body) { @@ -1587,8 +1586,7 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, WriteVector(elem, "user", body->get_userdata()); // write inertial - if (body->explicitinertial && - model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { + if (body->explicitinertial && model->inertiafromgeom!=mjINERTIAFROMGEOM_TRUE) { XMLElement* inertial = InsertEnd(elem, "inertial"); WriteAttr(inertial, "pos", 3, body->ipos); WriteAttr(inertial, "quat", 4, body->iquat, unitq); @@ -1602,9 +1600,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, if (body->joints[i]->frame != frame) { continue; } - std::string classname = body->joints[i]->frame && !body->joints[i]->frame->classname.empty() - ? body->joints[i]->frame->classname - : body->classname; + string classname = body->joints[i]->frame && !body->joints[i]->frame->classname.empty() + ? body->joints[i]->frame->classname + : body->classname; OneJoint(InsertEnd(elem, "joint"), body->joints[i], model->def_map[body->joints[i]->classname], classname.empty() ? childclass : classname); @@ -1615,9 +1613,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, if (body->geoms[i]->frame != frame) { continue; } - std::string classname = body->geoms[i]->frame && !body->geoms[i]->frame->classname.empty() - ? body->geoms[i]->frame->classname - : body->classname; + string classname = body->geoms[i]->frame && !body->geoms[i]->frame->classname.empty() + ? body->geoms[i]->frame->classname + : body->classname; OneGeom(InsertEnd(elem, "geom"), body->geoms[i], model->def_map[body->geoms[i]->classname], classname.empty() ? childclass : classname); @@ -1628,9 +1626,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, if (body->sites[i]->frame != frame) { continue; } - std::string classname = body->sites[i]->frame && !body->sites[i]->frame->classname.empty() - ? body->sites[i]->frame->classname - : body->classname; + string classname = body->sites[i]->frame && !body->sites[i]->frame->classname.empty() + ? body->sites[i]->frame->classname + : body->classname; OneSite(InsertEnd(elem, "site"), body->sites[i], model->def_map[body->sites[i]->classname], classname.empty() ? childclass : classname); @@ -1641,9 +1639,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, if (body->cameras[i]->frame != frame) { continue; } - std::string classname = body->cameras[i]->frame && !body->cameras[i]->frame->classname.empty() - ? body->cameras[i]->frame->classname - : body->classname; + string classname = body->cameras[i]->frame && !body->cameras[i]->frame->classname.empty() + ? body->cameras[i]->frame->classname + : body->classname; OneCamera(InsertEnd(elem, "camera"), body->cameras[i], model->def_map[body->cameras[i]->classname], classname.empty() ? childclass : classname); @@ -1654,9 +1652,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, if (body->lights[i]->frame != frame) { continue; } - std::string classname = body->lights[i]->frame && !body->lights[i]->frame->classname.empty() - ? body->lights[i]->frame->classname - : body->classname; + string classname = body->lights[i]->frame && !body->lights[i]->frame->classname.empty() + ? body->lights[i]->frame->classname + : body->classname; OneLight(InsertEnd(elem, "light"), body->lights[i], model->def_map[body->lights[i]->classname], classname.empty() ? childclass : classname); @@ -1674,9 +1672,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, // write body if its frame matches the current frame, avoid access if there are no bodies if (bframe == frame && !body->bodies.empty()) { - std::string classname = bframe && !bframe->classname.empty() - ? bframe->classname - : body->classname; + string classname = bframe && !bframe->classname.empty() + ? bframe->classname + : body->classname; Body(InsertEnd(elem, "body"), body->bodies[i], nullptr, classname.empty() ? childclass : classname); } @@ -1694,9 +1692,9 @@ void mjXWriter::Body(XMLElement* elem, mjCBody* body, mjCFrame* frame, // write frame if its frame matches the current frame if (fframe->frame == frame) { - std::string classname = fframe && !fframe->classname.empty() - ? fframe->classname - : body->classname; + string classname = fframe && !fframe->classname.empty() + ? fframe->classname + : body->classname; Body(OneFrame(elem, fframe), body, fframe, childclass); } @@ -1914,8 +1912,8 @@ void mjXWriter::Sensor(XMLElement* root) { for (int i=0; iSensors()[i]; - std::string instance_name = ""; - std::string plugin_name = ""; + string instance_name = ""; + string plugin_name = ""; // write sensor type and type-specific attributes switch (sensor->type) { From 4d9305fbb97efc574d2536ca33c94bcd6c31f2c7 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Aug 2024 07:44:42 -0700 Subject: [PATCH 22/24] Fix typo. PiperOrigin-RevId: 663728400 Change-Id: Ifaf266a515bacb7b1598d14eb435561cb2375224 --- doc/python.rst | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/python.rst b/doc/python.rst index 703caaa7..be1127fd 100644 --- a/doc/python.rst +++ b/doc/python.rst @@ -653,7 +653,7 @@ msh2obj.py The `msh2obj.py `__ script converts the :ref:`legacy .msh format` for surface meshes (different from the possibly-volumetric -:ref:`gmsh format` also using .msh), to OBJ files. The legacy format is depricated and will be removed +:ref:`gmsh format` also using .msh), to OBJ files. The legacy format is deprecated and will be removed in a future release. Please convert all legacy files to OBJ. From ce9c0ea351e4d105369b9bb3d4ca2017b6bf155b Mon Sep 17 00:00:00 2001 From: Taylor Howell Date: Fri, 16 Aug 2024 10:09:42 -0700 Subject: [PATCH 23/24] Modify MJX sensor_pos to handle unsupported sensors. PiperOrigin-RevId: 663768186 Change-Id: I2e7600e77855a96a47f012fb64b6cc43abbf6f64 --- mjx/mujoco/mjx/_src/sensor.py | 10 ++++++++-- mjx/mujoco/mjx/_src/sensor_test.py | 7 +++++++ .../mjx/test_data/unsupported_sensor.xml | 19 +++++++++++++++++++ 3 files changed, 34 insertions(+), 2 deletions(-) create mode 100644 mjx/mujoco/mjx/test_data/unsupported_sensor.xml diff --git a/mjx/mujoco/mjx/_src/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index b92c32e0..7dabeeb2 100644 --- a/mjx/mujoco/mjx/_src/sensor.py +++ b/mjx/mujoco/mjx/_src/sensor.py @@ -31,7 +31,8 @@ def sensor_pos(m: Model, d: Data) -> Data: """Compute position-dependent sensors values.""" # no position-dependent sensors - if sum(m.sensor_needstage == mujoco.mjtStage.mjSTAGE_POS) == 0: + stage_pos = m.sensor_needstage == mujoco.mjtStage.mjSTAGE_POS + if sum(stage_pos) == 0: return d # position and orientation by object type @@ -56,7 +57,7 @@ def sensor_pos(m: Model, d: Data) -> Data: sensors, adrs = [], [] - for sensor_type in set(m.sensor_type): + for sensor_type in set(m.sensor_type[stage_pos]): idx = m.sensor_type == sensor_type objid = m.sensor_objid[idx] adr = m.sensor_adr[idx] @@ -126,10 +127,15 @@ def sensor_pos(m: Model, d: Data) -> Data: adr = (adr[:, None] + np.arange(3)[None]).reshape(-1) elif sensor_type == SensorType.CLOCK: sensor = jp.repeat(d.time, sum(idx)) + else: + continue # unsupported sensor type sensors.append(sensor) adrs.append(adr) + if not adrs: + return d + sensordata = d.sensordata.at[np.concatenate(adrs)].set( jp.concatenate(sensors) ) diff --git a/mjx/mujoco/mjx/_src/sensor_test.py b/mjx/mujoco/mjx/_src/sensor_test.py index d30e928e..d120e757 100644 --- a/mjx/mujoco/mjx/_src/sensor_test.py +++ b/mjx/mujoco/mjx/_src/sensor_test.py @@ -62,6 +62,13 @@ class SensorTest(parameterized.TestCase): # sensor values _assert_eq(d.sensordata, dx.sensordata, 'sensordata') + def test_unsupported_sensor(self): + """Tests MJX sensor functions do not break for unsupported sensors.""" + m = test_util.load_test_file('unsupported_sensor.xml') + mx = mjx.put_model(m) + dx = jax.jit(mjx.forward)(mx, mjx.put_data(m, mujoco.MjData(m))) + _assert_eq(np.zeros(m.nsensordata), dx.sensordata, 'sensordata') + if __name__ == '__main__': absltest.main() diff --git a/mjx/mujoco/mjx/test_data/unsupported_sensor.xml b/mjx/mujoco/mjx/test_data/unsupported_sensor.xml new file mode 100644 index 00000000..4a164bd9 --- /dev/null +++ b/mjx/mujoco/mjx/test_data/unsupported_sensor.xml @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + From 390bce235283cab56df05b5bd1cc90ac58d81e4c Mon Sep 17 00:00:00 2001 From: Erik Frey Date: Fri, 16 Aug 2024 12:01:32 -0700 Subject: [PATCH 24/24] Avoid placing arrays on device for unused MJX fields. Add ``device`` parameter to mjx.make_data. PiperOrigin-RevId: 663808411 Change-Id: Ic55875f87f5c36b9dfe21e5ed5891d64c7003cab --- doc/changelog.rst | 10 +- mjx/mujoco/mjx/_src/io.py | 348 +++++++++++++++++++++-------------- mjx/mujoco/mjx/_src/types.py | 238 ++++++++++++------------ 3 files changed, 334 insertions(+), 262 deletions(-) diff --git a/doc/changelog.rst b/doc/changelog.rst index 40713d29..54b706c6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -13,20 +13,22 @@ General MJX ^^^ -4. Added ``efc_pos`` to ``mjx.Data``. +4. Added ``efc_pos`` to ``mjx.Data`` (:github:issue:`1388`). 5. Added position-dependent sensors: ``MAGNETOMETER``, ``JOINTPOS``, ``ACTUATORPOS``, ``BALLQUAT``, ``FRAMEPOS``, ``FRAMEXAXIS``, ``FRAMEYAXIS``, ``FRAMEZAXIS``, ``SUBTREECOM``, ``CLOCK``. +6. Changed default policy to avoid placing unused (MuJoCo-only) arrays on device. +7. Added ``device`` parameter to ``mjx.make_data`` to bring it to parity with ``mjx.put_model`` and ``mjx.put_data``. Bug fixes ^^^^^^^^^ -6. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, +8. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, contribution by :github:user:`michael-ahn`). -7. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit +9. Fixed a bug wherein, for models that have both muscles and stateless actuators and used one of the implicit integrators, wrong derivatives would be computed. Python bindings ^^^^^^^^^^^^^^^ -8. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). +10. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). Version 3.2.2 (Aug 8, 2024) diff --git a/mjx/mujoco/mjx/_src/io.py b/mjx/mujoco/mjx/_src/io.py index d10a4760..7ad1c261 100644 --- a/mjx/mujoco/mjx/_src/io.py +++ b/mjx/mujoco/mjx/_src/io.py @@ -70,15 +70,25 @@ def _make_statistic(s: mujoco.MjStatistic) -> types.Statistic: def put_model( - m: mujoco.MjModel, device=None, _check_unsupported=True + m: mujoco.MjModel, device=None, _full_compat: bool = False # pylint: disable=invalid-name ) -> types.Model: - """Puts mujoco.MjModel onto a device, resulting in mjx.Model.""" + """Puts mujoco.MjModel onto a device, resulting in mjx.Model. + + Args: + m: the model to put onto device + device: which device to use - if unspecified picks the default device + _full_compat: put all MjModel fields onto device irrespective of MJX support + This is an experimental feature. Avoid using it for now. + + Returns: + an mjx.Model placed on device + """ mesh_geomid = set() for g1, g2, ip in collision_driver.geom_pairs(m): t1, t2 = m.geom_type[[g1, g2]] # check collision function exists for type pair - if _check_unsupported and not collision_driver.has_collision_fn(t1, t2): + if not collision_driver.has_collision_fn(t1, t2) and not _full_compat: t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2) raise NotImplementedError(f'({t1}, {t2}) collisions not implemented.') # margin/gap not supported for meshes and height fields @@ -88,7 +98,7 @@ def put_model( margin = m.pair_margin[ip] else: margin = m.geom_margin[g1] + m.geom_margin[g2] - if _check_unsupported and margin.any(): + if margin.any() and not _full_compat: t1, t2 = mujoco.mjtGeom(t1), mujoco.mjtGeom(t2) raise NotImplementedError(f'({t1}, {t2}) margin/gap not implemented.') for t, g in [(t1, g1), (t2, g2)]: @@ -106,16 +116,19 @@ def put_model( (m.wrap_type, types.WrapType, mujoco.mjtWrap), ): missing = set(enum_field) - set(enum_type) - if _check_unsupported and missing: + if missing and not _full_compat: raise NotImplementedError( f'{[mj_type(m) for m in missing]} not supported' ) - if _check_unsupported and not np.allclose(m.dof_frictionloss, 0): + if not np.allclose(m.dof_frictionloss, 0) and not _full_compat: raise NotImplementedError('dof_frictionloss is not implemented.') - mjx_only = {'mesh_convex', 'geom_rbound_hfield'} - mj_field_names = {f.name for f in types.Model.fields()} - mjx_only + mj_field_names = { + f.name + for f in types.Model.fields() + if f.metadata.get('restricted_to') != 'mjx' + } fields = {f: getattr(m, f) for f in mj_field_names} fields['geom_rbound_hfield'] = fields['geom_rbound'] fields['cam_mat0'] = fields['cam_mat0'].reshape((-1, 3, 3)) @@ -135,29 +148,156 @@ def put_model( return jax.device_put(model, device=device) -def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: - """Allocate and initialize Data.""" +def make_data( + m: Union[types.Model, mujoco.MjModel], + device=None, + _full_compat: bool = False, # pylint: disable=invalid-name +) -> types.Data: + """Allocate and initialize Data. + + Args: + m: the model to use + device: which device to use - if unspecified picks the default device + _full_compat: create all MjData fields on device irrespective of MJX support + This is an experimental feature. Avoid using it for now. + If using this flag, also use _full_compat for put_model. + + Returns: + an initialized mjx.Data placed on device + """ dim = collision_driver.make_condim(m) efc_type = constraint.make_efc_type(m, dim) efc_address = constraint.make_efc_address(m, dim, efc_type) ne, nf, nl, nc = constraint.counts(efc_type) ncon, nefc = dim.size, ne + nf + nl + nc - contact = types.Contact( - dist=jp.zeros((ncon,), dtype=float), - pos=jp.zeros((ncon, 3), dtype=float), - frame=jp.zeros((ncon, 3, 3), dtype=float), - includemargin=jp.zeros((ncon,), dtype=float), - friction=jp.zeros((ncon, 5), dtype=float), - solref=jp.zeros((ncon, mujoco.mjNREF), dtype=float), - solreffriction=jp.zeros((ncon, mujoco.mjNREF), dtype=float), - solimp=jp.zeros((ncon, mujoco.mjNIMP), dtype=float), - dim=dim, - geom1=jp.full((ncon,), -1, dtype=jp.int32), - geom2=jp.full((ncon,), -1, dtype=jp.int32), - geom=jp.full((ncon, 2), -1, dtype=jp.int32), - efc_address=efc_address, - ) + with jax.default_device(device): + contact = types.Contact( + dist=jp.zeros((ncon,), dtype=float), + pos=jp.zeros((ncon, 3), dtype=float), + frame=jp.zeros((ncon, 3, 3), dtype=float), + includemargin=jp.zeros((ncon,), dtype=float), + friction=jp.zeros((ncon, 5), dtype=float), + solref=jp.zeros((ncon, mujoco.mjNREF), dtype=float), + solreffriction=jp.zeros((ncon, mujoco.mjNREF), dtype=float), + solimp=jp.zeros((ncon, mujoco.mjNIMP), dtype=float), + dim=dim, + geom1=jp.full((ncon,), -1, dtype=jp.int32), + geom2=jp.full((ncon,), -1, dtype=jp.int32), + geom=jp.full((ncon, 2), -1, dtype=jp.int32), + efc_address=efc_address, + ) + + zero_fields = { + 'solver_niter': (int,), + 'time': (float,), + 'qvel': (m.nv, float), + 'act': (m.na, float), + 'qacc_warmstart': (m.nv, float), + 'ctrl': (m.nu, float), + 'qfrc_applied': (m.nv, float), + 'xfrc_applied': (m.nbody, 6, float), + 'eq_active': (m.neq, jp.uint8), + 'mocap_pos': (m.nmocap, 3, float), + 'mocap_quat': (m.nmocap, 4, float), + 'qacc': (m.nv, float), + 'act_dot': (m.na, float), + 'userdata': (m.nuserdata, float), + 'sensordata': (m.nsensordata, float), + 'xpos': (m.nbody, 3, float), + 'xquat': (m.nbody, 4, float), + 'xmat': (m.nbody, 3, 3, float), + 'xipos': (m.nbody, 3, float), + 'ximat': (m.nbody, 3, 3, float), + 'xanchor': (m.njnt, 3, float), + 'xaxis': (m.njnt, 3, float), + 'geom_xpos': (m.ngeom, 3, float), + 'geom_xmat': (m.ngeom, 3, 3, float), + 'site_xpos': (m.nsite, 3, float), + 'site_xmat': (m.nsite, 3, 3, float), + 'cam_xpos': (m.ncam, 3, float), + 'cam_xmat': (m.ncam, 3, 3, float), + 'light_xpos': (m.nlight, 3, float), + 'light_xdir': (m.nlight, 3, float), + 'subtree_com': (m.nbody, 3, float), + 'cdof': (m.nv, 6, float), + 'cinert': (m.nbody, 10, float), + 'flexvert_xpos': (m.nflexvert, 3, float), + 'flexelem_aabb': (m.nflexelem, 6, float), + 'flexedge_J_rownnz': (m.nflexedge, jp.int32), + 'flexedge_J_rowadr': (m.nflexedge, jp.int32), + 'flexedge_J_colind': (m.nflexedge, m.nv, jp.int32), + 'flexedge_J': (m.nflexedge, m.nv, float), + 'flexedge_length': (m.nflexedge, float), + 'ten_wrapadr': (m.ntendon, jp.int32), + 'ten_wrapnum': (m.ntendon, jp.int32), + 'ten_J_rownnz': (m.ntendon, jp.int32), + 'ten_J_rowadr': (m.ntendon, jp.int32), + 'ten_J_colind': (m.ntendon, m.nv, jp.int32), + 'ten_J': (m.ntendon, m.nv, float), + 'ten_length': (m.ntendon, float), + 'wrap_obj': (m.nwrap, 2, jp.int32), + 'wrap_xpos': (m.nwrap, 6, float), + 'actuator_length': (m.nu, float), + 'actuator_moment': (m.nu, m.nv, float), + 'crb': (m.nbody, 10, float), + 'qM': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), + 'qLD': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), + 'qLDiagInv': (m.nM, float) if support.is_sparse(m) else (0, float), + 'qLDiagSqrtInv': (m.nv, float), + 'bvh_aabb_dyn': (m.nbvhdynamic, 6, float), + 'bvh_active': (m.nbvh, jp.uint8), + 'flexedge_velocity': (m.nflexedge, float), + 'ten_velocity': (m.ntendon, float), + 'actuator_velocity': (m.nu, float), + 'cvel': (m.nbody, 6, float), + 'cdof_dot': (m.nv, 6, float), + 'qfrc_bias': (m.nv, float), + 'qfrc_spring': (m.nv, float), + 'qfrc_damper': (m.nv, float), + 'qfrc_gravcomp': (m.nv, float), + 'qfrc_fluid': (m.nv, float), + 'qfrc_passive': (m.nv, float), + 'subtree_linvel': (m.nbody, 3, float), + 'subtree_angmom': (m.nbody, 3, float), + 'qH': (m.nM, float) if support.is_sparse(m) else (m.nv, m.nv, float), + 'qHDiagInv': (m.nv, float), + 'D_rownnz': (m.nv, jp.int32), + 'D_rowadr': (m.nv, jp.int32), + 'D_colind': (m.nD, jp.int32), + 'B_rownnz': (m.nbody, jp.int32), + 'B_rowadr': (m.nbody, jp.int32), + 'B_colind': (m.nB, jp.int32), + 'qDeriv': (m.nD, float), + 'qLU': (m.nD, float), + 'actuator_force': (m.nu, float), + 'qfrc_actuator': (m.nv, float), + 'qfrc_smooth': (m.nv, float), + 'qacc_smooth': (m.nv, float), + 'qfrc_constraint': (m.nv, float), + 'qfrc_inverse': (m.nv, float), + 'cacc': (m.nbody, 6, float), + 'cfrc_int': (m.nbody, 6, float), + 'cfrc_ext': (m.nbody, 6, float), + 'efc_J': (nefc, m.nv, float), + 'efc_pos': (nefc, float), + 'efc_frictionloss': (nefc, float), + 'efc_D': (nefc, float), + 'efc_aref': (nefc, float), + 'efc_force': (nefc, float), + '_qM_sparse': (m.nM, float), + '_qLD_sparse': (m.nM, float), + '_qLDiagInv_sparse': (m.nv, float), + } + + if not _full_compat: + for f in types.Data.fields(): + if f.metadata.get('restricted_to') in ('mujoco', 'mjx'): + zero_fields[f.name] = (0, zero_fields[f.name][-1]) + + zero_fields = { + k: jp.zeros(v[:-1], dtype=v[-1]) for k, v in zero_fields.items() + } d = types.Data( ne=ne, @@ -165,119 +305,10 @@ def make_data(m: Union[types.Model, mujoco.MjModel]) -> types.Data: nl=nl, nefc=nefc, ncon=ncon, - solver_niter=jp.zeros((), dtype=int), - time=jp.zeros((), dtype=float), qpos=jp.array(m.qpos0), - qvel=jp.zeros((m.nv,), dtype=float), - act=jp.zeros((m.na,), dtype=float), - qacc_warmstart=jp.zeros((m.nv,), dtype=float), - ctrl=jp.zeros((m.nu,), dtype=float), - qfrc_applied=jp.zeros((m.nv,), dtype=float), - xfrc_applied=jp.zeros((m.nbody, 6), dtype=float), - eq_active=jp.zeros((m.neq,), dtype=jp.uint8), - mocap_pos=jp.zeros((m.nmocap, 3), dtype=float), - mocap_quat=jp.zeros((m.nmocap, 4), dtype=float), - qacc=jp.zeros((m.nv,), dtype=float), - act_dot=jp.zeros((m.na,), dtype=float), - userdata=jp.zeros((m.nuserdata,), dtype=float), - sensordata=jp.zeros((m.nsensordata,), dtype=float), - xpos=jp.zeros((m.nbody, 3), dtype=float), - xquat=jp.zeros((m.nbody, 4), dtype=float), - xmat=jp.zeros((m.nbody, 3, 3), dtype=float), - xipos=jp.zeros((m.nbody, 3), dtype=float), - ximat=jp.zeros((m.nbody, 3, 3), dtype=float), - xanchor=jp.zeros((m.njnt, 3), dtype=float), - xaxis=jp.zeros((m.njnt, 3), dtype=float), - geom_xpos=jp.zeros((m.ngeom, 3), dtype=float), - geom_xmat=jp.zeros((m.ngeom, 3, 3), dtype=float), - site_xpos=jp.zeros((m.nsite, 3), dtype=float), - site_xmat=jp.zeros((m.nsite, 3, 3), dtype=float), - cam_xpos=jp.zeros((m.ncam, 3), dtype=float), - cam_xmat=jp.zeros((m.ncam, 3, 3), dtype=float), - light_xpos=jp.zeros((m.nlight, 3), dtype=float), - light_xdir=jp.zeros((m.nlight, 3), dtype=float), - subtree_com=jp.zeros((m.nbody, 3), dtype=float), - cdof=jp.zeros((m.nv, 6), dtype=float), - cinert=jp.zeros((m.nbody, 10), dtype=float), - flexvert_xpos=jp.zeros((m.nflexvert, 3), dtype=float), - flexelem_aabb=jp.zeros((m.nflexelem, 6), dtype=float), - flexedge_J_rownnz=jp.zeros((m.nflexedge,), dtype=jp.int32), - flexedge_J_rowadr=jp.zeros((m.nflexedge,), dtype=jp.int32), - flexedge_J_colind=jp.zeros((m.nflexedge, m.nv), dtype=jp.int32), - flexedge_J=jp.zeros((m.nflexedge, m.nv), dtype=float), - flexedge_length=jp.zeros((m.nflexedge,), dtype=float), - ten_wrapadr=jp.zeros((m.ntendon,), dtype=jp.int32), - ten_wrapnum=jp.zeros((m.ntendon,), dtype=jp.int32), - ten_J_rownnz=jp.zeros((m.ntendon,), dtype=jp.int32), - ten_J_rowadr=jp.zeros((m.ntendon,), dtype=jp.int32), - ten_J_colind=jp.zeros((m.ntendon, m.nv), dtype=jp.int32), - ten_J=jp.zeros((m.ntendon, m.nv), dtype=float), - ten_length=jp.zeros((m.ntendon,), dtype=float), - wrap_obj=jp.zeros((m.nwrap, 2), dtype=jp.int32), - wrap_xpos=jp.zeros((m.nwrap, 6), dtype=float), - actuator_length=jp.zeros((m.nu,), dtype=float), - actuator_moment=jp.zeros((m.nu, m.nv), dtype=float), - crb=jp.zeros((m.nbody, 10), dtype=float), - qM=( - jp.zeros((m.nM,), dtype=float) - if support.is_sparse(m) - else jp.zeros((m.nv, m.nv), dtype=float) - ), - qLD=( - jp.zeros((m.nM,), dtype=float) - if support.is_sparse(m) - else jp.zeros((m.nv, m.nv), dtype=float) - ), - qLDiagInv=( - jp.zeros((m.nv,), dtype=float) if support.is_sparse(m) - else jp.zeros((0,), dtype=float) - ), - qLDiagSqrtInv=jp.zeros((m.nv,), dtype=float), - bvh_aabb_dyn=jp.zeros((m.nbvhdynamic, 6), dtype=float), - bvh_active=jp.zeros((m.nbvh,), dtype=jp.uint8), - flexedge_velocity=jp.zeros((m.nflexedge,), dtype=float), - ten_velocity=jp.zeros((m.ntendon,), dtype=float), - actuator_velocity=jp.zeros((m.nu,), dtype=float), - cvel=jp.zeros((m.nbody, 6), dtype=float), - cdof_dot=jp.zeros((m.nv, 6), dtype=float), - qfrc_bias=jp.zeros((m.nv,), dtype=float), - qfrc_spring=jp.zeros((m.nv,), dtype=float), - qfrc_damper=jp.zeros((m.nv,), dtype=float), - qfrc_gravcomp=jp.zeros((m.nv,), dtype=float), - qfrc_fluid=jp.zeros((m.nv,), dtype=float), - qfrc_passive=jp.zeros((m.nv,), dtype=float), - subtree_linvel=jp.zeros((m.nbody, 3), dtype=float), - subtree_angmom=jp.zeros((m.nbody, 3), dtype=float), - qH=jp.zeros((m.nM,), dtype=float), - qHDiagInv=jp.zeros((m.nv,), dtype=float), - D_rownnz=jp.zeros((m.nv,), dtype=jp.int32), - D_rowadr=jp.zeros((m.nv,), dtype=jp.int32), - D_colind=jp.zeros((m.nD,), dtype=jp.int32), - B_rownnz=jp.zeros((m.nbody,), dtype=jp.int32), - B_rowadr=jp.zeros((m.nbody,), dtype=jp.int32), - B_colind=jp.zeros((m.nB,), dtype=jp.int32), - qDeriv=jp.zeros((m.nD,), dtype=float), - qLU=jp.zeros((m.nD,), dtype=float), - actuator_force=jp.zeros((m.nu,), dtype=float), - qfrc_actuator=jp.zeros((m.nv,), dtype=float), - qfrc_smooth=jp.zeros((m.nv,), dtype=float), - qacc_smooth=jp.zeros((m.nv,), dtype=float), - qfrc_constraint=jp.zeros((m.nv,), dtype=float), - qfrc_inverse=jp.zeros((m.nv,), dtype=float), - cacc=jp.zeros((m.nbody, 6), dtype=float), - cfrc_int=jp.zeros((m.nbody, 6), dtype=float), - cfrc_ext=jp.zeros((m.nbody, 6), dtype=float), contact=contact, efc_type=efc_type, - efc_J=jp.zeros((nefc, m.nv), dtype=float), - efc_pos=jp.zeros((nefc,), dtype=float), - efc_frictionloss=jp.zeros((nefc,), dtype=float), - efc_D=jp.zeros((nefc,), dtype=float), - efc_aref=jp.zeros((nefc,), dtype=float), - efc_force=jp.zeros((nefc,), dtype=float), - _qM_sparse=jp.zeros((m.nM), dtype=float), - _qLD_sparse=jp.zeros((m.nM), dtype=float), - _qLDiagInv_sparse=jp.zeros((m.nv,), dtype=float), + **zero_fields ) return d @@ -348,7 +379,8 @@ def get_data_into( result_i.efc_J_colind[:] = np.tile(np.arange(m.nv), nefc) for field in types.Data.fields(): - if field.name.startswith('_') and field.name.endswith('_sparse'): + restricted_to = field.metadata.get('restricted_to') + if restricted_to == 'mjx': continue if field.name == 'contact': @@ -376,6 +408,8 @@ def get_data_into( value = np.ones(m.nv) if isinstance(value, np.ndarray) and value.shape: + if restricted_to in ('mujoco', 'mjx') and value.shape == (0,): + continue # don't copy fields that are mujoco-only or MJX-only getattr(result_i, field.name)[:] = value else: setattr(result_i, field.name, value) @@ -416,8 +450,22 @@ def _make_contact( return types.Contact(**fields), contact_map -def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: - """Puts mujoco.MjData onto a device, resulting in mjx.Data.""" +def put_data( + m: mujoco.MjModel, d: mujoco.MjData, device=None, _full_compat: bool = False # pylint: disable=invalid-name +) -> types.Data: + """Puts mujoco.MjData onto a device, resulting in mjx.Data. + + Args: + m: the model to use + d: the data to put on device + device: which device to use - if unspecified picks the default device + _full_compat: put all MjModel fields onto device irrespective of MJX support + This is an experimental feature. Avoid using it for now. + If using this flag, also use _full_compat for put_model. + + Returns: + an mjx.Data placed on device + """ dim = collision_driver.make_condim(m) efc_type = constraint.make_efc_type(m, dim) efc_address = constraint.make_efc_address(m, dim, efc_type) @@ -434,8 +482,11 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: if d_val > val: raise ValueError(f'd.{name} too high, d.{name} = {d_val}, model = {val}') - fields = {f.name: getattr(d, f.name) for f in types.Data.fields() - if not f.name.endswith('_sparse')} + fields = { + f.name: getattr(d, f.name) + for f in types.Data.fields() + if f.metadata.get('restricted_to') != 'mjx' + } # MJX prefers square matrices for these fields: for fname in ('xmat', 'ximat', 'geom_xmat', 'site_xmat', 'cam_xmat'): @@ -490,9 +541,6 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: fields[fname] = value # convert qM and qLD if jacobian is dense - fields['_qM_sparse'] = fields['qM'] - fields['_qLD_sparse'] = fields['qLD'] - fields['_qLDiagInv_sparse'] = fields['qLDiagInv'] if not support.is_sparse(m): fields['qM'] = np.zeros((m.nv, m.nv)) mujoco.mj_fullM(m, fields['qM'], d.qM) @@ -504,6 +552,20 @@ def put_data(m: mujoco.MjModel, d: mujoco.MjData, device=None) -> types.Data: fields['qLD'] = np.zeros((m.nv, m.nv)) fields['qLDiagInv'] = np.zeros(0) + if _full_compat: + # full compatibility mode, we store sparse qM regardless of jacobian setting + fields['_qM_sparse'] = fields['qM'] + fields['_qLD_sparse'] = fields['qLD'] + fields['_qLDiagInv_sparse'] = fields['qLDiagInv'] + else: + fields['_qM_sparse'] = jp.zeros(0, dtype=float) + fields['_qLD_sparse'] = jp.zeros(0, dtype=float) + fields['_qLDiagInv_sparse'] = jp.zeros(0, dtype=float) + # otherwise clear out unused arrays + for f in types.Data.fields(): + if f.metadata.get('restricted_to') == 'mujoco': + fields[f.name] = np.zeros(0, dtype=fields[f.name].dtype) + fields['contact'] = contact fields.update(ne=ne, nf=nf, nl=nl, nefc=nefc, ncon=ncon, efc_type=efc_type) diff --git a/mjx/mujoco/mjx/_src/types.py b/mjx/mujoco/mjx/_src/types.py index 8b690c11..c3f2d2ff 100644 --- a/mjx/mujoco/mjx/_src/types.py +++ b/mjx/mujoco/mjx/_src/types.py @@ -14,6 +14,7 @@ # ============================================================================== """Base types used in MJX.""" +import dataclasses import enum from typing import Tuple import jax @@ -22,6 +23,13 @@ from mujoco.mjx._src.dataclasses import PyTreeNode # pylint: disable=g-importin import numpy as np +def _restricted_to(platform: str): + """Specifies whether a field exists in only MuJoCo or MJX.""" + if platform not in ('mujoco', 'mjx'): + raise ValueError(f'unknown platform: {platform}') + return dataclasses.field(metadata={'restricted_to': platform}) + + class DisableBit(enum.IntFlag): """Disable default feature bitflags. @@ -364,12 +372,12 @@ class Option(PyTreeNode): sdf_iterations: max number of iterations for gradient descent (not used) """ timestep: jax.Array - apirate: jax.Array + apirate: jax.Array = _restricted_to('mujoco') impratio: jax.Array tolerance: jax.Array ls_tolerance: jax.Array - noslip_tolerance: jax.Array - mpr_tolerance: jax.Array + noslip_tolerance: jax.Array = _restricted_to('mujoco') + mpr_tolerance: jax.Array = _restricted_to('mujoco') gravity: jax.Array wind: jax.Array magnetic: jax.Array @@ -379,20 +387,20 @@ class Option(PyTreeNode): o_solref: jax.Array o_solimp: jax.Array o_friction: jax.Array - has_fluid_params: bool + has_fluid_params: bool = _restricted_to('mjx') integrator: IntegratorType cone: ConeType jacobian: JacobianType solver: SolverType iterations: int ls_iterations: int - noslip_iterations: int - mpr_iterations: int + noslip_iterations: int = _restricted_to('mujoco') + mpr_iterations: int = _restricted_to('mujoco') disableflags: DisableBit enableflags: int disableactuator: int - sdf_initpoints: int - sdf_iterations: int + sdf_initpoints: int = _restricted_to('mujoco') + sdf_iterations: int = _restricted_to('mujoco') class Statistic(PyTreeNode): @@ -723,22 +731,22 @@ class Model(PyTreeNode): nu: int na: int nbody: int - nbvh: int - nbvhstatic: int - nbvhdynamic: int + nbvh: int = _restricted_to('mujoco') + nbvhstatic: int = _restricted_to('mujoco') + nbvhdynamic: int = _restricted_to('mujoco') njnt: int ngeom: int nsite: int ncam: int nlight: int - nflex: int - nflexvert: int - nflexedge: int - nflexelem: int - nflexelemdata: int - nflexshelldata: int - nflexevpair: int - nflextexcoord: int + nflex: int = _restricted_to('mujoco') + nflexvert: int = _restricted_to('mujoco') + nflexedge: int = _restricted_to('mujoco') + nflexelem: int = _restricted_to('mujoco') + nflexelemdata: int = _restricted_to('mujoco') + nflexshelldata: int = _restricted_to('mujoco') + nflexevpair: int = _restricted_to('mujoco') + nflextexcoord: int = _restricted_to('mujoco') nmesh: int nmeshvert: int nmeshnormal: int @@ -761,11 +769,11 @@ class Model(PyTreeNode): nM: int # pylint:disable=invalid-name nD: int # pylint:disable=invalid-name nB: int # pylint:disable=invalid-name - ntree: int + ntree: int = _restricted_to('mujoco') ngravcomp: int nuserdata: int nsensordata: int - narena: int + narena: int = _restricted_to('mujoco') opt: Option stat: Statistic qpos0: jax.Array @@ -794,11 +802,11 @@ class Model(PyTreeNode): body_margin: np.ndarray body_contype: np.ndarray body_conaffinity: np.ndarray - body_bvhadr: np.ndarray - body_bvhnum: np.ndarray - bvh_child: np.ndarray - bvh_nodeid: np.ndarray - bvh_aabb: np.ndarray + body_bvhadr: np.ndarray = _restricted_to('mujoco') + body_bvhnum: np.ndarray = _restricted_to('mujoco') + bvh_child: np.ndarray = _restricted_to('mujoco') + bvh_nodeid: np.ndarray = _restricted_to('mujoco') + bvh_aabb: np.ndarray = _restricted_to('mujoco') body_invweight0: jax.Array jnt_type: np.ndarray jnt_qposadr: np.ndarray @@ -844,7 +852,7 @@ class Model(PyTreeNode): geom_size: jax.Array geom_aabb: np.ndarray geom_rbound: jax.Array - geom_rbound_hfield: np.ndarray + geom_rbound_hfield: np.ndarray = _restricted_to('mjx') geom_pos: jax.Array geom_quat: jax.Array geom_friction: jax.Array @@ -870,54 +878,54 @@ class Model(PyTreeNode): cam_resolution: np.ndarray cam_sensorsize: np.ndarray cam_intrinsic: np.ndarray - light_mode: np.ndarray - light_bodyid: np.ndarray - light_targetbodyid: np.ndarray - light_pos: np.ndarray - light_dir: np.ndarray - light_poscom0: np.ndarray - light_pos0: np.ndarray - light_dir0: np.ndarray - flex_contype: np.ndarray - flex_conaffinity: np.ndarray - flex_condim: np.ndarray - flex_priority: np.ndarray - flex_solmix: np.ndarray - flex_solref: np.ndarray - flex_solimp: np.ndarray - flex_friction: np.ndarray - flex_margin: np.ndarray - flex_gap: np.ndarray - flex_internal: np.ndarray - flex_selfcollide: np.ndarray - flex_activelayers: np.ndarray - flex_dim: np.ndarray - flex_vertadr: np.ndarray - flex_vertnum: np.ndarray - flex_edgeadr: np.ndarray - flex_edgenum: np.ndarray - flex_elemadr: np.ndarray - flex_elemnum: np.ndarray - flex_elemdataadr: np.ndarray - flex_evpairadr: np.ndarray - flex_evpairnum: np.ndarray - flex_vertbodyid: np.ndarray - flex_edge: np.ndarray - flex_elem: np.ndarray - flex_elemlayer: np.ndarray - flex_evpair: np.ndarray - flex_vert: np.ndarray - flexedge_length0: np.ndarray - flexedge_invweight0: np.ndarray - flex_radius: np.ndarray - flex_edgestiffness: np.ndarray - flex_edgedamping: np.ndarray - flex_edgeequality: np.ndarray - flex_rigid: np.ndarray - flexedge_rigid: np.ndarray - flex_centered: np.ndarray - flex_bvhadr: np.ndarray - flex_bvhnum: np.ndarray + light_mode: np.ndarray = _restricted_to('mujoco') + light_bodyid: np.ndarray = _restricted_to('mujoco') + light_targetbodyid: np.ndarray = _restricted_to('mujoco') + light_pos: np.ndarray = _restricted_to('mujoco') + light_dir: np.ndarray = _restricted_to('mujoco') + light_poscom0: np.ndarray = _restricted_to('mujoco') + light_pos0: np.ndarray = _restricted_to('mujoco') + light_dir0: np.ndarray = _restricted_to('mujoco') + flex_contype: np.ndarray = _restricted_to('mujoco') + flex_conaffinity: np.ndarray = _restricted_to('mujoco') + flex_condim: np.ndarray = _restricted_to('mujoco') + flex_priority: np.ndarray = _restricted_to('mujoco') + flex_solmix: np.ndarray = _restricted_to('mujoco') + flex_solref: np.ndarray = _restricted_to('mujoco') + flex_solimp: np.ndarray = _restricted_to('mujoco') + flex_friction: np.ndarray = _restricted_to('mujoco') + flex_margin: np.ndarray = _restricted_to('mujoco') + flex_gap: np.ndarray = _restricted_to('mujoco') + flex_internal: np.ndarray = _restricted_to('mujoco') + flex_selfcollide: np.ndarray = _restricted_to('mujoco') + flex_activelayers: np.ndarray = _restricted_to('mujoco') + flex_dim: np.ndarray = _restricted_to('mujoco') + flex_vertadr: np.ndarray = _restricted_to('mujoco') + flex_vertnum: np.ndarray = _restricted_to('mujoco') + flex_edgeadr: np.ndarray = _restricted_to('mujoco') + flex_edgenum: np.ndarray = _restricted_to('mujoco') + flex_elemadr: np.ndarray = _restricted_to('mujoco') + flex_elemnum: np.ndarray = _restricted_to('mujoco') + flex_elemdataadr: np.ndarray = _restricted_to('mujoco') + flex_evpairadr: np.ndarray = _restricted_to('mujoco') + flex_evpairnum: np.ndarray = _restricted_to('mujoco') + flex_vertbodyid: np.ndarray = _restricted_to('mujoco') + flex_edge: np.ndarray = _restricted_to('mujoco') + flex_elem: np.ndarray = _restricted_to('mujoco') + flex_elemlayer: np.ndarray = _restricted_to('mujoco') + flex_evpair: np.ndarray = _restricted_to('mujoco') + flex_vert: np.ndarray = _restricted_to('mujoco') + flexedge_length0: np.ndarray = _restricted_to('mujoco') + flexedge_invweight0: np.ndarray = _restricted_to('mujoco') + flex_radius: np.ndarray = _restricted_to('mujoco') + flex_edgestiffness: np.ndarray = _restricted_to('mujoco') + flex_edgedamping: np.ndarray = _restricted_to('mujoco') + flex_edgeequality: np.ndarray = _restricted_to('mujoco') + flex_rigid: np.ndarray = _restricted_to('mujoco') + flexedge_rigid: np.ndarray = _restricted_to('mujoco') + flex_centered: np.ndarray = _restricted_to('mujoco') + flex_bvhadr: np.ndarray = _restricted_to('mujoco') + flex_bvhnum: np.ndarray = _restricted_to('mujoco') mesh_vertadr: np.ndarray mesh_vertnum: np.ndarray mesh_faceadr: np.ndarray @@ -929,7 +937,7 @@ class Model(PyTreeNode): mesh_graph: np.ndarray mesh_pos: np.ndarray mesh_quat: np.ndarray - mesh_convex: Tuple[ConvexMesh, ...] + mesh_convex: Tuple[ConvexMesh, ...] = _restricted_to('mjx') hfield_size: np.ndarray hfield_nrow: np.ndarray hfield_ncol: np.ndarray @@ -969,9 +977,9 @@ class Model(PyTreeNode): tendon_lengthspring: jax.Array tendon_length0: jax.Array tendon_invweight0: jax.Array - wrap_type: np.ndarray - wrap_objid: np.ndarray - wrap_prm: np.ndarray + wrap_type: np.ndarray = _restricted_to('mujoco') + wrap_objid: np.ndarray = _restricted_to('mujoco') + wrap_prm: np.ndarray = _restricted_to('mujoco') actuator_trntype: np.ndarray actuator_dyntype: np.ndarray actuator_gaintype: np.ndarray @@ -994,7 +1002,7 @@ class Model(PyTreeNode): actuator_cranklength: np.ndarray actuator_acc0: np.ndarray actuator_lengthrange: np.ndarray - actuator_plugin: np.ndarray + actuator_plugin: np.ndarray = _restricted_to('mujoco') sensor_type: np.ndarray sensor_datatype: np.ndarray sensor_needstage: np.ndarray @@ -1202,8 +1210,8 @@ class Data(PyTreeNode): xfrc_applied: jax.Array eq_active: jax.Array # mocap data: - mocap_pos: jax.Array - mocap_quat: jax.Array + mocap_pos: jax.Array = _restricted_to('mujoco') + mocap_quat: jax.Array = _restricted_to('mujoco') # dynamics: qacc: jax.Array act_dot: jax.Array @@ -1224,27 +1232,27 @@ class Data(PyTreeNode): site_xmat: jax.Array cam_xpos: jax.Array cam_xmat: jax.Array - light_xpos: jax.Array - light_xdir: jax.Array + light_xpos: jax.Array = _restricted_to('mujoco') + light_xdir: jax.Array = _restricted_to('mujoco') subtree_com: jax.Array cdof: jax.Array cinert: jax.Array - flexvert_xpos: jax.Array + flexvert_xpos: jax.Array = _restricted_to('mujoco') flexelem_aabb: jax.Array - flexedge_J_rownnz: jax.Array # pylint:disable=invalid-name - flexedge_J_rowadr: jax.Array # pylint:disable=invalid-name - flexedge_J_colind: jax.Array # pylint:disable=invalid-name - flexedge_J: jax.Array # pylint:disable=invalid-name - flexedge_length: jax.Array - ten_wrapadr: jax.Array - ten_wrapnum: jax.Array - ten_J_rownnz: jax.Array # pylint:disable=invalid-name - ten_J_rowadr: jax.Array # pylint:disable=invalid-name - ten_J_colind: jax.Array # pylint:disable=invalid-name + flexedge_J_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + flexedge_J_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + flexedge_J_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + flexedge_J: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + flexedge_length: jax.Array = _restricted_to('mujoco') + ten_wrapadr: jax.Array = _restricted_to('mujoco') + ten_wrapnum: jax.Array = _restricted_to('mujoco') + ten_J_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + ten_J_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + ten_J_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name ten_J: jax.Array # pylint:disable=invalid-name ten_length: jax.Array - wrap_obj: jax.Array - wrap_xpos: jax.Array + wrap_obj: jax.Array = _restricted_to('mujoco') + wrap_xpos: jax.Array = _restricted_to('mujoco') actuator_length: jax.Array actuator_moment: jax.Array crb: jax.Array @@ -1252,32 +1260,32 @@ class Data(PyTreeNode): qLD: jax.Array # pylint:disable=invalid-name qLDiagInv: jax.Array # pylint:disable=invalid-name qLDiagSqrtInv: jax.Array # pylint:disable=invalid-name - bvh_aabb_dyn: jax.Array - bvh_active: jax.Array + bvh_aabb_dyn: jax.Array = _restricted_to('mujoco') + bvh_active: jax.Array = _restricted_to('mujoco') # position, velocity dependent: - flexedge_velocity: jax.Array + flexedge_velocity: jax.Array = _restricted_to('mujoco') ten_velocity: jax.Array actuator_velocity: jax.Array cvel: jax.Array cdof_dot: jax.Array qfrc_bias: jax.Array - qfrc_spring: jax.Array - qfrc_damper: jax.Array + qfrc_spring: jax.Array = _restricted_to('mujoco') + qfrc_damper: jax.Array = _restricted_to('mujoco') qfrc_gravcomp: jax.Array qfrc_fluid: jax.Array qfrc_passive: jax.Array subtree_linvel: jax.Array subtree_angmom: jax.Array - qH: jax.Array # pylint:disable=invalid-name - qHDiagInv: jax.Array # pylint:disable=invalid-name - D_rownnz: jax.Array # pylint:disable=invalid-name - D_rowadr: jax.Array # pylint:disable=invalid-name - D_colind: jax.Array # pylint:disable=invalid-name - B_rownnz: jax.Array # pylint:disable=invalid-name - B_rowadr: jax.Array # pylint:disable=invalid-name - B_colind: jax.Array # pylint:disable=invalid-name - qDeriv: jax.Array # pylint:disable=invalid-name - qLU: jax.Array # pylint:disable=invalid-name + qH: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + qHDiagInv: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + D_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + B_rownnz: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + B_rowadr: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + B_colind: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + qDeriv: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name + qLU: jax.Array = _restricted_to('mujoco') # pylint:disable=invalid-name # position, velocity, control & acceleration dependent: qfrc_actuator: jax.Array actuator_force: jax.Array @@ -1302,6 +1310,6 @@ class Data(PyTreeNode): efc_force: jax.Array # sparse representation of qM, qLD, qLDiagInv, for compatibility with MuJoCo # when in dense mode - _qM_sparse: jax.Array # pylint:disable=invalid-name - _qLD_sparse: jax.Array # pylint:disable=invalid-name - _qLDiagInv_sparse: jax.Array # pylint:disable=invalid-name + _qM_sparse: jax.Array = _restricted_to('mjx') # pylint:disable=invalid-name + _qLD_sparse: jax.Array = _restricted_to('mjx') # pylint:disable=invalid-name + _qLDiagInv_sparse: jax.Array = _restricted_to('mjx') # pylint:disable=invalid-name