- 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

src/engine/engine_core_smooth.c

@@ -23,6 +23,7 @@
 #include "engine/engine_core_constraint.h"
 #include "engine/engine_io.h"
 #include "engine/engine_macro.h"
+#include "engine/engine_plugin.h"
 #include "engine/engine_support.h"
 #include "engine/engine_util_blas.h"
 #include "engine/engine_util_errmem.h"
@@ -1422,6 +1423,25 @@ void mj_passive(const mjModel* m, mjData* d) {
   if (mjcb_passive) {
     mjcb_passive(m, d);
   }
+
+  // plugin
+  if (m->nplugin) {
+    const int nslot = mjp_pluginCount();
+    // iterate over plugins, call compute if type is mjPLUGIN_PASSIVE
+    for (int i=0; i<m->nplugin; i++) {
+      const int slot = m->plugin[i];
+      const mjpPlugin* plugin = mjp_getPluginAtSlotUnsafe(slot, nslot);
+      if (!plugin) {
+        mju_error_i("invalid plugin slot: %d", slot);
+      }
+      if (plugin->type & mjPLUGIN_PASSIVE) {
+        if (!plugin->compute) {
+          mju_error_i("`compute` is a null function pointer for plugin at slot %d", slot);
+        }
+        plugin->compute(m, d, i, mjPLUGIN_PASSIVE);
+      }
+    }
+  }
 }
 
 

src/engine/engine_io.c

@@ -875,10 +875,14 @@ static mjData* _makeData(const mjModel* m) {
   for (int i = 0; i < m->nplugin; ++i) {
     d->plugin[i] = m->plugin[i];
     const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
-    if (!plugin->init) {
-      mju_error_i("`init` is a null function pointer for plugin at slot %d", m->plugin[i]);
+    if (plugin->init) {
+      if (plugin->init(m, d, i) < 0) {
+        mju_free(d->buffer);
+        mju_free(d->arena);
+        mju_free(d);
+        mju_error_i("plugin->init failed for plugin id %d", i);
+      }
     }
-    plugin->init(m, d, i);
   }
 
   return d;
@@ -1142,10 +1146,9 @@ static void _resetData(const mjModel* m, mjData* d, unsigned char debug_value) {
   for (int i = 0; i < m->nplugin; ++i) {
     d->plugin[i] = m->plugin[i];
     const mjpPlugin* plugin = mjp_getPluginAtSlot(m->plugin[i]);
-    if (!plugin->reset) {
-      mju_error_i("`reset` is a null function pointer for plugin at slot %d", m->plugin[i]);
+    if (plugin->reset) {
+      plugin->reset(m, d, i);
     }
-    plugin->reset(m, d, i);
   }
 }
 
@@ -1333,6 +1336,7 @@ const char* mj_validateReferences(const mjModel* m) {
   X(body_jntadr,        nbody,         njnt         , m->body_jntnum         ) \
   X(body_dofadr,        nbody,         nv           , m->body_dofnum         ) \
   X(body_geomadr,       nbody,         ngeom        , m->body_geomnum        ) \
+  X(body_plugin,        nbody,         nplugin      , 0                      ) \
   X(jnt_qposadr,        njnt,          nq           , 0                      ) \
   X(jnt_dofadr,         njnt,          nv           , 0                      ) \
   X(jnt_bodyid,         njnt,          nbody        , 0                      ) \
@@ -1364,7 +1368,7 @@ const char* mj_validateReferences(const mjModel* m) {
   X(pair_geom2,         npair,         ngeom        , 0                      ) \
   X(actuator_plugin,    nu,            nplugin      , 0                      ) \
   X(sensor_plugin,      nsensor,       nplugin      , 0                      ) \
-  X(plugin_stateadr,    nplugin,       npluginstate , 0                      ) \
+  X(plugin_stateadr,    nplugin,       npluginstate , m->plugin_statenum     ) \
   X(plugin_attradr,     nplugin,       npluginattr  , 0                      ) \
   X(tendon_adr,         ntendon,       nwrap        , m->tendon_num          ) \
   X(tendon_matid,       ntendon,       nmat         , 0                      ) \

src/engine/engine_util_blas.h

@@ -91,7 +91,7 @@ MJAPI void mju_addToScl3(mjtNum res[3], const mjtNum vec[3], mjtNum scl);
 // res = vec1 + vec2*scl
 MJAPI void mju_addScl3(mjtNum res[3], const mjtNum vec1[3], const mjtNum vec2[3], mjtNum scl);
 
-// normalize vector, return length before normalization
+// normalize vector, return length before normalization, set to [1, 0, 0] if norm is tiny
 MJAPI mjtNum mju_normalize3(mjtNum vec[3]);
 
 // compute vector length (without normalizing)