kinematics6M90G.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2008 by Neuronics AG                                    *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00006 #include <kinematics6M90G.h>
00007 
00008 namespace AnaGuess {
00009 
00011 Kinematics6M90G::Kinematics6M90G() {
00012         initialize();
00013 }
00015 Kinematics6M90G::~Kinematics6M90G() {
00016 
00017 }
00018 
00020 std::vector<double> Kinematics6M90G::getLinkLength() {
00021         std::vector<double> result(mSegmentLength);
00022         return result;
00023 }
00025 std::vector<int> Kinematics6M90G::getEpc() {
00026         std::vector<int> result(mEncodersPerCycle);
00027         return result;
00028 }
00030 std::vector<int> Kinematics6M90G::getEncOff() {
00031         std::vector<int> result(mEncoderOffset);
00032         return result;
00033 }
00035 std::vector<int> Kinematics6M90G::getDir() {
00036         std::vector<int> result(mRotationDirection);
00037         return result;
00038 }
00040 std::vector<double> Kinematics6M90G::getAngOff() {
00041         std::vector<double> result(mAngleOffset);
00042         return result;
00043 }
00045 std::vector<double> Kinematics6M90G::getAngStop() {
00046         std::vector<double> result(mAngleStop);
00047         return result;
00048 }
00050 std::vector<double> Kinematics6M90G::getAngRange() {
00051         std::vector<double> result;
00052         double diff;
00053         for (int i = 0; i < 6; i++) {
00054                 diff = mAngleStop[i] - mAngleOffset[i];
00055                 if (diff < 0) {
00056                         result.push_back(-diff);
00057                 } else {
00058                         result.push_back(diff);
00059                 }
00060         }
00061         return result;
00062 }
00064 std::vector<double> Kinematics6M90G::getAngMin() {
00065         std::vector<double> result;
00066         for (int i = 0; i < 6; i++) {
00067                 if (mAngleStop[i] < mAngleOffset[i]) {
00068                         result.push_back(mAngleStop[i]);
00069                 } else {
00070                         result.push_back(mAngleOffset[i]);
00071                 }
00072         }
00073         return result;
00074 }
00076 std::vector<double> Kinematics6M90G::getAngMax() {
00077         std::vector<double> result;
00078         for (int i = 0; i < 6; i++) {
00079                 if (mAngleStop[i] < mAngleOffset[i]) {
00080                         result.push_back(mAngleOffset[i]);
00081                 } else {
00082                         result.push_back(mAngleStop[i]);
00083                 }
00084         }
00085         return result;
00086 }
00087 
00089 bool Kinematics6M90G::setLinkLength(const std::vector<double> aLengths) {
00090         if ((int) aLengths.size() != mNumberOfSegments) {
00091                 return false;
00092         }
00093 
00094         for (int i = 0; i < mNumberOfSegments; ++i) {
00095                 mSegmentLength[i] = aLengths.at(i);
00096         }
00097 
00098         return true;
00099 }
00101 bool Kinematics6M90G::setAngOff(const std::vector<double> aAngOff) {
00102         if ((int) aAngOff.size() != mNumberOfMotors) {
00103                 return false;
00104         }
00105 
00106         for (int i = 0; i < mNumberOfMotors; ++i) {
00107                 mAngleOffset[i] = aAngOff.at(i);
00108         }
00109 
00110         return true;
00111 }
00113 bool Kinematics6M90G::setAngStop(const std::vector<double> aAngStop) {
00114         if ((int) aAngStop.size() != mNumberOfMotors) {
00115                 return false;
00116         }
00117 
00118         for (int i = 0; i < mNumberOfMotors; ++i) {
00119                 mAngleStop[i] = aAngStop.at(i);
00120         }
00121 
00122         return true;
00123 }
00124 
00126 bool Kinematics6M90G::enc2rad(std::vector<double>& aAngles, const std::vector<int> aEncoders) {
00127         for(int i = 0; i < 6; ++i) {
00128                 aAngles[i] = MHF::enc2rad(aEncoders[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
00129         }
00130         return true;
00131 }
00133 bool Kinematics6M90G::rad2enc(std::vector<int>& aEncoders, const std::vector<double> aAngles) {
00134         for(int i = 0; i < 6; ++i ) {
00135                 aEncoders[i] = MHF::rad2enc(aAngles[i], mAngleOffset[i], mEncodersPerCycle[i], mEncoderOffset[i], mRotationDirection[i]);
00136         }
00137         return true;
00138 }
00139 
00141 bool Kinematics6M90G::directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles) {
00142         if(!mIsInitialized) {
00143                 initialize();
00144         }
00145 
00146         // numering the angles starting by 0-5
00147 
00148         double x0, x1, x2, x3;
00149         double y0, y1, y2, y3;
00150         double z0, z1, z2, z3;
00151         double R13, R23, R33, R31, R32;
00152 
00153         std::vector<double> current_angles(6);
00154         for(int i = 0; i < 6; ++i) {
00155                 current_angles[i] = aAngles[i];
00156         }
00157 
00158         // needs refactoring:
00159         current_angles[1] = current_angles[1] - MHF_PI/2.0;
00160         current_angles[2] = current_angles[2] - MHF_PI;
00161         current_angles[3] = MHF_PI - current_angles[3];
00162 
00163         std::vector<double> pose(6);
00164 
00165         std::vector<double> cx(current_angles.size()), sx(current_angles.size());
00166         std::vector<double>::iterator cx_iter, sx_iter;
00167 
00168         std::vector<double> angle = current_angles;
00169 
00170         angle[2] = angle[1]+angle[2];
00171         angle[3] = angle[2]+angle[3];
00172 
00173         cx_iter = cx.begin();
00174         sx_iter = sx.begin();
00175         std::transform(angle.begin(), angle.end(), sx_iter, MHF::unary_precalc_sin<double>() );
00176         std::transform(angle.begin(), angle.end(), cx_iter, MHF::unary_precalc_cos<double>() );
00177 
00178         R13 = (-1*cx[0]*cx[3]*cx[4])-(sx[0]*sx[4]);
00179         R23 = (-1*sx[0]*cx[3]*cx[4])+(cx[0]*sx[4]);
00180         R33 = sx[3]*cx[4];
00181         R31 = cx[3];
00182         R32 =(-1)*sx[3]*sx[4];
00183 
00184         //x
00185         x0 =  cx[0]*sx[1];
00186         x1 =  cx[0]*sx[2];
00187         x2 =  cx[0]*sx[3];
00188         x3 = -cx[0]*cx[3]*cx[4]-sx[0]*sx[4];
00189         pose[0] = x0*mSegmentLength[0]+x1*mSegmentLength[1]+x2*mSegmentLength[2]+x3*mSegmentLength[3];
00190 
00191         //y
00192         y0 = sx[0]*sx[1];
00193         y1 = sx[0]*sx[2];
00194         y2 = sx[0]*sx[3];
00195         y3 = -sx[0]*cx[3]*cx[4]+cx[0]*sx[4];
00196         pose[1] = y0*mSegmentLength[0]+y1*mSegmentLength[1]+y2*mSegmentLength[2]+y3*mSegmentLength[3];
00197 
00198         //z
00199         z0 = cx[1];
00200         z1 = cx[2];
00201         z2 = cx[3];
00202         z3 = cx[4]*sx[3];
00203         pose[2] = z0*mSegmentLength[0]+z1*mSegmentLength[1]+z2*mSegmentLength[2]+z3*mSegmentLength[3];
00204 
00205         // phi, theta, psi
00206         pose[4] = acos(R33);
00207         if(pose[4]==0) {
00208                 pose[3] = atan2(pose[1],pose[0]);
00209                 pose[5] = 0;
00210         } else if(pose[4]==MHF_PI) {
00211                 pose[3] = atan2(pose[1],pose[0])+MHF_PI/2;
00212                 pose[5] = MHF_PI/2;
00213         } else {
00214                 pose[3] = atan2(R13,-R23);
00215                 pose[5] = atan2(R31,R32);
00216         }
00217 
00218 
00219         std::swap(aPosition, pose);
00220         return true;
00221 }
00223 bool Kinematics6M90G::inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition,
00224                 const std::vector<double> aStartingAngles) {
00225         if(!mIsInitialized) {
00226                 initialize();
00227         }
00228 
00229         // aPosition: Winkel Deg->Rad
00230         // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
00231         // 0-3 fr theta1_1
00232         // 4-7 fr theta1_2
00233 
00234         // Declaration
00235         position p_gr;
00236         position p_m;
00237         angles_container angle(cNrOfPossibleSolutions);
00238 
00239         // calculation of the gripper vector
00240         p_gr.x = mSegmentLength[3]*sin(aPosition[4])*sin(aPosition[3]);
00241         p_gr.y = -mSegmentLength[3]*sin(aPosition[4])*cos(aPosition[3]);
00242         p_gr.z = mSegmentLength[3]*cos(aPosition[4]);
00243 
00244         p_m.x = aPosition[0]-p_gr.x;
00245         p_m.y = aPosition[1]-p_gr.y;
00246         p_m.z = aPosition[2]-p_gr.z;
00247 
00248         // calculate theta1_1 and theta1_2
00249         angle[0].theta1 = MHF::atan1(p_m.x,p_m.y);
00250         angle[4].theta1 = angle[0].theta1+MHF_PI;
00251 
00252 
00253         // check the borders according to the settings
00254         if(angle[0].theta1>mAngleStop[0])
00255                 angle[0].theta1=angle[0].theta1-2.0*MHF_PI;
00256 
00257         if(angle[0].theta1<mAngleOffset[0])
00258                 angle[0].theta1=angle[0].theta1+2.0*MHF_PI;
00259 
00260         if(angle[4].theta1>mAngleStop[0])
00261                 angle[4].theta1=angle[4].theta1-2.0*MHF_PI;
00262 
00263         if(angle[4].theta1<mAngleOffset[0])
00264                 angle[4].theta1=angle[4].theta1+2.0*MHF_PI;
00265 
00266 
00267         //====THETA1_1==================
00268         //-------THETA234_1-------------
00269         IK_theta234theta5(angle[0], p_gr);
00270         IK_b1b2costh3_6MS(angle[0], p_m);
00271 
00272         angle[1]=angle[0];
00273         angle[0].theta3 =  acos(angle[0].costh3)-MHF_PI;
00274         thetacomp(angle[0], p_m);
00275         angle[1].theta3 =  -acos(angle[1].costh3)+MHF_PI;
00276         thetacomp(angle[1], p_m);
00277 
00278         //-------THETA234_2-------------
00279         angle[2].theta1=angle[0].theta1;
00280         angle[2].theta234=angle[0].theta234-MHF_PI;
00281         angle[2].theta5=MHF_PI-angle[0].theta5;
00282 
00283         IK_b1b2costh3_6MS(angle[2], p_m);
00284         angle[3]=angle[2];
00285         angle[2].theta3 =  acos(angle[2].costh3)-MHF_PI;
00286         thetacomp(angle[2], p_m);
00287         angle[3].theta3 =  -acos(angle[3].costh3)+MHF_PI;
00288         thetacomp(angle[3], p_m);
00289 
00290 
00291         //====THETA1_2==================
00292         //-------THETA234_1-------------
00293         IK_theta234theta5(angle[4], p_gr);
00294         IK_b1b2costh3_6MS(angle[4], p_m);
00295 
00296         angle[5]=angle[4];
00297         angle[4].theta3 =  acos(angle[4].costh3)-MHF_PI;
00298         thetacomp(angle[4], p_m);
00299         angle[5].theta3 =  -acos(angle[5].costh3)+MHF_PI;
00300         thetacomp(angle[5], p_m);
00301 
00302         //-------THETA234_2-------------
00303         angle[6].theta1=angle[4].theta1;
00304         angle[6].theta234=angle[4].theta234-MHF_PI;
00305         angle[6].theta5=MHF_PI-angle[4].theta5;
00306         IK_b1b2costh3_6MS(angle[6], p_m);
00307         angle[7]=angle[6];
00308         angle[6].theta3 =  acos(angle[6].costh3)-MHF_PI;
00309         thetacomp(angle[6], p_m);
00310         angle[7].theta3 =  -acos(angle[7].costh3)+MHF_PI;
00311         thetacomp(angle[7], p_m);
00312 
00313         // delete solutions out of range (in joint space)
00314         for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); /* do iter forward in body */ ) {
00315                 if( MHF::pow2(iter->costh3) <= 1.0) {
00316                         if(!angledef(*iter))
00317                                 iter = angle.erase(iter);
00318                         else
00319                                 ++iter;
00320                         continue;
00321                 }
00322                 iter = angle.erase(iter);
00323         }
00324 
00325 
00326         // check if solutions in range left
00327         if(angle.size() == 0) {
00328                 throw NoSolutionException();
00329         }
00330 
00331         // store possible solution angles to std::vector<std::vector<double>>
00332         std::vector< std::vector<double> > PossibleTargets;
00333         for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00334                 std::vector<double> possangles(5);
00335 
00336                 possangles[0] = i->theta1;
00337                 possangles[1] = i->theta2;
00338                 possangles[2] = i->theta3;
00339                 possangles[3] = i->theta4;
00340                 possangles[4] = i->theta5;
00341 
00342                 PossibleTargets.push_back(possangles);
00343         }
00344 
00345         // choose best solution
00346         std::vector< std::vector<double> >::const_iterator sol = KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
00347 
00348         if(sol == PossibleTargets.end()) {
00349                 throw NoSolutionException();
00350         }
00351 
00352         // copy solution to aAngles vector
00353         for (int i = aAngles.size(); i < 6; ++i)
00354                 aAngles.push_back(0.0);
00355         std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
00356         *gripper_iter = aStartingAngles[5]; // copy gripper-angle from current
00357 
00358         return true;
00359 }
00361 bool Kinematics6M90G::initialize() {
00362         // NOTE: data for Katana6M90A with angle gripper
00363         
00364         mIsInitialized = false;
00365 
00366         //fill in segment data
00367         mNumberOfSegments = 4;
00368 
00369         mSegmentLength.push_back(190.0);
00370         mSegmentLength.push_back(139.0);
00371         mSegmentLength.push_back(147.3);
00372         mSegmentLength.push_back(150.5);
00373 
00374         //fill in joint data
00375         mNumberOfMotors = 6;
00376 
00377         mAngleOffset.push_back(0.116064);
00378         mAngleStop.push_back(6.154904);
00379         mEncodersPerCycle.push_back(51200);
00380         mEncoderOffset.push_back(31000);
00381         mRotationDirection.push_back(1);
00382 
00383         mAngleOffset.push_back(2.168572);
00384         mAngleStop.push_back(-0.274889);
00385         mEncodersPerCycle.push_back(94976);
00386         mEncoderOffset.push_back(-31000);
00387         mRotationDirection.push_back(1);
00388 
00389         mAngleOffset.push_back(0.919789);
00390         mAngleStop.push_back(5.283112);
00391         mEncodersPerCycle.push_back(47488);
00392         mEncoderOffset.push_back(-31000);
00393         mRotationDirection.push_back(-1);
00394 
00395         mAngleOffset.push_back(1.108284);
00396         mAngleStop.push_back(5.122541);
00397         mEncodersPerCycle.push_back(51200);
00398         mEncoderOffset.push_back(31000);
00399         mRotationDirection.push_back(1);
00400 
00401         mAngleOffset.push_back(0.148353); // for 6M90B: -2.99324
00402         mAngleStop.push_back(6.117379); // for 6M90B: 2.975787
00403         mEncodersPerCycle.push_back(51200);
00404         mEncoderOffset.push_back(31000);
00405         mRotationDirection.push_back(1);
00406 
00407         mAngleOffset.push_back(-2.085668);
00408         mAngleStop.push_back(3.656465);
00409         mEncodersPerCycle.push_back(51200);
00410         mEncoderOffset.push_back(31000);
00411         mRotationDirection.push_back(1);
00412 
00413         mIsInitialized = true;
00414 
00415         return mIsInitialized;
00416 }
00418 void Kinematics6M90G::IK_theta234theta5(angles_calc& angle, const position &p_gr) const {
00419         angle.theta234 = -MHF::acotan( (  (p_gr.x * p_gr.z * cos(angle.theta1) ) -
00420                                      sqrt( ( -MHF::pow2(p_gr.z) ) *
00421                                            ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
00422                                          )
00423                                   ) / MHF::pow2(p_gr.z)
00424                                 );
00425 
00426         angle.theta5   = acos( p_gr.z/(mSegmentLength[3]*sin(angle.theta234)) );
00427 
00428         if(p_gr.z==0) {
00429                 angle.theta234=0;
00430                 angle.theta5=angle.theta1-MHF::atan1(-p_gr.x,-p_gr.y);
00431         }
00432 
00433         bool griptest;
00434         griptest = GripperTest(p_gr, angle);
00435         if(!griptest) {
00436                 angle.theta5=-angle.theta5;
00437                 griptest=GripperTest(p_gr, angle);
00438                 if(!griptest) {
00439                         angle.theta234 = -MHF::acotan( (   ( p_gr.x * p_gr.z * cos(angle.theta1) ) +
00440                                                       sqrt( ( -MHF::pow2(p_gr.z) ) *
00441                                                             ( -MHF::pow2(mSegmentLength[3]) + MHF::pow2(p_gr.x) + MHF::pow2(p_gr.z) ) * MHF::pow2(sin(angle.theta1))
00442                                                           )
00443                                                   ) / MHF::pow2(p_gr.z)
00444                                                 );
00445                         angle.theta5 =  acos( p_gr.z / (mSegmentLength[3]*sin(angle.theta234)) );
00446                         if(p_gr.z==0) {
00447                                 angle.theta234=-MHF_PI;
00448                                 angle.theta5=MHF::atan1(p_gr.x,p_gr.y) - angle.theta1;
00449                         }
00450 
00451                         griptest=GripperTest(p_gr, angle);
00452                         if(!griptest) {
00453                                 angle.theta5=-angle.theta5;
00454                         }
00455                 }
00456         }
00457 
00458 }
00460 bool Kinematics6M90G::GripperTest(const position &p_gr, const angles_calc &angle) const {
00461         double xgr2, ygr2, zgr2;
00462 
00463         xgr2 = -mSegmentLength[3]*(cos(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)+sin(angle.theta1)*sin(angle.theta5));
00464         ygr2 = -mSegmentLength[3]*(sin(angle.theta1)*cos(angle.theta234)*cos(angle.theta5)-cos(angle.theta1)*sin(angle.theta5));
00465         zgr2 =  mSegmentLength[3]*sin(angle.theta234)*cos(angle.theta5);
00466 
00467         if((MHF::pow2(p_gr.x-xgr2)+MHF::pow2(p_gr.y-ygr2)+MHF::pow2(p_gr.z-zgr2))>=cTolerance)
00468                 return false;
00469 
00470         return true;
00471 }
00473 void Kinematics6M90G::IK_b1b2costh3_6MS(angles_calc &angle, const position &p) const {
00474         double xg, yg, zg;
00475         double d5 = mSegmentLength[2] + mSegmentLength[3];
00476         xg = p.x + ( mSegmentLength[3] * cos(angle.theta1) * sin(angle.theta234) );
00477         yg = p.y + ( mSegmentLength[3] * sin(angle.theta1) * sin(angle.theta234) );
00478         zg = p.z + ( mSegmentLength[3]                     * cos(angle.theta234) );
00479 
00480 
00481         angle.b1 = xg*cos(angle.theta1) + yg*sin(angle.theta1) - d5*sin(angle.theta234);
00482         angle.b2 = zg - d5*cos(angle.theta234);
00483         angle.costh3 = -( MHF::pow2(angle.b1) + MHF::pow2(angle.b2) - MHF::pow2(mSegmentLength[0]) - MHF::pow2(mSegmentLength[1]) ) / ( 2.0*mSegmentLength[0]*mSegmentLength[1] );
00484 
00485 }
00487 void Kinematics6M90G::thetacomp(angles_calc &angle, const position &p_m) const {
00488         angle.theta2 = -MHF_PI/2.0 - ( MHF::atan0(angle.b1,angle.b2)+MHF::atan0(mSegmentLength[0]+mSegmentLength[1]*cos(angle.theta3),mSegmentLength[1]*sin(angle.theta3)) );
00489         angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00490 
00491         if(!PositionTest6MS(angle,p_m)) {
00492                 angle.theta2 = angle.theta2+MHF_PI;
00493                 angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00494         }
00495 
00496 }
00498 bool Kinematics6M90G::PositionTest6MS(const angles_calc &a, const position &p) const {
00499         double temp, xm2, ym2, zm2;
00500 
00501         temp = mSegmentLength[0]*sin(a.theta2)+mSegmentLength[1]*sin(a.theta2+a.theta3)+mSegmentLength[2]*sin(a.theta234);
00502         xm2 = cos(a.theta1)*temp;
00503         ym2 = sin(a.theta1)*temp;
00504         zm2 = mSegmentLength[0]*cos(a.theta2)+mSegmentLength[1]*cos(a.theta2+a.theta3)+mSegmentLength[2]*cos(a.theta234);
00505 
00506         if((MHF::pow2(p.x-xm2)+MHF::pow2(p.y-ym2)+MHF::pow2(p.z-zm2))>=cTolerance)
00507                 return false;
00508 
00509         return true;
00510 }
00512 bool Kinematics6M90G::angledef(angles_calc &a) const {
00513         // constants here. needs refactoring:
00514         a.theta2=MHF::anglereduce(a.theta2+MHF_PI/2.0);
00515         a.theta3=MHF::anglereduce(a.theta3+MHF_PI);
00516         a.theta4=MHF::anglereduce(MHF_PI-a.theta4);
00517         a.theta5=MHF::anglereduce(a.theta5);
00518 
00519         if(a.theta1>mAngleStop[0]) {
00520                 a.theta1=a.theta1-2.0*MHF_PI;
00521         }
00522         if(a.theta2>MHF_PI) {
00523                 a.theta2=a.theta2-2.0*MHF_PI;
00524         }
00525         if(a.theta5<mAngleOffset[4]) {
00526                 a.theta5=a.theta5+2.0*MHF_PI;
00527         }
00528 
00529         return AnglePositionTest(a);
00530 
00531 }
00533 bool Kinematics6M90G::AnglePositionTest(const angles_calc &a) const {
00534 
00535         if( (a.theta1+0.0087<mAngleOffset[0])||(a.theta1>mAngleStop[0]) )
00536                 return false;
00537 
00538         if( (a.theta2-0.0087>mAngleOffset[1])||(a.theta2<mAngleStop[1]) )
00539                 return false;
00540 
00541         if( (a.theta3<mAngleOffset[2])||(a.theta3>mAngleStop[2]) )
00542                 return false;
00543 
00544         if( (a.theta4<mAngleOffset[3])||(a.theta4>mAngleStop[3]) )
00545                 return false;
00546 
00547         if( (a.theta5<mAngleOffset[4])||(a.theta5>mAngleStop[4]) )
00548                 return false;
00549 
00550         return true;
00551 }
00553 } // namespace


kni
Author(s): Martin Günther
autogenerated on Thu Aug 27 2015 13:40:07