kni: kni: KatanaKinematics6M180.cpp Source 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/KatanaKinematics6M180.h"
00022 #include "common/MathHelperFunctions.h"
00023 #include <algorithm>
00024 
00025 namespace KNI {
00026 
00027 const double KatanaKinematics6M180::_tolerance = 0.001;
00028 const int KatanaKinematics6M180::_nrOfPossibleSolutions = 8;
00029 
00030 void
00031 KatanaKinematics6M180::DK(coordinates& solution, encoders const& current_encoders) const {
00032         using namespace KNI_MHF;
00033         // numering the angles starting by 0-5
00034 
00035         double factor;
00036         double R13, R23, R31, R32;
00037 
00038         angles current_angles(6);
00039         for(int z = 0; z < 6; ++z) {
00040                 current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
00041         }
00042 
00043         // needs refactoring:
00044         current_angles[1] = current_angles[1] - M_PI/2.0;
00045         current_angles[2] = current_angles[2] - M_PI;
00046         current_angles[3] = M_PI - current_angles[3];
00047         current_angles[4] = -current_angles[4];
00048 
00049         coordinates pose(6);
00050 
00051         angles cx(current_angles.size()), sx(current_angles.size());
00052         angles::iterator cx_iter, sx_iter;
00053 
00054         angles angle = current_angles;
00055 
00056         angle[2] = angle[1]+angle[2];
00057         angle[3] = angle[2]+angle[3];
00058 
00059         cx_iter = cx.begin();
00060         sx_iter = sx.begin();
00061         std::transform(angle.begin(), angle.end(), sx_iter, unary_precalc_sin<double>() );
00062         std::transform(angle.begin(), angle.end(), cx_iter, unary_precalc_cos<double>() );
00063 
00064         factor = (_length[0]*sx[1]+_length[1]*sx[2]+(_length[2]+_length[3])*sx[3]);
00065         // x = px (compare homogenous transformation matrix)
00066         pose[0] = cx[0]*factor;
00067 
00068         // y = pz (compare homogenous transformation matrix)
00069         pose[1] = sx[0]*factor;
00070 
00071         // z = pz (compare homogenous transformation matrix)
00072         pose[2] = _length[0]*cx[1]+_length[1]*cx[2]+(_length[2]+_length[3])*cx[3];
00073 
00074         // phi = atan2(R13/-R23) (compare homogenous transformatio nmatrix)
00075         R13 = cx[0]*sx[3];
00076         R23 = sx[0]*sx[3];
00077         pose[3] = atan2(R13,-R23);
00078 
00079         // theta = acos(R33) (compare homogenous transformation matrix)
00080         pose[4] = acos(cx[3]);
00081 
00082         // psi = atan2(R31/R32)  (compare homogenous transformation matrix)
00083         R31 = sx[3]*sx[4];
00084         R32 = sx[3]*cx[4];
00085         pose[5] = atan2(R31,R32);
00086 
00087 
00088         std::swap(solution, pose);
00089 }
00090 
00091 void
00092 KatanaKinematics6M180::init( metrics const& length, parameter_container const& parameters ) {
00093         assert( (length.size() == 4) && "You have to provide the metrics for exactly 4 links" ); // we have 4 links
00094         assert( (parameters.size() == 6) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
00095         _setLength( length );
00096         _setParameters ( parameters );
00097 }
00098 
00099 void
00100 KatanaKinematics6M180::IK_b1b2costh3_6M180(angles_calc &angle, const position &p) const {
00101         using namespace KNI_MHF;
00102 
00103         double d5 = _length[2] + _length[3];
00104 
00105         angle.b1 = p.x*cos(angle.theta1) + p.y*sin(angle.theta1) - d5*sin(angle.theta234);
00106         angle.b2 = p.z - d5*cos(angle.theta234);
00107         angle.costh3 = -( pow2(angle.b1) + pow2(angle.b2) - pow2(_length[0]) - pow2(_length[1]) ) / ( 2.0*_length[0]*_length[1] );
00108 
00109 }
00110 
00111 void
00112 KatanaKinematics6M180::thetacomp(angles_calc &angle, const position &p_m) const {
00113         using namespace KNI_MHF;
00114 
00115         angle.theta2 = -M_PI/2.0 - ( atan0(angle.b1,angle.b2)+atan0(_length[0]+_length[1]*cos(angle.theta3),_length[1]*sin(angle.theta3)) );
00116         angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00117 
00118         if(!PositionTest6M180(angle,p_m)) {
00119                 angle.theta2 = angle.theta2+M_PI;
00120                 angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00121         }
00122 
00123 }
00124 
00125 bool
00126 KatanaKinematics6M180::PositionTest6M180(const angles_calc &a, const position &p) const {
00127         using namespace KNI_MHF;
00128         double temp, xm2, ym2, zm2;
00129 
00130         temp = _length[0]*sin(a.theta2)+_length[1]*sin(a.theta2+a.theta3)+(_length[2]+_length[3])*sin(a.theta234);
00131         xm2 = cos(a.theta1)*temp;
00132         ym2 = sin(a.theta1)*temp;
00133         zm2 = _length[0]*cos(a.theta2)+_length[1]*cos(a.theta2+a.theta3)+(_length[2]+_length[3])*cos(a.theta234);
00134 
00135         if((pow2(p.x-xm2)+pow2(p.y-ym2)+pow2(p.z-zm2))>=_tolerance)
00136                 return false;
00137 
00138         return true;
00139 }
00140 
00141 bool
00142 KatanaKinematics6M180::angledef(angles_calc &a) const {
00143         using namespace KNI_MHF;
00144 
00145         // constants here. needs refactoring:
00146         a.theta2=anglereduce(a.theta2+M_PI/2.0);
00147         a.theta3=anglereduce(a.theta3+M_PI);
00148         a.theta4=anglereduce(M_PI-a.theta4);
00149         a.theta5=anglereduce(a.theta5);
00150 
00151         if(a.theta1>_parameters[0].angleStop) {
00152                 a.theta1=a.theta1-2.0*M_PI;
00153         }
00154         if(a.theta2>M_PI) {
00155                 a.theta2=a.theta2-2.0*M_PI;
00156         }
00157         if(a.theta5<_parameters[4].angleOffset) {
00158                 a.theta5=a.theta5+2.0*M_PI;
00159         }
00160 
00161         return AnglePositionTest(a);
00162 
00163 }
00164 
00165 bool
00166 KatanaKinematics6M180::AnglePositionTest(const angles_calc &a) const {
00167 
00168         if( (a.theta1+0.0087<_parameters[0].angleOffset)||(a.theta1>_parameters[0].angleStop) )
00169                 return false;
00170 
00171         if( (a.theta2-0.0087>_parameters[1].angleOffset)||(a.theta2<_parameters[1].angleStop) )
00172                 return false;
00173 
00174         if( (a.theta3<_parameters[2].angleOffset)||(a.theta3>_parameters[2].angleStop) )
00175                 return false;
00176 
00177         if( (a.theta4<_parameters[3].angleOffset)||(a.theta4>_parameters[3].angleStop) )
00178                 return false;
00179 
00180         if( (a.theta5<_parameters[4].angleOffset)||(a.theta5>_parameters[4].angleStop) )
00181                 return false;
00182 
00183         return true;
00184 }
00185 
00186 
00187 
00188 
00189 void
00190 KatanaKinematics6M180::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
00191         using namespace KNI_MHF;
00192 
00193         // pose: Winkel Deg->Rad
00194         // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
00195         // 0-3 fr theta1_1
00196         // 4-7 fr theta1_2
00197 
00198         // Declarations
00199         position p_m;
00200         angles_container angle(_nrOfPossibleSolutions);
00201         double coeff1, coeff2, theta234;
00202         double costh5, sinth5, theta5[2];
00203         double R11, R21, R31, R32;
00204         double phi, theta, psi;
00205 
00206         // Initialization
00207         p_m.x = pose[0];
00208         p_m.y = pose[1];
00209         p_m.z = pose[2];
00210 
00211         // calculate theta1_1 and theta1_2
00212         angle[0].theta1 = atan1(pose[0],pose[1]);
00213         if (angle[0].theta1 > M_PI) {
00214                 angle[0].theta1 = angle[0].theta1 - M_PI;
00215                 if (angle[0].theta1 > (179.91/180*M_PI)) {
00216                         angle[0].theta1 = angle[0].theta1 - M_PI;
00217                 }
00218         }
00219         angle[4].theta1 = angle[0].theta1+M_PI;
00220 
00221         theta = pose[4];
00222         psi = pose[5];
00223         phi = atan1(p_m.x,p_m.y)+M_PI/2.0;
00224         theta234 = pose[4];
00225 
00226         R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
00227         R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
00228         R31 = sin(theta)*sin(psi);
00229         R32 = sin(theta)*cos(psi);
00230 
00231         // calculate theta5
00232         if(theta234==0) {
00233                 //std::cout << "Warning: Singularity theta234=0 !" << std::endl;
00234                 for (int i=0; i<2; ++i) {
00235                         coeff1 = -sin(angle[i*4].theta1);
00236                         coeff2 = -cos(angle[i*4].theta1);
00237                         costh5 = coeff1*R11-coeff2*R21;
00238                         sinth5 = coeff1*R21+coeff2*R11;
00239                         theta5[i] = -findFirstEqualAngle(costh5, sinth5, _tolerance);
00240                 }
00241                 for (int i=0; i<_nrOfPossibleSolutions; ++i) {
00242                         if(i<4)
00243                                 angle[i].theta5 = theta5[0];
00244                         else
00245                                 angle[i].theta5 = theta5[1];
00246                 }
00247         } else if(theta234==M_PI) {
00248                 //std::cout << "Warning: Singularity theta234=PI !" << std::endl;
00249                 for (int i=0; i<2; ++i) {
00250                         coeff1 = -sin(angle[i*4].theta1);
00251                         coeff2 = cos(angle[i*4].theta1);
00252                         costh5 = coeff1*R11+coeff2*R21;
00253                         sinth5 = -coeff1*R21+coeff2*R11;
00254                         theta5[i] = -findFirstEqualAngle(costh5, sinth5, _tolerance);
00255                 }
00256                 for (int i=0; i<_nrOfPossibleSolutions; ++i) {
00257                         if(i<4)
00258                                 angle[i].theta5 = theta5[0];
00259                         else
00260                                 angle[i].theta5 = theta5[1];
00261                 }
00262         } else {
00263                 theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
00264                 theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
00265                 for (int i=0; i<_nrOfPossibleSolutions; ++i) {
00266                         if(i%4==0 || i%4==1)
00267                                 angle[i].theta5 = theta5[0];
00268                         else
00269                                 angle[i].theta5 = theta5[1];
00270                 }
00271         }
00272 
00273         //====THETA1_1==================
00274         //-------THETA234_1-------------
00275         angle[0].theta234 = pose[4];
00276         //angle[0].theta5 = pose[5];
00277         IK_b1b2costh3_6M180(angle[0], p_m);
00278 
00279         angle[1] =angle[0];
00280         angle[0].theta3 =  acos(angle[0].costh3)-M_PI;
00281         thetacomp(angle[0], p_m);
00282         angle[1].theta3 =  -acos(angle[1].costh3)+M_PI;
00283         thetacomp(angle[1], p_m);
00284 
00285         //-------THETA234_2-------------
00286         angle[2].theta1 = angle[0].theta1;
00287         angle[2].theta234 = -angle[0].theta234;
00288         //angle[2].theta5 = angle[0].theta5;
00289         IK_b1b2costh3_6M180(angle[2], p_m);
00290 
00291         angle[3] = angle[2];
00292         angle[2].theta3 =  acos(angle[2].costh3)-M_PI;
00293         thetacomp(angle[2], p_m);
00294         angle[3].theta3 =  -acos(angle[3].costh3)+M_PI;
00295         thetacomp(angle[3], p_m);
00296 
00297 
00298         //====THETA1_2==================
00299         //-------THETA234_1-------------
00300         angle[4].theta234 = pose[4];
00301         //angle[4].theta5 = pose[5];
00302         IK_b1b2costh3_6M180(angle[4], p_m);
00303 
00304         angle[5] = angle[4];
00305         angle[4].theta3 = acos(angle[4].costh3)-M_PI;
00306         thetacomp(angle[4], p_m);
00307         angle[5].theta3 = -acos(angle[5].costh3)+M_PI;
00308         thetacomp(angle[5], p_m);
00309 
00310         //-------THETA234_2-------------
00311         angle[6].theta1 = angle[4].theta1;
00312         angle[6].theta234 = -angle[4].theta234;
00313         //angle[6].theta5 = angle[4].theta5;
00314         IK_b1b2costh3_6M180(angle[6], p_m);
00315 
00316         angle[7] = angle[6];
00317         angle[6].theta3 =  acos(angle[6].costh3)-M_PI;
00318         thetacomp(angle[6], p_m);
00319         angle[7].theta3 =  -acos(angle[7].costh3)+M_PI;
00320         thetacomp(angle[7], p_m);
00321 
00322         for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
00323                 if( pow2(iter->costh3) <= 1.0) {
00324                         if(!angledef(*iter))
00325                                 iter = angle.erase(iter);
00326                         else
00327                                 ++iter;
00328                         continue;
00329                 }
00330                 iter = angle.erase(iter);
00331         }
00332 
00333 
00334         if(angle.size() == 0) {
00335                 throw NoSolutionException();
00336         }
00337 
00338         std::vector< std::vector<int> > PossibleTargetsInEncoders;
00339         for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00340                 std::vector<int> solution(5);
00341 
00342                 solution[0] = rad2enc(i->theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
00343                 solution[1] = rad2enc(i->theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
00344                 solution[2] = rad2enc(i->theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
00345                 solution[3] = rad2enc(i->theta4, _parameters[3].angleOffset, _parameters[3].epc, _parameters[3].encOffset, _parameters[3].rotDir);
00346                 solution[4] = rad2enc(i->theta5, _parameters[4].angleOffset, _parameters[4].epc, _parameters[4].encOffset, _parameters[4].rotDir);
00347 
00348                 PossibleTargetsInEncoders.push_back(solution);
00349         }
00350 
00351 
00352         std::vector< std::vector<int> >::const_iterator sol = KinematicsDefaultEncMinAlgorithm()(PossibleTargetsInEncoders.begin(), PossibleTargetsInEncoders.end(), current_encoders.begin(), current_encoders.end());
00353 
00354         assert( sol != PossibleTargetsInEncoders.end() && "All solutions are out of range");
00355 
00356 
00357         encoders::iterator gripper_encoder_iter = std::copy( (*sol).begin(), (*sol).end(), solution );
00358         *gripper_encoder_iter = current_encoders[5]; // copy gripper-encoders from current
00359 
00360 }
00361 
00362 
00363 
00364 } // NAMESPACE: KNI
00365