Accumulate rotation after updating pos and quat in replicate.

quagla · copybara-github · commit bae5175cf223 · 2025-03-18T02:51:05.000-07:00
Fixes #2501. PiperOrigin-RevId: 737929158 Change-Id: I0cad2de37190f1d30d427ae646acbde335b5be56
diff --git a/model/humanoid/100_humanoids.xml b/model/humanoid/100_humanoids.xml
@@ -37,8 +37,8 @@
     <light directional="true" diffuse=".9 .9 .9" specular="0.1 0.1 0.1" pos="0 0 5" dir="0 0 -1" castshadow="true"/>
     <light name="spotlight" mode="targetbodycom" target="world" diffuse="1 1 1" specular="0.3 0.3 0.3" pos="-6 -6 4" cutoff="60"/>
     <replicate count="10" euler="0 0 36" sep="-">
-      <frame pos="1 0 0">
-        <replicate count="10" euler="0 0 15" sep="-" offset="0.5 0 0">
+      <frame pos="1.2 0 0">
+        <replicate count="10" euler="0 0 17" sep="-" offset="0.6 0 0">
           <attach model="humanoid" body="torso" prefix="_"/>
         </replicate>
       </frame>
diff --git 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
@@ -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);
