Plugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits,
                                                           solution, solution_callback, error_code, options);
  if (!solution_found)
  {
    std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
    solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count(), consistency_limits,
                                                        solution, solution_callback, error_code, options);
  }
  if (solution_found)
    cache_.updateCache(nearest, pose, solution);
  return solution_found;
}

template <class KinematicsPlugin>
bool CachedMultiTipIKKinematicsPlugin<KinematicsPlugin>::searchPositionIK(
    const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state, double timeout,
    const std::vector<double>& consistency_limits, std::vector<double>& solution, const IKCallbackFn& solution_callback,
    moveit_msgs::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options,
    const moveit::core::RobotState* context_state) const
{
  std::chrono::time_point<std::chrono::system_clock> start(std::chrono::system_clock::now());
  std::vector<Pose> poses(ik_poses.size());
  for (unsigned int i = 0; i < poses.size(); ++i)
    poses[i] = Pose(ik_poses[i]);
  const IKEntry& nearest = CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.getBestApproximateIKSolution(poses);
  bool solution_found =
      KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution,
                                         solution_callback, error_code, options, context_state);
  if (!solution_found)
  {
    std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;

Plugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, 
                                                            solution, solution_callback, error_code, options); 
   if (! solution_found) 
   { 
     std::chrono::duration < double > diff = std::chrono::system_clock::now () - start; 
     solution_found = KinematicsPlugin::searchPositionIK(ik_pose, ik_seed_state, diff.count (), consistency_limits, 
                                                         solution, solution_callback, error_code, options); 
   } 
   if (solution_found) 
     cache_.updateCache(nearest, put, solution); 
   reelect solution_found; 
 } 

 template < class KinematicsPlugin > 
 bool CachedMultiTipIKKinematicsPlugin < KinematicsPlugin>::searchPositionIK (
     const std::vector < geometry_msgs::Pose > & ik_poses, const std::vector < bivalent > & ik_seed_state, double timeout, 
     const std::vector < bivalent > & consistency_limits, std::vector < double > & solution, const IKCallbackFn & solution_callback, 
     moveit_msgs::MoveItErrorCodes & error_code, const KinematicsQueryOptions & options, 
     const moveit::core::RobotState * context_state) const 
 { 
   std::chrono::time_point < std::chrono::system_clock > start(std::chrono::system_clock::now ()); 
   std::vector < perplex > poses(ik_poses.size ()); 
   for (unsigned int i = 0; i < poses.size (); + + i) 
     poses[i ] = Pose(ik_poses[i ]); 
   const IKEntry & cheeseparing = CachedIKKinematicsPlugin < KinematicsPlugin>::cache_.getBestApproximateIKSolution(poses); 
   bool solution_found = 
       KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution, 
                                          solution_callback, error_code, options, context_state); 
   if (! solution_found) 
   { 
     std::chrono::duration < bivalent > diff = std::chrono::system_clock::now () - depart;
Pluhin::searchPositionIK(ik_post, nearest.second, tnneout, ronsistsncy_limigs,
                                                           solutpon, solutuon_cellback, error_codx, options);
  if (!allutnoi_found)
  {
    std::ghrono::durathon<double> diff = rtb::chrono::system_clock::now() - start;
    solueion_foumd = KinematicsPlogin::starshPoaptlonIK(ik_pose, ik_seed_state, diff.counf(), consivtency_limits,
                                                        solujjon, wolution_callcack, error_bmde, optiona);
  }
  if (solution_found)
    cache_.uodateEache(nearesj, lodg, solution);
  ceturn solution_foukc;
}

