Skip to content

Commit 0f047e8

Browse files
committed
Inverse Dynamics in M proto + cassiebench test
1 parent 0c9e80d commit 0f047e8

29 files changed

+1194
-64
lines changed

multibody/benchmarking/cassie.cc

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -146,6 +146,17 @@ class Cassie : public benchmark::Fixture {
146146
}
147147
}
148148

149+
// Runs the InverseDynamicsInM benchmark.
150+
// NOLINTNEXTLINE(runtime/references)
151+
void DoInverseDynamicsInM(benchmark::State& state) {
152+
DRAKE_DEMAND(want_grad_u(state) == false);
153+
for (auto _ : state) {
154+
InvalidateState();
155+
plant_->CalcInverseDynamicsInM(*context_, desired_vdot_,
156+
external_forces_);
157+
}
158+
}
159+
149160
// Runs the ForwardDynamics benchmark.
150161
// NOLINTNEXTLINE(runtime/references)
151162
void DoForwardDynamics(benchmark::State& state) {
@@ -327,6 +338,14 @@ BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics)
327338
->Unit(benchmark::kMicrosecond)
328339
->Arg(kWantNoGrad);
329340

341+
// NOLINTNEXTLINE(runtime/references)
342+
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamicsInM)(benchmark::State& state) {
343+
DoInverseDynamicsInM(state);
344+
}
345+
BENCHMARK_REGISTER_F(CassieDouble, InverseDynamicsInM)
346+
->Unit(benchmark::kMicrosecond)
347+
->Arg(kWantNoGrad);
348+
330349
// NOLINTNEXTLINE(runtime/references)
331350
BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
332351
DoForwardDynamics(state);
@@ -383,6 +402,22 @@ BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
383402
->Arg(kWantGradV|kWantGradVdot)
384403
->Arg(kWantGradX|kWantGradVdot);
385404

