From a7014888c14fd1981083c7bbc0e31179c7d6ae4d Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Mon, 6 Jan 2025 16:02:38 -0600 Subject: [PATCH] Update tesseract_environment benchmarks --- .../test/benchmarks/check_trajectory_benchmarks.cpp | 10 +++++----- .../test/benchmarks/environment_clone_benchmarks.cpp | 12 ++++++------ .../test/benchmarks/kinematics_benchmarks.cpp | 6 +++--- 3 files changed, 14 insertions(+), 14 deletions(-) diff --git a/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp b/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp index 498de607e17..2fc665c4ad1 100644 --- a/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/check_trajectory_benchmarks.cpp @@ -67,7 +67,7 @@ static void BM_CHECK_TRAJECTORY_CONTINUOUS_SS(benchmark::State& state, static void BM_CHECK_TRAJECTORY_CONTINUOUS_MANIP(benchmark::State& state, std::vector contacts, tesseract_collision::ContinuousContactManager::Ptr manager, - tesseract_kinematics::JointGroup::Ptr manip, + tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_common::TrajArray traj, tesseract_collision::CollisionCheckConfig config, bool log_level_debug) @@ -106,7 +106,7 @@ static void BM_CHECK_TRAJECTORY_DISCRETE_SS(benchmark::State& state, static void BM_CHECK_TRAJECTORY_DISCRETE_MANIP(benchmark::State& state, std::vector contacts, tesseract_collision::DiscreteContactManager::Ptr manager, - tesseract_kinematics::JointGroup::Ptr manip, + tesseract_kinematics::JointGroup::ConstPtr manip, tesseract_common::TrajArray traj, tesseract_collision::CollisionCheckConfig config, bool log_level_debug) @@ -223,7 +223,7 @@ int main(int argc, char** argv) tesseract_collision::DiscreteContactManager::Ptr discrete_manager = env->getDiscreteContactManager(); tesseract_collision::ContinuousContactManager::Ptr continuous_manager = env->getContinuousContactManager(); tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); - tesseract_kinematics::JointGroup::Ptr joint_group = env->getJointGroup("manipulator"); + tesseract_kinematics::JointGroup::ConstPtr joint_group = env->getJointGroup("manipulator"); std::vector contacts; tesseract_collision::CollisionCheckConfig discrete_config; discrete_config.type = CollisionEvaluatorType::DISCRETE; @@ -255,7 +255,7 @@ int main(int argc, char** argv) std::function, tesseract_collision::ContinuousContactManager::Ptr, - tesseract_kinematics::JointGroup::Ptr, + tesseract_kinematics::JointGroup::ConstPtr, tesseract_common::TrajArray, tesseract_collision::CollisionCheckConfig, bool)> @@ -272,7 +272,7 @@ int main(int argc, char** argv) std::function, tesseract_collision::DiscreteContactManager::Ptr, - tesseract_kinematics::JointGroup::Ptr, + tesseract_kinematics::JointGroup::ConstPtr, tesseract_common::TrajArray, tesseract_collision::CollisionCheckConfig, bool)> diff --git a/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp b/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp index 7f39e2c536c..749b7c90cff 100644 --- a/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/environment_clone_benchmarks.cpp @@ -67,7 +67,7 @@ static void BM_SCENE_GRAPH_CLONE(benchmark::State& state, SceneGraph::Ptr sg) } /** @brief Benchmark that checks the Tesseract clone method*/ -static void BM_JOINT_GROUP_COPY(benchmark::State& state, JointGroup::Ptr jg) +static void BM_JOINT_GROUP_COPY(benchmark::State& state, JointGroup::ConstPtr jg) { JointGroup::Ptr clone; for (auto _ : state) @@ -76,7 +76,7 @@ static void BM_JOINT_GROUP_COPY(benchmark::State& state, JointGroup::Ptr jg) } } -static void BM_KINEMATIC_GROUP_COPY(benchmark::State& state, KinematicGroup::Ptr kg) +static void BM_KINEMATIC_GROUP_COPY(benchmark::State& state, KinematicGroup::ConstPtr kg) { KinematicGroup::Ptr clone; for (auto _ : state) @@ -93,8 +93,8 @@ int main(int argc, char** argv) Environment::Ptr env = std::make_shared(); env->init(*scene_graph, srdf); StateSolver::Ptr state_solver = env->getStateSolver(); - JointGroup::Ptr joint_group = env->getJointGroup("manipulator"); - KinematicGroup::Ptr kinematic_group = env->getKinematicGroup("manipulator"); + JointGroup::ConstPtr joint_group = env->getJointGroup("manipulator"); + KinematicGroup::ConstPtr kinematic_group = env->getKinematicGroup("manipulator"); ////////////////////////////////////// // Clone ////////////////////////////////////// @@ -124,7 +124,7 @@ int main(int argc, char** argv) } { - std::function BM_CLONE_FUNC = BM_JOINT_GROUP_COPY; + std::function BM_CLONE_FUNC = BM_JOINT_GROUP_COPY; std::string name = "BM_JOINT_GROUP_COPY"; benchmark::RegisterBenchmark(name.c_str(), BM_CLONE_FUNC, joint_group) ->UseRealTime() @@ -132,7 +132,7 @@ int main(int argc, char** argv) } { - std::function BM_CLONE_FUNC = BM_KINEMATIC_GROUP_COPY; + std::function BM_CLONE_FUNC = BM_KINEMATIC_GROUP_COPY; std::string name = "BM_KINEMATIC_GROUP_COPY"; benchmark::RegisterBenchmark(name.c_str(), BM_CLONE_FUNC, kinematic_group) ->UseRealTime() diff --git a/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp index 2a9f6f53c72..2a8e8b1be82 100644 --- a/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp +++ b/tesseract_environment/test/benchmarks/kinematics_benchmarks.cpp @@ -93,7 +93,7 @@ static void BM_CALC_FWD_KIN_MANIP(benchmark::State& state, CalcStateFn fn, const } static void BM_GET_JACOBIAN_MANIP(benchmark::State& state, - tesseract_kinematics::JointGroup::Ptr manip, + tesseract_kinematics::JointGroup::ConstPtr manip, const tesseract_common::TrajArray& traj, std::string link_name) { @@ -158,7 +158,7 @@ int main(int argc, char** argv) 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"); + tesseract_kinematics::JointGroup::ConstPtr joint_group = env->getJointGroup("manipulator"); std::string tip_link{ "tool0" }; ////////////////////////////////////// @@ -220,7 +220,7 @@ int main(int argc, char** argv) } { std::function + benchmark::State&, tesseract_kinematics::JointGroup::ConstPtr, 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)