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

Update BVH during fusestatic. Fixes #1069 and #1577.

Browse Source 
PiperOrigin-RevId: 631748750
Change-Id: I4e1ae01207568c90fc49bcd7d9f760f5187f1afa
This commit is contained in:
Alessio Quaglino
2024-05-08 04:12:04 -07:00
committed by Copybara-Service
parent 4eca3b048d
commit 96844db926
5 changed files with 42 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
doc/changelog.rst
+9
View File
@@ -2,6 +2,15 @@
Changelog
=========
Upcoming version (not yet released)
-----------------------------------
Bug fixes
^^^^^^^^^
1. Fixed a bug the could cause collisions to be missed when :ref:`fusestatic<compiler-fusestatic>` is enabled, as is
often the case for URDF imports.
Version 3.1.5 (May 7, 2024)
---------------------------
src/user/user_model.cc
+7 -1
View File
@@ -2969,12 +2969,18 @@ void mjCModel::FuseStatic(void) {
// recompute parent contype, conaffinity, and margin
par->contype = par->conaffinity = 0;
par->margin = 0;
for (const auto& geom : geoms) {
for (const auto& geom : par->geoms) {
par->contype |= geom->contype;
par->conaffinity |= geom->conaffinity;
par->margin = mju_max(par->margin, geom->margin);
}
// recompute BVH
int nbvhfuse = body->tree.nbvh + par->tree.nbvh;
par->ComputeBVH();
nbvhstatic += par->tree.nbvh - nbvhfuse;
nbvh += par->tree.nbvh - nbvhfuse;
//------------- delete body (without deleting children)
// delete allocation
src/user/user_objects.cc
+19 -8
View File
@@ -399,6 +399,7 @@ void mjCBoundingVolumeHierarchy::Set(mjtNum ipos_element[3], mjtNum iquat_elemen
void mjCBoundingVolumeHierarchy::AllocateBoundingVolumes(int nleaf) {
nbvh = 0;
bvh.clear();
child.clear();
nodeid.clear();
level.clear();
@@ -1333,6 +1334,23 @@ void mjCBody::MakeInertialExplicit() {
}
// compute bounding volume hierarchy
void mjCBody::ComputeBVH() {
if (geoms.empty()) {
return;
}
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
}
tree.CreateBVH();
}
// compiler
void mjCBody::Compile(void) {
CopyFromSpec();
@@ -1447,14 +1465,7 @@ void mjCBody::Compile(void) {
}
// compute bounding volume hierarchy
if (!geoms.empty()) {
tree.Set(ipos, iquat);
tree.AllocateBoundingVolumes(geoms.size());
for (int i=0; i<geoms.size(); i++) {
geoms[i]->SetBoundingVolume(tree.GetBoundingVolume(i));
}
tree.CreateBVH();
}
ComputeBVH();
// compile all joints, count dofs
dofnum = 0;
src/user/user_objects.h
+3
View File
@@ -293,6 +293,9 @@ class mjCBody : public mjCBody_, private mjsBody {
// set explicitinertial to true
void MakeInertialExplicit();
// compute the bounding volume hierarchy of the body.
void ComputeBVH();
// variables set by user
mjsBody spec;
test/user/user_model_test.cc
+4
View File
@@ -251,6 +251,7 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
<geom size="0.5" pos="1 0 0" contype="0" conaffinity="0"/>
<body>
<geom size="0.5" pos="0 1 0" contype="1" conaffinity="1"/>
<geom size="0.5" pos="0 -2 0" contype="1" conaffinity="1"/>
</body>
</body>
</worldbody>
@@ -273,6 +274,9 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) {
EXPECT_EQ(m_fuse->body_contype[1], 1);
EXPECT_EQ(m_fuse->body_conaffinity[1], 1);
EXPECT_EQ(m_no_fuse->body_bvhnum[2], 3);
EXPECT_EQ(m_fuse->body_bvhnum[1], 3);
mjData* d_fuse = mj_makeData(m_fuse);
mjData* d_no_fuse = mj_makeData(m_no_fuse);