diff --git a/examples/multibody/cassie_benchmark/test/cassie_bench.cc b/examples/multibody/cassie_benchmark/test/cassie_bench.cc index 1c199ed127d7..7ac46a990cfc 100644 --- a/examples/multibody/cassie_benchmark/test/cassie_bench.cc +++ b/examples/multibody/cassie_benchmark/test/cassie_bench.cc @@ -33,6 +33,39 @@ drake::test::LimitMallocParams LimitReleaseOnly(int max_num_allocations) { return { .max_num_allocations = max_num_allocations }; } +// Track and report simple streaming statistics on allocations. Variance +// tracking is adapted from: +// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm +class AllocationTracker { + public: + AllocationTracker() {} + + void Report(benchmark::State* state) { + state->counters["Allocs.min"] = min_; + state->counters["Allocs.max"] = max_; + state->counters["Allocs.mean"] = mean_; + state->counters["Allocs.stddev"] = + updates_ < 2 ? std::numeric_limits::quiet_NaN() + : std::sqrt(m2_ / (updates_ - 1)); + } + + void Update(int allocs) { + min_ = std::min(min_, allocs); + max_ = std::max(max_, allocs); + ++updates_; + double delta = allocs - mean_; + mean_ += delta / updates_; + m2_ += delta * (allocs - mean_); + } + + private: + int min_{std::numeric_limits::max()}; + int max_{std::numeric_limits::min()}; + int updates_{}; + double mean_{}; + double m2_{}; +}; + // Fixture that holds a Cassie robot model in a MultibodyPlant. The // class also holds a default context for the plant, and dimensions of its // state and inputs. @@ -85,6 +118,7 @@ class CassieDoubleFixture : public benchmark::Fixture { } protected: + AllocationTracker tracker_; std::unique_ptr> plant_{}; std::unique_ptr> context_; int nq_{}; @@ -94,57 +128,22 @@ class CassieDoubleFixture : public benchmark::Fixture { VectorXd x_{}; }; -// Track and report simple streaming statistics on allocations. Variance -// tracking is adapted from: -// https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance#Welford's_online_algorithm -class AllocationTracker { - public: - explicit AllocationTracker(benchmark::State* state) : state_(state) {} - - ~AllocationTracker() { - state_->counters["Allocs.min"] = min_; - state_->counters["Allocs.max"] = max_; - state_->counters["Allocs.mean"] = mean_; - state_->counters["Allocs.stddev"] = - updates_ < 2 ? std::numeric_limits::quiet_NaN() - : std::sqrt(m2_ / (updates_ - 1)); - } - - void Update(int allocs) { - min_ = std::min(min_, allocs); - max_ = std::max(max_, allocs); - ++updates_; - double delta = allocs - mean_; - mean_ += delta / updates_; - m2_ += delta * (allocs - mean_); - } - - private: - benchmark::State* state_{}; - int min_{std::numeric_limits::max()}; - int max_{std::numeric_limits::min()}; - int updates_{}; - double mean_{}; - double m2_{}; -}; - // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. BENCHMARK_F(CassieDoubleFixture, DoubleMassMatrix)(benchmark::State& state) { - AllocationTracker tracker(&state); MatrixXd M(nv_, nv_); for (auto _ : state) { // @see LimitMalloc note above. LimitMalloc guard({.max_num_allocations = 0}); InvalidateState(); plant_->CalcMassMatrix(*context_, &M); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } + tracker_.Report(&state); } BENCHMARK_F(CassieDoubleFixture, DoubleInverseDynamics) // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. (benchmark::State& state) { - AllocationTracker tracker(&state); VectorXd desired_vdot = VectorXd::Zero(nv_); multibody::MultibodyForces external_forces(*plant_); for (auto _ : state) { @@ -152,14 +151,14 @@ BENCHMARK_F(CassieDoubleFixture, DoubleInverseDynamics) LimitMalloc guard({.max_num_allocations = 3}); InvalidateState(); plant_->CalcInverseDynamics(*context_, desired_vdot, external_forces); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } + tracker_.Report(&state); } BENCHMARK_F(CassieDoubleFixture, DoubleForwardDynamics) // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. (benchmark::State& state) { - AllocationTracker tracker(&state); auto derivatives = plant_->AllocateTimeDerivatives(); auto& port_value = plant_->get_actuation_input_port().FixValue(context_.get(), u_); @@ -169,8 +168,9 @@ BENCHMARK_F(CassieDoubleFixture, DoubleForwardDynamics) InvalidateState(); port_value.GetMutableData(); // Invalidates caching of inputs. plant_->CalcTimeDerivatives(*context_, derivatives.get()); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } + tracker_.Report(&state); } // Fixture that holds a Cassie robot model in a MultibodyPlant. It @@ -198,7 +198,6 @@ class CassieAutodiffFixture : public CassieDoubleFixture { BENCHMARK_F(CassieAutodiffFixture, AutodiffMassMatrix) // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. (benchmark::State& state) { - AllocationTracker tracker(&state); MatrixX M_autodiff(nv_, nv_); auto x_autodiff = math::initializeAutoDiff(x_); plant_autodiff_->SetPositionsAndVelocities(context_autodiff_.get(), @@ -218,18 +217,18 @@ BENCHMARK_F(CassieAutodiffFixture, AutodiffMassMatrix) compute(); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } for (auto _ : state) { compute(); } + tracker_.Report(&state); } BENCHMARK_F(CassieAutodiffFixture, AutodiffInverseDynamics) // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. (benchmark::State& state) { - AllocationTracker tracker(&state); VectorXd desired_vdot = VectorXd::Zero(nv_); multibody::MultibodyForces external_forces_autodiff( *plant_autodiff_); @@ -255,18 +254,18 @@ BENCHMARK_F(CassieAutodiffFixture, AutodiffInverseDynamics) compute(); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } for (auto _ : state) { compute(); } + tracker_.Report(&state); } BENCHMARK_F(CassieAutodiffFixture, AutodiffForwardDynamics) // NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices. (benchmark::State& state) { - AllocationTracker tracker(&state); auto derivatives_autodiff = plant_autodiff_->AllocateTimeDerivatives(); auto u_autodiff = math::initializeAutoDiff(u_, nq_ + nv_ + nu_, nq_ + nv_); auto& port_value = plant_autodiff_->get_actuation_input_port().FixValue( @@ -291,12 +290,13 @@ BENCHMARK_F(CassieAutodiffFixture, AutodiffForwardDynamics) compute(); - tracker.Update(guard.num_allocations()); + tracker_.Update(guard.num_allocations()); } for (auto _ : state) { compute(); } + tracker_.Report(&state); } } // namespace