diff --git a/moveit_core/package.xml b/moveit_core/package.xml
index ca0fe4cb45..daff091d66 100644
--- a/moveit_core/package.xml
+++ b/moveit_core/package.xml
@@ -36,6 +36,7 @@
generate_parameter_library
geometric_shapes
geometry_msgs
+ google_benchmark_vendor
kdl_parser
libfcl-dev
moveit_common
@@ -67,6 +68,7 @@
moveit_resources_pr2_description
angles
orocos_kdl_vendor
+ ament_cmake_google_benchmark
ament_cmake_gtest
ament_cmake_gmock
ament_index_cpp
diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt
index 4a4f41c7de..1404a7b698 100644
--- a/moveit_core/robot_state/CMakeLists.txt
+++ b/moveit_core/robot_state/CMakeLists.txt
@@ -26,7 +26,9 @@ install(DIRECTORY include/ DESTINATION include/moveit_core)
# Unit tests
if(BUILD_TESTING)
+ find_package(ament_cmake_google_benchmark REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
+ find_package(benchmark REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)
find_package(resource_retriever REQUIRED)
@@ -77,4 +79,13 @@ if(BUILD_TESTING)
moveit_robot_state
moveit_kinematics_base
)
+
+ ament_add_google_benchmark(
+ robot_state_jacobian_benchmark
+ test/robot_state_jacobian_benchmark.cpp)
+ target_link_libraries(robot_state_jacobian_benchmark
+ moveit_robot_model
+ moveit_test_utils
+ moveit_robot_state
+ )
endif()
diff --git a/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp
new file mode 100644
index 0000000000..b4967dc0f3
--- /dev/null
+++ b/moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp
@@ -0,0 +1,80 @@
+/*********************************************************************
+ * 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
+
+// Robot and planning group for which the Jacobian will be benchmarked.
+constexpr char TEST_ROBOT[] = "panda";
+constexpr char TEST_GROUP[] = "panda_arm";
+
+static void BM_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))
+ {
+ 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);
+ }
+}
+
+BENCHMARK(BM_MoveItJacobian);