Skip to content

Commit 8381e83

Browse files
author
Paul Mitiguy
committed
Fix unit test.
1 parent b6d3e54 commit 8381e83

File tree

1 file changed

+14
-9
lines changed

1 file changed

+14
-9
lines changed

multibody/tree/test/rpy_ball_mobilizer_test.cc

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -117,7 +117,7 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
117117
mobilizer_->CalcNMatrix(*context_, &N);
118118
mobilizer_->CalcNplusMatrix(*context_, &Nplus);
119119

120-
// Verify that Nplus is the inverse of N and vice-versa.
120+
// Verify that Nplus (N⁺) is the inverse of N and vice-versa.
121121
const MatrixX<double> N_x_Nplus = N * Nplus;
122122
const MatrixX<double> Nplus_x_N = Nplus * N;
123123
EXPECT_TRUE(CompareMatrices(N_x_Nplus, Matrix3d::Identity(), kTolerance,
@@ -134,14 +134,19 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
134134
mobilizer_->CalcNDotMatrix(*context_, &NDot);
135135
mobilizer_->CalcNplusDotMatrix(*context_, &NplusDot);
136136

137-
// Verify that NplusDot is the inverse of NDot and vice-versa.
138-
const MatrixX<double> NDot_NplusDot = NDot * NplusDot;
139-
const MatrixX<double> NplusDot_NDot = NplusDot * NDot;
140-
// TODO(Mitiguy) Fix these to EXPECT_TRUE.
141-
EXPECT_FALSE(CompareMatrices(NDot_NplusDot, Matrix3d::Identity(), kTolerance,
142-
MatrixCompareType::relative));
143-
EXPECT_FALSE(CompareMatrices(NplusDot_NDot, Matrix3d::Identity(), kTolerance,
144-
MatrixCompareType::relative));
137+
// The Nplus(q) matrix N⁺ multiplied by N(q) is the identity matrix [I].
138+
// Hence the time-derivative of (N⁺ * N = [I]) is Ṅ⁺ * N + N⁺ * Ṅ = [0],
139+
// where [0] is the zero matrix. Therefore Ṅ⁺ * N = -N⁺ * Ṅ.
140+
// Similarly, the time derivative of (N * N⁺ = [I]) is Ṅ * N⁺ + N * Ṅ⁺ = [0],
141+
// so Ṅ * N⁺ = -N * Ṅ⁺. Verify these relationships.
142+
const MatrixX<double> NplusDot_N = NplusDot * N;
143+
const MatrixX<double> Nplus_NDot = Nplus * NDot;
144+
const MatrixX<double> NDot_Nplus = NDot * Nplus;
145+
const MatrixX<double> N_NplusDot = N * NplusDot;
146+
EXPECT_TRUE(CompareMatrices(NplusDot_N, -Nplus_NDot, kTolerance,
147+
MatrixCompareType::relative));
148+
EXPECT_TRUE(CompareMatrices(NDot_Nplus, -N_NplusDot, kTolerance,
149+
MatrixCompareType::relative));
145150
}
146151

147152
TEST_F(RpyBallMobilizerTest, MapUsesN) {

0 commit comments

Comments
 (0)