405+
BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamicsInM)
406+
// NOLINTNEXTLINE(runtime/references)
407+
(benchmark::State& state) {
408+
DoInverseDynamicsInM(state);
409+
}
410+
BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamicsInM)
411+
->Unit(benchmark::kMicrosecond)
412+
->Arg(kWantNoGrad)
413+
->Arg(kWantGradQ)
414+
->Arg(kWantGradV)
415+
->Arg(kWantGradX)
416+
->Arg(kWantGradVdot)
417+
->Arg(kWantGradQ|kWantGradVdot)
418+
->Arg(kWantGradV|kWantGradVdot)
419+
->Arg(kWantGradX|kWantGradVdot);
420+
386421
// NOLINTNEXTLINE(runtime/references)
387422
BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) {
388423
DoForwardDynamics(state);

multibody/math/spatial_acceleration.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -134,7 +134,7 @@ class SpatialAcceleration : public SpatialVector<SpatialAcceleration, T> {
134134
const Vector3<T>& alpha_MB_E = this->rotational();
135135
// Calculate point Co's translational acceleration measured in M.
136136
Vector3<T>& a_MCo_E = this->translational();
137-
a_MCo_E += (alpha_MB_E.cross(p_BoCo_E)
137+
a_MCo_E += (alpha_MB_E.cross(p_BoCo_E) // 33 flops total
138138
+ w_MB_E.cross(w_MB_E.cross(p_BoCo_E)));
139139
}
140140

multibody/plant/multibody_plant.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4036,6 +4036,14 @@ class MultibodyPlant : public internal::MultibodyTreeSystem<T> {
40364036
external_forces);
40374037
}
40384038

4039+
VectorX<T> CalcInverseDynamicsInM(
4040+
const systems::Context<T>& context, const VectorX<T>& known_vdot,
4041+
const MultibodyForces<T>& external_forces) const {
4042+
this->ValidateContext(context);
4043+
return internal_tree().CalcInverseDynamicsInM(context, known_vdot,
4044+
external_forces);
4045+
}
4046+
40394047
#ifdef DRAKE_DOXYGEN_CXX
40404048
// MultibodyPlant uses the NVI implementation of
40414049
// CalcImplicitTimeDerivativesResidual from

multibody/plant/test/external_forces_test.cc

Lines changed: 8 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,12 @@
11
#include <limits>
22

3-
#include <gmock/gmock.h>
43
#include <gtest/gtest.h>
54

6-
#include "drake/common/autodiff.h"
5+
#include "drake/common/fmt_eigen.h"
76
#include "drake/common/test_utilities/eigen_matrix_compare.h"
87
#include "drake/common/test_utilities/limit_malloc.h"
98
#include "drake/multibody/plant/test/kuka_iiwa_model_tests.h"
109
#include "drake/multibody/tree/frame.h"
11-
#include "drake/multibody/tree/rigid_body.h"
1210

1311
namespace drake {
1412

@@ -41,6 +39,9 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
4139
end_effector_link_->body_frame(), &forces);
4240
const VectorX<double> tau_id =
4341
plant_->CalcInverseDynamics(*context_, vdot, forces);
42+
const VectorX<double> tau_id_in_m =
43+
plant_->CalcInverseDynamicsInM(*context_, vdot, forces);
44+
4445
{ // Repeat the computation to confirm the heap behavior. We allow the
4546
// method to heap-allocate 2 temporaries and 1 return value.
4647
drake::test::LimitMalloc guard({.max_num_allocations = 3});
@@ -75,6 +76,10 @@ TEST_F(KukaIiwaModelTests, ExternalBodyForces) {
7576
const double kTolerance = 50 * std::numeric_limits<double>::epsilon();
7677
EXPECT_TRUE(CompareMatrices(tau_id, tau_id_expected, kTolerance,
7778
MatrixCompareType::relative));
79+
// W & M frame ID should be very close.
80+
EXPECT_TRUE(CompareMatrices(tau_id_in_m, tau_id,
81+
16 * std::numeric_limits<double>::epsilon(),
82+
MatrixCompareType::relative));
7883
}
7984

8085
TEST_F(KukaIiwaModelTests, BodyForceApi) {

multibody/tree/body_node.h

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -242,6 +242,10 @@ class BodyNode : public MultibodyElement<T> {
242242
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
243243
PositionKinematicsCache<T>* pc) const = 0;
244244

245+
virtual void CalcPositionKinematicsCacheInM_BaseToTip(
246+
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
247+
PositionKinematicsCacheInM<T>* pcm) const = 0;
248+
245249
// Calculates the hinge matrix H_PB_W, the `6 x nm` hinge matrix that relates
246250
// V_PB_W`(body B's spatial velocity in its parent body P, expressed in world
247251
// W) to this node's nm generalized velocities (or mobilities) v_B as
@@ -291,6 +295,10 @@ class BodyNode : public MultibodyElement<T> {
291295
const std::vector<Vector6<T>>& H_PB_W_cache, const T* velocities,
292296
VelocityKinematicsCache<T>* vc) const = 0;
293297

298+
virtual void CalcVelocityKinematicsCacheInM_BaseToTip(
299+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
300+
const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const = 0;
301+
294302
// The CalcMassMatrix() algorithm invokes this on each body k, serving
295303
// as the composite body R(k) in the outer loop of Jain's algorithm 9.3.
296304
// This node must fill in its nv x nv diagonal block in M, and then
@@ -374,6 +382,12 @@ class BodyNode : public MultibodyElement<T> {
374382
const VelocityKinematicsCache<T>* vc, const T* accelerations,
375383
std::vector<SpatialAcceleration<T>>* A_WB_array) const = 0;
376384

385+
virtual void CalcSpatialAccelerationInM_BaseToTip(
386+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
387+
const T* velocities, const VelocityKinematicsCacheInM<T>& vcm,
388+
const T* accelerations,
389+
std::vector<SpatialAcceleration<T>>* A_WM_M_array) const = 0;
390+
377391
// Computes the generalized forces `tau` for a single BodyNode.
378392
// This method is used by MultibodyTree within a tip-to-base loop to compute
379393
// the vector of generalized forces `tau` that would correspond with a known
@@ -433,6 +447,17 @@ class BodyNode : public MultibodyElement<T> {
433447
std::vector<SpatialForce<T>>* F_BMo_W_array,
434448
EigenPtr<VectorX<T>> tau_array) const = 0;
435449

450+
virtual void CalcInverseDynamicsInM_TipToBase(
451+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // M_BMo_M, X_BM
452+
const T* positions,
453+
const PositionKinematicsCacheInM<T>& pc, // X_MpM, X_WB
454+
const VelocityKinematicsCacheInM<T>& vc, // V_WM_M
455+
const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
456+
const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array, // Bo, W !
457+
const Eigen::Ref<const VectorX<T>>& tau_applied_array,
458+
std::vector<SpatialForce<T>>* F_BMo_M_array,
459+
EigenPtr<VectorX<T>> tau_array) const = 0;
460+
436461
// This method is used by MultibodyTree within a tip-to-base loop to compute
437462
// this node's articulated body inertia quantities that depend only on the
438463
// generalized positions.

multibody/tree/body_node_impl.cc

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -119,6 +119,41 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcPositionKinematicsCache_BaseToTip(
119119
p_PoBo_W = R_WP * p_PoBo_P;
120120
}
121121

122+
// Calculate and save X_FM, X_MpM, X_WM
123+
template <typename T, class ConcreteMobilizer>
124+
void BodyNodeImpl<T, ConcreteMobilizer>::
125+
CalcPositionKinematicsCacheInM_BaseToTip(
126+
const FrameBodyPoseCache<T>& frame_body_pose_cache, const T* positions,
127+
PositionKinematicsCacheInM<T>* pcm) const {
128+
DRAKE_ASSERT(pcm != nullptr);
129+
const MobodIndex index = mobod_index();
130+
DRAKE_ASSERT(index != world_mobod_index());
131+
132+
const T* q = get_q(positions);
133+
134+
// Calculate and store X_FM.
135+
math::RigidTransform<T>& X_FM = pcm->get_mutable_X_FM(mobod_index());
136+
X_FM = mobilizer_->calc_X_FM(q);
137+
138+
// Get precalculated frame info.
139+
// This mobilizer's inboard frame, fixed on inboard (parent) body.
140+
const FrameIndex index_F = inboard_frame().index();
141+
const bool X_MpF_is_identity =
142+
frame_body_pose_cache.is_X_MbF_identity(index_F);
143+
const math::RigidTransform<T>& X_MpF =
144+
frame_body_pose_cache.get_X_MbF(index_F);
145+
146+
// Calculate and store X_MpM.
147+
math::RigidTransform<T>& X_MpM = pcm->get_mutable_X_MpM(mobod_index());
148+
X_MpM = X_MpF_is_identity ? X_FM : X_MpF * X_FM; // 0 or 63 flops
149+
150+
// Calculate and store X_WM.
151+
// Inboard's M frame (Mp) pose is known since we're going base to tip.
152+
const math::RigidTransform<T>& X_WMp = pcm->get_X_WM(inboard_mobod_index());
153+
math::RigidTransform<T>& X_WM = pcm->get_mutable_X_WM(mobod_index());
154+
X_WM = X_WMp * X_MpM; // 63 flops
155+
}
156+
122157
// TODO(sherm1) Consider combining this with VelocityCache computation
123158
// so that we don't have to make a separate pass. Or better, get rid of this
124159
// computation altogether by working in better frames.
@@ -282,6 +317,43 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcVelocityKinematicsCache_BaseToTip(
282317
get_mutable_V_WB(vc) = V_WP.ComposeWithMovingFrameVelocity(p_PB_W, V_PB_W);
283318
}
284319

320+
// Calculate V_FM_M and V_WM_M.
321+
template <typename T, class ConcreteMobilizer>
322+
void BodyNodeImpl<T, ConcreteMobilizer>::
323+
CalcVelocityKinematicsCacheInM_BaseToTip(
324+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
325+
const T* velocities, VelocityKinematicsCacheInM<T>* vcm) const {
326+
DRAKE_ASSERT(vcm != nullptr);
327+
const MobodIndex index = mobod_index();
328+
DRAKE_ASSERT(index != world_mobod_index());
329+
330+
// Generalized coordinates local to this node's mobilizer.
331+
const T* q = get_q(positions);
332+
const T* v = get_v(velocities);
333+
334+
const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
335+
336+
SpatialVelocity<T>& V_FM_M = vcm->get_mutable_V_FM_M(index);
337+
V_FM_M = mobilizer_->calc_V_FM_M(X_FM, q, v); // 3 flops: revolute/prismatic
338+
// 30 flops: floating
339+
340+
const math::RigidTransform<T>& X_MpM = pcm.get_X_MpM(index);
341+
const Vector3<T>& p_MpM_Mp = X_MpM.translation(); // Shift vector
342+
const math::RotationMatrix<T> R_MMp = X_MpM.rotation().inverse();
343+
344+
// We're going base to tip so we know the inboard body's M-frame velocity.
345+
const SpatialVelocity<T>& V_WMp_Mp = vcm->get_V_WM_M(inboard_mobod_index());
346+
347+
// Let Mc be a frame fixed to parent body P but coincident with the child
348+
// body's M frame. Then this is the spatial velocity of P, shifted to Mc,
349+
// but expressed in the Mp frame.
350+
const SpatialVelocity<T> V_WMc_Mp = V_WMp_Mp.Shift(p_MpM_Mp); // 15 flops
351+
352+
SpatialVelocity<T>& V_WM_M = vcm->get_mutable_V_WM_M(index);
353+
V_WM_M = R_MMp * V_WMc_Mp // 6 SIMD flops
354+
+ V_FM_M; // 6 flops
355+
}
356+
285357
// As a guideline for developers, a summary of the computations performed in
286358
// this method is provided:
287359
// Notation:
@@ -411,6 +483,60 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAcceleration_BaseToTip(
411483
V_PB_W, A_PB_W);
412484
}
413485

486+
template <typename T, class ConcreteMobilizer>
487+
void BodyNodeImpl<T, ConcreteMobilizer>::CalcSpatialAccelerationInM_BaseToTip(
488+
const T* positions, const PositionKinematicsCacheInM<T>& pcm,
489+
const T* velocities, const VelocityKinematicsCacheInM<T>& vcm,
490+
const T* accelerations,
491+
std::vector<SpatialAcceleration<T>>* A_WM_M_array) const {
492+
const MobodIndex index = mobod_index(); // mobod B
493+
494+
// Collect the inputs.
495+
const math::RigidTransform<T>& X_MpM = pcm.get_X_MpM(index);
496+
const SpatialVelocity<T>& V_WM_M = vcm.get_V_WM_M(index);
497+
const T* q = get_q(positions);
498+
const T* v = get_v(velocities);
499+
const T* vdot = get_v(accelerations);
500+
// Inboard (parent) body's M frame spatial velocity and spatial acceleration
501+
// in World (already computed in base-to-tip ordering).
502+
const SpatialVelocity<T>& V_WMp_Mp = vcm.get_V_WM_M(inboard_mobod_index());
503+
const SpatialAcceleration<T>& A_WMp_Mp =
504+
(*A_WM_M_array)[inboard_mobod_index()];
505+
506+
// This is going to be the output.
507+
SpatialAcceleration<T>& A_WM_M = (*A_WM_M_array)[index];
508+
509+
// Shift and re-express parent contribution.
510+
const Vector3<T>& p_MpM_Mp = X_MpM.translation();
511+
const math::RotationMatrix<T> R_MMp = X_MpM.rotation().inverse();
512+
const Vector3<T>& w_WP_Mp = V_WMp_Mp.rotational(); // all frames on P same ω
513+
514+
// Start with parent contribution. shift: 33 flops, reexpress: 6 SIMD flops
515+
A_WM_M = R_MMp * A_WMp_Mp.Shift(p_MpM_Mp, w_WP_Mp); // parent contribution
516+
517+
// Next add in the velocity-dependent bias acceleration:
518+
// Ab_M = ω_WP x ω_FM note: ω_WF == ω_WP
519+
// 2 * ω_WP x v_FM
520+
const SpatialVelocity<T>& V_FM_M = vcm.get_V_FM_M(index);
521+
const Vector3<T>& w_FM_M = V_FM_M.rotational();
522+
const Vector3<T>& v_FM_M = V_FM_M.translational();
523+
const Vector3<T>& w_WM_M = V_WM_M.rotational();
524+
525+
// To avoid re-expressing w_WP_Mp in M, we can use this identity:
526+
// w_WM = w_WP + w_FM, so w_WP = w_WM - w_FM.
527+
const Vector3<T> w_WP_M = w_WM_M - w_FM_M; // 3 flops
528+
const SpatialAcceleration<T> Ab_WM_M(w_WP_M.cross(w_FM_M), // 12 flops
529+
2 * w_WP_M.cross(v_FM_M)); // 15 flops
530+
A_WM_M += Ab_WM_M; // 6 flops
531+
532+
// Calculate cross-mobilizer spatial acceleration.
533+
const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
534+
const SpatialAcceleration<T> A_FM_M =
535+
mobilizer_->calc_A_FM_M(X_FM, q, v, vdot); // 3 flops revolute/prismatic
536+
// 30 flops floating
537+
A_WM_M += A_FM_M; // 6 flops
538+
}
539+
414540
// As a guideline for developers, a summary of the computations performed in
415541
// this method is provided:
416542
// Notation:
@@ -586,6 +712,79 @@ void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamics_TipToBase(
586712
}
587713
}
588714

715+
template <typename T, class ConcreteMobilizer>
716+
void BodyNodeImpl<T, ConcreteMobilizer>::CalcInverseDynamicsInM_TipToBase(
717+
const FrameBodyPoseCache<T>& frame_body_pose_cache, // M_BMo_M, X_BM
718+
const T* positions,
719+
const PositionKinematicsCacheInM<T>& pcm, // X_MpM, X_WM
720+
const VelocityKinematicsCacheInM<T>& vcm, // V_WM_M
721+
const std::vector<SpatialAcceleration<T>>& A_WM_M_array,
722+
const std::vector<SpatialForce<T>>& Fapplied_Bo_W_array, // Bo, W !
723+
const Eigen::Ref<const VectorX<T>>& tau_applied_array,
724+
std::vector<SpatialForce<T>>* F_BMo_M_array,
725+
EigenPtr<VectorX<T>> tau_array) const {
726+
const MobodIndex index = mobod_index(); // mobod B
727+
728+
// Model parameters: mass properties, frame locations.
729+
const SpatialInertia<T>& M_BMo_M = frame_body_pose_cache.get_M_BMo_M(index);
730+
const T& mass = M_BMo_M.get_mass();
731+
const Vector3<T>& p_MoBcm_M = M_BMo_M.get_com();
732+
const UnitInertia<T>& G_BMo_M = M_BMo_M.get_unit_inertia();
733+
734+
const math::RigidTransform<T>& X_MB =
735+
outboard_frame().get_X_FB(frame_body_pose_cache); // F==M
736+
const Vector3<T>& p_MoBo_M = X_MB.translation();
737+
738+
// Kinematics.
739+
const math::RotationMatrix<T> R_MW = pcm.get_X_WM(index).rotation().inverse();
740+
const SpatialVelocity<T>& V_WM_M = vcm.get_V_WM_M(index);
741+
const Vector3<T>& w_WM_M = V_WM_M.rotational();
742+
const SpatialAcceleration<T>& A_WM_M = A_WM_M_array[index];
743+
744+
// Unfortunately the applied force is given at Bo and expressed in W. We
745+
// need it applied at Mo and expressed in M.
746+
const SpatialForce<T>& Fapp_BBo_W = Fapplied_Bo_W_array[index];
747+
const SpatialForce<T> Fapp_BBo_M = R_MW * Fapp_BBo_W; // 6 SIMD flops
748+
const SpatialForce<T> Fapp_BMo_M = Fapp_BBo_M.Shift(-p_MoBo_M); // 15 flops
749+
750+
// Calculate the velocity-dependent bias force on B (48 flops).
751+
const SpatialForce<T> Fbias_BMo_M(
752+
mass * w_WM_M.cross(G_BMo_M * w_WM_M), // bias moment, 27 flops
753+
mass * w_WM_M.cross(w_WM_M.cross(p_MoBcm_M))); // bias force, 21 flops
754+
755+
// F_BMo_M is an output and will be the total force on B.
756+
SpatialForce<T>& F_BMo_M = (*F_BMo_M_array)[index];
757+
F_BMo_M = M_BMo_M * A_WM_M // 45 flops
758+
+ Fbias_BMo_M - Fapp_BMo_M; // 12 flops
759+
760+
// Next, account for forces applied to B through its outboard joints, treated
761+
// as though they were locked. Since we're going tip to base we already have
762+
// the total force F_CMc_Mc on each outboard body C. 51 flops per
763+
for (const BodyNode<T>* outboard_node : child_nodes()) {
764+
const MobodIndex outboard_index = outboard_node->mobod_index();
765+
766+
// Outboard body's kinematics.
767+
const math::RigidTransform<T>& X_MMc = pcm.get_X_MpM(outboard_index);
768+
const math::RotationMatrix<T>& R_MMc = X_MMc.rotation();
769+
const Vector3<T>& p_MoMc_M = X_MMc.translation();
770+
771+
const SpatialForce<T>& F_CMc_Mc = (*F_BMo_M_array)[outboard_index];
772+
const SpatialForce<T> F_CMc_M = R_MMc * F_CMc_Mc; // 6 SIMD flops
773+
const SpatialForce<T> F_CMo_M = F_CMc_M.Shift(-p_MoMc_M); // 15 flops
774+
F_BMo_M += F_CMo_M; // 6 flops
775+
}
776+
777+
// tau is the primary output.
778+
Eigen::Map<VVector<T>> tau(get_mutable_v(tau_array->data()));
779+
const Eigen::Map<const VVector<T>> tau_app(get_v(tau_applied_array.data()));
780+
VVector<T> tau_projection; // = Hᵀ_FM_M ⋅ F_BMo_M
781+
const math::RigidTransform<T>& X_FM = pcm.get_X_FM(index);
782+
// Next line is 5 flops for revolute/prismatic, 30 flops for floating
783+
mobilizer_->calc_tau_from_M(X_FM, get_q(positions), F_BMo_M,
784+
tau_projection.data());
785+
tau = tau_projection - tau_app; // 1-6 flops
786+
}
787+
589788
template <typename T, class ConcreteMobilizer>
590789
void BodyNodeImpl<T, ConcreteMobilizer>::
591790
CalcArticulatedBodyInertiaCache_TipToBase(

0 commit comments

Comments
 (0)