Skip to content

Commit

Permalink
[geometry] Hydroelastic callback for compliant-compliant contact. (Ro…
Browse files Browse the repository at this point in the history 
…botLocomotion#16366)

* [geometry] Hydroelastic callback for compliant-compliant contact
  - Expand the cases of calculating contact surfaces.
  • Loading branch information
@DamrongGuoy
DamrongGuoy authored Jan 31, 2022
1 parent 5fc2507 commit b996d8e
Show file tree
Hide file tree
Showing 5 changed files with 252 additions and 56 deletions.
1 change: 1 addition & 0 deletions geometry/proximity/BUILD.bazel
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,6 +247,7 @@ drake_cc_library(
],
deps = [
":collision_filter",
":field_intersection",
":hydroelastic_internal",
":mesh_half_space_intersection",
":mesh_intersection",
Expand Down
103 changes: 92 additions & 11 deletions geometry/proximity/hydroelastic_callback.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,7 @@
#include "drake/common/eigen_types.h"
#include "drake/geometry/geometry_ids.h"
#include "drake/geometry/proximity/collision_filter.h"
#include "drake/geometry/proximity/field_intersection.h"
#include "drake/geometry/proximity/hydroelastic_internal.h"
#include "drake/geometry/proximity/mesh_half_space_intersection.h"
#include "drake/geometry/proximity/mesh_intersection.h"
Expand Down Expand Up @@ -95,7 +96,9 @@ enum class CalcContactSurfaceResult {
kUnsupported, //< Contact surface can't be computed for the geometry
//< pair.
kHalfSpaceHalfSpace, //< Contact between two half spaces; not allowed.
kSameCompliance, //< The two geometries have the same compliance type.
kRigidRigid, //< Contact between two rigid geometries; not allowed.
kCompliantHalfSpaceCompliantMesh, //< Contact between a compliant mesh and a
//< compliant half space; not allowed.
};

/* Computes ContactSurface using the algorithm appropriate to the Shape types
Expand Down Expand Up @@ -137,6 +140,29 @@ std::unique_ptr<ContactSurface<T>> DispatchRigidSoftCalculation(
}
}

/* Computes ContactSurface using the algorithm appropriate to the Shape types
represented by the given `compliant` geometries.
@pre None of the geometries are half spaces. */
template <typename T>
std::unique_ptr<ContactSurface<T>> DispatchCompliantCompliantCalculation(
const SoftGeometry& compliant0_F, const math::RigidTransform<T>& X_WF,
GeometryId id0, const SoftGeometry& compliant1_G,
const math::RigidTransform<T>& X_WG, GeometryId id1,
HydroelasticContactRepresentation representation) {
DRAKE_DEMAND(!compliant0_F.is_half_space() && !compliant1_G.is_half_space());

const VolumeMeshFieldLinear<double, double>& field0_F =
compliant0_F.pressure_field();
const Bvh<Obb, VolumeMesh<double>>& bvh0_F = compliant0_F.bvh();
const VolumeMeshFieldLinear<double, double>& field1_G =
compliant1_G.pressure_field();
const Bvh<Obb, VolumeMesh<double>>& bvh1_G = compliant1_G.bvh();

return ComputeContactSurfaceFromCompliantVolumes(
id0, field0_F, bvh0_F, X_WF, id1, field1_G, bvh1_G, X_WG,
representation);
}

/* Calculates the contact surface (if it exists) between two potentially
colliding geometries.
Expand All @@ -161,14 +187,62 @@ CalcContactSurfaceResult MaybeCalcContactSurface(
data->geometries.hydroelastic_type(encoding_a.id());
const HydroelasticType type_B =
data->geometries.hydroelastic_type(encoding_b.id());

// One or two objects have no hydroelastic type.
if (type_A == HydroelasticType::kUndefined ||
type_B == HydroelasticType::kUndefined) {
return CalcContactSurfaceResult::kUnsupported;
}
if (type_A == type_B) {
return CalcContactSurfaceResult::kSameCompliance;

// Rigid-rigid contact is not supported in hydroelastic contact model.
// Callers might optionally fall back to point contact model.
if (type_A == HydroelasticType::kRigid &&
type_B == HydroelasticType::kRigid) {
return CalcContactSurfaceResult::kRigidRigid;
}

// Compliant-compliant contact.
if (type_A == HydroelasticType::kSoft && type_B == HydroelasticType::kSoft) {
GeometryId id0 = encoding_a.id();
GeometryId id1 = encoding_b.id();
// Enforce consistent ordering for reproducibility/repeatability of
// simulation since the same pair of geometries (A,B) may be called
// either as (A,B) or (B,A).
if (id0.get_value() > id1.get_value()) {
std::swap(id0, id1);
}
const SoftGeometry& soft0 = data->geometries.soft_geometry(id0);
const SoftGeometry& soft1 = data->geometries.soft_geometry(id1);

// Halfspace vs. halfspace is not supported.
if (soft0.is_half_space() && soft1.is_half_space()) {
return CalcContactSurfaceResult::kHalfSpaceHalfSpace;
}

// Compliant-halfspace vs. compliant-mesh is not supported.
if (soft0.is_half_space() || soft1.is_half_space()) {
return CalcContactSurfaceResult::kCompliantHalfSpaceCompliantMesh;
}

// Compliant mesh vs. compliant mesh.
DRAKE_DEMAND(!soft0.is_half_space() && !soft1.is_half_space());
std::unique_ptr<ContactSurface<T>> surface =
DispatchCompliantCompliantCalculation(soft0, data->X_WGs.at(id0), id0,
soft1, data->X_WGs.at(id1), id1,
data->representation);
if (surface != nullptr) {
DRAKE_DEMAND(surface->id_M() < surface->id_N());
data->surfaces.emplace_back(std::move(*surface));
}
return CalcContactSurfaceResult::kCalculated;
}

// Rigid-compliant contact
DRAKE_DEMAND((type_A == HydroelasticType::kRigid &&
type_B == HydroelasticType::kSoft) ||
(type_A == HydroelasticType::kSoft &&
type_B == HydroelasticType::kRigid));

bool A_is_rigid = type_A == HydroelasticType::kRigid;
const GeometryId id_S = A_is_rigid ? encoding_b.id() : encoding_a.id();
const GeometryId id_R = A_is_rigid ? encoding_a.id() : encoding_b.id();
Expand Down Expand Up @@ -232,15 +306,22 @@ bool Callback(fcl::CollisionObjectd* object_A_ptr,
"id {} and a {} {} with id {}",
type_A, GetGeometryName(*object_A_ptr), encoding_a.id(), type_B,
GetGeometryName(*object_B_ptr), encoding_b.id()));
case CalcContactSurfaceResult::kSameCompliance:
// TODO(SeanCurtis-TRI): When we introduce soft-soft compliance, make
// sure we always dispatch the two soft objects in a fixed order based
// on their corresponding geometry ids. See penetration_as_point_pair
// Callback for an example.
case CalcContactSurfaceResult::kRigidRigid:
throw std::logic_error(fmt::format(
"Requested contact between two rigid objects ({} with id "
"{}, {} with id {}); that is not allowed in hydroelastic-only "
"contact. Please consider using hydroelastics with point-contact "
"fallback, e.g., QueryObject::ComputeContactSurfacesWithFallback() "
"or MultibodyPlant::set_contact_model("
"ContactModel::kHydroelasticWithFallback)",
GetGeometryName(*object_A_ptr), encoding_a.id(),
GetGeometryName(*object_B_ptr), encoding_b.id()));
case CalcContactSurfaceResult::kCompliantHalfSpaceCompliantMesh:
throw std::logic_error(fmt::format(
"Requested contact between two {} objects ({} with id "
"{}, {} with id {}); only rigid-soft pairs are currently supported",
type_A, GetGeometryName(*object_A_ptr), encoding_a.id(),
"Requested hydroelastic contact between two compliant geometries, "
"one of which is a half space ({} with id {}, {} with id {}); "
"that is not allowed",
GetGeometryName(*object_A_ptr), encoding_a.id(),
GetGeometryName(*object_B_ptr), encoding_b.id()));
case CalcContactSurfaceResult::kHalfSpaceHalfSpace:
throw std::logic_error(fmt::format(
Expand Down
155 changes: 127 additions & 28 deletions geometry/proximity/test/hydroelastic_callback_test.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -266,8 +266,10 @@ ::testing::AssertionResult ValidateDerivatives(const ContactSurface<double>&) {
// looking for positive indicators that the rigorously tested intersection code
// has been properly connected in the callback.
//
// Only one of grad e_N or grad e_M will be defined -- it is the field from the
// soft mesh.
// For rigid-compliant contact, only one of grad e_N or grad e_M will be
// defined -- it is the field from the compliant geometry. For
// compliant-compliant contact, both grad e_N and grad e_M will be checked --
// they are from two two compliant geometries.
::testing::AssertionResult ValidateDerivatives(
const ContactSurface<AutoDiffXd>& surface) {
if (surface.representation() ==
Expand Down Expand Up @@ -312,8 +314,7 @@ ::testing::AssertionResult ValidateDerivatives(
TYPED_TEST_SUITE(DispatchRigidSoftCalculationTests, ScalarTypes);

// Test suite for exercising DispatchRigidSoftCalculation with different scalar
// types. (Currently only double as the hydroelastic infrastructure doesn't
// support autodiff yet.)
// types.
template <typename T>
class DispatchRigidSoftCalculationTests : public ::testing::Test {};

Expand Down Expand Up @@ -458,6 +459,68 @@ TYPED_TEST(DispatchRigidSoftCalculationTests, SoftHalfSpaceRigidMesh) {
}
}

TYPED_TEST_SUITE(DispatchCompliantCompliantCalculationTests, ScalarTypes);

template <typename T>
class DispatchCompliantCompliantCalculationTests : public ::testing::Test {};

TYPED_TEST(DispatchCompliantCompliantCalculationTests,
CompliantMeshCompliantMesh) {
using T = TypeParam;

const bool colliding{true};

TestScene<T> scene{ShapeType::kSphere, ShapeType::kSphere};
const Geometries& geometries = scene.hydroelastic_geometries();
const GeometryId id_A = scene.id_A();
const GeometryId id_B = scene.id_B();
// Sphere A has a fixed position.
const RigidTransform<T>& X_WA = scene.pose_in_world(id_A);
scene.AddGeometry(HydroelasticType::kSoft, HydroelasticType::kSoft);

for (const auto representation :
{HydroelasticContactRepresentation::kTriangle,
HydroelasticContactRepresentation::kPolygon}) {
SCOPED_TRACE(
fmt::format("representation = {}", static_cast<int>(representation)));
{
// Case 1: Intersecting spheres.
scene.PoseGeometry(colliding);
const RigidTransform<T>& X_WB = scene.pose_in_world(id_B);

unique_ptr<ContactSurface<T>> surface =
DispatchCompliantCompliantCalculation(
geometries.soft_geometry(id_A), X_WA, id_A,
geometries.soft_geometry(id_B), X_WB, id_B, representation);
EXPECT_NE(surface, nullptr);
EXPECT_TRUE(ValidateDerivatives(*surface));
switch (representation) {
case HydroelasticContactRepresentation::kTriangle:
EXPECT_EQ(12, surface->tri_mesh_W().num_triangles());
break;
case HydroelasticContactRepresentation::kPolygon:
EXPECT_EQ(4, surface->poly_mesh_W().num_elements());
break;
}
}
}

{
// Case 2: Separated spheres.
scene.PoseGeometry(!colliding);
const RigidTransform<T>& X_WB = scene.pose_in_world(id_B);

// When they are not in contact, mesh representation is irrelevant; so,
// we'll simply pick one.
unique_ptr<ContactSurface<T>> surface =
DispatchCompliantCompliantCalculation(
geometries.soft_geometry(id_A), X_WA, id_A,
geometries.soft_geometry(id_B), X_WB, id_B,
HydroelasticContactRepresentation::kTriangle);
EXPECT_EQ(surface, nullptr);
}
}

TYPED_TEST_SUITE(MaybeCalcContactSurfaceTests, ScalarTypes);

// Test suite for exercising MaybeCalcContactSurface with different scalar
Expand Down Expand Up @@ -493,7 +556,7 @@ TYPED_TEST(MaybeCalcContactSurfaceTests, UndefinedGeometry) {
ASSERT_EQ(scene.surfaces().size(), 0u);
}

// Confirms that matching compliance (rigid) can't be evaluated.
// Confirms that rigid-rigid contact can't be evaluated.
TYPED_TEST(MaybeCalcContactSurfaceTests, BothRigid) {
using T = TypeParam;

Expand All @@ -502,37 +565,60 @@ TYPED_TEST(MaybeCalcContactSurfaceTests, BothRigid) {

CalcContactSurfaceResult result = MaybeCalcContactSurface<T>(
&scene.shape_A(), &scene.shape_B(), &scene.data());
EXPECT_EQ(result, CalcContactSurfaceResult::kSameCompliance);
EXPECT_EQ(result, CalcContactSurfaceResult::kRigidRigid);
EXPECT_EQ(scene.surfaces().size(), 0u);
}

// Confirms that matching compliance (soft) can't be evaluated.
TYPED_TEST(MaybeCalcContactSurfaceTests, BothSoft) {
TYPED_TEST(MaybeCalcContactSurfaceTests, BothCompliantOneHalfSpace) {
using T = TypeParam;

TestScene<T> scene{ShapeType::kSphere, ShapeType::kSphere};
TestScene<T> scene{ShapeType::kSphere, ShapeType::kHalfSpace};
scene.ConfigureScene(HydroelasticType::kSoft, HydroelasticType::kSoft);

CalcContactSurfaceResult result = MaybeCalcContactSurface<T>(
&scene.shape_A(), &scene.shape_B(), &scene.data());
EXPECT_EQ(result, CalcContactSurfaceResult::kSameCompliance);
EXPECT_EQ(result, CalcContactSurfaceResult::kCompliantHalfSpaceCompliantMesh);
EXPECT_EQ(scene.surfaces().size(), 0u);
}

// Confirms that colliding two half spaces is detected and reported.
TYPED_TEST(MaybeCalcContactSurfaceTests, TwoHalfSpaces) {
TYPED_TEST(MaybeCalcContactSurfaceTests, BothCompliantNonHalfSpace) {
using T = TypeParam;

TestScene<T> scene{ShapeType::kHalfSpace, ShapeType::kHalfSpace};
// They must have different compliance types in order to get past the same
// compliance type condition.
scene.ConfigureScene(HydroelasticType::kSoft, HydroelasticType::kRigid,
false /* are_colliding */);
TestScene<T> scene{ShapeType::kSphere, ShapeType::kSphere};
scene.ConfigureScene(HydroelasticType::kSoft, HydroelasticType::kSoft);

CalcContactSurfaceResult result = MaybeCalcContactSurface<T>(
&scene.shape_A(), &scene.shape_B(), &scene.data());
EXPECT_EQ(result, CalcContactSurfaceResult::kHalfSpaceHalfSpace);
EXPECT_EQ(scene.surfaces().size(), 0u);
EXPECT_EQ(result, CalcContactSurfaceResult::kCalculated);
EXPECT_EQ(scene.surfaces().size(), 1u);
}

