From 9ff6d29c86fd22224c1fe30b6d08c1fc58affdb6 Mon Sep 17 00:00:00 2001 From: satoimotaro Date: Wed, 19 Aug 2026 15:58:04 +0900 Subject: [PATCH 1/4] Apply scale and padding to mesh collision bodies 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. --- include/robot_self_filter/bodies.h | 16 ++++++++++++++++ include/robot_self_filter/self_mask.h | 20 +++++++------------- src/bodies.cpp | 18 +++++++++++++----- 3 files changed, 36 insertions(+), 18 deletions(-) diff --git a/include/robot_self_filter/bodies.h b/include/robot_self_filter/bodies.h index 8f15eea..b399e70 100644 --- a/include/robot_self_filter/bodies.h +++ b/include/robot_self_filter/bodies.h @@ -253,6 +253,20 @@ class ConvexMesh : public Body const std::vector &getTriangles() const { return m_triangles; } const std::vector &getScaledVertices() const { return m_scaledVertices; } + // A mesh takes one scale and one padding, like a sphere does. Without these the + // configured padding is silently ignored for every link whose collision geometry + // is a mesh, which for a URDF shipping .dae collisions is all of them. + void setScale(double s) + { + m_meshScale = s; + updateInternalData(); + } + void setPadding(double p) + { + m_meshPadding = p; + updateInternalData(); + } + protected: void useDimensions(const shapes::Shape *shape) override; void updateInternalData() override; @@ -265,6 +279,8 @@ class ConvexMesh : public Body std::vector m_scaledVertices; std::vector m_triangles; tf2::Transform m_iPose; + double m_meshScale = 1.0; + double m_meshPadding = 0.0; tf2::Vector3 m_center; tf2::Vector3 m_meshCenter; diff --git a/include/robot_self_filter/self_mask.h b/include/robot_self_filter/self_mask.h index 668425f..fc6d8c8 100644 --- a/include/robot_self_filter/self_mask.h +++ b/include/robot_self_filter/self_mask.h @@ -445,19 +445,13 @@ class SelfMask } case shapes::MESH: { - // For a mesh, you might do uniform scale/padding - // but there's no single "setScale" in the base class. - // In the improved code we do it similarly to a sphere: single scale/padding - auto mesh_body = dynamic_cast(sl.body); - // Possibly store your scale/padding if you want a custom approach - // For now, no direct function calls for multi-scale, so do uniform or skip - // You might implement your own approach. For demonstration: - // We'll ignore multi-dim box/cylinder arrays for the mesh - // because it's not natively supported. - // That means we'd do uniform scale/padding in `updateInternalData()`, - // if implemented in ConvexMesh. - // -> No direct calls needed here. - // (Or you could design a custom method to do it.) + // A mesh takes one scale and one padding, the same way a sphere does. + auto *mesh_body = dynamic_cast(sl.body); + if (mesh_body) + { + mesh_body->setScale(linfo.scale); + mesh_body->setPadding(linfo.padding); + } break; } default: diff --git a/src/bodies.cpp b/src/bodies.cpp index 330fe67..61756d8 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -593,17 +593,23 @@ void ConvexMesh::updateInternalData() tf2::Transform pose = m_pose; pose.setOrigin(m_pose * m_boxOffset); m_boundingBox.setPose(pose); - // Could apply uniform scale/padding if desired, left as-is for example + // The bounding box is the first rejection test in containsPoint, so it has to + // grow with the padding or a padded point never reaches the plane test. + m_boundingBox.setPadding(m_meshPadding, m_meshPadding, m_meshPadding); m_iPose = m_pose.inverse(); m_center = m_pose * m_meshCenter; - m_radiusB = m_meshRadiusB; + m_radiusB = m_meshRadiusB * m_meshScale + m_meshPadding; m_radiusBSqr = m_radiusB * m_radiusB; m_scaledVertices.resize(m_vertices.size()); for (size_t i=0; i 1e-9) + m_scaledVertices[i] = m_meshCenter + v * (m_meshScale + m_meshPadding / norm); + else + m_scaledVertices[i] = m_vertices[i]; } } bool ConvexMesh::isPointInsidePlanes(const tf2::Vector3 &point) const @@ -611,7 +617,9 @@ bool ConvexMesh::isPointInsidePlanes(const tf2::Vector3 &point) const for (auto &plane : m_planes) { double dist = plane(0)*point.x() + plane(1)*point.y() + plane(2)*point.z() + plane(3); - if (dist > 0.0) return false; + // Plane normals are unit length, so dist is a signed distance in metres and the + // padding shifts every face of the hull outward by that much. + if (dist > m_meshPadding) return false; } return true; } From c023f7c6835bc028ec112cf7ed27e174ba4cfe6e Mon Sep 17 00:00:00 2001 From: satoimotaro Date: Wed, 26 Aug 2026 16:57:10 +0900 Subject: [PATCH 2/4] Note which of scale and padding reach the containment 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. --- include/robot_self_filter/bodies.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/include/robot_self_filter/bodies.h b/include/robot_self_filter/bodies.h index b399e70..17b36a9 100644 --- a/include/robot_self_filter/bodies.h +++ b/include/robot_self_filter/bodies.h @@ -256,6 +256,10 @@ class ConvexMesh : public Body // A mesh takes one scale and one padding, like a sphere does. Without these the // configured padding is silently ignored for every link whose collision geometry // is a mesh, which for a URDF shipping .dae collisions is all of them. + // + // Padding reaches containsPoint through the plane test and the bounding box. + // Scale only reaches the vertices, which feed intersectsRay and the published + // markers -- containment ignores it, so keep scale at 1 if it has to be exact. void setScale(double s) { m_meshScale = s; From a7b37967638d0311122b231fd3fa6ad70d224601 Mon Sep 17 00:00:00 2001 From: satoimotaro Date: Wed, 26 Aug 2026 18:03:35 +0900 Subject: [PATCH 3/4] Make scale reach the mesh containment test as well 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. --- include/robot_self_filter/bodies.h | 16 +++++----------- src/bodies.cpp | 20 +++++++++++++------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/include/robot_self_filter/bodies.h b/include/robot_self_filter/bodies.h index 17b36a9..37aaabf 100644 --- a/include/robot_self_filter/bodies.h +++ b/include/robot_self_filter/bodies.h @@ -253,21 +253,15 @@ class ConvexMesh : public Body const std::vector &getTriangles() const { return m_triangles; } const std::vector &getScaledVertices() const { return m_scaledVertices; } - // A mesh takes one scale and one padding, like a sphere does. Without these the - // configured padding is silently ignored for every link whose collision geometry - // is a mesh, which for a URDF shipping .dae collisions is all of them. - // - // Padding reaches containsPoint through the plane test and the bounding box. - // Scale only reaches the vertices, which feed intersectsRay and the published - // markers -- containment ignores it, so keep scale at 1 if it has to be exact. + // A mesh takes one scale and one padding, the same way a sphere does. void setScale(double s) { - m_meshScale = s; + m_scale = s; updateInternalData(); } void setPadding(double p) { - m_meshPadding = p; + m_padding = p; updateInternalData(); } @@ -283,8 +277,8 @@ class ConvexMesh : public Body std::vector m_scaledVertices; std::vector m_triangles; tf2::Transform m_iPose; - double m_meshScale = 1.0; - double m_meshPadding = 0.0; + double m_scale = 1.0; + double m_padding = 0.0; tf2::Vector3 m_center; tf2::Vector3 m_meshCenter; diff --git a/src/bodies.cpp b/src/bodies.cpp index 61756d8..8ef831c 100644 --- a/src/bodies.cpp +++ b/src/bodies.cpp @@ -408,7 +408,11 @@ bool ConvexMesh::containsPoint(const tf2::Vector3 &p, bool) const { if (!m_boundingBox.containsPoint(p)) return false; tf2::Vector3 ip = m_iPose * p; - ip = m_meshCenter + (ip - m_meshCenter); // uniform scale/padding can be applied if desired + // The planes describe the unscaled hull, so bring the query point back into that + // frame rather than growing the hull. Dividing is what makes a scale above 1 a + // larger body; the code this replaced multiplied, which shrank it instead. + if (m_scale != 1.0) + ip = m_meshCenter + (ip - m_meshCenter) / m_scale; return isPointInsidePlanes(ip); } bool ConvexMesh::intersectsRay(const tf2::Vector3 &origin, @@ -595,10 +599,11 @@ void ConvexMesh::updateInternalData() m_boundingBox.setPose(pose); // The bounding box is the first rejection test in containsPoint, so it has to // grow with the padding or a padded point never reaches the plane test. - m_boundingBox.setPadding(m_meshPadding, m_meshPadding, m_meshPadding); + m_boundingBox.setScale(m_scale, m_scale, m_scale); + m_boundingBox.setPadding(m_padding, m_padding, m_padding); m_iPose = m_pose.inverse(); m_center = m_pose * m_meshCenter; - m_radiusB = m_meshRadiusB * m_meshScale + m_meshPadding; + m_radiusB = m_meshRadiusB * m_scale + m_padding; m_radiusBSqr = m_radiusB * m_radiusB; m_scaledVertices.resize(m_vertices.size()); @@ -607,7 +612,7 @@ void ConvexMesh::updateInternalData() tf2::Vector3 v = m_vertices[i] - m_meshCenter; double norm = v.length(); if (norm > 1e-9) - m_scaledVertices[i] = m_meshCenter + v * (m_meshScale + m_meshPadding / norm); + m_scaledVertices[i] = m_meshCenter + v * (m_scale + m_padding / norm); else m_scaledVertices[i] = m_vertices[i]; } @@ -617,9 +622,10 @@ bool ConvexMesh::isPointInsidePlanes(const tf2::Vector3 &point) const for (auto &plane : m_planes) { double dist = plane(0)*point.x() + plane(1)*point.y() + plane(2)*point.z() + plane(3); - // Plane normals are unit length, so dist is a signed distance in metres and the - // padding shifts every face of the hull outward by that much. - if (dist > m_meshPadding) return false; + // Plane normals are unit length, so dist is a signed distance, and the padding + // shifts every face of the hull outward by that much. containsPoint measures in + // the unscaled frame, so a padding given in metres divides by the scale here. + if (dist > m_padding / m_scale) return false; } return true; } From 0dbb75deffe6c103921acde67a214f481bd90ffe Mon Sep 17 00:00:00 2001 From: satoimotaro Date: Wed, 26 Aug 2026 18:08:44 +0900 Subject: [PATCH 4/4] Fall back to unscaled rather than dividing the padding by zero 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. --- include/robot_self_filter/bodies.h | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/include/robot_self_filter/bodies.h b/include/robot_self_filter/bodies.h index 37aaabf..b0e598b 100644 --- a/include/robot_self_filter/bodies.h +++ b/include/robot_self_filter/bodies.h @@ -256,7 +256,10 @@ class ConvexMesh : public Body // A mesh takes one scale and one padding, the same way a sphere does. void setScale(double s) { - m_scale = s; + // The containment test divides by this, so a non-positive value would turn the + // padding term into infinity, report every point as inside, and silently delete + // the whole cloud. Fall back to unscaled rather than doing that. + m_scale = (s > 0.0) ? s : 1.0; updateInternalData(); } void setPadding(double p)