forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
63 lines
2.1 KiB
63 lines
2.1 KiB
// @file
|
|
// Benchmark for GlobalInverseKinematics on the dual-arm TRI Homecart.
|
|
|
|
#include "drake/common/find_resource.h"
|
|
#include "drake/multibody/inverse_kinematics/global_inverse_kinematics.h"
|
|
#include "drake/multibody/parsing/parser.h"
|
|
#include "drake/solvers/solve.h"
|
|
#include "drake/tools/performance/fixture_common.h"
|
|
|
|
namespace drake {
|
|
namespace multibody {
|
|
namespace inverse_kinematics {
|
|
namespace {
|
|
|
|
class HomecartGlobalIkBenchmark : public benchmark::Fixture {
|
|
public:
|
|
HomecartGlobalIkBenchmark() {
|
|
tools::performance::AddMinMaxStatistics(this);
|
|
this->Unit(benchmark::kSecond);
|
|
|
|
// Set a fixed seed for random generation.
|
|
std::srand(1234);
|
|
}
|
|
};
|
|
|
|
// This benchmark adds only the posture cost (joint limits are also added in
|
|
// the constructor), so the optimal solution is the q0 passed to
|
|
// AddPostureCost. Passing this solution to SetInitialGuess demonstrates a
|
|
// limit on the speedup that can be obtained using SetInitialGuess; that
|
|
// speedup is substantial.
|
|
BENCHMARK_F(HomecartGlobalIkBenchmark, PostureCost)(benchmark::State& state) { // NOLINT
|
|
multibody::MultibodyPlant<double> plant(0.0);
|
|
geometry::SceneGraph<double> scene_graph;
|
|
plant.RegisterAsSourceForSceneGraph(&scene_graph);
|
|
multibody::Parser parser(&plant);
|
|
parser.AddModels(
|
|
FindResourceOrThrow("drake/manipulation/models/tri_homecart/"
|
|
"homecart_no_grippers.dmd.yaml"));
|
|
plant.Finalize();
|
|
|
|
Eigen::VectorXd q0(plant.num_positions());
|
|
// A somewhat arbitrary configuration with the arms posed comfortably inside
|
|
// the workspace.
|
|
q0 << 1.75, -1.04, 1.27, -1.79, -2.75, 0.25, -1.45, -2.5, -1.04, -1.32, 3.11,
|
|
0;
|
|
|
|
GlobalInverseKinematics global_ik(plant);
|
|
global_ik.AddPostureCost(q0,
|
|
Eigen::VectorXd::Constant(plant.num_bodies(), 1.0),
|
|
Eigen::VectorXd::Constant(plant.num_bodies(), 1));
|
|
|
|
for (auto _ : state) {
|
|
global_ik.SetInitialGuess(q0);
|
|
auto result = solvers::Solve(global_ik.prog());
|
|
DRAKE_DEMAND(result.is_success());
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
} // namespace inverse_kinematics
|
|
} // namespace multibody
|
|
} // namespace drake
|