00001 #include "IKGradientSolver.h"
00002 #include "Shared/fmat.h"
00003 #include <cmath>
00004 #include <valarray>
00005
00006 using namespace std;
00007
00008 const std::string IKGradientSolver::autoRegisterIKGradientSolver = IKSolver::getRegistry().registerType<IKGradientSolver>("IKGradientSolver");
00009 const std::string IKGradientSolver::autoRegisterDefaultIKSolver = IKSolver::getRegistry().registerType<IKGradientSolver>("");
00010
00011 bool IKGradientSolver::solve(const Point& pEff, const Rotation& oriEff,
00012 KinematicJoint& j, const Position& pTgt, float posPri,
00013 const Orientation& oriTgt, float oriPri) const {
00014 if (posPri==0 && oriPri==0)
00015 return true;
00016
00017 posPri=std::max(0.f,posPri);
00018 oriPri=std::max(0.f,oriPri);
00019
00020 KinematicJoint* firstMobileJoint = NULL;
00021 for(KinematicJoint *ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent())
00022 if (ancestor->isMobile())
00023 firstMobileJoint = ancestor;
00024 if (firstMobileJoint==NULL)
00025 return false;
00026
00027
00028 fmat::Transform Tj = j.getT(*firstMobileJoint);
00029 fmat::Column<3> pEffFMJ(Tj*pEff);
00030 fmat::fmatReal pDist = pEffFMJ.norm()/10;
00031
00032
00033 fmat::fmatReal oriDist = 1;
00034
00035 std::valarray<fmat::fmatReal> q(j.getDepth()+1);
00036 for(KinematicJoint* ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent())
00037 q[ancestor->getDepth()] = ancestor->getQ();
00038 std::valarray<fmat::fmatReal> dq, ndq(j.getDepth()+1);
00039 unsigned int iterCnt=0, overCnt=0;
00040 bool solved=false;
00041 while(iterCnt<MAX_ITER) {
00042 IKSolver::StepResult_t res = step(pEff,oriEff,j,pTgt,pDist,posPri,oriTgt,oriDist,oriPri,false);
00043 if (res==SUCCESS) {
00044 solved=true;
00045 break;
00046 }
00047 if (res!=PROGRESS) {
00048 std::cout << "Result " << res << std::endl;
00049 break;
00050 }
00051 for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent()) {
00052 ndq[ancestor->getDepth()] = ancestor->getQ() - q[ancestor->getDepth()];
00053 q[ancestor->getDepth()] = ancestor->getQ();
00054 }
00055 fmat::fmatReal qa=1;
00056 if (dq.size()==0) {
00057 dq.resize(j.getDepth()+1);
00058 dq=ndq;
00059 } else {
00060 qa = (dq*ndq).sum();
00061 std::swap(dq,ndq);
00062 }
00063
00064 if ( std::max(std::abs(dq.max()), std::abs(dq.min())) < QTOL ) {
00065 std::cout << "Converged" << std::endl;
00066 break;
00067 }
00068 if (qa<0) {
00069
00070
00071
00072 pDist *= 0.75f;
00073 oriDist *= 0.75f;
00074 ++overCnt;
00075 }
00076
00077
00078
00079
00080 ++iterCnt;
00081 }
00082 cout << "Iterations: " << iterCnt << " Overshoots: " << overCnt
00083 << " solved=" << solved << endl;
00084 return solved;
00085 }
00086
00087 IKSolver::StepResult_t IKGradientSolver::step(const Point& pEff, const Rotation& oriEff,
00088 KinematicJoint& j, const Position& pTgt, float pDist, float posPri,
00089 const Orientation& oriTgt, float oriDist, float oriPri,
00090 bool successInReach) const {
00091 if ((posPri<=0 && oriPri<=0) || (pDist<=0 && oriDist<=0))
00092 return SUCCESS;
00093
00094 posPri=std::max(0.f,posPri);
00095 oriPri=std::max(0.f,oriPri);
00096 pDist=std::max(0.f,pDist);
00097 oriDist=std::max(0.f,oriDist);
00098
00099 const float totPri = posPri+oriPri;
00100 posPri/=totPri;
00101 oriPri/=totPri;
00102
00103
00104 StepResult_t result = LIMITS;
00105
00106
00107 std::vector<int> mobile;
00108 mobile.reserve(j.getDepth()+1);
00109 for(KinematicJoint * ancestor=&j; ancestor!=NULL; ancestor=ancestor->getParent()) {
00110 if (ancestor->isMobile())
00111 mobile.push_back(ancestor->getDepth());
00112
00113
00114 }
00115 std::reverse(mobile.begin(),mobile.end());
00116
00117
00118 fmat::fmatReal distRemain = pDist;
00119 fmat::fmatReal angRemain = oriDist;
00120
00121 while(mobile.size()>0) {
00122 fmat::Transform Tj = j.getFullT();
00123 fmat::Column<3> pEffBase(Tj*pEff);
00124 fmat::Quaternion qj = j.getWorldQuaternion();
00125 Rotation oEffBase = qj*oriEff;
00126
00127 fmat::Column<3> pErr;
00128 pTgt.computeErrorGradient(pEffBase,oEffBase,pErr);
00129
00130 fmat::Quaternion oriErr;
00131 oriTgt.computeErrorGradient(pEffBase,oEffBase,oriErr);
00132 oriErr.normalize();
00133
00134 fmat::fmatReal pErrMagnitude = pErr.norm();
00135 fmat::fmatReal oriErrMagnitude = std::abs(oriErr.angle());
00136
00137
00138 if (pErrMagnitude/distRemain*posPri < .001 && oriErrMagnitude/angRemain*oriPri < .001) {
00139
00140 return SUCCESS;
00141 }
00142 if (pErrMagnitude*posPri < PTOL && oriErrMagnitude*oriPri < OTOL) {
00143
00144 return SUCCESS;
00145 }
00146 if (pErrMagnitude<distRemain && oriErrMagnitude<angRemain) {
00147 distRemain=pErrMagnitude;
00148 angRemain=oriErrMagnitude;
00149 if (successInReach)
00150 result = SUCCESS;
00151 } else if (pErrMagnitude<distRemain) {
00152 distRemain=pErrMagnitude;
00153 if (oriPri==0 && successInReach)
00154 result = SUCCESS;
00155 } else if (oriErrMagnitude<angRemain) {
00156 angRemain=oriErrMagnitude;
00157 if (posPri==0 && successInReach)
00158 result = SUCCESS;
00159 }
00160
00161 std::vector<fmat::Column<6> > J;
00162 j.getFullJacobian(pEffBase,J);
00163
00164 std::vector<fmat::Column<6> > Jm(mobile.size());
00165 for(size_t i=0; i<mobile.size(); ++i)
00166 Jm[i] = J[mobile[i]];
00167
00168
00169 std::valarray<fmat::fmatReal> q(mobile.size());
00170
00171
00172 if (posPri>0 && distRemain>0) {
00173
00174
00175
00176
00177
00178
00179
00180 std::valarray<fmat::fmatReal> posQ(Jm.size());
00181 for(unsigned int i=0; i<posQ.size(); ++i) {
00182 posQ[i]=fmat::dotProduct(fmat::SubVector<3>(Jm[i]),pErr);
00183 }
00184
00185
00186
00187
00188 fmat::Column<3> move;
00189 for(unsigned int i=0; i<posQ.size(); ++i)
00190 move+=fmat::SubVector<3>(Jm[i])*posQ[i];
00191
00192 fmat::fmatReal moveSize = move.norm();
00193 if (moveSize > std::numeric_limits<float>::epsilon())
00194 posQ *= distRemain / moveSize;
00195
00196
00197 if (oriErrMagnitude*oriPri < OTOL) {
00198
00199
00200 fmat::fmatReal moveCos = fmat::dotProduct(pErr/pErrMagnitude, move/moveSize);
00201 if (moveCos < QTOL) {
00202
00203 return RANGE;
00204 }
00205
00206
00207 fmat::fmatReal posqsum=0;
00208 for(unsigned int i=0; i<posQ.size(); ++i)
00209 posqsum+=posQ[i]*posQ[i];
00210 if (std::sqrt(posqsum) < std::numeric_limits<float>::epsilon()) {
00211
00212 return RANGE;
00213 }
00214 }
00215
00216
00217 posQ*=posPri;
00218 q += posQ;
00219 }
00220
00221
00222 if (oriPri>0 && angRemain>0) {
00223
00224 std::valarray<fmat::fmatReal> oriQ(Jm.size());
00225 fmat::Column<3> oriErrV = oriErr.axis();
00226
00227 for(unsigned int i=0; i<oriQ.size(); ++i) {
00228 oriQ[i] = fmat::dotProduct(fmat::SubVector<3>(Jm[i],3),oriErrV);
00229
00230 }
00231
00232
00233 fmat::fmatReal rotSize=0;
00234 for(unsigned int i=0; i<oriQ.size(); ++i)
00235 rotSize+=oriQ[i]*oriQ[i];
00236 rotSize=std::sqrt(rotSize);
00237
00238
00239
00240 if (rotSize > std::numeric_limits<float>::epsilon())
00241 oriQ *= oriDist * angRemain / rotSize;
00242
00243
00244 oriQ*=oriPri;
00245 q += oriQ;
00246 }
00247
00248
00249 size_t prevSize=mobile.size();
00250 KinematicJoint * ancestor=&j;
00251 for(int i=mobile.size()-1; i>=0; --i) {
00252 while(ancestor!=NULL && ancestor->getDepth()>static_cast<decltype(ancestor->getDepth())>(mobile[i]))
00253 ancestor=ancestor->getParent();
00254 q[i]+=ancestor->getQ();
00255 if (!ancestor->validQ(q[i])) {
00256
00257
00258 ancestor->tryQ(q[i]);
00259 std::vector<int>::iterator it=mobile.begin();
00260 std::advance(it,i);
00261 mobile.erase(it);
00262 }
00263 }
00264 if (mobile.size()<prevSize) {
00265
00266 fmat::Column<3> newpEffBase(j.getFullT()*pEff);
00267 distRemain -= (newpEffBase - pEffBase).norm();
00268
00269 fmat::Quaternion newoEffBase = j.getWorldQuaternion()*oriEff;
00270 fmat::Quaternion angMoved = crossProduct(newoEffBase, oEffBase);
00271
00272 angRemain -= std::abs(angMoved.angle());
00273
00274
00275 if (angRemain<=0 && distRemain<=0)
00276 return PROGRESS;
00277 if (distRemain<std::numeric_limits<float>::epsilon())
00278 distRemain=std::numeric_limits<float>::epsilon();
00279 if (angRemain<std::numeric_limits<float>::epsilon())
00280 angRemain=std::numeric_limits<float>::epsilon();
00281 } else {
00282
00283 ancestor=&j;
00284 for(int i=mobile.size()-1; i>=0; --i) {
00285 while(ancestor!=NULL && ancestor->getDepth()>static_cast<decltype(ancestor->getDepth())>(mobile[i]))
00286 ancestor=ancestor->getParent();
00287 ancestor->setQ(q[i]);
00288 }
00289 return (result==LIMITS) ? PROGRESS : result;
00290 }
00291 }
00292 return result;
00293 }
00294
00295
00296
00297
00298
00299 bool IKGradientSolver::solve(const Point& pEff, const Rotation& oriEff,
00300 KinematicJoint& j, const Position& pTgt, float posPri,
00301 const Orientation& oriTgt, float oriPri, unsigned int root) const {
00302 if (posPri==0 && oriPri==0) {
00303 return true;
00304 }
00305
00306 posPri=std::max(0.f,posPri);
00307 oriPri=std::max(0.f,oriPri);
00308
00309 KinematicJoint* firstMobileJoint = NULL;
00310 for(KinematicJoint *ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent()) {
00311 if (ancestor->isMobile()) {
00312 firstMobileJoint = ancestor;
00313 }
00314 if (ancestor->outputOffset == root) {
00315 break;
00316 }
00317 }
00318 if (firstMobileJoint==NULL) {
00319 return false;
00320 }
00321
00322
00323 fmat::Transform Tj = j.getT(*firstMobileJoint);
00324 fmat::Column<3> pEffFMJ(Tj*pEff);
00325 fmat::fmatReal pDist = pEffFMJ.norm()/10;
00326
00327
00328 fmat::fmatReal oriDist = 1;
00329
00330 std::valarray<fmat::fmatReal> q(j.getDepth()+1);
00331 for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent()) {
00332 q[ancestor->getDepth()] = ancestor->getQ();
00333 }
00334
00335 std::valarray<fmat::fmatReal> dq, ndq(j.getDepth()+1);
00336 unsigned int iterCnt=0, overCnt=0;
00337 bool solved=false;
00338 while(iterCnt<MAX_ITER) {
00339 IKSolver::StepResult_t res = step(pEff,oriEff,j,pTgt,pDist,posPri,oriTgt,oriDist,oriPri,false, root);
00340 if (res==SUCCESS) {
00341 solved=true;
00342 break;
00343 }
00344 if (res!=PROGRESS) {
00345 std::cout << "Result " << res << std::endl;
00346 break;
00347 }
00348 for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent()) {
00349 ndq[ancestor->getDepth()] = ancestor->getQ() - q[ancestor->getDepth()];
00350 q[ancestor->getDepth()] = ancestor->getQ();
00351 }
00352 fmat::fmatReal qa=1;
00353 if (dq.size()==0) {
00354 dq.resize(j.getDepth()+1);
00355 dq=ndq;
00356 } else {
00357 qa = (dq*ndq).sum();
00358 std::swap(dq,ndq);
00359 }
00360
00361 if ( std::max(std::abs(dq.max()), std::abs(dq.min())) < QTOL ) {
00362 std::cout << "Converged" << std::endl;
00363 break;
00364 }
00365 if (qa<0) {
00366
00367
00368
00369 pDist *= 0.75f;
00370 oriDist *= 0.75f;
00371 ++overCnt;
00372 }
00373
00374
00375
00376
00377 ++iterCnt;
00378 }
00379
00380
00381 return solved;
00382 }
00383
00384 IKSolver::StepResult_t IKGradientSolver::step(const Point& pEff, const Rotation& oriEff,
00385 KinematicJoint& j, const Position& pTgt, float pDist, float posPri,
00386 const Orientation& oriTgt, float oriDist, float oriPri,
00387 bool successInReach, unsigned int root) const {
00388 if ((posPri<=0 && oriPri<=0) || (pDist<=0 && oriDist<=0)) {
00389 return SUCCESS;
00390 }
00391
00392 posPri=std::max(0.f,posPri);
00393 oriPri=std::max(0.f,oriPri);
00394 pDist=std::max(0.f,pDist);
00395 oriDist=std::max(0.f,oriDist);
00396
00397 const float totPri = posPri+oriPri;
00398 posPri/=totPri;
00399 oriPri/=totPri;
00400
00401
00402 StepResult_t result = LIMITS;
00403
00404
00405 std::vector<int> mobile;
00406 mobile.reserve(j.getDepth()+1);
00407 for(KinematicJoint * ancestor=&j; ancestor!=NULL; ancestor=ancestor->getParent()) {
00408 if (ancestor->isMobile()) {
00409 mobile.push_back(ancestor->getDepth());
00410 }
00411 if (ancestor->outputOffset == root) {
00412 break;
00413 }
00414
00415
00416 }
00417 std::reverse(mobile.begin(),mobile.end());
00418
00419
00420 fmat::fmatReal distRemain = pDist;
00421 fmat::fmatReal angRemain = oriDist;
00422
00423 while(mobile.size()>0) {
00424 fmat::Transform Tj = j.getFullT();
00425 fmat::Column<3> pEffBase(Tj*pEff);
00426 fmat::Quaternion qj = j.getWorldQuaternion();
00427 Rotation oEffBase = qj*oriEff;
00428
00429 fmat::Column<3> pErr;
00430 pTgt.computeErrorGradient(pEffBase,oEffBase,pErr);
00431
00432 fmat::Quaternion oriErr;
00433 oriTgt.computeErrorGradient(pEffBase,oEffBase,oriErr);
00434 oriErr.normalize();
00435
00436 fmat::fmatReal pErrMagnitude = pErr.norm();
00437 fmat::fmatReal oriErrMagnitude = std::abs(oriErr.angle());
00438
00439
00440 if (pErrMagnitude/distRemain*posPri < .001 && oriErrMagnitude/angRemain*oriPri < .001) {
00441
00442 return SUCCESS;
00443 }
00444 if (pErrMagnitude*posPri < PTOL && oriErrMagnitude*oriPri < OTOL) {
00445
00446 return SUCCESS;
00447 }
00448 if (pErrMagnitude<distRemain && oriErrMagnitude<angRemain) {
00449 distRemain=pErrMagnitude;
00450 angRemain=oriErrMagnitude;
00451 if (successInReach) {
00452 result = SUCCESS;
00453 }
00454 } else if (pErrMagnitude<distRemain) {
00455 distRemain=pErrMagnitude;
00456 if (oriPri==0 && successInReach) {
00457 result = SUCCESS;
00458 }
00459 } else if (oriErrMagnitude<angRemain) {
00460 angRemain=oriErrMagnitude;
00461 if (posPri==0 && successInReach) {
00462 result = SUCCESS;
00463 }
00464 }
00465
00466 std::vector<fmat::Column<6> > J;
00467 j.getFullJacobian(pEffBase,J);
00468
00469 std::vector<fmat::Column<6> > Jm(mobile.size());
00470 for(size_t i=0; i<mobile.size(); ++i)
00471 Jm[i] = J[mobile[i]];
00472
00473
00474 std::valarray<fmat::fmatReal> q(mobile.size());
00475
00476 if (posPri>0 && distRemain>0) {
00477
00478
00479
00480
00481
00482
00483
00484 std::valarray<fmat::fmatReal> posQ(Jm.size());
00485 for(unsigned int i=0; i<posQ.size(); ++i) {
00486 posQ[i]=fmat::dotProduct(fmat::SubVector<3>(Jm[i]),pErr);
00487 }
00488
00489
00490
00491
00492 fmat::Column<3> move;
00493 for(unsigned int i=0; i<posQ.size(); ++i)
00494 move+=fmat::SubVector<3>(Jm[i])*posQ[i];
00495
00496 fmat::fmatReal moveSize = move.norm();
00497 if (moveSize > std::numeric_limits<float>::epsilon())
00498 posQ *= distRemain / moveSize;
00499
00500
00501 if (oriErrMagnitude*oriPri < OTOL) {
00502
00503
00504 fmat::fmatReal moveCos = fmat::dotProduct(pErr/pErrMagnitude, move/moveSize);
00505 if (moveCos < QTOL) {
00506
00507 return RANGE;
00508 }
00509
00510
00511 fmat::fmatReal posqsum=0;
00512 for(unsigned int i=0; i<posQ.size(); ++i)
00513 posqsum+=posQ[i]*posQ[i];
00514 if (std::sqrt(posqsum) < std::numeric_limits<float>::epsilon()) {
00515
00516 return RANGE;
00517 }
00518 }
00519
00520
00521 posQ*=posPri;
00522 q += posQ;
00523 }
00524
00525 if (oriPri>0 && angRemain>0) {
00526
00527 std::valarray<fmat::fmatReal> oriQ(Jm.size());
00528 fmat::Column<3> oriErrV = oriErr.axis();
00529
00530 for(unsigned int i=0; i<oriQ.size(); ++i) {
00531 oriQ[i] = fmat::dotProduct(fmat::SubVector<3>(Jm[i],3),oriErrV);
00532
00533 }
00534
00535
00536 fmat::fmatReal rotSize=0;
00537 for(unsigned int i=0; i<oriQ.size(); ++i)
00538 rotSize+=oriQ[i]*oriQ[i];
00539 rotSize=std::sqrt(rotSize);
00540
00541
00542
00543 if (rotSize > std::numeric_limits<float>::epsilon())
00544 oriQ *= oriDist * angRemain / rotSize;
00545
00546
00547 oriQ*=oriPri;
00548 q += oriQ;
00549 }
00550
00551
00552 size_t prevSize=mobile.size();
00553 KinematicJoint * ancestor=&j;
00554 for(int i=mobile.size()-1; i>=0; --i) {
00555 while(ancestor!=NULL && ancestor->getDepth()>static_cast<decltype(ancestor->getDepth())>(mobile[i]))
00556 ancestor=ancestor->getParent();
00557 q[i]+=ancestor->getQ();
00558 if (!ancestor->validQ(q[i])) {
00559
00560
00561 ancestor->tryQ(q[i]);
00562 std::vector<int>::iterator it=mobile.begin();
00563 std::advance(it,i);
00564 mobile.erase(it);
00565 }
00566 }
00567 if (mobile.size()<prevSize) {
00568
00569 fmat::Column<3> newpEffBase(j.getFullT()*pEff);
00570 distRemain -= (newpEffBase - pEffBase).norm();
00571
00572 fmat::Quaternion newoEffBase = j.getWorldQuaternion()*oriEff;
00573 fmat::Quaternion angMoved = crossProduct(newoEffBase, oEffBase);
00574
00575 angRemain -= std::abs(angMoved.angle());
00576
00577
00578 if (angRemain<=0 && distRemain<=0)
00579 return PROGRESS;
00580 if (distRemain<std::numeric_limits<float>::epsilon())
00581 distRemain=std::numeric_limits<float>::epsilon();
00582 if (angRemain<std::numeric_limits<float>::epsilon())
00583 angRemain=std::numeric_limits<float>::epsilon();
00584 } else {
00585
00586 ancestor=&j;
00587 for(int i=mobile.size()-1; i>=0; --i) {
00588 while(ancestor!=NULL && ancestor->getDepth()>static_cast<decltype(ancestor->getDepth())>(mobile[i]))
00589 ancestor=ancestor->getParent();
00590 ancestor->setQ(q[i]);
00591 }
00592 return (result==LIMITS) ? PROGRESS : result;
00593 }
00594 }
00595 return result;
00596 }
00597
00598
00599
00600
00601