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/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/XMLreference.rst b/doc/XMLreference.rst index b6f240ec..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` @@ -1106,7 +1112,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 +1197,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; @@ -3806,7 +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 not yet supported. When attaching, all keyframes will be deleted. + - :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 f601660f..54b706c6 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,8 +7,29 @@ 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. Added support for :ref:`attaching` keyframes. + +MJX +^^^ +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 +^^^^^^^^^ +8. Fixed a performance regression introduced in 3.1.7 in mesh Bounding Volume Hierarchies (:github:issue:`1875`, + contribution by :github:user:`michael-ahn`). +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 +^^^^^^^^^^^^^^^ +10. Added support for engine plugins in :ref:`mjSpec` (:github:issue:`1903`). + Version 3.2.2 (Aug 8, 2024) --------------------------- 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. 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. 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 202cfd9d..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=int), - geom2=jp.full((ncon,), -1, dtype=int), - geom=jp.full((ncon, 2), -1, dtype=int), - 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,118 +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_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 @@ -347,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': @@ -375,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) @@ -415,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) @@ -433,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'): @@ -461,7 +513,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]) @@ -482,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) @@ -496,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/sensor.py b/mjx/mujoco/mjx/_src/sensor.py index a59e9c58..7dabeeb2 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,115 @@ 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 + stage_pos = m.sensor_needstage == mujoco.mjtStage.mjSTAGE_POS + if sum(stage_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[stage_pos]): + 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)) + 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) + ) return d.replace(sensordata=sensordata) 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/_src/types.py b/mjx/mujoco/mjx/_src/types.py index d5c3bdcc..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. @@ -282,11 +290,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): @@ -329,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 @@ -344,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): @@ -688,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 @@ -726,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 @@ -759,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 @@ -809,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 @@ -835,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 @@ -894,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 @@ -934,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 @@ -959,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 @@ -1137,6 +1180,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,) @@ -1166,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 @@ -1188,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 @@ -1216,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 @@ -1257,6 +1301,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: @@ -1265,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 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 @@ - + + + + + + + + + + + + + + + + + + + - + + + + + + + + + + + + + + + + + + + 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 @@ + + + + + + + + + + + + + + + + + + + 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/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 18c4ffe7..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); } @@ -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 @@ -992,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/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/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); } diff --git a/src/user/user_api.cc b/src/user/user_api.cc index 1a03b274..4743f83b 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); + 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); + 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; } } @@ -604,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/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] = { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 0d25cc59..cbd37927 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -213,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; @@ -230,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; + } + } } @@ -315,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); @@ -335,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); @@ -343,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(); @@ -1153,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() { @@ -2860,16 +2883,36 @@ 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 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 (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(state_name), qpos + joint->qposadr_, joint->nq()); + if (qvel) mjuu_copyvec(joint->qvel(state_name), 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_); + for (unsigned int i=0; iactadr_ != -1 && actuator->actdim_ != -1 && act) { + actuator->act(state_name).assign(actuator->actdim_, 0); + mjuu_copyvec(actuator->act(state_name).data(), act + actuator->actadr_, actuator->actdim_); + } + if (ctrl) { + 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); } } } @@ -2890,59 +2933,93 @@ 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 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 (auto actuator : actuators_) { - if (mjuu_defined(actuator->act[0]) && act) { - mjuu_copyvec(act + actuator->actadr_, actuator->act.data(), actuator->actdim_); + for (unsigned int i=0; iact(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(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); +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); +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); // 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; info.qpos = !key->spec_qpos_.empty(); 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); - SaveState(key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); - break; // (b/350784262) save only the first keyframe for now + 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) { @@ -3484,7 +3561,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()) { @@ -3500,19 +3580,53 @@ 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); + } + 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; + 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); - RestoreState(m->qpos0, key->spec_qpos_.data(), key->spec_qvel_.data(), key->spec_act_.data()); + if (info.ctrl) key->spec_ctrl_.assign(nu, 0); + 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 22e37135..f4ccf839 100644 --- a/src/user/user_model.h +++ b/src/user/user_model.h @@ -36,9 +36,13 @@ typedef std::array mjListKeyMap; typedef struct mjKeyInfo_ { std::string name; + double time; bool qpos; bool qvel; bool act; + bool ctrl; + bool mpos; + bool mquat; } mjKeyInfo; class mjCModel_ : public mjsElement { @@ -282,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); - template void RestoreState(const mjtNum* pos0, T* qpos, T* qvel, T* act); + // 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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index fb0a18fd..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; } @@ -1408,9 +1405,11 @@ 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(); } + 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 +1417,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(); @@ -1769,8 +1786,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 +1845,24 @@ int mjCJoint::nv(mjtJoint joint_type) { +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(state_name).data(); +} + + + +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(state_name).data(); +} + + + void mjCJoint::PointToLocal() { spec.element = static_cast(this); spec.name = &name; @@ -2090,15 +2125,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; } @@ -2106,10 +2141,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; } @@ -2117,15 +2152,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 @@ -2135,18 +2172,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; } @@ -2185,16 +2220,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; } @@ -2203,6 +2238,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; @@ -2226,22 +2277,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; @@ -2251,6 +2286,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; @@ -2276,10 +2315,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; } @@ -2288,6 +2323,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; @@ -2323,12 +2364,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; } @@ -2338,6 +2373,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 @@ -2374,12 +2415,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; @@ -5349,7 +5384,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 +5410,8 @@ mjCActuator& mjCActuator::operator=(const mjCActuator& other) { void mjCActuator::ForgetKeyframes() { - act.clear(); - act.push_back(mjNAN); + act_.clear(); + ctrl_.clear(); } @@ -5386,6 +5422,24 @@ bool mjCActuator::is_actlimited() const { return islimited(actlimited, actrange) +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(state_name); +} + + + +mjtNum& mjCActuator::ctrl(const std::string& state_name) { + if (ctrl_.find(state_name) == ctrl_.end()) { + ctrl_[state_name] = mjNAN; + } + return ctrl_.at(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..c55cdcd0 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 @@ -254,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 { @@ -322,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 @@ -401,10 +410,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 +452,9 @@ class mjCJoint : public mjCJoint_, private mjsJoint { int nq() const { return nq(spec.type); } int nv() const { return nv(spec.type); } + mjtNum* qpos(const std::string& state_name); + mjtNum* qvel(const std::string& state_name); + private: int Compile(void); // compiler; return dofnum void PointToLocal(void); @@ -1390,9 +1402,10 @@ 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 + std::map ctrl_; // ctrl at the previous step // variable-size data std::string plugin_name; @@ -1431,6 +1444,9 @@ class mjCActuator : public mjCActuator_, private mjsActuator { bool is_forcelimited() const; bool is_actlimited() const; + std::vector& act(const std::string& state_name); + mjtNum& ctrl(const std::string& state_name); + private: void Compile(void); // compiler void CopyFromSpec(); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 83484035..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); @@ -1319,115 +1320,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 +1436,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 +1444,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 +1452,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 +1511,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 +1529,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 +1543,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,143 +1577,143 @@ 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; + string hfieldname, meshname, material; int n; // 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()); } @@ -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)) { @@ -1752,41 +1753,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 +1799,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 +1935,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 +2051,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 +2077,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 +2127,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,17 +2295,17 @@ 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()); } // make composite -void mjXReader::OneComposite(XMLElement* elem, mjsBody* pbody, mjsDefault* def) { +void mjXReader::OneComposite(XMLElement* elem, mjsBody* body, mjsDefault* def) { string text; int n; @@ -2328,7 +2329,7 @@ void mjXReader::OneComposite(XMLElement* elem, mjsBody* pbody, 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* pbody, 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* pbody, 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* pbody, 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); @@ -2526,7 +2527,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) { @@ -2537,7 +2538,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 +2660,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) { @@ -2672,8 +2673,8 @@ void mjXReader::OneFlexcomp(XMLElement* elem, mjsBody* pbody, 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"); @@ -2848,7 +2849,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,70 +2863,70 @@ 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); std::vector objtype; - std::string objname = ""; + string objname = ""; std::vector objprm; while (obj) { @@ -2935,20 +2936,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 +2957,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,64 +3123,64 @@ 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 - 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++) { @@ -3190,59 +3191,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 +3263,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()); } } } @@ -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()); } @@ -3302,19 +3303,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 +3328,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 +3352,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; + 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 +3403,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,12 +3446,12 @@ 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); // read attributes - std::string name, childclass; + string name, childclass; if (ReadAttrTxt(elem, "name", name)) { mjs_setString(pframe->name, name.c_str()); } @@ -3461,7 +3462,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 @@ -3469,7 +3470,7 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, 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); @@ -3494,7 +3495,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}; @@ -3520,11 +3521,11 @@ void mjXReader::Body(XMLElement* section, mjsBody* pbody, 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 - 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,52 +3547,51 @@ 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, 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; + 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 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); 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 +3644,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 +3687,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 +3717,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 +3751,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 +3817,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 +3833,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 +4074,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 +4173,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 @@ -4233,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 23c77c63..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); - 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 +182,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 +194,99 @@ void mjXWriter::OneFlex(XMLElement* elem, const mjCFlex* pflex) { // write mesh -void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* pmesh, mjCDef* def) { - std::string text; +void mjXWriter::OneMesh(XMLElement* elem, const mjCMesh* mesh, mjCDef* def) { + 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) { - std::string text; +void mjXWriter::OneSkin(XMLElement* elem, const mjCSkin* skin) { + 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 +295,127 @@ 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, - std::string_view classname) { +void mjXWriter::OneJoint(XMLElement* elem, const mjCJoint* joint, mjCDef* def, + 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, - std::string_view classname) { +void mjXWriter::OneGeom(XMLElement* elem, const mjCGeom* geom, mjCDef* def, 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 +425,228 @@ 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, - std::string_view classname) { +void mjXWriter::OneSite(XMLElement* elem, const mjCSite* site, mjCDef* def, 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, - std::string_view classname) { +void mjXWriter::OneCamera(XMLElement* elem, const mjCCamera* camera, mjCDef* def, + 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, - std::string_view classname) { +void mjXWriter::OneLight(XMLElement* elem, const mjCLight* light, mjCDef* def, + 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()); } } @@ -803,8 +803,8 @@ void mjXWriter::OneActuator(XMLElement* elem, const mjCActuator* pact, mjCDef* d // 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); @@ -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; } } @@ -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]); @@ -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()) { - std::string text; - Vector2String(text, phf->get_userdata(), phf->ncol); + WriteAttrInt(elem, "nrow", hfield->nrow); + WriteAttrInt(elem, "ncol", hfield->ncol); + if (!hfield->get_userdata().empty()) { + 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); } @@ -1734,21 +1732,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 +1765,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 +1792,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 +1822,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 +1882,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 +1911,231 @@ void mjXWriter::Sensor(XMLElement* root) { // write all sensors for (int i=0; iSensors()[i]; - std::string instance_name = ""; - std::string plugin_name = ""; + mjCSensor* sensor = model->Sensors()[i]; + string instance_name = ""; + 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 +2147,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 +2169,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 +2195,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 +2220,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 +2229,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 +2240,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 +2257,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 +2273,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; } diff --git a/test/engine/engine_collision_gjk_test.cc b/test/engine/engine_collision_gjk_test.cc index 86dd9f09..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"( @@ -218,7 +247,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); } 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, diff --git a/test/user/user_api_test.cc b/test/user/user_api_test.cc index e5c581d5..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; @@ -430,8 +466,8 @@ static constexpr char xml_child[] = R"( - - + + )"; @@ -501,9 +537,10 @@ TEST_F(MujocoTest, AttachSame) { - - - + + + + )"; @@ -620,7 +657,8 @@ TEST_F(MujocoTest, AttachDifferent) { - + + )"; @@ -741,7 +779,8 @@ TEST_F(MujocoTest, AttachFrame) { - + + )"; @@ -813,6 +852,11 @@ void TestDetachBody(bool compile) { + + + + + )"; // model with one cylinder and a hinge @@ -867,6 +911,8 @@ TEST_F(MujocoTest, PreserveState) { + + @@ -885,6 +931,7 @@ TEST_F(MujocoTest, PreserveState) { + @@ -912,6 +959,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); @@ -922,6 +977,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()); @@ -965,6 +1025,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); @@ -976,5 +1047,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