6 #include "KatanaKinematicsDecisionAlgorithms.h" 7 #include "MathHelperFunctions.h" 14 double dist(0),
sum(0), mindist=1000000;
15 t_iter index = targetEnc_end;
17 for(
t_iter target = targetEnc_begin; target != targetEnc_end; ++target) {
20 c_iter t = (*target).begin();
21 c_iter c = currentEnc_begin;
22 while( t != (*target).end() && c != currentEnc_end) {
23 sum += MHF::pow2<double>( *t - *c);
40 double dist(0),
sum(0), mindist=1000000;
41 t_iter index = targetRad_end;
43 for(
t_iter target = targetRad_begin; target != targetRad_end; ++target) {
46 c_iter t = (*target).begin();
47 c_iter c = currentRad_begin;
48 while( t != (*target).end() && c != currentRad_end) {
49 sum += MHF::pow2<double>( *t - *c);
std::vector< radians >::const_iterator t_iter
encoders::const_iterator c_iter
t_iter operator()(t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end)
radians::const_iterator c_iter
t_iter operator()(t_iter targetRad_begin, t_iter targetRad_end, c_iter currentRad_begin, c_iter currentRad_end)
std::vector< encoders >::const_iterator t_iter
Real sum(const BaseMatrix &B)