KatanaKinematicsDecisionAlgorithms.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2006 by Tiziano Mueller   *
00003  *   tiziano.mueller@neuronics.ch   *
00004  *                                                                         *
00005  *   This program is free software; you can redistribute it and/or modify  *
00006  *   it under the terms of the GNU General Public License as published by  *
00007  *   the Free Software Foundation; either version 2 of the License, or     *
00008  *   (at your option) any later version.                                   *
00009  *                                                                         *
00010  *   This program is distributed in the hope that it will be useful,       *
00011  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
00012  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
00013  *   GNU General Public License for more details.                          *
00014  *                                                                         *
00015  *   You should have received a copy of the GNU General Public License     *
00016  *   along with this program; if not, write to the                         *
00017  *   Free Software Foundation, Inc.,                                       *
00018  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
00019  ***************************************************************************/
00020 
00021 #include "KNI_InvKin/KatanaKinematicsDecisionAlgorithms.h"
00022 #include "common/MathHelperFunctions.h"
00023 
00024 namespace KNI {
00025 
00026 
00027 KinematicsDefaultEncMinAlgorithm::t_iter
00028 KinematicsDefaultEncMinAlgorithm::operator() (t_iter targetEnc_begin, t_iter targetEnc_end, c_iter currentEnc_begin, c_iter currentEnc_end ) {
00029         // assert on distance(tE_beg, tE_enc) != distance(cE_beg, cE_enc)
00030         //assert(::std::distance(targetEnc_begin, targetEnc_end) == ::std::distance(currentEnc_begin, currentEnc_end) && "Numbers of target encoders and current encoders provided don't match" );
00031         using namespace KNI_MHF;
00032         double dist(0), sum(0), mindist=1000000;
00033         t_iter index = targetEnc_end;
00034 
00035         for(t_iter target = targetEnc_begin; target != targetEnc_end; ++target) {
00036                 sum = 0;
00037 
00038                 c_iter t = (*target).begin();
00039                 c_iter c = currentEnc_begin;
00040                 while( t != (*target).end() && c != currentEnc_end) {
00041                         sum += pow2<double>( *t - *c);
00042                         ++t;
00043                         ++c;
00044                 }
00045                 dist = sqrt(sum);
00046                 if(dist < mindist) {
00047                         index = target;
00048                         mindist = dist;
00049                 }
00050         }
00051 
00052         return index;
00053 }
00054 
00055 
00056 }


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Mon Oct 6 2014 10:45:32