fix: apply scale and padding to mesh collision bodies by satoimotaro · Pull Request #3 · sbgisen/robot_self_filter

satoimotaro · 2026-08-26T10:09:05Z

Root cause

scale and padding never reached mesh collision bodies.

SelfMask applies them per shape type, but the shapes::MESH case of that switch was a
comment-only stub, and ConvexMesh had nothing to call: no setScale() / setPadding(), no
m_scale / m_padding members. Three further places inside ConvexMesh were placeholders as
well:

containsPoint() — ip = m_meshCenter + (ip - m_meshCenter);, an identity.
updateInternalData() — the bounding box got neither scale nor padding, m_radiusB = m_meshRadiusB
ignored both, and m_scaledVertices[i] = m_vertices[i] copied the vertices unscaled despite the name.
isPointInsidePlanes() — dist > 0.0, with no padding offset.

So padding worked for spheres, boxes and cylinders only. On a URDF whose collisions are all .dae
meshes the filter effectively ran at padding 0 no matter what the config said, dropping only points
strictly inside the convex hull. Returns off thin parts such as legs land just outside it and survived.

Confirmed inert rather than merely weak: padding 0.02 and 0.10 produced identical output
(137.0 / 5.9 / 8.9 points, the same on both).

This is a regression, not a design choice

Before the ROS 2 rewrite, 406eace had ConvexMesh::updateInternalData() call
m_boundingBox.setPadding(m_padding) / setScale(m_scale), compute
m_radiusB = m_meshRadiusB * m_scale + m_padding, scale the vertices by m_scale + m_padding / l,
and subtract m_padding in the plane test. 792e81a ("added launch files and params",
2025-01-16, 852 changed lines in bodies.cpp alone) replaced all of it with the
"Uniform scaling placeholder" stubs this PR removes.

Fixes

ConvexMesh gains m_scale / m_padding and setScale() / setPadding() — the single
scale/padding pair a sphere takes.
The shapes::MESH case in SelfMask calls them.
The four computations are restored to match 406eace: padded bounding box, m_radiusB,
scaled vertices, and dist > m_padding in the plane test.
containsPoint() divides the query point by the scale instead of multiplying. The planes
describe the unscaled hull, so dividing is what makes scale > 1 a larger body; 406eace
multiplied, which shrinks it. This is the one place where the restored code intentionally differs
from the pre-rewrite version.
setScale() falls back to 1.0 on a non-positive value. m_padding / m_scale in the plane test
would otherwise be infinite, report every point as inside, and silently delete the whole cloud.

Verification

Real Unitree Go2, walking rosbag, default_sphere_padding now reaching the 18 mesh links of the
robot description. Points within 3 cm of a leg centre line, per frame:

padding	points/frame	removed
unfiltered	10.72	—
0.02	0.19	98.2 %
0.04	0.01	99.9 %

Obstacle returns in front of the robot over the same bag: 23.2 → 23.5 points/frame, i.e. unchanged.
Nothing real is being eaten by the padding.

Throughput on the same bag at padding 0.04: 14.32 Hz out against a 14.63 Hz input
(859 vs 878 frames over 60 s), so the added work keeps up with the sensor.

Known limitation

scale != 1 is not exercised by the configuration this was measured with (it uses 1.0). The
bounding-box growth is not exact for a mesh whose bounding-box centre differs from its mesh centre,
because the box is scaled about its own centre while containsPoint() measures about the mesh
centre. Padding is unaffected.

🤖 Generated with Claude Code

ConvexMesh ignored both: the shape switch in SelfMask had an empty MESH branch, ConvexMesh had no scale or padding of its own, and updateInternalData copied the vertices through unchanged behind a "uniform scaling placeholder" comment. Only sphere, box and cylinder bodies ever saw the configured values. A URDF that ships .dae collisions therefore ran every link at zero padding, and the mask kept only points strictly inside the hull -- returns off the surface of a thin part such as a leg sit a fraction outside it and survived. Measured on a Go2 walking bag: points within 3 cm of a leg fell from 10.7 to 0.19 per frame once 2 cm of padding actually reached the mesh, and to 0.01 at 4 cm, with the obstacle returns in front of the robot unchanged (23.2 -> 23.5). Padding shifts every face of the convex hull outward, which for unit-length plane normals is a straight comparison against the signed distance. The bounding box grows with it too, since it is the first rejection test in containsPoint and would otherwise drop the padded points before the plane test.

Padding goes through the plane test and the bounding box, so it applies to containsPoint. Scale only reaches the vertices, which feed intersectsRay and the markers, leaving containment unscaled.

