Skip to content

Commit 57f640a

Browse files
author
Paul Mitiguy
committed
Add better tests, including testing exception messages.
1 parent e2b1adb commit 57f640a

File tree

3 files changed

+44
-2
lines changed

3 files changed

+44
-2
lines changed

multibody/tree/BUILD.bazel

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -517,6 +517,7 @@ drake_cc_googletest(
517517
":mobilizer_tester",
518518
":tree",
519519
"//common/test_utilities:eigen_matrix_compare",
520+
"//common/test_utilities:expect_no_throw",
520521
"//common/test_utilities:expect_throws_message",
521522
],
522523
)

multibody/tree/rpy_ball_mobilizer.cc

Lines changed: 9 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -138,8 +138,8 @@ void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(
138138
"{}(): The RpyBallMobilizer (implementing a BallRpyJoint) between "
139139
"body {} and body {} has reached a singularity. This occurs when the "
140140
"pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
141-
"configuration, we have pitch = {}. Drake does not yet support a "
142-
"comparable joint using quaternions, but the feature request is "
141+
"configuration, we have pitch = {} radians. Drake does not yet support "
142+
"a comparable joint using quaternions, but the feature request is "
143143
"tracked in https://github.com/RobotLocomotion/drake/issues/12404.",
144144
function_name, this->inboard_body().name(), this->outboard_body().name(),
145145
pitch));
@@ -293,6 +293,13 @@ void RpyBallMobilizer<T>::DoCalcNplusDotMatrix(
293293
const T sy = sin(angles[2]);
294294
const T cy = cos(angles[2]);
295295

296+
// Throw an exception with the proper function name if a singularity would be
297+
// encountered in DoMapVelocityToQDot().
298+
if (abs(cp) < 1.0e-3) {
299+
const char* function_name_less_Do = __func__ + 2;
300+
ThrowSinceCosPitchIsNearZero(angles[1], function_name_less_Do);
301+
}
302+
296303
// Calculate time-derivative of roll, pitch, and yaw angles.
297304
const Vector3<T> v = get_angular_velocity(context);
298305
Vector3<T> qdot;

multibody/tree/test/rpy_ball_mobilizer_test.cc

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,7 @@
44

55
#include "drake/common/eigen_types.h"
66
#include "drake/common/test_utilities/eigen_matrix_compare.h"
7+
#include "drake/common/test_utilities/expect_no_throw.h"
78
#include "drake/common/test_utilities/expect_throws_message.h"
89
#include "drake/math/rigid_transform.h"
910
#include "drake/math/rotation_matrix.h"
@@ -125,6 +126,15 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
125126
EXPECT_TRUE(CompareMatrices(Nplus_x_N, Matrix3d::Identity(), kTolerance,
126127
MatrixCompareType::relative));
127128

129+
// Also compare N(q) to numerical values produced by MotionGenesis.
130+
constexpr double epsilon64 = 64 * std::numeric_limits<double>::epsilon();
131+
MatrixX<double> Ncheck(3, 3);
132+
Ncheck.row(0) << 1.6180339887498949, 1.1755705045849463, 0;
133+
Ncheck.row(1) << -0.58778525229247314, 0.80901699437494745, 0;
134+
Ncheck.row(2) << -1.4012585384440732, -1.0180739209102541, 1;
135+
EXPECT_TRUE(
136+
CompareMatrices(N, Ncheck, epsilon64, MatrixCompareType::relative));
137+
128138
// Set a generic angular velocity.
129139
const Vector3<double> v(0.5, -0.7, 2.3);
130140
mobilizer_->SetAngularVelocity(context_.get(), v);
@@ -147,6 +157,30 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
147157
MatrixCompareType::relative));
148158
EXPECT_TRUE(CompareMatrices(NDot_Nplus, -N_NplusDot, kTolerance,
149159
MatrixCompareType::relative));
160+
161+
// Also compare Ṅ(q,q̇) to numerical values produced by MotionGenesis.
162+
MatrixX<double> NDotcheck(3, 3);
163+
NDotcheck.row(0) << -0.30720756492922385, 5.4924345293948527, 0;
164+
NDotcheck.row(1) << -1.8704654739876834, -1.358972714017757, 0;
165+
NDotcheck.row(2) << -0.42987052164155548, -5.2622033631883367, 0;
166+
EXPECT_TRUE(
167+
CompareMatrices(NDot, NDotcheck, epsilon64, MatrixCompareType::relative));
168+
169+
// Ensure cos(pitch) ≈ 0, throws an exception.
170+
const double pitch = M_PI / 2 + epsilon64;
171+
const Vector3d rpy_singular_value(M_PI / 3, pitch, -M_PI / 5);
172+
mobilizer_->SetAngles(context_.get(), rpy_singular_value);
173+
DRAKE_EXPECT_THROWS_MESSAGE(mobilizer_->CalcNMatrix(*context_, &N),
174+
"CalcNMatrix\\(\\): The RpyBallMobilizer .*"
175+
"has reached a singularity.*");
176+
DRAKE_EXPECT_NO_THROW(mobilizer_->CalcNplusMatrix(*context_, &Nplus));
177+
DRAKE_EXPECT_THROWS_MESSAGE(mobilizer_->CalcNDotMatrix(*context_, &NDot),
178+
"CalcNDotMatrix\\(\\): The RpyBallMobilizer .*"
179+
"has reached a singularity.*");
180+
DRAKE_EXPECT_THROWS_MESSAGE(
181+
mobilizer_->CalcNplusDotMatrix(*context_, &NplusDot),
182+
"CalcNplusDotMatrix\\(\\): The RpyBallMobilizer .*"
183+
"has reached a singularity.*");
150184
}
151185

152186
TEST_F(RpyBallMobilizerTest, MapUsesN) {

0 commit comments

Comments
 (0)