- Added passive forces plugins

- Added new `cable` composite type:
  * The `initial` parameter specifies the joint at the starting boundary: `free`, `ball`, or `none`.
  * The boundary bodies are exposed with the names:`B_left` and `B_right`.
  * The vertex initial positions can be specified directly in the XML with the parameter `vertex`.
  * The orientation of the body frame **is** the orientation of the material frame of the curve.

- Added new `cable` passive force plugin:
  * Twist and bending stiffness can be set separately with the parameters `twist` and `bend`.
  * The stress-free configuration can be set to be the initial one or flat with the flag `flat`.
  * New cable example showing the formation of plectoneme.
  * New coil example.
  * New belt example showing interaction between twist and anisotropy.
  * Added test using cantilever exact solution.

PiperOrigin-RevId: 480033694
Change-Id: I491271bce8fccb185961477e903e5a72d172c8a3

test/engine/engine_plugin_test.cc

@@ -35,6 +35,8 @@ using ::testing::HasSubstr;
 using ::testing::NotNull;
 
 constexpr int kNumFakePlugins = 30;
+constexpr int kNumTestPlugins = 3;
+const int kNumTruePlugins = mjp_pluginCount();
 
 class BaseTestPlugin {
  public:
@@ -186,6 +188,14 @@ class TestActuator : public BaseTestPlugin {
   }
 };
 
