From 7eb8c4b9f486662cddf5ae9011cb4bb7666203cd Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Wed, 1 May 2024 09:04:40 -0700 Subject: [PATCH] Set all flex edges to rigid when the flex is rigid. PiperOrigin-RevId: 629740153 Change-Id: Ib2fb6f64e3d160268d2521d63830f41e44d03b20 --- src/engine/engine_setconst.c | 10 ++++++++++ src/user/user_flexcomp.cc | 3 +++ src/user/user_model.cc | 12 ++++++++---- test/user/testdata/rigid_flex.xml | 25 +++++++++++++++++++++++++ test/user/user_flex_test.cc | 12 ++++++++++++ 5 files changed, 58 insertions(+), 4 deletions(-) create mode 100644 test/user/testdata/rigid_flex.xml diff --git a/src/engine/engine_setconst.c b/src/engine/engine_setconst.c index ff136d18..e1cd4907 100644 --- a/src/engine/engine_setconst.c +++ b/src/engine/engine_setconst.c @@ -15,6 +15,7 @@ #include "engine/engine_setconst.h" #include +#include #include #include @@ -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]; diff --git a/src/user/user_flexcomp.cc b/src/user/user_flexcomp.cc index ce49b644..6e7c08d2 100644 --- a/src/user/user_flexcomp.cc +++ b/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) { diff --git a/src/user/user_model.cc b/src/user/user_model.cc index e8a3791e..ec58d4c8 100644 --- a/src/user/user_model.cc +++ b/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 diff --git a/test/user/testdata/rigid_flex.xml b/test/user/testdata/rigid_flex.xml new file mode 100644 index 00000000..6982dc3d --- /dev/null +++ b/test/user/testdata/rigid_flex.xml @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + + + diff --git a/test/user/user_flex_test.cc b/test/user/user_flex_test.cc index 7b382dfe..257d6dca 100644 --- a/test/user/user_flex_test.cc +++ b/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 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");