4
4
5
5
#include " drake/common/eigen_types.h"
6
6
#include " drake/common/test_utilities/eigen_matrix_compare.h"
7
+ #include " drake/common/test_utilities/expect_no_throw.h"
7
8
#include " drake/common/test_utilities/expect_throws_message.h"
8
9
#include " drake/math/rigid_transform.h"
9
10
#include " drake/math/rotation_matrix.h"
@@ -125,6 +126,15 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
125
126
EXPECT_TRUE (CompareMatrices (Nplus_x_N, Matrix3d::Identity (), kTolerance ,
126
127
MatrixCompareType::relative));
127
128
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
+
128
138
// Set a generic angular velocity.
129
139
const Vector3<double > v (0.5 , -0.7 , 2.3 );
130
140
mobilizer_->SetAngularVelocity (context_.get (), v);
@@ -147,6 +157,30 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
147
157
MatrixCompareType::relative));
148
158
EXPECT_TRUE (CompareMatrices (NDot_Nplus, -N_NplusDot, kTolerance ,
149
159
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.*" );
150
184
}
151
185
152
186
TEST_F (RpyBallMobilizerTest, MapUsesN) {
0 commit comments