chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source 
PiperOrigin-RevId: 629740153
Change-Id: Ib2fb6f64e3d160268d2521d63830f41e44d03b20
This commit is contained in:
Alessio Quaglino
2024-05-01 09:04:40 -07:00
committed by Copybara-Service
parent ad5bee73ef
commit 7eb8c4b9f4
5 changed files with 58 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_setconst.c
+10
View File
@@ -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
+3
View File
@@ -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
+8 -4
View File
@@ -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
test/user/testdata/rigid_flex.xml
Vendored
+25
View File
@@ -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
+12
View File
@@ -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");