kinematics6M90T.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2008 by Neuronics AG                                    *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00006 #include <kinematics6M90T.h>
00007 
00008 namespace AnaGuess {
00009 
00011 Kinematics6M90T::Kinematics6M90T() {
00012         initialize();
00013 }
00015 Kinematics6M90T::~Kinematics6M90T() {
00016 }
00017 
00019 std::vector<double> Kinematics6M90T::getLinkLength() {
00020         std::vector<double> result(mSegmentLength);
00021         return result;
00022 }
00024 std::vector<int> Kinematics6M90T::getEpc() {
00025         std::vector<int> result(mEncodersPerCycle);
00026         return result;
00027 }
00029 std::vector<int> Kinematics6M90T::getEncOff() {
00030         std::vector<int> result(mEncoderOffset);
00031         return result;
00032 }
00034 std::vector<int> Kinematics6M90T::getDir() {
00035         std::vector<int> result(mRotationDirection);
00036         return result;
00037 }
00039 std::vector<double> Kinematics6M90T::getAngOff() {
00040         std::vector<double> result(mAngleOffset);
00041         return result;
00042 }
00044 std::vector<double> Kinematics6M90T::getAngStop() {
00045         std::vector<double> result(mAngleStop);
00046         return result;
00047 }
00049 std::vector<double> Kinematics6M90T::getAngRange() {
00050         std::vector<double> result;
00051         double diff;
00052         for (int i = 0; i < 6; i++) {
00053                 diff = mAngleStop[i] - mAngleOffset[i];
00054                 if (diff < 0) {
00055                         result.push_back(-diff);
00056                 } else {
00057                         result.push_back(diff);
00058                 }
00059         }
00060         return result;
00061 }
00063 std::vector<double> Kinematics6M90T::getAngMin() {
00064         std::vector<double> result;
00065         for (int i = 0; i < 6; i++) {
00066                 if (mAngleStop[i] < mAngleOffset[i]) {
00067                         result.push_back(mAngleStop[i]);
00068                 } else {
00069                         result.push_back(mAngleOffset[i]);
00070                 }
00071         }
00072         return result;
00073 }
00075 std::vector<double> Kinematics6M90T::getAngMax() {
00076         std::vector<double> result;
00077         for (int i = 0; i < 6; i++) {
00078                 if (mAngleStop[i] < mAngleOffset[i]) {
00079                         result.push_back(mAngleOffset[i]);
00080                 } else {
00081                         result.push_back(mAngleStop[i]);
00082                 }
00083         }
00084         return result;
00085 }
00086 
00088 bool Kinematics6M90T::setLinkLength(const std::vector<double> aLengths) {
00089         if ((int) aLengths.size() != mNumberOfSegments) {
00090                 return false;
00091         }
00092 
00093         for (int i = 0; i < mNumberOfSegments; ++i) {
00094                 mSegmentLength[i] = aLengths.at(i);
00095         }
00096 
00097         return true;
00098 }
00100 bool Kinematics6M90T::setAngOff(const std::vector<double> aAngOff) {
00101         if ((int) aAngOff.size() != mNumberOfMotors) {
00102                 return false;
00103         }
00104 
00105         for (int i = 0; i < mNumberOfMotors; ++i) {
00106                 mAngleOffset[i] = aAngOff.at(i);
00107         }
00108 
00109         return true;
00110 }
00112 bool Kinematics6M90T::setAngStop(const std::vector<double> aAngStop) {
00113         if ((int) aAngStop.size() != mNumberOfMotors) {
00114                 return false;
00115         }
00116 
00117         for (int i = 0; i < mNumberOfMotors; ++i) {
00118                 mAngleStop[i] = aAngStop.at(i);
00119         }
00120 
00121         return true;
00122 }
00123 
00125 bool Kinematics6M90T::enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders) {
00126         for(int i = 0; i < 6; ++i) {
00127                 aAngles[i] = MHF::enc2rad(aEncoders[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
00128         }
00129         return true;
00130 }
00132 bool Kinematics6M90T::rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles) {
00133         for(int i = 0; i < 6; ++i ) {
00134                 aEncoders[i] = MHF::rad2enc(aAngles[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
00135         }
00136         return true;
00137 }
00138 
00140 bool Kinematics6M90T::directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles) {
00141         if(!mIsInitialized) {
00142                 initialize();
00143         }
00144 
00145         // numering the angles starting by 0-5
00146 
00147         double x0, x1, x2, x3;
00148         double y0, y1, y2, y3;
00149         double z0, z1, z2, z3;
00150 
00151         std::vector<double> current_angles(6);
00152         for(int i = 0; i < 6; ++i) {
00153                 current_angles[i] = aAngles[i];
00154         }
00155 
00156         // needs refactoring:
00157         current_angles[1] = current_angles[1] - MHF_PI/2.0;
00158         current_angles[2] = current_angles[2] - MHF_PI;
00159         current_angles[3] = MHF_PI - current_angles[3];
00160         current_angles[5] = -current_angles[5];
00161 
00162         std::vector<double> pose(6);
00163 
00164         std::vector<double> cx(current_angles.size()), sx(current_angles.size());
00165         std::vector<double>::iterator cx_iter, sx_iter;
00166 
00167         std::vector<double> angle = current_angles;
00168 
00169         angle[2] = angle[1]+angle[2];
00170         angle[3] = angle[2]+angle[3];
00171 
00172         cx_iter = cx.begin();
00173         sx_iter = sx.begin();
00174         std::transform(angle.begin(), angle.end(), sx_iter, MHF::unary_precalc_sin<double>() );
00175         std::transform(angle.begin(), angle.end(), cx_iter, MHF::unary_precalc_cos<double>() );
00176 
00177 
00178         //x
00179         x0 =  cx[0]*sx[1];
00180         x1 =  cx[0]*sx[2];
00181         x2 =  cx[0]*sx[3];
00182         x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
00183         pose[0] = x0*mSegmentLength[0]+x1*mSegmentLength[1]+x2*mSegmentLength[2]+x3*mSegmentLength[3];
00184 
00185         //y
00186         y0 = sx[0]*sx[1];
00187         y1 = sx[0]*sx[2];
00188         y2 = sx[0]*sx[3];
00189         y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
00190         pose[1] = y0*mSegmentLength[0]+y1*mSegmentLength[1]+y2*mSegmentLength[2]+y3*mSegmentLength[3];
00191 
00192         //z
00193         z0 = cx[1];
00194         z1 = cx[2];
00195         z2 = cx[3];
00196         z3 = cx[4]*sx[3];
00197         pose[2] = z0*mSegmentLength[0]+z1*mSegmentLength[1]+z2*mSegmentLength[2]+z3*mSegmentLength[3];
00198 
00199 
00200         //theta
00201         pose[4] = acos(cx[4]*sx[3]);
00202 
00203         // phi & psi
00204 
00205 
00206         const double theta1 = angle[0];
00207         const double theta5 = angle[4];
00208         const double theta6 = angle[5];
00209         const double theta234 = angle[3];
00210 
00211         if( std::abs(pose[4])<cTolerance || std::abs(pose[4]-MHF_PI)<cTolerance ) { // catch the case where theta=0, resp. theta=180
00212                 //phi
00213                 std::vector<double> v1(2), v2(2);
00214 
00215                 double R11 = -sin(theta1)*cos(theta5)  *sin(theta6) + cos(theta1)*(sin(theta234)*cos(theta6)+cos(theta234)*sin(theta5)*sin(theta6));
00216                 double R21 =  sin(theta1)*sin(theta234)*cos(theta6) + sin(theta6)*(cos(theta1)  *cos(theta5)+cos(theta234)*sin(theta1)*sin(theta5));
00217 
00218                 v1[0] = acos( R11 );
00219                 v1[1] = -v1[0];
00220                 v2[0] = asin( R21 );
00221                 v2[1] = MHF_PI - v2[0];
00222 
00223                 pose[3] = MHF::anglereduce(findFirstEqualAngle(v1, v2));
00224 
00225                 //psi
00226                 pose[5] = 0;
00227         } else {
00228                 //phi
00229                 const double R13 = -cos(theta1)*cos(theta234)*cos(theta5) - sin(theta1)*sin(theta5);
00230                 const double R23 = -sin(theta1)*cos(theta234)*cos(theta5) + cos(theta1)*sin(theta5);
00231                 pose[3] = atan2(R13, -R23);
00232 
00233                 //psi
00234                 const double R31 =  cos(theta234)*cos(theta6) - sin(theta234)*sin(theta5)*sin(theta6);
00235                 const double R32 = -cos(theta234)*sin(theta6) - sin(theta234)*sin(theta5)*cos(theta6);
00236                 pose[5] = atan2(R31, R32);
00237         }
00238 
00239         std::swap(aPosition, pose);
00240         return true;
00241 }
00243 bool Kinematics6M90T::inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition,
00244                 const std::vector<double> aStartingAngles) {
00245         if(!mIsInitialized) {
00246                 initialize();
00247         }
00248         // pose: Winkel Deg->Rad
00249         // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
00250         // 0-3 fr theta1_1
00251         // 4-7 fr theta1_2
00252 
00253         // Declaration
00254         position p_gr;
00255         position p_m;
00256         angles_container angle(cNrOfPossibleSolutions);
00257 
00258         // calculation of the gripper vector
00259         p_gr.x = mSegmentLength[3]*sin(aPosition[4])*sin(aPosition[3]);
00260         p_gr.y = -mSegmentLength[3]*sin(aPosition[4])*cos(aPosition[3]);
00261         p_gr.z = mSegmentLength[3]*cos(aPosition[4]);
00262 
00263         p_m.x = aPosition[0]-p_gr.x;
00264         p_m.y = aPosition[1]-p_gr.y;
00265         p_m.z = aPosition[2]-p_gr.z;
00266 
00267         // calculate theta1_1 and theta1_2
00268         angle[0].theta1 = MHF::atan1(p_m.x,p_m.y);
00269         angle[4].theta1 = angle[0].theta1+MHF_PI;
00270 
00271 
00272         // check the borders according to the settings
00273         if(angle[0].theta1>mAngleStop[0])
00274                 angle[0].theta1=angle[0].theta1-2.0*MHF_PI;
00275 
00276         if(angle[0].theta1<mAngleOffset[0])
00277                 angle[0].theta1=angle[0].theta1+2.0*MHF_PI;
00278 
00279         if(angle[4].theta1>mAngleStop[0])
00280                 angle[4].theta1=angle[4].theta1-2.0*MHF_PI;
00281 
00282         if(angle[4].theta1<mAngleOffset[0])
00283                 angle[4].theta1=angle[4].theta1+2.0*MHF_PI;
00284 
00285 
00286         //====THETA1_1==================
00287         //-------THETA234_1-------------
00288         IK_theta234theta5(angle[0], p_gr);
00289         IK_b1b2costh3_6MS(angle[0], p_m);
00290 
00291         angle[1]=angle[0];
00292         angle[0].theta3 =  acos(angle[0].costh3)-MHF_PI;
00293         thetacomp(angle[0], p_m, aPosition);
00294         angle[1].theta3 =  -acos(angle[1].costh3)+MHF_PI;
00295         thetacomp(angle[1], p_m, aPosition);
00296 
00297         //-------THETA234_2-------------
00298         angle[2].theta1=angle[0].theta1;
00299         angle[2].theta234=angle[0].theta234-MHF_PI;
00300         angle[2].theta5=MHF_PI-angle[0].theta5;
00301 
00302         IK_b1b2costh3_6MS(angle[2], p_m);
00303         angle[3]=angle[2];
00304         angle[2].theta3 =  acos(angle[2].costh3)-MHF_PI;
00305         thetacomp(angle[2], p_m, aPosition);
00306         angle[3].theta3 =  -acos(angle[3].costh3)+MHF_PI;
00307         thetacomp(angle[3], p_m, aPosition);
00308 
00309 
00310         //====THETA1_2==================
00311         //-------THETA234_1-------------
00312         IK_theta234theta5(angle[4], p_gr);
00313         IK_b1b2costh3_6MS(angle[4], p_m);
00314 
00315         angle[5]=angle[4];
00316         angle[4].theta3 =  acos(angle[4].costh3)-MHF_PI;
00317         thetacomp(angle[4], p_m, aPosition);
00318         angle[5].theta3 =  -acos(angle[5].costh3)+MHF_PI;
00319         thetacomp(angle[5], p_m, aPosition);
00320 
00321         //-------THETA234_2-------------
00322         angle[6].theta1=angle[4].theta1;
00323         angle[6].theta234=angle[4].theta234-MHF_PI;
00324         angle[6].theta5=MHF_PI-angle[4].theta5;
00325         IK_b1b2costh3_6MS(angle[6], p_m);
00326         angle[7]=angle[6];
00327         angle[6].theta3 =  acos(angle[6].costh3)-MHF_PI;
00328         thetacomp(angle[6], p_m, aPosition);
00329         angle[7].theta3 =  -acos(angle[7].costh3)+MHF_PI;
00330         thetacomp(angle[7], p_m, aPosition);
00331 
00332         // delete solutions out of range (in joint space)
00333         for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
00334                 if( MHF::pow2(iter->costh3) <= 1.0) {
00335                         if(!angledef(*iter))
00336                                 iter = angle.erase(iter);
00337                         else
00338                                 ++iter;
00339                         continue;
00340                 }
00341                 iter = angle.erase(iter);
00342         }
00343 
00344 
00345         // check if solutions in range left
00346         if(angle.size() == 0) {
00347                 throw NoSolutionException();
00348         }
00349 
00350         // store possible solution angles to std::vector<std::vector<double>>
00351         std::vector< std::vector<double> > PossibleTargets;
00352         for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00353                 std::vector<double> possangles(6);
00354 
00355                 possangles[0] = i->theta1;
00356                 possangles[1] = i->theta2;
00357                 possangles[2] = i->theta3;
00358                 possangles[3] = i->theta4;
00359                 possangles[4] = i->theta5;
00360                 possangles[5] = i->theta6;
00361 
00362                 PossibleTargets.push_back(possangles);
00363         }
00364 
00365         // choose best solution
00366         std::vector< std::vector<double> >::const_iterator sol = KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
00367 
00368         if(sol == PossibleTargets.end()) {
00369                 throw NoSolutionException();
00370         }
00371 
00372         // copy solution to aAngles vector
00373         for (int i = aAngles.size(); i < 6; ++i)
00374                 aAngles.push_back(0.0);
00375         std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
00376 
00377         return true;
00378 }
00380 bool Kinematics6M90T::initialize() {
00381         // NOTE: data for Katana6M90A with flange
00382         
00383         mIsInitialized = false;
00384 
00385         //fill in segment data
00386         mNumberOfSegments = 4;
00387 
00388         mSegmentLength.push_back(190.0);
00389         mSegmentLength.push_back(139.0);
00390         mSegmentLength.push_back(147.3);
00391         mSegmentLength.push_back(36.0);
00392 
00393         //fill in joint data
00394         mNumberOfMotors = 6;
00395 
00396         mAngleOffset.push_back(0.116064);
00397         mAngleStop.push_back(6.154904);
00398         mEncodersPerCycle.push_back(51200);
00399         mEncoderOffset.push_back(31000);
00400         mRotationDirection.push_back(1);
00401 
00402         mAngleOffset.push_back(2.168572);
00403         mAngleStop.push_back(-0.274889);
00404         mEncodersPerCycle.push_back(94976);
00405         mEncoderOffset.push_back(-31000);
00406         mRotationDirection.push_back(1);
00407 
00408         mAngleOffset.push_back(0.919789);
00409         mAngleStop.push_back(5.283112);
00410         mEncodersPerCycle.push_back(47488);
00411         mEncoderOffset.push_back(-31000);
00412         mRotationDirection.push_back(-1);
00413 
00414         mAngleOffset.push_back(1.108284);
00415         mAngleStop.push_back(5.122541);
00416         mEncodersPerCycle.push_back(51200);
00417         mEncoderOffset.push_back(31000);
00418         mRotationDirection.push_back(1);
00419 
00420         mAngleOffset.push_back(0.148353); // for 6M90B: -2.99324
00421         mAngleStop.push_back(6.117379); // for 6M90B: 2.975787
00422         mEncodersPerCycle.push_back(51200);
00423         mEncoderOffset.push_back(31000);
00424         mRotationDirection.push_back(1);
00425 
00426         mAngleOffset.push_back(-2.085668);
00427         mAngleStop.push_back(3.656465);
00428         mEncodersPerCycle.push_back(51200);
00429         mEncoderOffset.push_back(31000);
00430         mRotationDirection.push_back(1);
00431 
00432         mIsInitialized = true;
00433 
00434         return mIsInitialized;
00435 }
00437 void Kinematics6M90T::IK_theta234theta5(angles_calc& angle, const position &p_gr) const {
00438         if(p_gr.z==0) {
00439                 angle.theta234=0;
00440                 angle.theta5=angle.theta1-MHF::atan1(-p_gr.x,-p_gr.y);
00441         } else {
00442                 angle.theta234 = -MHF::acotan( (  (p_gr.x * p_gr.z * cos(angle.theta1) ) -
00443                                      sqrt( ( -MHF::pow2(p_gr.z) ) *
00444                                            ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
00445                                          )
00446                                   ) / MHF::pow2(p_gr.z)
00447                                 );
00448                 angle.theta5   = acos( p_gr.z/(mSegmentLength[3]*sin(angle.theta234)) );
00449         }
00450 
00451         bool griptest;
00452         griptest = GripperTest(p_gr, angle);
00453         if(!griptest) {
00454                 angle.theta5=-angle.theta5;
00455                 griptest=GripperTest(p_gr, angle);
00456                 if(!griptest) {
00457                         angle.theta234 = -MHF::acotan( (   ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
00458                                                       sqrt( ( -MHF::pow2(p_gr.z) ) *
00459                                                             ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
00460                                                           )
00461                                                   ) / MHF::pow2(p_gr.z)
00462                                                 );
00463                         angle.theta5 =  acos( p_gr.z / (mSegmentLength[3]*sin(angle.theta234)) );
00464                         if(p_gr.z==0) {
00465                                 angle.theta234=-MHF_PI;
00466                                 angle.theta5=MHF::atan1(p_gr.x,p_gr.y) - angle.theta1;
00467                         }
00468 
00469                         griptest=GripperTest(p_gr, angle);
00470                         if(!griptest) {
00471                                 angle.theta5=-angle.theta5;
00472                         }
00473                 }
00474         }
00475 
00476 }
00478 bool Kinematics6M90T::GripperTest(const position &p_gr, const angles_calc &angle) const {
00479         double xgr2, ygr2, zgr2;
00480 
00481         xgr2 = -mSegmentLength[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
00482         ygr2 = -mSegmentLength[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
00483         zgr2 =  mSegmentLength[3]*sin(angle.theta234)*cos(angle.theta5);
00484 
00485         if((MHF::pow2(p_gr.x-xgr2)+MHF::pow2(p_gr.y-ygr2)+MHF::pow2(p_gr.z-zgr2))>=cTolerance)
00486                 return false;
00487 
00488         return true;
00489 }
00491 void Kinematics6M90T::IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const {
00492         double xg, yg, zg;
00493         double d5 = mSegmentLength[2] + mSegmentLength[3];
00494         xg = p.x + ( mSegmentLength[3] * cos(angle.theta1) * sin(angle.theta234) );
00495         yg = p.y + ( mSegmentLength[3] * sin(angle.theta1) * sin(angle.theta234) );
00496         zg = p.z + ( mSegmentLength[3]                     * cos(angle.theta234) );
00497 
00498 
00499         angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
00500         angle.b2 = zg - d5*cos(angle.theta234);
00501         angle.costh3 = -( MHF::pow2(angle.b1) + MHF::pow2(angle.b2) - MHF::pow2(mSegmentLength[0]) - MHF::pow2(mSegmentLength[1]) ) / ( 2.0*mSegmentLength[0]*mSegmentLength[1] );
00502 
00503 }
00505 double Kinematics6M90T::findFirstEqualAngle(const std::vector<double>& v1, const std::vector<double>& v2) const {
00506         for(std::vector<double>::const_iterator i = v1.begin(); i != v1.end(); ++i) {
00507                 for(std::vector<double>::const_iterator j = v2.begin(); j != v2.end(); ++j) {
00508                         if(std::abs(MHF::anglereduce(*j) - MHF::anglereduce(*i)) < cTolerance)
00509                                 return *i;
00510                 }
00511         }
00512         throw Exception("precondition for findFirstEqualAngle failed -> no equal angles found", -2);
00513         return 0;
00514 }
00515 
00516 
00517 void Kinematics6M90T::thetacomp(angles_calc &angle, const position &p_m, const std::vector<double>& pose) const {
00518         const double theta1   = angle.theta1;
00519         double theta2   = 0;
00520         const double theta3   = angle.theta3;
00521         double theta4   = 0;
00522         const double theta5   = angle.theta5;
00523         double theta6   = 0;
00524         const double theta234 = angle.theta234;
00525         const double b1       = angle.b1;
00526         const double b2       = angle.b2;
00527 
00528         const double phi   = pose[3];
00529         const double theta = pose[4];
00530         const double psi   = pose[5];
00531 
00532 
00533         theta2 = -MHF_PI/2.0 - ( MHF::atan0(b1, b2)+MHF::atan0(mSegmentLength[0]+mSegmentLength[1]*cos(theta3),mSegmentLength[1]*sin(theta3)) );
00534         theta4 = theta234 - theta2 - theta3;
00535 
00536         if(!PositionTest6MS(theta1, theta2, theta3, theta234 ,p_m)) {
00537                 theta2 = theta2+MHF_PI;
00538                 theta4 = theta234 - theta2 - theta3;
00539         }
00540 
00541         const double R11 = cos(phi)*cos(psi) - sin(phi)*cos(theta)*sin(psi);
00542         const double R21 = sin(phi)*cos(psi) + cos(phi)*cos(theta)*sin(psi);
00543 
00544         std::vector<double> theta16c(2), theta16s(2);
00545 
00546         if(std::abs(theta234 + MHF_PI/2) < cTolerance) {
00547                 if(std::abs(theta5) < cTolerance) {
00548                         theta16c[0] = acos(-R11);
00549                         theta16c[1] = -theta16c[0];
00550                         theta16s[0] = asin(-R21);
00551                         theta16s[1] = MHF_PI - theta16s[0];
00552 
00553                         theta6 = theta1 - findFirstEqualAngle(theta16c, theta16s);
00554 
00555                 } else if(std::abs(theta5-MHF_PI) < cTolerance) {
00556                         theta16c[0] = acos(-R11);
00557                         theta16c[1] = -theta16c[0];
00558                         theta16s[0] = asin(-R21);
00559                         theta16s[1] = MHF_PI - theta16s[0];
00560 
00561                         theta6 = findFirstEqualAngle(theta16c, theta16s) - theta1;
00562 
00563                 } else {
00564                         throw Exception("Special case \"|theta234+(1/2)*pi| = 0\" detected, but no solution found", -1);
00565                 }
00566 
00567         } else if(std::abs(theta234 + 3*MHF_PI/2) < cTolerance) {
00568                 if(std::abs(theta5) < cTolerance) {
00569                         theta16c[0] = acos(R11);
00570                         theta16c[1] = -theta16c[0];
00571                         theta16s[0] = asin(R21);
00572                         theta16s[1] = MHF_PI - theta16s[0];
00573 
00574                         theta6  = findFirstEqualAngle(theta16c, theta16s) - theta1;
00575 
00576                 } else if(std::abs(theta5-MHF_PI ) < cTolerance) {
00577                         theta16c[0] = acos(R11);
00578                         theta16c[1] = -theta16c[0];
00579                         theta16s[0] = asin(R21);
00580                         theta16s[1] = MHF_PI -theta16s[0];
00581 
00582                         theta6  = - theta1 - findFirstEqualAngle(theta16c, theta16s);
00583                 } else {
00584                         throw Exception("Special case \"|theta234+(3/2)*pi| = 0\" detected, but no solution found", -1);
00585                 }
00586 
00587         } else {
00588 
00589                 const double R31 = sin(theta)*sin(psi);
00590                 const double R32 = sin(theta)*cos(psi);
00591 
00592                 const double temp1 =  cos(theta234);
00593                 const double temp2 = -sin(theta234)*sin(theta5);
00594 
00595                 const double c = ( R31*temp1 + R32*temp2 ) / ( MHF::pow2(temp1) + MHF::pow2(temp2) );
00596                 const double s = ( R31*temp2 - R32*temp1 ) / ( MHF::pow2(temp1) + MHF::pow2(temp2) );
00597 
00598                 theta16c[0] = acos(c);
00599                 theta16c[1] = -theta16c[0];
00600                 theta16s[0] = asin(s);
00601                 theta16s[1] = MHF_PI - theta16s[0];
00602 
00603                 theta6 = findFirstEqualAngle(theta16c, theta16s);
00604         }
00605 
00606 
00607         angle.theta2 = theta2;
00608         angle.theta4 = theta4;
00609         angle.theta6 = theta6;
00610 }
00612 bool Kinematics6M90T::PositionTest6MS(const double& theta1, const double& theta2, const double& theta3, const double& theta234, const position &p) const {
00613         double temp, xm2, ym2, zm2;
00614 
00615         temp = mSegmentLength[0]*sin(theta2) + mSegmentLength[1]*sin(theta2+theta3) + mSegmentLength[2]*sin(theta234);
00616         xm2  = cos(theta1)*temp;
00617         ym2  = sin(theta1)*temp;
00618         zm2  = mSegmentLength[0]*cos(theta2) + mSegmentLength[1]*cos(theta2+theta3) + mSegmentLength[2]*cos(theta234);
00619 
00620         if((MHF::pow2(p.x-xm2)+MHF::pow2(p.y-ym2)+MHF::pow2(p.z-zm2))>=cTolerance)
00621                 return false;
00622 
00623         return true;
00624 }
00626 bool Kinematics6M90T::angledef(angles_calc &a) const {
00627         // constants here. needs refactoring:
00628         a.theta2=MHF::anglereduce(a.theta2+MHF_PI/2.0);
00629         a.theta3=MHF::anglereduce(a.theta3+MHF_PI);
00630         a.theta4=MHF::anglereduce(MHF_PI-a.theta4);
00631         a.theta5=MHF::anglereduce(a.theta5);
00632         a.theta6=-a.theta6;
00633 
00634         if(a.theta1>mAngleStop[0]) {
00635                 a.theta1=a.theta1-2.0*MHF_PI;
00636         }
00637         if(a.theta2>MHF_PI) {
00638                 a.theta2=a.theta2-2.0*MHF_PI;
00639         }
00640         if(a.theta6<mAngleOffset[5]) {
00641                 a.theta6=a.theta6+2.0*MHF_PI;
00642         } else if(a.theta6>mAngleStop[5]) {
00643                 a.theta6=a.theta6-2.0*MHF_PI;
00644         }
00645         if(a.theta5<mAngleOffset[4]) {
00646                 a.theta5 += 2.0*MHF_PI;
00647         }
00648 
00649         return AnglePositionTest(a);
00650 
00651 }
00653 bool Kinematics6M90T::AnglePositionTest(const angles_calc &a) const {
00654 
00655         if( (a.theta1+0.0087<mAngleOffset[0])||(a.theta1>mAngleStop[0]) ) {
00656                 return false;
00657         }
00658         if( (a.theta2-0.0087>mAngleOffset[1])||(a.theta2<mAngleStop[1]) ) {
00659                 return false;
00660         }
00661         if( (a.theta3<mAngleOffset[2])||(a.theta3>mAngleStop[2]) ) {
00662                 return false;
00663         }
00664 
00665         if( (a.theta4<mAngleOffset[3])||(a.theta4>mAngleStop[3]) ) {
00666                 return false;
00667         }
00668 
00669         if( (a.theta5<mAngleOffset[4])||(a.theta5>mAngleStop[4]) ) {
00670                 return false;
00671         }
00672         if( (a.theta6<mAngleOffset[5])||(a.theta6>mAngleStop[5]) ) {
00673                 return false;
00674         }
00675 
00676         return true;
00677 }
00679 } // namespace


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