Follow-up to the padding fix. setScale existed but only moved the vertices, so containsPoint behaved as if the body were unscaled while intersectsRay and the published markers used the scaled one. Bring the query point back into the unscaled frame instead of growing the hull. Note the direction: the code this replaces multiplied the point by the scale, which shrinks the effective body as the scale grows. Dividing is what makes a scale above 1 mean a larger body, and that is what the measurement shows -- on a Go2 walking bag with zero padding, removal goes 2.7 points per frame at scale 0.5, 17.9 at 1.0 and 40.4 at 1.5. Padding is given in metres, so it divides by the scale to stay in metres once the comparison moves into the unscaled frame. Rename the two members to m_scale and m_padding to match every other body here.

The containment test divides the padding by the scale, so a non-positive scale turns that term into infinity, reports every point as inside and deletes the whole cloud without a word.

h-wata

approveです。直していただく必要があるものはありません。

containsPoint()がscaleで割る変更は正しいことを確認しました。単位立方体でscaleとpaddingを変えて実測し、期待値と一致することを確認しています。

気づいた点を2つ、インラインコメントに書きました。

setPadding()にNaNを渡すガードが無く、1行で塞げます
Known limitationに「ずれる向きは自己点が残る側」と一行加えると親切です

Known limitationについて補足すると、実meshで測った最大のずれはbase.daeの約6cmでした。ただしscaleが1.0なら厳密にゼロになり、現状のconfigはすべてscale 1.0なので影響はありません。

他のopen PRとの関係も共有します。#1 、#2 とはファイルが重複せず、実際にマージして確認したので、この PR は他の2本を待たずに先に取り込めます。

h-wata · 2026-08-27T08:03:36Z

+  void setPadding(double p)
+  {
+    m_padding = p;
+    updateInternalData();
+  }


non-blocking

setScale()は非正値やNaNを1.0にフォールバックしますが、setPadding()にはそのガードがありません。

m_paddingにNaNを渡すと、内外判定がすべての点をinside扱いにしてしまい、点群が丸ごと消えます。

提案: m_padding = std::isfinite(p) ? p : 0.0; の1行で塞げます。

h-wata · 2026-08-27T08:03:36Z

  m_scaledVertices.resize(m_vertices.size());
  for (size_t i=0; i<m_vertices.size(); i++)
  {
-    // Uniform scaling placeholder
-    m_scaledVertices[i] = m_vertices[i];
+    tf2::Vector3 v = m_vertices[i] - m_meshCenter;
+    double norm = v.length();
+    if (norm > 1e-9)
+      m_scaledVertices[i] = m_meshCenter + v * (m_scale + m_padding / norm);
+    else
+      m_scaledVertices[i] = m_vertices[i];
  }


non-blocking

ここで作るm_scaledVerticesは、少し上のintersectsRay()の内外判定に使われます。intersectsRay()が使う面(m_planes)は常に未スケールなので、面と三角形がずれます。

そのぶんレイの当たり判定がわずかに外側まで伸びます。単位立方体ではpad 0.02で約11.5mm外側まで伸びました。

この構造自体は本PR以前からあり、paddingが常に0だったため今まで表面化していませんでした。

satoimotaro added 4 commits August 26, 2026 18:11

Note which of scale and padding reach the containment test

c023f7c

Padding goes through the plane test and the bounding box, so it applies to containsPoint. Scale only reaches the vertices, which feed intersectsRay and the markers, leaving containment unscaled.

Fall back to unscaled rather than dividing the padding by zero

0dbb75d

The containment test divides the padding by the scale, so a non-positive scale turns that term into infinity, reports every point as inside and deletes the whole cloud without a word.

h-wata approved these changes Aug 27, 2026

View reviewed changes

Navigation Menu

Uh oh!

fix: apply scale and padding to mesh collision bodies - #3

fix: apply scale and padding to mesh collision bodies#3
satoimotaro wants to merge 4 commits into
sbgisen:mainfrom
satoimotaro:fix/mesh-scale-padding

satoimotaro commented Aug 26, 2026

Uh oh!

h-wata left a comment •

edited

Loading

Uh oh!

h-wata Aug 27, 2026 •

edited

Loading

Uh oh!

h-wata Aug 27, 2026 •

edited

Loading

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

Uh oh!

Conversation

satoimotaro commented Aug 26, 2026

Root cause

This is a regression, not a design choice

Fixes

Verification

Known limitation

Uh oh!

h-wata left a comment • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

h-wata Aug 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

h-wata Aug 27, 2026 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

h-wata left a comment •

edited

Loading

h-wata Aug 27, 2026 •

edited

Loading

h-wata Aug 27, 2026 •

edited

Loading