Skip to content

Commit 46f7589

Browse files
sherm1sherm1
committed
Inverse Dynamics in M proto + cassiebench test
1 parent 3fba171 commit 46f7589

30 files changed

+1349
-172
lines changed

‎multibody/benchmarking/cassie.cc

Lines changed: 137 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -25,21 +25,19 @@ using systems::System;
2525
// In the benchmark case instantiations at the bottom of this file, we'll use
2626
// a bitmask for the case's "Arg" to denote which quantities are in scope as
2727
// either gradients (for T=AutoDiffXd) or variables (for T=Expression).
28-
constexpr int kWantNoGrad = 0x0;
29-
constexpr int kWantGradQ = 0x1;
30-
constexpr int kWantGradV = 0x2;
31-
constexpr int kWantGradX = kWantGradQ | kWantGradV;
28+
constexpr int kWantNoGrad = 0x0;
29+
constexpr int kWantGradQ = 0x1;
30+
constexpr int kWantGradV = 0x2;
31+
constexpr int kWantGradX = kWantGradQ | kWantGradV;
3232
constexpr int kWantGradVdot = 0x4;
33-
constexpr int kWantGradU = 0x8;
33+
constexpr int kWantGradU = 0x8;
3434

3535
// Fixture that holds a Cassie robot model and offers helper functions to
3636
// configure the benchmark case.
3737
template <typename T>
3838
class Cassie : public benchmark::Fixture {
3939
public:
40-
Cassie() {
41-
tools::performance::AddMinMaxStatistics(this);
42-
}
40+
Cassie() { tools::performance::AddMinMaxStatistics(this); }
4341

4442
void SetUp(benchmark::State& state) override {
4543
SetUpNonZeroState();
@@ -89,24 +87,28 @@ class Cassie : public benchmark::Fixture {
8987
// those would get computed once and re-used (like in real applications) but
9088
// with caching off they would get recalculated repeatedly, affecting the
9189
// timing results.
92-
void InvalidateInput() {
93-
input_.GetMutableData();
94-
}
95-
void InvalidateState() {
96-
context_->NoteContinuousStateChange();
97-
}
90+
void InvalidateInput() { input_.GetMutableData(); }
91+
void InvalidateState() { context_->NoteContinuousStateChange(); }
9892

9993
// Runs the PositionKinematics benchmark.
10094
// NOLINTNEXTLINE(runtime/references)
10195
void DoPositionKinematics(benchmark::State& state) {
10296
DRAKE_DEMAND(want_grad_vdot(state) == false);
10397
DRAKE_DEMAND(want_grad_u(state) == false);
104-
// A distal body. Asking for pose of one body calculates poses of all
105-
// of them in the PositionKinematicsCache.
106-
const RigidBody<T>& toe_right = plant_->GetBodyByName("toe_right");
10798
for (auto _ : state) {
10899
InvalidateState();
109-
plant_->EvalBodyPoseInWorld(*context_, toe_right);
100+
plant_->EvalPositionKinematics(*context_);
101+
}
102+
}
103+
104+
// Runs the PositionKinematicsInM benchmark.
105+
// NOLINTNEXTLINE(runtime/references)
106+
void DoPositionKinematicsInM(benchmark::State& state) {
107+
DRAKE_DEMAND(want_grad_vdot(state) == false);
108+
DRAKE_DEMAND(want_grad_u(state) == false);
109+
for (auto _ : state) {
110+
InvalidateState();
111+
plant_->EvalPositionKinematicsInM(*context_);
110112
}
111113
}
112114

@@ -115,13 +117,22 @@ class Cassie : public benchmark::Fixture {
115117
void DoPosAndVelKinematics(benchmark::State& state) {
116118
DRAKE_DEMAND(want_grad_vdot(state) == false);
117119
DRAKE_DEMAND(want_grad_u(state) == false);
118-
// A distal body. Asking for kinematics of one body calculates it for all
119-
// of them in the PositionKinematicsCache and VelocityKinematicsCache.
120-
const RigidBody<T>& toe_right = plant_->GetBodyByName("toe_right");
121120
for (auto _ : state) {
122121
InvalidateState();
123122
// This requires both position and velocity kinematics.
124-
plant_->EvalBodySpatialVelocityInWorld(*context_, toe_right);
123+
plant_->EvalVelocityKinematics(*context_);
124+
}
125+
}
126+
127+
// Runs the PosAndVelKinematicsInM benchmark.
128+
// NOLINTNEXTLINE(runtime/references)
129+
void DoPosAndVelKinematicsInM(benchmark::State& state) {
130+
DRAKE_DEMAND(want_grad_vdot(state) == false);
131+
DRAKE_DEMAND(want_grad_u(state) == false);
132+
for (auto _ : state) {
133+
InvalidateState();
134+
// This requires both position and velocity kinematics.
135+
plant_->EvalVelocityKinematicsInM(*context_);
125136
}
126137
}
127138

@@ -146,6 +157,17 @@ class Cassie : public benchmark::Fixture {
146157
}
147158
}
148159

160+
// Runs the InverseDynamicsInM benchmark.
161+
// NOLINTNEXTLINE(runtime/references)
162+
void DoInverseDynamicsInM(benchmark::State& state) {
163+
DRAKE_DEMAND(want_grad_u(state) == false);
164+
for (auto _ : state) {
165+
InvalidateState();
166+
plant_->CalcInverseDynamicsInM(*context_, desired_vdot_,
167+
external_forces_);
168+
}
169+
}
170+
149171
// Runs the ForwardDynamics benchmark.
150172
// NOLINTNEXTLINE(runtime/references)
151173
void DoForwardDynamics(benchmark::State& state) {
@@ -201,8 +223,8 @@ void Cassie<T>::SetUpNonZeroState() {
201223
VectorX<T>::LinSpaced(nq_ + nv_, 0.1, 0.9));
202224
for (const BodyIndex& index : plant_->GetFloatingBaseBodies()) {
203225
const RigidBody<T>& body = plant_->get_body(index);
204-
const RigidTransform<T> pose(
205-
RollPitchYaw<T>(0.1, 0.2, 0.3), Vector3<T>(0.4, 0.5, 0.6));
226+
const RigidTransform<T> pose(RollPitchYaw<T>(0.1, 0.2, 0.3),
227+
Vector3<T>(0.4, 0.5, 0.6));
206228
plant_->SetFreeBodyPose(context_.get(), body, pose);
207229
}
208230

@@ -303,6 +325,15 @@ BENCHMARK_REGISTER_F(CassieDouble, PositionKinematics)
303325
->Unit(benchmark::kMicrosecond)
304326
->Arg(kWantNoGrad);
305327

328+
// NOLINTNEXTLINE(runtime/references)
329+
BENCHMARK_DEFINE_F(CassieDouble,
330+
PositionKinematicsInM)(benchmark::State& state) {
331+
DoPositionKinematicsInM(state);
332+
}
333+
BENCHMARK_REGISTER_F(CassieDouble, PositionKinematicsInM)
334+
->Unit(benchmark::kMicrosecond)
335+
->Arg(kWantNoGrad);
336+
306337
// NOLINTNEXTLINE(runtime/references)
307338
BENCHMARK_DEFINE_F(CassieDouble, PosAndVelKinematics)(benchmark::State& state) {
308339
DoPosAndVelKinematics(state);
@@ -311,29 +342,46 @@ BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematics)
311342
->Unit(benchmark::kMicrosecond)
312343
->Arg(kWantNoGrad);
313344

345+
// NOLINTNEXTLINE(runtime/references)
346+
BENCHMARK_DEFINE_F(CassieDouble,
347+
PosAndVelKinematicsInM)(benchmark::State& state) {
348+
DoPosAndVelKinematicsInM(state);
349+
}
350+
BENCHMARK_REGISTER_F(CassieDouble, PosAndVelKinematicsInM)
351+
->Unit(benchmark::kMicrosecond)
352+
->Arg(kWantNoGrad);
353+
314354
// NOLINTNEXTLINE(runtime/references)
315355
BENCHMARK_DEFINE_F(CassieDouble, MassMatrix)(benchmark::State& state) {
316356
DoMassMatrix(state);
317357
}
318358
BENCHMARK_REGISTER_F(CassieDouble, MassMatrix)
319-
->Unit(benchmark::kMicrosecond)
320-
->Arg(kWantNoGrad);
359+
->Unit(benchmark::kMicrosecond)
360+
->Arg(kWantNoGrad);
321361

322362
// NOLINTNEXTLINE(runtime/references)
323363
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamics)(benchmark::State& state) {
324364
DoInverseDynamics(state);
325365
}
326366
BENCHMARK_REGISTER_F(CassieDouble, InverseDynamics)
327-
->Unit(benchmark::kMicrosecond)
328-
->Arg(kWantNoGrad);
367+
->Unit(benchmark::kMicrosecond)
368+
->Arg(kWantNoGrad);
369+
370+
// NOLINTNEXTLINE(runtime/references)
371+
BENCHMARK_DEFINE_F(CassieDouble, InverseDynamicsInM)(benchmark::State& state) {
372+
DoInverseDynamicsInM(state);
373+
}
374+
BENCHMARK_REGISTER_F(CassieDouble, InverseDynamicsInM)
375+
->Unit(benchmark::kMicrosecond)
376+
->Arg(kWantNoGrad);
329377

330378
// NOLINTNEXTLINE(runtime/references)
331379
BENCHMARK_DEFINE_F(CassieDouble, ForwardDynamics)(benchmark::State& state) {
332380
DoForwardDynamics(state);
333381
}
334382
BENCHMARK_REGISTER_F(CassieDouble, ForwardDynamics)
335-
->Unit(benchmark::kMicrosecond)
336-
->Arg(kWantNoGrad);
383+
->Unit(benchmark::kMicrosecond)
384+
->Arg(kWantNoGrad);
337385

338386
BENCHMARK_DEFINE_F(CassieAutoDiff, PositionKinematics)
339387
// NOLINTNEXTLINE(runtime/references)
@@ -362,41 +410,57 @@ BENCHMARK_DEFINE_F(CassieAutoDiff, MassMatrix)(benchmark::State& state) {
362410
DoMassMatrix(state);
363411
}
364412
BENCHMARK_REGISTER_F(CassieAutoDiff, MassMatrix)
365-
->Unit(benchmark::kMicrosecond)
366-
->Arg(kWantNoGrad)
367-
->Arg(kWantGradQ)
368-
->Arg(kWantGradV)
369-
->Arg(kWantGradX);
413+
->Unit(benchmark::kMicrosecond)
414+
->Arg(kWantNoGrad)
415+
->Arg(kWantGradQ)
416+
->Arg(kWantGradV)
417+
->Arg(kWantGradX);
370418

371419
// NOLINTNEXTLINE(runtime/references)
372420
BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamics)(benchmark::State& state) {
373421
DoInverseDynamics(state);
374422
}
375423
BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamics)
376-
->Unit(benchmark::kMicrosecond)
377-
->Arg(kWantNoGrad)
378-
->Arg(kWantGradQ)
379-
->Arg(kWantGradV)
380-
->Arg(kWantGradX)
381-
->Arg(kWantGradVdot)
382-
->Arg(kWantGradQ|kWantGradVdot)
383-
->Arg(kWantGradV|kWantGradVdot)
384-
->Arg(kWantGradX|kWantGradVdot);
424+
->Unit(benchmark::kMicrosecond)
425+
->Arg(kWantNoGrad)
426+
->Arg(kWantGradQ)
427+
->Arg(kWantGradV)
428+
->Arg(kWantGradX)
429+
->Arg(kWantGradVdot)
430+
->Arg(kWantGradQ | kWantGradVdot)
431+
->Arg(kWantGradV | kWantGradVdot)
432+
->Arg(kWantGradX | kWantGradVdot);
433+
434+
BENCHMARK_DEFINE_F(CassieAutoDiff, InverseDynamicsInM)
435+
// NOLINTNEXTLINE(runtime/references)
436+
(benchmark::State& state) {
437+
DoInverseDynamicsInM(state);
438+
}
439+
BENCHMARK_REGISTER_F(CassieAutoDiff, InverseDynamicsInM)
440+
->Unit(benchmark::kMicrosecond)
441+
->Arg(kWantNoGrad)
442+
->Arg(kWantGradQ)
443+
->Arg(kWantGradV)
444+
->Arg(kWantGradX)
445+
->Arg(kWantGradVdot)
446+
->Arg(kWantGradQ | kWantGradVdot)
447+
->Arg(kWantGradV | kWantGradVdot)
448+
->Arg(kWantGradX | kWantGradVdot);
385449

386450
// NOLINTNEXTLINE(runtime/references)
387451
BENCHMARK_DEFINE_F(CassieAutoDiff, ForwardDynamics)(benchmark::State& state) {
388452
DoForwardDynamics(state);
389453
}
390454
BENCHMARK_REGISTER_F(CassieAutoDiff, ForwardDynamics)
391-
->Unit(benchmark::kMicrosecond)
392-
->Arg(kWantNoGrad)
393-
->Arg(kWantGradQ)
394-
->Arg(kWantGradV)
395-
->Arg(kWantGradX)
396-
->Arg(kWantGradU)
397-
->Arg(kWantGradQ|kWantGradU)
398-
->Arg(kWantGradV|kWantGradU)
399-
->Arg(kWantGradX|kWantGradU);
455+
->Unit(benchmark::kMicrosecond)
456+
->Arg(kWantNoGrad)
457+
->Arg(kWantGradQ)
458+
->Arg(kWantGradV)
459+
->Arg(kWantGradX)
460+
->Arg(kWantGradU)
461+
->Arg(kWantGradQ | kWantGradU)
462+
->Arg(kWantGradV | kWantGradU)
463+
->Arg(kWantGradX | kWantGradU);
400464

401465
BENCHMARK_DEFINE_F(CassieExpression, PositionKinematics)
402466
// NOLINTNEXTLINE(runtime/references)
@@ -425,38 +489,38 @@ BENCHMARK_DEFINE_F(CassieExpression, MassMatrix)(benchmark::State& state) {
425489
DoMassMatrix(state);
426490
}
427491
BENCHMARK_REGISTER_F(CassieExpression, MassMatrix)
428-
->Unit(benchmark::kMicrosecond)
429-
->Arg(kWantNoGrad)
430-
->Arg(kWantGradQ)
431-
->Arg(kWantGradV)
432-
->Arg(kWantGradX);
492+
->Unit(benchmark::kMicrosecond)
493+
->Arg(kWantNoGrad)
494+
->Arg(kWantGradQ)
495+
->Arg(kWantGradV)
496+
->Arg(kWantGradX);
433497

434498
// NOLINTNEXTLINE(runtime/references)
435499
BENCHMARK_DEFINE_F(CassieExpression, InverseDynamics)(benchmark::State& state) {
436500
DoInverseDynamics(state);
437501
}
438502
BENCHMARK_REGISTER_F(CassieExpression, InverseDynamics)
439-
->Unit(benchmark::kMicrosecond)
440-
->Arg(kWantNoGrad)
441-
->Arg(kWantGradQ)
442-
->Arg(kWantGradV)
443-
->Arg(kWantGradX)
444-
->Arg(kWantGradVdot)
445-
->Arg(kWantGradQ|kWantGradVdot)
446-
->Arg(kWantGradV|kWantGradVdot)
447-
->Arg(kWantGradX|kWantGradVdot);
503+
->Unit(benchmark::kMicrosecond)
504+
->Arg(kWantNoGrad)
505+
->Arg(kWantGradQ)
506+
->Arg(kWantGradV)
507+
->Arg(kWantGradX)
508+
->Arg(kWantGradVdot)
509+
->Arg(kWantGradQ | kWantGradVdot)
510+
->Arg(kWantGradV | kWantGradVdot)
511+
->Arg(kWantGradX | kWantGradVdot);
448512

449513
// NOLINTNEXTLINE(runtime/references)
450514
BENCHMARK_DEFINE_F(CassieExpression, ForwardDynamics)(benchmark::State& state) {
451515
DoForwardDynamics(state);
452516
}
453517
BENCHMARK_REGISTER_F(CassieExpression, ForwardDynamics)
454-
->Unit(benchmark::kMicrosecond)
455-
->Arg(kWantNoGrad)
456-
// N.B. MbP does not support forward dynamics with Variables in 'q'.
457-
->Arg(kWantGradV)
458-
->Arg(kWantGradU)
459-
->Arg(kWantGradV|kWantGradU);
518+
->Unit(benchmark::kMicrosecond)
519+
->Arg(kWantNoGrad)
520+
// N.B. MbP does not support forward dynamics with Variables in 'q'.
521+
->Arg(kWantGradV)
522+
->Arg(kWantGradU)
523+
->Arg(kWantGradV | kWantGradU);
460524

461525
} // namespace
462526
} // namespace multibody

