Kinematyka odwrotna z OpenGL/Eigen3: niestabilna jakobiańska pseudoinwazyjna

Question

Próbuję zaimplementować prosty test odwrotnej kinematyki przy użyciu metody OpenGL, Eigen3 i "Jacobian pseudoinverse".

System działa dobrze przy użyciu algorytmu "Jakobian transpose", jednak gdy tylko spróbuję użyć "pseudoinwersji", stawy stają się niestabilne i zaczynają szarpać się (w końcu zamierają całkowicie - chyba że używam "awaryjnego" przeniesienia "Jakobian"). Zbadałem problem i okazało się, że w niektórych przypadkach Jacobian.inverse() * Jakobian ma zero determinanty i nie można go odwrócić. Jednak widziałem inne dema w Internecie (YouTube), które twierdzą, że używają tej samej metody i nie wydają się mieć tego problemu. Więc nie jestem pewien, gdzie jest przyczyna problemu. Kod jest załączony poniżej:

* .h:

struct Ik{ 
    float targetAngle; 
    float ikLength; 
    VectorXf angles; 
    Vector3f root, target; 
    Vector3f jointPos(int ikIndex); 
    size_t size() const; 
    Vector3f getEndPos(int index, const VectorXf& vec); 
    void resize(size_t size); 
    void update(float t); 
    void render(); 
    Ik(): targetAngle(0), ikLength(10){ 
    } 
}; 

* .cpp:

size_t Ik::size() const{ 
    return angles.rows(); 
} 

Vector3f Ik::getEndPos(int index, const VectorXf& vec){ 
    Vector3f pos(0, 0, 0); 
    while(true){ 
     Eigen::Affine3f t; 
     float radAngle = pi*vec[index]/180.0f; 
     t = Eigen::AngleAxisf(radAngle, Vector3f(-1, 0, 0)) 
      * Eigen::Translation3f(Vector3f(0, 0, ikLength)); 
     pos = t * pos; 

     if (index == 0) 
      break; 
     index--; 
    } 
    return pos; 
} 

void Ik::resize(size_t size){ 
    angles.resize(size); 
    angles.setZero(); 
} 

void drawMarker(Vector3f p){ 
    glBegin(GL_LINES); 
    glVertex3f(p[0]-1, p[1], p[2]); 
    glVertex3f(p[0]+1, p[1], p[2]); 
    glVertex3f(p[0], p[1]-1, p[2]); 
    glVertex3f(p[0], p[1]+1, p[2]); 
    glVertex3f(p[0], p[1], p[2]-1); 
    glVertex3f(p[0], p[1], p[2]+1); 
    glEnd(); 
} 

void drawIkArm(float length){ 
    glBegin(GL_LINES); 
    float f = 0.25f; 
    glVertex3f(0, 0, length); 
    glVertex3f(-f, -f, 0); 
    glVertex3f(0, 0, length); 
    glVertex3f(f, -f, 0); 
    glVertex3f(0, 0, length); 
    glVertex3f(f, f, 0); 
    glVertex3f(0, 0, length); 
    glVertex3f(-f, f, 0); 
    glEnd(); 
    glBegin(GL_LINE_LOOP); 
    glVertex3f(f, f, 0); 
    glVertex3f(-f, f, 0); 
    glVertex3f(-f, -f, 0); 
    glVertex3f(f, -f, 0); 
    glEnd(); 
} 

