Add kinematics benchmarks

tesseract_environment/test/benchmarks/CMakeLists.txt

@@ -24,3 +24,4 @@ endmacro()
 
 add_benchmark(${PROJECT_NAME}_clone_benchmark environment_clone_benchmarks.cpp)
 add_benchmark(${PROJECT_NAME}_check_trajectory check_trajectory_benchmarks.cpp)
+add_benchmark(${PROJECT_NAME}_kinematics kinematics_benchmarks.cpp)

tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp

@@ -0,0 +1,233 @@
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <benchmark/benchmark.h>
+#include <algorithm>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+#include <tesseract_environment/environment.h>
+#include <tesseract_state_solver/state_solver.h>
+#include <tesseract_common/resource_locator.h>
+#include <tesseract_urdf/urdf_parser.h>
+#include <tesseract_support/tesseract_support_resource_locator.h>
+#include <tesseract_geometry/impl/sphere.h>
+#include <tesseract_environment/utils.h>
+
+using namespace tesseract_scene_graph;
+using namespace tesseract_srdf;
+using namespace tesseract_collision;
+using namespace tesseract_environment;
+
+SceneGraph::Ptr getSceneGraph()
+{
+  std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.urdf";
+
+  tesseract_common::TesseractSupportResourceLocator locator;
+  return tesseract_urdf::parseURDFFile(path, locator);
+}
+
+SRDFModel::Ptr getSRDFModel(const SceneGraph& scene_graph)
+{
+  std::string path = std::string(TESSERACT_SUPPORT_DIR) + "/urdf/lbr_iiwa_14_r820.srdf";
+  tesseract_common::TesseractSupportResourceLocator locator;
+
+  auto srdf = std::make_shared<SRDFModel>();
+  srdf->initFile(scene_graph, path, locator);
+
+  return srdf;
+}
+
+using CalcStateFn = std::function<tesseract_common::TransformMap(const Eigen::Ref<const Eigen::VectorXd>& state)>;
+
+static void BM_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS(benchmark::State& state,
+                                                     CalcStateFn fn,
+                                                     const tesseract_common::TrajArray& traj)
+{
+  tesseract_common::TransformMap transform_map;
+  for (auto _ : state)
+  {
+    for (Eigen::Index i = 0; i < traj.rows(); i++)
+    {
+      benchmark::DoNotOptimize(transform_map = fn(traj.row(i)));
+    }
+  }
+}
+
+static void BM_SET_AND_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS(benchmark::State& state,
+                                                             CalcStateFn fn,
+                                                             const tesseract_common::TrajArray& traj)
+{
+  tesseract_common::TransformMap transform_map;
+  for (auto _ : state)
+  {
+    for (Eigen::Index i = 0; i < traj.rows(); i++)
+    {
+      benchmark::DoNotOptimize(transform_map = fn(traj.row(i)));
+    }
+  }
+}
+
+static void BM_GET_JACOBIAN_JOINT_NAMES_JOINT_VALUES_SS(benchmark::State& state,
+                                                        tesseract_scene_graph::StateSolver::Ptr state_solver,
+                                                        std::vector<std::string> joint_names,
+                                                        const tesseract_common::TrajArray& traj,
+                                                        std::string link_name)
+{
+  Eigen::MatrixXd jacobian;
+  for (auto _ : state)
+  {
+    for (Eigen::Index i = 0; i < traj.rows(); i++)
+    {
+      benchmark::DoNotOptimize(jacobian = state_solver->getJacobian(joint_names, traj.row(i), link_name));
+    }
+  }
+}
+
+static void BM_CALC_FWD_KIN_MANIP(benchmark::State& state, CalcStateFn fn, const tesseract_common::TrajArray& traj)
+{
+  tesseract_common::TransformMap transforms;
+  for (auto _ : state)
+  {
+    for (Eigen::Index i = 0; i < traj.rows(); i++)
+    {
+      benchmark::DoNotOptimize(transforms = fn(traj.row(i)));
+    }
+  }
+}
+
+static void BM_GET_JACOBIAN_MANIP(benchmark::State& state,
+                                  tesseract_kinematics::JointGroup::Ptr manip,
+                                  const tesseract_common::TrajArray& traj,
+                                  std::string link_name)
+{
+  Eigen::MatrixXd jacobian;
+  for (auto _ : state)
+  {
+    for (Eigen::Index i = 0; i < traj.rows(); i++)
+    {
+      benchmark::DoNotOptimize(jacobian = manip->calcJacobian(traj.row(i), link_name));
+    }
+  }
+}
+
+int main(int argc, char** argv)
+{
+  auto env = std::make_shared<Environment>();
+  tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph();
+  auto srdf = getSRDFModel(*scene_graph);
+  env->init(*scene_graph, srdf);
+  env->setResourceLocator(std::make_shared<tesseract_common::TesseractSupportResourceLocator>());
+
+  // Set the robot initial state
+  std::vector<std::string> joint_names;
+  joint_names.emplace_back("joint_a1");
+  joint_names.emplace_back("joint_a2");
+  joint_names.emplace_back("joint_a3");
+  joint_names.emplace_back("joint_a4");
+  joint_names.emplace_back("joint_a5");
+  joint_names.emplace_back("joint_a6");
+  joint_names.emplace_back("joint_a7");
+
+  Eigen::VectorXd joint_start_pos(7);
+  joint_start_pos(0) = -0.4;
+  joint_start_pos(1) = 0.2762;
+  joint_start_pos(2) = 0.0;
+  joint_start_pos(3) = -1.3348;
+  joint_start_pos(4) = 0.0;
+  joint_start_pos(5) = 1.4959;
+  joint_start_pos(6) = 0.0;
+
+  Eigen::VectorXd joint_end_pos(7);
+  joint_end_pos(0) = 0.4;
+  joint_end_pos(1) = 0.2762;
+  joint_end_pos(2) = 0.0;
+  joint_end_pos(3) = -1.3348;
+  joint_end_pos(4) = 0.0;
+  joint_end_pos(5) = 1.4959;
+  joint_end_pos(6) = 0.0;
+
+  Eigen::VectorXd joint_pos_collision(7);
+  joint_pos_collision(0) = 0.0;
+  joint_pos_collision(1) = 0.2762;
+  joint_pos_collision(2) = 0.0;
+  joint_pos_collision(3) = -1.3348;
+  joint_pos_collision(4) = 0.0;
+  joint_pos_collision(5) = 1.4959;
+  joint_pos_collision(6) = 0.0;
+
+  tesseract_common::TrajArray traj(5, joint_start_pos.size());
+  for (int i = 0; i < joint_start_pos.size(); ++i)
+    traj.col(i) = Eigen::VectorXd::LinSpaced(5, joint_start_pos(i), joint_end_pos(i));
+
+  tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver();
+  tesseract_kinematics::JointGroup::Ptr joint_group = env->getJointGroup("manipulator");
+  std::string tip_link{ "tool0" };
+
+  //////////////////////////////////////
+  // Benchmarks
+  //////////////////////////////////////
+
+  {
+    tesseract_scene_graph::StateSolver::Ptr local_ss = state_solver->clone();
+    CalcStateFn fn = [local_ss,
+                      joint_names](const Eigen::Ref<const Eigen::VectorXd>& state) -> tesseract_common::TransformMap {
+      return local_ss->getState(joint_names, state).link_transforms;
+    };
+
+    std::function<void(benchmark::State&, CalcStateFn, const tesseract_common::TrajArray&)> BM_GET_STATE_JN_JV_SS =
+        BM_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS;
+    std::string name = "BM_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS";
+    benchmark::RegisterBenchmark(name.c_str(), BM_GET_STATE_JN_JV_SS, fn, traj)
+        ->UseRealTime()
+        ->Unit(benchmark::TimeUnit::kMicrosecond);
+  }
+  {
+    tesseract_scene_graph::StateSolver::Ptr local_ss = state_solver->clone();
+    CalcStateFn fn = [local_ss,
+                      joint_names](const Eigen::Ref<const Eigen::VectorXd>& state) -> tesseract_common::TransformMap {
+      local_ss->setState(joint_names, state);
+      return local_ss->getState().link_transforms;
+    };
+
+    std::function<void(benchmark::State&, CalcStateFn, const tesseract_common::TrajArray&)>
+        BM_SET_AND_GET_STATE_JN_JV_SS = BM_SET_AND_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS;
+    std::string name = "BM_SET_AND_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS";
+    benchmark::RegisterBenchmark(name.c_str(), BM_SET_AND_GET_STATE_JN_JV_SS, fn, traj)
+        ->UseRealTime()
+        ->Unit(benchmark::TimeUnit::kMicrosecond);
+  }
+  {
+    std::function<void(benchmark::State&,
+                       tesseract_scene_graph::StateSolver::Ptr,
+                       std::vector<std::string>,
+                       const tesseract_common::TrajArray&,
+                       std::string)>
+        BM_GET_JACOBIAN_JN_JV_SS = BM_GET_JACOBIAN_JOINT_NAMES_JOINT_VALUES_SS;
+    std::string name = "BM_GET_JACOBIAN_JOINT_NAMES_JOINT_VALUES_SS";
+    benchmark::RegisterBenchmark(name.c_str(), BM_GET_JACOBIAN_JN_JV_SS, state_solver, joint_names, traj, tip_link)
+        ->UseRealTime()
+        ->Unit(benchmark::TimeUnit::kMicrosecond);
+  }
+  {
+    CalcStateFn fn = [joint_group](const Eigen::Ref<const Eigen::VectorXd>& state) -> tesseract_common::TransformMap {
+      return joint_group->calcFwdKin(state);
+    };
+
+    std::function<void(benchmark::State&, CalcStateFn, const tesseract_common::TrajArray&)> BM_CFK_MANIP =
+        BM_CALC_FWD_KIN_MANIP;
+    std::string name = "BM_CALC_FWD_KIN_MANIP";
+    benchmark::RegisterBenchmark(name.c_str(), BM_CFK_MANIP, fn, traj)
+        ->UseRealTime()
+        ->Unit(benchmark::TimeUnit::kMicrosecond);
+  }
+  {
+    std::function<void(
+        benchmark::State&, tesseract_kinematics::JointGroup::Ptr, const tesseract_common::TrajArray&, std::string)>
+        BM_CJ_MANIP = BM_GET_JACOBIAN_MANIP;
+    std::string name = "BM_GET_JACOBIAN_MANIP";
+    benchmark::RegisterBenchmark(name.c_str(), BM_CJ_MANIP, joint_group, traj, tip_link)
+        ->UseRealTime()
+        ->Unit(benchmark::TimeUnit::kMicrosecond);
+  }
+
+  benchmark::Initialize(&argc, argv);
+  benchmark::RunSpecifiedBenchmarks();
+}