From 05150546e301d0777e64bf059382d931e6aaa18f Mon Sep 17 00:00:00 2001 From: Yuval Tassa Date: Fri, 16 Feb 2024 04:41:49 -0800 Subject: [PATCH] Add `inheritrange` attribute to `position` and `intvelocity` actuators. PiperOrigin-RevId: 607650147 Change-Id: Ibc1846f89179786d4f93435c3924687b4c5d1432 --- doc/XMLreference.rst | 26 +++- doc/XMLschema.rst | 28 ++-- doc/changelog.rst | 19 ++- doc/conf.py | 4 +- src/user/user_api.h | 1 + src/user/user_objects.cc | 202 ++++++++++++++++++----------- src/user/user_objects.h | 2 + src/xml/xml_native_reader.cc | 30 +++-- test/user/user_objects_test.cc | 49 +++++++ test/xml/xml_native_reader_test.cc | 10 +- test/xml/xml_native_writer_test.cc | 60 +++++++++ 11 files changed, 318 insertions(+), 113 deletions(-) diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst index 483c21ab..e463ffe6 100644 --- a/doc/XMLreference.rst +++ b/doc/XMLreference.rst @@ -5430,6 +5430,20 @@ This element has one custom attribute in addition to the common attributes: Damping applied by the actuator. When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. +.. _actuator-position-inheritrange: + +:at:`inheritrange`: :at-val:`real, "0"` + Automatically set the actuator's :at:`ctrlrange` to match the transmission target's :at:`range`. The default value + means "disabled". A positive value :at-val:`X` sets the :at:`ctrlrange` around the midpoint of the target range, + scaled by :at-val:`X`. For example if the target joint has :at:`range` of :at-val:`[0, 1]`, then a value of + :at-val:`1.0` will set :at:`ctrlrange` to :at-val:`[0, 1]`; values of :at-val:`0.8` and :at-val:`1.2` will set the + :at:`ctrlrange` to :at-val:`[0.1, 0.9]` and :at-val:`[-0.1, 1.1]`, respectively. Values smaller than 1 are useful for + not hitting the limits; values larger than 1 are useful for maintaining control authority at the limits (being able + to push on them). This attribute is exclusive with :at:`ctrlrange` and available only for joint and tendon + transmissions which have :at:`range` defined. Note that while :at:`inheritrange` is available both as a + :ref:`position` attribute and in the :ref:`default class`, + saved XMLs always convert it to explicit :at:`ctrlrange` at the actuator. + .. _actuator-velocity: :el-prefix:`actuator/` |-| **velocity** (*) @@ -5578,10 +5592,16 @@ This element has one custom attribute in addition to the common attributes: Damping applied by the actuator. When using this attribute, it is recommended to use the implicitfast or implicit :ref:`integrators`. +.. _actuator-intvelocity-inheritrange: + +:at:`inheritrange`: :at-val:`real, "0"` + Identical to :ref:`position/inheritrange`, but sets :at:`actrange` (which has the same + length semantics as the transmission target) rather than :at:`ctrlrange` (which has velocity semantics). + .. _actuator-damper: :el-prefix:`actuator/` |-| **damper** (*) -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This element is an active damper which produces a force proportional to both velocity and control: ``F = - kv * velocity * control``, where ``kv`` must be nonnegative. :at:`ctrlrange` is required and must also be nonnegative. @@ -7728,6 +7748,8 @@ tendon, slidersite, cranksite. .. _default-position-ctrlrange: +.. _default-position-inheritrange: + .. _default-position-forcerange: .. _default-position-gear: @@ -7788,6 +7810,8 @@ refsite, tendon, slidersite, cranksite. .. _default-intvelocity-actrange: +.. _default-intvelocity-inheritrange: + .. _default-intvelocity-gear: .. _default-intvelocity-cranklength: diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst index 6bd56fcf..03a651af 100644 --- a/doc/XMLschema.rst +++ b/doc/XMLschema.rst @@ -774,13 +774,15 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`name` | :ref:`class` | :ref:`group` | :ref:`ctrllimited` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`forcerange` | :ref:`lengthrange` | | +| | | | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`inheritrange` | :ref:`forcerange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`gear` | :ref:`cranklength` | :ref:`user` | :ref:`joint` | | +| | | | :ref:`lengthrange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`jointinparent` | :ref:`tendon` | :ref:`slidersite` | :ref:`cranksite` | | +| | | | :ref:`joint` | :ref:`jointinparent` | :ref:`tendon` | :ref:`slidersite` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`site` | :ref:`refsite` | :ref:`kp` | :ref:`kv` | | +| | | | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | :ref:`kp` | | +| | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +| | | | :ref:`kv` | | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | @@ -806,13 +808,13 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`forcerange` | :ref:`actrange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`lengthrange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | +| | | | :ref:`inheritrange` | :ref:`lengthrange` | :ref:`gear` | :ref:`cranklength` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`joint` | :ref:`jointinparent` | :ref:`tendon` | :ref:`slidersite` | | +| | | | :ref:`user` | :ref:`joint` | :ref:`jointinparent` | :ref:`tendon` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | :ref:`kp` | | +| | | | :ref:`slidersite` | :ref:`cranksite` | :ref:`site` | :ref:`refsite` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`kv` | | | | | +| | | | :ref:`kp` | :ref:`kv` | | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| actuator |br| |_| |L| | | .. table:: | @@ -1440,11 +1442,11 @@ | :ref:`position | ? | :class: mjcf-attributes | | ` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`ctrllimited` | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`forcerange` | | +| | | | :ref:`ctrllimited` | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`inheritrange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`gear` | :ref:`cranklength` | :ref:`user` | :ref:`group` | | +| | | | :ref:`forcerange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`kp` | :ref:`kv` | | | | +| | | | :ref:`group` | :ref:`kp` | :ref:`kv` | | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | @@ -1464,9 +1466,9 @@ | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | | | | | :ref:`ctrllimited` | :ref:`forcelimited` | :ref:`ctrlrange` | :ref:`forcerange` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`actrange` | :ref:`gear` | :ref:`cranklength` | :ref:`user` | | +| | | | :ref:`actrange` | :ref:`inheritrange` | :ref:`gear` | :ref:`cranklength` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | -| | | | :ref:`group` | :ref:`kp` | :ref:`kv` | | | +| | | | :ref:`user` | :ref:`group` | :ref:`kp` | :ref:`kv` | | | | | +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ | +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+ | |_| default |br| |_| |L| | | .. table:: | diff --git a/doc/changelog.rst b/doc/changelog.rst index b7f33c2e..2c6f6aaf 100644 --- a/doc/changelog.rst +++ b/doc/changelog.rst @@ -5,20 +5,27 @@ Changelog Upcoming version (not yet released) ----------------------------------- +General +^^^^^^^ +1. Added the :at:`inheritrange` attribute to :ref:`position` and + :ref:`intvelocity` actuators, allowing convenient setting of the actuator's + :at:`ctrlrange` or :at:`actrange` (respectively), according to the range of the transmission + target (joint or tendon). See :ref:`position/inheritrange` for + details. + MJX ^^^ - -1. Improved performance of getting and putting device data. +2. Improved performance of getting and putting device data. - Use ``tobytes()`` for numpy array serialization, which is orders of magnitude faster than converting to tuples. - Avoid reallocating host ``mjData`` arrays when array shapes are unchanged. - Speed up calculation of ``mjx.ncon`` for models with many geoms. - Avoid calling ``mjx.ncon`` in ``mjx.get_data_into`` when ``nc`` can be derived from ``mjx.Data``. -2. Fixed a bug in ``mjx-viewer`` that prevented it from running. Updated ``mjx-viewer`` to use newer +3. Fixed a bug in ``mjx-viewer`` that prevented it from running. Updated ``mjx-viewer`` to use newer ``mjx.get_data_into`` function call. -3. Fixed a bug in ``mjx.euler`` that applied incorrect damping when using dense mass matrices. -4. Fixed a bug in ``mjx.solve`` that was causing slow convergence when using ``mjSOL_NEWTON`` in :ref:`mjtSolver`. -5. Added support for :ref:`mjOption.impratio` to ``mjx.Model``. +4. Fixed a bug in ``mjx.euler`` that applied incorrect damping when using dense mass matrices. +5. Fixed a bug in ``mjx.solve`` that was causing slow convergence when using ``mjSOL_NEWTON`` in :ref:`mjtSolver`. +6. Added support for :ref:`mjOption.impratio` to ``mjx.Model``. Version 3.1.2 (February 05, 2024) ----------------------------------- diff --git a/doc/conf.py b/doc/conf.py index bc244774..d9ca5bfb 100644 --- a/doc/conf.py +++ b/doc/conf.py @@ -118,7 +118,7 @@ SHARED_CSS_VARIABLES = { html_theme_options = { 'light_css_variables': { 'font-stack--monospace': 'Inconsolata,Consolas,ui-monospace,monospace', - 'at-color': '#bc103e', + 'at-color': '#830b2b', 'at-val-color': '#bc103e', 'body-color': '#14234b', 'color-highlight-on-target': '#e5e8ed', @@ -131,7 +131,7 @@ html_theme_options = { 'wy-nav-side-background-color': '#0053d6', }, 'dark_css_variables': { - 'at-color': '#ff95a6', + 'at-color': '#ffaab7', 'at-val-color': '#ff95a6', 'body-color': '#14234b', 'color-admonition-background': '#1e1e21', diff --git a/src/user/user_api.h b/src/user/user_api.h index d6142abe..a446d68f 100644 --- a/src/user/user_api.h +++ b/src/user/user_api.h @@ -559,6 +559,7 @@ typedef struct _mjmActuator { // actuator specification mjString slidersite; // site defining cylinder, for slider-crank double cranklength; // crank length, for slider-crank double lengthrange[2]; // transmission length range + double inheritrange; // automatic range setting for position and intvelocity // input/output clamping int ctrllimited; // are control limits defined (mjtLimited) diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index f017995c..50ad9770 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -4272,85 +4272,6 @@ void mjCActuator::Compile(void) { } userdata_.resize(model->nuser_actuator); - // if limited is auto, check for inconsistency wrt to autolimits - if (forcelimited == mjLIMITED_AUTO) { - bool hasrange = !(forcerange[0]==0 && forcerange[1]==0); - checklimited(this, model->autolimits, "actuator", "force", forcelimited, hasrange); - } - if (ctrllimited == mjLIMITED_AUTO) { - bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0); - checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange); - } - if (actlimited == mjLIMITED_AUTO) { - bool hasrange = !(actrange[0]==0 && actrange[1]==0); - checklimited(this, model->autolimits, "actuator", "act", actlimited, hasrange); - } - - // check limits - if (forcerange[0]>=forcerange[1] && is_forcelimited()) { - throw mjCError(this, "invalid force range for actuator '%s' (id = %d)", name.c_str(), id); - } - if (ctrlrange[0]>=ctrlrange[1] && is_ctrllimited()) { - throw mjCError(this, "invalid control range for actuator '%s' (id = %d)", name.c_str(), id); - } - if (actrange[0]>=actrange[1] && is_actlimited()) { - throw mjCError(this, "invalid actrange for actuator '%s' (id = %d)", name.c_str(), id); - } - if (is_actlimited() && dyntype == mjDYN_NONE) { - throw mjCError(this, "actrange specified but dyntype is 'none' in actuator '%s' (id = %d)", - name.c_str(), id); - } - - // check and set actdim - if (actdim > 1 && dyntype != mjDYN_USER) { - throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator '%s' (id = %d)", - name.c_str(), id); - } - if (actdim == 1 && dyntype == mjDYN_NONE) { - throw mjCError(this, "invalid actdim 1 in stateless actuator '%s' (id = %d)", name.c_str(), id); - } - if (actdim == 0 && dyntype != mjDYN_NONE) { - throw mjCError(this, "invalid actdim 0 in stateful actuator '%s' (id = %d)", name.c_str(), id); - } - - // set actdim - if (actdim < 0) { - actdim = (dyntype != mjDYN_NONE); - } - - // check muscle parameters - for (int i=0; i<2; i++) { - // select gain or bias - double* prm = NULL; - if (i==0 && gaintype==mjGAIN_MUSCLE) { - prm = gainprm; - } else if (i==1 && biastype==mjBIAS_MUSCLE) { - prm = biasprm; - } - - // nothing to check - if (!prm) { - continue; - } - - // range - if (prm[0]>=prm[1]) { - throw mjCError(this, "range[0]=1 || prm[5]<=1) { - throw mjCError(this, "lmin<10 - if (prm[3]<=0 || prm[6]<=0 || prm[7]<=0 || prm[8]<=0) { - throw mjCError(this, - "positive scale, vmax, fpmax, fvmax required in muscle '%s' (id = %d)", - name.c_str(), id); - } - } - // check for missing target name if (target_.empty()) { throw mjCError(this, @@ -4434,6 +4355,129 @@ void mjCActuator::Compile(void) { trnid[0] = ptarget->id; } + // handle inheritrange + if (gaintype == mjGAIN_FIXED && biastype == mjBIAS_AFFINE && + gainprm[0] == -biasprm[1] && inheritrange > 0) { + // semantic of actuator is the same as transmission, inheritrange is applicable + double* range; + if (dyntype == mjDYN_NONE) { + // position actuator + range = ctrlrange; + } else if (dyntype == mjDYN_INTEGRATOR) { + // intvelocity actuator + range = actrange; + } else { + throw mjCError(this, "inheritrange only available for position " + "and intvelocity actuators '%s' (id = %d)", name.c_str(), id); + } + + const double* target_range; + if (trntype == mjTRN_JOINT) { + pjnt = (mjCJoint*) ptarget; + if (pjnt->spec.type != mjJNT_HINGE && pjnt->spec.type != mjJNT_SLIDE) { + throw mjCError(this, "inheritrange can only be used with hinge and slide joints, " + "actuator '%s' (id = %d)", name.c_str(), id); + } + target_range = pjnt->get_range(); + } else if (trntype == mjTRN_TENDON) { + mjCTendon* pten = (mjCTendon*) ptarget; + target_range = pten->get_range(); + } else { + throw mjCError(this, "inheritrange can only be used with joint and tendon transmission, " + "actuator '%s' (id = %d)", name.c_str(), id); + } + + if (target_range[0] == target_range[1]) { + throw mjCError(this, "inheritrange used but target '%s' has no range defined in actuator %d", + target_.c_str(), id); + } + + // set range automatically + double mean = 0.5*(target_range[1] + target_range[0]); + double radius = 0.5*(target_range[1] - target_range[0]) * inheritrange; + range[0] = mean - radius; + range[1] = mean + radius; + } + + // if limited is auto, check for inconsistency wrt to autolimits + if (forcelimited == mjLIMITED_AUTO) { + bool hasrange = !(forcerange[0]==0 && forcerange[1]==0); + checklimited(this, model->autolimits, "actuator", "force", forcelimited, hasrange); + } + if (ctrllimited == mjLIMITED_AUTO) { + bool hasrange = !(ctrlrange[0]==0 && ctrlrange[1]==0); + checklimited(this, model->autolimits, "actuator", "ctrl", ctrllimited, hasrange); + } + if (actlimited == mjLIMITED_AUTO) { + bool hasrange = !(actrange[0]==0 && actrange[1]==0); + checklimited(this, model->autolimits, "actuator", "act", actlimited, hasrange); + } + + // check limits + if (forcerange[0]>=forcerange[1] && is_forcelimited()) { + throw mjCError(this, "invalid force range for actuator '%s' (id = %d)", name.c_str(), id); + } + if (ctrlrange[0]>=ctrlrange[1] && is_ctrllimited()) { + throw mjCError(this, "invalid control range for actuator '%s' (id = %d)", name.c_str(), id); + } + if (actrange[0]>=actrange[1] && is_actlimited()) { + throw mjCError(this, "invalid actrange for actuator '%s' (id = %d)", name.c_str(), id); + } + if (is_actlimited() && dyntype == mjDYN_NONE) { + throw mjCError(this, "actrange specified but dyntype is 'none' in actuator '%s' (id = %d)", + name.c_str(), id); + } + + // check and set actdim + if (actdim > 1 && dyntype != mjDYN_USER) { + throw mjCError(this, "actdim > 1 is only allowed for dyntype 'user' in actuator '%s' (id = %d)", + name.c_str(), id); + } + if (actdim == 1 && dyntype == mjDYN_NONE) { + throw mjCError(this, "invalid actdim 1 in stateless actuator '%s' (id = %d)", name.c_str(), id); + } + if (actdim == 0 && dyntype != mjDYN_NONE) { + throw mjCError(this, "invalid actdim 0 in stateful actuator '%s' (id = %d)", name.c_str(), id); + } + + // set actdim + if (actdim < 0) { + actdim = (dyntype != mjDYN_NONE); + } + + // check muscle parameters + for (int i=0; i<2; i++) { + // select gain or bias + double* prm = NULL; + if (i==0 && gaintype==mjGAIN_MUSCLE) { + prm = gainprm; + } else if (i==1 && biastype==mjBIAS_MUSCLE) { + prm = biasprm; + } + + // nothing to check + if (!prm) { + continue; + } + + // range + if (prm[0]>=prm[1]) { + throw mjCError(this, "range[0]=1 || prm[5]<=1) { + throw mjCError(this, "lmin<10 + if (prm[3]<=0 || prm[6]<=0 || prm[7]<=0 || prm[8]<=0) { + throw mjCError(this, + "positive scale, vmax, fpmax, fvmax required in muscle '%s' (id = %d)", + name.c_str(), id); + } + } + // plugin if (plugin.active) { if (plugin_name.empty() && plugin_instance_name.empty()) { diff --git a/src/user/user_objects.h b/src/user/user_objects.h index 804ec564..b843862e 100644 --- a/src/user/user_objects.h +++ b/src/user/user_objects.h @@ -336,6 +336,7 @@ class mjCJoint : public mjCBase, private mjmJoint { // used by mjXWriter and mjCModel const std::vector& get_userdata() { return userdata_; } + const double* get_range() { return range; } bool is_limited() const; bool is_actfrclimited() const; @@ -1102,6 +1103,7 @@ class mjCTendon : public mjCBase, private mjmTendon { // used by mjXWriter and mjCModel const std::vector& get_userdata() { return userdata_; } + const double* get_range() { return range; } void CopyFromSpec(); void PointToLocal(); diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc index 3f9ce15a..bb5f82d7 100644 --- a/src/xml/xml_native_reader.cc +++ b/src/xml/xml_native_reader.cc @@ -168,14 +168,14 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"}, {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group"}, - {"position", "?", "10", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", - "gear", "cranklength", "user", "group", + {"position", "?", "11", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", + "forcerange", "gear", "cranklength", "user", "group", "kp", "kv"}, {"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "gear", "cranklength", "user", "group", "kv"}, - {"intvelocity", "?", "11", "ctrllimited", "forcelimited", - "ctrlrange", "forcerange", "actrange", + {"intvelocity", "?", "12", "ctrllimited", "forcelimited", + "ctrlrange", "forcerange", "actrange", "inheritrange", "gear", "cranklength", "user", "group", "kp", "kv"}, {"damper", "?", "8", "forcelimited", "ctrlrange", "forcerange", @@ -381,8 +381,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "ctrllimited", "forcelimited", "ctrlrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"}, - {"position", "*", "20", "name", "class", "group", - "ctrllimited", "forcelimited", "ctrlrange", "forcerange", + {"position", "*", "21", "name", "class", "group", + "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv"}, @@ -391,9 +391,9 @@ const char* MJCF[nMJCF][mjXATTRNUM] = { "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kv"}, - {"intvelocity", "*", "21", "name", "class", "group", + {"intvelocity", "*", "22", "name", "class", "group", "ctrllimited", "forcelimited", - "ctrlrange", "forcerange", "actrange", "lengthrange", + "ctrlrange", "forcerange", "actrange", "inheritrange", "lengthrange", "gear", "cranklength", "user", "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite", "kp", "kv"}, @@ -2085,6 +2085,19 @@ void mjXReader::OneActuator(XMLElement* elem, mjmActuator* pact) { pact->biasprm[2] *= -1; } + ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text); + if (pact->inheritrange > 0) { + if (type == "position") { + if (pact->ctrlrange[0] || pact->ctrlrange[1]) { + throw mjXError(elem, "ctrlrange and inheritrange cannot both be defined"); + } + } else { + if (pact->actrange[0] || pact->actrange[1]) { + throw mjXError(elem, "actrange and inheritrange cannot both be defined"); + } + } + } + // implied parameters pact->gaintype = mjGAIN_FIXED; pact->biastype = mjBIAS_AFFINE; @@ -2122,7 +2135,6 @@ void mjXReader::OneActuator(XMLElement* elem, mjmActuator* pact) { pact->gainprm[2] = -pact->gainprm[2]; // require nonnegative range - ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text); if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { throw mjXError(elem, "damper control range cannot be negative"); } diff --git a/test/user/user_objects_test.cc b/test/user/user_objects_test.cc index 9da99ec8..d652a54c 100644 --- a/test/user/user_objects_test.cc +++ b/test/user/user_objects_test.cc @@ -909,6 +909,55 @@ TEST_F(ActuatorTest, ActuatorOrderDoesntMatter) { mj_deleteModel(model1); } +// ------------- test inheritrange attribute ---------------------------------- + +using InheritrangeTest = MujocoTest; + +TEST_F(InheritrangeTest, ErrorIfTargetMissingRange) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, IsNull()); + EXPECT_THAT(error.data(), HasSubstr("target 'jnt' has no range defined")); +} + +TEST_F(InheritrangeTest, WorksForDegrees) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + )"; + std::array error; + mjModel* model = LoadModelFromString(xml, error.data(), error.size()); + ASSERT_THAT(model, NotNull()) << error.data(); + EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[0], mjPI/2); + EXPECT_DOUBLE_EQ(model->actuator_ctrlrange[1], mjPI); + + mj_deleteModel(model); +} + // ------------- test actlimited and actrange fields --------------------------- diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc index 8be293fb..5ce35ec2 100644 --- a/test/xml/xml_native_reader_test.cc +++ b/test/xml/xml_native_reader_test.cc @@ -1065,10 +1065,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) { - + - + @@ -1077,7 +1077,7 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) { - + @@ -1107,6 +1107,10 @@ TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) { EXPECT_EQ(model->actuator_gainprm[i*mjNGAIN + j], 0.0); } } + EXPECT_EQ(model->actuator_ctrlrange[0*2 + 0], -1.0); + EXPECT_EQ(model->actuator_ctrlrange[0*2 + 1], 3.0); + EXPECT_EQ(model->actuator_actrange[1*2 + 0], 0.5); + EXPECT_EQ(model->actuator_actrange[1*2 + 1], 1.5); mj_deleteModel(model); } diff --git a/test/xml/xml_native_writer_test.cc b/test/xml/xml_native_writer_test.cc index 1d2bf472..f65b70b4 100644 --- a/test/xml/xml_native_writer_test.cc +++ b/test/xml/xml_native_writer_test.cc @@ -1025,6 +1025,66 @@ TEST_F(XMLWriterTest, TrimsDefaults) { mj_deleteModel(model); } +TEST_F(XMLWriterTest, InheritrangeSavedAsRange) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\"")); + EXPECT_THAT(saved_xml, HasSubstr("actrange=\"-1.5 -0.5\"")); + mj_deleteModel(model); +} + +TEST_F(XMLWriterTest, InheritedInheritrangeSavedAsRange) { + static constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + + + )"; + mjModel* model = LoadModelFromString(xml); + ASSERT_THAT(model, NotNull()); + std::string saved_xml = SaveAndReadXml(model); + EXPECT_THAT(saved_xml, Not(HasSubstr("inheritrange"))); + EXPECT_THAT(saved_xml, HasSubstr("ctrlrange=\"-1 3\"")); + EXPECT_THAT(saved_xml, HasSubstr("actrange=\"0.5 1.5\"")); + mj_deleteModel(model); +} + // check that no precision is lost when saving XMLs with FullFloatPrecision TEST_F(XMLWriterTest, SetPrecision) { static constexpr char xml[] = R"(