Skip to content

Commit 8d39970

Browse files
jwnimmer-trijwnimmer-tri
authored
Revert "MakeMobilizerForJoint() can now create Frames..." (#22688)
This reverts commit 246ed15.
1 parent c86befd commit 8d39970

31 files changed

+284
-408
lines changed

‎multibody/plant/multibody_plant.cc

+44-32
Original file line numberDiff line numberDiff line change
@@ -3895,8 +3895,8 @@ void MultibodyPlant<T>::CalcReactionForces(
38953895
// for the discrete solvers we have today, though it might change for future
38963896
// solvers that prefer an implicit evaluation of these terms.
38973897
// TODO(amcastro-tri): Consider having a
3898-
// DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs
3899-
// this computation consistently with its discrete update.
3898+
// DiscreteUpdateManager::EvalReactionForces() to ensure the manager performs
3899+
// this computation consistently with its discrete update.
39003900
const VectorX<T>& vdot = this->EvalForwardDynamics(context).get_vdot();
39013901
std::vector<SpatialAcceleration<T>> A_WB_vector(num_bodies());
39023902
std::vector<SpatialForce<T>> F_BMo_W_vector(num_bodies());
@@ -3908,9 +3908,9 @@ void MultibodyPlant<T>::CalcReactionForces(
39083908
// Since vdot is the result of Fapplied and tau_applied we expect the result
39093909
// from inverse dynamics to be zero.
39103910
// TODO(amcastro-tri): find a better estimation for this bound. For instance,
3911-
// we can make an estimation based on the trace of the mass matrix (Jain
3912-
// 2011, Eq. 4.21). For now we only ASSERT though with a better estimation we
3913-
// could promote this to a DEMAND.
3911+
// we can make an estimation based on the trace of the mass matrix (Jain 2011,
3912+
// Eq. 4.21). For now we only ASSERT though with a better estimation we could
3913+
// promote this to a DEMAND.
39143914
// TODO(amcastro-tri) Uncomment this line once issue #12473 is resolved.
39153915
// DRAKE_ASSERT(tau_id.norm() <
39163916
// 100 * num_velocities() *
@@ -3924,49 +3924,61 @@ void MultibodyPlant<T>::CalcReactionForces(
39243924
internal_tree().get_joint_mobilizer(joint_index);
39253925
const internal::Mobilizer<T>& mobilizer =
39263926
internal_tree().get_mobilizer(mobilizer_index);
3927-
3928-
// Reversed means the joint's parent(child) body is the outboard(inboard)
3929-
// body for the mobilizer.
3930-
const bool is_reversed = mobilizer.mobod().is_reversed();
3927+
const internal::MobodIndex mobod_index = mobilizer.mobod().index();
39313928

39323929
// F_BMo_W is the mobilizer reaction force on mobilized body B at the origin
39333930
// Mo of the mobilizer's outboard frame M, expressed in the world frame W.
3934-
const SpatialForce<T>& F_BMo_W = F_BMo_W_vector[mobilizer_index];
3935-
3936-
// But the quantity of interest, F_CJc_Jc, is the joint's reaction force on
3937-
// the joint's child body C at the joint's child frame Jc, expressed in Jc.
3938-
SpatialForce<T>& F_CJc_Jc = output->at(joint.ordinal());
3931+
const SpatialForce<T>& F_BMo_W = F_BMo_W_vector[mobod_index];
39393932

39403933
// Frames of interest:
3934+
const Frame<T>& frame_Jp = joint.frame_on_parent();
39413935
const Frame<T>& frame_Jc = joint.frame_on_child();
3942-
const Frame<T>& frame_M = mobilizer.outboard_frame();
3936+
const FrameIndex F_index = mobilizer.inboard_frame().index();
3937+
const FrameIndex M_index = mobilizer.outboard_frame().index();
3938+
const FrameIndex Jp_index = frame_Jp.index();
3939+
const FrameIndex Jc_index = frame_Jc.index();
3940+
3941+
// In Drake we must have either:
3942+
// - Jp == F and Jc == M (typical case)
3943+
// - Jp == M and Jc == F (mobilizer is reversed from joint)
3944+
DRAKE_DEMAND((Jp_index == F_index && Jc_index == M_index) ||
3945+
(Jp_index == M_index && Jc_index == F_index));
3946+
3947+
// Mobilizer is reversed if the joint's parent frame Jp is the mobilizer's
3948+
// outboard frame M.
3949+
const bool is_reversed = (Jp_index == M_index);
39433950

39443951
// We'll need this in both cases below since we're required to report
39453952
// the reaction force expressed in the joint's child frame Jc.
39463953
const RotationMatrix<T> R_JcW =
39473954
frame_Jc.CalcRotationMatrixInWorld(context).inverse();
39483955

3949-
if (&frame_M == &frame_Jc) {
3950-
// This is the easy case. Just need to re-express.
3951-
F_CJc_Jc = R_JcW * F_BMo_W;
3952-
continue;
3953-
}
3956+
// The quantity of interest, F_CJc_Jc, is the joint's reaction force on the
3957+
// joint's child body C at the joint's child frame Jc, expressed in Jc.
3958+
SpatialForce<T>& F_CJc_Jc = output->at(joint.ordinal());
3959+
if (!is_reversed) {
3960+
F_CJc_Jc = R_JcW * F_BMo_W; // The typical case: Mo==Jc and B==C.
3961+
} else {
3962+
// For this reversed case, F_BMo_W is the reaction on the joint's _parent_
3963+
// body at Jp, expressed in W.
3964+
const SpatialForce<T>& F_PJp_W = F_BMo_W; // Reversed: Mo==Jp and B==P.
39543965

3955-
// If the mobilizer is reversed, Newton's 3ʳᵈ law (action/reaction) (and
3956-
// knowing Drake's joints are massless) says the force on the child at M
3957-
// is equal and opposite to the force on the parent at M.
3958-
const SpatialForce<T> F_CMo_W = is_reversed ? -F_BMo_W : F_BMo_W;
3959-
const SpatialForce<T> F_CMo_Jc = R_JcW * F_CMo_W; // Reexpress in Jc.
3966+
// Newton's 3ʳᵈ law (action/reaction) (and knowing Drake's joints are
3967+
// massless) says the force on the child _at Jp_ is equal and opposite to
3968+
// the force on the parent at Jp.
3969+
const SpatialForce<T> F_CJp_W = -F_PJp_W;
3970+
const SpatialForce<T> F_CJp_Jc = R_JcW * F_CJp_W; // Reexpress in Jc.
39603971

3961-
// However, the reaction force we want to report on the child is at Jc,
3962-
// not M. We need to shift the application point from Mo to Jco.
3972+
// However, the reaction force we want to report on the child is at Jc,
3973+
// not Jp. We need to shift the application point from Jp to Jc.
39633974

3964-
// Find the shift vector p_MoJco_Jc (= -p_JcoMo_Jc).
3965-
const RigidTransform<T> X_JcM = frame_M.CalcPose(context, frame_Jc);
3966-
const Vector3<T> p_MoJco_Jc = -X_JcM.translation();
3975+
// Find the shift vector p_JpJc_Jc (= -p_JcJp_Jc).
3976+
const RigidTransform<T> X_JcJp = frame_Jp.CalcPose(context, frame_Jc);
3977+
const Vector3<T> p_JpJc_Jc = -X_JcJp.translation();
39673978

3968-
// Perform the M->Jc shift.
3969-
F_CJc_Jc = F_CMo_Jc.Shift(p_MoJco_Jc);
3979+
// Perform the Jp->Jc shift.
3980+
F_CJc_Jc = F_CJp_Jc.Shift(p_JpJc_Jc);
3981+
}
39703982
}
39713983
}
39723984

‎multibody/plant/test/sap_driver_multidof_joints_test.cc

+4-6
Original file line numberDiff line numberDiff line change
@@ -92,15 +92,13 @@ class MultiDofJointWithLimits final : public Joint<T> {
9292
int do_get_position_start() const override { return 0; }
9393

9494
std::unique_ptr<Mobilizer<T>> MakeMobilizerForJoint(
95-
const SpanningForest::Mobod& mobod,
96-
MultibodyTree<T>* tree) const override {
97-
DRAKE_DEMAND(tree != nullptr); // Just a sanity check; we don't need it.
95+
const SpanningForest::Mobod& mobod) const override {
9896
const auto [inboard_frame, outboard_frame] =
9997
this->tree_frames(mobod.is_reversed());
10098
// TODO(sherm1) The mobilizer needs to be reversed, not just the frames.
101-
// The only restriction here relevant for these tests is that we provide a
102-
// mobilizer with kNumDofs positions and velocities, so that indexes are
103-
// consistent during MultibodyPlant::Finalize().
99+
// The only restriction here relevant for these tests is that we provide a
100+
// mobilizer with kNumDofs positions and velocities, so that indexes are
101+
// consistent during MultibodyPlant::Finalize().
104102
auto revolute_mobilizer = std::make_unique<internal::RpyBallMobilizer<T>>(
105103
mobod, *inboard_frame, *outboard_frame);
106104
return revolute_mobilizer;

‎multibody/rational/rational_forward_kinematics.cc

+14-7
Original file line numberDiff line numberDiff line change
@@ -179,11 +179,10 @@ RationalForwardKinematics::Pose<T> RationalForwardKinematics::
179179
template <typename T>
180180
RationalForwardKinematics::Pose<T>
181181
RationalForwardKinematics::CalcWeldJointChildBodyPose(
182-
const math::RigidTransformd& X_PF, const math::RigidTransformd& X_MC,
183-
const Pose<T>& X_AP) const {
184-
// X_FM is always identity for a Weld mobilizer.
185-
const Matrix3<double> R_FM = Matrix3<double>::Identity();
186-
const Vector3<double> p_FM = Vector3<double>::Zero();
182+
const math::RigidTransformd& X_FM, const math::RigidTransformd& X_PF,
183+
const math::RigidTransformd& X_MC, const Pose<T>& X_AP) const {
184+
const Matrix3<double> R_FM = X_FM.rotation().matrix();
185+
const Vector3<double> p_FM = X_FM.translation();
187186
const Matrix3<T>& R_AP = X_AP.rotation;
188187
const Vector3<T>& p_AP = X_AP.position;
189188
return CalcChildPose(R_AP, p_AP, X_PF, X_MC, R_FM, p_FM);
@@ -288,9 +287,17 @@ RationalForwardKinematics::CalcChildBodyPoseAsMultilinearPolynomial(
288287
return CalcPrismaticJointChildLinkPose(axis_F, X_PF, X_MC, X_AP,
289288
q_star(s_index), s_[s_index]);
290289
} else if (IsWeld(*mobilizer)) {
291-
return CalcWeldJointChildBodyPose(X_PF, X_MC, X_AP);
290+
const internal::WeldMobilizer<double>* weld_mobilizer =
291+
static_cast<const internal::WeldMobilizer<double>*>(mobilizer);
292+
math::RigidTransformd X_FM;
293+
if (!is_order_reversed) {
294+
X_FM = weld_mobilizer->get_X_FM();
295+
} else {
296+
X_FM = weld_mobilizer->get_X_FM().inverse();
297+
}
298+
return CalcWeldJointChildBodyPose(X_FM, X_PF, X_MC, X_AP);
292299
}
293-
// Successful construction guarantees that all supported mobilizers are
300+
// Successful construction guarantess that all supported mobilizers are
294301
// handled.
295302
DRAKE_UNREACHABLE();
296303
}

‎multibody/rational/rational_forward_kinematics.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -233,7 +233,8 @@ class RationalForwardKinematics {
233233
// Computes the pose of the body, connected to its parent body through a
234234
// weld joint.
235235
template <typename T>
236-
Pose<T> CalcWeldJointChildBodyPose(const math::RigidTransformd& X_PF,
236+
Pose<T> CalcWeldJointChildBodyPose(const math::RigidTransformd& X_FM,
237+
const math::RigidTransformd& X_PF,
237238
const math::RigidTransformd& X_MC,
238239
const Pose<T>& X_AP) const;
239240

‎multibody/tree/ball_rpy_joint.cc

+1-2
Original file line numberDiff line numberDiff line change
@@ -71,8 +71,7 @@ std::unique_ptr<Joint<T>> BallRpyJoint<T>::DoShallowClone() const {
7171
// in the header file.
7272
template <typename T>
7373
std::unique_ptr<internal::Mobilizer<T>> BallRpyJoint<T>::MakeMobilizerForJoint(
74-
const internal::SpanningForest::Mobod& mobod,
75-
internal::MultibodyTree<T>*) const {
74+
const internal::SpanningForest::Mobod& mobod) const {
7675
const auto [inboard_frame, outboard_frame] =
7776
this->tree_frames(mobod.is_reversed());
7877
// TODO(sherm1) The mobilizer needs to be reversed, not just the frames.

‎multibody/tree/ball_rpy_joint.h

+16-17
Original file line numberDiff line numberDiff line change
@@ -71,9 +71,9 @@ class BallRpyJoint final : public Joint<T> {
7171
DRAKE_THROW_UNLESS(damping >= 0);
7272
}
7373

74-
~BallRpyJoint() final;
74+
~BallRpyJoint() override;
7575

76-
const std::string& type_name() const final;
76+
const std::string& type_name() const override;
7777

7878
/// Returns `this` joint's default damping constant in N⋅m⋅s. The damping
7979
/// torque (in N⋅m) is modeled as `τ = -damping⋅ω`, i.e. opposing motion, with
@@ -191,7 +191,7 @@ class BallRpyJoint final : public Joint<T> {
191191
/// Adding forces per-dof makes no physical sense. Therefore, this method
192192
/// throws an exception if invoked.
193193
void DoAddInOneForce(const systems::Context<T>&, int, const T&,
194-
MultibodyForces<T>*) const final {
194+
MultibodyForces<T>*) const override {
195195
throw std::logic_error(
196196
"Ball RPY joints do not allow applying forces to individual degrees of "
197197
"freedom.");
@@ -203,7 +203,7 @@ class BallRpyJoint final : public Joint<T> {
203203
/// viscous law `τ = -d⋅ω`, with d the damping coefficient (see
204204
/// default_damping()).
205205
void DoAddInDamping(const systems::Context<T>& context,
206-
MultibodyForces<T>* forces) const final {
206+
MultibodyForces<T>* forces) const override {
207207
Eigen::Ref<VectorX<T>> t_BMo_F =
208208
get_mobilizer().get_mutable_generalized_forces_from_array(
209209
&forces->mutable_generalized_forces());
@@ -212,48 +212,47 @@ class BallRpyJoint final : public Joint<T> {
212212
}
213213

214214
private:
215-
int do_get_velocity_start() const final {
215+
int do_get_velocity_start() const override {
216216
return get_mobilizer().velocity_start_in_v();
217217
}
218218

219-
int do_get_num_velocities() const final { return 3; }
219+
int do_get_num_velocities() const override { return 3; }
220220

221-
int do_get_position_start() const final {
221+
int do_get_position_start() const override {
222222
return get_mobilizer().position_start_in_q();
223223
}
224224

225-
int do_get_num_positions() const final { return 3; }
225+
int do_get_num_positions() const override { return 3; }
226226

227-
std::string do_get_position_suffix(int index) const final {
227+
std::string do_get_position_suffix(int index) const override {
228228
return get_mobilizer().position_suffix(index);
229229
}
230230

231-
std::string do_get_velocity_suffix(int index) const final {
231+
std::string do_get_velocity_suffix(int index) const override {
232232
return get_mobilizer().velocity_suffix(index);
233233
}
234234

235235
void do_set_default_positions(
236-
const VectorX<double>& default_positions) final {
236+
const VectorX<double>& default_positions) override {
237237
if (this->has_mobilizer()) {
238238
get_mutable_mobilizer().set_default_position(default_positions);
239239
}
240240
}
241241

242242
// Joint<T> overrides:
243243
std::unique_ptr<internal::Mobilizer<T>> MakeMobilizerForJoint(
244-
const internal::SpanningForest::Mobod& mobod,
245-
internal::MultibodyTree<T>* tree) const final;
244+
const internal::SpanningForest::Mobod& mobod) const override;
246245

247246
std::unique_ptr<Joint<double>> DoCloneToScalar(
248-
const internal::MultibodyTree<double>& tree_clone) const final;
247+
const internal::MultibodyTree<double>& tree_clone) const override;
249248

250249
std::unique_ptr<Joint<AutoDiffXd>> DoCloneToScalar(
251-
const internal::MultibodyTree<AutoDiffXd>& tree_clone) const final;
250+
const internal::MultibodyTree<AutoDiffXd>& tree_clone) const override;
252251

253252
std::unique_ptr<Joint<symbolic::Expression>> DoCloneToScalar(
254-
const internal::MultibodyTree<symbolic::Expression>&) const final;
253+
const internal::MultibodyTree<symbolic::Expression>&) const override;
255254

256-
std::unique_ptr<Joint<T>> DoShallowClone() const final;
255+
std::unique_ptr<Joint<T>> DoShallowClone() const override;
257256

258257
// Make BallRpyJoint templated on every other scalar type a friend of
259258
// BallRpyJoint<T> so that CloneToScalar<ToAnyOtherScalar>() can access

‎multibody/tree/curvilinear_joint.cc

+1-2
Original file line numberDiff line numberDiff line change
@@ -101,8 +101,7 @@ std::unique_ptr<Joint<T>> CurvilinearJoint<T>::DoShallowClone() const {
101101
template <typename T>
102102
std::unique_ptr<internal::Mobilizer<T>>
103103
CurvilinearJoint<T>::MakeMobilizerForJoint(
104-
const internal::SpanningForest::Mobod& mobod,
105-
internal::MultibodyTree<T>*) const {
104+
const internal::SpanningForest::Mobod& mobod) const {
106105
const auto [inboard_frame, outboard_frame] =
107106
this->tree_frames(mobod.is_reversed());
108107
// TODO(sherm1) The mobilizer needs to be reversed, not just the frames.

‎multibody/tree/curvilinear_joint.h

+18-19
Original file line numberDiff line numberDiff line change
@@ -121,9 +121,9 @@ class CurvilinearJoint final : public Joint<T> {
121121
curvilinear_path,
122122
double pos_lower_limit, double pos_upper_limit, double damping = 0);
123123

124-
~CurvilinearJoint() final;
124+
~CurvilinearJoint() override;
125125

126-
const std::string& type_name() const final;
126+
const std::string& type_name() const override;
127127

128128
/** Returns `this` joint's default damping constant in N⋅s/m. */
129129
double default_damping() const { return this->default_damping_vector()[0]; }
@@ -269,7 +269,7 @@ class CurvilinearJoint final : public Joint<T> {
269269
@see The public NVI AddInOneForce() for details. */
270270
void DoAddInOneForce(const systems::Context<T>&, int joint_dof,
271271
const T& joint_tau,
272-
MultibodyForces<T>* forces) const final {
272+
MultibodyForces<T>* forces) const override {
273273
DRAKE_DEMAND(joint_dof == 0);
274274
Eigen::Ref<VectorX<T>> tau_mob =
275275
get_mobilizer().get_mutable_generalized_forces_from_array(
@@ -288,63 +288,62 @@ class CurvilinearJoint final : public Joint<T> {
288288
@param[out] forces The MultibodyForces object to which the damping force is
289289
added. */
290290
void DoAddInDamping(const systems::Context<T>& context,
291-
MultibodyForces<T>* forces) const final {
291+
MultibodyForces<T>* forces) const override {
292292
const T damping_force =
293293
-this->GetDamping(context) * get_tangential_velocity(context);
294294
AddInForce(context, damping_force, forces);
295295
}
296296

297297
private:
298-
int do_get_velocity_start() const final {
298+
int do_get_velocity_start() const override {
299299
return get_mobilizer().velocity_start_in_v();
300300
}
301301

302-
int do_get_num_velocities() const final { return 1; }
302+
int do_get_num_velocities() const override { return 1; }
303303

304-
int do_get_position_start() const final {
304+
int do_get_position_start() const override {
305305
return get_mobilizer().position_start_in_q();
306306
}
307307

308-
int do_get_num_positions() const final { return 1; }
308+
int do_get_num_positions() const override { return 1; }
309309

310-
std::string do_get_position_suffix(int index) const final {
310+
std::string do_get_position_suffix(int index) const override {
311311
return get_mobilizer().position_suffix(index);
312312
}
313313

314-
std::string do_get_velocity_suffix(int index) const final {
314+
std::string do_get_velocity_suffix(int index) const override {
315315
return get_mobilizer().velocity_suffix(index);
316316
}
317317

318318
void do_set_default_positions(
319-
const VectorX<double>& default_positions) final {
319+
const VectorX<double>& default_positions) override {
320320
if (this->has_mobilizer()) {
321321
get_mutable_mobilizer().set_default_position(default_positions);
322322
}
323323
}
324324

325-
const T& DoGetOnePosition(const systems::Context<T>& context) const final {
325+
const T& DoGetOnePosition(const systems::Context<T>& context) const override {
326326
return get_distance(context);
327327
}
328328

329-
const T& DoGetOneVelocity(const systems::Context<T>& context) const final {
329+
const T& DoGetOneVelocity(const systems::Context<T>& context) const override {
330330
return get_tangential_velocity(context);
331331
}
332332

333333
// Joint<T> overrides:
334334
std::unique_ptr<internal::Mobilizer<T>> MakeMobilizerForJoint(
335-
const internal::SpanningForest::Mobod& mobod,
336-
internal::MultibodyTree<T>* tree) const final;
335+
const internal::SpanningForest::Mobod& mobod) const override;
337336

338337
std::unique_ptr<Joint<double>> DoCloneToScalar(
339-
const internal::MultibodyTree<double>& tree_clone) const final;
338+
const internal::MultibodyTree<double>& tree_clone) const override;
340339

341340
std::unique_ptr<Joint<AutoDiffXd>> DoCloneToScalar(
342-
const internal::MultibodyTree<AutoDiffXd>& tree_clone) const final;
341+
const internal::MultibodyTree<AutoDiffXd>& tree_clone) const override;
343342

344343
std::unique_ptr<Joint<symbolic::Expression>> DoCloneToScalar(
345-
const internal::MultibodyTree<symbolic::Expression>&) const final;
344+
const internal::MultibodyTree<symbolic::Expression>&) const override;
346345

347-
std::unique_ptr<Joint<T>> DoShallowClone() const final;
346+
std::unique_ptr<Joint<T>> DoShallowClone() const override;
348347

349348
// Make CurvilinearJoint templated on every other scalar type a friend of
350349
// CurvilinearJoint<T> so that CloneToScalar<ToAnyOtherScalar>() can access

0 commit comments

Comments
 (0)