IKGradientSolver.cc

Go to the documentation of this file.

#include "IKGradientSolver.h"
#include "Shared/fmat.h"
#include <cmath>
#include <valarray>

using namespace std;

const std::string IKGradientSolver::autoRegisterIKGradientSolver = IKSolver::getRegistry().registerType<IKGradientSolver>("IKGradientSolver");
const std::string IKGradientSolver::autoRegisterDefaultIKSolver = IKSolver::getRegistry().registerType<IKGradientSolver>("");

bool IKGradientSolver::solve(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float posPri, const Orientation& oriTgt, float oriPri) const {
  if(posPri==0 && oriPri==0)
    return true;

  posPri=std::max(0.f,posPri);
  oriPri=std::max(0.f,oriPri);
  
  KinematicJoint* firstMobileJoint = NULL;
  for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent())
    if(ancestor->isMobile())
      firstMobileJoint = ancestor;
  if(firstMobileJoint==NULL)
    return false;
  
  // choose pDist by heuristic: proportional to distance between effector and base frame
  fmat::Transform Tj = j.getT(*firstMobileJoint);
  fmat::Column<3> pEffFMJ(Tj*pEff);
  fmat::fmatReal pDist = pEffFMJ.norm()/10;
  
  // rotation is scale invariant, just start with 1
  fmat::fmatReal oriDist = 1;
  
  std::valarray<fmat::fmatReal> q(j.getDepth()+1);
  for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent())
    q[ancestor->getDepth()] = ancestor->getQ();
  std::valarray<fmat::fmatReal> dq, ndq(j.getDepth()+1);
  unsigned int iterCnt=0, overCnt=0;
  bool solved=false;
  while(iterCnt<MAX_ITER) {
    IKSolver::StepResult_t res = step(pEff,oriEff,j,pTgt,pDist,posPri,oriTgt,oriDist,oriPri,false);
    if(res==SUCCESS) {
      solved=true;
      break;
    }
    if(res!=PROGRESS) {
      //std::cout << "Result " << res << std::endl;
      break;
    }
    for(KinematicJoint * ancestor = &j; ancestor!=NULL; ancestor = ancestor->getParent()) {
      ndq[ancestor->getDepth()] = ancestor->getQ() - q[ancestor->getDepth()];
      q[ancestor->getDepth()] = ancestor->getQ();
    }
    fmat::fmatReal qa=1;
    if(dq.size()==0) {
      dq.resize(j.getDepth()+1);
      dq=ndq;
    } else {
      qa = (dq*ndq).sum();
      std::swap(dq,ndq);
    }
    if(std::max(std::abs(dq.max()),std::abs(dq.min())) < QTOL) { // only need abs on min: if negative, it will be moreso
      //std::cout << "Converged" << std::endl;
      break; // not actually making progress... probably out of range
    }
    if(qa<0) {
      // this step reversed direction of previous step, probably overshot target, so cut distance in half for next try
      pDist *= .75f;
      oriDist *= .75f;
      ++overCnt;
    }/* else {
      // more gradual convergence
      pDist*=.9;
      oriDist*=.9;
    }*/
    ++iterCnt;
  }
  // cout << "Iterations: " << iterCnt << "  Overshoots: " << overCnt << endl;
  return solved;
}

