Skip to content

Commit

Permalink
[geometry] Split infeasible vertices in MakeInflatedMesh() (RobotLoco…
Browse files Browse the repository at this point in the history 
…motion#21780)

[geometry] Split infeasible vertices in MakeInflatedMesh()

Co-authored-by: amcastro-tri <[email protected]>
Co-authored-by: Sean Curtis <[email protected]>
  • Loading branch information
@joemasterjohn @amcastro-tri @SeanCurtis-TRI
3 people authored Aug 7, 2024
1 parent f88ad9e commit bca71e1
Show file tree
Hide file tree
Showing 7 changed files with 594 additions and 255 deletions.
1 change: 1 addition & 0 deletions geometry/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -773,6 +773,7 @@ filegroup(
srcs = [
"test/cube_as_6_squares.vtk",
"test/cube_as_volume.vtk",
"test/inflation_infeasible_vertex.vtk",
"test/non_convex_mesh.vtk",
"test/one_negative_tetrahedron.vtk",
"test/one_tetrahedron.vtk",
Expand Down
339 changes: 228 additions & 111 deletions geometry/proximity/inflate_mesh.cc
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,7 @@
#include "drake/geometry/proximity/inflate_mesh.h"

#include <limits>
#include <map>
#include <memory>
#include <utility>
#include <vector>
Expand All @@ -12,72 +13,217 @@
namespace drake {
namespace geometry {
namespace internal {
namespace {

using Eigen::MatrixXd;
using Eigen::Vector3d;
using Eigen::VectorXd;

/* Implements the QP program to inflate a mesh by a given margin amount.
This can be written as:
min 1/2‖u‖|²
s.t. uᵢ⋅n̂ₐ ≥ δ
where for each i-th surface we add a linear constraint involving
all adjacent faces to vertex i with normal n̂ₐ. */
class InflateProgram {
public:
DRAKE_NO_COPY_NO_MOVE_NO_ASSIGN(InflateProgram);

// Create an optimization program to inflate `mesh` and amount `margin.`
// @note `mesh` is aliased and must remain valid for the lifetime of this
// class.
// @pre mesh is not nullptr.
InflateProgram(const VolumeMesh<double>* mesh, double margin);

VolumeMesh<double> Solve() const;

private:
// Makes a dimensionless version of program described in the class's
// documentation to minimize round-off errors. We use the margin δ to define a
// dimensionless displacement ũ = u/δ, and write the dimensionless program as:
// min 1/2‖ũ‖|²
// s.t. ũᵢ⋅n̂ₐ ≥ 1
//
// N.B. The program is "separable", i.e. we could solve the displacement for
// each surface vertex separately, independently of all other vertices. That
// strategy could be considered for instance for thread parallelization.
void MakeProgram();

const VolumeMesh<double>& mesh_;
double margin_{0.0};
std::unique_ptr<solvers::MathematicalProgram> prog_;
VectorX<symbolic::Variable> u_; // displacements
// Map from surface indexes (i.e. indexes into u) to vertices in the original
// volume mesh, i.e. mesh_.
std::vector<int> surface_to_volume_vertices_;
};

InflateProgram::InflateProgram(const VolumeMesh<double>* mesh, double margin)
: mesh_{*mesh}, margin_(margin) {
DRAKE_DEMAND(margin >= 0);
DRAKE_DEMAND(mesh != nullptr);
if (margin > 0) MakeProgram();
namespace {

/* Given a surface and a collection `incident_faces` to a vertex (not given
explicitly), makes the program to "inflate" the vertex outwards.
To minimize round-off errors, we make the problem dimensionless by definining
the dimensionless displacement ũ = u/δ where δ is the margin. The dimensionless
program is:
min 1/2‖ũ‖|²
s.t. ũ⋅n̂ₐ ≥ 1, ∀ n̂ₐ ∈ normals(incident_faces)
where n̂ₐ is the normal of a face in `incident_faces`.
Note: `incident_faces` will not contain _all_ triangles incident to the implied
vertex. This function is helper for resolving displacement for infeasible
vertex displacement. They are infeasible because the _full_ set of incident
faces define a set of infeasible constraints.
@pre incident_faces.size() > 1. If there is only a single incident face, the
solution of the optimization is trivial: δ⋅n̂ₐ. Calling this is overkill. */
std::unique_ptr<solvers::MathematicalProgram> MakeVertexProgram(
const TriangleSurfaceMesh<double>& surface,
const std::vector<int>& incident_faces) {
DRAKE_DEMAND(incident_faces.size() > 1);

auto prog = std::make_unique<solvers::MathematicalProgram>();
const int nv = 3;
auto u = prog->NewContinuousVariables(nv);
prog->AddQuadraticCost(MatrixXd::Identity(nv, nv), VectorXd::Zero(nv), u,
true /* it is convex */);
/* Add one constraint per incident face, inflating w.r.t. all incident faces
simultaneously. */
const int num_faces = incident_faces.size();
const VectorXd lb = VectorXd::Ones(num_faces);
const VectorXd ub =
VectorXd::Constant(num_faces, std::numeric_limits<double>::infinity());
MatrixXd A(num_faces, 3);
for (int f = 0; f < num_faces; ++f) {
const Vector3d& normal = surface.face_normal(incident_faces[f]);
A.row(f) = normal.transpose();
}
prog->AddLinearConstraint(A, lb, ub, u);

return prog;
}

void InflateProgram::MakeProgram() {
prog_ = std::make_unique<solvers::MathematicalProgram>();
/* Given a set of `faces` that lie on the surface of a volume mesh, all incident
to a common vertex, partitions the faces based on the tetrahedra the faces
come from.
@returns The partitioning of incident faces. Each entry of the return value
contains those incident faces which are faces of a common tetrahedron. (Looking
up the tet index from `tri_to_tet` should confirm this.)
If the volume mesh is a valid hydroelastic or deformable volume mesh, there
should be no more than one face in each entry. */
std::vector<std::vector<int>> PartitionIncidentFaces(
const std::vector<int>& faces, const std::vector<TetFace>& tri_to_tet) {
/* Builds a map from the faces' original tets to the face indices. We're using
a map so that the partitions are well-ordered on all platforms, otherwise
the changes to the tetrahedral mesh will vary from platform to platform. */
std::map<int, std::vector<int>> tet_groups;
for (int f : faces) {
tet_groups[tri_to_tet[f].tet_index].push_back(f);
}

/* Extract groups. */
std::vector<std::vector<int>> groups;
groups.reserve(tet_groups.size());
for (auto& [_, fg] : tet_groups) {
groups.push_back(std::move(fg));
}

return groups;
}

/* Computes the displacement for an *implied* vertex. It is a vertex that is
incident to all of the faces enumerated in `face_group`. The displacement
satisfies the optimization program documented for MakeVertexProgram() and
only depends on the face normals and `margin` value. */
Vector3d CalculateDisplacement(const TriangleSurfaceMesh<double>& mesh_surface,
const std::vector<int>& face_group,
double margin) {
if (face_group.size() == 1) {
/* A single face has a trivial solution. */
return margin * mesh_surface.face_normal(face_group[0]);
} else {
std::unique_ptr<solvers::MathematicalProgram> prog =
MakeVertexProgram(mesh_surface, face_group);
solvers::ClarabelSolver solver;
const solvers::MathematicalProgramResult result = solver.Solve(*prog);
if (!result.is_success()) {
/* At this point, it isn't clear what circumstances would cause failure.
Most likely an extremely degenerate case (e.g., a flat tetrahedron with
two anti-parallel faces on the surface). As such, it's difficult to
know if there's a more helpful message that could be given. */
const solvers::ClarabelSolver::Details& details =
result.get_solver_details<solvers::ClarabelSolver>();
throw std::logic_error(
fmt::format("Program fails with status: {}.", details.status));
}

return margin * result.get_x_val();
}
}

/* Creates a new VolumeElement (tetrahedron) from an old tet. The new tet has
all the same vertices except for one. The kth vertex uses the value
`new_index`, for k ∈ [0, 3]. */
VolumeElement SwapTetVertex(const VolumeElement& tet, int k, int new_index) {
// clang-format off
return VolumeElement(
k == 0 ? new_index : tet.vertex(0),
k == 1 ? new_index : tet.vertex(1),
k == 2 ? new_index : tet.vertex(2),
k == 3 ? new_index : tet.vertex(3));
// clang-format on
}

/* We were unable to compute a displacement for the indicated `surface_vertex`.
This is because the faces incident to that vertex defined a set of infeasible
constraints.
The solution is to partition the set of faces into smaller groups and to
duplicate the problematic vertex so each group has its own copy. The result of
this duplication is:
- New vertex values get appended to `vertices`.
- New displacement values appended to `u`.
- The definitions of *existing* tetrahedra in `tetrahedra` get changed to
reference duplicated vertices.
- Mapping from surface vertex to volume vertex is extended in
`surface_to_volume_vertices`.
- Mapping from split vertices to original vertices is stored in
`split_vertex_to_original` */
void ProcessUnfeasibleVertex(const VolumeMesh<double>& mesh,
const TriangleSurfaceMesh<double>& mesh_surface,
double margin, int surface_vertex,
const std::vector<int>& incident_faces,
const std::vector<TetFace>& tri_to_tet,
std::vector<Vector3d>* u,
std::vector<Vector3d>* vertices,
std::vector<VolumeElement>* tetrahedra,
std::vector<int>* surface_to_volume_vertices,
std::map<int, int>* split_vertex_to_original) {
/* Find v's local index in its tetrahedron. */
auto find_tet_local_index = [&](int tet_index, int vertex_index) {
const VolumeElement& tet = mesh.element(tet_index);
for (int k = 0; k < 4; ++k) {
if (vertex_index == tet.vertex(k)) return k;
}
DRAKE_UNREACHABLE();
};

/* The index of the volume vertex to which `surface_vertex` corresponds. */
const int volume_vertex = (*surface_to_volume_vertices)[surface_vertex];

const std::vector<std::vector<int>> face_groups =
PartitionIncidentFaces(incident_faces, tri_to_tet);
/* The only way to resolve the infeasible displacement is to partition the
faces into _multiple_ groups. */
DRAKE_DEMAND(face_groups.size() > 1);

/* For each partition, we will have _one_ vertex in the resulting mesh; this
will require copying the original vertex. However, we only need to make N-1
copies; the first group will use the original vertex. */
(*u)[surface_vertex] =
CalculateDisplacement(mesh_surface, face_groups[0], margin);

/* Intentional copy -- we'll be pushing to `vertices` and don't want to risk
an invalidated reference. */
const Vector3d p = (*vertices)[volume_vertex];
for (int i = 1; i < ssize(face_groups); ++i) {
const std::vector<int>& face_group = face_groups[i];

/* Duplicate the vertex value. */
const int dupe_volume_index = ssize(*vertices);
surface_to_volume_vertices->push_back(dupe_volume_index);
split_vertex_to_original->emplace(dupe_volume_index, volume_vertex);
vertices->push_back(p);
u->push_back(CalculateDisplacement(mesh_surface, face_group, margin));

/* Update volume elements to point to the newly added duplicate. */
const int tet_index = tri_to_tet[face_group[0]].tet_index;
const int k = find_tet_local_index(tet_index, volume_vertex);
(*tetrahedra)[tet_index] =
SwapTetVertex((*tetrahedra)[tet_index], k, dupe_volume_index);
}
}

} // namespace

VolumeMesh<double> MakeInflatedMesh(
const VolumeMesh<double>& mesh, double margin,
std::map<int, int>* split_vertex_to_original) {
DRAKE_THROW_UNLESS(margin >= 0);
DRAKE_THROW_UNLESS(split_vertex_to_original != nullptr);

// Surface mesh and map to volume vertices.
std::vector<int> surface_to_volume_vertices;
std::vector<TetFace> tri_to_tet;
const TriangleSurfaceMesh<double> mesh_surface =
ConvertVolumeToSurfaceMeshWithBoundaryVertices(
mesh_, &surface_to_volume_vertices_);
mesh, &surface_to_volume_vertices, &tri_to_tet);
const int num_surface_vertices = mesh_surface.num_vertices();
DRAKE_DEMAND(ssize(surface_to_volume_vertices_) == num_surface_vertices);

const int num_vars = 3 * num_surface_vertices;
u_ = prog_->NewContinuousVariables(num_vars, "u");

prog_->AddQuadraticCost(MatrixXd::Identity(num_vars, num_vars),
VectorXd::Zero(num_vars), u_,
true /* it is convex */);
DRAKE_DEMAND(ssize(surface_to_volume_vertices) == num_surface_vertices);

// Determine adjacent faces to each vertex on the surface.
std::vector<std::vector<int>> adjacent_faces(
Expand All @@ -90,70 +236,41 @@ void InflateProgram::MakeProgram() {
}
}

// For each surface vertex we add as many linear constraints as adjacent
// faces to that vertex. These constraints enforce that the mesh actually
// inflates (avoiding deflation regions).
for (int v = 0; v < num_surface_vertices; ++v) {
const std::vector<int>& faces = adjacent_faces[v];

// One linear constraint per face.
const int num_faces = faces.size();
const VectorXd lb = VectorXd::Ones(num_faces);
const VectorXd ub =
VectorXd::Constant(num_faces, std::numeric_limits<double>::infinity());
MatrixXd A(num_faces, 3);
for (int f = 0; f < num_faces; ++f) {
const Vector3d& normal = mesh_surface.face_normal(faces[f]);
A.row(f) = normal.transpose();
}
prog_->AddLinearConstraint(A, lb, ub, u_.segment<3>(3 * v));
}
}
std::vector<Vector3d> vertices = mesh.vertices();
std::vector<VolumeElement> tetrahedra = mesh.tetrahedra();
std::vector<Vector3d> u(num_surface_vertices, Vector3d::Zero());

VolumeMesh<double> InflateProgram::Solve() const {
if (margin_ == 0) return mesh_;

// N.B. By experimentation with meshes of different complexity, we determined
// that Clarabel performed best in terms of both accuracy and computational
// performance using solver default parameters.
solvers::ClarabelSolver solver;
const solvers::MathematicalProgramResult result = solver.Solve(*prog_);

if (!result.is_success()) {
throw std::runtime_error(
"Failure to inflate mesh. Unless there is a bug, the procedure to "
"apply margins to non-convex meshes is guaranteed to succeed. You "
"might also want to check your volume mesh is not somehow degenerate. "
"Otherwise, please open a Drake issue.");
}
/* Process each surface vertex separately. In the case where a vertex is
infeasible, we will end up appending new vertices to `vertices`. It is
essential that this for loop limit itself to the *original* vertices. */
for (int s = 0; s < num_surface_vertices; ++s) {
const std::vector<int>& faces = adjacent_faces[s];

std::unique_ptr<solvers::MathematicalProgram> prog =
MakeVertexProgram(mesh_surface, faces);
solvers::ClarabelSolver solver;
const solvers::MathematicalProgramResult result = solver.Solve(*prog);

// The solution corresponds to the dimensionless displacements ũ for each
// vertex of the input volume mesh. Scaling by the margin, gives us the
// displacements u.
const VectorXd u = margin_ * result.get_x_val();
if (result.is_success()) {
u[s] = margin * result.get_x_val();
} else {
ProcessUnfeasibleVertex(
mesh, mesh_surface, margin, s, faces, tri_to_tet, &u, &vertices,
&tetrahedra, &surface_to_volume_vertices, split_vertex_to_original);
}
}

// First copy all vertices.
std::vector<Vector3d> vertices = mesh_.vertices();
DRAKE_DEMAND(surface_to_volume_vertices.size() == u.size());

// Apply displacement to each surface vertex.
for (int s = 0; s < ssize(surface_to_volume_vertices_); ++s) {
const int v = surface_to_volume_vertices_[s];
vertices[v] += u.segment<3>(3 * s);
for (int s = 0; s < ssize(u); ++s) {
int v = surface_to_volume_vertices[s];
vertices[v] += u[s];
}

std::vector<VolumeElement> tetrahedra = mesh_.tetrahedra();
return VolumeMesh<double>(std::move(tetrahedra), std::move(vertices));
}

} // namespace

VolumeMesh<double> MakeInflatedMesh(const VolumeMesh<double>& mesh,
double margin) {
DRAKE_THROW_UNLESS(margin >= 0);
InflateProgram program(&mesh, margin);
return program.Solve();
}

} // namespace internal
} // namespace geometry
} // namespace drake
Loading

0 comments on commit bca71e1

Please sign in to comment.