chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 4 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>