From d361f7dce2eea9cdd2d6fe16ffd1a536a5548187 Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sun, 24 Mar 2024 21:14:28 -0500 Subject: [PATCH] Add kinematics benchmarks --- .../test/benchmarks/CMakeLists.txt | 1 + .../test/benchmarks/kinematics_benchmarks.cpp | 233 ++++++++++++++++++ 2 files changed, 234 insertions(+) create mode 100644 tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp diff --git a/tesseract_environment/test/benchmarks/CMakeLists.txt b/tesseract_environment/test/benchmarks/CMakeLists.txt index 868f7b07da8..da577c7a3be 100644 --- a/tesseract_environment/test/benchmarks/CMakeLists.txt +++ b/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) diff --git a/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp new file mode 100644 index 00000000000..4ea49c10b18 --- /dev/null +++ b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp @@ -0,0 +1,233 @@ +#include +TESSERACT_COMMON_IGNORE_WARNINGS_PUSH +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP +#include +#include +#include +#include +#include +#include +#include + +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(); + srdf->initFile(scene_graph, path, locator); + + return srdf; +} + +using CalcStateFn = std::function& 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 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(); + tesseract_scene_graph::SceneGraph::Ptr scene_graph = getSceneGraph(); + auto srdf = getSRDFModel(*scene_graph); + env->init(*scene_graph, srdf); + env->setResourceLocator(std::make_shared()); + + // Set the robot initial state + std::vector 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& state) -> tesseract_common::TransformMap { + return local_ss->getState(joint_names, state).link_transforms; + }; + + std::function 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& state) -> tesseract_common::TransformMap { + local_ss->setState(joint_names, state); + return local_ss->getState().link_transforms; + }; + + std::function + 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, + 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& state) -> tesseract_common::TransformMap { + return joint_group->calcFwdKin(state); + }; + + std::function 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 + 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(); +}