Skip to content

Commit

Permalink
Simplify code.
Browse files Browse the repository at this point in the history
  • Loading branch information
Paul Mitiguy committed Dec 19, 2024
1 parent c3de572 commit 7d6af8a
Show file tree
Hide file tree
Showing 3 changed files with 30 additions and 39 deletions.
20 changes: 10 additions & 10 deletions multibody/tree/rotational_inertia.cc
Original file line number Diff line number Diff line change
Expand Up @@ -239,17 +239,17 @@ std::string RotationalInertia<T>::GetInvalidityReport() const {
}

template <typename T>
void RotationalInertia<T>::ThrowNotPhysicallyValid(
void RotationalInertia<T>::ThrowIfNotPhysicallyValidImpl(
const char* func_name) const {
std::string error_message = fmt::format(
"{}(): The rotational inertia\n"
"{}did not pass the test CouldBePhysicallyValid().",
func_name, *this);

// Provide additional information if a moment of inertia is non-negative
// or if moments of inertia do not satisfy the triangle inequality.
error_message += GetInvalidityReport();
throw std::logic_error(error_message);
DRAKE_DEMAND(func_name != nullptr);
const std::string reason_for_invalidity = GetInvalidityReport();
if (!reason_for_invalidity.empty()) {
const std::string error_message = fmt::format(
"{}(): The rotational inertia\n"
"{}did not pass the test CouldBePhysicallyValid().{}",
func_name, *this, reason_for_invalidity);
throw std::logic_error(error_message);
}
}

// TODO(Mitiguy) Consider using this code (or code similar to this) to write
Expand Down
41 changes: 19 additions & 22 deletions multibody/tree/rotational_inertia.h
Original file line number Diff line number Diff line change
Expand Up @@ -605,7 +605,7 @@ class RotationalInertia {
/// calculated (eigenvalue solver) or if scalar type T cannot be
/// converted to a double.
boolean<T> CouldBePhysicallyValid() const {
return boolean<T>(GetInvalidityReport().empty() == true);
return boolean<T>(GetInvalidityReport().empty());
}

/// Re-expresses `this` rotational inertia `I_BP_E` in place to `I_BP_A`.
Expand Down Expand Up @@ -953,11 +953,6 @@ class RotationalInertia {
return moment_max <= epsilon && product_max <= epsilon;
}

// Returns an error string if `this` RotationalInertia is verifiably invalid
// or else returns an empty string (e.g., if unable to test validity because
// the type T underlying this method is symbolic).
std::string GetInvalidityReport() const;

// Tests whether each moment of inertia is non-negative (to within ε) and
// tests whether moments of inertia satisfy the triangle-inequality.
// The triangle-inequality test requires ε when the sum of two moments are
Expand All @@ -982,32 +977,34 @@ class RotationalInertia {
return Ixx + epsilon >= 0 && Iyy + epsilon >= 0 && Izz + epsilon >= 0;
}

// ==========================================================================
// The following set of methods, ThrowIfSomeCondition(), are used within
// assertions or demands. We do not try to attempt a smart way throw based on
// a given symbolic::Formula but instead we make these methods a no-throw
// for non-numeric types.
// Returns an error string if `this` RotationalInertia is verifiably invalid
// or else returns an empty string (an empty string does not _guarantee_
// validity). For numerical type T, validity includes tests that principal
// moments of inertia (eigenvalues) are positive and satisfy the triangle
// inequality. For symbolic type T, tests are rudimentary (e.g., test for NaN
// moments or products of inertia).
std::string GetInvalidityReport() const;

// Throw an exception if GetInvalidityReport() returns an error string.
void ThrowIfNotPhysicallyValidImpl(const char* func_name) const;

// This method is used to demand the physical validity of a RotationalInertia
// at either construction or after an operation that could lead to
// non-physical results when a user provides data that is not valid. For
// numerical T-types this would imply computing the rotational inertia
// eigenvalues and checking if they are positive and satisfy the triangle
// inequality.
// SFINAE for numeric types.
template <typename T1 = T>
typename std::enable_if_t<scalar_predicate<T1>::is_bool>
ThrowIfNotPhysicallyValid(const char* func_name) {
DRAKE_DEMAND(func_name != nullptr);
if (!CouldBePhysicallyValid()) ThrowNotPhysicallyValid(func_name);
ThrowIfNotPhysicallyValidImpl(func_name);
}

// SFINAE for non-numeric types. See documentation in the implementation for
// numeric types.
// SFINAE for non-numeric types -- does nothing;
template <typename T1 = T>
typename std::enable_if_t<!scalar_predicate<T1>::is_bool>
ThrowIfNotPhysicallyValid(const char*) {}

[[noreturn]] void ThrowNotPhysicallyValid(const char* func_name) const;
// ==========================================================================
// The following set of methods, ThrowIfSomeCondition(), are used within
// assertions or demands. We do not try to attempt a smart way throw based on
// a given symbolic::Formula but instead we make these methods a no-throw
// for non-numeric types.

// Throws an exception if a rotational inertia is multiplied by a negative
// number - which implies that the resulting rotational inertia is invalid.
Expand Down
8 changes: 1 addition & 7 deletions multibody/tree/test/rotational_inertia_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -131,13 +131,7 @@ GTEST_TEST(RotationalInertia, MakeFromMomentsAndProductsOfInertia) {

// Check for a thrown exception with proper error message when creating a
// rotational inertia with NaN moments/products of inertia.
std::string expected_message =
"MakeFromMomentsAndProductsOfInertia\\(\\): The rotational inertia\n"
"\\[nan 0 0\\]\n"
"\\[ 0 13 0\\]\n"
"\\[ 0 0 10\\]\n"
"did not pass the test CouldBePhysicallyValid\\(\\)\\.\n"
"NaN detected in RotationalInertia\\.";
std::string expected_message = "[^]*NaN detected in RotationalInertia\\.";
constexpr double nan = std::numeric_limits<double>::quiet_NaN();
DRAKE_EXPECT_THROWS_MESSAGE(
RotationalInertia<double>::MakeFromMomentsAndProductsOfInertia(
Expand Down

0 comments on commit 7d6af8a

Please sign in to comment.