Add ik_time to ReachRecord

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<std::string, double> seed_state,
-              const std::map<std::string, double> goal_state, const double score);
+              const std::map<std::string, double> 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);
   }
 };
 

include/reach/utils.h

@@ -39,9 +39,9 @@ static std::map<std::string, T> zip(const std::vector<std::string>& keys, const
   return map;
 }
 
-std::tuple<std::vector<double>, double> evaluateIK(const Eigen::Isometry3d& target,
-                                                   const std::map<std::string, double>& seed,
-                                                   IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator);
+std::tuple<std::vector<double>, double, double> evaluateIK(const Eigen::Isometry3d& target,
+                                                           const std::map<std::string, double>& seed,
+                                                           IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator);
 
 using SearchTreePtr = pcl::search::KdTree<pcl::PointXYZ>::Ptr;
 SearchTreePtr createSearchTree(const VectorIsometry3d& poses);

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);

src/reach_study.cpp

@@ -114,18 +114,18 @@ void ReachStudy::run()
     try
     {
       std::vector<double> 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<std::mutex> lock{ mutex_ };
         active_result->operator[](i) = msg;
       }
     }
     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<std::mutex> lock{ mutex_ };
         active_result->operator[](i) = msg;

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<double> 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

src/types.cpp

@@ -71,8 +71,9 @@ namespace reach
 {
 ReachRecord::ReachRecord(const bool reached_, const Eigen::Isometry3d& goal_,
                          const std::map<std::string, double> seed_state_,
-                         const std::map<std::string, double> goal_state_, const double score_)
-  : reached(reached_), goal(goal_), seed_state(seed_state_), goal_state(goal_state_), score(score_)
+                         const std::map<std::string, double> 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 +84,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<double>::epsilon();
+  const bool times_match = std::abs(ik_time - rhs.ik_time) < std::numeric_limits<double>::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)

src/utils.cpp

@@ -16,14 +16,20 @@
 #include <reach/utils.h>
 
 #include <iostream>
+#include <chrono>
 
 namespace reach
 {
-std::tuple<std::vector<double>, double> evaluateIK(const Eigen::Isometry3d& target,
+std::tuple<std::vector<double>, double, double> evaluateIK(const Eigen::Isometry3d& target,
                                                    const std::map<std::string, double>& seed,
                                                    IKSolver::ConstPtr ik_solver, Evaluator::ConstPtr evaluator)
 {
+  auto ik_start_time = std::chrono::high_resolution_clock::now();
   std::vector<std::vector<double>> poses = ik_solver->solveIK(target, seed);
+  auto ik_end_time = std::chrono::high_resolution_clock::now();
+  std::chrono::duration<double> ik_duration = ik_end_time - ik_start_time;
+  double ik_time = ik_duration.count();
+
   const std::vector<std::string> joint_names = ik_solver->getJointNames();
   double best_score = 0.0;
   std::size_t best_idx = 0;
@@ -38,7 +44,7 @@ std::tuple<std::vector<double>, 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<std::size_t, ReachRecord> reachNeighborsDirect(const ReachResult& db, c
     {
       // Use current point's IK solution as seed
       std::vector<double> 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<std::size_t, ReachRecord> 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<double> 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;