Skip to content

Commit

Permalink
examples/acrobot: Add an autodiff benchmark
Browse files Browse the repository at this point in the history
Relevant to: RobotLocomotion#8482

This patch rewrites Hongkai's original program (from an old branch) to use
google benchmark, removes some obsolete measurements (RigidBodyTree,
AutoDiffUpTo73d), and adds some new ones (MBP vanilla CalcMassMatrix()).

This benchmark set is nice because is captures the small-problem (only four
derivatives!) end of the autodiff problem space.

A possible plan would be to wrap this program with controlled-experiment
scripts, similar to those in examples/multibody/cassie_benchmark, and use it to
help drive further autodiff optimization work.
  • Loading branch information
rpoyner-tri committed Dec 8, 2020
1 parent c7f2469 commit 4a15062
Show file tree
Hide file tree
Showing 2 changed files with 168 additions and 0 deletions.
15 changes: 15 additions & 0 deletions examples/acrobot/BUILD.bazel
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,21 @@ drake_cc_googletest(
],
)

drake_cc_binary(
name = "benchmark_autodiff",
srcs = ["benchmark_autodiff.cc"],
deps = [
":acrobot_plant",
"//common:essential",
"//common:find_resource",
"//math:autodiff",
"//math:gradient",
"//multibody/benchmarks/acrobot:make_acrobot_plant",
"//multibody/parsing",
"@googlebenchmark//:benchmark",
],
)

install_data()

add_lint_tests()
153 changes: 153 additions & 0 deletions examples/acrobot/benchmark_autodiff.cc
Original file line number Diff line number Diff line change
@@ -0,0 +1,153 @@
/// @file
/// Benchmarks for Acrobot autodiff, with and without MultibodyPlant.
///
/// This program is a successor to Hongkai Dai's original benchmark; see #8482.

#include <benchmark/benchmark.h>

#include "drake/common/eigen_types.h"
#include "drake/common/find_resource.h"
#include "drake/examples/acrobot/acrobot_plant.h"
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
#include "drake/multibody/benchmarks/acrobot/make_acrobot_plant.h"
#include "drake/multibody/parsing/parser.h"

using Eigen::MatrixXd;
using drake::multibody::MultibodyPlant;
using drake::systems::Context;
using drake::systems::System;

namespace drake {
namespace examples {
namespace acrobot {
namespace {

template <typename T>
class FixtureBase : public benchmark::Fixture {
public:
FixtureBase() {
ComputeStatistics("min", [](const std::vector<double>& v) -> double {
return *(std::min_element(std::begin(v), std::end(v)));
});
ComputeStatistics("max", [](const std::vector<double>& v) -> double {
return *(std::max_element(std::begin(v), std::end(v)));
});
}

void Populate(const System<T>& plant) {
context_ = plant.CreateDefaultContext();
x_ = context_->get_continuous_state_vector().CopyToVector();
}

void InvalidateState() {
context_->NoteContinuousStateChange();
}

protected:
std::unique_ptr<Context<T>> context_;
VectorX<T> x_{};
};

template <typename T>
class AcrobotFixture : public FixtureBase<T> {
public:
using benchmark::Fixture::SetUp;
void SetUp(benchmark::State&) override {
plant_ = std::make_unique<AcrobotPlant<T>>();
this->Populate(*plant_);
}

protected:
std::unique_ptr<AcrobotPlant<T>> plant_{};
};

using AcrobotFixtureD = AcrobotFixture<double>;
using AcrobotFixtureAdx = AcrobotFixture<AutoDiffXd>;

// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
BENCHMARK_F(AcrobotFixtureD, AcrobotDMassMatrix)(benchmark::State& state) {
for (auto _ : state) {
InvalidateState();
plant_->MassMatrix(*context_);
}
}

// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
BENCHMARK_F(AcrobotFixtureAdx, AcrobotAdxMassMatrix)(benchmark::State& state) {
for (auto _ : state) {
InvalidateState();
plant_->MassMatrix(*context_);
}
}

template <typename T>
class MultibodyFixture : public FixtureBase<T> {
public:
using benchmark::Fixture::SetUp;
void SetUp(benchmark::State&) override {
auto double_plant = multibody::benchmarks::acrobot::MakeAcrobotPlant(
multibody::benchmarks::acrobot::AcrobotParameters(), true);
if constexpr (std::is_same_v<T, double>) {
plant_ = std::move(double_plant);
} else {
plant_.reset(dynamic_cast<MultibodyPlant<AutoDiffXd>*>(
double_plant->ToAutoDiffXd().release()));
}
nv_ = plant_->num_velocities();
this->Populate(*plant_);
}

protected:
std::unique_ptr<MultibodyPlant<T>> plant_{};
int nv_{};
};

using MultibodyFixtureD = MultibodyFixture<double>;
using MultibodyFixtureAdx = MultibodyFixture<AutoDiffXd>;

// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
BENCHMARK_F(MultibodyFixtureD, MultibodyDMassMatrix)(benchmark::State& state) {
MatrixXd M(nv_, nv_);
for (auto _ : state) {
InvalidateState();
plant_->CalcMassMatrix(*context_, &M);
}
}

BENCHMARK_F(MultibodyFixtureAdx, MultibodyAdxMassMatrix)
// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
(benchmark::State& state) {
MatrixX<AutoDiffXd> M(nv_, nv_);
for (auto _ : state) {
InvalidateState();
plant_->CalcMassMatrix(*context_, &M);
}
}

BENCHMARK_F(MultibodyFixtureD, MultibodyDMassMatrixViaInverseDynamics)
// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
(benchmark::State& state) {
MatrixXd M(nv_, nv_);
for (auto _ : state) {
InvalidateState();
plant_->CalcMassMatrixViaInverseDynamics(*context_, &M);
}
}

BENCHMARK_F(MultibodyFixtureAdx, MultibodyAdxMassMatrixViaInverseDynamics)
// NOLINTNEXTLINE(runtime/references) cpplint disapproves of gbench choices.
(benchmark::State& state) {
MatrixX<AutoDiffXd> M(nv_, nv_);
for (auto _ : state) {
InvalidateState();
plant_->CalcMassMatrixViaInverseDynamics(*context_, &M);
}
}

} // namespace
} // namespace acrobot
} // namespace examples
} // namespace drake

BENCHMARK_MAIN();

0 comments on commit 4a15062

Please sign in to comment.