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

Add site semantics to weld constraint. Fixes #1896

Browse Source 
PiperOrigin-RevId: 675112981
Change-Id: Ie4c01440d54116075df89f480591700762892cbd
This commit is contained in:
Taylor Howell
2024-09-16 05:44:53 -07:00
committed by Copybara-Service
parent 2da15cf137
commit 60a1921b34
13 changed files with 368 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/testdata/equality_site.xml
Vendored
+48
View File
@@ -0,0 +1,48 @@
<mujoco model="Connect and Weld defined with sites">
<option viscosity="1">
<flag contact="disable"/>
</option>
<visual>
<scale framewidth="0.05" framelength="0.4"/>
</visual>
<default>
<default class="free">
<geom type="box" size=".2" fromto=".2 .2 0 .2 .2 -2" rgba=".4 .7 .6 1"/>
</default>
<site rgba=".0 .7 0 1"/>
</default>
<worldbody>
<geom pos="0 0 -2" type="plane" size="10 10 .01"/>
<light pos="0 0 20"/>
<site name="a1" pos="-2 0 0" euler="-90 0 0"/>
<body pos="-2 1 0">
<freejoint/>
<geom class="free"/>
<site name="a2"/>
</body>
<site name="b1" euler="-90 0 0"/>
<body pos="0 1 0">
<freejoint/>
<geom class="free"/>
<site name="b2"/>
</body>
<site name="c1" pos="2 0 0" euler="-90 0 0"/>
<body pos="2 1 0">
<freejoint/>
<geom class="free"/>
<site name="c2"/>
</body>
</worldbody>
<equality>
<connect name="connect" site1="a1" site2="a2"/>
<weld name="weak weld" site1="b1" site2="b2" torquescale="0.2"/>
<weld name="weld" site1="c1" site2="c2"/>
</equality>
</mujoco>
test/engine/testdata/weld.xml
Vendored
+11
View File
@@ -47,6 +47,16 @@
<freejoint/>
<geom class="free"/>
</body>
<body name="box5" pos="5 0 0">
<geom type="box" class="static"/>
<site name="box5"/>
</body>
<body name="beam5" pos="5 0 0">
<freejoint/>
<geom class="free"/>
<site name="beam5" pos="0 -1 0" euler="45 20 5"/>
</body>
</worldbody>
<equality>
@@ -54,5 +64,6 @@
<weld name="anchor no torques" body1="box2" body2="beam2" torquescale="0" anchor="0 -2 0"/>
<weld name="relpose" body1="box3" body2="beam3" relpose="0 0 0 1 -.3 0 0"/>
<weld name="relpose+anchor" body1="box4" body2="beam4" relpose="0 0 0 1 -.3 0 0" anchor="0 0 -1"/>
<weld name="site" site1="box5" site2="beam5"/>
</equality>
</mujoco>
test/user/user_objects_test.cc
+112 -7
View File
@@ -2328,14 +2328,14 @@ TEST_F(UserObjectsTest, BadConnect) {
string xml = base.replace(pos, len, "<connect body1='1' anchor='0 0 1'/>");
char error[1024];
mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
EXPECT_THAT(m, NotNull()) << error;
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 1, 0, 0, 2));
mj_deleteModel(m);
// good model using site semantic
xml = base.replace(pos, len, "<connect site1='0' site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
EXPECT_THAT(m, NotNull()) << error;
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 6), ElementsAre(0, 0, 0, 0, 0, 0));
mj_deleteModel(m);
@@ -2345,7 +2345,7 @@ TEST_F(UserObjectsTest, BadConnect) {
// bad model (missing anchor)
xml = base.replace(pos, len, "<connect body1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
EXPECT_THAT(m, IsNull());
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_missing));
char error_mixed[] = "body and site semantics cannot be mixed"
@@ -2354,13 +2354,13 @@ TEST_F(UserObjectsTest, BadConnect) {
// bad model (mixing body and site)
xml = base.replace(pos, len, "<connect body1='1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
EXPECT_THAT(m, IsNull());
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// load spec with no constraints
xml = base.erase(pos, len);
mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
EXPECT_THAT(s, NotNull()) << error;
ASSERT_THAT(s, NotNull()) << error;
// add a connect but don't set objtype
mjsEquality* equality = mjs_addEquality(s, nullptr);
@@ -2370,14 +2370,119 @@ TEST_F(UserObjectsTest, BadConnect) {
// expect compilation to fail
m = mj_compile(s, nullptr);
EXPECT_THAT(m, IsNull());
ASSERT_THAT(m, IsNull());
EXPECT_THAT(mjs_getError(s),
HasSubstr("connect constraint supports only sites and bodies"));
// set objtype, expect compilation to succeed
equality->objtype = mjOBJ_SITE;
m = mj_compile(s, nullptr);
EXPECT_THAT(m, NotNull()) << mjs_getError(s);
ASSERT_THAT(m, NotNull()) << mjs_getError(s);
mj_deleteModel(m);
mj_deleteSpec(s);
}
TEST_F(UserObjectsTest, BadWeld) {
string base = R"(
<mujoco>
<worldbody>
<site name="0" size="1"/>
<body name="1" pos="0 0 1">
<freejoint/>
<geom size="1"/>
<site name="1"/>
</body>
</worldbody>
<equality>
WELD
</equality>
</mujoco>
)";
int pos = base.find("WELD");
int len = 4;
// good model using body semantic
string xml = base.replace(pos, len, "<weld body1='1' anchor='0 0 1' torquescale='2'/>");
char error[1024];
mjModel* m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 10),
ElementsAre(0, 0, 1, 0, 0, 0, 1, 0, 0, 0));
mj_deleteModel(m);
// good model using site semantic
xml = base.replace(pos, len, "<weld site1='0' site2='1' torquescale='2'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, NotNull()) << error;
EXPECT_THAT(AsVector(m->eq_data, 10),
ElementsAre(0, 1, 0, 0, 0, 0, 0, 0, 0, 0));
mj_deleteModel(m);
char error_mixed[] =
"body and site semantics cannot be mixed"
"\nElement 'weld', line 12";
// bad model (mixing body and site)
xml = base.replace(pos, len, "<weld body1='1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (mixing site and anchor)
xml = base.replace(pos, len, "<weld anchor='0 0 1' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (mixing site and relpose)
xml = base.replace(pos, len, "<weld relpose='0 0 0 1 0 0 0' site1='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
// bad model (body and site semantics are valid, but both are specified)
xml = base.replace(pos, len, "<weld body1='1' site1='1' site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_mixed));
char error_underspecified[] =
"either body1 must be defined and optionally {body2, anchor, "
"relpose}, or site1 and site2 must be defined\nElement 'weld', line 12";
// bad model (underspecified body semantics)
xml = base.replace(pos, len, "<weld anchor='0 0 1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_underspecified));
// bad model (underspecified site semantics)
xml = base.replace(pos, len, "<weld site2='1'/>");
m = LoadModelFromString(xml.c_str(), error, sizeof(error));
ASSERT_THAT(m, IsNull());
EXPECT_THAT(error, HasSubstr(error_underspecified));
// load spec with no constraints
xml = base.erase(pos, len);
mjSpec* s = mj_parseXMLString(xml.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(s, NotNull()) << error;
// add a weld but don't set objtype
mjsEquality* equality = mjs_addEquality(s, nullptr);
equality->type = mjEQ_WELD;
mjs_setString(equality->name1, "0");
mjs_setString(equality->name2, "1");
// expect compilation to fail
m = mj_compile(s, nullptr);
ASSERT_THAT(m, IsNull());
EXPECT_THAT(mjs_getError(s),
HasSubstr("weld constraint supports only sites and bodies"));
// set objtype, expect compilation to succeed
equality->objtype = mjOBJ_SITE;
m = mj_compile(s, nullptr);
ASSERT_THAT(m, NotNull()) << mjs_getError(s);
mj_deleteModel(m);
mj_deleteSpec(s);
}