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

Add kv damping attribute to position and intvelocity actuator shortcuts.

Browse Source 
PiperOrigin-RevId: 588377097
Change-Id: I8ded2ff982e52ed8673019979e29b706ef65c743
This commit is contained in:
Yuval Tassa
2023-12-06 04:27:34 -08:00
committed by Copybara-Service
parent b3ccf67ebf
commit 762371c3e4
8 changed files with 250 additions and 142 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/xml/xml_native_reader_test.cc
+181 -87
View File
@@ -531,92 +531,6 @@ TEST_F(XMLReaderTest, ReadShellParameter) {
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, ReadsDamper) {
static constexpr char xml[] = R"(
<mujoco>
<compiler autolimits="true"/>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" ctrlrange="0 1"/>
<general joint="jnt" ctrllimited="true" ctrlrange="0 1" gaintype="affine" biastype="none"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_THAT(model->actuator_gaintype[0], Eq(mjGAIN_AFFINE));
EXPECT_THAT(model->actuator_gaintype[1], Eq(mjGAIN_AFFINE));
EXPECT_THAT(model->actuator_biastype[0], Eq(mjBIAS_NONE));
EXPECT_THAT(model->actuator_biastype[1], Eq(mjBIAS_NONE));
mj_deleteModel(model);
}
TEST_F(XMLReaderTest, RequiresPoisitiveDamping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="-1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("damping coefficient cannot be negative"));
}
TEST_F(XMLReaderTest, RequiresControlRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10" limited="true"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid control range"));
}
TEST_F(XMLReaderTest, PositiveControlRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1" ctrlrange="-1 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("control range cannot be negative"));
}
TEST_F(XMLReaderTest, ReadsSkinGroups) {
static constexpr char xml[] = R"(
@@ -779,6 +693,186 @@ TEST_F(ActuatorTest, ReadsByte) {
using ActuatorParseTest = MujocoTest;
TEST_F(ActuatorParseTest, ReadsDamper) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" ctrlrange="0 1"/>
<general joint="jnt" ctrllimited="true" ctrlrange="0 1" gaintype="affine" biastype="none"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_THAT(model->actuator_gaintype[0], Eq(mjGAIN_AFFINE));
EXPECT_THAT(model->actuator_gaintype[1], Eq(mjGAIN_AFFINE));
EXPECT_THAT(model->actuator_biastype[0], Eq(mjBIAS_NONE));
EXPECT_THAT(model->actuator_biastype[1], Eq(mjBIAS_NONE));
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, DamperRequiresPositiveDamping) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="-1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(),
HasSubstr("damping coefficient cannot be negative"));
}
TEST_F(ActuatorParseTest, DamperRequiresControlRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10" limited="true"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid control range"));
}
TEST_F(ActuatorParseTest, DamperPositiveControlRange) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0" range="-10 10"/>
</body>
</worldbody>
<actuator>
<damper joint="jnt" kv="1" ctrlrange="-1 0"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("control range cannot be negative"));
}
TEST_F(ActuatorParseTest, ReadsPositionIntvelKv) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" kv="2"/>
<intvelocity joint="jnt" actrange="-1 1" kv="3"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull());
EXPECT_THAT(model->actuator_biasprm[0*mjNBIAS + 2], Eq(-2.0));
EXPECT_THAT(model->actuator_biasprm[1*mjNBIAS + 2], Eq(-3.0));
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, RequirePositiveKv) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" kv="-2"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("kv cannot be negative"));
}
TEST_F(ActuatorParseTest, PositionIntvelocityVelocityDefaultsPropagate) {
static constexpr char xml[] = R"(
<mujoco>
<default>
<default class="position">
<position kp="3" kv="4"/>
</default>
<default class="intvelocity">
<intvelocity kp="5" kv="6" actrange="-1 1"/>
</default>
<default class="velocity">
<velocity kv="7"/>
</default>
</default>
<worldbody>
<body>
<geom size="1"/>
<joint name="jnt" type="slide" axis="1 0 0"/>
</body>
</worldbody>
<actuator>
<position joint="jnt" class="position"/>
<intvelocity joint="jnt" class="intvelocity"/>
<velocity joint="jnt" class="velocity"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, NotNull()) << error.data();
EXPECT_EQ(model->actuator_gainprm[0*mjNGAIN + 0], 3.0);
EXPECT_EQ(model->actuator_biasprm[0*mjNBIAS + 1], -3.0);
EXPECT_EQ(model->actuator_biasprm[0*mjNBIAS + 2], -4.0);
EXPECT_EQ(model->actuator_gainprm[1*mjNGAIN + 0], 5.0);
EXPECT_EQ(model->actuator_biasprm[1*mjNBIAS + 1], -5.0);
EXPECT_EQ(model->actuator_biasprm[1*mjNBIAS + 2], -6.0);
EXPECT_EQ(model->actuator_gainprm[2*mjNGAIN + 0], 7.0);
EXPECT_EQ(model->actuator_biasprm[2*mjNBIAS + 1], 0.0);
EXPECT_EQ(model->actuator_biasprm[2*mjNBIAS + 2], -7.0);
for (int i = 0; i < model->nu; i++) {
for (int j = 3; j < mjNBIAS; j++) {
EXPECT_EQ(model->actuator_biasprm[i*mjNBIAS + j], 0.0);
}
for (int j = 3; j < mjNGAIN; j++) {
EXPECT_EQ(model->actuator_gainprm[i*mjNGAIN + j], 0.0);
}
}
mj_deleteModel(model);
}
TEST_F(ActuatorParseTest, IntvelocityCheckEquivalence) {
static constexpr char xml[] = R"(
<mujoco>
@@ -871,7 +965,7 @@ TEST_F(ActuatorParseTest, IntvelocityNoActrangeThrowsError) {
std::array<char, 1024> error;
mjModel* model = LoadModelFromString(xml, error.data(), error.size());
ASSERT_THAT(model, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid activation range for actuator"));
EXPECT_THAT(error.data(), HasSubstr("invalid actrange for actuator"));
}
TEST_F(ActuatorParseTest, IntvelocityDefaultsPropagate) {