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

Recompute sameframe flags in mj_setConst.

Browse Source 
The `body_sameframe`, `geom_sameframe`, and `site_sameframe` flags are now recomputed within `mj_setConst` based on the current model geometry. This allows these flags to be updated if the model's body/geom/site frames are modified after the initial compilation. Tests are added to verify the correct recomputation and its effect on forward kinematics. The compiler also checks that its initial computation matches the result of `mj_setConst`.

Fixes #3029, #1628.

PiperOrigin-RevId: 941758649
Change-Id: I7b995f8c67ad305bca5b52732436ba578c905d73
This commit is contained in:
Yuval Tassa
2026-07-02 10:48:21 -07:00
committed by Copybara-Service
parent 0dfa4b509a
commit 14c0b0c92b
3 changed files with 258 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_setconst_test.cc
+147
View File
@@ -457,5 +457,152 @@ TEST_F(SetConstTest, DofLength) {
EXPECT_NEAR(model->dof_length[10], 5, tol);
}
TEST_F(SetConstTest, BodySameframeRecomputed) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="slide"/>
<geom size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d(mj_makeData(m.get()));
int b = mj_name2id(m.get(), mjOBJ_BODY, "B1");
// initially sameframe should be BODY (ipos=0, iquat=identity)
EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODY);
// perturb body_ipos, call mj_setConst
m->body_ipos[3*b+0] = 1.0;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODYROT);
// also perturb body_iquat
m->body_iquat[4*b+0] = 0.5;
m->body_iquat[4*b+1] = 0.5;
m->body_iquat[4*b+2] = 0.5;
m->body_iquat[4*b+3] = 0.5;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_NONE);
// restore to identity, should go back to BODY
m->body_ipos[3*b+0] = 0;
m->body_iquat[4*b+0] = 1;
m->body_iquat[4*b+1] = 0;
m->body_iquat[4*b+2] = 0;
m->body_iquat[4*b+3] = 0;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODY);
}
TEST_F(SetConstTest, GeomSameframeRecomputed) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="slide"/>
<geom name="G1" size=".1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d(mj_makeData(m.get()));
int g = mj_name2id(m.get(), mjOBJ_GEOM, "G1");
// initially sameframe should be BODY
EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODY);
// perturb geom_pos
m->geom_pos[3*g+1] = 0.5;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODYROT);
// restore, should go back to BODY
m->geom_pos[3*g+1] = 0;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->geom_sameframe[g], mjSAMEFRAME_BODY);
}
TEST_F(SetConstTest, SiteSameframeRecomputed) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1">
<joint type="slide"/>
<geom size=".1"/>
<site name="S1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d(mj_makeData(m.get()));
int s = mj_name2id(m.get(), mjOBJ_SITE, "S1");
// initially sameframe should be BODY
EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODY);
// perturb site_pos
m->site_pos[3*s+2] = 0.3;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODYROT);
// restore
m->site_pos[3*s+2] = 0;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->site_sameframe[s], mjSAMEFRAME_BODY);
}
TEST_F(SetConstTest, SameframeKinematicsCorrect) {
constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body name="B1" pos="1 0 0">
<joint type="slide" axis="1 0 0"/>
<geom name="G1" size=".1"/>
<site name="S1"/>
</body>
</worldbody>
</mujoco>
)";
char error[1024];
MjModelPtr m = LoadModelFromString(xml, error, sizeof(error));
ASSERT_THAT(m.get(), NotNull()) << error;
MjDataPtr d(mj_makeData(m.get()));
int b = mj_name2id(m.get(), mjOBJ_BODY, "B1");
int g = mj_name2id(m.get(), mjOBJ_GEOM, "G1");
// perturb body inertial offset, breaking sameframe
m->body_ipos[3*b+1] = 0.5;
mj_setConst(m.get(), d.get());
EXPECT_EQ(m->body_sameframe[b], mjSAMEFRAME_BODYROT);
// run forward kinematics, check that xipos != xpos
mj_forward(m.get(), d.get());
EXPECT_NEAR(d->xipos[3*b+1], 0.5, MjTol(1e-10, 1e-6));
EXPECT_NEAR(d->xpos[3*b+1], 0.0, MjTol(1e-10, 1e-6));
// perturb geom_pos, check geom global position
m->geom_pos[3*g+2] = 0.3;
mj_setConst(m.get(), d.get());
EXPECT_NE(m->geom_sameframe[g], mjSAMEFRAME_BODY);
mj_forward(m.get(), d.get());
EXPECT_NEAR(d->geom_xpos[3*g+2], 0.3, MjTol(1e-10, 1e-6));
}
} // namespace
} // namespace mujoco