Skip to content

Commit 8d89fc4

Browse files
author
Paul Mitiguy
committed
Add function name to error message when there is a singularity.
1 parent 28c0c00 commit 8d89fc4

File tree

2 files changed

+19
-6
lines changed

2 files changed

+19
-6
lines changed

multibody/tree/rpy_ball_mobilizer.cc

Lines changed: 17 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -132,15 +132,17 @@ void RpyBallMobilizer<T>::ProjectSpatialForce(
132132
}
133133

134134
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 {
136137
throw std::runtime_error(fmt::format(
137-
"The RpyBallMobilizer (implementing a BallRpyJoint) between "
138+
"{}(): The RpyBallMobilizer (implementing a BallRpyJoint) between "
138139
"body {} and body {} has reached a singularity. This occurs when the "
139140
"pitch angle takes values near π/2 + kπ, ∀ k ∈ ℤ. At the current "
140141
"configuration, we have pitch = {}. Drake does not yet support a "
141142
"comparable joint using quaternions, but the feature request is "
142143
"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));
144146
}
145147

146148
template <typename T>
@@ -162,7 +164,10 @@ void RpyBallMobilizer<T>::DoCalcNMatrix(const systems::Context<T>& context,
162164
using std::sin;
163165
const Vector3<T> angles = get_angles(context);
164166
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+
}
166171
const T sp = sin(angles[1]);
167172
const T sy = sin(angles[2]);
168173
const T cy = cos(angles[2]);
@@ -229,7 +234,10 @@ void RpyBallMobilizer<T>::DoCalcNDotMatrix(const systems::Context<T>& context,
229234
const T sp = sin(angles[1]);
230235
const T sy = sin(angles[2]);
231236
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+
}
233241
const T cpi = 1.0 / cp;
234242
const T cpiSqr = cpi * cpi;
235243

@@ -343,7 +351,10 @@ void RpyBallMobilizer<T>::DoMapVelocityToQDot(
343351
const T cp = cos(angles[1]);
344352
const T sy = sin(angles[2]);
345353
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+
}
347358
const T cpi = 1.0 / cp;
348359

349360
// Although we can calculate q̇ = N(q) * v, it is more efficient to implicitly

multibody/tree/rpy_ball_mobilizer.h

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -308,6 +308,8 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
308308
// Certain roll pitch yaw calculations (e.g., calculating the N(q) matrix)
309309
// have a singularity (divide-by-zero error) when cos(pitch) ≈ 0.
310310
void ThrowSinceCosPitchIsNearZero(const T& pitch) const;
311+
void ThrowSinceCosPitchIsNearZero(const T& pitch,
312+
const char* function_name) const;
311313

312314
private:
313315
// Helper method to make a clone templated on ToScalar.

0 commit comments

Comments
 (0)