使用OpenGL/Eigen3的逆運動學：不穩定的雅可比僞逆

Question

我試圖使用OpenGL，Eigen3和"Jacobian pseudoinverse"方法實現簡單的逆運動學測試。但是，只要我嘗試使用「僞逆」，關節變得不穩定並開始四處亂跳（最終它們完全凍結 - 除非我使用「雅可比轉置」回退計算），但系統可以正常使用「雅可比轉置」算法。 ）。我調查了這個問題，結果發現在某些情況下，Jacobian.inverse（）* Jacobian具有零行列式，不能倒置。不過，我看到互聯網上的其他演示（Youtube）聲稱使用相同的方法，但他們似乎沒有這個問題。所以我不確定問題的原因在哪裏。代碼附加以下：

的* .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

聽起來像是你的系統太硬。

1. 嘗試使用特徵SVD方法：請參閱http://eigen.tuxfamily.org/dox-2.0/TutorialAdvancedLinearAlgebra.html。這是計算量更大，但也可能更安全。如果您正在解決aX = b問題，最好使用專用於反轉矩陣的方法。 （a是你的雅可比而X是你想要的）。
2. 最後，嘗試通過將對角線乘以1.0001等因子來欺騙矩陣中的條件。這不會改變太多的解決方案（特別是對於一款遊戲），但可以更好地解決問題。
3. 我很好奇你爲什麼選擇不使用時間迭代方案，如RK4。

編輯：我沒有讀你的廣泛的帖子，所以我刪除了對泉水的參考。我想在你的情況下，元素會有某種形式的機械交互。

Answer 2

像這樣的東西應該工作。

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

的問題是，如你所說，你的方法無法計算僞逆時（J*J'）是奇異的。

Wikipedia說：「[pseudoinverse]是通過將每個非零的對角線條目替換爲其倒數形成的」。計算pinv(A)爲inv(A'*A)*A'未通過「非零」點。