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

Add netforce reduction to contact sensor.

Browse Source 
PiperOrigin-RevId: 783020713
Change-Id: I0d11ccc4e1fd16d07e6f116d64a26485346d9cf1
This commit is contained in:
Yuval Tassa
2025-07-14 13:26:49 -07:00
committed by Copybara-Service
parent d0e4771c8c
commit 576a34bcf2
4 changed files with 195 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/engine/engine_sensor_test.cc
+73
View File
@@ -14,6 +14,7 @@
// Tests for engine/engine_sensor.c.
#include <algorithm>
#include <cstddef>
#include <string>
#include <vector>
@@ -863,6 +864,78 @@ TEST_F(SensorTest, ContactSubtree) {
mj_deleteModel(model);
}
TEST_F(SensorTest, ContactNet) {
const string xml_path =
GetTestDataFilePath("engine/testdata/sensor/contact_net.xml");
char error[1024];
mjModel* model = mj_loadXML(xml_path.c_str(), nullptr, error, sizeof(error));
ASSERT_THAT(model, NotNull()) << error;
int b1 = mj_name2id(model, mjOBJ_BODY, "b1");
int b2 = mj_name2id(model, mjOBJ_BODY, "b2");
int nv = model->nv;
mjData* data = mj_makeData(model);
for (mjtCone cone : {mjCONE_PYRAMIDAL, mjCONE_ELLIPTIC}) {
model->opt.cone = cone;
mj_resetData(model, data);
// for each timestep, compare the net force computation to qfrc_constraint
// data->ncon varies in [0, 6]
int nconmax = 0;
while (data->time < 0.2) {
mj_step(model, data);
vector<mjtNum> qfrc_expected = AsVector(data->qfrc_constraint, nv);
// check net force, sensor returns body1 -> body2
vector net12 = GetSensor(model, data, "net12");
EXPECT_EQ(net12.size(), 9);
mjtNum* force = net12.data();
mjtNum* torque = net12.data() + 3;
mjtNum* point = net12.data() + 6;
// apply wrench to b2
vector<mjtNum> qfrc(nv, 0.0);
mj_applyFT(model, data, force, torque, point, b2, qfrc.data());
// apply opposite wrench to b1
mju_scl3(force, force, -1);
mju_scl3(torque, torque, -1);
mj_applyFT(model, data, force, torque, point, b1, qfrc.data());
// compare
EXPECT_THAT(qfrc, Pointwise(DoubleNear(1e-6), qfrc_expected));
// check net force, sensor returns body2 -> body1
vector net21 = GetSensor(model, data, "net21");
EXPECT_EQ(net21.size(), 9);
force = net21.data();
torque = net21.data() + 3;
point = net21.data() + 6;
qfrc.assign(nv, 0.0);
// apply wrench to b1
mj_applyFT(model, data, force, torque, point, b1, qfrc.data());
// apply opposite wrench to b2
mju_scl3(force, force, -1);
mju_scl3(torque, torque, -1);
mj_applyFT(model, data, force, torque, point, b2, qfrc.data());
// compare
EXPECT_THAT(qfrc, Pointwise(DoubleNear(1e-6), qfrc_expected));
nconmax = std::max(nconmax, data->ncon);
}
// at least 5 contacts happened
EXPECT_GT(nconmax, 4);
}
mj_deleteData(data);
mj_deleteModel(model);
}
TEST_F(SensorTest, CameraProjection) {
constexpr char xml[] = R"(
<mujoco>
test/engine/testdata/sensor/contact_net.xml
Vendored
+31
View File
@@ -0,0 +1,31 @@
<mujoco model="contact net">
<statistic meansize="0.05"/>
<default>
<geom condim="6"/>
</default>
<worldbody>
<light pos="0 0 3"/>
<geom name="floor" type="plane" size=".5 1 .01"/>
<body name="b1" pos="0 0 .4" euler="5 5 5">
<freejoint/>
<geom type="box" pos="0 0 .06" size=".15 .15 .03" rgba=".8 .2 .1 .2"/>
<geom type="box" pos="-.05 0 .005" size=".04 .04 .025" rgba=".8 .2 .1 1" euler="1 1 1"/>
<geom size=".05" pos=".1 -.1 .04" rgba=".8 .2 .1 1"/>
</body>
<body name="b2" pos="0 0 .2">
<joint type="ball" springdamper="0.1 1"/>
<geom type="box" size=".2 .2 .05" rgba=".8 .2 .1 1"/>
<geom size=".05" pos=".1 .1 .05" rgba=".8 .2 .1 1"/>
<geom type="box" size=".05 .05 .01" pos=".1 -.1 .06" rgba=".8 .2 .1 1" euler="2 2 2"/>
</body>
</worldbody>
<sensor>
<contact name="net12" body1="b1" body2="b2" data="force torque pos" reduce="netforce"/>
<contact name="net21" body1="b2" body2="b1" data="force torque pos" reduce="netforce"/>
</sensor>
</mujoco>