kinematics6M180.cpp
Go to the documentation of this file.
00001 /***************************************************************************
00002  *   Copyright (C) 2008 by Neuronics AG                                    *
00003  *   support@neuronics.ch                                                  *
00004  ***************************************************************************/
00005 
00006 #include <kinematics6M180.h>
00007 //#include <iostream>
00008 namespace AnaGuess {
00009 
00011 Kinematics6M180::Kinematics6M180() {
00012         initialize();
00013 }
00015 Kinematics6M180::~Kinematics6M180() {
00016 }
00017 
00019 std::vector<double> Kinematics6M180::getLinkLength() {
00020         std::vector<double> result(mSegmentLength);
00021         return result;
00022 }
00024 std::vector<int> Kinematics6M180::getEpc() {
00025         std::vector<int> result(mEncodersPerCycle);
00026         return result;
00027 }
00029 std::vector<int> Kinematics6M180::getEncOff() {
00030         std::vector<int> result(mEncoderOffset);
00031         return result;
00032 }
00034 std::vector<int> Kinematics6M180::getDir() {
00035         std::vector<int> result(mRotationDirection);
00036         return result;
00037 }
00039 std::vector<double> Kinematics6M180::getAngOff() {
00040         std::vector<double> result(mAngleOffset);
00041         return result;
00042 }
00044 std::vector<double> Kinematics6M180::getAngStop() {
00045         std::vector<double> result(mAngleStop);
00046         return result;
00047 }
00049 std::vector<double> Kinematics6M180::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> Kinematics6M180::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> Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::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 Kinematics6M180::directKinematics(std::vector<double>& aPosition, const std::vector<double> aAngles) {
00141         if(!mIsInitialized) {
00142                 initialize();
00143         }
00144 
00145         // copy aAngles to currentAngles
00146         std::vector<double> currentAngles(6);
00147         for(int i = 0; i < 6; ++i) {
00148                 currentAngles[i] = aAngles[i];
00149         }
00150 
00151         // adjust angles (needs refactoring)
00152         currentAngles[1] = currentAngles[1] - MHF_PI/2.0;
00153         currentAngles[2] = currentAngles[2] - MHF_PI;
00154         currentAngles[3] = MHF_PI - currentAngles[3];
00155         currentAngles[4] = -currentAngles[4];
00156 
00157         double factor;
00158         double r13, r23, r31, r32;
00159 
00160         std::vector<double> pose(6);
00161 
00162         std::vector<double> cx(currentAngles.size()), sx(currentAngles.size());
00163         std::vector<double>::iterator cx_iter, sx_iter;
00164 
00165         std::vector<double> angle = currentAngles;
00166 
00167         angle[2] = angle[1]+angle[2];
00168         angle[3] = angle[2]+angle[3];
00169 
00170         cx_iter = cx.begin();
00171         sx_iter = sx.begin();
00172         std::transform(angle.begin(), angle.end(), sx_iter, MHF::unary_precalc_sin<double>() );
00173         std::transform(angle.begin(), angle.end(), cx_iter, MHF::unary_precalc_cos<double>() );
00174 
00175         factor = (mSegmentLength[0]*sx[1]+mSegmentLength[1]*sx[2]+(mSegmentLength[2]+mSegmentLength[3])*sx[3]);
00176         // x = px (compare homogenous transformation matrix)
00177         pose[0] = cx[0]*factor;
00178 
00179         // y = py (compare homogenous transformation matrix)
00180         pose[1] = sx[0]*factor;
00181 
00182         // z = pz (compare homogenous transformation matrix)
00183         pose[2] = mSegmentLength[0]*cx[1]+mSegmentLength[1]*cx[2]+(mSegmentLength[2]+mSegmentLength[3])*cx[3];
00184 
00185         // phi = atan2(r13/-r23) (compare homogenous transformation matrix)
00186         r13 = cx[0]*sx[3];
00187         r23 = sx[0]*sx[3];
00188         pose[3] = atan2(r13,-r23);
00189 
00190         // theta = acos(r33) (compare homogenous transformation matrix)
00191         pose[4] = acos(cx[3]);
00192 
00193         // psi = atan2(r31/r32)  (compare homogenous transformation matrix)
00194         r31 = sx[3]*sx[4];
00195         r32 = sx[3]*cx[4];
00196         pose[5] = atan2(r31,r32);
00197 
00198         std::swap(aPosition, pose);
00199         return true;
00200 }
00202 bool Kinematics6M180::inverseKinematics(std::vector<double>& aAngles, const std::vector<double> aPosition,
00203                 const std::vector<double> aStartingAngles) {
00204         if(!mIsInitialized) {
00205                 initialize();
00206         }
00207 
00208         // Alle 8 Loeungen werden in einem Array angle, welches aus 8 Structs besteht, gespeichert:
00209         // 0-3 fuer theta1_1
00210         // 4-7 fuer theta1_2
00211 
00212         // Declarations
00213         position p_m;
00214         angles_container angle(cNrOfPossibleSolutions);
00215         double coeff1, coeff2, theta234;
00216         double costh5, sinth5, theta5[2];
00217         double R11, R21, R31, R32;
00218         double phi, theta, psi;
00219 
00220         // Initialization
00221         p_m.x = aPosition[0];
00222         p_m.y = aPosition[1];
00223         p_m.z = aPosition[2];
00224 
00225         // calculate theta1_1 and theta1_2
00226         angle[0].theta1 = MHF::atan1(aPosition[0],aPosition[1]);
00227         if (angle[0].theta1 > MHF_PI) {
00228                 angle[0].theta1 = angle[0].theta1 - MHF_PI;
00229                 if (angle[0].theta1 > (179.91/180*MHF_PI)) {
00230                         angle[0].theta1 = angle[0].theta1 - MHF_PI;
00231                 }
00232         }
00233         angle[4].theta1 = angle[0].theta1+MHF_PI;
00234 
00235         theta = aPosition[4];
00236         psi = aPosition[5];
00237         phi = MHF::atan1(p_m.x,p_m.y)+MHF_PI/2.0;
00238         theta234 = aPosition[4];
00239 
00240         R11 = cos(phi)*cos(psi)-sin(phi)*cos(theta)*sin(psi);
00241         R21 = sin(phi)*cos(psi)+cos(phi)*cos(theta)*sin(psi);
00242         R31 = sin(theta)*sin(psi);
00243         R32 = sin(theta)*cos(psi);
00244 
00245         // calculate theta5
00246         if(theta234==0) {
00247                 //std::cout << "Warning: Singularity theta234=0 !" << std::endl;
00248                 for (int i=0; i<2; ++i) {
00249                         coeff1 = -sin(angle[i*4].theta1);
00250                         coeff2 = -cos(angle[i*4].theta1);
00251                         costh5 = coeff1*R11-coeff2*R21;
00252                         sinth5 = coeff1*R21+coeff2*R11;
00253                         theta5[i] = -MHF::findFirstEqualAngle(costh5, sinth5, cTolerance);
00254                 }
00255                 for (int i=0; i<cNrOfPossibleSolutions; ++i) {
00256                         if(i<4)
00257                                 angle[i].theta5 = theta5[0];
00258                         else
00259                                 angle[i].theta5 = theta5[1];
00260                 }
00261         } else if(theta234==MHF_PI) {
00262                 //std::cout << "Warning: Singularity theta234=PI !" << std::endl;
00263                 for (int i=0; i<2; ++i) {
00264                         coeff1 = -sin(angle[i*4].theta1);
00265                         coeff2 = cos(angle[i*4].theta1);
00266                         costh5 = coeff1*R11+coeff2*R21;
00267                         sinth5 = -coeff1*R21+coeff2*R11;
00268                         theta5[i] = -MHF::findFirstEqualAngle(costh5, sinth5, cTolerance);
00269                 }
00270                 for (int i=0; i<cNrOfPossibleSolutions; ++i) {
00271                         if(i<4)
00272                                 angle[i].theta5 = theta5[0];
00273                         else
00274                                 angle[i].theta5 = theta5[1];
00275                 }
00276         } else {
00277                 theta5[0] = -atan2(R31/sin(theta234),R32/sin(theta234));
00278                 theta5[1] = -atan2(R31/sin(-theta234),R32/sin(-theta234));
00279                 for (int i=0; i<cNrOfPossibleSolutions; ++i) {
00280                         if(i%4==0 || i%4==1)
00281                                 angle[i].theta5 = theta5[0];
00282                         else
00283                                 angle[i].theta5 = theta5[1];
00284                 }
00285         }
00286 
00287         //====THETA1_1==================
00288         //-------THETA234_1-------------
00289         angle[0].theta234 = aPosition[4];
00290         //angle[0].theta5 = pose[5];
00291         IK_b1b2costh3_6M180(angle[0], p_m);
00292 
00293         angle[1] =angle[0];
00294         angle[0].theta3 =  acos(angle[0].costh3)-MHF_PI;
00295         thetacomp(angle[0], p_m);
00296         angle[1].theta3 =  -acos(angle[1].costh3)+MHF_PI;
00297         thetacomp(angle[1], p_m);
00298 
00299         //-------THETA234_2-------------
00300         angle[2].theta1 = angle[0].theta1;
00301         angle[2].theta234 = -angle[0].theta234;
00302         //angle[2].theta5 = angle[0].theta5;
00303         IK_b1b2costh3_6M180(angle[2], p_m);
00304 
00305         angle[3] = angle[2];
00306         angle[2].theta3 =  acos(angle[2].costh3)-MHF_PI;
00307         thetacomp(angle[2], p_m);
00308         angle[3].theta3 =  -acos(angle[3].costh3)+MHF_PI;
00309         thetacomp(angle[3], p_m);
00310 
00311         //====THETA1_2==================
00312         //-------THETA234_1-------------
00313         angle[4].theta234 = aPosition[4];
00314         //angle[4].theta5 = pose[5];
00315         IK_b1b2costh3_6M180(angle[4], p_m);
00316 
00317         angle[5] = angle[4];
00318         angle[4].theta3 = acos(angle[4].costh3)-MHF_PI;
00319         thetacomp(angle[4], p_m);
00320         angle[5].theta3 = -acos(angle[5].costh3)+MHF_PI;
00321         thetacomp(angle[5], p_m);
00322 
00323         //-------THETA234_2-------------
00324         angle[6].theta1 = angle[4].theta1;
00325         angle[6].theta234 = -angle[4].theta234;
00326         //angle[6].theta5 = angle[4].theta5;
00327         IK_b1b2costh3_6M180(angle[6], p_m);
00328 
00329         angle[7] = angle[6];
00330         angle[6].theta3 =  acos(angle[6].costh3)-MHF_PI;
00331         thetacomp(angle[6], p_m);
00332         angle[7].theta3 =  -acos(angle[7].costh3)+MHF_PI;
00333         thetacomp(angle[7], p_m);
00334 
00335         // delete solutions out of range (in joint space)
00336         for( std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end();) {
00337                 if( MHF::pow2(iter->costh3) <= 1.0) {
00338                         if(!angledef(*iter))
00339                                 iter = angle.erase(iter);
00340                         else
00341                                 ++iter;
00342                         continue;
00343                 }
00344                 iter = angle.erase(iter);
00345         }
00346 
00347         // check if solutions in range left
00348         if(angle.size() == 0) {
00349                 throw NoSolutionException();
00350         }
00351 
00352         // store possible solution angles to std::vector<std::vector<double>>
00353         std::vector< std::vector<double> > PossibleTargets;
00354         for( std::vector<angles_calc>::iterator i = angle.begin(); i != angle.end(); ++i ) {
00355                 std::vector<double> possangles(5);
00356 
00357                 possangles[0] = i->theta1;
00358                 possangles[1] = i->theta2;
00359                 possangles[2] = i->theta3;
00360                 possangles[3] = i->theta4;
00361                 possangles[4] = i->theta5;
00362 
00363                 PossibleTargets.push_back(possangles);
00364         }
00365 
00366         // choose best solution
00367         std::vector< std::vector<double> >::const_iterator sol = KinematicsDefaultRadMinAlgorithm()(PossibleTargets.begin(), PossibleTargets.end(), aStartingAngles.begin(), aStartingAngles.end());
00368 
00369         if(sol == PossibleTargets.end()) {
00370                 throw NoSolutionException();
00371         }
00372 
00373         // copy solution to aAngles vector
00374         for (int i = aAngles.size(); i < 6; ++i)
00375                 aAngles.push_back(0.0);
00376         std::vector<double>::iterator gripper_iter = std::copy( (*sol).begin(), (*sol).end(), aAngles.begin() );
00377         *gripper_iter = aStartingAngles[5]; // copy gripper-angle from current
00378 
00379         return true;
00380 }
00382 bool Kinematics6M180::initialize() {
00383         // NOTE: data for Katana6M180 with flange
00384         
00385         mIsInitialized = false;
00386 
00387         //fill in segment data
00388         mNumberOfSegments = 4;
00389 
00390         mSegmentLength.push_back(190.0);
00391         mSegmentLength.push_back(139.0);
00392         mSegmentLength.push_back(147.3);
00393         mSegmentLength.push_back(41.0); // for anglegripper: 155.5
00394 
00395         //fill in joint data
00396         mNumberOfMotors = 6;
00397 
00398         mAngleOffset.push_back(0.116064);
00399         mAngleStop.push_back(6.154904);
00400         mEncodersPerCycle.push_back(51200);
00401         mEncoderOffset.push_back(31000);
00402         mRotationDirection.push_back(1);
00403 
00404         mAngleOffset.push_back(2.168572);
00405         mAngleStop.push_back(-0.274889);
00406         mEncodersPerCycle.push_back(94976);
00407         mEncoderOffset.push_back(-31000);
00408         mRotationDirection.push_back(1);
00409 
00410         mAngleOffset.push_back(0.919789);
00411         mAngleStop.push_back(5.283112);
00412         mEncodersPerCycle.push_back(47488);
00413         mEncoderOffset.push_back(-31000);
00414         mRotationDirection.push_back(-1);
00415 
00416         mAngleOffset.push_back(1.108284);
00417         mAngleStop.push_back(5.122541);
00418         mEncodersPerCycle.push_back(51200);
00419         mEncoderOffset.push_back(31000);
00420         mRotationDirection.push_back(1);
00421 
00422         mAngleOffset.push_back(0.148353);
00423         mAngleStop.push_back(6.117379);
00424         mEncodersPerCycle.push_back(51200);
00425         mEncoderOffset.push_back(31000);
00426         mRotationDirection.push_back(1);
00427 
00428         mAngleOffset.push_back(-2.085668);
00429         mAngleStop.push_back(3.656465);
00430         mEncodersPerCycle.push_back(51200);
00431         mEncoderOffset.push_back(31000);
00432         mRotationDirection.push_back(1);
00433 
00434         mIsInitialized = true;
00435 
00436         return mIsInitialized;
00437 }
00439 void Kinematics6M180::IK_b1b2costh3_6M180(angles_calc &angle, const position &p) const {
00440         double d5 = mSegmentLength[2] + mSegmentLength[3];
00441 
00442         angle.b1 = p.x*cos(angle.theta1) + p.y*sin(angle.theta1) - d5*sin(angle.theta234);
00443         angle.b2 = p.z - d5*cos(angle.theta234);
00444         angle.costh3 = -( MHF::pow2(angle.b1) + MHF::pow2(angle.b2) - MHF::pow2(mSegmentLength[0]) - MHF::pow2(mSegmentLength[1]) ) / ( 2.0*mSegmentLength[0]*mSegmentLength[1] );
00445 
00446 }
00448 void Kinematics6M180::thetacomp(angles_calc &angle, const position &p_m) const {
00449         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)) );
00450         angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00451 
00452         if(!PositionTest6M180(angle,p_m)) {
00453                 angle.theta2 = angle.theta2+MHF_PI;
00454                 angle.theta4 = angle.theta234-angle.theta2-angle.theta3;
00455         }
00456 
00457 }
00459 bool Kinematics6M180::PositionTest6M180(const angles_calc &a, const position &p) const {
00460         double temp, xm2, ym2, zm2;
00461 
00462         temp = mSegmentLength[0]*sin(a.theta2)+mSegmentLength[1]*sin(a.theta2+a.theta3)+(mSegmentLength[2]+mSegmentLength[3])*sin(a.theta234);
00463         xm2 = cos(a.theta1)*temp;
00464         ym2 = sin(a.theta1)*temp;
00465         zm2 = mSegmentLength[0]*cos(a.theta2)+mSegmentLength[1]*cos(a.theta2+a.theta3)+(mSegmentLength[2]+mSegmentLength[3])*cos(a.theta234);
00466 
00467         if((MHF::pow2(p.x-xm2)+MHF::pow2(p.y-ym2)+MHF::pow2(p.z-zm2))>=cTolerance)
00468                 return false;
00469 
00470         return true;
00471 }
00473 bool Kinematics6M180::angledef(angles_calc &a) const {
00474         // constants here. needs refactoring:
00475         a.theta2=MHF::anglereduce(a.theta2+MHF_PI/2.0);
00476         a.theta3=MHF::anglereduce(a.theta3+MHF_PI);
00477         a.theta4=MHF::anglereduce(MHF_PI-a.theta4);
00478         a.theta5=MHF::anglereduce(a.theta5);
00479 
00480         if(a.theta1>mAngleStop[0]) {
00481                 a.theta1=a.theta1-2.0*MHF_PI;
00482         }
00483         if(a.theta2>MHF_PI) {
00484                 a.theta2=a.theta2-2.0*MHF_PI;
00485         }
00486         if(a.theta5<mAngleOffset[4]) {
00487                 a.theta5=a.theta5+2.0*MHF_PI;
00488         }
00489 
00490         return AnglePositionTest(a);
00491 
00492 }
00494 bool Kinematics6M180::AnglePositionTest(const angles_calc &a) const {
00495 
00496         if( (a.theta1+0.0087<mAngleOffset[0])||(a.theta1>mAngleStop[0]) )
00497                 return false;
00498 
00499         if( (a.theta2-0.0087>mAngleOffset[1])||(a.theta2<mAngleStop[1]) )
00500                 return false;
00501 
00502         if( (a.theta3<mAngleOffset[2])||(a.theta3>mAngleStop[2]) )
00503                 return false;
00504 
00505         if( (a.theta4<mAngleOffset[3])||(a.theta4>mAngleStop[3]) )
00506                 return false;
00507 
00508         if( (a.theta5<mAngleOffset[4])||(a.theta5>mAngleStop[4]) )
00509                 return false;
00510 
00511         return true;
00512 }
00514 
00515 } // namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


kni
Author(s): Neuronics AG (see AUTHORS.txt); ROS wrapper by Martin Günther
autogenerated on Tue May 28 2013 14:52:54