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];