00001
00002
00003
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
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
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
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
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
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
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
00230
00231
00232
00233
00234
00235 position p_gr;
00236 position p_m;
00237 angles_container angle(cNrOfPossibleSolutions);
00238
00239
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
00249 angle[0].theta1 = MHF::atan1(p_m.x,p_m.y);
00250 angle[4].theta1 = angle[0].theta1+MHF_PI;
00251
00252
00253
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
00268
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
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
00292
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
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
00314 for( ::std::vector<angles_calc>::iterator iter = angle.begin(); iter != angle.end(); ) {
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
00327 if(angle.size() == 0) {
00328 throw NoSolutionException();
00329 }
00330
00331
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
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
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];
00357
00358 return true;
00359 }
00361 bool Kinematics6M90G::initialize() {
00362
00363
00364 mIsInitialized = false;
00365
00366
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
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);
00402 mAngleStop.push_back(6.117379);
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
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 }