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
-      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);
+      if (pfl->rigid) {
+        m->flexedge_rigid[edge_adr+k] = 1;
+      } else {
+        // 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