@@ -132,15 +132,17 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
132
132
}
133
133
134
134
template <typename T>
135
- void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(const T& pitch) const {
135
+ void RpyBallMobilizer<T>::ThrowSinceCosPitchIsNearZero(
136
+ const T& pitch, const char * function_name) const {
136
137
throw std::runtime_error (fmt::format (
137
- " The RpyBallMobilizer (implementing a BallRpyJoint) between "
138
+ " {}(): The RpyBallMobilizer (implementing a BallRpyJoint) between "
138
139
" body {} and body {} has reached a singularity. This occurs when the "
139
140
" pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
140
141
" configuration, we have pitch = {}. Drake does not yet support a "
141
142
" comparable joint using quaternions, but the feature request is "
142
143
" tracked in https://github.com/RobotLocomotion/drake/issues/12404." ,
143
- this ->inboard_body ().name (), this ->outboard_body ().name (), pitch));
144
+ function_name, this ->inboard_body ().name (), this ->outboard_body ().name (),
145
+ pitch));
144
146
}
145
147
146
148
template <typename T>
@@ -162,7 +164,10 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
162
164
using std::sin;
163
165
const Vector3<T> angles = get_angles (context);
164
166
const T cp = cos (angles[1 ]);
165
- if (abs (cp) < 1.0e-3 ) ThrowSinceCosPitchIsNearZero (angles[1 ]);
167
+ if (abs (cp) < 1.0e-3 ) {
168
+ const char * function_name_less_Do = __func__ + 2 ;
169
+ ThrowSinceCosPitchIsNearZero (angles[1 ], function_name_less_Do);
170
+ }
166
171
const T sp = sin (angles[1 ]);
167
172
const T sy = sin (angles[2 ]);
168
173
const T cy = cos (angles[2 ]);
@@ -229,7 +234,10 @@ void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
229
234
const T sp = sin (angles[1 ]);
230
235
const T sy = sin (angles[2 ]);
231
236
const T cy = cos (angles[2 ]);
232
- if (abs (cp) < 1.0e-3 ) ThrowSinceCosPitchIsNearZero (angles[1 ]);
237
+ if (abs (cp) < 1.0e-3 ) {
238
+ const char * function_name_less_Do = __func__ + 2 ;
239
+ ThrowSinceCosPitchIsNearZero (angles[1 ], function_name_less_Do);
240
+ }
233
241
const T cpi = 1.0 / cp;
234
242
const T cpiSqr = cpi * cpi;
235
243
@@ -343,7 +351,10 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
343
351
const T cp = cos (angles[1 ]);
344
352
const T sy = sin (angles[2 ]);
345
353
const T cy = cos (angles[2 ]);
346
- if (abs (cp) < 1.0e-3 ) ThrowSinceCosPitchIsNearZero (angles[1 ]);
354
+ if (abs (cp) < 1.0e-3 ) {
355
+ const char * function_name_less_Do = __func__ + 2 ;
356
+ ThrowSinceCosPitchIsNearZero (angles[1 ], function_name_less_Do);
357
+ }
347
358
const T cpi = 1.0 / cp;
348
359
349
360
// Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly
0 commit comments