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

Skip tangential components in tactile sensor if mesh has only normals.

Browse Source 
PiperOrigin-RevId: 879036130
Change-Id: Ic4184dd535700c5099768dabf5009ba96b218542
This commit is contained in:
Alessio Quaglino
2026-03-05 06:35:02 -08:00
committed by Copybara-Service
parent fce504beea
commit 8dc5cc4c70
2 changed files with 91 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/engine/engine_sensor.c
+29 -10
View File
@@ -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++;
}
}
test/engine/engine_sensor_test.cc
+62
View File
@@ -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"(
<mujoco>
<option>
<flag multiccd="enable"/>
</option>
<asset>
<mesh name="sensor_mesh" builtin="sphere" params="0"/>
</asset>
<worldbody>
<body pos="0 0 1">
<freejoint/>
<geom name="sensor_geom" type="mesh" mesh="sensor_mesh"/>
</body>
<body>
<geom type="box" size=".7 .7 .3"/>
</body>
</worldbody>
<sensor>
<tactile geom="sensor_geom" mesh="sensor_mesh"/>
</sensor>
</mujoco>
)";
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