Set all flex edges to rigid when the flex is rigid.

PiperOrigin-RevId: 629740153
Change-Id: Ib2fb6f64e3d160268d2521d63830f41e44d03b20

src/engine/engine_setconst.c

@@ -15,6 +15,7 @@
 #include "engine/engine_setconst.h"
 
 #include <stdio.h>
+#include <string.h>
 
 #include <mujoco/mjdata.h>
 #include <mujoco/mjmacro.h>
@@ -92,6 +93,11 @@ static void set0(mjModel* m, mjData* d) {
   // compute dof_M0 for CRB algorithm
   mj_setM0(m, d);
 
+  // save flex_rigid, temporarily make all flexes non-rigid
+  mjtByte* rigid = mju_malloc(m->nflex);
+  memcpy(rigid, m->flex_rigid, m->nflex);
+  memset(m->flex_rigid, 0, m->nflex);
+
   // run remaining computations
   mj_crb(m, d);
   mj_factorM(m, d);
@@ -99,6 +105,10 @@ static void set0(mjModel* m, mjData* d) {
   mj_tendon(m, d);
   mj_transmission(m, d);
 
+  // restore flex rigidity
+  memcpy(m->flex_rigid, rigid, m->nflex);
+  mju_free(rigid);
+
   // restore camera and light mode
   for (int i=0; i < m->ncam; i++) {
     m->cam_mode[i] = cammode[i];

src/user/user_flexcomp.cc

@@ -408,6 +408,9 @@ bool mjCFlexcomp::Make(mjSpec* spec, mjsBody* body, char* error, int error_sz) {
   mjs_setString(pf->name, name.c_str());
   mjs_setInt(pf->elem, element.data(), element.size());
   mjs_setFloat(pf->texcoord, texcoord.data(), texcoord.size());
+  if (!centered) {
+    mjs_setDouble(pf->vert, point.data(), point.size());
+  }
 
   // rigid: set parent name, nothing else to do
   if (rigid) {

src/user/user_model.cc

@@ -2372,10 +2372,14 @@ void mjCModel::CopyObjects(mjModel* m) {
       m->flex_edge[2*(edge_adr+k)] = pfl->edge[k].first;
       m->flex_edge[2*(edge_adr+k)+1] = pfl->edge[k].second;
 
-      // check if vertex body weldids are the same
+      if (pfl->rigid) {
-      int b1 = pfl->vertbodyid[pfl->edge[k].first];
+        m->flexedge_rigid[edge_adr+k] = 1;
-      int b2 = pfl->vertbodyid[pfl->edge[k].second];
+      } else {
-      m->flexedge_rigid[edge_adr+k] = (bodies[b1]->weldid == bodies[b2]->weldid);
+        // check if vertex body weldids are the same
+        int b1 = pfl->vertbodyid[pfl->edge[k].first];
+        int b2 = pfl->vertbodyid[pfl->edge[k].second];
+        m->flexedge_rigid[edge_adr+k] = (bodies[b1]->weldid == bodies[b2]->weldid);
+      }
     }
 
     // advance counters

test/user/testdata/rigid_flex.xml

@@ -0,0 +1,25 @@
+<mujoco model='test'>
+  <asset>
+    <mesh file="stanford_bunny.obj"/>
+  </asset>
+
+  <worldbody>
+    <geom type="plane" size=".5 .5 .05"/>
+    <body name="bunny1" pos="0 0 .4" euler="90 0 0">
+      <freejoint/>
+      <geom type="mesh" mesh="stanford_bunny" contype="0" conaffinity="0"/>
+      <flexcomp type="mesh" file="stanford_bunny.obj"
+          rgba="0 .7 .7 1" name="bunny1" mass="5" rigid="true">
+          <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
+      </flexcomp>
+    </body>
+    <body name="bunny2" pos="0 0 .2" euler="90 0 0">
+      <freejoint/>
+      <geom type="mesh" mesh="stanford_bunny" contype="0" conaffinity="0"/>
+      <flexcomp type="mesh" file="stanford_bunny.obj"
+          rgba="0 .7 .7 1" name="bunny2" mass="5" rigid="true">
+          <contact condim="3" solref="0.01 1" solimp=".95 .99 .0001" selfcollide="none"/>
+      </flexcomp>
+    </body>
+  </worldbody>
+</mujoco>

test/user/user_flex_test.cc

@@ -231,6 +231,18 @@ TEST_F(UserFlexTest, CreateBVHSuccess) {
   mj_deleteData(d);
 }
 
+TEST_F(UserFlexTest, RigidFlex) {
+  const std::string xml_path =
+      GetTestDataFilePath("user/testdata/rigid_flex.xml");
+  std::array<char, 1024> error;
+  mjModel* m = mj_loadXML(xml_path.c_str(), 0, error.data(), error.size());
+  ASSERT_THAT(m, NotNull()) << error.data();
+  mjData* d = mj_makeData(m);
+  mj_step(m, d);
+  mj_deleteModel(m);
+  mj_deleteData(d);
+}
+
 TEST_F(UserFlexTest, LoadMSHBinaryGMSH_41_Success) {
   const std::string xml_path =
       GetTestDataFilePath("user/testdata/shark_41_binary_gmshApp.xml");