KatanaKinematics6M90T.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/KatanaKinematics6M90T.h"
00022 #include "common/MathHelperFunctions.h"
00023 #include <algorithm>
00024 
00025 namespace KNI {
00026 
00027 const double KatanaKinematics6M90T::_tolerance = 0.001;
00028 const int KatanaKinematics6M90T::_nrOfPossibleSolutions = 8;
00029 
00030 void
00031 KatanaKinematics6M90T::DK(coordinates& solution, encoders const& current_encoders) const {
00032         using namespace KNI_MHF;
00033         // numering the angles starting by 0-5
00034 
00035         double x0, x1, x2, x3;
00036         double y0, y1, y2, y3;
00037         double z0, z1, z2, z3;
00038 
00039         angles current_angles(current_encoders.size());
00040         for(unsigned int z = 0; z < current_angles.size(); ++z) {
00041                 current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
00042         }
00043 
00044         // needs refactoring:
00045         current_angles[1] = current_angles[1] - M_PI/2.0;
00046         current_angles[2] = current_angles[2] - M_PI;
00047         current_angles[3] = M_PI - current_angles[3];
00048         current_angles[5] = -current_angles[5];
00049 
00050         coordinates pose(6);
00051 
00052         angles cx(current_angles.size()), sx(current_angles.size());
00053         angles::iterator cx_iter, sx_iter;
00054 
00055         angles angle = current_angles;
00056 
00057         angle[2] = angle[1]+angle[2];
00058         angle[3] = angle[2]+angle[3];
00059 
00060         cx_iter = cx.begin();
00061         sx_iter = sx.begin();
00062         std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
00063         std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
00064 
00065 
00066         //x
00067         x0 =  cx[0]*sx[1];
00068         x1 =  cx[0]*sx[2];
00069         x2 =  cx[0]*sx[3];
00070         x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
00071         pose[0] = x0*_length[0]+x1*_length[1]+x2*_length[2]+x3*_length[3];
00072 
00073         //y
00074         y0 = sx[0]*sx[1];
00075         y1 = sx[0]*sx[2];
00076         y2 = sx[0]*sx[3];
00077         y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
00078         pose[1] = y0*_length[0]+y1*_length[1]+y2*_length[2]+y3*_length[3];
00079 
00080         //z
00081         z0 = cx[1];
00082         z1 = cx[2];
00083         z2 = cx[3];
00084         z3 = cx[4]*sx[3];
00085         pose[2] = z0*_length[0]+z1*_length[1]+z2*_length[2]+z3*_length[3];
00086 
00087 
00088         //theta
00089         pose[4] = acos(cx[4]*sx[3]);
00090 
00091         // phi & psi
00092 
00093 
00094         const double theta1 = angle[0];
00095         const double theta5 = angle[4];
00096         const double theta6 = angle[5];
00097         const double theta234 = angle[3];
00098 
00099         if( std::abs(pose[4])<_tolerance || std::abs(pose[4]-M_PI)<_tolerance ) { // catch the case where theta=0, resp. theta=180
00100                 //phi
00101                 angles v1(2), v2(2);
00102 
00103                 double R11 = -sin(theta1)*cos(theta5)  *sin(theta6) + cos(theta1)*(sin(theta234)*cos(theta6)+cos(theta234)*sin(theta5)*sin(theta6));
00104                 double R21 =  sin(theta1)*sin(theta234)*cos(theta6) + sin(theta6)*(cos(theta1)  *cos(theta5)+cos(theta234)*sin(theta1)*sin(theta5));
00105 
00106                 v1[0] = acos( R11 );
00107                 v1[1] = -v1[0];
00108                 v2[0] = asin( R21 );
00109                 v2[1] = M_PI - v2[0];
00110 
00111                 pose[3] = anglereduce(findFirstEqualAngle(v1, v2));
00112 
00113                 //psi
00114                 pose[5] = 0;
00115         } else {
00116                 //phi
00117                 const double R13 = -cos(theta1)*cos(theta234)*cos(theta5) - sin(theta1)*sin(theta5);
00118                 const double R23 = -sin(theta1)*cos(theta234)*cos(theta5) + cos(theta1)*sin(theta5);
00119                 pose[3] = atan2(R13, -R23);
00120 
00121                 //psi
00122                 const double R31 =  cos(theta234)*cos(theta6) - sin(theta234)*sin(theta5)*sin(theta6);
00123                 const double R32 = -cos(theta234)*sin(theta6) - sin(theta234)*sin(theta5)*cos(theta6);
00124                 pose[5] = atan2(R31, R32);
00125         }
00126 
00127         std::swap(solution, pose);
00128 }
00129 
00130 void
00131 KatanaKinematics6M90T::init( metrics const& length, parameter_container const& parameters ) {
00132         assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" ); // we have 4 links
00133         assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
00134         _setLength( length );
00135         _setParameters ( parameters );
00136 }
00137 
00138 
00139 void
00140 KatanaKinematics6M90T::IK_theta234theta5(angles_calc& angle, const position &p_gr) const {
00141         using namespace KNI_MHF;
00142 
00143         if(p_gr.z==0) {
00144                 angle.theta234=0;
00145                 angle.theta5=angle.theta1-atan1(-p_gr.x,-p_gr.y);
00146         } else {
00147                 angle.theta234 = -acotan( (  (p_gr.x * p_gr.z * cos(angle.theta1) ) -
00148                                      sqrt( ( -pow2(p_gr.z) ) *
00149                                            ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
00150                                          )
00151                                   ) / pow2(p_gr.z)
00152                                 );
00153                 angle.theta5   = acos( p_gr.z/(_length[3]*sin(angle.theta234)) );
00154         }
00155 
00156         bool griptest;
00157         griptest = GripperTest(p_gr, angle);
00158         if(!griptest) {
00159                 angle.theta5=-angle.theta5;
00160                 griptest=GripperTest(p_gr, angle);
00161                 if(!griptest) {
00162                         angle.theta234 = -acotan( (   ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
00163                                                       sqrt( ( -pow2(p_gr.z) ) *
00164                                                             ( -pow2(_length[3]) + pow2(p_gr.x) + pow2(p_gr.z) ) * pow2(sin(angle.theta1))
00165                                                           )
00166                                                   ) / pow2(p_gr.z)
00167                                                 );
00168                         angle.theta5 =  acos( p_gr.z / (_length[3]*sin(angle.theta234)) );
00169                         if(p_gr.z==0) {
00170                                 angle.theta234=-M_PI;
00171                                 angle.theta5=atan1(p_gr.x,p_gr.y) - angle.theta1;
00172                         }
00173 
00174                         griptest=GripperTest(p_gr, angle);
00175                         if(!griptest) {
00176                                 angle.theta5=-angle.theta5;
00177                         }
00178                 }
00179         }
00180 
00181 }
00182 
00183 bool
00184 KatanaKinematics6M90T::GripperTest(const position &p_gr, const angles_calc &angle) const {
00185         using namespace KNI_MHF;
00186         double xgr2, ygr2, zgr2;
00187 
00188         xgr2 = -_length[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
00189         ygr2 = -_length[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
00190         zgr2 =  _length[3]*sin(angle.theta234)*cos(angle.theta5);
00191 
00192         if((pow2(p_gr.x-xgr2)+pow2(p_gr.y-ygr2)+pow2(p_gr.z-zgr2))>=_tolerance)
00193                 return false;
00194 
00195         return true;
00196 }
00197 
00198 void
00199 KatanaKinematics6M90T::IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const {
00200         using namespace KNI_MHF;
00201 
00202         double xg, yg, zg;
00203         double d5 = _length[2] + _length[3];
00204         xg = p.x + ( _length[3] * cos(angle.theta1) * sin(angle.theta234) );
00205         yg = p.y + ( _length[3] * sin(angle.theta1) * sin(angle.theta234) );
00206         zg = p.z + ( _length[3]                     * cos(angle.theta234) );
00207 
00208 
00209         angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
00210         angle.b2 = zg - d5*cos(angle.theta234);
00211         angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
00212 
00213 }
00214 
00215 double
00216 KatanaKinematics6M90T::findFirstEqualAngle(const angles& v1, const angles& v2) const {
00217         using namespace KNI_MHF;
00218         for(angles::const_iterator i = v1.begin(); i != v1.end(); ++i) {
00219                 for(angles::const_iterator j = v2.begin(); j != v2.end(); ++j) {
00220                         if(std::abs(anglereduce(*j) - anglereduce(*i)) < _tolerance)
00221                                 return *i;
00222                 }
00223         }
00224         assert(!"precondition for findFirstEqualAngle failed -> no equal angles found");
00225         return 0;
00226 }
00227 
00228 
00229 void
00230 KatanaKinematics6M90T::thetacomp(angles_calc &angle, const position &p_m, const coordinates& pose) const {
00231         using namespace KNI_MHF;
00232 
00233         const double theta1   = angle.theta1;
00234         double theta2   = 0;
00235         const double theta3   = angle.theta3;
00236         double theta4   = 0;
00237         const double theta5   = angle.theta5;
00238         double theta6   = 0;
00239         const double theta234 = angle.theta234;
00240         const double b1       = angle.b1;
00241         const double b2       = angle.b2;
00242 
00243         const double phi   = pose[3];
00244         const double theta = pose[4];
00245         const double psi   = pose[5];
00246 
00247 
00248         theta2 = -M_PI/2.0 - ( atan0(b1, b2)+atan0(_length[0]+_length[1]*cos(theta3),_length[1]*sin(theta3)) );
00249         theta4 = theta234 - theta2 - theta3;
00250 
00251         if(!PositionTest6MS(theta1, theta2, theta3, theta234 ,p_m)) {
00252                 theta2 = theta2+M_PI;
00253                 theta4 = theta234 - theta2 - theta3;
00254         }
00255 
00256         const double R11 = cos(phi)*cos(psi) - sin(phi)*cos(theta)*sin(psi);
00257         const double R21 = sin(phi)*cos(psi) + cos(phi)*cos(theta)*sin(psi);
00258 
00259         std::vector<double> theta16c(2), theta16s(2);
00260 
00261         if(std::abs(theta234 + M_PI/2) < _tolerance) {
00262                 if(std::abs(theta5) < _tolerance) {
00263                         theta16c[0] = acos(-R11);
00264                         theta16c[1] = -theta16c[0];
00265                         theta16s[0] = asin(-R21);
00266                         theta16s[1] = M_PI - theta16s[0];
00267 
00268                         theta6 = theta1 - findFirstEqualAngle(theta16c, theta16s);
00269 
00270                 } else if(std::abs(theta5-M_PI) < _tolerance) {
00271                         theta16c[0] = acos(-R11);
00272                         theta16c[1] = -theta16c[0];
00273                         theta16s[0] = asin(-R21);
00274                         theta16s[1] = M_PI - theta16s[0];
00275 
00276                         theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
00277 
00278                 } else {
00279                         assert(!"Special case \"|theta234+(1/2)*pi| = 0\" detected, but no solution found");
00280                 }
00281 
00282         } else if(std::abs(theta234 + 3*M_PI/2) < _tolerance) {
00283                 if(std::abs(theta5) < _tolerance) {
00284                         theta16c[0] = acos(R11);
00285                         theta16c[1] = -theta16c[0];
00286                         theta16s[0] = asin(R21);
00287                         theta16s[1] = M_PI - theta16s[0];
00288 
00289                         theta6  = findFirstEqualAngle(theta16c, theta16s) - theta1;
00290 
00291                 } else if(std::abs(theta5-M_PI ) < _tolerance) {
00292                         theta16c[0] = acos(R11);
00293                         theta16c[1] = -theta16c[0];
00294                         theta16s[0] = asin(R21);
00295                         theta16s[1] = M_PI -theta16s[0];
00296 
00297                         theta6  = - theta1 - findFirstEqualAngle(theta16c, theta16s);
00298                 } else {
00299                         assert(!"Special case \"|theta234+(3/2)*pi| = 0\" detected, but no solution found");
00300                 }
00301 
00302         } else {
00303 
00304                 const double R31 = sin(theta)*sin(psi);
00305                 const double R32 = sin(theta)*cos(psi);
00306 
00307                 const double temp1 =  cos(theta234);
00308                 const double temp2 = -sin(theta234)*sin(theta5);
00309 
00310                 const double c = ( R31*temp1 + R32*temp2 ) / ( pow2(temp1) + pow2(temp2) );
00311                 const double s = ( R31*temp2 - R32*temp1 ) / ( pow2(temp1) + pow2(temp2) );
00312 
00313                 theta16c[0] = acos(c);
00314                 theta16c[1] = -theta16c[0];
00315                 theta16s[0] = asin(s);
00316                 theta16s[1] = M_PI - theta16s[0];
00317 
00318                 theta6 = findFirstEqualAngle(theta16c, theta16s);
00319         }
00320 
00321 
00322         angle.theta2 = theta2;
00323         angle.theta4 = theta4;
00324         angle.theta6 = theta6;
00325 }
00326 
00327 bool
00328 KatanaKinematics6M90T::PositionTest6MS(const double& theta1, const double& theta2, const double& theta3, const double& theta234, const position &p) const {
00329         using namespace KNI_MHF;
00330         double temp, xm2, ym2, zm2;
00331 
00332         temp = _length[0]*sin(theta2) + _length[1]*sin(theta2+theta3) + _length[2]*sin(theta234);
00333         xm2  = cos(theta1)*temp;
00334         ym2  = sin(theta1)*temp;
00335         zm2  = _length[0]*cos(theta2) + _length[1]*cos(theta2+theta3) + _length[2]*cos(theta234);
00336 
00337         if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
00338                 return false;
00339 
00340         return true;
00341 }
00342 
00343 bool
00344 KatanaKinematics6M90T::angledef(angles_calc &a) const {
00345         using namespace KNI_MHF;
00346 
00347         // constants here. needs refactoring:
00348         a.theta2=anglereduce(a.theta2+M_PI/2.0);
00349         a.theta3=anglereduce(a.theta3+M_PI);
00350         a.theta4=anglereduce(M_PI-a.theta4);
00351         a.theta5=anglereduce(a.theta5);
00352         a.theta6=-a.theta6;
00353 
00354         if(a.theta1>_parameters[0].angleStop) {
00355                 a.theta1=a.theta1-2.0*M_PI;
00356         }
00357         if(a.theta2>M_PI) {
00358                 a.theta2=a.theta2-2.0*M_PI;
00359         }
00360         if(a.theta6<_parameters[5].angleOffset) {
00361                 a.theta6=a.theta6+2.0*M_PI;
00362         } else if(a.theta6>_parameters[5].angleStop) {
00363                 a.theta6=a.theta6-2.0*M_PI;
00364         }
00365         if(a.theta5<_parameters[4].angleOffset) {
00366                 a.theta5 += 2.0*M_PI;
00367         }
00368 
00369         return AnglePositionTest(a);
00370 
00371 }
00372 
00373 bool
00374 KatanaKinematics6M90T::AnglePositionTest(const angles_calc &a) const {
00375 
00376         if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) ) {
00377                 return false;
00378         }
00379         if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) ) {
00380                 return false;
00381         }
00382         if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) ) {
00383                 return false;
00384         }
00385 
00386         if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) ) {
00387                 return false;
00388         }
00389 
00390         if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) ) {
00391                 return false;
00392         }
00393         if( (a.theta6<_parameters[5].angleOffset)||(a.theta6>_parameters[5].angleStop) ) {
00394                 return false;
00395         }
00396 
00397         return true;
00398 }
00399 
00400 
00401 
00402 
00403 void
00404 KatanaKinematics6M90T::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
00405         using namespace KNI_MHF;
00406 
00407         // pose: Winkel Deg->Rad
00408         // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
00409         // 0-3 fr theta1_1
00410         // 4-7 fr theta1_2
00411 
00412         // Declaration
00413         position p_gr;
00414         position p_m;
00415         angles_container angle(_nrOfPossibleSolutions);
00416 
00417         // calculation of the gripper vector
00418         p_gr.x = _length[3]*sin(pose[4])*sin(pose[3]);
00419         p_gr.y = -_length[3]*sin(pose[4])*cos(pose[3]);
00420         p_gr.z = _length[3]*cos(pose[4]);
00421 
00422         p_m.x = pose[0]-p_gr.x;
00423         p_m.y = pose[1]-p_gr.y;
00424         p_m.z = pose[2]-p_gr.z;
00425 
00426         // calculate theta1_1 and theta1_2
00427         angle[0].theta1 = atan1(p_m.x,p_m.y);
00428         angle[4].theta1 = angle[0].theta1+M_PI;
00429 
00430 
00431         // check the borders according to the settings
00432         if(angle[0].theta1>_parameters[0].angleStop)
00433                 angle[0].theta1=angle[0].theta1-2.0*M_PI;
00434 
00435         if(angle[0].theta1<_parameters[0].angleOffset)
00436                 angle[0].theta1=angle[0].theta1+2.0*M_PI;
00437 
00438         if(angle[4].theta1>_parameters[0].angleStop)
00439                 angle[4].theta1=angle[4].theta1-2.0*M_PI;
00440 
00441         if(angle[4].theta1<_parameters[0].angleOffset)
00442                 angle[4].theta1=angle[4].theta1+2.0*M_PI;
00443 
00444 
00445         //====THETA1_1==================
00446         //-------THETA234_1-------------
00447         IK_theta234theta5(angle[0], p_gr);
00448         IK_b1b2costh3_6MS(angle[0], p_m);
00449 
00450         angle[1]=angle[0];
00451         angle[0].theta3 =  acos(angle[0].costh3)-M_PI;
00452         thetacomp(angle[0], p_m, pose);
00453         angle[1].theta3 =  -acos(angle[1].costh3)+M_PI;
00454         thetacomp(angle[1], p_m, pose);
00455 
00456         //-------THETA234_2-------------
00457         angle[2].theta1=angle[0].theta1;
00458         angle[2].theta234=angle[0].theta234-M_PI;
00459         angle[2].theta5=M_PI-angle[0].theta5;
00460 
00461         IK_b1b2costh3_6MS(angle[2], p_m);
00462         angle[3]=angle[2];
00463         angle[2].theta3 =  acos(angle[2].costh3)-M_PI;
00464         thetacomp(angle[2], p_m, pose);
00465         angle[3].theta3 =  -acos(angle[3].costh3)+M_PI;
00466         thetacomp(angle[3], p_m, pose);
00467 
00468 
00469         //====THETA1_2==================
00470         //-------THETA234_1-------------
00471         IK_theta234theta5(angle[4], p_gr);
00472         IK_b1b2costh3_6MS(angle[4], p_m);
00473 
00474         angle[5]=angle[4];
00475         angle[4].theta3 =  acos(angle[4].costh3)-M_PI;
00476         thetacomp(angle[4], p_m, pose);
00477         angle[5].theta3 =  -acos(angle[5].costh3)+M_PI;
00478         thetacomp(angle[5], p_m, pose);
00479 
00480         //-------THETA234_2-------------
00481         angle[6].theta1=angle[4].theta1;
00482         angle[6].theta234=angle[4].theta234-M_PI;
00483         angle[6].theta5=M_PI-angle[4].theta5;
00484         IK_b1b2costh3_6MS(angle[6], p_m);
00485         angle[7]=angle[6];
00486         angle[6].theta3 =  acos(angle[6].costh3)-M_PI;
00487         thetacomp(angle[6], p_m, pose);
00488         angle[7].theta3 =  -acos(angle[7].costh3)+M_PI;
00489         thetacomp(angle[7], p_m, pose);
00490 
00491         for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
00492                 if( pow2(iter->costh3) <= 1.0) {
00493                         if(!angledef(*iter))
00494                                 iter = angle.erase(iter);
00495                         else
00496                                 ++iter;
00497                         continue;
00498                 }
00499                 iter = angle.erase(iter);
00500         }
00501 
00502 
00503         if(angle.size() == 0) {
00504                 throw NoSolutionException();
00505         }
00506 
00507         ::std::vector< ::std::vector<int> > PossibleTargetsInEncoders;
00508         for( ::std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00509                 ::std::vector<int> solution(6);
00510 
00511                 solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
00512                 solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
00513                 solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
00514                 solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
00515                 solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
00516                 solution[5] = rad2enc(i->theta6, _parameters[5].angleOffset, _parameters[5].epc, _parameters[5].encOffset, _parameters[5].rotDir);
00517                 PossibleTargetsInEncoders.push_back(solution);
00518         }
00519 
00520 
00521         ::std::vector< ::std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
00522 
00523         assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
00524 
00525 
00526         encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
00527 
00528 }
00529 
00530 
00531 
00532 } // NAMESPACE: KNI
00533 


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