‎multibody/math/spatial_velocity.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -107,7 +107,7 @@ class SpatialVelocity : public SpatialVector<SpatialVelocity, T> {
107107
/// @see ShiftInPlace() for more information and how V_MC_E is calculated.
108108
SpatialVelocity<T> Shift(const Vector3<T>& offset) const {
109109
SpatialVelocity<T> result(*this);
110-
result.ShiftInPlace(offset);
110+
result.ShiftInPlace(offset); // 12 flops
111111
return result;
112112
}
113113

‎multibody/plant/multibody_plant.h

Lines changed: 8 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -4033,6 +4033,14 @@ class MultibodyPlant final : public internal::MultibodyTreeSystem<T> {
40334033
external_forces);
40344034
}
40354035

4036+
VectorX<T> CalcInverseDynamicsInM(
4037+
const systems::Context<T>& context, const VectorX<T>& known_vdot,
4038+
const MultibodyForces<T>& external_forces) const {
4039+
this->ValidateContext(context);
4040+
return internal_tree().CalcInverseDynamicsInM(context, known_vdot,
4041+
external_forces);
4042+
}
4043+
40364044
#ifdef DRAKE_DOXYGEN_CXX
40374045
// MultibodyPlant uses the NVI implementation of
40384046
// 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) {

0 commit comments

Comments
 (0)