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

Accumulate rotation after updating pos and quat in replicate.

Browse Source 
Fixes #2501.

PiperOrigin-RevId: 737929158
Change-Id: I0cad2de37190f1d30d427ae646acbde335b5be56
This commit is contained in:
Alessio Quaglino
2025-03-18 02:50:12 -07:00
committed by Copybara-Service
parent 1bf24e9f67
commit bae5175cf2
3 changed files with 21 additions and 11 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/xml/xml_native_reader_test.cc
+13 -5
View File
@@ -1230,7 +1230,7 @@ TEST_F(XMLReaderTest, ParseReplicate) {
</asset>
<worldbody>
<replicate count="101" euler="0 0 1.8">
<replicate count="101" offset="3 0 .1" euler="0 0 1.8">
<body name="body" pos="0 -1 0">
<joint type="slide"/>
<geom name="g" size="1"/>
@@ -1260,6 +1260,7 @@ TEST_F(XMLReaderTest, ParseReplicate) {
EXPECT_THAT(m, testing::NotNull()) << error.data();
EXPECT_THAT(m->ngeom, 105);
EXPECT_THAT(m->nsensor, 4);
EXPECT_THAT(m->nbody, 102);
// check that the separator is used correctly
for (int i = 0; i < 2; ++i) {
@@ -1289,12 +1290,19 @@ TEST_F(XMLReaderTest, ParseReplicate) {
}
}
// check body positions
mjtNum pos[2] = {0, 0};
for (int i = 1; i < 102; ++i) {
mjtNum theta = (i-1) * 1.8 * mjPI / 180;
EXPECT_NEAR(m->body_pos[3*i+0], pos[0] + sin(theta), 1e-8) << i;
EXPECT_NEAR(m->body_pos[3*i+1], pos[1] - cos(theta), 1e-8) << i;
EXPECT_NEAR(m->body_pos[3*i+2], (i-1) * .1, 1e-8);
pos[0] += 3 * cos(theta);
pos[1] += 3 * sin(theta);
}
// check that the final pose is correct
int n = m->nbody-1;
EXPECT_THAT(m->nbody, 102);
EXPECT_NEAR(m->body_pos[3*n+0], 0, 1e-8);
EXPECT_NEAR(m->body_pos[3*n+1], 1, 1e-8);
EXPECT_EQ(m->body_pos[3*n+2], 0);
EXPECT_NEAR(m->body_quat[4*n+0], 0, 1e-8);
EXPECT_EQ(m->body_quat[4*n+1], 0);
EXPECT_EQ(m->body_quat[4*n+2], 0);