Skip to content

Commit

Permalink
Modifying parsing semantics for hydroelastics (RobotLocomotion#12529)
Browse files Browse the repository at this point in the history
1. No longer determine compliance from <drake:elastic_modulus/>.
2. Requires at most one of <drake:soft_hydroelastic/> or
   <drake:rigid_hydroelastic/>.
3. Changing the logic for triggering hydroelastic representation generation
   to make use of the declared property.
  • Loading branch information
SeanCurtis-TRI authored Jan 9, 2020
1 parent a05b5b0 commit c5e632c
Show file tree
Hide file tree
Showing 19 changed files with 575 additions and 328 deletions.
46 changes: 2 additions & 44 deletions geometry/proximity/hydroelastic_internal.cc
Original file line number Diff line number Diff line change
Expand Up @@ -26,23 +26,6 @@ using std::make_unique;
using std::move;
using std::vector;

std::ostream& operator<<(std::ostream& out, const HydroelasticType& type) {
switch (type) {
case HydroelasticType::kUndefined:
out << "undefined";
break;
case HydroelasticType::kRigid:
out << "rigid";
break;
case HydroelasticType::kSoft:
out << "soft";
break;
default:
DRAKE_UNREACHABLE();
}
return out;
}

SoftGeometry& SoftGeometry::operator=(const SoftGeometry& g) {
if (this == &g) return *this;

Expand Down Expand Up @@ -78,7 +61,8 @@ void Geometries::RemoveGeometry(GeometryId id) {

void Geometries::MaybeAddGeometry(const Shape& shape, GeometryId id,
const ProximityProperties& properties) {
const HydroelasticType type = Classify(properties);
const HydroelasticType type = properties.GetPropertyOrDefault(
kHydroGroup, kComplianceType, HydroelasticType::kUndefined);
if (type != HydroelasticType::kUndefined) {
ReifyData data{type, id, properties};
shape.Reify(this, &data);
Expand Down Expand Up @@ -148,32 +132,6 @@ void Geometries::AddGeometry(GeometryId id, RigidGeometry geometry) {
rigid_geometries_.insert({id, move(geometry)});
}

HydroelasticType Classify(const ProximityProperties& props) {
// Presence of the hydroelastic group triggers an attempt to represent it.
if (props.HasGroup(kHydroGroup)) {
if (props.HasProperty(kMaterialGroup, kElastic)) {
const double elastic_modulus =
props.GetProperty<double>(kMaterialGroup, kElastic);
if (std::isinf(elastic_modulus)) {
return HydroelasticType::kRigid;
} else if (elastic_modulus <= 0) {
throw std::logic_error(
fmt::format("Properties contain a bad value for the ('{}', '{}') "
"property: {}; the value must be positive",
kMaterialGroup, kElastic, elastic_modulus));
} else {
return HydroelasticType::kSoft;
}
} else {
throw std::logic_error(
fmt::format("Properties contain the '{}' group but is missing the "
"('{}', '{}') property; compliance cannot be determined",
kHydroGroup, kMaterialGroup, kElastic));
}
}
return HydroelasticType::kUndefined;
}

// Validator interface for use with extracting valid properties. It is
// instantiated with shape (e.g., "Sphere", "Box", etc.) and compliance (i.e.,
// "rigid" or "soft") strings (to help give intelligible error messages) and
Expand Down
35 changes: 0 additions & 35 deletions geometry/proximity/hydroelastic_internal.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,23 +22,6 @@ namespace geometry {
namespace internal {
namespace hydroelastic {

// TODO(SeanCurtis-TRI): Update this to have an additional classification: kBoth
// when we have the need from the algorithm. For example: when we have two
// very stiff objects, we'd want to process them as soft. But when one
// very stiff and one very soft object interact, it might make sense to
// consider the stiff object as effectively rigid and simplify the computation.
// In this case, the object would get two representations.
/** Classification of the type of representation a shape has for the
hydroelastic contact model: rigid or soft. */
enum class HydroelasticType {
kUndefined,
kRigid,
kSoft
};

/** Streaming operator for writing hydroelastic type to output stream. */
std::ostream& operator<<(std::ostream& out, const HydroelasticType& type);

// TODO(SeanCurtis-TRI): When we do soft-soft contact, we'll need ∇p̃(e) as well.
// ∇p̃(e) is piecewise constant -- one ℜ³ vector per tetrahedron.
/** Defines a soft mesh -- a mesh, its linearized pressure field, p̃(e), and its
Expand Down Expand Up @@ -233,24 +216,6 @@ class Geometries final : public ShapeReifier {
std::unordered_map<GeometryId, RigidGeometry> rigid_geometries_;
};

/** Assesses the properties to determine *if* the geometry with the given
`properties` requires a geometric representation and, if so, whether that
representation is soft or rigid.
The conditions are as follows:
- If `properties` does not have the group "hydroelastic", then it requires no
hydroelastic representation.
- Otherwise, its compliance is based on the (material, elastic modulus)
property. The disposition based on the property is:
- absent --> error
- infinity --> rigid
- greater than zero and less than infinity --> soft
- less than or equal to zero --> error
@throws std::logic_error if any of the above error conditions are met. */
HydroelasticType Classify(const ProximityProperties& properties);

/** @name Creating hydroelastic representations of shapes
By default *no* shapes are supported with hydroelastic representations.
Expand Down
2 changes: 1 addition & 1 deletion geometry/proximity/test/hydroelastic_callback_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ class HydroelasticCallbackTyped : public ::testing::Test {

// Confirms that if the intersecting pair is missing hydroelastic representation
// that an exception is thrown. This test should apply *no* collision filters
// to guarantee that the body off the callback gets exercised in all cases.
// to guarantee that the body of the callback gets exercised in all cases.
TYPED_TEST(HydroelasticCallbackTyped,
ThrowForMissingHydroelasticRepresentation) {
using T = TypeParam;
Expand Down
68 changes: 0 additions & 68 deletions geometry/proximity/test/hydroelastic_internal_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -106,74 +106,6 @@ GTEST_TEST(Hydroelastic, RemoveGeometry) {
}
}

// Tests the classification of proximity properties.
GTEST_TEST(Hydroelastic, Classify) {
// Case: props with no declared "hydroelastic" group is, by definition, an
// unclassified hydroelastic type.
EXPECT_EQ(Classify(ProximityProperties()), HydroelasticType::kUndefined);

// Case: Presence of elastic modulus is insufficient to trigger hydroelastic.
{
ProximityProperties props;
props.AddProperty(kMaterialGroup, kElastic, 1e8);
EXPECT_EQ(Classify(props), HydroelasticType::kUndefined);
}

// The *presence* of the kHydroGroup will trigger the attempt to classify a
// hydroelastic representation. This uses a property that has no bearing on
// that classification to be that trigger.
ProximityProperties trigger_hydro;
trigger_hydro.AddProperty(kHydroGroup, "dummy", 1);

// Case: missing elastic modulus is a classification error.
{
DRAKE_EXPECT_THROWS_MESSAGE(Classify(trigger_hydro), std::logic_error,
"Properties .+ missing .+ property; compliance cannot be determined");
}

// Case: Infinite "elastic_modulus" should report as rigid.
{
ProximityProperties props(trigger_hydro);
props.AddProperty(kMaterialGroup, kElastic,
std::numeric_limits<double>::infinity());
EXPECT_EQ(Classify(props), HydroelasticType::kRigid);
}

// Case: Really, really huge, but not infinite "elastic modulus" should report
// as soft.
{
ProximityProperties props(trigger_hydro);
props.AddProperty(kMaterialGroup, kElastic,
std::numeric_limits<double>::max());
EXPECT_EQ(Classify(props), HydroelasticType::kSoft);
}

// Case: Microscopically small elastic modulus should report as soft.
{
ProximityProperties props(trigger_hydro);
props.AddProperty(kMaterialGroup, kElastic, 1e-14);
EXPECT_EQ(Classify(props), HydroelasticType::kSoft);
}

// Case: Zero elastic modulus should throw an error.
{
ProximityProperties props(trigger_hydro);
props.AddProperty(kMaterialGroup, kElastic, 0.0);
DRAKE_EXPECT_THROWS_MESSAGE(
Classify(props), std::logic_error,
fmt::format(".+bad value for .+'{}'.*", kElastic));
}

// Case: Negative elastic modulus should throw an error.
{
ProximityProperties props(trigger_hydro);
props.AddProperty(kMaterialGroup, kElastic, -1.0);
DRAKE_EXPECT_THROWS_MESSAGE(
Classify(props), std::logic_error,
fmt::format(".+bad value for .+'{}'.*", kElastic));
}
}

class HydroelasticRigidGeometryTest : public ::testing::Test {
protected:
/** Creates a simple set of properties for generating rigid geometry. */
Expand Down
2 changes: 1 addition & 1 deletion geometry/proximity_engine.cc
Original file line number Diff line number Diff line change
Expand Up @@ -36,7 +36,7 @@ namespace internal {

using Eigen::Vector3d;
using fcl::CollisionObjectd;
using drake::geometry::internal::hydroelastic::HydroelasticType;
using drake::geometry::internal::HydroelasticType;
using math::RigidTransform;
using math::RigidTransformd;
using std::make_shared;
Expand Down
42 changes: 35 additions & 7 deletions geometry/proximity_properties.cc
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,24 @@ const char* const kHcDissipation = "hunt_crossley_dissipation";

const char* const kHydroGroup = "hydroelastic";
const char* const kRezHint = "resolution_hint";
const char* const kComplianceType = "compliance_type";

std::ostream& operator<<(std::ostream& out, const HydroelasticType& type) {
switch (type) {
case HydroelasticType::kUndefined:
out << "undefined";
break;
case HydroelasticType::kRigid:
out << "rigid";
break;
case HydroelasticType::kSoft:
out << "soft";
break;
default:
DRAKE_UNREACHABLE();
}
return out;
}

} // namespace internal

Expand All @@ -22,23 +40,33 @@ void AddRigidHydroelasticProperties(double resolution_hint,
DRAKE_DEMAND(properties);
properties->AddProperty(internal::kHydroGroup, internal::kRezHint,
resolution_hint);
AddRigidHydroelasticProperties(properties);
}

void AddRigidHydroelasticProperties(ProximityProperties* properties) {
// Creation of a hydroelastic representation requires the existence of the
// "hydroelastic" property group. We make use of the resolution hint
// infrastructure to introduce it, but pass in a negative value to confirm
// this overload isn't used for any shape that truly cares about the
// resolution hint.
const double resolution_hint = -1.0;
AddRigidHydroelasticProperties(resolution_hint, properties);
DRAKE_DEMAND(properties);
// The bare minimum of defining a rigid geometry is to declare its compliance
// type. Downstream consumers (ProximityEngine) will determine if this is
// sufficient.
properties->AddProperty(internal::kHydroGroup, internal::kComplianceType,
internal::HydroelasticType::kRigid);
}

void AddSoftHydroelasticProperties(double resolution_hint,
ProximityProperties* properties) {
DRAKE_DEMAND(properties);
properties->AddProperty(internal::kHydroGroup, internal::kRezHint,
resolution_hint);
AddSoftHydroelasticProperties(properties);
}

void AddSoftHydroelasticProperties(ProximityProperties* properties) {
DRAKE_DEMAND(properties);
// The bare minimum of defining a soft geometry is to declare its compliance
// type. Downstream consumers (ProximityEngine) will determine if this is
// sufficient.
properties->AddProperty(internal::kHydroGroup, internal::kComplianceType,
internal::HydroelasticType::kSoft);
}

} // namespace geometry
Expand Down
35 changes: 31 additions & 4 deletions geometry/proximity_properties.h
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@
no way limit the inclusion of any other additional, arbitrary properties.
*/

#include <ostream>

#include "drake/geometry/geometry_roles.h"

namespace drake {
Expand Down Expand Up @@ -57,8 +59,28 @@ extern const char* const kHcDissipation; ///< Hunt-Crossley dissipation
*/
//@{

extern const char* const kHydroGroup; ///< Hydroelastic group name.
extern const char* const kRezHint; ///< Resolution hint property name.
extern const char* const kHydroGroup; ///< Hydroelastic group name.
extern const char* const kRezHint; ///< Resolution hint property name.
extern const char* const kComplianceType; ///< Compliance type property name.

//@}

// TODO(SeanCurtis-TRI): Update this to have an additional classification: kBoth
// when we have the need from the algorithm. For example: when we have two
// very stiff objects, we'd want to process them as soft. But when one
// very stiff and one very soft object interact, it might make sense to
// consider the stiff object as effectively rigid and simplify the computation.
// In this case, the object would get two representations.
/** Classification of the type of representation a shape has for the
hydroelastic contact model: rigid or soft. */
enum class HydroelasticType {
kUndefined,
kRigid,
kSoft
};

/** Streaming operator for writing hydroelastic type to output stream. */
std::ostream& operator<<(std::ostream& out, const HydroelasticType& type);

} // namespace internal

Expand All @@ -78,8 +100,8 @@ void AddRigidHydroelasticProperties(double resolution_hint,
ProximityProperties* properties);

/** Overload, intended for shapes that don't get tessellated in their
hydroelastic representation (e.g., HalfSpace and Mesh),
see `@ref MODULE_NOT_WRITTEN_YET`. */
hydroelastic representation (e.g., HalfSpace and Mesh).
See @ref MODULE_NOT_WRITTEN_YET. */
void AddRigidHydroelasticProperties(ProximityProperties* properties);

// TODO(SeanCurtis-TRI): Add module that explains resolution hint and reference
Expand All @@ -101,6 +123,11 @@ void AddRigidHydroelasticProperties(ProximityProperties* properties);
void AddSoftHydroelasticProperties(double resolution_hint,
ProximityProperties* properties);

/** Overload, intended for shapes that don't get tessellated in their
hydroelastic representation (e.g., HalfSpace).
See @ref MODULE_NOT_WRITTEN_YET. */
void AddSoftHydroelasticProperties(ProximityProperties* properties);

//@}

} // namespace geometry
Expand Down
3 changes: 1 addition & 2 deletions geometry/test/proximity_engine_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -59,7 +59,7 @@ class ProximityEngineTester {
}

template <typename T>
static hydroelastic::HydroelasticType hydroelastic_type(
static HydroelasticType hydroelastic_type(
GeometryId id, const ProximityEngine<T>& engine) {
return engine.hydroelastic_geometries().hydroelastic_type(id);
}
Expand All @@ -69,7 +69,6 @@ namespace {

constexpr double kInf = std::numeric_limits<double>::infinity();

using hydroelastic::HydroelasticType;
using math::RigidTransform;
using math::RigidTransformd;
using math::RollPitchYawd;
Expand Down
Loading

0 comments on commit c5e632c

Please sign in to comment.