From acb8c53a0f15df16f4d5b29616c274001efbadcb Mon Sep 17 00:00:00 2001
From: Mario Prats <marioprats@gmail.com>
Date: Thu, 24 Aug 2023 02:50:17 -0700
Subject: [PATCH] Add a benchmark for 'getJacobian'

---
 moveit_core/package.xml                       |  2 +
 moveit_core/robot_state/CMakeLists.txt        | 11 +++
 .../test/robot_state_jacobian_benchmark.cpp   | 80 +++++++++++++++++++
 3 files changed, 93 insertions(+)
 create mode 100644 moveit_core/robot_state/test/robot_state_jacobian_benchmark.cpp

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 @@
   <depend>generate_parameter_library</depend>
   <depend>geometric_shapes</depend>
   <depend>geometry_msgs</depend>
+  <depend>google_benchmark_vendor</depend>
   <depend>kdl_parser</depend>
   <depend>libfcl-dev</depend>
   <depend>moveit_common</depend>
@@ -67,6 +68,7 @@
   <test_depend>moveit_resources_pr2_description</test_depend>
   <test_depend>angles</test_depend>
   <test_depend>orocos_kdl_vendor</test_depend>
+  <test_depend>ament_cmake_google_benchmark</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_index_cpp</test_depend>
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 <benchmark/benchmark.h>
+#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 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);