Skip to content

Commit

Permalink
Removing grad_H from ContactSurface (RobotLocomotion#12432)
Browse files Browse the repository at this point in the history 
We are no longer using the grad_h value to define the normal of the contact
surface. Instead, we're using the triangle normals of the surface itself.
So, we remove the field and change downstream references to use the face
normal instead.

This act revealed some imprecision in the documented and implemented
behaviors of the triangle normals. Where appropriate new documentation has
been added (and tested) to help clarify the expectations of normals.
  • Loading branch information
@SeanCurtis-TRI
SeanCurtis-TRI authored Dec 3, 2019
1 parent 7adf1f0 commit acef24b
Show file tree
Hide file tree
Showing 14 changed files with 182 additions and 316 deletions.
75 changes: 12 additions & 63 deletions geometry/proximity/mesh_intersection.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -163,6 +163,8 @@ Vector3<T> CalcIntersection(const Vector3<T>& p_FA,
// = 0 when a != b.
}

// TODO(SeanCurtis-TRI): This function duplicates functionality implemented in
// mesh_half_space_intersection.h. Reconcile the two implementations.
// TODO(DamrongGuoy): Avoid duplicate vertices mentioned in the note below and
// check whether we can have other as yet undocumented degenerate cases.
/** Intersects a polygon with the half space H. It keeps the part of
Expand All @@ -171,14 +173,19 @@ Vector3<T> CalcIntersection(const Vector3<T>& p_FA,
in a common frame F.
@param[in] polygon_vertices_F
Input polygon is represented as a sequence of positions of its vertices.
The input polygon is allowed to have zero area.
@param[in] H_F
The clipping half space H in frame F.
@return
Output polygon is represented as a sequence of positions of its vertices.
It could be an empty sequence if the input polygon is entirely outside
the half space. It could be the same as the input polygon if the input
polygon is entirely inside the half space.
polygon is entirely inside the half space. The output polygon is guaranteed
to be planar (within floating point tolerance) and, if the polygon has
area, the normal implied by the winding will be the same as the input
polygon.
@pre `polygon_vertices_F` has at least three vertices.
@pre the vertices in `polygon_vertices_F` are all planar.
@note
1. For an input polygon P that is parallel to the plane of the half space,
there are three cases:
Expand Down Expand Up @@ -459,41 +466,6 @@ void AddPolygonToMeshData(
}
}

// TODO(SeanCurtis-TRI): Make this a property of the surface mesh.
/** Computes the field of unit normal vectors for the input `surface` mesh. This
field defines the outward normal value over the domain of the mesh. In order
for the field to be continuous, the underlying mesh must be a closed manifold
with no duplicate vertices.
*/
template <typename T>
std::unique_ptr<SurfaceMeshField<Vector3<T>, T>> ComputeNormalField(
const SurfaceMesh<T>& surface) {
// We define the normal field as a *linear* mesh field. So, we define a
// per-vertex normal based on the area-weighted combination of the incident
// face normals (computed as a right-handed normal of the triangle).

std::vector<Vector3<T>> normal_values(surface.num_vertices(),
Vector3<T>::Zero());
for (SurfaceFaceIndex face_index(0); face_index < surface.num_faces();
++face_index) {
const SurfaceFace& face = surface.element(face_index);
const Vector3<T>& A = surface.vertex(face.vertex(0)).r_MV();
const Vector3<T>& B = surface.vertex(face.vertex(1)).r_MV();
const Vector3<T>& C = surface.vertex(face.vertex(2)).r_MV();
Vector3<T> unit_normal = (B - A).cross(C - A).normalized();
for (int v = 0; v < 3; ++v) {
DRAKE_ASSERT(surface.area(face_index) > T(0.0));
normal_values[face.vertex(v)] += surface.area(face_index) * unit_normal;
}
}
for (SurfaceVertexIndex vertex_index(0);
vertex_index < surface.num_vertices(); ++vertex_index) {
normal_values[vertex_index].normalize();
}
return std::make_unique<SurfaceMeshFieldLinear<Vector3<T>, T>>(
"normal", std::move(normal_values), &surface);
}

// TODO(DamrongGuoy): Maintain book keeping to avoid duplicate vertices and
// remove the note in the function documentation.

Expand All @@ -520,10 +492,6 @@ std::unique_ptr<SurfaceMeshField<Vector3<T>, T>> ComputeNormalField(
@param[out] e_MN
The sampled field values on the intersecting surface (samples to support
a linear mesh field -- i.e., one per vertex).
@param[out] grad_h_MN_M
The unit vector field on the intersecting surface (surface normals). Each
vector is expressed in M's frame but is parallel with the surface normals
at the same point.
@note
The output surface mesh may have duplicate vertices.
*/
Expand All @@ -533,9 +501,8 @@ void SampleVolumeFieldOnSurface(
const SurfaceMesh<T>& surface_N,
const math::RigidTransform<T>& X_MN,
std::unique_ptr<SurfaceMesh<T>>* surface_MN_M,
std::unique_ptr<SurfaceMeshFieldLinear<T, T>>* e_MN,
std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>>* grad_h_MN_M) {
auto normal_field_N = ComputeNormalField(surface_N);
std::unique_ptr<SurfaceMeshFieldLinear<T, T>>* e_MN) {

// TODO(DamrongGuoy): Store normal_field_N in SurfaceMesh to avoid
// recomputing every time. Right now it is not straightforward to store
// SurfaceMeshField inside SurfaceMesh due to imperfect library packaging.
Expand All @@ -552,7 +519,6 @@ void SampleVolumeFieldOnSurface(

// TODO(DamrongGuoy): Use the broadphase to avoid O(n^2) check of all
// tetrahedrons against all triangles.
const math::RigidTransform<T> X_NM = X_MN.inverse();
for (VolumeElementIndex tet_index(0); tet_index < mesh_M.num_elements();
++tet_index) {
for (SurfaceFaceIndex tri_index(0); tri_index < surface_N.num_faces();
Expand All @@ -579,22 +545,14 @@ void SampleVolumeFieldOnSurface(
const Vector3<T>& r_MV = surface_vertices_M[v].r_MV();
const T pressure = volume_field_M.EvaluateCartesian(tet_index, r_MV);
surface_e.push_back(pressure);
const Vector3<T> r_NV = X_NM * r_MV;
const Vector3<T> normal_N =
normal_field_N->EvaluateCartesian(tri_index, r_NV);
Vector3<T> normal_M = X_MN.rotation() * normal_N;
surface_normals_M.push_back(normal_M);
}
}
}
DRAKE_DEMAND(surface_vertices_M.size() == surface_e.size());
DRAKE_DEMAND(surface_vertices_M.size() == surface_normals_M.size());
*surface_MN_M = std::make_unique<SurfaceMesh<T>>(
std::move(surface_faces), std::move(surface_vertices_M));
*e_MN = std::make_unique<SurfaceMeshFieldLinear<T, T>>(
"e", std::move(surface_e), surface_MN_M->get());
*grad_h_MN_M = std::make_unique<SurfaceMeshFieldLinear<Vector3<T>, T>>(
"grad_h_MN_M", std::move(surface_normals_M), surface_MN_M->get());
}

/** Computes the contact surface between a soft geometry S and a rigid
Expand Down Expand Up @@ -661,22 +619,13 @@ ComputeContactSurfaceFromSoftVolumeRigidSurface(
std::unique_ptr<SurfaceMesh<T>> surface_SR;
std::unique_ptr<SurfaceMeshFieldLinear<T, T>> e_SR;

// The gradient field will be computed as expressed in Frame S and then
// re-expressed in the world frame.
std::unique_ptr<SurfaceMeshFieldLinear<Vector3<T>, T>> grad_h_SR;
SampleVolumeFieldOnSurface(field_S, mesh_R, X_SR, &surface_SR, &e_SR,
&grad_h_SR);
SampleVolumeFieldOnSurface(field_S, mesh_R, X_SR, &surface_SR, &e_SR);

// Transform the mesh from the S frame to the world frame.
surface_SR->TransformVertices(X_WS);

// Re-express the gradient from the S frame to the world frame.
for (Vector3<T>& gradient_value : grad_h_SR->mutable_values())
gradient_value = X_WS.rotation() * gradient_value;

return std::make_unique<ContactSurface<T>>(
id_S, id_R, std::move(surface_SR), std::move(e_SR),
std::move(grad_h_SR));
id_S, id_R, std::move(surface_SR), std::move(e_SR));
}

// NOTE: This is a short-term hack to allow ProximityEngine to compile when
Expand Down
66 changes: 6 additions & 60 deletions geometry/proximity/test/contact_surface_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -18,7 +18,7 @@ namespace drake {
namespace geometry {

// TODO(DamrongGuoy): Remove this helper class when ContactSurface allows
// direct access to e_MN_ and grad_h_MN_W_.
// direct access to e_MN_.
template <typename T>
class ContactSurfaceTester {
public:
Expand All @@ -30,21 +30,11 @@ class ContactSurfaceTester {
return *(surface_.e_MN_);
}

const SurfaceMeshFieldLinear<Vector3<T>, T>& grad_h_MN_W() const {
DRAKE_DEMAND(surface_.grad_h_MN_W_ != nullptr);
return *(surface_.grad_h_MN_W_);
}

SurfaceMeshFieldLinear<T, T>& mutable_e_MN() const {
DRAKE_DEMAND(surface_.e_MN_ != nullptr);
return *(surface_.e_MN_);
}

SurfaceMeshFieldLinear<Vector3<T>, T>& mutable_grad_h_MN_W() const {
DRAKE_DEMAND(surface_.grad_h_MN_W_ != nullptr);
return *(surface_.grad_h_MN_W_);
}

SurfaceMesh<T>& mutable_mesh_W() const {
DRAKE_DEMAND(surface_.mesh_W_ != nullptr);
return *(surface_.mesh_W_);
Expand Down Expand Up @@ -130,20 +120,8 @@ ContactSurface<T> TestContactSurface() {
auto e_field = std::make_unique<SurfaceMeshFieldLinear<T, T>>(
"e", std::move(e_values), surface_mesh.get());

// Slightly different values of grad_h_MN_W at each vertex.
// We give names to the values at vertices for testing later.
const Vector3<T> g0(-0.1, -0.1, 1.);
const Vector3<T> g1(0.1, -0.1, 1.);
const Vector3<T> g2(0.1, 0.1, 1.);
const Vector3<T> g3(-0.1, 0.1, 1.);
std::vector<Vector3<T>> grad_h_MN_W_values = {g0, g1, g2, g3};
auto grad_h_MN_W_field =
std::make_unique<SurfaceMeshFieldLinear<Vector3<T>, T>>(
"grad_h_MN_W", std::move(grad_h_MN_W_values), surface_mesh.get());

ContactSurface<T> contact_surface(id_M, id_N, std::move(surface_mesh),
std::move(e_field),
std::move(grad_h_MN_W_field));
std::move(e_field));

// Start testing the ContactSurface<> data structure.
EXPECT_EQ(id_M, contact_surface.id_M());
Expand All @@ -160,21 +138,6 @@ ContactSurface<T> TestContactSurface() {
const T expect_e = b(0) * e0 + b(1) * e1 + b(2) * e2;
EXPECT_EQ(expect_e, contact_surface.EvaluateE_MN(f0, b));
}
// Tests evaluation of `grad_h_MN_W` on face f1 {2, 3, 0}.
{
const SurfaceFaceIndex f1(1);
const typename SurfaceMesh<T>::Barycentric b{0.6, 0.3, 0.1};
// On face f1, we have these quantities.
//---+--------+----------+-----------------
// v | vertex | grad_h_MN_W | barycentric
//---+--------+-------------+--------------
// 0 | v2 | g2 | 0.6
// 1 | v3 | g3 | 0.3
// 2 | v0 | g0 | 0.1
//---+--------+-------------+--------------
const Vector3<T> expect_g = T(0.6) * g2 + T(0.3) * g3 + T(0.1) * g0;
EXPECT_EQ(expect_g, contact_surface.EvaluateGrad_h_MN_W(f1, b));
}
// Tests area() of triangular faces.
{
EXPECT_EQ(T(0.5), contact_surface.mesh_W().area(SurfaceFaceIndex(0)));
Expand Down Expand Up @@ -205,7 +168,6 @@ GTEST_TEST(ContactSurfaceTest, TestCopy) {
const SurfaceFaceIndex f(0);
const typename SurfaceMesh<double>::Barycentric b{0.2, 0.3, 0.5};
EXPECT_EQ(original.EvaluateE_MN(f, b), copy.EvaluateE_MN(f, b));
EXPECT_EQ(original.EvaluateGrad_h_MN_W(f, b), copy.EvaluateGrad_h_MN_W(f, b));
}

// Tests the equality comparisons.
Expand All @@ -227,12 +189,6 @@ GTEST_TEST(ContactSurfaceTest, TestEqual) {
ContactSurfaceTester<double>(surface2).mutable_e_MN().mutable_values()[0] +=
2.0;
EXPECT_FALSE(surface.Equal(surface2));

// Different grad h field.
auto surface3 = ContactSurface<double>(surface);
ContactSurfaceTester<double>(surface3).mutable_grad_h_MN_W()
.mutable_values()[0][0] += 2.;
EXPECT_FALSE(surface.Equal(surface3));
}

// Tests the constructor of ContactSurface that when id_M is greater than
Expand All @@ -243,11 +199,9 @@ GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
auto mesh = std::make_unique<SurfaceMesh<double>>(original.mesh_W());
SurfaceMesh<double>* mesh_pointer = mesh.get();
// TODO(DamrongGuoy): Remove `original_tester` when ContactSurface allows
// direct access to e_MN and grad_h_MN_W.
// direct access to e_MN.
const ContactSurfaceTester<double> original_tester(original);
std::vector<double> e_MN_values = original_tester.e_MN().values();
std::vector<Vector3<double>> grad_h_MN_W_values =
original_tester.grad_h_MN_W().values();

// Create id_M after id_N, so id_M > id_N. This condition will trigger
// SwapMAndN in the constructor of ContactSurface.
Expand All @@ -257,9 +211,7 @@ GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
ContactSurface<double> dut(
id_M, id_N, std::move(mesh),
std::make_unique<SurfaceMeshFieldLinear<double, double>>(
"e_MN", std::move(e_MN_values), mesh_pointer),
std::make_unique<SurfaceMeshFieldLinear<Vector3<double>, double>>(
"grad_h_MN_W", std::move(grad_h_MN_W_values), mesh_pointer));
"e_MN", std::move(e_MN_values), mesh_pointer));

// We rely on the underlying meshes and mesh fields to *do* the right thing.
// These tests are just to confirm that those things changed where we
Expand All @@ -280,17 +232,11 @@ GTEST_TEST(ContactSurfaceTest, TestSwapMAndN) {
are_identical(dut.mesh_W().element(f), original.mesh_W().element(f)));
}

// Test the mesh fields by evaluating each field, once per face for an
// arbitrary point Q on the interior of the triangle. We expect:
// e_MN function hasn't changed.
// grad_H function has been mirrored.
// Evalute the mesh field, once per face for an arbitrary point Q on the
// interior of the triangle. We expect e_MN function hasn't changed.
const SurfaceMesh<double>::Barycentric b_Q{0.25, 0.25, 0.5};
for (SurfaceFaceIndex f(0); f < original.mesh_W().num_faces(); ++f) {
EXPECT_EQ(dut.EvaluateE_MN(f, b_Q), original.EvaluateE_MN(f, b_Q));
const Vector3d expected_normal = -original.EvaluateGrad_h_MN_W(f, b_Q);
EXPECT_TRUE(CompareMatrices(dut.EvaluateGrad_h_MN_W(f, b_Q),
expected_normal,
std::numeric_limits<double>::epsilon()));
}
}

Expand Down
Loading

0 comments on commit acef24b

Please sign in to comment.