TYPED_TEST(MaybeCalcContactSurfaceTests, BothHalfSpace) {
using T = TypeParam;

for (const HydroelasticType first_type :
{HydroelasticType::kRigid, HydroelasticType::kSoft}) {
for (const HydroelasticType second_type :
{HydroelasticType::kRigid, HydroelasticType::kSoft}) {
SCOPED_TRACE(fmt::format("Use first_type = {}, second_type = {}.",
static_cast<int>(first_type),
static_cast<int>(second_type)));
TestScene<T> scene{ShapeType::kHalfSpace, ShapeType::kHalfSpace};
scene.ConfigureScene(first_type, second_type, false /* are_colliding */);

CalcContactSurfaceResult result = MaybeCalcContactSurface<T>(
&scene.shape_A(), &scene.shape_B(), &scene.data());

if (first_type == HydroelasticType::kRigid &&
second_type == HydroelasticType::kRigid) {
EXPECT_EQ(result, CalcContactSurfaceResult::kRigidRigid);
EXPECT_EQ(scene.surfaces().size(), 0u);
} else {
EXPECT_EQ(result, CalcContactSurfaceResult::kHalfSpaceHalfSpace);
EXPECT_EQ(scene.surfaces().size(), 0u);
}
}
}
}

// Confirms that a valid pair that are, nevertheless, not colliding does not
Expand Down Expand Up @@ -591,8 +677,6 @@ TYPED_TEST_SUITE(StrictHydroelasticCallbackTyped, ScalarTypes);
// ultimately percolate outward).
// - throw on any return value that isn't wholly successful.
// - respect collision filtering.
// (Currently only double as the hydroelastic infrastructure doesn't support
// autodiff yet.)
template <typename T>
class StrictHydroelasticCallbackTyped : public ::testing::Test {};

