From bae5175cf223a19fd4aff46e9588190039d225c1 Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Tue, 18 Mar 2025 02:50:12 -0700 Subject: [PATCH] Accumulate rotation after updating pos and quat in `replicate`. Fixes #2501. PiperOrigin-RevId: 737929158 Change-Id: I0cad2de37190f1d30d427ae646acbde335b5be56 --- model/humanoid/100_humanoids.xml | 4 ++-- src/xml/xml_native_reader.cc | 10 ++++++---- test/xml/xml_native_reader_test.cc | 18 +++++++++++++----- 3 files changed, 21 insertions(+), 11 deletions(-) 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);