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 <benchmark/benchmark.h>
+#include <kdl_parser/kdl_parser.hpp>
+#include <kdl/chainjnttojacsolver.hpp>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/utils/robot_model_test_utils.h>
-#include <eigen_stl_containers/eigen_stl_containers.h>
-#include <chrono>
-#include <gtest/gtest.h>
+#include <random_numbers/random_numbers.h>
+
+// 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<std::chrono::steady_clock> 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<double> 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<moveit::core::RobotState> robot_state;
+ benchmark::DoNotOptimize(robot_state = std::make_unique<moveit::core::RobotState>(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<moveit::core::RobotState> robot_state_copy;
+ benchmark::DoNotOptimize(robot_state_copy = std::make_unique<moveit::core::RobotState>(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 <benchmark/benchmark.h>
-#include <kdl_parser/kdl_parser.hpp>
-#include <kdl/chainjnttojacsolver.hpp>
-#include <moveit/robot_model/robot_model.h>
-#include <moveit/robot_state/robot_state.h>
-#include <moveit/utils/robot_model_test_utils.h>
-#include <random_numbers/random_numbers.h>
-
-// 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);