Skip to content

Commit 8deeaa2

Browse files
sherm1sherm1
committed
Attempt to speed up I*H calculation (in M).
1 parent 37515dc commit 8deeaa2

15 files changed

+43
-37
lines changed

‎multibody/tree/body_node_impl.cc

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -798,7 +798,7 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamicsInM_TipToBase(
798798
VVector<T> tau_projection; // = Hᵀ_FM_M ⋅ F_BMo_M
799799
const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
800800
// Next line is 0 flops for revolute/prismatic, 30 flops for floating
801-
mobilizer_->calc_tau_from_M(X_FM, get_q(positions), F_BMo_M,
801+
mobilizer_->calc_tau_from_M(X_FM, get_q(positions), F_BMo_M.get_coeffs(),
802802
tau_projection.data());
803803
tau = tau_projection - tau_app; // 1-6 flops
804804
}

‎multibody/tree/body_node_impl_mass_matrix.cc

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -200,8 +200,8 @@ void BodyNodeImpl<T, ConcreteMobilizer>::
200200
auto M_diag = M->template block<kNv, kNv>(B_start_in_v, B_start_in_v);
201201

202202
for (int j = 0; j < kNv; ++j) {
203-
const SpatialForce<T>& F =
204-
*reinterpret_cast<const SpatialForce<T>*>(Fm_BMo_M.col(j).data());
203+
const Vector6<T>& F =
204+
*reinterpret_cast<const Vector6<T>*>(Fm_BMo_M.col(j).data());
205205
mobilizer_->calc_tau_from_M(X_FM, q, F, M_diag.col(j).data());
206206
}
207207

@@ -272,8 +272,8 @@ void BodyNodeImpl<T, ConcreteMobilizer>::
272272
auto M_sym = M->template block<Bnv, kNv>(B_start_in_v, P_start_in_v); \
273273
for (int j = 0; j < Bnv; ++j) { \
274274
/* Calculates tau = H_FM_Mᵀ * F_PMp_M */ \
275-
const SpatialForce<T>& F = *reinterpret_cast<const SpatialForce<T>*>( \
276-
Fm_PMp_Mp.col(j).data()); \
275+
const Vector6<T>& F = \
276+
*reinterpret_cast<const Vector6<T>*>(Fm_PMp_Mp.col(j).data()); \
277277
mobilizer_->calc_tau_from_M(X_FM, q, F, M_block.col(j).data()); \
278278
} \
279279
M_sym = M_block.transpose(); \

‎multibody/tree/curvilinear_mobilizer.cc

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -132,12 +132,12 @@ void CurvilinearMobilizer<T>::calc_tau(const T* q,
132132
template <typename T>
133133
void CurvilinearMobilizer<T>::calc_tau_from_M(const math::RigidTransform<T>&,
134134
const T* q,
135-
const SpatialForce<T>& F_BMo_M,
135+
const Vector6<T>& F_BMo_M,
136136
T* tau) const {
137137
DRAKE_ASSERT(tau != nullptr);
138138
const T& rho = curvilinear_path_.curvature(*q);
139-
const Vector3<T>& t_M = F_BMo_M.rotational();
140-
const Vector3<T>& f_M = F_BMo_M.translational();
139+
const auto t_M = F_BMo_M.template head<3>();
140+
const auto f_M = F_BMo_M.template tail<3>();
141141
tau[0] = rho * t_M[2] + f_M[0];
142142
}
143143

‎multibody/tree/curvilinear_mobilizer.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -189,7 +189,7 @@ class CurvilinearMobilizer final : public MobilizerImpl<T, 1, 1> {
189189
M frame instead of F (producing the same resulting tau). Returns
190190
tau = H_FM_Mᵀ⋅F_BMo_M (see class comments). */
191191
void calc_tau_from_M(const math::RigidTransform<T>&, const T* q,
192-
const SpatialForce<T>& F_BMo_M, T* tau) const;
192+
const Vector6<T>& F_BMo_M, T* tau) const;
193193

194194
math::RigidTransform<T> CalcAcrossMobilizerTransform(
195195
const systems::Context<T>& context) const final;

‎multibody/tree/mobilizer_impl.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -102,7 +102,7 @@ careful when interpreting them as Eigen vectors for computation purposes.
102102
103103
// Returns tau = H_FM_Mᵀ(q)⋅F_BMo_M
104104
void calc_tau_from_M(const RigidTransform<T>& X_FM, const T* q,
105-
const SpatialForce<T>& F_BMo_M, T* tau) const;
105+
const Vector6<T>& F_BMo_M, T* tau) const;
106106
107107
MobilizerImpl also provides a number of size specific methods to retrieve
108108
multibody quantities of interest from caching structures. These are common

‎multibody/tree/planar_mobilizer.h

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -215,16 +215,16 @@ class PlanarMobilizer final : public MobilizerImpl<T, 3, 3> {
215215

216216
/* Returns tau = H_FM_Mᵀ⋅F_M. See class comments. */
217217
void calc_tau_from_M(const math::RigidTransform<T>&, const T* q,
218-
const SpatialForce<T>& F_BMo_M, T* tau) const {
218+
const Vector6<T>& F_BMo_M, T* tau) const {
219219
// TODO(sherm1) These are already available in the passed-in X_FM.
220220
using std::sin, std::cos;
221221
DRAKE_ASSERT(tau != nullptr);
222222
const T s = sin(q[2]), c = cos(q[2]);
223-
const Vector3<T>& t_B_M = F_BMo_M.rotational(); // torque
224-
const Vector3<T>& f_BMo_M = F_BMo_M.translational(); // force
225-
tau[0] = c * f_BMo_M[0] - s * f_BMo_M[1]; // force along x
226-
tau[1] = s * f_BMo_M[0] + c * f_BMo_M[1]; // force along y
227-
tau[2] = t_B_M[2]; // torque about z
223+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
224+
const auto f_BMo_M = F_BMo_M.template tail<3>(); // translational (force)
225+
tau[0] = c * f_BMo_M[0] - s * f_BMo_M[1]; // force along x
226+
tau[1] = s * f_BMo_M[0] + c * f_BMo_M[1]; // force along y
227+
tau[2] = t_B_M[2]; // torque about z
228228
}
229229

230230
math::RigidTransform<T> CalcAcrossMobilizerTransform(

‎multibody/tree/prismatic_mobilizer.h

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -307,9 +307,9 @@ class PrismaticMobilizerAxial final : public PrismaticMobilizer<T> {
307307
// Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [0₃ᵀ axis_Mᵀ], and
308308
// axis_M == axis_F (see class comments).
309309
void calc_tau_from_M(const math::RigidTransform<T>&, const T*,
310-
const SpatialForce<T>& F_BMo_M, T* tau) const {
310+
const Vector6<T>& F_BMo_M, T* tau) const {
311311
DRAKE_ASSERT(tau != nullptr);
312-
const Vector3<T>& f_BMo_F = F_BMo_M.translational();
312+
const auto f_BMo_F = F_BMo_M.template tail<3>(); // translational
313313
tau[0] = f_BMo_F[axis];
314314
}
315315

‎multibody/tree/quaternion_floating_mobilizer.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -276,10 +276,12 @@ class QuaternionFloatingMobilizer final : public MobilizerImpl<T, 7, 6> {
276276

277277
// Returns tau = H_FM_Mᵀ⋅F_M. See class comments.
278278
void calc_tau_from_M(const math::RigidTransform<T>& X_FM, const T* q,
279-
const SpatialForce<T>& F_BMo_M, T* tau) const {
279+
const Vector6<T>& F_BMo_M, T* tau) const {
280280
DRAKE_ASSERT(q != nullptr && tau != nullptr);
281281
const math::RotationMatrix<T>& R_FM = X_FM.rotation();
282-
const SpatialForce<T> F_BMo_F = R_FM * F_BMo_M; // 30 flops
282+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
283+
const auto f_BMo_M = F_BMo_M.template tail<3>(); // translational (force)
284+
const SpatialForce<T> F_BMo_F(R_FM * t_B_M, R_FM * f_BMo_M); // 30 flops
283285
calc_tau(nullptr, F_BMo_F, &*tau);
284286
}
285287

‎multibody/tree/revolute_mobilizer.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -312,10 +312,10 @@ class RevoluteMobilizerAxial final : public RevoluteMobilizer<T> {
312312
// Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [axis_Mᵀ 0₃ᵀ] and
313313
// axis_M == 100, 010, or 001.
314314
void calc_tau_from_M(const math::RigidTransform<T>&, const T*,
315-
const SpatialForce<T>& F_BMo_M, T* tau) const {
315+
const Vector6<T>& F_BMo_M, T* tau) const {
316316
DRAKE_ASSERT(tau != nullptr);
317-
const Vector3<T>& t_BMo_M = F_BMo_M.rotational();
318-
tau[0] = t_BMo_M[axis];
317+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
318+
tau[0] = t_B_M[axis];
319319
}
320320

321321
private:

‎multibody/tree/rpy_ball_mobilizer.h

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -241,12 +241,12 @@ class RpyBallMobilizer final : public MobilizerImpl<T, 3, 3> {
241241

242242
// Returns tau = H_FM_Mᵀ⋅F_M. See class comments.
243243
void calc_tau_from_M(const math::RigidTransform<T>& X_FM, const T* q,
244-
const SpatialForce<T>& F_BMo_M, T* tau) const {
244+
const Vector6<T>& F_BMo_M, T* tau) const {
245245
DRAKE_ASSERT(q != nullptr && tau != nullptr);
246246
const math::RotationMatrix<T>& R_FM = X_FM.rotation();
247-
const Vector3<T>& t_BMo_M = F_BMo_M.rotational();
247+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
248248
Eigen::Map<VVector<T>> tau_as_vector(tau);
249-
tau_as_vector = R_FM * t_BMo_M; // 15 flops
249+
tau_as_vector = R_FM * t_B_M; // 15 flops
250250
}
251251

252252
math::RigidTransform<T> CalcAcrossMobilizerTransform(

‎multibody/tree/rpy_floating_mobilizer.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -291,10 +291,12 @@ class RpyFloatingMobilizer final : public MobilizerImpl<T, 6, 6> {
291291

292292
// Returns tau = H_FM_Mᵀ⋅F_M. See class comments.
293293
void calc_tau_from_M(const math::RigidTransform<T>& X_FM, const T* q,
294-
const SpatialForce<T>& F_BMo_M, T* tau) const {
294+
const Vector6<T>& F_BMo_M, T* tau) const {
295295
DRAKE_ASSERT(q != nullptr && tau != nullptr);
296296
const math::RotationMatrix<T>& R_FM = X_FM.rotation();
297-
const SpatialForce<T> F_BMo_F = R_FM * F_BMo_M; // 30 flops
297+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
298+
const auto f_BMo_M = F_BMo_M.template tail<3>(); // translational (force)
299+
const SpatialForce<T> F_BMo_F(R_FM * t_B_M, R_FM * f_BMo_M); // 30 flops
298300
calc_tau(nullptr, F_BMo_F, &*tau);
299301
}
300302

‎multibody/tree/screw_mobilizer.h

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -257,12 +257,12 @@ class ScrewMobilizer final : public MobilizerImpl<T, 1, 1> {
257257
/* Returns tau = H_FM_Mᵀ⋅F_M, where H_FM_Mᵀ = [axis_Mᵀ f⋅axis_Mᵀ] and
258258
axis_M == axis_F (see class comments). 12 flops */
259259
void calc_tau_from_M(const math::RigidTransform<T>&, const T*,
260-
const SpatialForce<T>& F_BMo_M, T* tau) const {
260+
const Vector6<T>& F_BMo_M, T* tau) const {
261261
DRAKE_ASSERT(tau != nullptr);
262262
const T f = screw_pitch_ / (2 * M_PI);
263-
const Vector3<T>& t_B_M = F_BMo_M.rotational(); // torque
264-
const Vector3<T>& f_BMo_M = F_BMo_M.translational(); // force
265-
tau[0] = axis_.dot(t_B_M) + f * axis_.dot(f_BMo_M); // This is axis_M.
263+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
264+
const auto f_BMo_M = F_BMo_M.template tail<3>(); // translational (force)
265+
tau[0] = axis_.dot(t_B_M) + f * axis_.dot(f_BMo_M); // This is axis_M.
266266
}
267267

268268
math::RigidTransform<T> CalcAcrossMobilizerTransform(

‎multibody/tree/test/curvilinear_mobilizer_test.cc

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -218,8 +218,8 @@ TEST_F(CurvilinearMobilizerTest, ProjectSpatialForce) {
218218
// Taking the force in the M frame shouldn't change the result.
219219
const SpatialForce<double> F_Mo_M = R_FM.transpose() * F_Mo_F;
220220
double tau_from_M{};
221-
mobilizer_->calc_tau_from_M(RigidTransformd() /*not used*/, &distance, F_Mo_M,
222-
&tau_from_M);
221+
mobilizer_->calc_tau_from_M(RigidTransformd() /*not used*/, &distance,
222+
F_Mo_M.get_coeffs(), &tau_from_M);
223223
EXPECT_NEAR(tau_from_M, tau_expected, 4.0 * kEpsilon);
224224
}
225225

‎multibody/tree/universal_mobilizer.h

Lines changed: 4 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -204,10 +204,12 @@ class UniversalMobilizer final : public MobilizerImpl<T, 2, 2> {
204204
// Returns tau = H_FM_Mᵀ⋅F_M.
205205
// TODO(sherm1) Just re-expressing here; could likely do better.
206206
void calc_tau_from_M(const math::RigidTransform<T>& X_FM, const T* q,
207-
const SpatialForce<T>& F_BMo_M, T* tau) const {
207+
const Vector6<T>& F_BMo_M, T* tau) const {
208208
DRAKE_ASSERT(q != nullptr && tau != nullptr);
209209
const math::RotationMatrix<T>& R_FM = X_FM.rotation();
210-
const SpatialForce<T> F_BMo_F = R_FM * F_BMo_M;
210+
const auto t_B_M = F_BMo_M.template head<3>(); // rotational (torque)
211+
const auto f_BMo_M = F_BMo_M.template tail<3>(); // translational (force)
212+
const SpatialForce<T> F_BMo_F(R_FM * t_B_M, R_FM * f_BMo_M); // 30 flops
211213
return calc_tau(q, F_BMo_F, &*tau);
212214
}
213215

‎multibody/tree/weld_mobilizer.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -112,7 +112,7 @@ class WeldMobilizer final : public MobilizerImpl<T, 0, 0> {
112112
// Does nothing since there are no taus.
113113
void calc_tau(const T*, const SpatialForce<T>&, T*) const {}
114114
void calc_tau_from_M(const math::RigidTransform<T>&, const T*,
115-
const SpatialForce<T>&, T*) const {}
115+
const Vector6<T>&, T*) const {}
116116

117117
math::RigidTransform<T> CalcAcrossMobilizerTransform(
118118
const systems::Context<T>&) const final;

0 commit comments

Comments
 (0)