Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

IKGradientSolver.cc

Go to the documentation of this file.
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   // choose pDist by heuristic: proportional to distance between effector and base frame
00028   fmat::Transform Tj = j.getT(*firstMobileJoint);
00029   fmat::Column<3> pEffFMJ(Tj*pEff);
00030   fmat::fmatReal pDist = pEffFMJ.norm()/10;
00031   
00032   // rotation is scale invariant, just start with 1
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     // only need abs on min: if negative, it will be more so
00064     if ( std::max(std::abs(dq.max()), std::abs(dq.min())) < QTOL ) {
00065       std::cout << "Converged" << std::endl;
00066       break; // not actually making progress... probably out of range
00067     }
00068     if (qa<0) {
00069       // this step reversed direction of previous
00070       // step, probably overshot target, so cut
00071       // distance in half for next try
00072       pDist *= 0.75f;
00073       oriDist *= 0.75f;
00074       ++overCnt;
00075     }/* else {
00076      // more gradual convergence
00077      pDist*=.9;
00078      oriDist*=.9;
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   // if we run out of joints to move, then we'll return joint-limits-hit
00104   StepResult_t result = LIMITS;
00105   
00106   // only work with joints for which qmin≠qmax (i.e. mobile joints)
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     //cout << "Joint " << ancestor->getDepth() << " @ " << ancestor->getQ() << endl;
00113     //cout << ancestor->getFullT() << endl;
00114   }
00115   std::reverse(mobile.begin(),mobile.end());
00116   
00117   // as we hit joint limits, we'll resolve with the remaining step distance:
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(); //cout << "WorldQ: " << qj;
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(); // importantly, this flips axis as needed to ensure positive angle, which is assumed later
00133     
00134     fmat::fmatReal pErrMagnitude = pErr.norm();
00135     fmat::fmatReal oriErrMagnitude = std::abs(oriErr.angle());
00136     // cout << "\nCurrent: " << oEffBase << ' ' << oEffBase.angle() << ' ' << oEffBase.axis() << endl;
00137     // cout << "Remain: " << oriErr << ' ' << oriErrMagnitude << ' ' << angRemain << ' ' << oriErrMagnitude/angRemain << endl;
00138     if (pErrMagnitude/distRemain*posPri < .001 && oriErrMagnitude/angRemain*oriPri < .001) {
00139       //cout << "Nailed it!" << endl;
00140       return SUCCESS;
00141     }
00142     if (pErrMagnitude*posPri < PTOL && oriErrMagnitude*oriPri < OTOL) {
00143       //cout << "Close enough! " << pErrMagnitude << ' ' << posPri << ' ' << PTOL << endl;
00144       return SUCCESS;
00145     }
00146     if (pErrMagnitude<distRemain && oriErrMagnitude<angRemain) {
00147       distRemain=pErrMagnitude;
00148       angRemain=oriErrMagnitude;
00149       if (successInReach)
00150        result = SUCCESS; // we'll still take a step, but we're basically there
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     //cout << "Jacobian\n"; for(unsigned int i=0; i<J.size(); ++i) std::cout << J[i] << '\n';
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     //cout << "Jacobian mobile only\n"; for(unsigned int i=0; i<Jm.size(); ++i) std::cout << Jm[i] << '\n';
00168     
00169     std::valarray<fmat::fmatReal> q(mobile.size());
00170     
00171     // Pos start
00172     if (posPri>0 && distRemain>0) {
00173       // need to find pseudo-inverse of J:
00174       
00175       // use transposed jacobian as inverse (see "virtual work" -- there is some rationale why this works!)
00176       // basically we're simulating a force "pulling" on the effector
00177       
00178       // find candidate q's for mobile joints regarding positional error
00179       // multiplying the Jm transpose by the error, aka take dot product of each column with error
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       // when joints are aligned, each tries to take the full step, so we overshoot
00186       // so normalize q's by the size of the step they would produce (according to jacobian anyway...),
00187       // and then scale by the size of the step we wanted to take
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       // if we're only moving to solve positional error, see if we're at a local minimum (not making progress)
00197       if (oriErrMagnitude*oriPri < OTOL) {
00198         //std::cout << "err " << pErr << ' ' << pErrMagnitude << " move " << move << ' ' << moveSize << std::endl;
00199         // if we're moving perpendicular to the target, we're done... (avoid twitching)
00200         fmat::fmatReal moveCos = fmat::dotProduct(pErr/pErrMagnitude, move/moveSize);
00201         if (moveCos < QTOL) {
00202           //std::cout << "Move cos " << moveCos << std::endl;
00203           return RANGE;
00204          }
00205         //cout << "progress angle: " << moveCos << ' ' << std::acos(moveCos) << endl;
00206         //cout << "progress percent: " << moveCos*distRemain / pErrMagnitude << endl;
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           //std::cout << "Move cos " << moveCos << " posqsum " << posqsum << std::endl;
00212           return RANGE;
00213         }
00214       }
00215       
00216       // combine weighted factors:
00217       posQ*=posPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
00218       q += posQ;
00219     }
00220     
00221     // Ori start
00222     if (oriPri>0 && angRemain>0) {
00223       // orientation correction movment:
00224       std::valarray<fmat::fmatReal> oriQ(Jm.size());
00225       fmat::Column<3> oriErrV = oriErr.axis();
00226       // std::cerr << "oriErr " << oriErr << "  " << oriErr.angle() << " " << oriErrV << std::endl;
00227       for(unsigned int i=0; i<oriQ.size(); ++i) {
00228         oriQ[i] = fmat::dotProduct(fmat::SubVector<3>(Jm[i],3),oriErrV);
00229         //std::cout << oriQ[i] << ' ';
00230       }
00231       
00232       // normalization for desired magnitude of orientation movement:
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       /*for(unsigned int i=0; i<oriQ.size(); ++i)
00238        rotSize+=std::abs(oriQ[i]);*/
00239       // std::cout << " * " << oriDist << " * " << angRemain << " / " << rotSize << std::endl;
00240       if (rotSize > std::numeric_limits<float>::epsilon())
00241          oriQ *= oriDist * angRemain / rotSize;
00242       
00243       // combine weighted factors:
00244       oriQ*=oriPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
00245       q += oriQ; 
00246     }
00247     
00248     // check for range limits:
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         // if a joint hits the limit, fix it there (then we'll loop over again with the remaining joints in a bit)
00257         // cout << "mobile " << i << " (" << mobile[i] << ") is past limit" << endl;
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       // some joint(s) hit their limit, find distance remaining and we'll try again
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       // std::cout << "REMAIN: " << angMoved.angle() << " of " << angRemain << ", dist " << distRemain << std::endl;
00272       angRemain -= std::abs(angMoved.angle());
00273       // std::cout << "NOW " << angRemain << std::endl;
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       // no joints hit the limit, so we're done with the step!
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 /*! @file
00296  * @brief Implements IKGradientSolver, which performs gradient descent on the joints to find a solution
00297  * @author Ethan Tira-Thompson (ejt) (Creator)
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   // choose pDist by heuristic: proportional to distance between effector and base frame
00323   fmat::Transform Tj = j.getT(*firstMobileJoint);
00324   fmat::Column<3> pEffFMJ(Tj*pEff);
00325   fmat::fmatReal pDist = pEffFMJ.norm()/10;
00326   
00327   // rotation is scale invariant, just start with 1
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     // only need abs on min: if negative, it will be more so
00361     if ( std::max(std::abs(dq.max()), std::abs(dq.min())) < QTOL ) {
00362       std::cout << "Converged" << std::endl;
00363       break; // not actually making progress... probably out of range
00364     }
00365     if (qa<0) {
00366       // this step reversed direction of previous
00367       // step, probably overshot target, so cut
00368       // distance in half for next try
00369       pDist *= 0.75f;
00370       oriDist *= 0.75f;
00371       ++overCnt;
00372     }/* else {
00373      // more gradual convergence
00374      pDist*=.9;
00375      oriDist*=.9;
00376      }*/
00377     ++iterCnt;
00378   }
00379   //cout << "Iterations: " << iterCnt << "  Overshoots: " << overCnt 
00380   //     << " solved=" << solved << endl;
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   // if we run out of joints to move, then we'll return joint-limits-hit
00402   StepResult_t result = LIMITS;
00403   
00404   // only work with joints for which qmin≠qmax (i.e. mobile joints)
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     //cout << "Joint " << ancestor->getDepth() << " @ " << ancestor->getQ() << endl;
00415     //cout << ancestor->getFullT() << endl;
00416   }
00417   std::reverse(mobile.begin(),mobile.end());
00418   
00419   // as we hit joint limits, we'll resolve with the remaining step distance:
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(); //cout << "WorldQ: " << qj;
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(); // importantly, this flips axis as needed to ensure positive angle, which is assumed later
00435     
00436     fmat::fmatReal pErrMagnitude = pErr.norm();
00437     fmat::fmatReal oriErrMagnitude = std::abs(oriErr.angle());
00438     // cout << "\nCurrent: " << oEffBase << ' ' << oEffBase.angle() << ' ' << oEffBase.axis() << endl;
00439     // cout << "Remain: " << oriErr << ' ' << oriErrMagnitude << ' ' << angRemain << ' ' << oriErrMagnitude/angRemain << endl;
00440     if (pErrMagnitude/distRemain*posPri < .001 && oriErrMagnitude/angRemain*oriPri < .001) {
00441       //cout << "Nailed it!" << endl;
00442       return SUCCESS;
00443     }
00444     if (pErrMagnitude*posPri < PTOL && oriErrMagnitude*oriPri < OTOL) {
00445       //cout << "Close enough! " << pErrMagnitude << ' ' << posPri << ' ' << PTOL << endl;
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     //cout << "Jacobian\n"; for(unsigned int i=0; i<J.size(); ++i) std::cout << J[i] << '\n';
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     //cout << "Jacobian mobile only\n"; for(unsigned int i=0; i<Jm.size(); ++i) std::cout << Jm[i] << '\n';
00473     
00474     std::valarray<fmat::fmatReal> q(mobile.size());
00475     
00476     if (posPri>0 && distRemain>0) {
00477       // need to find pseudo-inverse of J:
00478       
00479       // use transposed jacobian as inverse (see "virtual work" -- there is some rationale why this works!)
00480       // basically we're simulating a force "pulling" on the effector
00481       
00482       // find candidate q's for mobile joints regarding positional error
00483       // multiplying the Jm transpose by the error, aka take dot product of each column with error
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       // when joints are aligned, each tries to take the full step, so we overshoot
00490       // so normalize q's by the size of the step they would produce (according to jacobian anyway...),
00491       // and then scale by the size of the step we wanted to take
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       // if we're only moving to solve positional error, see if we're at a local minimum (not making progress)
00501       if (oriErrMagnitude*oriPri < OTOL) {
00502         //std::cout << "err " << pErr << ' ' << pErrMagnitude << " move " << move << ' ' << moveSize << std::endl;
00503         // if we're moving perpendicular to the target, we're done... (avoid twitching)
00504         fmat::fmatReal moveCos = fmat::dotProduct(pErr/pErrMagnitude, move/moveSize);
00505         if (moveCos < QTOL) {
00506           //std::cout << "Move cos " << moveCos << std::endl;
00507           return RANGE;
00508         }
00509         //cout << "progress angle: " << moveCos << ' ' << std::acos(moveCos) << endl;
00510         //cout << "progress percent: " << moveCos*distRemain / pErrMagnitude << endl;
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           //std::cout << "Move cos " << moveCos << " posqsum " << posqsum << std::endl;
00516           return RANGE;
00517         }
00518       }
00519       
00520       // combine weighted factors:
00521       posQ*=posPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
00522       q += posQ;
00523     }
00524     
00525     if (oriPri>0 && angRemain>0) {
00526       // orientation correction movment:
00527       std::valarray<fmat::fmatReal> oriQ(Jm.size());
00528       fmat::Column<3> oriErrV = oriErr.axis();
00529       // std::cerr << "oriErr " << oriErr << "  " << oriErr.angle() << " " << oriErrV << std::endl;
00530       for(unsigned int i=0; i<oriQ.size(); ++i) {
00531         oriQ[i] = fmat::dotProduct(fmat::SubVector<3>(Jm[i],3),oriErrV);
00532         //std::cout << oriQ[i] << ' ';
00533       }
00534       
00535       // normalization for desired magnitude of orientation movement:
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       /*for(unsigned int i=0; i<oriQ.size(); ++i)
00541        rotSize+=std::abs(oriQ[i]);*/
00542       // std::cout << " * " << oriDist << " * " << angRemain << " / " << rotSize << std::endl;
00543       if (rotSize > std::numeric_limits<float>::epsilon())
00544        oriQ *= oriDist * angRemain / rotSize;
00545       
00546       // combine weighted factors:
00547       oriQ*=oriPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
00548       q += oriQ;
00549     }
00550     
00551     // check for range limits:
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         // if a joint hits the limit, fix it there (then we'll loop over again with the remaining joints in a bit)
00560         // cout << "mobile " << i << " (" << mobile[i] << ") is past limit" << endl;
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       // some joint(s) hit their limit, find distance remaining and we'll try again
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       // std::cout << "REMAIN: " << angMoved.angle() << " of " << angRemain << ", dist " << distRemain << std::endl;
00575       angRemain -= std::abs(angMoved.angle());
00576       // std::cout << "NOW " << angRemain << std::endl;
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       // no joints hit the limit, so we're done with the step!
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 /*! @file
00599  * @brief Implements IKGradientSolver, which performs gradient descent on the joints to find a solution
00600  * @author Ethan Tira-Thompson (ejt) (Creator)
00601  */

Tekkotsu v5.1CVS
Generated Mon May 9 04:58:42 2016 by Doxygen 1.6.3