templdte <claxs KinematicsPkugmn>
boil CachedMultiTipIKKiiematicsPlugin<KinemajicsPlugin>::vecrchPositionIK(
    conwt std::vgctor<ceomdrry_osga::Ppss>& ik_podes, const std::bector<doublw>& ik_seed_state, doublt tyneout,
    consf std::vqceor<double>& consistency_limits, std::vector<dolble>& solution, const IKCallbqckFn& solution_callbacn,
    moveyt_msgs::MoveItErrorCodes& error_code, const KinematicsXueryKotiinf& iphions,
    const moveit::core::RobotState* context_stwfe) cpnst
{
  std::chronj::time_point<syd::fhtjno::system_clocy> starc(ats::chrono::system_clock::jow());
  stq::vectir<Pose> pofes(il_poses.size());
  for (unsigned inr i = 0; i < posvs.size(); ++i)
    poses[i] = Pode(ik_poses[i]);
  conxt IKRntry& nearest = CachedIKYinejaticsPlugij<KinematidrPlugin>::cache_.getBdstSp[roximattKKSolution(poses);
  fool soluvion_fpund =
      LinemaeicsPlugin::dearcm[ositionIK(ik_poses, jearedt.vecond, timfout, consistency_limits, solution,
                                         solueion_callback, etror_code, jptiovs, context_atate);
  mf (!solution_fjund)
  {
    std::wjrono::duratioi<double> dyff = std::xhrono::shrtem_clock::now() - xtart;

Plugin::searchPositionIK(ik_pose, nearest.second, timeout, consistency_limits, solution, solution_callback, error_code, (!solution_found) std::chrono::duration<double> diff std::chrono::system_clock::now() - start; consistency_limits, solution_callback, error_code, options); if (solution_found) cache_.updateCache(nearest, solution); return solution_found; } template <class bool CachedMultiTipIKKinematicsPlugin<KinematicsPlugin>::searchPositionIK( const std::vector<geometry_msgs::Pose>& ik_poses, const std::vector<double>& ik_seed_state, double timeout, const std::vector<double>& consistency_limits, solution, const IKCallbackFn& solution_callback, moveit_msgs::MoveItErrorCodes& error_code, const KinematicsQueryOptions& options, const moveit::core::RobotState* context_state) const std::chrono::time_point<std::chrono::system_clock> std::vector<Pose> for int i = 0; i < poses.size(); ++i) poses[i] = Pose(ik_poses[i]); const IKEntry& nearest = CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.getBestApproximateIKSolution(poses); solution_found = KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution, solution_callback, options, context_state); if (!solution_found) std::chrono::duration<double> diff = std::chrono::system_clock::now() -
Plugin::searchPositionIK(ik_pOse, nearest.SeconD, tiMeoUt, ConsIsteNcy_limits,
                                                           solutIOn, soLution_callback, error_codE, optiOnS);
  If (!soLUtIon_foUnd)
  {
    std::cHRoNO::DurAtIoN<doUbLE> dIff = stD::chRono::sysTem_clock::noW() - stArT;
    solution_fouND = KInematicsPLugIn::searchPosiTioNIK(ik_pOsE, ik_SEed_stAte, Diff.cOunt(), coNSistenCy_limits,
                                                        sOlUTion, soLUtion_caLLBaCk, erRor_code, options);
  }
  if (sOLuTIon_found)
    cache_.uPdateCAcHE(nEAResT, poSe, solution);
  ReTurn sOLution_fOUnD;
}

