00001
00002
00003
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
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
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
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
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
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
00201 pose[4] = acos(cx[4]*sx[3]);
00202
00203
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 ) {
00212
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
00226 pose[5] = 0;
00227 } else {
00228
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
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
00249
00250
00251
00252
00253
00254 position p_gr;
00255 position p_m;
00256 angles_container angle(cNrOfPossibleSolutions);
00257
00258
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
00268 angle[0].theta1 = MHF::atan1(p_m.x,p_m.y);
00269 angle[4].theta1 = angle[0].theta1+MHF_PI;
00270
00271
00272
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
00287
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
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
00311
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
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
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
00346 if(angle.size() == 0) {
00347 throw NoSolutionException();
00348 }
00349
00350
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
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
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
00382
00383 mIsInitialized = false;
00384
00385
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
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);
00421 mAngleStop.push_back(6.117379);
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
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 }