@@ -117,7 +117,7 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
117
117
mobilizer_->CalcNMatrix (*context_, &N);
118
118
mobilizer_->CalcNplusMatrix (*context_, &Nplus);
119
119
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.
121
121
const MatrixX<double > N_x_Nplus = N * Nplus;
122
122
const MatrixX<double > Nplus_x_N = Nplus * N;
123
123
EXPECT_TRUE (CompareMatrices (N_x_Nplus, Matrix3d::Identity (), kTolerance ,
@@ -134,14 +134,19 @@ TEST_F(RpyBallMobilizerTest, KinematicMapping) {
134
134
mobilizer_->CalcNDotMatrix (*context_, &NDot);
135
135
mobilizer_->CalcNplusDotMatrix (*context_, &NplusDot);
136
136
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));
145
150
}
146
151
147
152
TEST_F (RpyBallMobilizerTest, MapUsesN) {
0 commit comments