From d6553ed192eec115a488bcb60581f14b77bc8a2c Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 25 Apr 2024 07:45:07 -0700 Subject: [PATCH] Update contype and conaffinity if fusestatic is true. Fixes #1577. PiperOrigin-RevId: 628073529 Change-Id: I2fe88736d12ec16d72bbd4dce5d349b930147b50 --- src/user/user_model.cc | 9 +++++++++ test/user/user_model_test.cc | 9 +++++++-- 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/user/user_model.cc b/src/user/user_model.cc index 2d4b14e5..8f579bbc 100644 --- a/src/user/user_model.cc +++ b/src/user/user_model.cc @@ -2955,6 +2955,15 @@ void mjCModel::FuseStatic(void) { sites.clear(); FuseReindex(bodies[0]); + // recompute parent contype, conaffinity, and margin + par->contype = par->conaffinity = 0; + par->margin = 0; + for (const auto& geom : geoms) { + par->contype |= geom->contype; + par->conaffinity |= geom->conaffinity; + par->margin = mju_max(par->margin, geom->margin); + } + //------------- delete body (without deleting children) // delete allocation diff --git a/test/user/user_model_test.cc b/test/user/user_model_test.cc index bc0da348..d50b123e 100644 --- a/test/user/user_model_test.cc +++ b/test/user/user_model_test.cc @@ -248,9 +248,9 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { - + - + @@ -268,6 +268,11 @@ TEST_F(FuseStaticTest, FuseStaticEquivalent) { EXPECT_EQ(m_fuse->nbody, 2) << "Expecting a world body and one other body"; EXPECT_EQ(m_no_fuse->nbody, 3) << "Expecting a world body and two others"; + EXPECT_EQ(m_no_fuse->body_contype[2], 1); + EXPECT_EQ(m_no_fuse->body_conaffinity[2], 1); + EXPECT_EQ(m_fuse->body_contype[1], 1); + EXPECT_EQ(m_fuse->body_conaffinity[1], 1); + mjData* d_fuse = mj_makeData(m_fuse); mjData* d_no_fuse = mj_makeData(m_no_fuse);