diff --git a/src/engine/engine_sensor.c b/src/engine/engine_sensor.c index b011cba6..eb93fa14 100644 --- a/src/engine/engine_sensor.c +++ b/src/engine/engine_sensor.c @@ -1157,6 +1157,11 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens int node = 0; float* mesh_vert = m->mesh_vert + 3*m->mesh_vertadr[mesh_id]; float* mesh_normal = m->mesh_normal + 3*m->mesh_normaladr[mesh_id]; + + // check if mesh has frame or normal + int has_frame = (m->mesh_normalnum[mesh_id] == 3 * m->mesh_vertnum[mesh_id]); + int normal_stride = has_frame ? 9 : 3; + for (int j = 0; j < ncon; j++) { // position in site frame mjtNum pos[3] = {mesh_vert[3*j + 0], mesh_vert[3*j + 1], mesh_vert[3*j + 2]}; @@ -1196,23 +1201,37 @@ static void mj_computeSensorAcc(const mjModel* m, mjData* d, int i, mjtNum* sens d->subtree_com + 3 * m->body_rootid[body], NULL); mju_sub3(vel_rel, vel_sensor+3, vel_other+3); - mjtNum normal[3] = {mesh_normal[9*j + 0], mesh_normal[9*j + 1], mesh_normal[9*j + 2]}; - mjtNum tang1[3] = {mesh_normal[9*j + 3], mesh_normal[9*j + 4], mesh_normal[9*j + 5]}; - mjtNum tang2[3] = {mesh_normal[9*j + 6], mesh_normal[9*j + 7], mesh_normal[9*j + 8]}; - - // get contact force/torque, rotate into node frame + // get normal + mjtNum normal[3] = {mesh_normal[normal_stride*j + 0], + mesh_normal[normal_stride*j + 1], + mesh_normal[normal_stride*j + 2]}; mju_rotVecQuat(normal, normal, m->mesh_quat + 4 * mesh_id); - mju_rotVecQuat(tang1, tang1, m->mesh_quat + 4 * mesh_id); - mju_rotVecQuat(tang2, tang2, m->mesh_quat + 4 * mesh_id); + + // get contact force/torque mjtNum force[3]; mjtNum kMaxDepth = 0.05; mjtNum pressure = depth / mju_max(kMaxDepth - depth, mjMINVAL); mju_scl3(force, normal, pressure); - // one row of mat^T * force + // normal force forcesT[0*ncon + node] = mju_dot3(force, normal); - forcesT[1*ncon + node] = mju_abs(mju_dot3(vel_rel, tang1)); - forcesT[2*ncon + node] = mju_abs(mju_dot3(vel_rel, tang2)); + + // tangent forces (require tangent frame) + if (has_frame) { + mjtNum tang1[3] = {mesh_normal[normal_stride*j + 3], + mesh_normal[normal_stride*j + 4], + mesh_normal[normal_stride*j + 5]}; + mjtNum tang2[3] = {mesh_normal[normal_stride*j + 6], + mesh_normal[normal_stride*j + 7], + mesh_normal[normal_stride*j + 8]}; + mju_rotVecQuat(tang1, tang1, m->mesh_quat + 4 * mesh_id); + mju_rotVecQuat(tang2, tang2, m->mesh_quat + 4 * mesh_id); + forcesT[1*ncon + node] = mju_abs(mju_dot3(vel_rel, tang1)); + forcesT[2*ncon + node] = mju_abs(mju_dot3(vel_rel, tang2)); + } else { + forcesT[1*ncon + node] = 0; + forcesT[2*ncon + node] = 0; + } node++; } } diff --git a/test/engine/engine_sensor_test.cc b/test/engine/engine_sensor_test.cc index 135789c2..cd7c3f33 100644 --- a/test/engine/engine_sensor_test.cc +++ b/test/engine/engine_sensor_test.cc @@ -1626,5 +1626,67 @@ TEST_F(SensorTest, AccelerometerAfterStep1) { mj_deleteModel(model); } +// Test tactile sensor reads non-zero values when contacts occur +TEST_F(SensorTest, TactileSkipTangents) { + constexpr char xml[] = R"( + + + + + + + + + + + + + + + + + + + )"; + char error[1024]; + mjModel* model = LoadModelFromString(xml, error, sizeof(error)); + ASSERT_THAT(model, NotNull()) << error; + ASSERT_GT(model->nsensordata, 0) << "No sensor data allocated"; + mjData* data = mj_makeData(model); + + // Use mj_forward to compute collisions and sensors at t=0 + mj_forward(model, data); + + // Verify initial state + EXPECT_EQ(data->time, 0.0); + EXPECT_GT(data->ncon, 0) << "No contacts generated"; + + // Tactile sensor layout: [normal_forces..., tang1_forces..., tang2_forces...] + int ntaxel = model->nsensordata / 3; + ASSERT_EQ(model->nsensordata % 3, 0) << "Sensor dim should be divisible by 3"; + + // No tangents, so tangent components should be zero + for (int i = ntaxel; i < model->nsensordata; i++) { + EXPECT_EQ(data->sensordata[i], 0.0) + << "Tangent component at index " << i << " should be 0"; + } + + // Normal force components: verify count, sign, and magnitude ~-0.8 + int nonzero_count = 0; + for (int i = 0; i < ntaxel; i++) { + if (data->sensordata[i] != 0) { + nonzero_count++; + EXPECT_NEAR(data->sensordata[i], -0.8, 0.1) + << "Normal force at taxel " << i; + } + } + EXPECT_EQ(nonzero_count, 2) << "Expected 2 taxels in contact"; + + mj_deleteData(data); + mj_deleteModel(model); +} + } // namespace } // namespace mujoco