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

Copy joint state to the modified mjSpec when detaching bodies.

Browse Source 
Fixes #2085.

PiperOrigin-RevId: 678351210
Change-Id: I2a546877dd50abc7805099f8d5c40807ffe780e6
This commit is contained in:
Alessio Quaglino
2024-09-24 12:25:37 -07:00
committed by Copybara-Service
parent b09c1a977e
commit f0d52813b8
2 changed files with 39 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/user/user_model.cc
+19 -15
View File
@@ -402,21 +402,28 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
oldmodel.ProcessLists(/*checkrepeat=*/false);
}
// create global lists in this model if not compiled
if (!IsCompiled()) {
MakeLists(bodies_[0]);
ProcessLists(/*checkrepeat=*/false);
}
// TODO: carry over pending keyframes
key_pending_.clear();
StoreKeyframes();
// all keyframes are now pending and they will be resized
DeleteAll(keys_);
// remove body from tree
mjCBody* world = bodies_[0];
*world -= subtree;
// create global lists
if (compiled) {
ResetTreeLists();
}
// update global lists
ResetTreeLists();
MakeLists(world);
ProcessLists(/*checkrepeat=*/false);
// store keyframes in the old model
oldmodel.key_pending_.clear();
oldmodel.StoreKeyframes();
// check if we have to remove anything else
RemoveFromList(pairs_, oldmodel);
RemoveFromList(excludes_, oldmodel);
@@ -425,12 +432,6 @@ mjCModel& mjCModel::operator-=(const mjCBody& subtree) {
RemoveFromList(actuators_, oldmodel);
RemoveFromList(sensors_, oldmodel);
// move all keyframes to pending so that they will be resized
DeleteAll(keys_);
for (const auto& key : oldmodel.key_pending_) {
key_pending_.push_back(key);
}
// restore to the original state
if (!compiled) {
ResetTreeLists();
@@ -3091,7 +3092,10 @@ void mjCModel::StoreKeyframes() {
resetlists = true;
}
SaveDofOffsets();
// do not change the offset computed during compilation in case the user wants to recompile
if (!compiled) {
SaveDofOffsets();
}
for (auto& key : keys_) {
mjKeyInfo info;
test/user/user_api_test.cc
+20 -11
View File
@@ -538,7 +538,10 @@ static constexpr char xml_child[] = R"(
</body>
</frame>
</frame>
<body name="ignore"/>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</worldbody>
@@ -563,8 +566,8 @@ static constexpr char xml_child[] = R"(
</contact>
<keyframe>
<key name="two" time="2" qpos="2" act="2 2" ctrl="2 2"/>
<key name="three" time="3" qpos="3" act="3 3" ctrl="3 3"/>
<key name="two" time="2" qpos="2 22" act="2 2" ctrl="2 2"/>
<key name="three" time="3" qpos="3 33" act="3 3" ctrl="3 3"/>
</keyframe>
</mujoco>)";
@@ -594,7 +597,10 @@ TEST_F(MujocoTest, AttachSame) {
<body name="targetbody"/>
<body/>
</body>
<body name="ignore"/>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0">
<body name="attached-body-1">
<joint type="hinge" name="attached-hinge-1"/>
@@ -634,10 +640,10 @@ TEST_F(MujocoTest, AttachSame) {
</contact>
<keyframe>
<key name="two" time="2" qpos="2 0" act="2 2 0 0" ctrl="2 2 0 0"/>
<key name="three" time="3" qpos="3 0" act="3 3 0 0" ctrl="3 3 0 0"/>
<key name="attached-two-1" time="2" qpos="0 2" act="0 0 2 2" ctrl="0 0 2 2"/>
<key name="attached-three-1" time="3" qpos="0 3" act="0 0 3 3" ctrl="0 0 3 3"/>
<key name="two" time="2" qpos="2 22 0" act="2 2 0 0" ctrl="2 2 0 0"/>
<key name="three" time="3" qpos="3 33 0" act="3 3 0 0" ctrl="3 3 0 0"/>
<key name="attached-two-1" time="2" qpos="0 22 2" act="0 0 2 2" ctrl="0 0 2 2"/>
<key name="attached-three-1" time="3" qpos="0 33 3" act="0 0 3 3" ctrl="0 0 3 3"/>
</keyframe>
</mujoco>)";
@@ -942,7 +948,10 @@ void TestDetachBody(bool compile) {
<frame name="cframe">
</frame>
</frame>
<body name="ignore"/>
<body name="ignore">
<geom size=".1"/>
<joint type="slide"/>
</body>
<frame name="frame" pos=".1 0 0" euler="0 90 0"/>
</worldbody>
@@ -951,8 +960,8 @@ void TestDetachBody(bool compile) {
</sensor>
<keyframe>
<key name="two" time="2"/>
<key name="three" time="3"/>
<key name="two" time="2" qpos="22"/>
<key name="three" time="3" qpos="33"/>
</keyframe>
</mujoco>)";