chenlin/Mujoco_WASM
1
0
Fork 0
Code Issues Pull Requests Actions 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
src/engine/engine_sensor.c
+91 -11
View File
@@ -351,6 +351,35 @@ static void copySensorData(const mjModel* m, const mjData* d,
}
}
// compute total wrench about one point, in the global frame
static void total_wrench(mjtNum force[3], mjtNum torque[3], const mjtNum point[3], int n,
const mjtNum *wrench, const mjtNum *pos, const mjtNum *frame) {
mju_zero3(force);
mju_zero3(torque);
for (int j = 0; j < n; ++j) {
// rotate force, torque from contact frame to global frame
mjtNum force_j[3], torque_j[3];
mju_mulMatTVec3(force_j, frame + 9*j, wrench + 6*j);
mju_mulMatTVec3(torque_j, frame + 9*j, wrench + 6*j + 3);
// add to total force, torque
mju_addTo3(force, force_j);
mju_addTo3(torque, torque_j);
// add induced moment: torque += (pos - point) x force
mjtNum diff[3];
mju_sub3(diff, pos + 3*j, point);
mjtNum induced_torque[3];
mju_cross(induced_torque, diff, force_j);
mju_addTo3(torque, induced_torque);
}
}
//-------------------------------- sensor ----------------------------------------------------------
// position-dependent sensors
@@ -929,7 +958,15 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
case mjSENS_CONTACT: // contact
{
// prepare sizes and indices, check consistency
// local reduce enum for readability
enum {
REDUCE_NONE = 0,
REDUCE_MINDIST = 1,
REDUCE_MAXFORCE = 2,
REDUCE_NETFORCE = 3,
};
// prepare sizes and indices
int dataspec = m->sensor_intprm[i*mjNSENS];
int size = mju_condataSize(dataspec); // size of each slot
int dim = m->sensor_dim[i]; // total sensor array dimension
@@ -967,14 +1004,12 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
match[nmatch].flip = match_j < 0;
// save sorting criterion, if required
if (reduce) {
if (reduce == 1) {
match[nmatch].criterion = d->contact[j].dist;
} else {
mjtNum forcetorque[6];
mj_contactForce(m, d, j, forcetorque);
match[nmatch].criterion = -mju_dot3(forcetorque, forcetorque);
}
if (reduce == REDUCE_MINDIST) {
match[nmatch].criterion = d->contact[j].dist;
} else if (reduce == REDUCE_MAXFORCE) {
mjtNum forcetorque[6];
mj_contactForce(m, d, j, forcetorque);
match[nmatch].criterion = -mju_dot3(forcetorque, forcetorque);
}
// increment number of matching contacts
@@ -984,12 +1019,57 @@ void mj_sensorAcc(const mjModel* m, mjData* d) {
// number of slots to be filled
int nslot = mjMIN(num, nmatch);
// partial sort to get bottom nslot contacts given reduction criterion
if (reduce) {
// partial sort to get bottom nslot contacts if sorted reduction
if (reduce == REDUCE_MINDIST || reduce == REDUCE_MAXFORCE) {
ContactInfo *heap = mjSTACKALLOC(d, nslot, ContactInfo);
ContactSelect(match, heap, nmatch, nslot, NULL);
}
// netforce reduction
else if (reduce == REDUCE_NETFORCE) {
mjtNum *wrench = mjSTACKALLOC(d, nmatch * 6, mjtNum);
mjtNum *pos = mjSTACKALLOC(d, nmatch * 3, mjtNum);
mjtNum *frame = mjSTACKALLOC(d, nmatch * 9, mjtNum);
// precompute wrenches, positions, and frames, maybe flip wrench
for (int j=0; j < nmatch; j++) {
int conid = match[j].id;
mj_contactForce(m, d, conid, wrench + 6*j);
mju_copy3(pos + 3*j, d->contact[conid].pos);
mju_copy(frame + 9*j, d->contact[conid].frame, 9);
if (match[j].flip) {
mju_scl(wrench + 6*j , wrench + 6*j, -1, 6);
}
}
// compute point: force-weighted centroid of contact positions
mjtNum point[3] = {0};
mjtNum total_force = 0;
for (int j=0; j < nmatch; j++) {
mjtNum weight = mju_norm3(wrench + 6*j);
mju_addToScl3(point, pos + 3*j, weight);
total_force += weight;
}
mju_scl3(point, point, 1.0 / mjMAX(total_force, mjMINVAL));
// compute total wrench about point, in the global frame
mjtNum force[3], torque[3];
total_wrench(force, torque, point, nmatch, wrench, pos, frame);
// write data to slot 0
if (data[mjCONDATA_FOUND]) *data[mjCONDATA_FOUND] = nmatch;
if (data[mjCONDATA_FORCE]) mju_copy3(data[mjCONDATA_FORCE], force);
if (data[mjCONDATA_TORQUE]) mju_copy3(data[mjCONDATA_TORQUE], torque);
if (data[mjCONDATA_DIST]) *data[mjCONDATA_DIST] = 0;
if (data[mjCONDATA_POS]) mju_copy3(data[mjCONDATA_POS], point);
if (data[mjCONDATA_NORMAL]) data[mjCONDATA_NORMAL][0] = 1;
if (data[mjCONDATA_TANGENT]) data[mjCONDATA_TANGENT][1] = 1;
// done with this sensor
mj_freeStack(d);
break;
}
// copy data into slots, increment pointers
for (int j=0; j < nslot; j++) {
copySensorData(m, d, data, match[j].id, match[j].flip, nmatch);
src/user/user_objects.cc
-6
View File
@@ -7089,12 +7089,6 @@ void mjCSensor::Compile(void) {
"expected one of {0, 1, 2, 3}", "", intprm[1]);
}
// netforce not yet implemented
if (intprm[1] == 3) {
throw mjCError(this, "netforce reduction is not yet implemented\n"
"please contact the developers if you need this feature");
}
needstage = mjSTAGE_ACC;
datatype = mjDATATYPE_REAL;
break;
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>