+class TestPassive {
+ public:
+  TestPassive(const mjModel* m, mjData* d, int instance) {}
+  void Reset() {}
+  void Compute() {}
+  void Advance() {}
+};
+
 int RegisterSensorPlugin() {
   mjpPlugin plugin;
   mjp_defaultPlugin(&plugin);
@@ -206,6 +216,7 @@ int RegisterSensorPlugin() {
     auto* sensor = new TestSensor(m, d, instance);
     d->plugin_data[instance] = reinterpret_cast<uintptr_t>(sensor);
     TestSensor::InitCount()++;
+    return 0;
   };
   plugin.destroy = +[](mjData* d, int instance) {
     delete reinterpret_cast<TestSensor*>(d->plugin_data[instance]);
@@ -247,6 +258,7 @@ int RegisterActuatorPlugin() {
     auto* actuator = new TestActuator(m, d, instance);
     d->plugin_data[instance] = reinterpret_cast<uintptr_t>(actuator);
     TestActuator::InitCount()++;
+    return 0;
   };
   plugin.destroy = +[](mjData* d, int instance) {
     delete reinterpret_cast<TestActuator*>(d->plugin_data[instance]);
@@ -270,6 +282,42 @@ int RegisterActuatorPlugin() {
   return mjp_registerPlugin(&plugin);
 }
 
+int RegisterPassivePlugin() {
+  mjpPlugin plugin;
+  mjp_defaultPlugin(&plugin);
+
+  plugin.name = "mujoco.test.passive";
+
+  const char* attributes[] = {"attribute"};
+  plugin.nattribute = sizeof(attributes) / sizeof(*attributes);
+  plugin.attributes = attributes;
+
+  plugin.type |= mjPLUGIN_PASSIVE;
+
+  plugin.nstate = +[](const mjModel* m, int instance) { return 0; };
+
+  plugin.init = +[](const mjModel* m, mjData* d, int instance) {
+    auto* passive = new TestPassive(m, d, instance);
+    d->plugin_data[instance] = reinterpret_cast<uintptr_t>(passive);
+    return 0;
+  };
+  plugin.destroy = +[](mjData* d, int instance) {
+    delete reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
+    d->plugin_data[instance] = 0;
+  };
+
+  plugin.reset = +[](const mjModel* m, mjData* d, int instance) {
+    auto passive = reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
+    passive->Reset();
+  };
+  plugin.compute = +[](const mjModel* m, mjData* d, int instance, int type) {
+    auto passive = reinterpret_cast<TestPassive*>(d->plugin_data[instance]);
+    passive->Compute();
+  };
+
+  return mjp_registerPlugin(&plugin);
+}
+
 class PluginTest : public MujocoTest {
  public:
   // register all plugins
@@ -285,6 +333,7 @@ class PluginTest : public MujocoTest {
     }
 
     RegisterActuatorPlugin();
+    RegisterPassivePlugin();
   }
 };
 
@@ -303,9 +352,13 @@ constexpr char xml[] = R"(
         <config key="multiplier" value="0.125"/>
       </instance>
     </required>
+    <required plugin="mujoco.test.passive"/>
   </extension>
   <worldbody>
     <body>
+      <plugin plugin="mujoco.test.passive">
+        <config key="attribute" value="0"/>
+      </plugin>
       <geom type="capsule" size="0.1" fromto="-1 0 0 -1 0 -1"/>
       <joint name="h1" type="hinge"/>
     </body>
@@ -335,7 +388,7 @@ constexpr char xml[] = R"(
 )";
 
 TEST_F(PluginTest, MultiplePluginTableBlocks) {
-  EXPECT_EQ(mjp_pluginCount(), kNumFakePlugins + 2);
+  EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
 
   const mjpPlugin* last_plugin = nullptr;
   int table_count = 0;
@@ -343,7 +396,7 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) {
     int slot;
     std::string name = absl::StrFormat("mujoco.test.fake%u", i);
     const mjpPlugin* plugin = mjp_getPlugin(name.c_str(), &slot);
-    EXPECT_EQ(slot, i);
+    EXPECT_EQ(slot, kNumTruePlugins+i);
     EXPECT_THAT(plugin, NotNull());
     if (plugin - last_plugin != 1) {
       ++table_count;
@@ -359,9 +412,10 @@ TEST_F(PluginTest, MultiplePluginTableBlocks) {
 }
 
 TEST_F(PluginTest, RegisterIdenticalPlugin) {
-  EXPECT_EQ(RegisterSensorPlugin(), 0);
-  EXPECT_EQ(RegisterActuatorPlugin(), kNumFakePlugins + 1);
-  EXPECT_EQ(mjp_pluginCount(), kNumFakePlugins + 2);
+  EXPECT_EQ(RegisterSensorPlugin(), kNumTruePlugins);
+  EXPECT_EQ(RegisterActuatorPlugin(), kNumTruePlugins + kNumFakePlugins + 1);
+  EXPECT_EQ(RegisterPassivePlugin(), kNumTruePlugins + kNumFakePlugins + 2);
+  EXPECT_EQ(mjp_pluginCount(), kNumTruePlugins + kNumFakePlugins + kNumTestPlugins);
 }
 
 TEST_F(PluginTest, SaveXml) {
@@ -431,7 +485,7 @@ TEST_F(PluginTest, SensorPlugin) {
   EXPECT_EQ(TestSensor::InitCount(), expected_init_count);
   EXPECT_EQ(TestSensor::DestroyCount(), expected_destroy_count);
 
-  EXPECT_EQ(m->nplugin, 5);
+  EXPECT_EQ(m->nplugin, 6);
   EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "twosensors"), 0);
   EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "threesensors"), 1);
 
@@ -450,7 +504,7 @@ TEST_F(PluginTest, SensorPlugin) {
                   testing::ElementsAreArray<int>({3*(i+1), 6*j, 3*j}));
       EXPECT_THAT(*reinterpret_cast<mjtNum(*)[3]>(d->plugin_state +
                                                   m->plugin_stateadr[4]),
-                  testing::ElementsAreArray<int>({5*(i+1), 10*j, 5*j}));
+                  testing::ElementsAreArray<int>({4*(i+1), 8*j, 4*j}));
       EXPECT_THAT(*reinterpret_cast<mjtNum(*)[18]>(d->sensordata),
                   testing::ElementsAreArray<int>({   i+1,   2*j,   j,
                                                   5*(i+1), 10*j, 5*j,
@@ -489,7 +543,7 @@ TEST_F(PluginTest, ActuatorPlugin) {
   EXPECT_EQ(TestActuator::InitCount(), expected_init_count);
   EXPECT_EQ(TestActuator::DestroyCount(), expected_destroy_count);
 
-  EXPECT_EQ(m->nplugin, 5);
+  EXPECT_EQ(m->nplugin, 6);
   EXPECT_EQ(mj_name2id(m, mjOBJ_PLUGIN, "actuator2"), 2);
 
   mjData* d = mj_makeData(m);

test/plugin/elasticity/CMakeLists.txt

@@ -0,0 +1,17 @@
+# Copyright 2022 DeepMind Technologies Limited
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+mujoco_test(cable_test)
+target_link_libraries(cable_test fixture gmock cable)
+

test/plugin/elasticity/cable_test.cc

@@ -0,0 +1,160 @@
+// Copyright 2022 DeepMind Technologies Limited
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// Tests for plugin-related functionalities.
+
+#include <array>
+#include <cstdint>
+#include <cstring>
+#include <sstream>
+#include <vector>
+
+#include <gmock/gmock.h>
+#include <gtest/gtest.h>
+#include <mujoco/mujoco.h>
+#include "test/fixture.h"
+
+namespace mujoco {
+namespace {
+using PluginTest = MujocoTest;
+
+
+// -------------------------------- cable -----------------------------------
+
+TEST_F(PluginTest, CantileverIntoCircle) {
+  static constexpr char cantilever_xml[] = R"(
+  <mujoco>
+    <option gravity="0 0 0"/>
+    <extension>
+      <required plugin="mujoco.elasticity.cable"/>
+    </extension>
+    <worldbody>
+      <geom type="plane" size="0 0 1" quat="1 0 0 0"/>
+      <site name="reference" pos="0 0 0"/>
+      <composite type="cable" curve="s" count="41 1 1" size="1" offset="0 0 1" initial="none">
+        <plugin plugin="mujoco.elasticity.cable">
+          <config key="twist" value="1e6"/>
+          <config key="bend" value="1e9"/>
+        </plugin>
+        <joint kind="main" damping="2"/>
+        <geom type="capsule" size=".005" density="1"/>
+      </composite>
+    </worldbody>
+    <contact>
+      <exclude body1="B_first" body2="B_last"/>
+    </contact>
+    <sensor>
+      <framepos objtype="site" objname="S_last"/>
+    </sensor>
+    <actuator>
+      <motor site="S_last" gear="0 0 0 0 1 0" ctrllimited="true" ctrlrange="0 4"/>
+    </actuator>
+  </mujoco>
+  )";
+
+  char error[1024] = {0};
+
+  mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::NotNull()) << error;
+  mjData* d = mj_makeData(m);
+
+  // see Oliver Weeger, Sai-Kit Yeung, Martin L. Dunn, "Isogeometric collocation methods for Cosserat rods and rod
+  // structures", section 7.1 (DOI: j.cma.2016.05.009), the torque for achieving an angle phi is phi * E * Iy.
+  mjtNum Iy = mjPI * pow(0.005, 4) / 4;
+  mjtNum torque = 2 * mjPI * 1e9 * Iy;
+  for (int i=0; i < 1300; i++) {
+    if (i < 300) {
+      d->ctrl[0] += torque / 300;
+    }
+    mj_step(m, d);
+  }
+  EXPECT_NEAR(d->sensordata[0], 0, std::numeric_limits<float>::epsilon());
+  EXPECT_NEAR(d->sensordata[1], 0, std::numeric_limits<float>::epsilon());
+  EXPECT_NEAR(d->sensordata[2], 1, std::numeric_limits<float>::epsilon());
+  mj_deleteData(d);
+  mj_deleteModel(m);
+}
+
+TEST_F(PluginTest, InvalidTxtAttribute) {
+  static constexpr char cantilever_xml[] = R"(
+  <mujoco>
+    <extension>
+      <required plugin="mujoco.elasticity.cable">
+        <instance name="invalid">
+          <config key="twist" value="one"/>
+          <config key="bend" value="1"/>
+        </instance>
+      </required>
+    </extension>
+    <worldbody>
+      <geom type="plane" size="0 0 1" quat="1 0 0 0"/>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024] = {0};
+
+  mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::IsNull());
+}
+
+TEST_F(PluginTest, InvalidMixedAttribute) {
+  static constexpr char cantilever_xml[] = R"(
+  <mujoco>
+    <extension>
+      <required plugin="mujoco.elasticity.cable">
+        <instance name="invalid">
+          <config key="twist" value="1"/>
+          <config key="bend" value="1 is not a number"/>
+        </instance>
+      </required>
+    </extension>
+    <worldbody>
+      <geom type="plane" size="0 0 1" quat="1 0 0 0"/>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024] = {0};
+
+  mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::IsNull());
+}
+
+TEST_F(PluginTest, ValidAttributes) {
+  static constexpr char cantilever_xml[] = R"(
+  <mujoco>
+    <extension>
+      <required plugin="mujoco.elasticity.cable">
+        <instance name="invalid">
+          <config key="twist" value="0.0"/>
+          <config key="bend" value=" 0  "/>
+        </instance>
+      </required>
+    </extension>
+    <worldbody>
+      <geom type="plane" size="0 0 1" quat="1 0 0 0"/>
+    </worldbody>
+  </mujoco>
+  )";
+
+  char error[1024] = {0};
+
+  mjModel* m = LoadModelFromString(cantilever_xml, error, sizeof(error));
+  ASSERT_THAT(m, testing::NotNull()) << error;
+  mj_deleteModel(m);
+}
+
+}  // namespace
+}  // namespace mujoco

test/sample/testspeed_test.sh

@@ -54,6 +54,9 @@ for model_dir in ${MODEL_DIRS[@]}; do
       echo "Skipping $model" >&2
       continue
     fi
+    if grep -q "plugin" $model; then
+      continue
+    fi
     test_model "$model"
   done
 done

test/xml/xml_native_writer_test.cc

@@ -848,7 +848,8 @@ TEST_F(XMLWriterTest, WriteReadCompare) {
         std::string xml = p.path().string();
 
         // if file is meant to fail, skip it
-        if (absl::StrContains(p.path().string(), "malformed_")) {
+        if (absl::StrContains(p.path().string(), "malformed_") ||
+            absl::StrContains(p.path().string(), "plugin")) {
           continue;
         }