From e064a8497a72b7e417f911fa2b4cb87d7c809a59 Mon Sep 17 00:00:00 2001 From: Mario Prats Date: Wed, 15 Nov 2023 05:43:28 -0800 Subject: [PATCH] Consolidate RobotState benchmarks in single file (#2528) * Consolidate RobotState benchmarks in single file * some cosmetics * style fixes * additional comments --- moveit_core/robot_state/CMakeLists.txt | 17 +- .../test/robot_state_benchmark.cpp | 374 ++++++++++++++---- .../test/robot_state_jacobian_benchmark.cpp | 133 ------- 3 files changed, 298 insertions(+), 226 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..8a5e8e46a3 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,346 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Robert Haschke */ +/* 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 +#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"; -// Helper class to measure time within a scoped block and output the result -class ScopedTimer +// Number of iterations to use in matrix multiplication / inversion benchmarks. +constexpr int MATRIX_OPS_N_ITERATIONS = 1e7; + +namespace +{ +Eigen::Isometry3d createTestIsometry() { - const char* const msg_; - double* const gold_standard_; - const std::chrono::time_point start_; + // 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 -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()) +// 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 = createTestIsometry(); + for (auto _ : st) { + for (int i = 0; i < n_iters; ++i) + { + Eigen::Affine3d result; + benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix()); + benchmark::ClobberMemory(); + } } +} - ~ScopedTimer() +// 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 = createTestIsometry(); + 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_) +// 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 = createTestIsometry(); + 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 +// Benchmark time to invert an Eigen::Isometry3d. +static void inverseIsometry3d(benchmark::State& st) { -protected: - void SetUp() override + int n_iters = st.range(0); + Eigen::Isometry3d isometry = createTestIsometry(); + 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 +// Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry). +static void inverseAffineIsometry(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = createTestIsometry(); + 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(); + } } +} + +// Benchmark time to invert an Eigen::Affine3d. +static void inverseAffine(benchmark::State& st) +{ + int n_iters = st.range(0); + Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Affine3d affine; + affine.matrix() = isometry.matrix(); -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; -}; + 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) +// Benchmark time to invert an Eigen::Matrix4d. +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 = createTestIsometry(); + 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) +// Benchmark time to construct a RobotState given a RobotModel. +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(PANDA_TEST_ROBOT); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { - 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(); + st.SkipWithError("The planning group doesn't exist."); + return; } + + for (auto _ : st) { - 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(); + for (int i = 0; i < n_states; i++) + { + std::unique_ptr robot_state; + benchmark::DoNotOptimize(robot_state = std::make_unique(robot_model)); + benchmark::ClobberMemory(); + } } +} + +// 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); + + // Make sure the group exists, otherwise exit early with an error. + if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) { - 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_]; + st.SkipWithError("The planning group doesn't exist."); + return; + } + + // Robot state. + moveit::core::RobotState robot_state(robot_model); + robot_state.setToDefaultValues(); + + 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(); + } } } -TEST_F(Timing, inverse) +// Benchmark time to call `setToRandomPositions` and `update` on a RobotState. +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_TEST_ROBOT); + 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; +} + +// Benchmark time to call `setToRandomPositions` and `getGlobalLinkTransform` on a RobotState. +static void robotStateForwardKinematics(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); + + 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(PR2_TIP_LINK))); + benchmark::ClobberMemory(); + } } +} + +// Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function. +static void moveItJacobian(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)) { - 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(PANDA_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) +// Benchmark time to compute the Jacobian using KDL. +static void kdlJacobian(benchmark::State& st) { - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + 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)) + { + 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)) + { + 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) + { + // 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]); + st.ResumeTiming(); + jacobian_solver.JntToJac(kdl_q, jacobian); + } } + +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(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);