From e23226509c873d3e7dde9c6b3c61801c22ca9a4f Mon Sep 17 00:00:00 2001 From: Alessio Quaglino Date: Thu, 17 Jul 2025 08:13:45 -0700 Subject: [PATCH] Tidy up of touch stress plugin. PiperOrigin-RevId: 784186240 Change-Id: I47298ff2b7676a540781561817670c5f00c1b63a --- plugin/sensor/touch_stress.cc | 26 +++++++------------------- 1 file changed, 7 insertions(+), 19 deletions(-) diff --git a/plugin/sensor/touch_stress.cc b/plugin/sensor/touch_stress.cc index 96cb9237..03502f33 100644 --- a/plugin/sensor/touch_stress.cc +++ b/plugin/sensor/touch_stress.cc @@ -96,14 +96,6 @@ void BinEdges(mjtNum* x_edges, mjtNum* y_edges, int size[2], mjtNum fov[2], mju_scl(y_edges, y_edges, fov[1]*mjPI / 180, size[1] + 1); } -// Permute 3-vector from 0,1,2 to 2,0,1. -static void xyz2zxy(mjtNum* x) { - mjtNum z = x[2]; - x[2] = x[1]; - x[1] = x[0]; - x[0] = z; -} - // Transform spherical (azimuth, elevation, radius) to Cartesian (x,y,z). void SphericalToCartesian(const mjtNum aer[3], mjtNum xyz[3]) { mjtNum a = aer[0], e = aer[1], r = aer[2]; @@ -372,21 +364,17 @@ void TouchStress::Compute(const mjModel* m, mjData* d, int instance) { mju_sub3(vel_rel, vel_sensor+3, vel_other+3); // Get contact force/torque, rotate into node frame. - mjtNum force[3]; - mjtNum tmp_force[3], normal[3]; + mjtNum frc[3]; + mjtNum normal[3]; mjtNum kMaxDepth = 0.05; mjtNum pressure = 1 / (kMaxDepth - depth) - 1 / kMaxDepth; mjc_gradient(m, d, &sensor_sdf, normal, pos); - mju_scl3(tmp_force, normal, pressure); - mju_mulMatTVec3(force, mat, tmp_force); - force[0] = mju_abs(mju_dot3(vel_rel, mat + 0)); - force[1] = mju_abs(mju_dot3(vel_rel, mat + 3)); + mju_scl3(frc, normal, pressure); - // Permute forces from x,y,z to z,x,y (normal, tangent, tangent) - xyz2zxy(force); - forcesT[0*ncon + node] = force[0]; - forcesT[1*ncon + node] = force[1]; - forcesT[2*ncon + node] = force[2]; + // one row of mat^T * force + forcesT[0*ncon + node] = mat[2]*frc[0] + mat[5]*frc[1] + mat[8]*frc[2]; + forcesT[1*ncon + node] = mju_abs(mju_dot3(vel_rel, mat + 0)); + forcesT[2*ncon + node] = mju_abs(mju_dot3(vel_rel, mat + 3)); node++; } }