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

Add actuatorgravcomp joint attribute, to treat gravity compensation forces as applied by actuators, rather than passive buoyancy.

Browse Source 
PiperOrigin-RevId: 620049593
Change-Id: I2c8a9dc152c087b408e4f904838034271a7dd910
This commit is contained in:
Yuval Tassa
2024-03-28 14:00:55 -07:00
committed by Copybara-Service
parent d258d5e152
commit 47ba72ea59
16 changed files with 257 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_forward_test.cc
+74
View File
@@ -765,6 +765,80 @@ TEST_F(ActuatorTest, ActuatorForceClamping) {
mj_deleteModel(model);
}
// Apply gravity compensation via actuators
TEST_F(ActuatorTest, ActuatorGravcomp) {
static constexpr char xml[] = R"(
<mujoco>
<worldbody>
<body gravcomp="1">
<joint name="joint" type="slide" axis="0 0 1"
actuatorfrcrange="-2 2" actuatorgravcomp="true"/>
<geom type="box" size=".05 .05 .05" mass="1"/>
</body>
</worldbody>
<actuator>
<motor name="actuator" joint="joint"/>
</actuator>
<sensor>
<actuatorfrc actuator="actuator"/>
<jointactuatorfrc joint="joint"/>
</sensor>
</mujoco>
)";
mjModel* model = LoadModelFromString(xml);
mjData* data = mj_makeData(model);
mj_forward(model, data);
// expect force clamping as specified in the model
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 2);
// reduce gravity so gravcomp is not clamped
model->opt.gravity[2] = -1;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0);
EXPECT_EQ(data->qfrc_actuator[0], 1);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0);
EXPECT_EQ(data->sensordata[1], 1);
// add control, see that it adds up
data->ctrl[0] = 0.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 0.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 0.5);
EXPECT_EQ(data->sensordata[1], 1.5);
// add larger control, expect clamping
data->ctrl[0] = 1.5;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 2);
EXPECT_EQ(data->qfrc_passive[0], 0);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 2);
// disable actgravcomp, expect gravcomp as a passive force
model->jnt_actgravcomp[0] = 0;
mj_forward(model, data);
EXPECT_EQ(data->actuator_force[0], 1.5);
EXPECT_EQ(data->qfrc_actuator[0], 1.5);
EXPECT_EQ(data->qfrc_passive[0], 1);
EXPECT_EQ(data->sensordata[0], 1.5);
EXPECT_EQ(data->sensordata[1], 1.5);
mj_deleteData(data);
mj_deleteModel(model);
}
// ----------------------- filterexact actuators -------------------------------
using FilterExactTest = MujocoTest;
test/engine/testdata/actuation/refsite.xml
Vendored
+6 -6
View File
@@ -6,7 +6,7 @@
(e.g. 4 consecutive ball joints is not a realistic kinematic design)
-->
<option viscosity="10" integrator="implicit">
<flag gravity="disable" contact="disable"/>
<flag contact="disable"/>
</option>
<statistic meansize=".05"/>
@@ -29,7 +29,7 @@
commands is achievable by individual joint actuators with the specified torque limits.
See https://mujoco.readthedocs.io/en/latest//modeling.html#actuator-force-clamping
-->
<joint stiffness="1e-1" actuatorfrcrange="-1 1"/>
<joint armature="1e-3" damping="1e-3" stiffness="1e-2" actuatorfrcrange="-1 1" actuatorgravcomp="true"/>
<site type="box" size=".012 .012 .012" rgba=".7 .7 .8 1"/>
</default>
@@ -37,19 +37,19 @@
<light pos="0 0 2"/>
<geom type="box" size=".25 .25 .01" pos="0 0 -.01"/>
<site name="reference" pos="0 0 .25"/>
<body name="arm" pos="-.25 .25 0">
<body name="arm" pos="-.25 .25 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 1 0"/>
<geom type="box" size=".01" fromto="0 0 0 0 0 .25"/>
<body pos="0 0 .25">
<body pos="0 0 .25" gravcomp="1">
<joint axis="0 1 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 .25 0 0"/>
<body pos=".25 0 0">
<body pos=".25 0 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<geom type="box" size=".01" fromto="0 0 0 0 -.2 0"/>
<body pos="0 -.2 0">
<body pos="0 -.2 0" gravcomp="1">
<joint axis="1 0 0"/>
<joint axis="0 0 1"/>
<joint axis="0 1 0"/>