Allow plugin actuators to have activation state.

Accept new attributes on the plugin/actuator tag:
  - dyntype
  - dynprm
  - actlimited
  - actrange
  - actearly

This addresses some of the requests in #1098.

PiperOrigin-RevId: 584866281
Change-Id: I4f7559e446888216093efba4ced9119cfc0528a4

test/engine/engine_plugin_test.cc

@@ -26,11 +26,13 @@
 #include <gmock/gmock.h>
 #include <gtest/gtest.h>
 #include <absl/strings/str_format.h>
+#include <absl/strings/str_replace.h>
 #include <mujoco/mujoco.h>
 #include "test/fixture.h"
 
 namespace mujoco {
 namespace {
+using ::testing::DoubleNear;
 using ::testing::HasSubstr;
 using ::testing::NotNull;
 
@@ -136,15 +138,22 @@ class TestSensor : public BaseTestPlugin {
 class TestActuator : public BaseTestPlugin {
  public:
   static constexpr mjtNum kDefaultMultiplier = 1.0;
+  static constexpr mjtNum kActDotValue = 13.0;
 
   TestActuator(const mjModel* m, mjData* d, int instance)
-      : BaseTestPlugin(m, d, instance) {
+      : BaseTestPlugin(m, d, instance), instance_(instance) {
     const char* s = mj_getPluginConfig(m, instance, "multiplier");
     if (*s) {
-        std::stringstream(s) >> multiplier;
-      } else {
-        multiplier = kDefaultMultiplier;
-      }
+      std::stringstream(s) >> multiplier;
+    } else {
+      multiplier = kDefaultMultiplier;
+    }
+    s = mj_getPluginConfig(m, instance, "actdim");
+    if (*s) {
+      std::stringstream(s) >> actdim_;
+    } else {
+      actdim_ = 0;
+    }
     for (int i = 0; i < m->nu; ++i) {
       if (m->actuator_plugin[i] == instance) {
         actuators.push_back(&d->actuator_force[i]);
@@ -172,9 +181,20 @@ class TestActuator : public BaseTestPlugin {
     WriteActuatorForce();
   }
 
+  void ActDot(const mjModel* m, mjData* d) {
+    for (int i = 0; i < m->nu; ++i) {
+      if (m->actuator_plugin[i] != instance_) {
+        continue;
+      }
+      mju_fill(d->act_dot + m->actuator_actadr[i], kActDotValue, actdim_);
+    }
+  }
+
  private:
   mjtNum multiplier;
   std::vector<mjtNum*> actuators;
+  int instance_;
+  int actdim_;
 
   void WriteActuatorForce() {
     for (mjtNum* actuator_force : actuators) {
@@ -242,7 +262,7 @@ int RegisterActuatorPlugin() {
 
   plugin.name = "mujoco.test.actuator";
 
-  const char* attributes[] = {"stride", "multiplier"};
+  const char* attributes[] = {"stride", "multiplier", "actdim"};
   plugin.nattribute = sizeof(attributes) / sizeof(*attributes);
   plugin.attributes = attributes;
 
@@ -275,6 +295,20 @@ int RegisterActuatorPlugin() {
     auto actuator = reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
     actuator->Advance();
   };
+  plugin.actuator_actdim =
+      +[](const mjModel* m, int instance, int actuator_id) {
+    const char* actdim_str = mj_getPluginConfig(m, instance, "actdim");
+    if (actdim_str) {
+      int actdim = 0;
+      std::stringstream(actdim_str) >> actdim;
+      return actdim;
+    }
+    return 0;
+  };
+  plugin.actuator_act_dot = +[](const mjModel* m, mjData* d, int instance) {
+    auto actuator = reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
+    actuator->ActDot(m, d);
+  };
 
   return mjp_registerPlugin(&plugin);
 }
@@ -348,6 +382,7 @@ constexpr char xml[] = R"(
       <instance name="actuator2">
         <config key="stride" value="2"/>
         <config key="multiplier" value="0.125"/>
+        <config key="actdim" value="3"/>
       </instance>
     </plugin>
     <plugin plugin="mujoco.test.passive"/>
@@ -381,6 +416,7 @@ constexpr char xml[] = R"(
       <config key="multiplier" value="0.03125"/>
     </plugin>
     <plugin joint="h2" instance="actuator2"/>
+    <plugin joint="h2" dyntype="filter" dynprm="0.9" instance="actuator2"/>
   </actuator>
 </mujoco>
 )";
@@ -457,8 +493,9 @@ TEST_F(EnginePluginTest, SaveXml) {
   int actuator_end = expected_xml.find(actuator_close) + actuator_close.size();
   ASSERT_NE(actuator_end, std::string::npos);
   ASSERT_LE(actuator_start, actuator_end);
-  auto expected_actuator_section =
-      expected_xml.substr(actuator_start, actuator_end - actuator_start);
+  auto expected_actuator_section = absl::StrReplaceAll(
+      expected_xml.substr(actuator_start, actuator_end - actuator_start),
+      {{"dynprm=\"0.9\"", "dynprm=\"0.9 0 0 0 0 0 0 0 0 0\""}});
 
   EXPECT_THAT(saved_xml, HasSubstr(expected_extension_section));
   EXPECT_THAT(saved_xml, HasSubstr(expected_sensor_section));
@@ -579,5 +616,48 @@ TEST_F(EnginePluginTest, ActuatorPlugin) {
   EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
 }
 
+TEST_F(EnginePluginTest, FilteredActuatorPlugin) {
+  char error[1024] = {0};
+  mjModel* m = LoadModelFromString(xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::NotNull()) << error;
+
+  // Expecting 7 actuator state variables: 3x2 from actuator2 instances, and 1
+  // from setting dyntype="filter" on one of the plugin actuators
+  EXPECT_EQ(m->na, 7);
+  EXPECT_EQ(m->actuator_actnum[0], 0);
+  EXPECT_EQ(m->actuator_actnum[1], 3);
+  EXPECT_EQ(m->actuator_actnum[2], 4);
+  EXPECT_EQ(m->actuator_actadr[0], -1);
+  EXPECT_EQ(m->actuator_actadr[1], 0);
+  EXPECT_EQ(m->actuator_actadr[2], 3);
+
+  mjData* d = mj_makeData(m);
+  EXPECT_EQ(d->act[0], 0.0);
+  mju_fill(d->ctrl, 1, m->nu);
+  // start with nonzero act for the filter
+  d->act[6] = 0.5;
+  mj_step(m, d);
+
+  for (int i = 0; i < 6; ++i) {
+    // act_dot should be computed by the plugin
+    mjtNum expected_act_dot = TestActuator::kActDotValue;
+    EXPECT_THAT(d->act_dot[i], DoubleNear(expected_act_dot, 1e-6));
+
+    // act_dot from the plugin should be Euler-integrated
+    mjtNum expected_act = expected_act_dot * m->opt.timestep;
+    EXPECT_THAT(d->act[i], DoubleNear(expected_act, 1e-6));
+  }
+
+  // actuator filter state should be updated outside the plugin for filter
+  // actuators.
+  mjtNum expected_act_dot = 0.5 / m->actuator_dynprm[mjNDYN * 2];
+  EXPECT_THAT(d->act_dot[6], DoubleNear(expected_act_dot, 1e-6));
+  EXPECT_THAT(d->act[6],
+              DoubleNear(0.5 + expected_act_dot * m->opt.timestep, 1e-6));
+
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
 }  // namespace
 }  // namespace mujoco