From a0af61b536f5e3d751dd6ff4e9d49e9e23672592 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Mon, 30 Oct 2023 13:01:13 +0100 Subject: [PATCH 1/4] Consolidate RobotState benchmarks in single file --- moveit_core/robot_state/CMakeLists.txt | 17 +- .../test/robot_state_benchmark.cpp | 368 ++++++++++++++---- .../test/robot_state_jacobian_benchmark.cpp | 133 ------- 3 files changed, 291 insertions(+), 227 deletions(-) delete mode 100644 moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 90dec83a73..6448b21296 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -50,15 +50,6 @@ if(BUILD_TESTING) moveit_robot_state ) - # As an executable, this benchmark is not run as a test by default - ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp) - target_link_libraries(test_robot_state_benchmark - moveit_test_utils - moveit_utils - moveit_exceptions - moveit_robot_state - ) - ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) target_link_libraries(test_robot_state_complex moveit_test_utils @@ -83,12 +74,12 @@ if(BUILD_TESTING) ) ament_add_google_benchmark( - robot_state_jacobian_benchmark - test/robot_state_jacobian_benchmark.cpp) - ament_target_dependencies(robot_state_jacobian_benchmark + robot_state_benchmark + test/robot_state_benchmark.cpp) + ament_target_dependencies(robot_state_benchmark kdl_parser ) - target_link_libraries(robot_state_jacobian_benchmark + target_link_libraries(robot_state_benchmark moveit_robot_model moveit_test_utils moveit_robot_state diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 148f0ab3a8..6bc764e00b 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2018, CITEC Bielefeld + * Copyright (c) 2023, PickNik Robotics. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -32,132 +32,338 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* Author: Mario Prats, Robert Haschke */ + +// To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. + +#include +#include +#include #include #include #include -#include -#include -#include +#include -// Helper class to measure time within a scoped block and output the result -class ScopedTimer -{ - const char* const msg_; - double* const gold_standard_; - const std::chrono::time_point start_; +// Robot and planning group for benchmarks, except for RobotStateUpdate and RobotStateForwardKinematics, which will use +// the pr2. +constexpr char TEST_ROBOT[] = "panda"; +constexpr char TEST_GROUP[] = "panda_arm"; -public: - // if gold_standard is provided, a relative increase/decrease is shown too - ScopedTimer(const char* msg = "", double* gold_standard = nullptr) - : msg_(msg), gold_standard_(gold_standard), start_(std::chrono::steady_clock::now()) +static void MultiplyAffineTimesMatrix(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + for (auto _ : st) { + for (int i = 0; i < n_iters; ++i) + { + Eigen::Affine3d result; + benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix()); + benchmark::ClobberMemory(); + } } +} - ~ScopedTimer() +static void MultiplyMatrixTimesMatrix(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + for (auto _ : st) { - std::chrono::duration elapsed = std::chrono::steady_clock::now() - start_; - std::cerr << msg_ << elapsed.count() * 1000. << "ms "; + for (int i = 0; i < n_iters; ++i) + { + Eigen::Matrix4d result; + benchmark::DoNotOptimize(result = isometry.matrix() * isometry.matrix()); + benchmark::ClobberMemory(); + } + } +} - if (gold_standard_) +static void MultiplyIsometryTimesIsometry(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + for (auto _ : st) + { + for (int i = 0; i < n_iters; ++i) { - if (*gold_standard_ == 0) - *gold_standard_ = elapsed.count(); - std::cerr << 100 * elapsed.count() / *gold_standard_ << '%'; + Eigen::Isometry3d result; + benchmark::DoNotOptimize(result = isometry * isometry); + benchmark::ClobberMemory(); } - std::cerr << '\n'; } -}; +} -class Timing : public testing::Test +static void InverseIsometry3d(benchmark::State& st) { -protected: - void SetUp() override + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + for (auto _ : st) { - Eigen::Isometry3d iso = Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); - transforms_.push_back(Eigen::Isometry3d::Identity()); // result - transforms_.push_back(iso); // input + for (int i = 0; i < n_iters; ++i) + { + Eigen::Isometry3d result; + benchmark::DoNotOptimize(result = isometry.inverse()); + benchmark::ClobberMemory(); + } } +} - void TearDown() override +static void InverseAffineIsometry(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Affine3d affine; + affine.matrix() = isometry.matrix(); + + for (auto _ : st) { + for (int i = 0; i < n_iters; ++i) + { + Eigen::Affine3d result; + benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine()); + benchmark::ClobberMemory(); + } } +} -public: - const Eigen::Isometry3d id = Eigen::Isometry3d::Identity(); - // put transforms into a vector to avoid compiler optimization on variables - EigenSTL::vector_Isometry3d transforms_; - volatile size_t result_idx_ = 0; - volatile size_t input_idx_ = 1; -}; +static void InverseAffine(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Affine3d affine; + affine.matrix() = isometry.matrix(); + + for (auto _ : st) + { + for (int i = 0; i < n_iters; ++i) + { + Eigen::Affine3d result; + benchmark::DoNotOptimize(result = affine.inverse().affine()); + benchmark::ClobberMemory(); + } + } +} -TEST_F(Timing, stateUpdate) +static void InverseMatrix4d(benchmark::State& st) { - moveit::core::RobotModelPtr model = moveit::core::loadTestingRobotModel("pr2"); - ASSERT_TRUE(bool(model)); - moveit::core::RobotState state(model); - ScopedTimer t("RobotState updates: "); - for (unsigned i = 0; i < 1e5; ++i) + int n_iters = st.range(0); + Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * + Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Affine3d affine; + affine.matrix() = isometry.matrix(); + + for (auto _ : st) { - state.setToRandomPositions(); - state.update(); + for (int i = 0; i < n_iters; ++i) + { + Eigen::Affine3d result; + benchmark::DoNotOptimize(result = affine.matrix().inverse()); + benchmark::ClobberMemory(); + } } } -TEST_F(Timing, multiply) +static void RobotStateConstruct(benchmark::State& st) { - size_t runs = 1e7; - double gold_standard = 0; + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + + for (auto _ : st) { - ScopedTimer t("Eigen::Affine * Eigen::Matrix: ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_].affine().noalias() = transforms_[input_idx_].affine() * transforms_[input_idx_].matrix(); + for (int i = 0; i < n_states; i++) + { + std::unique_ptr robot_state; + benchmark::DoNotOptimize(robot_state = std::make_unique(robot_model)); + benchmark::ClobberMemory(); + } } +} + +static void RobotStateCopy(benchmark::State& st) +{ + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) { - ScopedTimer t("Eigen::Matrix * Eigen::Matrix: ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_].matrix().noalias() = transforms_[input_idx_].matrix() * transforms_[input_idx_].matrix(); + st.SkipWithError("The planning group doesn't exist."); + return; } + + // Robot state. + moveit::core::RobotState robot_state(robot_model); + robot_state.setToDefaultValues(); + + for (auto _ : st) { - ScopedTimer t("Eigen::Isometry * Eigen::Isometry: ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_] = transforms_[input_idx_] * transforms_[input_idx_]; + for (int i = 0; i < n_states; i++) + { + std::unique_ptr robot_state_copy; + benchmark::DoNotOptimize(robot_state_copy = std::make_unique(robot_state)); + benchmark::ClobberMemory(); + } } } -TEST_F(Timing, inverse) +static void RobotStateUpdate(benchmark::State& st) { - EigenSTL::vector_Affine3d affine(1); - affine[0].matrix() = transforms_[input_idx_].matrix(); - size_t runs = 1e7; - double gold_standard = 0; + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel("pr2"); + moveit::core::RobotState state(robot_model); + + for (auto _ : st) { - ScopedTimer t("Isometry3d::inverse(): ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_] = transforms_[input_idx_].inverse(); + for (int i = 0; i < n_states; ++i) + { + state.setToRandomPositions(); + state.update(); + benchmark::ClobberMemory(); + } } - volatile size_t input_idx = 0; +} + +static void RobotStateForwardKinematics(benchmark::State& st) +{ + int n_states = st.range(0); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel("pr2"); + moveit::core::RobotState state(robot_model); + + for (auto _ : st) { - ScopedTimer t("Affine3d::inverse(Eigen::Isometry): ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse(Eigen::Isometry).affine(); + for (int i = 0; i < n_states; ++i) + { + state.setToRandomPositions(); + Eigen::Isometry3d transform; + benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel("r_wrist_roll_" + "link"))); + benchmark::ClobberMemory(); + } } +} + +static void MoveItJacobian(benchmark::State& st) +{ + // Load a test robot model. + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) { - ScopedTimer t("Affine3d::inverse(): ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_].affine().noalias() = affine[input_idx].inverse().affine(); + st.SkipWithError("The planning group doesn't exist."); + return; } + + // Robot state. + moveit::core::RobotState kinematic_state(robot_model); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + + // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint + // configurations. + random_numbers::RandomNumberGenerator rng(0); + + for (auto _ : st) { - ScopedTimer t("Matrix4d::inverse(): ", &gold_standard); - for (size_t i = 0; i < runs; ++i) - transforms_[result_idx_].matrix().noalias() = affine[input_idx].matrix().inverse(); + // Time only the jacobian computation, not the forward kinematics. + st.PauseTiming(); + kinematic_state.setToRandomPositions(jmg, rng); + kinematic_state.updateLinkTransforms(); + st.ResumeTiming(); + kinematic_state.getJacobian(jmg); } } -int main(int argc, char** argv) +static void KdlJacobian(benchmark::State& st) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(TEST_GROUP)) + { + st.SkipWithError("The planning group doesn't exist."); + return; + } + + // Robot state. + moveit::core::RobotState kinematic_state(robot_model); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + + // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint + // configurations. + random_numbers::RandomNumberGenerator rng(0); + + KDL::Tree kdl_tree; + if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree)) + { + st.SkipWithError("Can't create KDL tree."); + return; + } + + KDL::Chain kdl_chain; + if (!kdl_tree.getChain(jmg->getJointModels().front()->getParentLinkModel()->getName(), + jmg->getLinkModelNames().back(), kdl_chain)) + { + st.SkipWithError("Can't create KDL Chain."); + return; + } + + KDL::ChainJntToJacSolver jacobian_solver(kdl_chain); + + for (auto _ : st) + { + st.PauseTiming(); + kinematic_state.setToRandomPositions(jmg, rng); + kinematic_state.updateLinkTransforms(); + KDL::Jacobian jacobian(kdl_chain.getNrOfJoints()); + KDL::JntArray kdl_q; + kdl_q.resize(kdl_chain.getNrOfJoints()); + kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]); + st.ResumeTiming(); + jacobian_solver.JntToJac(kdl_q, jacobian); + } } + +BENCHMARK(MultiplyAffineTimesMatrix)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(MultiplyMatrixTimesMatrix)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(MultiplyIsometryTimesIsometry)->Arg(1e7)->Unit(benchmark::kMillisecond); + +BENCHMARK(InverseIsometry3d)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseAffineIsometry)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseAffine)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseMatrix4d)->Arg(1e7)->Unit(benchmark::kMillisecond); + +BENCHMARK(RobotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); +BENCHMARK(RobotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); +BENCHMARK(RobotStateUpdate)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); +BENCHMARK(RobotStateForwardKinematics)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); + +BENCHMARK(MoveItJacobian); +BENCHMARK(KdlJacobian); diff --git a/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp deleted file mode 100644 index 3cd89dd69a..0000000000 --- a/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp +++ /dev/null @@ -1,133 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2023, PickNik Robotics. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -/* Author: Mario Prats */ - -// To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. - -#include -#include -#include -#include -#include -#include -#include - -// Robot and planning group for which the Jacobian will be benchmarked. -constexpr char TEST_ROBOT[] = "panda"; -constexpr char TEST_GROUP[] = "panda_arm"; - -static void bmMoveItJacobian(benchmark::State& st) -{ - // Load a test robot model. - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); - - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) - { - st.SkipWithError("The planning group doesn't exist."); - return; - } - - // Robot state. - moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); - - // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint - // configurations. - random_numbers::RandomNumberGenerator rng(0); - - for (auto _ : st) - { - // Time only the jacobian computation, not the forward kinematics. - st.PauseTiming(); - kinematic_state.setToRandomPositions(jmg, rng); - kinematic_state.updateLinkTransforms(); - st.ResumeTiming(); - kinematic_state.getJacobian(jmg); - } -} - -static void bmKdlJacobian(benchmark::State& st) -{ - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); - - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) - { - st.SkipWithError("The planning group doesn't exist."); - return; - } - - // Robot state. - moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); - - // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint - // configurations. - random_numbers::RandomNumberGenerator rng(0); - - KDL::Tree kdl_tree; - if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree)) - { - st.SkipWithError("Can't create KDL tree."); - return; - } - - KDL::Chain kdl_chain; - if (!kdl_tree.getChain(jmg->getJointModels().front()->getParentLinkModel()->getName(), - jmg->getLinkModelNames().back(), kdl_chain)) - { - st.SkipWithError("Can't create KDL Chain."); - return; - } - - KDL::ChainJntToJacSolver jacobian_solver(kdl_chain); - - for (auto _ : st) - { - st.PauseTiming(); - kinematic_state.setToRandomPositions(jmg, rng); - kinematic_state.updateLinkTransforms(); - KDL::Jacobian jacobian(kdl_chain.getNrOfJoints()); - KDL::JntArray kdl_q; - kdl_q.resize(kdl_chain.getNrOfJoints()); - kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]); - st.ResumeTiming(); - jacobian_solver.JntToJac(kdl_q, jacobian); - } -} - -BENCHMARK(bmMoveItJacobian); -BENCHMARK(bmKdlJacobian); From bb1567bea19392cc675e1c836aedf0662c7e5b6e Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Tue, 14 Nov 2023 09:06:59 +0100 Subject: [PATCH 2/4] some cosmetics --- .../test/robot_state_benchmark.cpp | 56 ++++++++++--------- 1 file changed, 30 insertions(+), 26 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 6bc764e00b..16d741b1b9 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Mario Prats, Robert Haschke */ +/* Author: Robert Haschke, Mario Prats */ // To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. @@ -44,10 +44,14 @@ #include #include -// Robot and planning group for benchmarks, except for RobotStateUpdate and RobotStateForwardKinematics, which will use -// the pr2. -constexpr char TEST_ROBOT[] = "panda"; -constexpr char TEST_GROUP[] = "panda_arm"; +// Robot and planning group for benchmarks. +constexpr char PANDA_TEST_ROBOT[] = "panda"; +constexpr char PANDA_TEST_GROUP[] = "panda_arm"; +constexpr char PR2_TEST_ROBOT[] = "pr2"; +constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link"; + +// Number of iterations to use in matrix multiplication / inversion benchmarks. +constexpr int MATRIX_OPS_N_ITERATIONS = 1e7; static void MultiplyAffineTimesMatrix(benchmark::State& st) { @@ -187,10 +191,10 @@ static void InverseMatrix4d(benchmark::State& st) static void RobotStateConstruct(benchmark::State& st) { int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { st.SkipWithError("The planning group doesn't exist."); return; @@ -210,10 +214,10 @@ static void RobotStateConstruct(benchmark::State& st) static void RobotStateCopy(benchmark::State& st) { int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { st.SkipWithError("The planning group doesn't exist."); return; @@ -237,7 +241,7 @@ static void RobotStateCopy(benchmark::State& st) static void RobotStateUpdate(benchmark::State& st) { int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel("pr2"); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); moveit::core::RobotState state(robot_model); for (auto _ : st) @@ -254,7 +258,7 @@ static void RobotStateUpdate(benchmark::State& st) static void RobotStateForwardKinematics(benchmark::State& st) { int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel("pr2"); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); moveit::core::RobotState state(robot_model); for (auto _ : st) @@ -263,8 +267,7 @@ static void RobotStateForwardKinematics(benchmark::State& st) { state.setToRandomPositions(); Eigen::Isometry3d transform; - benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel("r_wrist_roll_" - "link"))); + benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel(PR2_TIP_LINK))); benchmark::ClobberMemory(); } } @@ -273,10 +276,10 @@ static void RobotStateForwardKinematics(benchmark::State& st) static void MoveItJacobian(benchmark::State& st) { // Load a test robot model. - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { st.SkipWithError("The planning group doesn't exist."); return; @@ -284,7 +287,7 @@ static void MoveItJacobian(benchmark::State& st) // Robot state. moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP); // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint // configurations. @@ -303,10 +306,10 @@ static void MoveItJacobian(benchmark::State& st) static void KdlJacobian(benchmark::State& st) { - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(TEST_ROBOT); + const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(TEST_GROUP)) + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { st.SkipWithError("The planning group doesn't exist."); return; @@ -314,7 +317,7 @@ static void KdlJacobian(benchmark::State& st) // Robot state. moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(TEST_GROUP); + const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP); // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint // configurations. @@ -339,6 +342,7 @@ static void KdlJacobian(benchmark::State& st) for (auto _ : st) { + // Time only the jacobian computation, not the forward kinematics. st.PauseTiming(); kinematic_state.setToRandomPositions(jmg, rng); kinematic_state.updateLinkTransforms(); @@ -351,14 +355,14 @@ static void KdlJacobian(benchmark::State& st) } } -BENCHMARK(MultiplyAffineTimesMatrix)->Arg(1e7)->Unit(benchmark::kMillisecond); -BENCHMARK(MultiplyMatrixTimesMatrix)->Arg(1e7)->Unit(benchmark::kMillisecond); -BENCHMARK(MultiplyIsometryTimesIsometry)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(MultiplyAffineTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(MultiplyMatrixTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(MultiplyIsometryTimesIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseIsometry3d)->Arg(1e7)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseAffineIsometry)->Arg(1e7)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseAffine)->Arg(1e7)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseMatrix4d)->Arg(1e7)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseIsometry3d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseAffineIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseAffine)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(InverseMatrix4d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); BENCHMARK(RobotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); BENCHMARK(RobotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); From bfb978507ed2499b16bbe2f7f6f34567687e0ef0 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Tue, 14 Nov 2023 10:11:15 +0100 Subject: [PATCH 3/4] style fixes --- .../test/robot_state_benchmark.cpp | 52 +++++++++---------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 16d741b1b9..cdf96d4c20 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -53,7 +53,7 @@ constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link"; // Number of iterations to use in matrix multiplication / inversion benchmarks. constexpr int MATRIX_OPS_N_ITERATIONS = 1e7; -static void MultiplyAffineTimesMatrix(benchmark::State& st) +static void multiplyAffineTimesMatrix(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -71,7 +71,7 @@ static void MultiplyAffineTimesMatrix(benchmark::State& st) } } -static void MultiplyMatrixTimesMatrix(benchmark::State& st) +static void multiplyMatrixTimesMatrix(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -89,7 +89,7 @@ static void MultiplyMatrixTimesMatrix(benchmark::State& st) } } -static void MultiplyIsometryTimesIsometry(benchmark::State& st) +static void multiplyIsometryTimesIsometry(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -107,7 +107,7 @@ static void MultiplyIsometryTimesIsometry(benchmark::State& st) } } -static void InverseIsometry3d(benchmark::State& st) +static void inverseIsometry3d(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -125,7 +125,7 @@ static void InverseIsometry3d(benchmark::State& st) } } -static void InverseAffineIsometry(benchmark::State& st) +static void inverseAffineIsometry(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -146,7 +146,7 @@ static void InverseAffineIsometry(benchmark::State& st) } } -static void InverseAffine(benchmark::State& st) +static void inverseAffine(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -167,7 +167,7 @@ static void InverseAffine(benchmark::State& st) } } -static void InverseMatrix4d(benchmark::State& st) +static void inverseMatrix4d(benchmark::State& st) { int n_iters = st.range(0); Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * @@ -188,7 +188,7 @@ static void InverseMatrix4d(benchmark::State& st) } } -static void RobotStateConstruct(benchmark::State& st) +static void robotStateConstruct(benchmark::State& st) { int n_states = st.range(0); const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); @@ -211,7 +211,7 @@ static void RobotStateConstruct(benchmark::State& st) } } -static void RobotStateCopy(benchmark::State& st) +static void robotStateCopy(benchmark::State& st) { int n_states = st.range(0); const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); @@ -238,7 +238,7 @@ static void RobotStateCopy(benchmark::State& st) } } -static void RobotStateUpdate(benchmark::State& st) +static void robotStateUpdate(benchmark::State& st) { int n_states = st.range(0); const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); @@ -255,7 +255,7 @@ static void RobotStateUpdate(benchmark::State& st) } } -static void RobotStateForwardKinematics(benchmark::State& st) +static void robotStateForwardKinematics(benchmark::State& st) { int n_states = st.range(0); const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); @@ -273,7 +273,7 @@ static void RobotStateForwardKinematics(benchmark::State& st) } } -static void MoveItJacobian(benchmark::State& st) +static void moveItJacobian(benchmark::State& st) { // Load a test robot model. const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); @@ -304,7 +304,7 @@ static void MoveItJacobian(benchmark::State& st) } } -static void KdlJacobian(benchmark::State& st) +static void kdlJacobian(benchmark::State& st) { const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); @@ -355,19 +355,19 @@ static void KdlJacobian(benchmark::State& st) } } -BENCHMARK(MultiplyAffineTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(MultiplyMatrixTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(MultiplyIsometryTimesIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(multiplyAffineTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(multiplyMatrixTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(multiplyIsometryTimesIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseIsometry3d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseAffineIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseAffine)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(InverseMatrix4d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(inverseIsometry3d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(inverseAffineIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(inverseAffine)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); +BENCHMARK(inverseMatrix4d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(RobotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); -BENCHMARK(RobotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); -BENCHMARK(RobotStateUpdate)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); -BENCHMARK(RobotStateForwardKinematics)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); +BENCHMARK(robotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); +BENCHMARK(robotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); +BENCHMARK(robotStateUpdate)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); +BENCHMARK(robotStateForwardKinematics)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); -BENCHMARK(MoveItJacobian); -BENCHMARK(KdlJacobian); +BENCHMARK(moveItJacobian); +BENCHMARK(kdlJacobian); From 32fe788ce12ba568148b992433f09ae098fe0f98 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 15 Nov 2023 12:00:59 +0100 Subject: [PATCH 4/4] additional comments --- .../test/robot_state_benchmark.cpp | 60 ++++++++++--------- 1 file changed, 32 insertions(+), 28 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index cdf96d4c20..8a5e8e46a3 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -34,6 +34,7 @@ /* Author: Robert Haschke, Mario Prats */ +// This file contains various benchmarks related to RobotState and matrix multiplication and inverse with Eigen types. // To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. #include @@ -53,13 +54,22 @@ constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link"; // Number of iterations to use in matrix multiplication / inversion benchmarks. constexpr int MATRIX_OPS_N_ITERATIONS = 1e7; +namespace +{ +Eigen::Isometry3d createTestIsometry() +{ + // An arbitrary Eigen::Isometry3d object. + return Eigen::Translation3d(1, 2, 3) * Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); +} +} // namespace + +// Benchmark time to multiply an Eigen::Affine3d with an Eigen::Matrix4d. static void multiplyAffineTimesMatrix(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); for (auto _ : st) { for (int i = 0; i < n_iters; ++i) @@ -71,13 +81,11 @@ static void multiplyAffineTimesMatrix(benchmark::State& st) } } +// Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d. static void multiplyMatrixTimesMatrix(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); for (auto _ : st) { for (int i = 0; i < n_iters; ++i) @@ -89,13 +97,11 @@ static void multiplyMatrixTimesMatrix(benchmark::State& st) } } +// Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d. static void multiplyIsometryTimesIsometry(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); for (auto _ : st) { for (int i = 0; i < n_iters; ++i) @@ -107,13 +113,11 @@ static void multiplyIsometryTimesIsometry(benchmark::State& st) } } +// Benchmark time to invert an Eigen::Isometry3d. static void inverseIsometry3d(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); for (auto _ : st) { for (int i = 0; i < n_iters; ++i) @@ -125,13 +129,11 @@ static void inverseIsometry3d(benchmark::State& st) } } +// Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry). static void inverseAffineIsometry(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); Eigen::Affine3d affine; affine.matrix() = isometry.matrix(); @@ -146,13 +148,11 @@ static void inverseAffineIsometry(benchmark::State& st) } } +// Benchmark time to invert an Eigen::Affine3d. static void inverseAffine(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); Eigen::Affine3d affine; affine.matrix() = isometry.matrix(); @@ -167,13 +167,11 @@ static void inverseAffine(benchmark::State& st) } } +// Benchmark time to invert an Eigen::Matrix4d. static void inverseMatrix4d(benchmark::State& st) { int n_iters = st.range(0); - Eigen::Isometry3d isometry = Eigen::Translation3d(1, 2, 3) * - Eigen::AngleAxisd(0.13 * M_PI, Eigen::Vector3d::UnitX()) * - Eigen::AngleAxisd(0.29 * M_PI, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.42 * M_PI, Eigen::Vector3d::UnitZ()); + Eigen::Isometry3d isometry = createTestIsometry(); Eigen::Affine3d affine; affine.matrix() = isometry.matrix(); @@ -188,6 +186,7 @@ static void inverseMatrix4d(benchmark::State& st) } } +// Benchmark time to construct a RobotState given a RobotModel. static void robotStateConstruct(benchmark::State& st) { int n_states = st.range(0); @@ -211,6 +210,7 @@ static void robotStateConstruct(benchmark::State& st) } } +// Benchmark time to copy a RobotState. static void robotStateCopy(benchmark::State& st) { int n_states = st.range(0); @@ -238,6 +238,7 @@ static void robotStateCopy(benchmark::State& st) } } +// Benchmark time to call `setToRandomPositions` and `update` on a RobotState. static void robotStateUpdate(benchmark::State& st) { int n_states = st.range(0); @@ -255,6 +256,7 @@ static void robotStateUpdate(benchmark::State& st) } } +// Benchmark time to call `setToRandomPositions` and `getGlobalLinkTransform` on a RobotState. static void robotStateForwardKinematics(benchmark::State& st) { int n_states = st.range(0); @@ -273,6 +275,7 @@ static void robotStateForwardKinematics(benchmark::State& st) } } +// Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function. static void moveItJacobian(benchmark::State& st) { // Load a test robot model. @@ -304,6 +307,7 @@ static void moveItJacobian(benchmark::State& st) } } +// Benchmark time to compute the Jacobian using KDL. static void kdlJacobian(benchmark::State& st) { const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT);