Add timeconst attribute to position actuator.

PiperOrigin-RevId: 635018979 Change-Id: I919573f4241ecb4a4c61c3b8b6641b31aa79eca4
google-deepmind · May 18, 2024 · 727893f · 727893f
1 parent 875823b
commit 727893f
Show file tree

Hide file tree

Showing 6 changed files with 127 additions and 18 deletions.
diff --git a/doc/XMLreference.rst b/doc/XMLreference.rst
@@ -4972,15 +4972,16 @@ This element does not have custom attributes. It only has common attributes, whi
 :el-prefix:`actuator/` |-| **position** (*)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-This element creates a position servo. The underlying :el:`general` attributes are set as follows:
+This element creates a position servo with an optional first-order filter. The underlying :el:`general` attributes are
+set as follows:
 
-========= ======= ========= =========
-Attribute Setting Attribute Setting
-========= ======= ========= =========
-dyntype   none    dynprm    1 0 0
-gaintype  fixed   gainprm   kp 0 0
-biastype  affine  biasprm   0 -kp -kv
-========= ======= ========= =========
+========= =================== ========= =============
+Attribute Setting             Attribute Setting
+========= =================== ========= =============
+dyntype   none or filterexact dynprm    timeconst 0 0
+gaintype  fixed               gainprm   kp 0 0
+biastype  affine              biasprm   0 -kp -kv
+========= =================== ========= =============
 
 
 This element has one custom attribute in addition to the common attributes:
@@ -5040,6 +5041,13 @@ 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<geIntegration>`.
 
+.. _actuator-position-timeconst:
+
+:at:`timeconst`: :at-val:`real, "0"`
+   Time-constant of the first-order filter.  If larger than zero, the actuator uses the :at:`filterexact`
+   :ref:`dynamics type<actuator-general-dyntype>`, if zero (the default) no filter is used.
+
+
 .. _actuator-position-inheritrange:
 
 :at:`inheritrange`: :at-val:`real, "0"`
@@ -8085,6 +8093,8 @@ tendon, slidersite, cranksite.
 
 .. _default-position-kv:
 
+.. _default-position-timeconst:
+
 :el-prefix:`default/` |-| **position** (?)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 

diff --git a/doc/XMLschema.rst b/doc/XMLschema.rst
@@ -705,7 +705,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`cranksite<actuator-position-cranksite>`                   | :ref:`site<actuator-position-site>`                             | :ref:`refsite<actuator-position-refsite>`                       | :ref:`kp<actuator-position-kp>`                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`kv<actuator-position-kv>`                                 |                                                                 |                                                                 |                                                                 | |
+|                                    |    |    | :ref:`kv<actuator-position-kv>`                                 | :ref:`timeconst<actuator-position-timeconst>`                   |                                                                 |                                                                 | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| actuator |br| |_| |L|          |    | .. table::                                                                                                                                                                                                                                                                   |
@@ -1477,7 +1477,7 @@
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 |                                    |    |    | :ref:`forcerange<default-position-forcerange>`                  | :ref:`gear<default-position-gear>`                              | :ref:`cranklength<default-position-cranklength>`                | :ref:`user<default-position-user>`                              | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
-|                                    |    |    | :ref:`group<default-position-group>`                            | :ref:`kp<default-position-kp>`                                  | :ref:`kv<default-position-kv>`                                  |                                                                 | |
+|                                    |    |    | :ref:`group<default-position-group>`                            | :ref:`kp<default-position-kp>`                                  | :ref:`kv<default-position-kv>`                                  | :ref:`timeconst<default-position-timeconst>`                    | |
 |                                    |    |    +-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+-----------------------------------------------------------------+ |
 +------------------------------------+----+------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------+
 | |_| default |br| |_| |L|           |    | .. table::                                                                                                                                                                                                                                                                   |

diff --git a/doc/changelog.rst b/doc/changelog.rst
@@ -11,13 +11,15 @@ General
 1. Added :ref:`mj_geomDistance` for computing the shortest signed distance between two geoms and optionally a segment
    connecting them. Relatedly, added the 3 sensors: :ref:`distance<sensor-distance>`, :ref:`normal<sensor-normal>`,
    :ref:`fromto<sensor-fromto>`. See the function and sensor documentation for details. Fixes :github:issue:`51`.
+2. Added :ref:`timeconst<actuator-position-timeconst>` attribute to the :ref:`position actuator<actuator-position>`.
+   When set to a positive value, the actuator is made stateful with :at:`filterexact` dynamics.
 
 Bug fixes
 ^^^^^^^^^
 
-2. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
+3. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
    often the case for URDF imports. Fixes :github:issue:`1069`, :github:issue:`1577`.
-3. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing
+4. Fixed a bug that was causing the visualization of SDF iterations to write outside the size of the vector storing
    them. Fixes :github:issue:`1539`.
 
 Version 3.1.5 (May 7, 2024)

diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc
@@ -5125,7 +5125,7 @@ void mjCActuator::Compile(void) {
       gainprm[0] == -biasprm[1] && inheritrange > 0) {
     // semantic of actuator is the same as transmission, inheritrange is applicable
     double* range;
-    if (dyntype == mjDYN_NONE) {
+    if (dyntype == mjDYN_NONE || dyntype == mjDYN_FILTEREXACT) {
       // position actuator
       range = ctrlrange;
     } else if (dyntype == mjDYN_INTEGRATOR) {

diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc
@@ -177,9 +177,8 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "dyntype", "gaintype", "biastype", "dynprm", "gainprm", "biasprm", "actearly"},
         {"motor", "?", "8", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "cranklength", "user", "group"},
-        {"position", "?", "11", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
-            "forcerange", "gear", "cranklength", "user", "group",
-            "kp", "kv"},
+        {"position", "?", "12", "ctrllimited", "forcelimited", "ctrlrange", "inheritrange",
+            "forcerange", "gear", "cranklength", "user", "group", "kp", "kv", "timeconst"},
         {"velocity", "?", "9", "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "gear", "cranklength", "user", "group",
             "kv"},
@@ -390,11 +389,11 @@ const char* MJCF[nMJCF][mjXATTRNUM] = {
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite"},
-        {"position", "*", "21", "name", "class", "group",
+        {"position", "*", "22", "name", "class", "group",
             "ctrllimited", "forcelimited", "ctrlrange", "inheritrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
             "joint", "jointinparent", "tendon", "slidersite", "cranksite", "site", "refsite",
-            "kp", "kv"},
+            "kp", "kv", "timeconst"},
         {"velocity", "*", "19", "name", "class", "group",
             "ctrllimited", "forcelimited", "ctrlrange", "forcerange",
             "lengthrange", "gear", "cranklength", "user",
@@ -2094,6 +2093,12 @@ void mjXReader::OneActuator(XMLElement* elem, mjsActuator* pact) {
       pact->biasprm[2] *= -1;
     }
 
+    if (ReadAttr(elem, "timeconst", 1, pact->dynprm, text)) {
+      if (pact->dynprm[0] < 0)
+        throw mjXError(elem, "timeconst cannot be negative");
+      pact->dyntype = pact->dynprm[0] ? mjDYN_FILTEREXACT : mjDYN_NONE;
+    }
+
     ReadAttr(elem, "inheritrange", 1, &pact->inheritrange, text);
     if (pact->inheritrange > 0) {
       if (type == "position") {

diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc
@@ -1407,6 +1407,98 @@ TEST_F(ActuatorTest, ReadsByte) {
 
 using ActuatorParseTest = MujocoTest;
 
+TEST_F(ActuatorParseTest, PositionTimeconst) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="jnt"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <position joint="jnt" timeconst="2"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull());
+  ASSERT_NEAR(model->actuator_dynprm[0], 2.0, 1e-6);
+  EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_FILTEREXACT));
+  mj_deleteModel(model);
+}
+
+TEST_F(ActuatorParseTest, PositionTimeconstInheritrange) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="jnt" range="-1 1"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <position joint="jnt" inheritrange="1" timeconst="2"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull());
+  mj_deleteModel(model);
+}
+
+TEST_F(ActuatorParseTest, PositionTimeconstDefault) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <default>
+      <position timeconst="1"/>
+    </default>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="jnt"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <position joint="jnt"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull());
+  ASSERT_NEAR(model->actuator_dynprm[0], 1.0, 1e-6);
+  EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_FILTEREXACT));
+  mj_deleteModel(model);
+}
+
+TEST_F(ActuatorParseTest, PositionTimeconstDefaultOverride) {
+  static constexpr char xml[] = R"(
+  <mujoco>
+    <default>
+      <position timeconst="1"/>
+    </default>
+    <worldbody>
+      <body>
+        <geom size="1"/>
+        <joint name="jnt"/>
+      </body>
+    </worldbody>
+    <actuator>
+      <position joint="jnt" timeconst="0"/>
+    </actuator>
+  </mujoco>
+  )";
+  std::array<char, 1024> error;
+  mjModel* model = LoadModelFromString(xml, error.data(), error.size());
+  ASSERT_THAT(model, NotNull());
+  EXPECT_FALSE(model->actuator_dynprm[0]);
+  EXPECT_THAT(model->actuator_dyntype[0], Eq(mjDYN_NONE));
+  mj_deleteModel(model);
+}
+
 TEST_F(ActuatorParseTest, ReadsDamper) {
   static constexpr char xml[] = R"(
   <mujoco>