From a3feae5b1abfc049066e43bacf6ffc6976d121d6 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Thu, 21 Dec 2023 15:22:54 +0100 Subject: [PATCH] Fix RobotState benchmarks Actually allocate the given number of states instead of just one. --- .../test/robot_state_benchmark.cpp | 201 ++++++++---------- 1 file changed, 89 insertions(+), 112 deletions(-) diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 18f3dcb1000..8fab419964c 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -38,18 +38,16 @@ // 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 // 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"; namespace { @@ -169,147 +167,127 @@ static void inverseMatrix4d(benchmark::State& st) } } -// Benchmark time to construct a RobotState given a RobotModel. -static void robotStateConstruct(benchmark::State& st) +struct RobotStateBenchmark : ::benchmark::Fixture { - int n_states = st.range(0); - 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(PANDA_TEST_GROUP)) + void SetUp(const ::benchmark::State&) { - st.SkipWithError("The planning group doesn't exist."); - return; - } - - for (auto _ : st) - { - for (int i = 0; i < n_states; i++) + if (rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_WARN) != RCUTILS_RET_OK) { - std::unique_ptr robot_state; - benchmark::DoNotOptimize(robot_state = std::make_unique(robot_model)); - benchmark::ClobberMemory(); } + + robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); } -} -// Benchmark time to copy a RobotState. -static void robotStateCopy(benchmark::State& st) -{ - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); + std::vector constructStates(size_t num) + { + std::vector states; + states.reserve(num); + for (size_t i = 0; i < num; i++) + states.emplace_back(robot_model); + return states; + } + std::vector constructStates(size_t num, const moveit::core::RobotState& state) + { + std::vector states; + states.reserve(num); + for (size_t i = 0; i < num; i++) + states.emplace_back(state); + return states; + } - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) + std::vector random_permudation(size_t num) { - st.SkipWithError("The planning group doesn't exist."); - return; + std::vector result; + result.reserve(num); + for (size_t i = 0; i < num; i++) + result.push_back(i); + + std::random_device random_device; + std::mt19937 generator(random_device()); + + std::shuffle(result.begin(), result.end(), generator); + return result; } - // Robot state. - moveit::core::RobotState robot_state(robot_model); - robot_state.setToDefaultValues(); + moveit::core::RobotModelPtr robot_model; +}; +// Benchmark time to construct RobotStates +BENCHMARK_DEFINE_F(RobotStateBenchmark, construct)(benchmark::State& st) +{ for (auto _ : st) { - 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(); - } + auto states = constructStates(st.range(0)); + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); } } -// Benchmark time to call `setToRandomPositions` and `update` on a RobotState. -static void robotStateUpdate(benchmark::State& st) +// Benchmark time to copy-construct a RobotState. +BENCHMARK_DEFINE_F(RobotStateBenchmark, copyConstruct)(benchmark::State& st) { - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); moveit::core::RobotState state(robot_model); + state.setToDefaultValues(); + state.update(); for (auto _ : st) { - for (int i = 0; i < n_states; ++i) - { - state.setToRandomPositions(); - state.update(); - benchmark::ClobberMemory(); - } + auto states = constructStates(st.range(0), state); + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); } } -// Benchmark time to call `setToRandomPositions` and `getGlobalLinkTransform` on a RobotState. -static void robotStateForwardKinematics(benchmark::State& st) +// Benchmark time to call `setToRandomPositions` and `update` on RobotState. +BENCHMARK_DEFINE_F(RobotStateBenchmark, update)(benchmark::State& st) { - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); - moveit::core::RobotState state(robot_model); - + auto states = constructStates(st.range(0)); + auto permutation = random_permudation(states.size()); for (auto _ : st) { - for (int i = 0; i < n_states; ++i) + for (auto i : permutation) // process states in random order to challenge the cache { - state.setToRandomPositions(); - Eigen::Isometry3d transform; - benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel(PR2_TIP_LINK))); - benchmark::ClobberMemory(); + states[i].setToRandomPositions(); + states[i].update(); } } } // Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function. -static void moveItJacobian(benchmark::State& st) +BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianMoveIt)(benchmark::State& st) { - // Load a test robot model. - 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(PANDA_TEST_GROUP)) + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(PANDA_TEST_GROUP); + if (!jmg) { 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(PANDA_TEST_GROUP); - - // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint - // configurations. + // Manually seeded RandomNumberGenerator for deterministic results 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(); + state.setToRandomPositions(jmg, rng); + state.updateLinkTransforms(); st.ResumeTiming(); - kinematic_state.getJacobian(jmg); + state.getJacobian(jmg); } } // Benchmark time to compute the Jacobian using KDL. -static void kdlJacobian(benchmark::State& st) +BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianKDL)(benchmark::State& st) { - 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(PANDA_TEST_GROUP)) + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(PANDA_TEST_GROUP); + if (!jmg) { 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(PANDA_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)) { @@ -317,28 +295,22 @@ static void kdlJacobian(benchmark::State& st) 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::TreeJntToJacSolver jacobian_solver(kdl_tree); + KDL::Jacobian jacobian(kdl_tree.getNrOfJoints()); + KDL::JntArray kdl_q(kdl_tree.getNrOfJoints()); + const std::string tip_link = jmg->getLinkModelNames().back(); - KDL::ChainJntToJacSolver jacobian_solver(kdl_chain); + // Manually seeded RandomNumberGenerator for deterministic results + 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(); - KDL::Jacobian jacobian(kdl_chain.getNrOfJoints()); - KDL::JntArray kdl_q; - kdl_q.resize(kdl_chain.getNrOfJoints()); - kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]); + state.setToRandomPositions(jmg, rng); + state.copyJointGroupPositions(jmg, &kdl_q.data[0]); st.ResumeTiming(); - jacobian_solver.JntToJac(kdl_q, jacobian); + jacobian_solver.JntToJac(kdl_q, jacobian, tip_link); } } @@ -353,10 +325,15 @@ BENCHMARK(inverseAffineIsometry); BENCHMARK(inverseAffine); BENCHMARK(inverseMatrix4d); -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_REGISTER_F(RobotStateBenchmark, construct) + ->RangeMultiplier(10) + ->Range(100, 10000) + ->Unit(benchmark::kMillisecond); +BENCHMARK_REGISTER_F(RobotStateBenchmark, copyConstruct) + ->RangeMultiplier(10) + ->Range(100, 10000) + ->Unit(benchmark::kMillisecond); +BENCHMARK_REGISTER_F(RobotStateBenchmark, update)->RangeMultiplier(10)->Range(10, 10000)->Unit(benchmark::kMillisecond); + +BENCHMARK_REGISTER_F(RobotStateBenchmark, jacobianMoveIt); +BENCHMARK_REGISTER_F(RobotStateBenchmark, jacobianKDL);