From 47ba72ea59a40be1e7e03765de91d8ec51f24d22 Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Thu, 28 Mar 2024 14:00:55 -0700 Subject: [PATCH] Add `actuatorgravcomp` joint attribute, to treat gravity compensation forces as applied by actuators, rather than passive buoyancy. PiperOrigin-RevId: 620049593 Change-Id: I2c8a9dc152c087b408e4f904838034271a7dd910 --- doc/XMLreference.rst | 68 ++++++++++++-------- doc/XMLschema.rst | 10 +-- doc/changelog.rst | 45 +++++++------ doc/includes/references.h | 7 +- include/mujoco/mjmodel.h | 7 +- include/mujoco/mjxmacro.h | 1 + introspect/structs.py | 13 +++- src/engine/engine_forward.c | 48 ++++++++++++-- src/engine/engine_passive.c | 33 +++++++++- src/user/user_api.h | 1 + src/user/user_model.cc | 1 + src/xml/xml_native_reader.cc | 14 ++-- src/xml/xml_native_writer.cc | 1 + test/engine/engine_forward_test.cc | 74 ++++++++++++++++++++++ test/engine/testdata/actuation/refsite.xml | 12 ++-- unity/Runtime/Bindings/MjBindings.cs | 1 + 16 files changed, 257 insertions(+), 79 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index e9bbde58..81bb0c3d 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -2354,24 +2354,6 @@ rotations as unit quaternions. joint inertia in the model reference configuration. Note that the format is the same as the solref parameter of the constraint solver. -.. _body-joint-limited: - -:at:`limited`: :at-val:`[false, true, auto], "auto"` - This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute - is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this - attribute is "auto", and :at:`autolimits` is set in :ref:`compiler `, joint limits will be enabled - if range is defined. - -.. _body-joint-actuatorfrclimited: - -:at:`actuatorfrclimited`: :at-val:`[false, true, auto], "auto"` - This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for - details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This - attribute interacts with the actuatorfrcrange attribute below. If this attribute is "false", actuator force - clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and - :at:`autolimits` is set in :ref:`compiler `, actuator force clamping will be enabled if actuatorfrcrange - is defined. - .. _body-joint-solreflimit: .. _body-joint-solimplimit: @@ -2400,16 +2382,44 @@ rotations as unit quaternions. joints, the limit is imposed on the angle of rotation (relative to the reference configuration) regardless of the axis of rotation. Only the second range parameter is used for ball joints; the first range parameter should be set to 0. See the :ref:`Limit ` section in the Computation chapter for more information. - |br| Setting this attribute without specifying :at:`limited` is an error, unless :at:`autolimits` is set in + |br| Setting this attribute without specifying :at:`limited` is an error if :at:`autolimits` is "false" in :ref:`compiler `. +.. _body-joint-limited: + +:at:`limited`: :at-val:`[false, true, auto], "auto"` + This attribute specifies if the joint has limits. It interacts with the range attribute below. If this attribute + is "false", joint limits are disabled. If this attribute is "true", joint limits are enabled. If this + attribute is "auto", and :at:`autolimits` is set in :ref:`compiler `, joint limits will be enabled + if range is defined. + .. _body-joint-actuatorfrcrange: :at:`actuatorfrcrange`: :at-val:`real(2), "0 0"` Range for clamping total actuator forces acting on this joint. See :ref:`CForceRange` for details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| The compiler expects the first value to be smaller than the second value. |br| Setting this attribute without specifying :at:`actuatorfrclimited` - is an error, unless :at:`compiler-autolimits` is set. + is an error if :at:`compiler-autolimits` is "false". + +.. _body-joint-actuatorfrclimited: + +:at:`actuatorfrclimited`: :at-val:`[false, true, auto], "auto"` + This attribute specifies whether actuator forces acting on the joint should be clamped. See :ref:`CForceRange` for + details. It is available only for scalar joints (hinge and slider) and ignored for ball and free joints. |br| This + attribute interacts with the actuatorfrcrange attribute below. If this attribute is "false", actuator force + clamping is disabled. If it is "true", actuator force clamping is enabled. If this attribute is "auto", and + :at:`autolimits` is set in :ref:`compiler `, actuator force clamping will be enabled if + :at:`actuatorfrcrange` is defined. + +.. _body-joint-actuatorgravcomp: + +:at:`actuatorgravcomp`: :at-val:`[false, true], "false"` + If this flag is enabled, gravity compensation applied to this joint is added to actuator forces + (``mjData.qfrc_actuator``) rather than passive forces (``mjData.qfrc_passive``). Notionally, this means that gravity + compensation is the result of a control system rather than natural buoyancy. In practice, enabling this flag is + useful when joint-level actuator force clamping is used. In this case, the total actuation force applied on a joint, + including gravity compensation, is guaranteed to not exceeed the specified limits. See :ref:`CForceRange` and + :ref:`actuatorfrcrange` for more details on this type of force limit. .. _body-joint-margin: @@ -5096,14 +5106,14 @@ specify them independently. :at:`ctrlrange`: :at-val:`real(2), "0 0"` Range for clamping the control input. The first value must be smaller than the second value. - |br| Setting this attribute without specifying :at:`ctrllimited` is an error, unless :at:`autolimits` is set in + |br| Setting this attribute without specifying :at:`ctrllimited` is an error if :at:`autolimits` is "false" in :ref:`compiler `. .. _actuator-general-forcerange: :at:`forcerange`: :at-val:`real(2), "0 0"` Range for clamping the force output. The first value must be no greater than the second value. - |br| Setting this attribute without specifying :at:`forcelimited` is an error, unless :at:`autolimits` is set in + |br| Setting this attribute without specifying :at:`forcelimited` is an error if :at:`autolimits` is "false" in :ref:`compiler `. .. _actuator-general-actrange: @@ -5111,7 +5121,7 @@ specify them independently. :at:`actrange`: :at-val:`real(2), "0 0"` Range for clamping the activation state. The first value must be no greater than the second value. See the :ref:`Activation clamping ` section for more details. - |br| Setting this attribute without specifying :at:`actlimited` is an error, unless :at:`autolimits` is set in + |br| Setting this attribute without specifying :at:`actlimited` is an error if :at:`autolimits` is "false" in :ref:`compiler `. .. _actuator-general-lengthrange: @@ -6489,10 +6499,12 @@ arms determined by the transmission). This sensor can be attached to any actuato :el-prefix:`sensor/` |-| **jointactuatorfrc** (*) ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the -generalized force contributed by all actuators to a single scalar joint (hinge or slider). This type of sensor is -important when multiple actuators act on a single joint or when a single actuator act on multiple joints. See -:ref:`CForceRange` for details. +This element creates an actuator force sensor, measured at a joint. The quantity being sensed is the generalized force +contributed by all actuators to a single scalar joint (hinge or slider). If the joint's +:ref:`actuatorgravcomp` attribute is "true", this sensor will also measure contributions by +gravity compensation forces (which are added directly to the joint and would *not* register in the +:ref:`actuatorfrc`) sensor. This type of sensor is important when multiple actuators act on a single +joint or when a single actuator act on multiple joints. See :ref:`CForceRange` for details. .. _sensor-jointactuatorfrc-name: @@ -7386,6 +7398,8 @@ if omitted. .. _default-joint-actuatorfrclimited: +.. _default-joint-actuatorgravcomp: + .. _default-joint-solreflimit: .. _default-joint-solimplimit: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 03a651af..3327930d 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -286,9 +286,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`solimpfriction` | :ref:`stiffness` | :ref:`range` | :ref:`actuatorfrcrange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`margin` | :ref:`ref` | :ref:`springref` | :ref:`armature` | | +| | | | :ref:`actuatorgravcomp` | :ref:`margin` | :ref:`ref` | :ref:`springref` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`damping` | :ref:`frictionloss` | :ref:`user` | | | +| | | | :ref:`armature` | :ref:`damping` | :ref:`frictionloss` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| body |br| |_| |L| | | .. table:: | @@ -1320,11 +1320,11 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`solimplimit` | :ref:`solreffriction` | :ref:`solimpfriction` | :ref:`stiffness` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`range` | :ref:`actuatorfrcrange` | :ref:`margin` | :ref:`ref` | | +| | | | :ref:`range` | :ref:`actuatorfrcrange` | :ref:`actuatorgravcomp` | :ref:`margin` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`springref` | :ref:`armature` | :ref:`damping` | :ref:`frictionloss` | | +| | | | :ref:`ref` | :ref:`springref` | :ref:`armature` | :ref:`damping` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`user` | | | | | +| | | | :ref:`frictionloss` | :ref:`user` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index 940f74f2..027d3221 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -7,15 +7,35 @@ Upcoming version (not yet released) General ^^^^^^^ -1. Added support for gmsh format 2.2, as generated by e.g. `fTetwild `__. +1. Added the :ref:`actuatorgravcomp` joint attribute. When enabled, gravity compensation + forces on the joint are treated as applied by actuators. See attribute documentation for more details. The example + model + `refsite.xml `__, + which demostrates Cartesian actuation of an arm, has been updated to use this attribute. +2. Added support for gmsh format 2.2, as generated by e.g. `fTetwild `__. MJX ^^^ +3. Improved performance of SAT for convex collisions. +4. Fixed bug for sphere/capsule-convex deep penetration. +5. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``. +6. Throw an error for margin/gap for convex mesh collisions, since they are not supported. + +Simulate +^^^^^^^^ +7. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the + :ref:`invdiscrete` or :ref:`sensornoise` + flags would actually toggle the other flag. + +Python bindings +^^^^^^^^^^^^^^^ +8. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID). + The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse + Kinematics, is available here: |ls_colab| + +.. |ls_colab| image:: https://colab.research.google.com/assets/colab-badge.svg + :target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb -2. Improved performance of SAT for convex collisions. -3. Fixed bug for sphere/capsule-convex deep penetration. -4. Fixed bug where ``mjx.Data`` produced by ``mjx.put_data`` had different treedef than ``mjx.make_data``. -5. Throw an error for margin/gap for convex mesh collisions, since they are not supported. Version 3.1.3 (March 5th, 2024) ------------------------------- @@ -37,7 +57,6 @@ General MJX ^^^ - 4. Improved performance of getting and putting device data. - Use ``tobytes()`` for numpy array serialization, which is orders of magnitude faster than converting to tuples. @@ -56,21 +75,9 @@ Python bindings ^^^^^^^^^^^^^^^ 11. Fixed incorrect data types in the bindings for the ``geom``, ``vert``, ``elem``, and ``flex`` array members of the ``mjContact`` struct, and all array members of the ``mjrContext`` struct. -12. Added the ``mujoco.minimize`` Python module for nonlinear least-squares, designed for System Identification (sysID). - The sysID tutorial is work in progress, but a pedagogical colab notebook with examples, including Inverse - Kinematics, is available here: |ls_colab| - -.. |ls_colab| image:: https://colab.research.google.com/assets/colab-badge.svg - :target: https://colab.research.google.com/github/google-deepmind/mujoco/blob/main/python/least_squares.ipynb - -Simulate -^^^^^^^^ -13. Fixed bug in order of enable flag strings. Before this change, using the simulate UI to toggle the - :ref:`invdiscrete` or :ref:`sensornoise` - flags would actually toggle the other flag. Version 3.1.2 (February 05, 2024) ------------------------------------ +--------------------------------- General ^^^^^^^ diff --git a/doc/includes/references.h b/doc/includes/references.h index 6fd2a650..8bb4ee1a 100644 --- a/doc/includes/references.h +++ b/doc/includes/references.h @@ -926,9 +926,9 @@ struct mjModel_ { int nemax; // number of potential equality-constraint rows int njmax; // number of available rows in constraint Jacobian int nconmax; // number of potential contacts in contact list - int nuserdata; // number of extra fields in mjData - int nsensordata; // number of fields in sensor data vector - int npluginstate; // number of fields in plugin state vector + int nuserdata; // number of mjtNums reserved for the user + int nsensordata; // number of mjtNums in sensor data vector + int npluginstate; // number of mjtNums in plugin state vector size_t narena; // number of bytes in the mjData arena (inclusive of stack) size_t nbuffer; // number of bytes in buffer @@ -993,6 +993,7 @@ struct mjModel_ { int* jnt_group; // group for visibility (njnt x 1) mjtByte* jnt_limited; // does joint have limits (njnt x 1) mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1) + mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (njnt x 1) mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF) mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) diff --git a/include/mujoco/mjmodel.h b/include/mujoco/mjmodel.h index 15840a82..f4c480c6 100644 --- a/include/mujoco/mjmodel.h +++ b/include/mujoco/mjmodel.h @@ -641,9 +641,9 @@ struct mjModel_ { int nemax; // number of potential equality-constraint rows int njmax; // number of available rows in constraint Jacobian int nconmax; // number of potential contacts in contact list - int nuserdata; // number of extra fields in mjData - int nsensordata; // number of fields in sensor data vector - int npluginstate; // number of fields in plugin state vector + int nuserdata; // number of mjtNums reserved for the user + int nsensordata; // number of mjtNums in sensor data vector + int npluginstate; // number of mjtNums in plugin state vector size_t narena; // number of bytes in the mjData arena (inclusive of stack) size_t nbuffer; // number of bytes in buffer @@ -708,6 +708,7 @@ struct mjModel_ { int* jnt_group; // group for visibility (njnt x 1) mjtByte* jnt_limited; // does joint have limits (njnt x 1) mjtByte* jnt_actfrclimited; // does joint have actuator force limits (njnt x 1) + mjtByte* jnt_actgravcomp; // is gravcomp force applied via actuators (njnt x 1) mjtNum* jnt_solref; // constraint solver reference: limit (njnt x mjNREF) mjtNum* jnt_solimp; // constraint solver impedance: limit (njnt x mjNIMP) mjtNum* jnt_pos; // local anchor position (njnt x 3) diff --git a/include/mujoco/mjxmacro.h b/include/mujoco/mjxmacro.h index a46aea71..7cb4fa7b 100644 --- a/include/mujoco/mjxmacro.h +++ b/include/mujoco/mjxmacro.h @@ -215,6 +215,7 @@ XMJV( int, jnt_group, njnt, 1 ) \ X ( mjtByte, jnt_limited, njnt, 1 ) \ X ( mjtByte, jnt_actfrclimited, njnt, 1 ) \ + X ( mjtByte, jnt_actgravcomp, njnt, 1 ) \ X ( mjtNum, jnt_solref, njnt, mjNREF ) \ X ( mjtNum, jnt_solimp, njnt, mjNIMP ) \ X ( mjtNum, jnt_pos, njnt, 3 ) \ diff --git a/introspect/structs.py b/introspect/structs.py index 67c44261..980a9f0f 100644 --- a/introspect/structs.py +++ b/introspect/structs.py @@ -1218,17 +1218,17 @@ STRUCTS: Mapping[str, StructDecl] = dict([ StructFieldDecl( name='nuserdata', type=ValueType(name='int'), - doc='number of extra fields in mjData', + doc='number of mjtNums reserved for the user', ), StructFieldDecl( name='nsensordata', type=ValueType(name='int'), - doc='number of fields in sensor data vector', + doc='number of mjtNums in sensor data vector', ), StructFieldDecl( name='npluginstate', type=ValueType(name='int'), - doc='number of fields in plugin state vector', + doc='number of mjtNums in plugin state vector', ), StructFieldDecl( name='narena', @@ -1556,6 +1556,13 @@ STRUCTS: Mapping[str, StructDecl] = dict([ ), doc='does joint have actuator force limits (njnt x 1)', ), + StructFieldDecl( + name='jnt_actgravcomp', + type=PointerType( + inner_type=ValueType(name='mjtByte'), + ), + doc='is gravcomp force applied via actuators (njnt x 1)', + ), StructFieldDecl( name='jnt_solref', type=PointerType( diff --git a/src/engine/engine_forward.c b/src/engine/engine_forward.c index bcaa55ff..8275f751 100644 --- a/src/engine/engine_forward.c +++ b/src/engine/engine_forward.c @@ -244,7 +244,7 @@ static mjtNum nextActivation(const mjModel* m, const mjData* d, // clamp vector to range -static void mju_clamp(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, +static void clampVec(mjtNum* vec, const mjtNum* range, const mjtByte* limited, int n, const int* index) { for (int i=0; i < n; i++) { int j = index ? index[i] : i; @@ -277,7 +277,7 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { mjtNum *ctrl = mj_stackAllocNum(d, nu); mju_copy(ctrl, d->ctrl, nu); if (!mjDISABLED(mjDSBL_CLAMPCTRL)) { - mju_clamp(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL); + clampVec(ctrl, m->actuator_ctrlrange, m->actuator_ctrllimited, nu, NULL); } // check controls, set all to 0 if any are bad @@ -463,13 +463,47 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { } // clamp actuator_force - mju_clamp(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); + clampVec(force, m->actuator_forcerange, m->actuator_forcelimited, nu, NULL); // qfrc_actuator = moment' * force mju_mulMatTVec(d->qfrc_actuator, moment, force, nu, nv); - // clamp qfrc_actuator - mju_clamp(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr); + // actuator-level gravity compensation + if (!mjDISABLED(mjDSBL_GRAVITY) && mju_norm3(m->opt.gravity)) { + int njnt = m->njnt; + for (int i=0; i < njnt; i++) { + // skip if gravcomp added as passive force + if (!m->jnt_actgravcomp[i]) { + continue; + } + + // get number of dofs for this joint + int dofnum; + switch (m->jnt_type[i]) { + case mjJNT_HINGE: + case mjJNT_SLIDE: + dofnum = 1; + break; + + case mjJNT_BALL: + dofnum = 3; + break; + + case mjJNT_FREE: + dofnum = 6; + break; + } + + // add gravcomp force + int dofadr = m->jnt_dofadr[i]; + for (int j=0; j < dofnum; j++) { + d->qfrc_actuator[dofadr+j] += d->qfrc_gravcomp[dofadr+j]; + } + } + } + + // clamp qfrc_actuator to joint-level actuator force limits + clampVec(d->qfrc_actuator, m->jnt_actfrcrange, m->jnt_actfrclimited, m->njnt, m->jnt_dofadr); mj_freeStack(d); TM_END(mjTIMER_ACTUATION); @@ -481,13 +515,13 @@ void mj_fwdActuation(const mjModel* m, mjData* d) { void mj_fwdAcceleration(const mjModel* m, mjData* d) { int nv = m->nv; - // qforce = sum of all non-constraint forces + // qfrc_smooth = sum of all non-constraint forces mju_sub(d->qfrc_smooth, d->qfrc_passive, d->qfrc_bias, nv); // qfrc_bias is negative mju_addTo(d->qfrc_smooth, d->qfrc_applied, nv); mju_addTo(d->qfrc_smooth, d->qfrc_actuator, nv); mj_xfrcAccumulate(m, d, d->qfrc_smooth); - // qacc_smooth = M \ qfr_smooth + // qacc_smooth = M \ qfrc_smooth mj_solveM(m, d, d->qacc_smooth, d->qfrc_smooth, 1); } diff --git a/src/engine/engine_passive.c b/src/engine/engine_passive.c index 4b714527..6df4ef23 100644 --- a/src/engine/engine_passive.c +++ b/src/engine/engine_passive.c @@ -255,8 +255,39 @@ void mj_passive(const mjModel* m, mjData* d) { // add passive forces into qfrc_passive mju_add(d->qfrc_passive, d->qfrc_spring, d->qfrc_damper, nv); - if (has_gravcomp) mju_addTo(d->qfrc_passive, d->qfrc_gravcomp, nv); if (has_fluid) mju_addTo(d->qfrc_passive, d->qfrc_fluid, nv); + if (has_gravcomp) { + int njnt = m->njnt; + for (int i=0; i < njnt; i++) { + // skip if gravcomp added via actuators + if (m->jnt_actgravcomp[i]) { + continue; + } + + // get number of dofs for this joint + int dofnum; + switch (m->jnt_type[i]) { + case mjJNT_HINGE: + case mjJNT_SLIDE: + dofnum = 1; + break; + + case mjJNT_BALL: + dofnum = 3; + break; + + case mjJNT_FREE: + dofnum = 6; + break; + } + + // add gravcomp force + int dofadr = m->jnt_dofadr[i]; + for (int j=0; j < dofnum; j++) { + d->qfrc_passive[dofadr+j] += d->qfrc_gravcomp[dofadr+j]; + } + } + } // user callback: add custom passive forces if (mjcb_passive) { diff --git a/src/user/user_api.h b/src/user/user_api.h index b448f3e4..c12a4e4c 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -230,6 +230,7 @@ typedef struct _mjsJoint { // joint specification // other int group; // group + mjtByte actgravcomp; // is gravcomp force applied via actuators double urdfeffort; // effort (urdf) mjDoubleVec userdata; // user data mjString info; // message appended to compiler errors diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 0217359c..c80077be 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -1768,6 +1768,7 @@ void mjCModel::CopyTree(mjModel* m) { m->jnt_group[jid] = pj->group; m->jnt_limited[jid] = (mjtByte)pj->is_limited(); m->jnt_actfrclimited[jid] = (mjtByte)pj->is_actfrclimited(); + m->jnt_actgravcomp[jid] = pj->actgravcomp; m->jnt_qposadr[jid] = qposadr; m->jnt_dofadr[jid] = dofadr; m->jnt_bodyid[jid] = pj->body->id; diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 48f5547f..11811c2b 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -138,10 +138,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"mesh", "?", "1", "scale"}, {"material", "?", "8", "texture", "emission", "specular", "shininess", "reflectance", "rgba", "texrepeat", "texuniform"}, - {"joint", "?", "21", "type", "group", "pos", "axis", "springdamper", + {"joint", "?", "22", "type", "group", "pos", "axis", "springdamper", "limited", "actuatorfrclimited", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", "stiffness", "range", "actuatorfrcrange", - "margin", "ref", "springref", "armature", "damping", "frictionloss", "user"}, + "actuatorgravcomp", "margin", "ref", "springref", "armature", "damping", + "frictionloss", "user"}, {"geom", "?", "31", "type", "pos", "quat", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", "shellinertia", "solmix", "solref", "solimp", @@ -240,11 +241,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { {"<"}, {"inertial", "?", "9", "pos", "quat", "mass", "diaginertia", "axisangle", "xyaxes", "zaxis", "euler", "fullinertia"}, - {"joint", "*", "23", "name", "class", "type", "group", "pos", "axis", + {"joint", "*", "24", "name", "class", "type", "group", "pos", "axis", "springdamper", "limited", "actuatorfrclimited", "solreflimit", "solimplimit", "solreffriction", "solimpfriction", - "stiffness", "range", "actuatorfrcrange", "margin", "ref", "springref", - "armature", "damping", "frictionloss", "user"}, + "stiffness", "range", "actuatorfrcrange", "actuatorgravcomp", "margin", "ref", + "springref", "armature", "damping", "frictionloss", "user"}, {"freejoint", "*", "2", "name", "group"}, {"geom", "*", "33", "name", "class", "type", "contype", "conaffinity", "condim", "group", "priority", "size", "material", "friction", "mass", "density", @@ -1573,6 +1574,9 @@ void mjXReader::OneJoint(XMLElement* elem, mjsJoint* pjoint) { ReadAttr(elem, "armature", 1, &pjoint->armature, text); ReadAttr(elem, "damping", 1, &pjoint->damping, text); ReadAttr(elem, "frictionloss", 1, &pjoint->frictionloss, text); + if (MapValue(elem, "actuatorgravcomp", &n, bool_map, 2)) { + pjoint->actgravcomp = (n==1); + } // read userdata if (ReadVector(elem, "user", userdata, text)) { diff --git a/src/xml/xml_native_writer.cc b/src/xml/xml_native_writer.cc index 9a20b95a..9f6d14f0 100644 --- a/src/xml/xml_native_writer.cc +++ b/src/xml/xml_native_writer.cc @@ -307,6 +307,7 @@ void mjXWriter::OneJoint(XMLElement* elem, mjCJoint* pjoint, mjCDef* def) { WriteAttr(elem, "range", 2, pjoint->range, def->joint.range); WriteAttrKey(elem, "actuatorfrclimited", TFAuto_map, 3, pjoint->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); diff --git a/test/engine/engine_forward_test.cc b/test/engine/engine_forward_test.cc index 07af31bf..6f5087d9 100644 --- a/test/engine/engine_forward_test.cc +++ b/test/engine/engine_forward_test.cc @@ -765,6 +765,80 @@ TEST_F(ActuatorTest, ActuatorForceClamping) { mj_deleteModel(model); } +// Apply gravity compensation via actuators +TEST_F(ActuatorTest, ActuatorGravcomp) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + mjData* data = mj_makeData(model); + + mj_forward(model, data); + + // expect force clamping as specified in the model + EXPECT_EQ(data->actuator_force[0], 0); + EXPECT_EQ(data->qfrc_actuator[0], 2); + EXPECT_EQ(data->qfrc_passive[0], 0); + EXPECT_EQ(data->sensordata[0], 0); + EXPECT_EQ(data->sensordata[1], 2); + + // reduce gravity so gravcomp is not clamped + model->opt.gravity[2] = -1; + mj_forward(model, data); + EXPECT_EQ(data->actuator_force[0], 0); + EXPECT_EQ(data->qfrc_actuator[0], 1); + EXPECT_EQ(data->qfrc_passive[0], 0); + EXPECT_EQ(data->sensordata[0], 0); + EXPECT_EQ(data->sensordata[1], 1); + + // add control, see that it adds up + data->ctrl[0] = 0.5; + mj_forward(model, data); + EXPECT_EQ(data->actuator_force[0], 0.5); + EXPECT_EQ(data->qfrc_actuator[0], 1.5); + EXPECT_EQ(data->qfrc_passive[0], 0); + EXPECT_EQ(data->sensordata[0], 0.5); + EXPECT_EQ(data->sensordata[1], 1.5); + + // add larger control, expect clamping + data->ctrl[0] = 1.5; + mj_forward(model, data); + EXPECT_EQ(data->actuator_force[0], 1.5); + EXPECT_EQ(data->qfrc_actuator[0], 2); + EXPECT_EQ(data->qfrc_passive[0], 0); + EXPECT_EQ(data->sensordata[0], 1.5); + EXPECT_EQ(data->sensordata[1], 2); + + // disable actgravcomp, expect gravcomp as a passive force + model->jnt_actgravcomp[0] = 0; + mj_forward(model, data); + EXPECT_EQ(data->actuator_force[0], 1.5); + EXPECT_EQ(data->qfrc_actuator[0], 1.5); + EXPECT_EQ(data->qfrc_passive[0], 1); + EXPECT_EQ(data->sensordata[0], 1.5); + EXPECT_EQ(data->sensordata[1], 1.5); + + mj_deleteData(data); + mj_deleteModel(model); +} + // ----------------------- filterexact actuators ------------------------------- using FilterExactTest = MujocoTest; diff --git a/test/engine/testdata/actuation/refsite.xml b/test/engine/testdata/actuation/refsite.xml index eda8e284..7374d624 100644 --- a/test/engine/testdata/actuation/refsite.xml +++ b/test/engine/testdata/actuation/refsite.xml @@ -6,7 +6,7 @@ (e.g. 4 consecutive ball joints is not a realistic kinematic design) --> @@ -29,7 +29,7 @@ commands is achievable by individual joint actuators with the specified torque limits. See https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping --> - + @@ -37,19 +37,19 @@ - + - + - + - + diff --git a/unity/Runtime/Bindings/MjBindings.cs b/unity/Runtime/Bindings/MjBindings.cs index 278ed9e5..76a72196 100644 --- a/unity/Runtime/Bindings/MjBindings.cs +++ b/unity/Runtime/Bindings/MjBindings.cs @@ -5218,6 +5218,7 @@ public unsafe struct mjModel_ { public int* jnt_group; public byte* jnt_limited; public byte* jnt_actfrclimited; + public byte* jnt_actgravcomp; public double* jnt_solref; public double* jnt_solimp; public double* jnt_pos;