diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index 29f3e009..29b90bf2 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -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); diff --git a/src/user/user_objects.cc b/src/user/user_objects.cc index 8922d508..9e26d21c 100644 --- a/src/user/user_objects.cc +++ b/src/user/user_objects.cc @@ -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; diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index ef9a1eac..fdb5282d 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -14,6 +14,7 @@ // Tests for engine/engine_sensor.c. +#include #include #include #include @@ -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 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 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"( diff --git a/test/engine/testdata/sensor/contact_net.xml b/test/engine/testdata/sensor/contact_net.xml new file mode 100644 index 00000000..65c67827 --- /dev/null +++ b/test/engine/testdata/sensor/contact_net.xml @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +