diff --git a/model/humanoid/100_humanoids.xml b/model/humanoid/100_humanoids.xml
index 926c6130..ca7c8d60 100644
--- a/model/humanoid/100_humanoids.xml
+++ b/model/humanoid/100_humanoids.xml
@@ -37,8 +37,8 @@
-
-
+
+
diff --git a/src/xml/xml_native_reader.cc b/src/xml/xml_native_reader.cc
index c726a9fb..104c606b 100644
--- a/src/xml/xml_native_reader.cc
+++ b/src/xml/xml_native_reader.cc
@@ -3544,17 +3544,19 @@ void mjXReader::Body(XMLElement* section, mjsBody* body, mjsFrame* frame,
// update pframe and attach
for (int i = 0; i < count; i++) {
- // accumulate rotation
- mjuu_setvec(pframe->pos, pos[0], pos[1], pos[2]);
- mjuu_frameaccum(pos, quat, offset, rotation);
-
// overwrite orientation to increase precision
alt.euler[0] = i*euler[0];
alt.euler[1] = i*euler[1];
alt.euler[2] = i*euler[2];
mjs_resolveOrientation(quat, spec->compiler.degree, spec->compiler.eulerseq, &alt);
+
+ // set position and orientation
+ mjuu_setvec(pframe->pos, pos[0], pos[1], pos[2]);
mjuu_setvec(pframe->quat, quat[0], quat[1], quat[2], quat[3]);
+ // accumulate rotation
+ mjuu_frameaccum(pos, quat, offset, rotation);
+
// process suffix
string suffix = separator;
UpdateString(suffix, count, i);
diff --git a/test/xml/xml_native_reader_test.cc b/test/xml/xml_native_reader_test.cc
index 06e7b30a..52650330 100644
--- a/test/xml/xml_native_reader_test.cc
+++ b/test/xml/xml_native_reader_test.cc
@@ -1230,7 +1230,7 @@ TEST_F(XMLReaderTest, ParseReplicate) {
-
+
@@ -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);