Expand All @@ -617,19 +701,18 @@ TYPED_TEST(StrictHydroelasticCallbackTyped,
"hydroelastic representation .+ rigid .+ undefined .+");
}

// Confirms that if the intersecting pair has the same compliance (rigid-rigid)
// or (soft-soft), that an exception is thrown. This test applies *no* collision
// filters to guarantee that the body of the callback gets exercised in all
// cases.
TYPED_TEST(StrictHydroelasticCallbackTyped, ThrowForSameComplianceType) {
// Confirms that if the intersecting pair is rigid-rigid, an exception is
// thrown. This test applies *no* collision filters to guarantee that the
// body of the callback gets exercised in all cases.
TYPED_TEST(StrictHydroelasticCallbackTyped, ThrowForRigidRigid) {
using T = TypeParam;

TestScene<T> scene{ShapeType::kSphere, ShapeType::kSphere};
scene.ConfigureScene(HydroelasticType::kRigid, HydroelasticType::kRigid);

// We test only a single "same-compliance" configuration (rigid, rigid)
// because we rely on the tests on MaybeCalcContactSurface() to have explored
// all the ways that the kSameCompliance calculation result is returned. This
// all the ways that the calculation result is returned. This
// configuration is representative of that set.
DRAKE_EXPECT_THROWS_MESSAGE(
Callback<T>(&scene.shape_A(), &scene.shape_B(), &scene.data()),
Expand All @@ -653,6 +736,22 @@ TYPED_TEST(StrictHydroelasticCallbackTyped, ThrowForTwoHalfSpaces) {
"Requested contact between two half spaces .+");
}

TYPED_TEST(StrictHydroelasticCallbackTyped,
ThrowForCompliantHalfSpaceAndNonHalfSpace) {
using T = TypeParam;

TestScene<T> scene{ShapeType::kHalfSpace, ShapeType::kSphere};
// They must have different compliance types in order to get past the same
// compliance type condition.
scene.ConfigureScene(HydroelasticType::kSoft, HydroelasticType::kSoft);

DRAKE_EXPECT_THROWS_MESSAGE(
Callback<T>(&scene.shape_A(), &scene.shape_B(), &scene.data()),
std::logic_error,
"Requested hydroelastic contact between two compliant geometries, one "
"of which is a half space .+");
}

// Confirms that if the pair contains unsupported geometry, as long as they are
// filtered, they don't pose a problem. The "lack of support" comes from the
// fact that the ids don't map to anything in the hydroelastic geometry set.
Expand Down Expand Up @@ -739,7 +838,7 @@ TYPED_TEST(HydroelasticCallbackFallbackTyped, PointPairForSameComplianceType) {

// We test only a single "same-compliance" configuration (rigid, rigid)
// because we rely on the tests on MaybeCalcContactSurface() to have explored
// all the ways that the kSameCompliance calculation result is returned. This
// all the ways that the calculation result is returned. This
// configuration is representative of that set.
vector<PenetrationAsPointPair<T>> point_pairs;
CallbackWithFallbackData<T> data{scene.data(), &point_pairs};
Expand Down
Loading

0 comments on commit b996d8e

Please sign in to comment.