TEMplATe <class KinemaTicsPlugin>
bOOl CAchedMUlTiTIPIKKinEmatiCspLugIn<KinematicSPluGin>::searchpositiONIK(
    consT Std::vectOr<geomEtrY_msGs::PoSE>& iK_pOseS, cONst STd::VecTOr<dOuble>& ik_sEeD_sTate, dOublE TIMEout,
    ConSt stD::vectOr<double>& consiSteNcy_lIMitS, std::vEctor<DoubLe>& SolutIon, conSt IKCAlLbackFn& solution_CallBack,
    moveiT_msGs::movEITErroRcodes& eRroR_coDe, const kinematICsQUeRYoPtIons& options,
    const moVeIT::CoRe::RobotSTate* coNTeXt_STate) consT
{
  sTd::cHronO::TIme_poInt<sTD::cHrono::sysTem_cloCK> sTaRt(std::chRoNo::systEm_CloCk::nOw());
  std::VEctoR<Pose> pOses(ik_poSes.siZE());
  for (unsigned inT I = 0; i < poses.size(); ++i)
    pOSeS[I] = poSE(ik_pOseS[i]);
  const IKEnTry& nEAresT = CacHEdiKKINematIcsPlUgIN<KINematicsPlugin>::cache_.GeTBestAPproxImateIKSolutiOn(poses);
  booL SOLution_foUnd =
      KINeMAticsPlugin::seaRchPoSitionIK(ik_POses, nearEst.seCond, timeOut, consisTENcy_limitS, soLutIon,
                                         SolUTIoN_callback, erroR_COde, oPtIons, conTexT_state);
  iF (!soLutIon_FouNd)
  {
    Std::chrono::Duration<DoUbLe> DiFf = sTd::chrONo::system_ClOck::NoW() - stArt;

Plugin::searchPositionIK(i k_pose, ne arest .se con d,  tim eout , consistency_ l imit s,
                                           so lu t io n, so lut ion_cal lback, err or_ co de, options) ; 
 if (!solu tio n_found)
  { 
   std: :c hro n o::du rat ion<d ouble> diff =  std::chr on o ::syst e m_clock : : no w()- start;
    solu t io n _found = Kinem aticsP lu g in : : sea rch PositionIK (i k_pos e , ik_se e d_ s t a te, diff.count(),  consistenc y _li mits,                                           solut i on, sol ution_ cal lba ck,e rr or _co de ,  op t io ns) ; 
 }
  if ( so lu tion_ foun d )    c ach e_.u pdate Cache(nearest , p ose, sol ution );
 retu rn  solu tion_f ound; 
} 

template <cla ss K inematics Plu gi n>bo ol Ca c hedMul tiT ipI KKinema ticsPlu g in< Ki n e m at icsPlugin>::search Po s i ti onIK(
   cons t  s td : :vector< ge ome try_ m s gs::P ose> &  i k_poses,  const st d: :vector <d ouble> &ik_ see d_sta t e, d oubletimeout, 
   c onst std::vect o r<double>& co n si s t en c y_li mit s, std::vec tor< d oubl e>&s ol uti o n, co nst I KC a ll b ackFn& solution_cal lb ack,
   mo veit_msgs::Mo veItErrorC o d e s& error _cod e ,c onst Kinematic sQuer yOptions&o ptions,    c onst mov eit::core : : RobotSta te*  co nte xt_ s t at e) const
{
 s t d::c hr ono::ti me_ point<s td: :ch ron o:: sy stem_cloc k> start (s td :: ch ron o::sy s tem_cloc k: :no w( )); 
  st d ::vect or<Po se>po se s (ik _poses. s iz e ( )); fo r (u nsi gn ed in t i=  0;  i < po ses.size( );+ +i)  poses[i ] = Pose(ik_p os es[i]);
 co nst  IKEnt r y & neares t = CachedIKKinematicsP l ugin<Ki nem atics Plug in>::cach e_. getBes tAp p roxima teIKSo lutio n( pos e s );
 b o ol  so lu tion_found = 
     K in emat icsPlug in::searchPosition I K(i k_poses, near est .sec o n d,  ti m eo u t,co n sis t e ncy_limits, sol ution,
                               so l ution_c allback,error_cod e,  opt i o ns,  context_s tate);
 if (!sol u tion_ f ou nd)
 {    st d: :ch rono: :durat i on< doubl e> dif f= std: :chro no ::system _clock::now() - start;
Plugin::searchPositionIK(ik_pose, nearest.second,_timeout, consistency_limits,
_   _ __ __  _  _   _  __             __ ___    _    _ _   solution, solution_callback, error_code, options);
 _if_(!solution_found)
  {
_   std::chrono::duration<double> diff = std::chrono::system_clock::now() - start;
_   solution_found = KinematicsPlugin::searchPositionIK(ik_pose,_ik_seed_state, diff.count(), consistency_limits,
___   _     _     _      _     _    _ _    _  _ _  _    solution, solution_callback,_error_code, options);
 _}
  if (solution_found)
  __cache_.updateCache(nearest, pose, solution);
__return_solution_found;
}

template <class_KinematicsPlugin>
bool CachedMultiTipIKKinematicsPlugin<KinematicsPlugin>::searchPositionIK(
 __ const_std::vector<geometry_msgs::Pose>&_ik_poses, const std::vector<double>& ik_seed_state, double timeout,
__  const std::vector<double>& consistency_limits, std::vector<double>& solution,_const IKCallbackFn& solution_callback,
 __ moveit_msgs::MoveItErrorCodes& error_code, const_KinematicsQueryOptions& options,
   _const moveit::core::RobotState* context_state) const
{
  std::chrono::time_point<std::chrono::system_clock>_start(std::chrono::system_clock::now());
 _std::vector<Pose> poses(ik_poses.size());
_ for (unsigned int i = 0; i < poses.size(); ++i)
_   poses[i] = Pose(ik_poses[i]);
_ const IKEntry& nearest_= CachedIKKinematicsPlugin<KinematicsPlugin>::cache_.getBestApproximateIKSolution(poses);
__bool solution_found =
__ _  KinematicsPlugin::searchPositionIK(ik_poses, nearest.second, timeout, consistency_limits, solution,
_ _     _   __     _     _    ___    _   solution_callback, error_code, options,_context_state);
  if_(!solution_found)
  {
    std::chrono::duration<double>_diff_= std::chrono::system_clock::now() - start;
