From 5022e3832e90f988f1b6d3015b2794ea26e527b1 Mon Sep 17 00:00:00 2001 From: Jeremy Nimmer Date: Tue, 16 Nov 2021 14:18:09 -0800 Subject: [PATCH] [math] Improve printing of symbolic RollPitchYaw and RigidTransform (#16097) --- math/rigid_transform.cc | 15 +----- math/roll_pitch_yaw.cc | 35 +++++++++++-- math/roll_pitch_yaw.h | 12 +---- math/test/rigid_transform_test.cc | 83 +++++++++++++++++-------------- math/test/roll_pitch_yaw_test.cc | 19 ++++--- 5 files changed, 92 insertions(+), 72 deletions(-) diff --git a/math/rigid_transform.cc b/math/rigid_transform.cc index 5b34c9afc36d..7be1022ed010 100644 --- a/math/rigid_transform.cc +++ b/math/rigid_transform.cc @@ -5,20 +5,9 @@ namespace math { template std::ostream& operator<<(std::ostream& out, const RigidTransform& X) { + const RollPitchYaw rpy(X.rotation()); const Vector3& p = X.translation(); - if constexpr (scalar_predicate::is_bool) { - const RotationMatrix& R = X.rotation(); - const RollPitchYaw rpy(R); - out << rpy; - out << fmt::format(" xyz = {} {} {}", p.x(), p.y(), p.z());; - } else { - // TODO(14927) For symbolic type T, stream roll, pitch, yaw if conversion - // from RotationMatrix to RollPitchYaw can be done in a way that provides - // meaningful output to the end-user or developer (it is not trivial how - // to do this symbolic conversion) and does not require an Environment. - out << "rpy = symbolic (not supported)"; - out << " xyz = " << p.x() << " " << p.y() << " " << p.z(); - } + out << fmt::format("{} xyz = {} {} {}", rpy, p.x(), p.y(), p.z());; return out; } diff --git a/math/roll_pitch_yaw.cc b/math/roll_pitch_yaw.cc index 64910559bf24..18bf01efbc73 100644 --- a/math/roll_pitch_yaw.cc +++ b/math/roll_pitch_yaw.cc @@ -5,6 +5,7 @@ #include #include +#include "drake/common/cond.h" #include "drake/math/rotation_matrix.h" namespace drake { @@ -141,10 +142,10 @@ Vector3 CalcRollPitchYawFromQuaternionAndRotationMatrix( T q3 = zA + zB; // Third angle in rotation sequence. // If necessary, modify angles q1 and/or q3 to be between -pi and pi. - if (q1 > M_PI) q1 = q1 - 2 * M_PI; - if (q1 < -M_PI) q1 = q1 + 2 * M_PI; - if (q3 > M_PI) q3 = q3 - 2 * M_PI; - if (q3 < -M_PI) q3 = q3 + 2 * M_PI; + q1 = if_then_else(q1 > M_PI, q1 - 2 * M_PI, q1); + q1 = if_then_else(q1 < -M_PI, q1 + 2 * M_PI, q1); + q3 = if_then_else(q3 > M_PI, q3 - 2 * M_PI, q3); + q3 = if_then_else(q3 < -M_PI, q3 + 2 * M_PI, q3); // Return in Drake/ROS conventional SpaceXYZ q1, q2, q3 (roll-pitch-yaw) order // (which is equivalent to BodyZYX q3, q2, q1 order). @@ -175,7 +176,8 @@ void RollPitchYaw::SetFromQuaternionAndRotationMatrix( constexpr double kEpsilon = std::numeric_limits::epsilon(); const RotationMatrix R_quaternion(quaternion); constexpr double tolerance = 20 * kEpsilon; - if (!R_quaternion.IsNearlyEqualTo(R, tolerance)) { + if (scalar_predicate::is_bool && + !R_quaternion.IsNearlyEqualTo(R, tolerance)) { std::string message = fmt::format("RollPitchYaw::{}():" " An element of the RotationMatrix R passed to this method differs by" " more than {:G} from the corresponding element of the RotationMatrix" @@ -231,6 +233,29 @@ void RollPitchYaw::ThrowPitchAngleViolatesGimbalLockTolerance( throw std::runtime_error(message); } +template +std::ostream& operator<<(std::ostream& out, const RollPitchYaw& rpy) { + // Helper to represent an angle as a terse string. If the angle is symbolic + // and ends up a string that's too long, return a placeholder instead. + auto repr = [](const T& angle) { + std::string result = fmt::format("{}", angle); + if (std::is_same_v && (result.size() >= 30)) { + result = ""; + } + return result; + }; + const T& roll = rpy.roll_angle(); + const T& pitch = rpy.pitch_angle(); + const T& yaw = rpy.yaw_angle(); + out << fmt::format("rpy = {} {} {}", repr(roll), repr(pitch), repr(yaw)); + return out; +} + +DRAKE_DEFINE_FUNCTION_TEMPLATE_INSTANTIATIONS_ON_DEFAULT_SCALARS(( + static_cast&)>( + &operator<< ) +)) + } // namespace math } // namespace drake diff --git a/math/roll_pitch_yaw.h b/math/roll_pitch_yaw.h index 6cf05c25765e..39966926bc83 100644 --- a/math/roll_pitch_yaw.h +++ b/math/roll_pitch_yaw.h @@ -655,17 +655,7 @@ class RollPitchYaw { /// `std::ostream`. Especially useful for debugging. /// @relates RollPitchYaw. template -inline std::ostream& operator<<(std::ostream& out, const RollPitchYaw& rpy) { - const T& roll = rpy.roll_angle(); - const T& pitch = rpy.pitch_angle(); - const T& yaw = rpy.yaw_angle(); - if constexpr (scalar_predicate::is_bool) { - out << fmt::format("rpy = {} {} {}", roll, pitch, yaw); - } else { - out << "rpy = " << roll << " " << pitch << " " << yaw; - } - return out; -} +std::ostream& operator<<(std::ostream& out, const RollPitchYaw& rpy); /// Abbreviation (alias/typedef) for a RollPitchYaw double scalar type. /// @relates RollPitchYaw diff --git a/math/test/rigid_transform_test.cc b/math/test/rigid_transform_test.cc index 7d7380444bfb..e87f683b22e4 100644 --- a/math/test/rigid_transform_test.cc +++ b/math/test/rigid_transform_test.cc @@ -14,6 +14,9 @@ namespace { using Eigen::Matrix3d; using Eigen::Vector3d; +using symbolic::Expression; +using symbolic::Formula; +using symbolic::Variable; constexpr double kEpsilon = std::numeric_limits::epsilon(); @@ -452,7 +455,6 @@ GTEST_TEST(RigidTransform, OperatorMultiplyByRigidTransform) { EXPECT_TRUE(will_be_X_CA.IsNearlyEqualTo(X_CA_expected, 32 * kEpsilon)); // Repeat both tests for the non-double implementations. - using symbolic::Expression; const RigidTransform X_BAx = X_BA.cast(); const RigidTransform X_CBx = X_CB.cast(); const RigidTransform X_CAx = X_CBx * X_BAx; @@ -481,8 +483,6 @@ GTEST_TEST(RigidTransform, InvertAndCompose) { EXPECT_TRUE(X_AC.IsNearlyEqualTo(X_AC_expected, 32 * kEpsilon)); // Now check the implementation for T ≠ double. - using symbolic::Expression; - const RigidTransform X_BAx = X_BA.cast(); const RigidTransform X_BCx = X_BC.cast(); const RigidTransform X_ACx = X_BAx.InvertAndCompose(X_BCx); @@ -530,72 +530,72 @@ GTEST_TEST(RigidTransform, CastFromDoubleToAutoDiffXd) { } // Verify RigidTransform is compatible with symbolic::Expression. This includes, -// construction and methods involving Bool specialized for symbolic::Expression, -// namely: IsExactlyIdentity(), IsIdentityToEpsilon(), IsNearlyEqualTo(). +// construction and methods involving Bool specialized for Expression, namely: +// IsExactlyIdentity(), IsIdentityToEpsilon(), IsNearlyEqualTo(). GTEST_TEST(RigidTransform, SymbolicRigidTransformSimpleTests) { - // Test RigidTransform can be constructed with symbolic::Expression. - RigidTransform X; + // Test RigidTransform can be constructed with Expression. + RigidTransform X; - // Test IsExactlyIdentity() nominally works with symbolic::Expression. - symbolic::Formula test_Bool = X.IsExactlyIdentity(); + // Test IsExactlyIdentity() nominally works with Expression. + Formula test_Bool = X.IsExactlyIdentity(); EXPECT_TRUE(test_Bool); - // Test IsIdentityToEpsilon() nominally works with symbolic::Expression. + // Test IsIdentityToEpsilon() nominally works with Expression. test_Bool = X.IsIdentityToEpsilon(kEpsilon); EXPECT_TRUE(test_Bool); - // Test IsExactlyEqualTo() nominally works for symbolic::Expression. - const RigidTransform& X_built_in_identity = - RigidTransform::Identity(); + // Test IsExactlyEqualTo() nominally works for Expression. + const RigidTransform& X_built_in_identity = + RigidTransform::Identity(); test_Bool = X.IsExactlyEqualTo(X_built_in_identity); EXPECT_TRUE(test_Bool); - // Test IsNearlyEqualTo() nominally works for symbolic::Expression. + // Test IsNearlyEqualTo() nominally works for Expression. test_Bool = X.IsNearlyEqualTo(X_built_in_identity, kEpsilon); EXPECT_TRUE(test_Bool); // Now perform the same tests on a non-identity transform. - const Vector3 p_symbolic(1, 2, 3); + const Vector3 p_symbolic(1, 2, 3); X.set_translation(p_symbolic); - // Test IsExactlyIdentity() works with symbolic::Expression. + // Test IsExactlyIdentity() works with Expression. test_Bool = X.IsExactlyIdentity(); EXPECT_FALSE(test_Bool); - // Test IsIdentityToEpsilon() works with symbolic::Expression. + // Test IsIdentityToEpsilon() works with Expression. test_Bool = X.IsIdentityToEpsilon(kEpsilon); EXPECT_FALSE(test_Bool); - // Test IsExactlyEqualTo() works for symbolic::Expression. + // Test IsExactlyEqualTo() works for Expression. test_Bool = X.IsExactlyEqualTo(X_built_in_identity); EXPECT_FALSE(test_Bool); - // Test IsNearlyEqualTo() works for symbolic::Expression. + // Test IsNearlyEqualTo() works for Expression. test_Bool = X.IsNearlyEqualTo(X_built_in_identity, kEpsilon); EXPECT_FALSE(test_Bool); } // Test that symbolic conversions may throw exceptions. GTEST_TEST(RigidTransform, SymbolicRigidTransformThrowsExceptions) { - const symbolic::Variable x("x"); // Arbitrary variable. - Matrix3 m_symbolic; + const Variable x("x"); // Arbitrary variable. + Matrix3 m_symbolic; m_symbolic << 1, 0, 0, 0, 1, 0, 0, 0, x; // Not necessarily an identity matrix. - RotationMatrix R_symbolic(m_symbolic); - const Vector3 p_symbolic(0, 0, 0); - const RigidTransform X_symbolic(R_symbolic, p_symbolic); + RotationMatrix R_symbolic(m_symbolic); + const Vector3 p_symbolic(0, 0, 0); + const RigidTransform X_symbolic(R_symbolic, p_symbolic); // The next four tests should throw exceptions since the tests are // inconclusive because the value of x is unknown. - symbolic::Formula test_Bool = X_symbolic.IsExactlyIdentity(); + Formula test_Bool = X_symbolic.IsExactlyIdentity(); EXPECT_THROW(test_Bool.Evaluate(), std::runtime_error); test_Bool = X_symbolic.IsIdentityToEpsilon(kEpsilon); EXPECT_THROW(test_Bool.Evaluate(), std::runtime_error); - const RigidTransform& X_identity = - RigidTransform::Identity(); + const RigidTransform& X_identity = + RigidTransform::Identity(); test_Bool = X_symbolic.IsExactlyEqualTo(X_identity); EXPECT_THROW(test_Bool.Evaluate(), std::runtime_error); @@ -797,10 +797,11 @@ void VerifyStreamInsertionOperator(const RigidTransform X_AB, // “rpy = 0.12499999999999997 0.25 0.4999999999999999 xyz = 4.0 3.0 2.0 std::stringstream stream; stream << X_AB; const std::string stream_string = stream.str(); - EXPECT_EQ(stream_string.find("rpy = "), 0); + ASSERT_EQ(stream_string.find("rpy = "), 0); const char* cstring = stream_string.c_str() + 6; // Start of double value. double roll, pitch, yaw; - sscanf(cstring, "%lf %lf %lf ", &roll, &pitch, &yaw); + int match_count = sscanf(cstring, "%lf %lf %lf ", &roll, &pitch, &yaw); + ASSERT_EQ(match_count, 3) << "When scanning " << stream_string; EXPECT_TRUE(CompareMatrices(Vector3(roll, pitch, yaw), rpy_expected, 4 * kEpsilon)); @@ -837,19 +838,27 @@ GTEST_TEST(RigidTransform, StreamInsertionOperator) { RigidTransform(rpy_autodiff, xyz_autodiff), rpy_values, xyz_expected_string); - // Test stream insertion for RigidTransform. + // Test stream insertion for RigidTransform. // Note: A numerical process calculates RollPitchYaw from a RotationMatrix. // Verify that RigidTransform prints a symbolic placeholder for its rotational - // component (roll-pitch-yaw string) when T = symbolic::Expression. - const symbolic::Variable x("x"), y("y"), z("z"); - const symbolic::Variable roll("roll"), pitch("pitch"), yaw("yaw"); - const Vector3 xyz_symbolic(x, y, z); - RollPitchYaw rpy_symbolic(roll, pitch, yaw); - RigidTransform X_symbolic(rpy_symbolic, xyz_symbolic); + // component (roll-pitch-yaw string) when T = Expression. + const Variable x("x"), y("y"), z("z"); + const Variable roll("roll"), pitch("pitch"), yaw("yaw"); + const Vector3 xyz_symbolic(x, y, z); + RollPitchYaw rpy_symbolic(roll, pitch, yaw); + RigidTransform X_symbolic(rpy_symbolic, xyz_symbolic); std::stringstream stream; stream << X_symbolic; const std::string expected_string = - "rpy = symbolic (not supported) xyz = x y z"; + "rpy = xyz = x y z"; EXPECT_EQ(expected_string, stream.str()); + + // Test stream insertion for RigidTransform when the expression + // is only constants (i.e., with no free variables). + const RollPitchYaw rpy_const_expr(-0.1, 0.2, 0.3); + const Vector3 xyz_const_expr(-10, 20, 30); + VerifyStreamInsertionOperator( + RigidTransform(rpy_const_expr, xyz_const_expr), + Vector3d{-0.1, 0.2, 0.3}, "xyz = -10 20 30"); } } // namespace diff --git a/math/test/roll_pitch_yaw_test.cc b/math/test/roll_pitch_yaw_test.cc index a22c9156eafc..601dc7ff83d5 100644 --- a/math/test/roll_pitch_yaw_test.cc +++ b/math/test/roll_pitch_yaw_test.cc @@ -16,6 +16,8 @@ namespace { using Eigen::Vector3d; using Eigen::Matrix3d; +using symbolic::Expression; +using symbolic::Variable; const double kEpsilon = std::numeric_limits::epsilon(); @@ -432,9 +434,6 @@ GTEST_TEST(RollPitchYaw, CalcRpyDDtFromAngularAccel) { // symbolic::Expression. We focus only on methods that required tweaks in // order to compile against Expression. GTEST_TEST(RollPitchYaw, SymbolicTest) { - using symbolic::Expression; - using symbolic::Variable; - const Variable r1("r1"); const Variable r2("r2"); const Variable p1("p1"); @@ -487,11 +486,19 @@ GTEST_TEST(RollPitchYaw, StreamInsertionOperator) { EXPECT_EQ(rpy_expected_string, streamB.str()); // Test stream insertion for RollPitchYaw. - const symbolic::Variable r("roll"), p("pitch"), y("yaw"); - const RollPitchYaw rpy_symbolic(r, p, y); + const symbolic::Variable r("foo"), p("bar"), y("baz"); + const RollPitchYaw rpy_symbolic(r, p, y); std::stringstream streamC; streamC << rpy_symbolic; - rpy_expected_string = "rpy = roll pitch yaw"; + rpy_expected_string = "rpy = foo bar baz"; EXPECT_EQ(rpy_expected_string, streamC.str()); + + // When the expression strings are very long, they will be truncated. + const Expression big_expr = sqrt(pow(r, 2) + pow(p, 2) + pow(y, 2)); + const RollPitchYaw rpy_symbolic_huge( + big_expr, big_expr, big_expr); + std::stringstream streamD; streamD << rpy_symbolic_huge; + rpy_expected_string = "rpy = "; + EXPECT_EQ(rpy_expected_string, streamD.str()); }