void Ik::update(float t){ 
    targetAngle += t * pi*2.0f/10.0f; 
    while (t > pi*2.0f) 
     t -= pi*2.0f; 
    target << 0, 8 + 3*sinf(targetAngle), cosf(targetAngle)*4.0f+5.0f; 

    Vector3f tmpTarget = target; 
    Vector3f targetDiff = tmpTarget - root; 
    float l = targetDiff.norm(); 
    float maxLen = ikLength*(float)angles.size() - 0.01f; 
    if (l > maxLen){ 
     targetDiff *= maxLen/l; 
     l = targetDiff.norm(); 
     tmpTarget = root + targetDiff; 
    } 

    Vector3f endPos = getEndPos(size()-1, angles); 
    Vector3f diff = tmpTarget - endPos; 


    float maxAngle = 360.0f/(float)angles.size(); 

    for(int loop = 0; loop < 1; loop++){ 
     MatrixXf jacobian(diff.rows(), angles.rows()); 
     jacobian.setZero(); 
     float step = 1.0f; 
     for (int i = 0; i < angles.size(); i++){ 
      Vector3f curRoot = root; 
      if (i) 
       curRoot = getEndPos(i-1, angles); 
      Vector3f axis(1, 0, 0); 
      Vector3f n = endPos - curRoot; 
      float l = n.norm(); 
      if (l) 
       n /= l; 
      n = n.cross(axis); 
      if (l) 
       n *= l*step*pi/180.0f; 
      //std::cout << n << "\n"; 

      for (int j = 0; j < 3; j++) 
       jacobian(j, i) = n[j]; 
     } 

     std::cout << jacobian << std::endl; 
     MatrixXf jjt = jacobian.transpose()*jacobian; 
     //std::cout << jjt << std::endl; 
     float d = jjt.determinant(); 
     MatrixXf invJ; 
     float scale = 0.1f; 
     if (!d /*|| true*/){ 
      invJ = jacobian.transpose(); 
      scale = 5.0f; 
      std::cout << "fallback to jacobian transpose!\n"; 
     } 
     else{ 
      invJ = jjt.inverse()*jacobian.transpose(); 
      std::cout << "jacobian pseudo-inverse!\n"; 
     } 
     //std::cout << invJ << std::endl; 

     VectorXf add = invJ*diff*step*scale; 
     //std::cout << add << std::endl; 
     float maxSpeed = 15.0f; 
     for (int i = 0; i < add.size(); i++){ 
      float& cur = add[i]; 
      cur = std::max(-maxSpeed, std::min(maxSpeed, cur)); 
     } 
     angles += add; 
     for (int i = 0; i < angles.size(); i++){ 
      float& cur = angles[i]; 
      if (i) 
       cur = std::max(-maxAngle, std::min(maxAngle, cur)); 
     } 
    } 
} 

void Ik::render(){ 
    glPushMatrix(); 
    glTranslatef(root[0], root[1], root[2]); 
    for (int i = 0; i < angles.size(); i++){ 
     glRotatef(angles[i], -1, 0, 0); 
     drawIkArm(ikLength); 
     glTranslatef(0, 0, ikLength); 
    } 
    glPopMatrix(); 
    drawMarker(target); 
    for (int i = 0; i < angles.size(); i++) 
     drawMarker(getEndPos(i, angles)); 
} 

Answer 1

Brzmi jak system jest zbyt sztywny.

1. Spróbuj użyć metod Eigen SVD: patrz http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.html. To jest bardziej intensywne obliczeniowo, ale może też bezpieczniejsze. Jeśli rozwiązujesz problem aX = b, najlepiej użyć metod dedykowanych odwróceniu macierzy. (a jest twoim Jacobianem i X jest tym, czego chcesz).
2. Na koniec spróbuj oszukać kondycję na macierzy, mnożąc przekątną przez współczynnik taki jak 1,0001. Nie zmieni to zbytnio rozwiązania (szczególnie w przypadku gry), ale umożliwi lepsze rozwiązywanie.
3. Jestem ciekawy, dlaczego zdecydowałeś się nie używać iteracyjnego schematu czasowego, takiego jak RK4.

edytuj: Nie czytałem dużo twojego obszernego postu, więc usunąłem odniesienie do sprężyn. Sądzę, że w twoim przypadku elementy miałyby jakąś mechaniczną interakcję.

Answer 2

Coś takiego powinno działać.

VectorXf solveViaSVD(const MatrixXf & jacobian,const VectorXf & diff) { 
    Eigen::JacobiSVD<MatrixXf> svd (jacobian,ComputeThinU | ComputeThinV); 
    return svd.solve(diff); 
} 

Problemem jest, jak pan powiedział, że nie sposób obliczyć pseudo-odwrotność kiedy (J*J') jest w liczbie pojedynczej.

Wikipedia mówi: "[pseudoinwersja] jest utworzona przez zastąpienie każdego niezerowego2 przekątnej przez jego odwrotność". Obliczanie pinv(A) jako inv(A'*A)*A' kończy się niepowodzeniem w punkcie "niezerowym".