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

Add tendon actuator force limits and tendon actuator force sensor.

Browse Source 
PiperOrigin-RevId: 745096883
Change-Id: Ib9acb727fbbfc6b0b0323ee6a889053a7a878056
This commit is contained in:
Taylor Howell
2025-04-08 05:15:27 -07:00
committed by Copybara-Service
parent 16e49f2761
commit 96dda6ea75
26 changed files with 464 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_forward_test.cc
+54
View File
@@ -46,6 +46,8 @@ static const char* const kDampedActuatorsPath =
"engine/testdata/derivative/damped_actuators.xml";
static const char* const kJointForceClamp =
"engine/testdata/actuation/joint_force_clamp.xml";
static const char* const kTendonForceClamp =
"engine/testdata/actuation/tendon_force_clamp.xml";
using ::testing::Pointwise;
using ::testing::DoubleNear;
@@ -1393,5 +1395,57 @@ TEST_F(ActuatorTest, DisableActuatorOutOfRange) {
mj_deleteModel(model);
}
TEST_F(ActuatorTest, TendonActuatorForceRange) {
const std::string xml_path = GetTestDataFilePath(kTendonForceClamp);
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, nullptr, 0);
mjData* data = mj_makeData(model);
EXPECT_EQ(model->tendon_actfrclimited[0], 0);
EXPECT_EQ(model->tendon_actfrcrange[0], 0);
EXPECT_EQ(model->tendon_actfrcrange[1], 0);
EXPECT_EQ(model->tendon_actfrclimited[1], 1);
EXPECT_EQ(model->tendon_actfrcrange[2], -1);
EXPECT_EQ(model->tendon_actfrcrange[3], 1);
EXPECT_EQ(model->tendon_actfrclimited[2], 1);
EXPECT_EQ(model->tendon_actfrcrange[4], -10);
EXPECT_EQ(model->tendon_actfrcrange[5], 10);
EXPECT_EQ(model->tendon_actfrclimited[3], 1);
EXPECT_EQ(model->tendon_actfrcrange[6], 0);
EXPECT_EQ(model->tendon_actfrcrange[7], 1);
data->ctrl[0] = 1;
data->ctrl[1] = 1;
data->ctrl[2] = 1;
data->ctrl[3] = -1;
data->ctrl[4] = 1;
data->ctrl[5] = -20;
data->ctrl[6] = 5;
data->ctrl[7] = -5;
mj_forward(model, data);
EXPECT_NEAR(data->actuator_force[0], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[1], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[2], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[3], -1, 1e-6);
EXPECT_NEAR(data->actuator_force[4], 1, 1e-6);
EXPECT_NEAR(data->actuator_force[5], -10, 1e-6);
EXPECT_NEAR(data->actuator_force[6], 5, 1e-6);
EXPECT_NEAR(data->actuator_force[7], -5, 1e-6);
EXPECT_EQ(data->sensordata[0], 3);
EXPECT_EQ(data->sensordata[1], 0);
EXPECT_EQ(data->sensordata[2], -10);
EXPECT_EQ(data->sensordata[3], 0);
mj_deleteData(data);
mj_deleteModel(model);
}
} // namespace
} // namespace mujoco
test/engine/testdata/actuation/tendon_force_clamp.xml
Vendored
+55
View File
@@ -0,0 +1,55 @@
<mujoco model="fixed_site">
<worldbody>
<body>
<joint name="joint0" type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
<site name="site0" pos="0.25 0 0.1" size="0.025"/>
<body pos="0.5 0 0">
<joint name="joint1" type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
<site name="site1" pos="0.25 0 0.1" size="0.025"/>
<body pos="0.5 0 0">
<joint name="joint2" type="hinge" axis="0 1 0"/>
<geom type="capsule" size="0.05 0.5" fromto="0 0 0 0.5 0 0"/>
<site name="site2" pos="0.25 0 0.1" size="0.025"/>
</body>
</body>
</body>
</worldbody>
<tendon>
<spatial name="spatial0" width="0.0125">
<site site="site0"/>
<site site="site1"/>
<site site="site2"/>
</spatial>
<spatial name="spatial1" width="0.0125" actuatorfrclimited="true" actuatorfrcrange="-1 1">
<site site="site1"/>
<site site="site2"/>
</spatial>
<fixed name="fixed0" actuatorfrclimited="true" actuatorfrcrange="-10 10">
<joint joint="joint0" coef=".1"/>
<joint joint="joint1" coef=".2"/>
<joint joint="joint2" coef=".3"/>
</fixed>
<fixed name="fixed1" actuatorfrclimited="true" actuatorfrcrange="0 1">
<joint joint="joint0" coef=".1"/>
<joint joint="joint2" coef=".3"/>
</fixed>
</tendon>
<actuator>
<motor tendon="spatial0"/>
<motor tendon="spatial0"/>
<motor tendon="spatial0"/>
<motor tendon="spatial1"/>
<motor tendon="spatial1"/>
<motor tendon="fixed0"/>
<motor tendon="fixed1"/>
<motor tendon="fixed1"/>
</actuator>
<sensor>
<tendonactuatorfrc tendon="spatial0"/>
<tendonactuatorfrc tendon="spatial1"/>
<tendonactuatorfrc tendon="fixed0"/>
<tendonactuatorfrc tendon="fixed1"/>
</sensor>
</mujoco>
test/user/user_objects_test.cc
+45
View File
@@ -2070,6 +2070,51 @@ TEST_F(TendonTest, SiteBetweenPulleyNotAllowed) {
EXPECT_THAT(error.data(), HasSubstr("line 9"));
}
TEST_F(TendonTest, ActuatorForceRangeNotAllowed) {
std::string xml = R"(
<mujoco>
<worldbody>
<site name="site0"/>
<site name="site1"/>
</worldbody>
<tendon>
<spatial name="spatial" actuatorfrclimited="true" actuatorfrcrange="{}">
<site site="site0"/>
<site site="site1"/>
</spatial>
</tendon>
<actuator>
<motor tendon="spatial"/>
</actuator>
</mujoco>
)";
std::array<char, 1024> error;
std::string str_replace = "{}";
size_t rng_ind = xml.find(str_replace);
std::string xml0 = xml;
std::string range0 = "-2 -1";
xml0.replace(rng_ind, str_replace.length(), range0);
mjModel* m0 = LoadModelFromString(xml0.c_str(), error.data(), error.size());
EXPECT_THAT(m0, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
std::string xml1 = xml;
std::string range1 = "1 2";
xml1.replace(rng_ind, str_replace.length(), range1);
mjModel* m1 = LoadModelFromString(xml1.c_str(), error.data(), error.size());
EXPECT_THAT(m1, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
std::string xml2 = xml;
std::string range2 = "1 0";
xml2.replace(rng_ind, str_replace.length(), range2);
mjModel* m2 = LoadModelFromString(xml2.c_str(), error.data(), error.size());
EXPECT_THAT(m2, IsNull());
EXPECT_THAT(error.data(), HasSubstr("invalid actuatorfrcrange in tendon"));
}
// ------------- tests for tendon springrange ----------------------------------
using SpringrangeTest = MujocoTest;