Fix bugs in actuator parsing.

- Adhesion actuator defaults were not parsed, now fixed.
- Both damper and adhesion actuators were requiring `ctrlrange` at parse time, which prevented this attribute from being inherited from defaults, now fixed.
- Added tests for all 3 cases.

PiperOrigin-RevId: 475145255
Change-Id: I8dd6196e20734136d990f522893ebd4d156cd7a8
This commit is contained in:
Yuval Tassa
2022-09-18 11:09:23 -07:00
committed by Copybara-Service
parent 142c0eb200
commit 5dc26cf4a0
2 changed files with 95 additions and 17 deletions
+5 -4
View File
@@ -1598,7 +1598,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
pact->gainprm[2] = -pact->gainprm[2]; pact->gainprm[2] = -pact->gainprm[2];
// require nonnegative range // require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true); ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "damper control range cannot be negative"); throw mjXError(elem, "damper control range cannot be negative");
} }
@@ -1676,7 +1676,7 @@ void mjXReader::OneActuator(XMLElement* elem, mjCActuator* pact) {
throw mjXError(elem, "adhesion gain cannot be negative"); throw mjXError(elem, "adhesion gain cannot be negative");
// require nonnegative range // require nonnegative range
ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text, true); ReadAttr(elem, "ctrlrange", 2, pact->ctrlrange, text);
if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) { if (pact->ctrlrange[0]<0 || pact->ctrlrange[1]<0) {
throw mjXError(elem, "adhesion control range cannot be negative"); throw mjXError(elem, "adhesion control range cannot be negative");
} }
@@ -1947,7 +1947,7 @@ void mjXReader::Default(XMLElement* section, int parentid) {
// read tendon // read tendon
else if (name=="tendon") OneTendon(elem, &def->tendon); else if (name=="tendon") OneTendon(elem, &def->tendon);
// read actuator: general, motor, position, velocity, cylinder // read actuator
else if (name=="general" || else if (name=="general" ||
name=="motor" || name=="motor" ||
name=="position" || name=="position" ||
@@ -1955,7 +1955,8 @@ void mjXReader::Default(XMLElement* section, int parentid) {
name=="damper" || name=="damper" ||
name=="intvelocity" || name=="intvelocity" ||
name=="cylinder" || name=="cylinder" ||
name=="muscle") { name=="muscle" ||
name=="adhesion") {
OneActuator(elem, &def->actuator); OneActuator(elem, &def->actuator);
} }
+90 -13
View File
@@ -365,7 +365,7 @@ TEST_F(UserDataTest, RequiresControlRange) {
<worldbody> <worldbody>
<body> <body>
<geom size="1"/> <geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/> <joint name="jnt" type="slide" axis="1 0 0" range="-10 10" limited="true"/>
</body> </body>
</worldbody> </worldbody>
<actuator> <actuator>
@@ -376,7 +376,7 @@ TEST_F(UserDataTest, RequiresControlRange) {
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull()); ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("required attribute missing: 'ctrlrange'")); EXPECT_THAT(error.data(), HasSubstr("invalid control range"));
} }
TEST_F(UserDataTest, PositiveControlRange) { TEST_F(UserDataTest, PositiveControlRange) {
@@ -553,11 +553,11 @@ TEST_F(ActuatorTest, ReadsByte) {
mj_deleteModel(model); mj_deleteModel(model);
} }
// ------------- test intvelocity parsing --------------------------------------- // ---------------- test actuator parsing --------------------------------------
using IntegratedVelocityTest = MujocoTest; using ActuatorParseTest = MujocoTest;
TEST_F(IntegratedVelocityTest, CheckEquivalence) { TEST_F(ActuatorParseTest, IntvelocityCheckEquivalence) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
@@ -568,14 +568,14 @@ TEST_F(IntegratedVelocityTest, CheckEquivalence) {
</worldbody> </worldbody>
<actuator> <actuator>
<intvelocity joint="hinge" kp="2.5" actrange="-1.57 1.57"/> <intvelocity joint="hinge" kp="2.5" actrange="-1.57 1.57"/>
<general joint="hinge" actlimited="true" actrange="-1.57 1.57" dyntype="integrator" <general joint="hinge" actlimited="true" actrange="-1.57 1.57"
biastype="affine" gainprm="2.5" biasprm="0 -2.5 0"/> dyntype="integrator" biastype="affine" gainprm="2.5" biasprm="0 -2.5 0"/>
</actuator> </actuator>
</mujoco> </mujoco>
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull()); ASSERT_THAT(model, NotNull());
// same actlimited // same actlimited
EXPECT_EQ(model->actuator_actlimited[0], 1); EXPECT_EQ(model->actuator_actlimited[0], 1);
EXPECT_EQ(model->actuator_actlimited[1], 1); EXPECT_EQ(model->actuator_actlimited[1], 1);
@@ -606,7 +606,7 @@ TEST_F(IntegratedVelocityTest, CheckEquivalence) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(IntegratedVelocityTest, CheckDefaultsIfNotSpecified) { TEST_F(ActuatorParseTest, IntvelocityCheckDefaultsIfNotSpecified) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
@@ -622,7 +622,7 @@ TEST_F(IntegratedVelocityTest, CheckDefaultsIfNotSpecified) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull()); ASSERT_THAT(model, NotNull());
// check that by default kp = 1 // check that by default kp = 1
EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 1.0); EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 1.0);
// check that biasprm is (0, -1, 0) // check that biasprm is (0, -1, 0)
@@ -632,7 +632,7 @@ TEST_F(IntegratedVelocityTest, CheckDefaultsIfNotSpecified) {
mj_deleteModel(model); mj_deleteModel(model);
} }
TEST_F(IntegratedVelocityTest, NoActrangeThrowsError) { TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<worldbody> <worldbody>
@@ -652,7 +652,7 @@ TEST_F(IntegratedVelocityTest, NoActrangeThrowsError) {
EXPECT_THAT(error.data(), HasSubstr("invalid activation range for actuator")); EXPECT_THAT(error.data(), HasSubstr("invalid activation range for actuator"));
} }
TEST_F(IntegratedVelocityTest, DefaultsPropagate) { TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) {
static constexpr char xml[] = R"( static constexpr char xml[] = R"(
<mujoco> <mujoco>
<default> <default>
@@ -676,7 +676,7 @@ TEST_F(IntegratedVelocityTest, DefaultsPropagate) {
)"; )";
std::array<char, 1024> error; std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size()); mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, testing::NotNull()); ASSERT_THAT(model, NotNull());
EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 5); EXPECT_DOUBLE_EQ(model->actuator_gainprm[0], 5);
EXPECT_DOUBLE_EQ(model->actuator_gainprm[mjNGAIN], 1); EXPECT_DOUBLE_EQ(model->actuator_gainprm[mjNGAIN], 1);
EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 0], 0); EXPECT_DOUBLE_EQ(model->actuator_actrange[0 + 0], 0);
@@ -686,5 +686,82 @@ TEST_F(IntegratedVelocityTest, DefaultsPropagate) {
mj_deleteModel(model); mj_deleteModel(model);
} }
// ------------- test adhesion parsing -----------------------------------------
TEST_F(ActuatorParseTest, AdhesionDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<adhesion ctrlrange="0 3"/>
</default>
<worldbody>
<body name="sphere">
<geom size="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="sphere"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->actuator_ctrlrange[0], 0);
EXPECT_EQ(model->actuator_ctrlrange[1], 3);
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, ErrorBadAdhesionDefaults) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<adhesion ctrlrange="-1 0"/>
</default>
<worldbody>
<body name="sphere">
<geom size="1"/>
</body>
</worldbody>
<actuator>
<adhesion body="sphere"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("adhesion control range cannot be negative"));
}
// make sure range requirement is not enforced at parse time
TEST_F(ActuatorParseTest, DampersDontRequireRange) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<damper ctrlrange="0 2"/>
</default>
<worldbody>
<body name="sphere">
<joint name="hinge"/>
<geom size="1"/>
</body>
</worldbody>
<actuator>
<damper joint="hinge"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_EQ(model->actuator_ctrlrange[0], 0);
EXPECT_EQ(model->actuator_ctrlrange[1], 2);
mj_deleteModel(model);
}
} // namespace } // namespace
} // namespace mujoco } // namespace mujoco