IKSolver::StepResult_t IKGradientSolver::step(const Point& pEff, const Rotation& oriEff, KinematicJoint& j, const Position& pTgt, float pDist, float posPri, const Orientation& oriTgt, float oriDist, float oriPri, bool successInReach) const {
  if((posPri<=0 && oriPri<=0) || (pDist<=0 && oriDist<=0))
    return SUCCESS;
  
  posPri=std::max(0.f,posPri);
  oriPri=std::max(0.f,oriPri);
  pDist=std::max(0.f,pDist);
  oriDist=std::max(0.f,oriDist);
  
  const float totPri = posPri+oriPri;
  posPri/=totPri;
  oriPri/=totPri;
  
  // if we run out of joints to move, then we'll return joint-limits-hit
  StepResult_t result = LIMITS;
  
  // only work with joints for which qmin≠qmax (i.e. mobile joints)
  std::vector<int> mobile;
  mobile.reserve(j.getDepth()+1);
  for(KinematicJoint * ancestor=&j; ancestor!=NULL; ancestor=ancestor->getParent()) {
    if(ancestor->isMobile())
      mobile.push_back(ancestor->getDepth());
    //cout << "Joint " << ancestor->getDepth() << " @ " << ancestor->getQ() << endl;
    //cout << ancestor->getFullT() << endl;
  }
  std::reverse(mobile.begin(),mobile.end());
  
  // as we hit joint limits, we'll resolve with the remaining step distance:
  fmat::fmatReal distRemain = pDist;
  fmat::fmatReal angRemain = oriDist;
  
  while(mobile.size()>0) {
    
    fmat::Transform Tj = j.getFullT();
    fmat::Column<3> pEffBase(Tj*pEff);
    fmat::Quaternion qj = j.getWorldQuaternion(); //cout << "WorldQ: " << qj;
    Rotation oEffBase = qj*oriEff;
    
    fmat::Column<3> pErr;
    pTgt.computeErrorGradient(pEffBase,oEffBase,pErr);
    
    fmat::Quaternion oriErr;
    oriTgt.computeErrorGradient(pEffBase,oEffBase,oriErr);
    oriErr.normalize(); // importantly, this flips axis as needed to ensure positive angle, which is assumed later
    
    fmat::fmatReal pErrMagnitude = pErr.norm();
    fmat::fmatReal oriErrMagnitude = std::abs(oriErr.angle());
    // cout << "\nCurrent: " << oEffBase << ' ' << oEffBase.angle() << ' ' << oEffBase.axis() << endl;
    // cout << "Remain: " << oriErr << ' ' << oriErrMagnitude << ' ' << angRemain << ' ' << oriErrMagnitude/angRemain << endl;
    if(pErrMagnitude/distRemain*posPri < .001 && oriErrMagnitude/angRemain*oriPri < .001) {
      //cout << "Nailed it!" << endl;
      return SUCCESS;
    }
    if(pErrMagnitude*posPri < PTOL && oriErrMagnitude*oriPri < OTOL) {
      //cout << "Close enough! " << pErrMagnitude << ' ' << posPri << ' ' << PTOL << endl;
      return SUCCESS;
    }
    if(pErrMagnitude<distRemain && oriErrMagnitude<angRemain) {
      distRemain=pErrMagnitude;
      angRemain=oriErrMagnitude;
      if(successInReach)
        result = SUCCESS; // we'll still take a step, but we're basically there
    } else if(pErrMagnitude<distRemain) {
      distRemain=pErrMagnitude;
      if(oriPri==0 && successInReach)
        result = SUCCESS;
    } else if(oriErrMagnitude<angRemain) {
      angRemain=oriErrMagnitude;
      if(posPri==0 && successInReach)
        result = SUCCESS;
    }
    
    std::vector<fmat::Column<6> > J;
    j.getFullJacobian(pEffBase,J);
    //cout << "Jacobian\n"; for(unsigned int i=0; i<J.size(); ++i) std::cout << J[i] << '\n';
    std::vector<fmat::Column<6> > Jm(mobile.size());
    for(size_t i=0; i<mobile.size(); ++i)
      Jm[i] = J[mobile[i]];
    //cout << "Jacobian mobile only\n"; for(unsigned int i=0; i<Jm.size(); ++i) std::cout << Jm[i] << '\n';
    
    std::valarray<fmat::fmatReal> q(mobile.size());
    
    if(posPri>0 && distRemain>0) {
      // need to find pseudo-inverse of J:
      
      // use transposed jacobian as inverse (see "virtual work" -- there is some rationale why this works!)
      // basically we're simulating a force "pulling" on the effector
      
      // find candidate q's for mobile joints regarding positional error
      // multiplying the Jm transpose by the error, aka take dot product of each column with error
      std::valarray<fmat::fmatReal> posQ(Jm.size());
      for(unsigned int i=0; i<posQ.size(); ++i) {
        posQ[i]=fmat::dotProduct(fmat::SubVector<3>(Jm[i]),pErr);
      }
      
      // when joints are aligned, each tries to take the full step, so we overshoot
      // so normalize q's by the size of the step they would produce (according to jacobian anyway...),
      // and then scale by the size of the step we wanted to take
      fmat::Column<3> move;
      for(unsigned int i=0; i<posQ.size(); ++i)
        move+=fmat::SubVector<3>(Jm[i])*posQ[i];
      
      fmat::fmatReal moveSize = move.norm();
      if(moveSize > std::numeric_limits<float>::epsilon())
        posQ *= distRemain / moveSize;
      
      // if we're only moving to solve positional error, see if we're at a local minimum (not making progress)
      if(oriErrMagnitude*oriPri < OTOL) {
        //std::cout << "err " << pErr << ' ' << pErrMagnitude << " move " << move << ' ' << moveSize << std::endl;
        // if we're moving perpendicular to the target, we're done... (avoid twitching)
        fmat::fmatReal moveCos = fmat::dotProduct(pErr/pErrMagnitude, move/moveSize);
        if(moveCos < QTOL) {
          //std::cout << "Move cos " << moveCos << std::endl;
          return RANGE;
        }
        //cout << "progress angle: " << moveCos << ' ' << std::acos(moveCos) << endl;
        //cout << "progress percent: " << moveCos*distRemain / pErrMagnitude << endl;
        fmat::fmatReal posqsum=0;
        for(unsigned int i=0; i<posQ.size(); ++i)
          posqsum+=posQ[i]*posQ[i];
        if(std::sqrt(posqsum) < std::numeric_limits<float>::epsilon()) {
          //std::cout << "Move cos " << moveCos << " posqsum " << posqsum << std::endl;
          return RANGE;
        }
      }
      
      // combine weighted factors:
      posQ*=posPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
      q += posQ;
    }
    
    if(oriPri>0 && angRemain>0) {
      // orientation correction movment:
      std::valarray<fmat::fmatReal> oriQ(Jm.size());
      fmat::Column<3> oriErrV = oriErr.axis();
      // std::cerr << "oriErr " << oriErr << "  " << oriErr.angle() << " " << oriErrV << std::endl;
      for(unsigned int i=0; i<oriQ.size(); ++i) {
        oriQ[i] = fmat::dotProduct(fmat::SubVector<3>(Jm[i],3),oriErrV);
        //std::cout << oriQ[i] << ' ';
      }
      
      // normalization for desired magnitude of orientation movement:
      fmat::fmatReal rotSize=0;
      for(unsigned int i=0; i<oriQ.size(); ++i)
        rotSize+=oriQ[i]*oriQ[i];
      rotSize=std::sqrt(rotSize);
      /*for(unsigned int i=0; i<oriQ.size(); ++i)
        rotSize+=std::abs(oriQ[i]);*/
      // std::cout << " * " << oriDist << " * " << angRemain << " / " << rotSize << std::endl;
      if(rotSize > std::numeric_limits<float>::epsilon())
        oriQ *= oriDist * angRemain / rotSize;
      
      // combine weighted factors:
      oriQ*=oriPri; // posPri and oriPri are normalized at the top so we don't divide by (posPri+oriPri)
      q += oriQ; 
    }
    
    // check for range limits:
    size_t prevSize=mobile.size();
    KinematicJoint * ancestor=&j;
    for(int i=mobile.size()-1; i>=0; --i) {
      while(ancestor!=NULL && ancestor->getDepth()>static_cast<typeof(ancestor->getDepth())>(mobile[i]))
        ancestor=ancestor->getParent();
      q[i]+=ancestor->getQ();
      if(!ancestor->validQ(q[i])) {
        // if a joint hits the limit, fix it there (then we'll loop over again with the remaining joints in a bit)
        // cout << "mobile " << i << " (" << mobile[i] << ") is past limit" << endl;
        ancestor->tryQ(q[i]);
        std::vector<int>::iterator it=mobile.begin();
        std::advance(it,i);
        mobile.erase(it);
      }
    }
    if(mobile.size()<prevSize) {
      // some joint(s) hit their limit, find distance remaining and we'll try again
      fmat::Column<3> newpEffBase(j.getFullT()*pEff);
      distRemain -= (newpEffBase - pEffBase).norm();
      
      fmat::Quaternion newoEffBase = j.getWorldQuaternion()*oriEff;
      fmat::Quaternion angMoved = crossProduct(newoEffBase, oEffBase);
      // std::cout << "REMAIN: " << angMoved.angle() << " of " << angRemain << ", dist " << distRemain << std::endl;
      angRemain -= std::abs(angMoved.angle());
      // std::cout << "NOW " << angRemain << std::endl;

      if(angRemain<=0 && distRemain<=0)
        return PROGRESS;
      if(distRemain<std::numeric_limits<float>::epsilon())
        distRemain=std::numeric_limits<float>::epsilon();
      if(angRemain<std::numeric_limits<float>::epsilon())
        angRemain=std::numeric_limits<float>::epsilon();
    } else {
      // no joints hit the limit, so we're done with the step!
      ancestor=&j;
      for(int i=mobile.size()-1; i>=0; --i) {
        while(ancestor!=NULL && ancestor->getDepth()>static_cast<typeof(ancestor->getDepth())>(mobile[i]))
          ancestor=ancestor->getParent();
        ancestor->setQ(q[i]);
      }
      return (result==LIMITS) ? PROGRESS : result;
    }
  }
  return result;
}

/*! @file
 * @brief Implements IKGradientSolver, which performs gradient descent on the joints to find a solution
 * @author Ethan Tira-Thompson (ejt) (Creator)
 */