From a068bd03d3f75da52fe9b89e9e3c50363f301bab Mon Sep 17 00:00:00 2001 From: Timon Engelke Date: Mon, 15 Apr 2024 11:27:48 +0200 Subject: [PATCH] Add ik_time to ReachRecord --- include/reach/types.h | 6 +++++- include/reach/utils.h | 6 +++--- src/python/python_bindings.cpp | 1 + src/reach_study.cpp | 8 ++++---- src/reach_visualizer.cpp | 5 +++-- src/types.cpp | 7 ++++--- src/utils.cpp | 23 +++++++++++++++-------- 7 files changed, 35 insertions(+), 21 deletions(-) diff --git a/include/reach/types.h b/include/reach/types.h index a05257c6..ce9a0040 100644 --- a/include/reach/types.h +++ b/include/reach/types.h @@ -32,7 +32,7 @@ class ReachRecord public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW ReachRecord(const bool reached, const Eigen::Isometry3d& goal, const std::map seed_state, - const std::map goal_state, const double score); + const std::map goal_state, const double score, const double ik_time); ReachRecord() = default; ReachRecord(const ReachRecord&) = default; ReachRecord& operator=(const ReachRecord&) = default; @@ -55,6 +55,9 @@ class ReachRecord /** @brief Reachability score associated with this target pose */ double score; + /** @brief Time taken to solve the IK for this target pose */ + double ik_time; + private: friend class boost::serialization::access; @@ -66,6 +69,7 @@ class ReachRecord ar& BOOST_SERIALIZATION_NVP(seed_state); ar& BOOST_SERIALIZATION_NVP(goal_state); ar& BOOST_SERIALIZATION_NVP(score); + ar& BOOST_SERIALIZATION_NVP(ik_time); } }; diff --git a/include/reach/utils.h b/include/reach/utils.h index 99495277..938a4a2d 100644 --- a/include/reach/utils.h +++ b/include/reach/utils.h @@ -39,9 +39,9 @@ static std::map zip(const std::vector& keys, const return map; } -std::tuple, double> evaluateIK(const Eigen::Isometry3d& target, - const std::map& seed, - IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator); +std::tuple, double, double> evaluateIK(const Eigen::Isometry3d& target, + const std::map& seed, + IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator); using SearchTreePtr = pcl::search::KdTree::Ptr; SearchTreePtr createSearchTree(const VectorIsometry3d& poses); diff --git a/src/python/python_bindings.cpp b/src/python/python_bindings.cpp index 574dd985..261d34fa 100644 --- a/src/python/python_bindings.cpp +++ b/src/python/python_bindings.cpp @@ -257,6 +257,7 @@ BOOST_PYTHON_MODULE(MODULE_NAME) .def( "goal", +[](const ReachRecord& r) -> bp::numpy::ndarray { return fromEigen(r.goal); }) .def_readwrite("score", &ReachRecord::score) + .def_readwrite("ik_time", &ReachRecord::ik_time) .def_readwrite("goal_state", &ReachRecord::goal_state) .def_readwrite("seed_state", &ReachRecord::seed_state) .def_readwrite("reached", &ReachRecord::reached); diff --git a/src/reach_study.cpp b/src/reach_study.cpp index 0c9e0b4a..0949ae15 100644 --- a/src/reach_study.cpp +++ b/src/reach_study.cpp @@ -114,10 +114,10 @@ void ReachStudy::run() try { std::vector solution; - double score; - std::tie(solution, score) = evaluateIK(tgt_frame, params_.seed_state, ik_solver_, evaluator_); + double score, ik_time; + std::tie(solution, score, ik_time) = evaluateIK(tgt_frame, params_.seed_state, ik_solver_, evaluator_); - ReachRecord msg(true, tgt_frame, params_.seed_state, zip(ik_solver_->getJointNames(), solution), score); + ReachRecord msg(true, tgt_frame, params_.seed_state, zip(ik_solver_->getJointNames(), solution), score, ik_time); { std::lock_guard lock{ mutex_ }; active_result->operator[](i) = msg; @@ -125,7 +125,7 @@ void ReachStudy::run() } catch (const std::exception&) { - ReachRecord msg(false, tgt_frame, params_.seed_state, params_.seed_state, 0.0); + ReachRecord msg(false, tgt_frame, params_.seed_state, params_.seed_state, 0.0, 0.0); { std::lock_guard lock{ mutex_ }; active_result->operator[](i) = msg; diff --git a/src/reach_visualizer.cpp b/src/reach_visualizer.cpp index c54fffb8..8114cccf 100644 --- a/src/reach_visualizer.cpp +++ b/src/reach_visualizer.cpp @@ -44,11 +44,12 @@ void ReachVisualizer::reSolveIK(const std::size_t record_idx) // Re-solve IK at the selected marker std::vector goal_pose; - double score; - std::tie(goal_pose, score) = evaluateIK(lookup.goal, lookup.seed_state, solver_, evaluator_); + double score, ik_time; + std::tie(goal_pose, score, ik_time) = evaluateIK(lookup.goal, lookup.seed_state, solver_, evaluator_); lookup.reached = true; lookup.score = score; + lookup.ik_time = ik_time; lookup.goal_state = zip(solver_->getJointNames(), goal_pose); // Update the interactive marker server diff --git a/src/types.cpp b/src/types.cpp index 330da56f..d141a16d 100644 --- a/src/types.cpp +++ b/src/types.cpp @@ -71,8 +71,8 @@ namespace reach { ReachRecord::ReachRecord(const bool reached_, const Eigen::Isometry3d& goal_, const std::map seed_state_, - const std::map goal_state_, const double score_) - : reached(reached_), goal(goal_), seed_state(seed_state_), goal_state(goal_state_), score(score_) + const std::map goal_state_, const double score_, const double ik_time_) + : reached(reached_), goal(goal_), seed_state(seed_state_), goal_state(goal_state_), score(score_), ik_time(ik_time_) { } @@ -83,8 +83,9 @@ bool ReachRecord::operator==(const ReachRecord& rhs) const const bool goal_states_match = isApprox(goal_state, rhs.goal_state); const bool seed_states_match = isApprox(seed_state, rhs.seed_state); const bool scores_match = std::abs(score - rhs.score) < std::numeric_limits::epsilon(); + const bool times_match = std::abs(ik_time - rhs.ik_time) < std::numeric_limits::epsilon(); - return reach_match && goals_match && goal_states_match && seed_states_match && scores_match; + return reach_match && goals_match && goal_states_match && seed_states_match && scores_match && times_match; } ReachResultSummary calculateResults(const ReachResult& db) diff --git a/src/utils.cpp b/src/utils.cpp index b42afe6f..4b2a3f8d 100644 --- a/src/utils.cpp +++ b/src/utils.cpp @@ -16,14 +16,20 @@ #include #include +#include namespace reach { -std::tuple, double> evaluateIK(const Eigen::Isometry3d& target, - const std::map& seed, - IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator) +std::tuple, double, double> evaluateIK(const Eigen::Isometry3d& target, + const std::map& seed, + IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator) { + auto ik_start_time = std::chrono::high_resolution_clock::now(); std::vector> poses = ik_solver->solveIK(target, seed); + auto ik_end_time = std::chrono::high_resolution_clock::now(); + std::chrono::duration ik_duration = ik_end_time - ik_start_time; + double ik_time = ik_duration.count(); + const std::vector joint_names = ik_solver->getJointNames(); double best_score = 0.0; std::size_t best_idx = 0; @@ -38,7 +44,7 @@ std::tuple, double> evaluateIK(const Eigen::Isometry3d& targ } } - return std::make_tuple(poses.at(best_idx), best_score); + return std::make_tuple(poses.at(best_idx), best_score, ik_time); } SearchTreePtr createSearchTree(const VectorIsometry3d& poses) @@ -121,8 +127,8 @@ std::map reachNeighborsDirect(const ReachResult& db, c { // Use current point's IK solution as seed std::vector new_solution; - double score; - std::tie(new_solution, score) = evaluateIK(it->second.goal, rec.goal_state, solver, evaluator); + double score, ik_time; + std::tie(new_solution, score, ik_time) = evaluateIK(it->second.goal, rec.goal_state, solver, evaluator); // Update the record if (!it->second.reached || score > it->second.score) @@ -131,6 +137,7 @@ std::map reachNeighborsDirect(const ReachResult& db, c it->second.seed_state = rec.goal_state; it->second.goal_state = zip(solver->getJointNames(), new_solution); it->second.score = score; + it->second.ik_time = ik_time; } ++it; } @@ -177,8 +184,8 @@ void reachNeighborsRecursive(const ReachResult& db, const ReachRecord& rec, IKSo // Use current point's IK solution as seed std::vector new_pose; - double score; - std::tie(new_pose, score) = evaluateIK(neighbor.goal, rec.goal_state, solver, evaluator); + double score, ik_time; + std::tie(new_pose, score, ik_time) = evaluateIK(neighbor.goal, rec.goal_state, solver, evaluator); // Store information in new reach record object // neighbor.seed_state = rec.goal_state;