chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
model/humanoid/100_humanoids.xml
+2 -2
View File
@@ -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>
src/xml/xml_native_reader.cc
+6 -4
View File
@@ -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);
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);